Add ut, declaration

diff --git a/api/bcar.h b/api/bcar.h
index 170027f3ae1dddc65ed106dcc9ee9d7347589cb0..fbeab22c5e58e451408486e614470532c4e32004 100644 (file)
--- a/api/bcar.h
+++ b/api/bcar.h
@@ -269,4 +269,22 @@ intersect(
         double x2, double y2
 );
 
+/* \brief Return the smallest angle between three points.
+
+\see https://math.stackexchange.com/questions/361412/finding-the-angle-between-three-points
+
+\param x1
+\param y1
+\param x2
+\param y2
+\param x3
+\param y3
+*/
+double
+angle_between_three_points(
+        double x1, double y1,
+        double x2, double y2,
+        double x3, double y3
+);
+
 #endif /* BCAR_H */

diff --git a/src/bcar.cc b/src/bcar.cc
index 09cf2d887c7c399e860f18565c5bb4d868468df4..99a3e18f1d67d0c3fac20bdd6ea670929e052771 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -358,3 +358,12 @@ intersect(
         double iy2 = (-D*dx - std::abs(dy)*sqrt(r*r * dr*dr - D*D)) / (dr*dr);
         return std::make_tuple(true, ix1, iy1, ix2, iy2);
 }
+
+double
+angle_between_three_points(
+        double x1, double y1,
+        double x2, double y2,
+        double x3, double y3
+) {
+        return 0;
+}

diff --git a/ut/bcar.t.cc b/ut/bcar.t.cc
index 77287ffc17ea6c0b8e8ae88b68d6d09be83a682a..cb244cf17af52e8c2c37a41d9e1664ee750ecca1 100644 (file)
--- a/ut/bcar.t.cc
+++ b/ut/bcar.t.cc
@@ -125,6 +125,21 @@ WVTEST_MAIN("test collide functions")
         WVPASS(!std::get<0>(tmpi10));
 }
 
+WVTEST_MAIN("auxiliary angle between three points")
+{
+        double a;
+        a = angle_between_three_points(1, 0, 0, 0, 0, 1);
+        WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
+        a = angle_between_three_points(0, 1, 0, 0, 1, 0);
+        WVPASSEQ_DOUBLE(a, M_PI/2, 0.00001);
+        a = angle_between_three_points(2, 2, 1, 1, 0, 0);
+        WVPASSEQ_DOUBLE(a, 0, 0.00001);
+        a = angle_between_three_points(-2, 2, -1, 1, -1, 2);
+        WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
+        a = angle_between_three_points(-1, 2, -1, 1, -2, 2);
+        WVPASSEQ_DOUBLE(a, M_PI/4, 0.00001);
+}
+
 WVTEST_MAIN("drivable")
 {
         double tmp_double_1 = 0;