From ffe34f00fe8b4622b12e8fa1c51fc7e7433bf7f0 Mon Sep 17 00:00:00 2001 From: Matt Date: Sun, 21 Nov 2021 19:50:15 -0500 Subject: [PATCH] Ensure skew is always in [-90, 90] (#319) --- .../org/photonvision/vision/target/TargetCalculations.java | 4 ++++ .../photonvision/vision/target/TargetCalculationsTest.java | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java index d04db786c..c0633a2e5 100644 --- a/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java +++ b/photon-core/src/main/java/org/photonvision/vision/target/TargetCalculations.java @@ -42,6 +42,10 @@ public class TargetCalculations { if (isLandscape && minAreaRect.size.width < minAreaRect.size.height) angle += 90; else if (!isLandscape && minAreaRect.size.height < minAreaRect.size.width) angle += 90; + // Ensure skew is bounded on [-90, 90] + while (angle > 90) angle -= 180; + while (angle < -90) angle += 180; + return angle; } diff --git a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java index 003a5e1af..6afcba9d6 100644 --- a/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java +++ b/photon-core/src/test/java/org/photonvision/vision/target/TargetCalculationsTest.java @@ -103,7 +103,7 @@ public class TargetCalculationsTest { public void testSkewCalculation() { // Setup var isLandscape = true; - var rect = new RotatedRect(new Point(), new Size(10, 5), -10); + var rect = new RotatedRect(new Point(), new Size(10, 5), 170); // Compute min area rect var points = new Point[4];