Resolve "Implement multiple BestGaolKickTargets for the Ball"

Closes #1668

See merge request main/Sumatra!1421

sumatra-commit: bf67505489a3e6e86458433729974d4725b339e8

modules/common-math/src/main/java/edu/tigers/sumatra/math/AngleMath.java

@@ -46,6 +46,8 @@ public final class AngleMath
 	/**
 	 * Get the smallest difference between angle1 and angle2.<br>
 	 * <code>norm( angle1 - angle2 )</code>
+	 * A positive difference represents a counter clockwise turn from angle2 to angle1
+	 * A negative difference represents a clockwise turn from angle2 to angle1
 	 *
 	 * @param angle1 first angle
 	 * @param angle2 second angle
@@ -57,6 +59,51 @@ public final class AngleMath
 	}
 
 
+	/**
+	 * Get the resulting angle after rotating angleStart in rotationDirection for angleAmount.<br>
+	 * Rotating with a negative value of angleAmount is equivalent to rotating the opposite direction with the absolute amount
+	 *
+	 * @param angleStart
+	 * @param angleAmount
+	 * @param rotationDirection
+	 * @return
+	 */
+	public static double rotateAngle(final double angleStart, final double angleAmount,
+			final ERotationDirection rotationDirection)
+	{
+		return normalizeAngle(
+				rotationDirection == ERotationDirection.NO_ROTATION ? angleStart :
+						rotationDirection == ERotationDirection.CCW_ROTATION ?
+								angleStart + angleAmount :
+								angleStart - angleAmount
+		);
+	}
+
+
+	public static ERotationDirection rotationDirection(final double angleStart, final double angleEnd)
+	{
+		final var difference = AngleMath.difference(angleEnd, angleStart);
+		if (Math.abs(difference) < 1e-6)
+		{
+			return ERotationDirection.NO_ROTATION;
+		} else if (difference < 0)
+		{
+			return ERotationDirection.CW_ROTATION;
+		} else
+		{
+			return ERotationDirection.CCW_ROTATION;
+		}
+	}
+
+
+	public enum ERotationDirection
+	{
+		NO_ROTATION,
+		CW_ROTATION,
+		CCW_ROTATION
+	}
+
+
 	/**
 	 * Get the absolute smallest difference between angle1 and angle2.<br>
 	 * <code>abs(norm(angle1-angle2))</code>

modules/common-math/src/test/java/edu/tigers/sumatra/math/AngleMathTest.java

@@ -4,23 +4,23 @@
 
 package edu.tigers.sumatra.math;
 
+import org.junit.Test;
+
 import static org.assertj.core.api.Assertions.assertThat;
 import static org.assertj.core.api.Assertions.withinPercentage;
 import static org.junit.Assert.assertEquals;
 
-import org.junit.Test;
-
 
 /**
  * Testing the AngleMath module.
- * 
+ *
  * @author stei_ol
  */
 public class AngleMathTest
 {
-	private static final double	ACCURACY	= 0.001;
-	
-	
+	private static final double ACCURACY = 0.001;
+
+
 	@Test
 	public void testNormalizeAngle()
 	{
@@ -32,8 +32,8 @@ public class AngleMathTest
 		assertEquals(AngleMath.normalizeAngle(5f * AngleMath.PI), AngleMath.PI, ACCURACY);
 		assertEquals(AngleMath.normalizeAngle(4f * AngleMath.PI), 0, ACCURACY);
 	}
-	
-	
+
+
 	@Test
 	public void testTrigonometry()
 	{
@@ -41,17 +41,17 @@ public class AngleMathTest
 		assertEquals(SumatraMath.sin(AngleMath.PI), 0, ACCURACY);
 		assertEquals(SumatraMath.sin(4.5f), -0.9775301, ACCURACY);
 		assertEquals(SumatraMath.sin(-34), -0.529, ACCURACY);
-		
+
 		// cos
 		assertEquals(SumatraMath.cos(5), 0.28366, ACCURACY);
 		assertEquals(SumatraMath.cos(-0.1f), 0.9950, ACCURACY);
-		
+
 		// tan
 		assertEquals(SumatraMath.tan(3), -0.1425, ACCURACY);
 		assertEquals(SumatraMath.tan(-2), 2.185, ACCURACY);
 	}
-	
-	
+
+
 	@Test
 	public void testDifference()
 	{
@@ -63,8 +63,36 @@ public class AngleMathTest
 		assertThat(AngleMath.difference(0, 0)).isEqualTo(0.0);
 		assertThat(AngleMath.difference(50, 10)).isEqualTo(AngleMath.normalizeAngle(40));
 	}
-	
-	
+
+
+	@Test
+	public void testRotateAngle()
+	{
+		assertThat(AngleMath.rotateAngle(-1, 2, AngleMath.ERotationDirection.CCW_ROTATION)).isEqualTo(1);
+		assertThat(AngleMath.rotateAngle(1, 2, AngleMath.ERotationDirection.CW_ROTATION)).isEqualTo(-1);
+		assertThat(AngleMath.rotateAngle(0, 42, AngleMath.ERotationDirection.NO_ROTATION)).isEqualTo(0);
+		assertThat(AngleMath.rotateAngle(15 * AngleMath.PI_TWO, 42, AngleMath.ERotationDirection.NO_ROTATION))
+				.isEqualTo(0);
+		assertThat(AngleMath.rotateAngle(-1, 2 + 10 * AngleMath.PI_TWO, AngleMath.ERotationDirection.CCW_ROTATION))
+				.isEqualTo(1);
+	}
+
+
+	@Test
+	public void testRotationDirection()
+	{
+		assertThat(AngleMath.rotationDirection(-1, 1)).isEqualTo(AngleMath.ERotationDirection.CCW_ROTATION);
+		assertThat(AngleMath.rotationDirection(1, -1)).isEqualTo(AngleMath.ERotationDirection.CW_ROTATION);
+		assertThat(AngleMath.rotationDirection(1, 1)).isEqualTo(AngleMath.ERotationDirection.NO_ROTATION);
+		assertThat(AngleMath.rotationDirection(-1 + 10 * AngleMath.PI_TWO, 1 + 10 * AngleMath.PI_TWO))
+				.isEqualTo(AngleMath.ERotationDirection.CCW_ROTATION);
+		assertThat(AngleMath.rotationDirection(1 + 10 * AngleMath.PI_TWO, -1))
+				.isEqualTo(AngleMath.ERotationDirection.CW_ROTATION);
+		assertThat(AngleMath.rotationDirection(15 * AngleMath.PI_TWO, 10 * AngleMath.PI_TWO))
+				.isEqualTo(AngleMath.ERotationDirection.NO_ROTATION);
+	}
+
+
 	@Test
 	public void testDeg2Rad()
 	{
@@ -78,8 +106,8 @@ public class AngleMathTest
 		assertThat(AngleMath.deg2rad(360)).isCloseTo(AngleMath.PI_TWO, withinPercentage(0.1));
 		assertThat(AngleMath.deg2rad(450)).isCloseTo(AngleMath.PI_TWO + AngleMath.PI_HALF, withinPercentage(0.1));
 	}
-	
-	
+
+
 	@Test
 	public void testRad2Deg()
 	{