void calculateMinAndMaxDistances(const std::vector<geometry_msgs::Point>& footprint, double& min_dist, double& max_dist)
{
min_dist = std::numeric_limits<double>::max();
max_dist = 0.0;
if (footprint.size() <= 2)
{
return;
}
for (unsigned int i = 0; i < footprint.size() - 1; ++i)
{
// check the distance from the robot center point to the first vertex
double vertex_dist = distance(0.0, 0.0, footprint[i].x, footprint[i].y);
double edge_dist = distanceToLine(0.0, 0.0, footprint[i].x, footprint[i].y,
footprint[i + 1].x, footprint[i + 1].y);
min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}
// we also need to do the last vertex and the first vertex
double vertex_dist = distance(0.0, 0.0, footprint.back().x, footprint.back().y);
double edge_dist = distanceToLine(0.0, 0.0, footprint.back().x, footprint.back().y,
footprint.front().x, footprint.front().y);
min_dist = std::min(min_dist, std::min(vertex_dist, edge_dist));
max_dist = std::max(max_dist, std::max(vertex_dist, edge_dist));
}
bool gqbGraphSimple::insideLine(wxPoint &pt, wxPoint &p1, wxPoint &p2, int threshold=7)
{
bool value=false;
if(distanceToLine(pt,p1,p2)<threshold)
{
value=true;
}
return value;
}
