Point PathFollowingGeometry::getFeedforwardCurveCenterPoint()
{
if (!this->has_feedforward_center) {
PointArr points = this->getCurvatureDetectionPoints();
if (points.size() >= 5) {
Point p1 = points.at(0);
Point p2 = points.at(2);
Point p3 = points.at(4);
//ROS_INFO("p1 %.2f,%2.f; p2 %.2f,%2.f; p3 %.2f,%2.f", p1.x, p1.y, p2.x, p2.y, p3.x, p3.y);
//Equation 12
double l12 = (p2.y-p1.y)/(p2.x-p1.x);
double l23 = (p3.y-p2.y)/(p3.x-p2.x);
//Equation 10
this->feedforward_center.x = -(p1.y+p2.y)/2+(p2.y+p3.y)/2
-(p1.x+p2.x)/(2*l12)+(p2.x+p3.x)/(2*l23);
//Equation 11
this->feedforward_center.y = -(p2.y+p3.y)/2-this->feedforward_center.x/l23*this->feedforward_center.x+(p2.x+p3.x)/(2*l23);
this->has_feedforward_center = true;
}
else {
//ROS_WARN("Can't get curve center. Expected 5 detection points.");
}
}
return this->feedforward_center;
}
Point PathFollowingGeometry::getClosestPointFromSet(Point referencePoint, PointArr pointSet)
{
Point result;
if (pointSet.size() > 0) {
result = pointSet.at(0);
double closest_distance = distance(referencePoint, result);
for (PointArrIt it = pointSet.begin()+1; it < pointSet.end(); it++) {
double new_distance = distance(referencePoint, *it);
//ROS_INFO("%.2f,%.2f -> %.2f,%.2f = %.4f", referencePoint.x, referencePoint.y, (*it).x, (*it).y, new_distance);
if (new_distance < closest_distance) {
closest_distance = new_distance;
result = *it;
}
}
// ROS_WARN("Shortest %.2f,%.2f -> %.2f,%.2f = %.4f", referencePoint.x, referencePoint.y, result.x, result.y, closest_distance);
}
else {
result = referencePoint;
}
return result;
}