Change smaller angle of two computation

In fact, it's not important to find the smaller angle but the angle
between node closer to the current car cc.

diff --git a/src/psp.cc b/src/psp.cc
index 740630fe750e2a2234e0bf123e6e8db56edff1e6..1888db6e8f4af90de40bfead796bc3e99a28d1f1 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -305,9 +305,10 @@ double smaller_angle_of_two(
         double x1, double y1, // first ending point
         double x2, double y2 // second ending point
 ) {
-        double a1 = ::angle_between_three_points(sx, sy, cx, cy, x1, y1);
-        double a2 = ::angle_between_three_points(sx, sy, cx, cy, x2, y2);
-        return std::min(a1, a2);
+        if (edist(sx, sy, x1, y1) < edist(sx, sy, x2, y2))
+                return ::angle_between_three_points(sx, sy, cx, cy, x1, y1);
+        else
+                return ::angle_between_three_points(sx, sy, cx, cy, x2, y2);
 }
 
 void PSPlanner::fe_parallel()