view/rangesCtrl/ConvertToGrid.cpp

/*!
  \file
  \brief Łf[^Ě X-Y ˝ĘWJ

  \author Satofumi KAMIMURA

  $Id$
*/

#include "ConvertToGrid.h"
#include "RangeSensorInterface.h"
#include "MathUtils.h"


void beego::convertToGrid(std::vector<Grid<int> >& points, long data[],
                          size_t size, const RangeSensorInterface* ranges,
                          const Position<int>& offset) {

  long min_length = ranges->getMinDistance();
  long max_length = ranges->getMaxDistance();
  Grid<int> p;
  for (size_t i = 0; i < size; ++i) {
    long length = data[i];
    if ((length < min_length) || (length > max_length)) {
      continue;
    }
    // _Ěo^
    double radian = ranges->index2rad(i) - offset.angle.to_rad();
    p.x = static_cast<int>(length * cos(radian)) + offset.x;
    p.y = static_cast<int>(length * sin(radian)) + offset.y;

    points.push_back(p);
  }
}