Refactor smaller angle of two code

diff --git a/src/psp.cc b/src/psp.cc
index 12d897dfe3d1c66f82d6b013683a9feaeefea1fe..23f6e799a2ac1fab688c8dbb1e4e900d691e1e22 100644 (file)
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -299,6 +299,17 @@ void PSPlanner::fe()
         }
 }
 
+double smaller_angle_of_two(
+        double sx, double sy, // common start point
+        double cx, double cy, // common middle point
+        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);
+}
+
 void PSPlanner::fe_parallel()
 {
         BicycleCar bco = BicycleCar(this->gc());
@@ -372,17 +383,12 @@ void PSPlanner::fe_parallel()
                                         this->ps().x2(), this->ps().y2(),
                                         this->ps().x3(), this->ps().y3()
                                 );
-                                double a21 = ::angle_between_three_points(
-                                    this->cc().rrx(), this->cc().rry(),
-                                    cclx, ccly,
-                                    std::get<1>(cli2), std::get<2>(cli2)
+                                double a2 = smaller_angle_of_two(
+                                        this->cc().rrx(), this->cc().rry(),
+                                        cclx, ccly,
+                                        std::get<1>(cli2), std::get<2>(cli2),
+                                        std::get<3>(cli2), std::get<4>(cli2)
                                 );
-                                double a22 = ::angle_between_three_points(
-                                    this->cc().rrx(), this->cc().rry(),
-                                    cclx, ccly,
-                                    std::get<3>(cli2), std::get<4>(cli2)
-                                );
-                                double a2 = std::min(a21, a22);
                         } else if (ccl_lr >= ccl_p1) {
                                 // in parking slot
                                 // TODO (ccl, lr) x (p1, p2)
@@ -391,17 +397,12 @@ void PSPlanner::fe_parallel()
                                         this->ps().x2(), this->ps().y2(),
                                         this->ps().x3(), this->ps().y3()
                                 );
-                                double a21 = ::angle_between_three_points(
-                                    this->cc().rrx(), this->cc().rry(),
-                                    cclx, ccly,
-                                    std::get<1>(cli2), std::get<2>(cli2)
-                                );
-                                double a22 = ::angle_between_three_points(
-                                    this->cc().rrx(), this->cc().rry(),
-                                    cclx, ccly,
-                                    std::get<3>(cli2), std::get<4>(cli2)
+                                double a2 = smaller_angle_of_two(
+                                        this->cc().rrx(), this->cc().rry(),
+                                        cclx, ccly,
+                                        std::get<1>(cli2), std::get<2>(cli2),
+                                        std::get<3>(cli2), std::get<4>(cli2)
                                 );
-                                double a2 = std::min(a21, a22);
                         }
 
                         double r1 = sqrt(