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.h

diff --git a/y2022/vision/geometry.h b/y2022/vision/geometry.h
index 75a6496..31e8629 100644
--- a/y2022/vision/geometry.h
+++ b/y2022/vision/geometry.h
@@ -106,16 +106,18 @@
     return std::atan2(p_prime.y, p_prime.x);
   }
 
+  // Expects all angles to be from 0 to 2pi
+  // TODO(milind): handle wrapping
+  static inline bool AngleInRange(double theta, double theta_min,
+                                  double theta_max) {
+    return (
+        (theta >= theta_min && theta <= theta_max) ||
+        (theta_min > theta_max && (theta >= theta_min || theta <= theta_max)));
+  }
+
   inline bool InAngleRange(cv::Point2d p, double theta_min,
                            double theta_max) const {
-    const double theta = AngleOf(p);
-
-    // Handle the case if the bounds wrap around 2pi
-    const double max_diff = aos::math::NormalizeAngle(theta_max - theta);
-    const double min_diff = aos::math::NormalizeAngle(theta - theta_min);
-
-    return ((theta == theta_max) || (theta == theta_min) ||
-            (std::signbit(min_diff) == std::signbit(max_diff)));
+    return AngleInRange(AngleOf(p), theta_min, theta_max);
   }
 
  private:

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