homologation: add get_point_to_line_distance() function

diff --git a/src/robofsm/homologation.cc b/src/robofsm/homologation.cc
index ae3f9c1be139e9071b2e18d86c7fb64ec9a9fbe5..ad84149ae729257eb08a828e9aff0008bcb0c510 100644 (file)
--- a/src/robofsm/homologation.cc
+++ b/src/robofsm/homologation.cc
@@ -41,6 +41,8 @@ struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
 static const Point containerPosition(PLAYGROUND_WIDTH_M - 0.25, 0.02); // blue container Position
 
 // returns pointer to next real (non-fake) corn which was not yet collected
+// TODO: note: use this for "short_time_to_end: situation only, otherwise
+// it is better to try to rush more corns at once
 struct corn * choose_next_corn()
 {
        Point cornPosition;
@@ -63,9 +65,23 @@ struct corn * choose_next_corn()
        return minCorn;
 }
 
+/**
+ * Computes and returns line to point distance
+ * @param[in] p the point coords
+ * @param[in] lp1 coords of one of the points on the line
+ * @param[in] lp2 coords of the second point on the line
+ */
+double get_point_to_line_distance(const Point &p, const Point &lp1, const Point &lp2)
+{
+       double distance = fabs((lp2.x - lp1.x)*(lp1.y-p.y) - (lp1.x - p.x)*(lp2.y - lp1.y))
+               / sqrt((lp2.x - lp1.x)*(lp2.x - lp1.x) + (lp2.y - lp1.y)*(lp2.y - lp1.y));
+       return distance;
+}
+
 Pos * get_corn_approach_position(struct corn *corn)
 {
-       
+       // TODO
+       return NULL;
 }