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; } }
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()); }
bool hit(const T &obj, ObstacleGroup &hitSet) const { for (const_iterator it = begin(); it!=end(); ++it) { if ((*it)->hit(obj)) { hitSet.add(*it); } } return !hitSet.empty(); }
void OpenSteer:: BoxObstacle:: findIntersectionWithVehiclePath (const AbstractVehicle& vehicle, PathIntersection& pi) const { // abbreviations const float w = width; // dimensions const float h = height; const float d = depth; const Vec3 s = side (); // local space const Vec3 u = up (); const Vec3 f = forward (); const Vec3 p = position (); const Vec3 hw = s * (0.5f * width); // offsets for face centers const Vec3 hh = u * (0.5f * height); const Vec3 hd = f * (0.5f * depth); const seenFromState sf = seenFrom (); // the box's six rectangular faces RectangleObstacle r1 (w, h, s, u, f, p + hd, sf); // front RectangleObstacle r2 (w, h, -s, u, -f, p - hd, sf); // back RectangleObstacle r3 (d, h, -f, u, s, p + hw, sf); // side RectangleObstacle r4 (d, h, f, u, -s, p - hw, sf); // other side RectangleObstacle r5 (w, d, s, -f, u, p + hh, sf); // top RectangleObstacle r6 (w, d, -s, -f, -u, p - hh, sf); // bottom // group the six RectangleObstacle faces together ObstacleGroup faces; faces.push_back (&r1); faces.push_back (&r2); faces.push_back (&r3); faces.push_back (&r4); faces.push_back (&r5); faces.push_back (&r6); // find first intersection of vehicle path with group of six faces PathIntersection next; firstPathIntersectionWithObstacleGroup (vehicle, faces, pi, next); // when intersection found, adjust PathIntersection for the box case if (pi.intersect) { pi.obstacle = this; pi.steerHint = ((pi.surfacePoint - position ()).normalize () * (pi.vehicleOutside ? 1.0f : -1.0f)); } }
Geometry2d::Point OurRobot::findGoalOnPath(const Geometry2d::Point& pose, const Planning::Path& path, const ObstacleGroup& obstacles) { const bool blend_verbose = false; // empty path case - leave robot stationary if (path.empty()) return pose; // find closest point on path to pose float max = path.points[0].distTo(pose); unsigned int ip = 0; for (unsigned i=0; i<path.points.size(); i++) { if (path.points[i].distTo(pose) < max) { max = path.points[i].distTo(pose); ip = i; } } if (blend_verbose) addText(QString("cur pt %1=(%2,%3)").arg(ip).arg(path.points[ip].x).arg(path.points[ip].y)); // go to nearest point if only point or closest point is the goal if (path.size() == 1) { if (blend_verbose) addText(QString("blend:simple_path")); return path.points[0]; } // can't mix, just go to endpoint if (path.size() == 2) { if (blend_verbose) addText(QString("blend:size2")); return path.points[1]; } // FIXME: does not blend the last segment // All other cases: proportionally blend the next two points together for a smoother // path, so long as it is still viable if (blend_verbose) addText(QString("blend:segments=%1").arg(path.points.size()-1)); // pull out relevant points Point p0 = pos; Point p1; Point p2; if (path.size() > ip+2) { p1 = path.points[ip+1]; p2 = path.points[ip+2]; } else if (path.size() > ip+1) { p1 = path.points[ip]; p2 = path.points[ip+1]; } else { p1 = path.points[ip-1]; p2 = path.points[ip]; } Geometry2d::Segment target_seg(p1, p2); if (blend_verbose) addText(QString("pos=(%1,%2)").arg(pos.x,5).arg(pos.y,5)); if (blend_verbose) addText(QString("path[0]=(%1,%2)").arg(path.points[0].x).arg(path.points[0].y)); if (blend_verbose) addText(QString("p1=(%1,%2)").arg(p1.x,5).arg(p1.y,5)); if (blend_verbose) addText(QString("p2=(%1,%2)").arg(p2.x,5).arg(p2.y,5)); // final endpoint handling if (target_seg.nearPointPerp(p0, 0.02) && p0.nearPoint(p2, 0.03)) { if (2 == path.size()-1) { if (blend_verbose) addText(QString("blend:at_end")); return p2; } else { // reset this segment to next one if (blend_verbose) addText(QString("blend:reset_segment")); Point temp(p1); p1 = p2; p2 = temp; target_seg = Geometry2d::Segment(p1, p2); } } float dist1 = p0.distTo(p1), dist2 = p1.distTo(p2); if (blend_verbose) addText(QString("blend:d1=%1,d2=%2").arg(dist1).arg(dist2)); // endpoint handling if (dist1 < 0.02) { if (blend_verbose) addText(QString("blend:dist1small=%1").arg(dist1)); return p2; /// just go to next point } // short segment handling if (p1.distTo(p2) < 0.05) { if (blend_verbose) addText(QString("blend:dist2small=%1").arg(dist2)); return p2; /// just go to next point } // close to segment - go to end of segment if (target_seg.nearPoint(p0, 0.03)) { if (blend_verbose) addText(QString("blend:closeToSegment")); return p2; } // mix the next point between the first and second point // if we are far away from p1, want scale to be closer to p1 // if we are close to p1, want scale to be closer to p2 float scale = 1 - clamp(dist1/dist2, 0.0f, 1.0f); Geometry2d::Point targetPos = p1 + (p2-p1)*scale; if (blend_verbose) { addText(QString("blend:scale=%1").arg(scale)); addText(QString("blend:dist1=%1").arg(dist1)); addText(QString("blend:dist2=%1").arg(dist2)); } // check for collisions on blended path Geometry2d::Segment shortcut(p0, targetPos); Geometry2d::Point result = p1; if (!obstacles.hit(shortcut)) { if (blend_verbose) addText(QString("blend:shortcut_succeed")); result = targetPos; } else if (result.nearPoint(pose, 0.05)) { if (blend_verbose) addText(QString("blend:shortcut_failed")); result = result + (result-pose).normalized() * 0.10; } if (blend_verbose) addText(QString("point (%1, %2)").arg(result.x).arg(result.y)); return result; }