commit 8e2582bb4dfe257becac52e97063d02a1b3bbd1e
author Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 06 15:14:15 2022 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 06 18:18:03 2022 -0800
tree 9394986b7ca201e26eed68512f117186b126d2e2
parent 1629151fd055062ab1ec1999541f646872e9203f

Fix collision avoidance min turret angle

We wrap from 0 to 2 pi, but this was a negative angle...

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I97a00201fd68b572d4fbdb77440f5598f7a6f720

y2022/vision/geometry_test.cc

diff --git a/y2022/vision/geometry_test.cc b/y2022/vision/geometry_test.cc
index 9d2159b..0a5a759 100644
--- a/y2022/vision/geometry_test.cc
+++ b/y2022/vision/geometry_test.cc
@@ -81,10 +81,7 @@
 
     const cv::Point2d kZeroPoint = {c->center.x + c->radius, c->center.y};
     EXPECT_NEAR(c->AngleOf(kZeroPoint), 0.0, 1e-5);
-    EXPECT_TRUE(
-        c->InAngleRange(kZeroPoint, -2.1 * 2.0 * M_PI, -1.9 * 2.0 * M_PI));
-    EXPECT_TRUE(
-        c->InAngleRange(kZeroPoint, 1.9 * 2.0 * M_PI, 2.1 * 2.0 * M_PI));
+    EXPECT_TRUE(c->InAngleRange(kZeroPoint, (2.0 * M_PI) - 0.1, 0.1));
     EXPECT_EQ(c->DistanceTo(kZeroPoint), 0.0);
 
     // Test the distance to another point
@@ -101,6 +98,14 @@
     auto c = Circle::Fit({{-6.0, 3.2}, {-3.0, 2.0}, {-6.0, 3.2}});
     EXPECT_FALSE(c.has_value());
   }
+  // Test if angles are in ranges
+  {
+    EXPECT_TRUE(Circle::AngleInRange(0.5, 0.4, 0.6));
+    EXPECT_TRUE(Circle::AngleInRange(0, (2.0 * M_PI) - 0.2, 0.2));
+    EXPECT_FALSE(
+        Circle::AngleInRange(0, (2.0 * M_PI) - 0.2, (2.0 * M_PI) - 0.1));
+    EXPECT_TRUE(Circle::AngleInRange(0.5, (2.0 * M_PI) - 0.1, 0.6));
+  }
 }
 
 }  // namespace y2022::vision::testing