/*!
  \file
  \brief RangeFinder データの 2D 展開

  \author Satofumi KAMIMURA

  $Id: convert2d.cpp 1584 2009-12-25 23:25:11Z satofumi $
*/

#include "convert2d.h"
#include "RangeFinder.h"

using namespace qrk;
using namespace std;


void qrk::convert2d(std::vector<Point<long> >& points,
                    const RangeFinder& range_finder,
                    const std::vector<long>& data,
                    const Position<long>& offset)
{
  long min_distance = range_finder.minDistance();
  long max_distance = range_finder.maxDistance();

  double offset_radian = offset.to_rad();

  int index = 0;
  for (vector<long>::const_iterator it = data.begin();
       it != data.end(); ++it, ++index) {
    long distance = *it;
    if ((distance <= min_distance) || (distance >= max_distance)) {
      continue;
    }

    double radian = range_finder.index2rad(index) + offset_radian;
    int x = static_cast<long>(distance * cos(radian)) + offset.x;
    int y = static_cast<long>(distance * sin(radian)) + offset.y;

    points.push_back(Point<long>(x, y));
  }
}
