RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid,
float max_range)
{
gm::Point np2 = p2;
if (max_range > 0) {
double distance = euclideanDistance(p1,p2);
if (distance > max_range) {
np2.x = ((p2.x - p1.x) * max_range/distance) + p1.x;
np2.y = ((p2.y - p1.y) * max_range/distance) + p1.y;
}
}
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, np2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, np2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(np2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
RayTraceIterRange rayTrace (const nm::MapMetaData& info, const gm::Point& p1, const gm::Point& p2,
bool project_onto_grid, bool project_source_onto_grid)
{
Cell c1 = pointCell(info, p1);
Cell c2 = pointCell(info, p2);
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Ray tracing between " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
const RayTraceIterator done(c1, c1, true);
const RayTraceIterRange empty_range(done, done);
if (!withinBounds(info, c1)) {
if (project_source_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c2, c1);
if (c)
c1 = *c;
else
return empty_range;
}
else
throw PointOutOfBoundsException(p1);
}
if (!withinBounds(info, p2)) {
if (project_onto_grid) {
const optional<Cell> c = rayTraceOntoGrid(info, c1, c2);
if (c)
c2 = *c;
else
return empty_range;
}
else {
throw PointOutOfBoundsException(p2);
}
}
ROS_DEBUG_STREAM_NAMED ("ray_trace", "Projected ray trace endpoints to " << c1.x <<
", " << c1.y << " and " << c2.x << ", " << c2.y);
return rayTrace(c1, c2);
}
