void Planning::Path::startFrom(const Geometry2d::Point& pt, Planning::Path& result) const {
// path will start at the current robot pose
result.clear();
result.points.push_back(pt);
if (points.empty())
return;
// handle simple paths
if (points.size() == 1) {
result.points.push_back(points.front());
return;
}
// find where to start the path
Geometry2d::Segment close_segment;
float dist = -1;
unsigned int i = (points.front().nearPoint(pt, 0.02)) ? 1 : 0;
vector<Geometry2d::Point>::const_iterator path_start = ++points.begin();
for (; i < (points.size() - 1); ++i)
{
Geometry2d::Segment s(points[i], points[i+1]);
const float d = s.distTo(pt);
if (dist < 0 || d < dist)
{
close_segment = s;
dist = d;
}
}
// slice path
// new path will be pt, [closest point on nearest segment], [i+1 to end]
if (dist > 0.0 && dist < 0.02) {
Geometry2d::Point intersection_pt = close_segment.nearestPoint(pt);
result.points.push_back(intersection_pt);
}
result.points.insert(result.points.end(), path_start, points.end());
}