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