Resolve "Trajectory paths are incorrectly drawn in certain constellations"

Closes #1623

See merge request main/Sumatra!1362

sumatra-commit: 618d82f46be65d27b5c81f25af507cee34e4d433

modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableTrajectoryPath.java

@@ -63,8 +63,7 @@ public class DrawableTrajectoryPath implements IDrawableShape
 		{
 			IVector2 pos = trajXY.getPositionMM(t).getXYVector();
 			IVector2 vel = trajXY.getVelocity(t).getXYVector();
-			if ((vLast == null)
-					|| (vel.angleToAbs(vLast).orElse(vel.getAngle(0)) > PRECISION))
+			if (shouldAddPoint(vLast, vel))
 			{
 				points.add(pos);
 				vLast = vel;
@@ -74,6 +73,24 @@ public class DrawableTrajectoryPath implements IDrawableShape
 	}
 
 
+	private boolean shouldAddPoint(IVector2 vLast, IVector2 vCur)
+	{
+		if (vLast == null)
+		{
+			// first point
+			return true;
+		}
+
+		var vDiff = vCur.angleToAbs(vLast);
+		if (vDiff.isEmpty())
+		{
+			// vCur or vLast is zero, this only happens at the beginning or end of the trajectory
+			return true;
+		}
+		return vDiff.get() > PRECISION;
+	}
+
+
 	@Override
 	public void paintShape(final Graphics2D g, final IDrawableTool tool, final boolean invert)
 	{