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