bool Planning::Path::hit(const ObstacleGroup &obstacles, unsigned int start) const
{
if (start >= points.size())
{
// Empty path or starting beyond end of path
return false;
}
// The set of obstacles the starting point was inside of
ObstacleGroup hit;
obstacles.hit(points[start], hit);
for (unsigned int i = start; i < (points.size() - 1); ++i)
{
ObstacleGroup newHit;
obstacles.hit(Geometry2d::Segment(points[i], points[i + 1]), newHit);
try
{
set_difference(newHit.begin(), newHit.end(), hit.begin(), hit.end(), ExceptionIterator<ObstaclePtr>());
} catch (exception& e)
{
// Going into a new obstacle
return true;
}
}
// Didn't hit anything or never left any obstacle
return obstacles.hit(points.back());
}
void
OpenSteer::Obstacle::
firstPathIntersectionWithObstacleGroup (const AbstractVehicle& vehicle,
const ObstacleGroup& obstacles,
PathIntersection& nearest,
PathIntersection& next)
{
// test all obstacles in group for an intersection with the vehicle's
// future path, select the one whose point of intersection is nearest
next.intersect = false;
nearest.intersect = false;
for (ObstacleIterator o = obstacles.begin(); o != obstacles.end(); ++o)
{
// find nearest point (if any) where vehicle path intersects obstacle
// o, storing the results in PathIntersection object "next"
(**o).findIntersectionWithVehiclePath (vehicle, next);
// if this is the first intersection found, or it is the nearest found
// so far, store it in PathIntersection object "nearest"
const bool firstFound = !nearest.intersect;
const bool nearestFound = (next.intersect &&
(next.distance < nearest.distance));
if (firstFound || nearestFound) nearest = next;
}
}
