From 59b446fa60c96b15953823a5d5041a5e2d978979 Mon Sep 17 00:00:00 2001
From: NicolaiO <nicolai.ommer@gmail.com>
Date: Sat, 13 Jul 2024 20:46:14 +0000
Subject: [PATCH] Resolve "Ball placement role should pull ball with exact 45
 degree angle out of goal corners"

Closes #1946

See merge request main/Sumatra!1868

sumatra-commit: 36f058f7ca836b109a153ebe805226df6b9e5af1
---
 .../edu/tigers/sumatra/math/SumatraMath.java  | 13 ++++++++++++
 .../tigers/sumatra/math/vector/AVector2.java  | 10 ++++++++++
 .../tigers/sumatra/math/vector/IVector2.java  |  8 ++++++++
 .../edu/tigers/sumatra/geometry/Goal.java     | 20 +++++++++++++++++++
 4 files changed, 51 insertions(+)

diff --git a/modules/common-math/src/main/java/edu/tigers/sumatra/math/SumatraMath.java b/modules/common-math/src/main/java/edu/tigers/sumatra/math/SumatraMath.java
index e6ab5ad4..da546e6c 100644
--- a/modules/common-math/src/main/java/edu/tigers/sumatra/math/SumatraMath.java
+++ b/modules/common-math/src/main/java/edu/tigers/sumatra/math/SumatraMath.java
@@ -222,6 +222,19 @@ public static int cap(final int value, final int bound1, final int bound2)
 	}
 
 
+	/**
+	 * Add a magnitude to the absolute of a value.
+	 *
+	 * @param baseValue the base value
+	 * @param offset    the magnitude to add to the absolute of the base value
+	 * @return the result
+	 */
+	public static double addMagnitude(double baseValue, double offset)
+	{
+		return Math.signum(baseValue) * (Math.abs(baseValue) + offset);
+	}
+
+
 	/**
 	 * Solves for the real roots of a quadratic equation with real
 	 * coefficients. The quadratic equation is of the form
diff --git a/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/AVector2.java b/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/AVector2.java
index 3793b9af..32523369 100644
--- a/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/AVector2.java
+++ b/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/AVector2.java
@@ -303,4 +303,14 @@ public Vector2 projectOntoThis(IVector2 other)
 		var normalized = normalizeNew();
 		return normalized.multiply(normalized.scalarProduct(other));
 	}
+
+
+	@Override
+	public Vector2 addMagnitude(IVector2 offset)
+	{
+		return Vector2.fromXY(
+				SumatraMath.addMagnitude(x(), offset.x()),
+				SumatraMath.addMagnitude(y(), offset.y())
+		);
+	}
 }
diff --git a/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/IVector2.java b/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/IVector2.java
index a75431e0..3ea7c166 100644
--- a/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/IVector2.java
+++ b/modules/common-math/src/main/java/edu/tigers/sumatra/math/vector/IVector2.java
@@ -270,4 +270,12 @@ default int getNumDimensions()
 	 * @return
 	 */
 	Vector2 projectOntoThis(IVector2 other);
+
+	/**
+	 * Add a magnitude to the absolute of this vector.
+	 *
+	 * @param offset the magnitude to add
+	 * @return the result
+	 */
+	Vector2 addMagnitude(IVector2 offset);
 }
\ No newline at end of file
diff --git a/modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Goal.java b/modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Goal.java
index e068e703..5f20bc66 100644
--- a/modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Goal.java
+++ b/modules/moduli-geometry/src/main/java/edu/tigers/sumatra/geometry/Goal.java
@@ -14,6 +14,8 @@
 import edu.tigers.sumatra.math.vector.Vector2;
 import edu.tigers.sumatra.math.vector.Vector2f;
 
+import java.util.List;
+
 
 /**
  * This is an immutable representation of a goal.
@@ -94,6 +96,15 @@ public Vector2f getRightPost()
 	}
 
 
+	public List<IVector2> getGoalPosts()
+	{
+		return List.of(
+				getLeftPost(),
+				getRightPost()
+		);
+	}
+
+
 	/**
 	 * @return the unbound goal line
 	 */
@@ -127,6 +138,15 @@ public IRectangle getRectangle()
 	}
 
 
+	public List<IVector2> getCorners()
+	{
+		return List.of(
+				leftPost.addMagnitude(Vector2.fromXY(depth, 0)),
+				rightPost.addMagnitude(Vector2.fromXY(depth, 0))
+		);
+	}
+
+
 	/**
 	 * @param point
 	 * @param margin