libs/range_sensor/convert2d.cpp

Go to the documentation of this file.

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

using namespace std;


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