/*!
  \file
  \brief 距離データの 2D 変換

  \author Satofumi KAMIMURA

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

#include "convert2d.h"
#include "RangeSensor.h"

using namespace std;


/*!
  \brief データの二次元展開

  \param[out] points 二次元データ
  \param[in] sensor レンジセンサーインターフェース
  \param[in] data 測定データ
  \param[in] offset 二次元展開時のオフセット位置
  \param[in] max_length 制限する距離の最大値
*/
void qrk::convert2d(vector<qrk::Point<long> >& points,
                    const RangeSensor* sensor, const vector<long>& data,
                    const Position<long>& offset, int max_length)
{
  int min_distance = sensor->minDistance();
  int max_distance = (max_length < 0) ? sensor->maxDistance() : max_length;

  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 = sensor->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));
  }
}


void qrk::convert2d(vector<qrk::Point<long> >& points,
                    const RangeSensor* sensor, const vector<long>& data,
                    int max_length)
{
  convert2d(points, sensor, data, Position<long>(0, 0, deg(0)), max_length);
}
