Bugfix: fix crash in some circumstances caused by Avoid crossing perimeters. #2271

This commit is contained in:
Alessandro Ranellucci 2014-09-23 20:19:47 +02:00
parent 24d67c42c6
commit 6573ae002a
3 changed files with 9 additions and 3 deletions

View File

@ -226,6 +226,9 @@ MotionPlannerGraph::find_node(const Point &point) const
void void
MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline) MotionPlannerGraph::shortest_path(size_t from, size_t to, Polyline* polyline)
{ {
// this prevents a crash in case for some reason we got here with an empty adjacency list
if (this->adjacency_list.empty()) return;
const weight_t max_weight = std::numeric_limits<weight_t>::infinity(); const weight_t max_weight = std::numeric_limits<weight_t>::infinity();
std::vector<weight_t> min_distance; std::vector<weight_t> min_distance;

View File

@ -107,10 +107,13 @@ Point::nearest_point_index(const PointPtrs &points) const
return this->nearest_point_index(p); return this->nearest_point_index(p);
} }
void bool
Point::nearest_point(const Points &points, Point* point) const Point::nearest_point(const Points &points, Point* point) const
{ {
*point = points.at(this->nearest_point_index(points)); int idx = this->nearest_point_index(points);
if (idx == -1) return false;
*point = points.at(idx);
return true;
} }
double double

View File

@ -37,7 +37,7 @@ class Point
int nearest_point_index(const Points &points) const; int nearest_point_index(const Points &points) const;
int nearest_point_index(const PointConstPtrs &points) const; int nearest_point_index(const PointConstPtrs &points) const;
int nearest_point_index(const PointPtrs &points) const; int nearest_point_index(const PointPtrs &points) const;
void nearest_point(const Points &points, Point* point) const; bool nearest_point(const Points &points, Point* point) const;
double distance_to(const Point &point) const; double distance_to(const Point &point) const;
double distance_to(const Line &line) const; double distance_to(const Line &line) const;
double ccw(const Point &p1, const Point &p2) const; double ccw(const Point &p1, const Point &p2) const;