Lots of improvements to MotionPlanner/avoid_crossing_perimeters. Smoother paths and several edge cases now handled better

This commit is contained in:
Alessandro Ranellucci 2015-01-06 20:52:36 +01:00
parent 5e100abe25
commit 8f4cbefd0d
17 changed files with 386 additions and 113 deletions

View file

@ -104,6 +104,32 @@ Point::nearest_point_index(const PointConstPtrs &points) const
return idx;
}
/* This method finds the point that is closest to both this point and the supplied one */
size_t
Point::nearest_waypoint_index(const Points &points, const Point &dest) const
{
size_t idx = -1;
double distance = -1; // double because long is limited to 2147483647 on some platforms and it's not enough
for (Points::const_iterator p = points.begin(); p != points.end(); ++p) {
// distance from this to candidate
double d = pow(this->x - p->x, 2) + pow(this->y - p->y, 2);
// distance from candidate to dest
d += pow(p->x - dest.x, 2) + pow(p->y - dest.y, 2);
// if the total distance is greater than current min distance, ignore it
if (distance != -1 && d > distance) continue;
idx = p - points.begin();
distance = d;
if (distance < EPSILON) break;
}
return idx;
}
int
Point::nearest_point_index(const PointPtrs &points) const
{
@ -123,6 +149,15 @@ Point::nearest_point(const Points &points, Point* point) const
return true;
}
bool
Point::nearest_waypoint(const Points &points, const Point &dest, Point* point) const
{
int idx = this->nearest_waypoint_index(points, dest);
if (idx == -1) return false;
*point = points.at(idx);
return true;
}
double
Point::distance_to(const Point &point) const
{