Pass interception improvements

See merge request main/Sumatra!1897

sumatra-commit: 10b52e6f891b87985cce9bf95461d60ffb2db3f9

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

@@ -3,93 +3,49 @@
  */
 package edu.tigers.sumatra.drawable;
 
-import org.apache.commons.lang.Validate;
+import edu.tigers.sumatra.math.SumatraMath;
+import lombok.AccessLevel;
+import lombok.NoArgsConstructor;
 
 import java.awt.AlphaComposite;
 import java.awt.Color;
 import java.awt.Graphics2D;
-import java.util.ArrayList;
 import java.util.List;
 
 
 /**
  * Create color pickers
  */
+@NoArgsConstructor(access = AccessLevel.PRIVATE)
 public final class ColorPickerFactory
 {
-	private static final List<Color> colors = new ArrayList<>();
+	private static final List<Color> COLORS = List.of(
+			new Color(0xba55d3),
+			new Color(0x1e90ff),
+			new Color(0xfa8072),
+			new Color(0xdda0dd),
+			new Color(0xff1493),
+			new Color(0x98fb98),
+			new Color(0x87cefa),
+			new Color(0xffe4b5),
+			new Color(0x2f4f4f),
+			new Color(0x2e8b57),
+			new Color(0x800000),
+			new Color(0x808000),
+			new Color(0x000080),
+			new Color(0xffa500)
+	);
 
-	static
-	{
-		colors.add(new Color(0xE11408));
-		colors.add(new Color(0xE11C08));
-		colors.add(new Color(0xE12308));
-		colors.add(new Color(0xE12A08));
-		colors.add(new Color(0xE13108));
-		colors.add(new Color(0xE13908));
-		colors.add(new Color(0xE14008));
-		colors.add(new Color(0xE14708));
-		colors.add(new Color(0xE14E07));
-		colors.add(new Color(0xE15607));
-		colors.add(new Color(0xE15D07));
-		colors.add(new Color(0xE16407));
-		colors.add(new Color(0xE16B07));
-		colors.add(new Color(0xE17307));
-		colors.add(new Color(0xE17A07));
-		colors.add(new Color(0xE18206));
-		colors.add(new Color(0xE28906));
-		colors.add(new Color(0xE29006));
-		colors.add(new Color(0xE29806));
-		colors.add(new Color(0xE29F06));
-		colors.add(new Color(0xE2A706));
-		colors.add(new Color(0xE2AE06));
-		colors.add(new Color(0xE2B505));
-		colors.add(new Color(0xE2BD05));
-		colors.add(new Color(0xE2C405));
-		colors.add(new Color(0xE2CC05));
-		colors.add(new Color(0xE2D305));
-		colors.add(new Color(0xE2DB05));
-		colors.add(new Color(0xE2E205));
-		colors.add(new Color(0xDBE204));
-		colors.add(new Color(0xD3E204));
-		colors.add(new Color(0xCCE204));
-		colors.add(new Color(0xC4E304));
-		colors.add(new Color(0xBDE304));
-		colors.add(new Color(0xB5E304));
-		colors.add(new Color(0xAEE304));
-		colors.add(new Color(0xA6E303));
-		colors.add(new Color(0x9FE303));
-		colors.add(new Color(0x97E303));
-		colors.add(new Color(0x8FE303));
-		colors.add(new Color(0x88E303));
-		colors.add(new Color(0x80E303));
-		colors.add(new Color(0x79E303));
-		colors.add(new Color(0x71E302));
-		colors.add(new Color(0x69E302));
-		colors.add(new Color(0x62E302));
-		colors.add(new Color(0x5AE302));
-		colors.add(new Color(0x52E302));
-		colors.add(new Color(0x4BE402));
-		colors.add(new Color(0x43E402));
-		colors.add(new Color(0x3BE401));
-		colors.add(new Color(0x33E401));
-		colors.add(new Color(0x2CE401));
-		colors.add(new Color(0x24E401));
-		colors.add(new Color(0x1CE401));
-		colors.add(new Color(0x14E401));
-		colors.add(new Color(0x0DE401));
-		colors.add(new Color(0x05E400));
-		colors.add(new Color(0x00E404));
-		colors.add(new Color(0x00E40B));
-		colors.add(new Color(0x00E413));
-		colors.add(new Color(0x00E41A));
-		colors.add(new Color(0x00E422));
-		colors.add(new Color(0x00E52A));
-	}
 
-
-	private ColorPickerFactory()
+	/**
+	 * Get a color from a static color map with visually distinct colors.
+	 *
+	 * @param index
+	 * @return
+	 */
+	public static Color getDistinctColor(final int index)
 	{
+		return COLORS.get(index % COLORS.size());
 	}
 
 
@@ -185,12 +141,19 @@ public Color applyColor(final Graphics2D g, final double relValue)
 			@Override
 			public Color getColor(final double relValue)
 			{
-				Validate.isTrue(relValue >= 0, "relValue must be greater or equal to zero: ", relValue);
-				Validate.isTrue(relValue <= 1, "relValue must be smaller or equal to one: ", relValue);
-				int red = (int) ((color2.getRed() * relValue) + (color1.getRed() * (1 - relValue)));
-				int green = (int) ((color2.getGreen() * relValue) + (color1.getGreen() * (1 - relValue)));
-				int blue = (int) ((color2.getBlue() * relValue) + (color1.getBlue() * (1 - relValue)));
-				return new Color(red, green, blue);
+				float value = (float) SumatraMath.cap(relValue, 0, 1);
+
+				float[] hsb1 = new float[3];
+				float[] hsb2 = new float[3];
+				Color.RGBtoHSB(color1.getRed(), color1.getGreen(), color1.getBlue(), hsb1);
+				Color.RGBtoHSB(color2.getRed(), color2.getGreen(), color2.getBlue(), hsb2);
+
+				float[] hsb = new float[3];
+				for (int i = 0; i < 3; i++)
+				{
+					hsb[i] = (hsb2[i] * value) + (hsb1[i] * (1 - value));
+				}
+				return Color.getHSBColor(hsb[0], hsb[1], hsb[2]);
 			}
 		};
 	}
@@ -232,39 +195,7 @@ public Color getColor(final double relValue)
 	 */
 	public static IColorPicker greenRedGradient()
 	{
-		return new IColorPicker()
-		{
-
-			@Override
-			public Color getColor(final double relValue)
-			{
-				return gradientFromList(relValue);
-			}
-
-
-			@Override
-			public Color applyColor(final Graphics2D g, final double relValue)
-			{
-				Color c = getColor(relValue);
-				g.setColor(c);
-				return c;
-			}
-		};
-	}
-
-
-	private static Color gradientFromList(final double relValue)
-	{
-		double step = 1.0 / colors.size();
-		for (int i = 0; i < colors.size(); i++)
-		{
-			double val = (i + 1) * step;
-			if (relValue <= val)
-			{
-				return colors.get(i);
-			}
-		}
-		return Color.black;
+		return ColorPickerFactory.scaledDouble(new Color(0xE11408), new Color(0x00E52A));
 	}
 
 

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

@@ -29,7 +29,7 @@ public class DrawableAnnotation implements IDrawableShape
 	private IVector2 offset = Vector2f.ZERO_VECTOR;
 	private boolean centerHorizontally = false;
 	private Color color = Color.BLACK;
-	private int fontHeight = 50;
+	private double fontHeight = 50;
 	private boolean bold = false;
 
 
@@ -81,24 +81,13 @@ public void paintShape(final Graphics2D g, final IDrawableTool tool, final boole
 		double drawingX = transPoint.x() + tool.scaleGlobalToGui(offset.x());
 		double drawingY = transPoint.y() + tool.scaleGlobalToGui(offset.y());
 
-		drawingY += (textHeight / 2) - g.getFontMetrics(font).getDescent();
-
-		if (offset.x() < 0)
-		{
-			drawingX -= maxWidth;
-		}
 
 		if (centerHorizontally)
 		{
-			if (offset.x() < 0)
-			{
-				drawingX += maxWidth / 2;
-			} else
-			{
-				drawingX -= maxWidth / 2;
-			}
+			drawingX += -maxWidth / 2;
 		}
 
+		drawingY += (textHeight / 2) - g.getFontMetrics(font).getDescent();
 		drawingY -= (numLines - 1) * lineHeight;
 
 		for (String txt : lines)
@@ -124,7 +113,7 @@ public final DrawableAnnotation setColor(final Color color)
 	 * @param fontHeight the fontHeight to set in [mm]
 	 * @return
 	 */
-	public final DrawableAnnotation withFontHeight(final int fontHeight)
+	public final DrawableAnnotation withFontHeight(final double fontHeight)
 	{
 		this.fontHeight = fontHeight;
 		return this;

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

@@ -125,6 +125,7 @@ private IDrawableShape createNumber(IVector3 point)
 		}
 		String text = DF.format(number);
 		return new DrawableAnnotation(point.getXYVector(), text)
+				.withFontHeight(cellExtentX / 2)
 				.withCenterHorizontally(true);
 	}
 }

modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AMovingRobot.java

@@ -19,13 +19,13 @@ public abstract class AMovingRobot implements IMovingRobot
 	private final IVector2 dir;
 	private final double radius;
 	protected final double speed;
-	protected final double reactionTime;
+	private final double reactionDuration;
 
 
 	@Override
 	public ICircle getMovingHorizon(final double tHorizon, double tAdditionalReaction)
 	{
-		var p = forwardBackwardOffset(tHorizon, tAdditionalReaction);
+		var p = forwardBackwardOffsetWithReaction(tHorizon, tAdditionalReaction);
 
 		double dynamicRadius = Math.abs(p.forward() - p.backward()) / 2;
 		IVector2 center = pos.addNew(dir.multiplyNew(p.backward() + dynamicRadius));
@@ -36,7 +36,7 @@ public ICircle getMovingHorizon(final double tHorizon, double tAdditionalReactio
 	@Override
 	public ITube getMovingHorizonTube(final double tHorizon, double tAdditionalReaction)
 	{
-		var p = forwardBackwardOffset(tHorizon, tAdditionalReaction);
+		var p = forwardBackwardOffsetWithReaction(tHorizon, tAdditionalReaction);
 
 		return Tube.create(
 				dir.multiplyNew(p.backward()).add(pos),
@@ -46,6 +46,20 @@ public ITube getMovingHorizonTube(final double tHorizon, double tAdditionalReact
 	}
 
 
+	private MovingOffsets forwardBackwardOffsetWithReaction(double tHorizon, double additionalReactionDuration)
+	{
+		double tReaction = Math.min(reactionDuration + additionalReactionDuration, tHorizon);
+		double t = Math.max(0, tHorizon - tReaction);
+		double distReaction = speed * tReaction * 1000;
+
+		var offset = forwardBackwardOffset(t);
+		return new MovingOffsets(
+				distReaction + offset.forward(),
+				distReaction + offset.backward()
+		);
+	}
+
+
 	@Override
 	public IVector2 getPos()
 	{
@@ -61,10 +75,9 @@ public double getSpeed()
 
 
 	/**
-	 * @param tHorizon            time horizon in seconds
-	 * @param tAdditionalReaction additional reaction time in seconds
+	 * @param t time horizon in seconds
 	 * @return moving offsets
 	 */
-	abstract MovingOffsets forwardBackwardOffset(double tHorizon, double tAdditionalReaction);
+	abstract MovingOffsets forwardBackwardOffset(double t);
 
 }

modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AcceleratingRobot.java

@@ -33,14 +33,11 @@ public class AcceleratingRobot extends AMovingRobot
 
 
 	@Override
-	MovingOffsets forwardBackwardOffset(double tHorizon, double tAdditionalReaction)
+	MovingOffsets forwardBackwardOffset(double t)
 	{
-		double tReaction = Math.min(reactionTime + tAdditionalReaction, tHorizon);
-
-		double distReaction = speed * tReaction * 1000;
 		return new MovingOffsets(
-				distReaction + trajectoryForward.getPositionMM(tHorizon - tReaction),
-				distReaction + trajectoryBackward.getPositionMM(tHorizon - tReaction)
+				trajectoryForward.getPositionMM(t),
+				trajectoryBackward.getPositionMM(t)
 		);
 	}
 }

modules/common/src/main/java/edu/tigers/sumatra/movingrobot/StoppingRobot.java

@@ -36,13 +36,8 @@ public class StoppingRobot extends AMovingRobot
 
 
 	@Override
-	MovingOffsets forwardBackwardOffset(double tHorizon, double tAdditionalReaction)
+	MovingOffsets forwardBackwardOffset(double t)
 	{
-		double tReaction = Math.min(reactionTime + tAdditionalReaction, tHorizon);
-
-		double t = tHorizon - tReaction;
-
-		double distReaction = speed * tReaction;
 		double tBrake = speed / brkLimit;
 
 		if (tBrake > t)
@@ -52,15 +47,17 @@ MovingOffsets forwardBackwardOffset(double tHorizon, double tAdditionalReaction)
 			double vAfterT = speed - dvAfterT;
 			double distBrakePartially = vAfterT * t + 0.5 * dvAfterT * t;
 			return new MovingOffsets(
-					(distReaction + distBrakePartially) * 1000,
-					(distReaction + distBrakePartially) * 1000
+					distBrakePartially * 1000,
+					distBrakePartially * 1000
 			);
 		}
 
 		double distBrake = 0.5 * speed * tBrake;
+		double forward = distance(speed, t);
+		double backward = distBrake - distance(0, t - tBrake);
 		return new MovingOffsets(
-				(distReaction + distance(speed, t)) * 1000,
-				(distReaction + distBrake - distance(0, t - tBrake)) * 1000
+				forward * 1000,
+				backward * 1000
 		);
 	}
 

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/BorderVisCalc.java

@@ -41,7 +41,8 @@ public void process(final WorldFrameWrapper wfw, final ShapeMap shapeMap)
 		var lengthHalf = Geometry.getFieldLength() / 2.0;
 		var lengthQuarter = Geometry.getFieldLength() / 4.0;
 
-		shapes.add(new DrawableFieldBackground(Geometry.getField(), Geometry.getBoundaryWidth()));
+		shapeMap.get(EWpShapesLayer.FIELD_BACKGROUND)
+				.add(new DrawableFieldBackground(Geometry.getField(), Geometry.getBoundaryWidth()));
 		drawLine(shapes, new DrawableRectangle(Geometry.getField()));
 		drawLine(shapes, new DrawableCircle(Geometry.getCenterCircle()));
 		drawLine(shapes, new DrawableLine(Vector2.fromY(-widthHalf), Vector2.fromY(widthHalf)));

modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/EWpShapesLayer.java

@@ -25,6 +25,8 @@ public final class EWpShapesLayer
 	private static final String CAT_BALL = "Ball";
 	private static final String BOT_STATES = "States";
 
+	public static final IShapeLayerIdentifier FIELD_BACKGROUND = F.create(
+			F.category(FIELD).category(FIELD_LINES).layerName("Background").visibleByDefault(true).orderId(-500));
 	public static final IShapeLayerIdentifier FIELD_LINES_REGULAR = F.create(
 			F.category(FIELD).category(FIELD_LINES).layerName("Regular").visibleByDefault(true).orderId(-100));
 	public static final IShapeLayerIdentifier FIELD_LINES_ADDITIONAL = F.create(

modules/sumatra-model/src/main/java/edu/tigers/sumatra/model/ModuliStateAdapter.java

@@ -9,7 +9,6 @@
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
 
-import javax.swing.SwingUtilities;
 import java.beans.PropertyChangeEvent;
 import java.beans.PropertyChangeListener;
 import java.util.List;
@@ -75,11 +74,11 @@ public void propertyChange(final PropertyChangeEvent evt)
 			observers.stream().parallel().forEach(o -> {
 				try
 				{
-					log.trace("Notify {}", o.getClass());
-					SwingUtilities.invokeLater(() -> o.onModuliStateChanged(newState));
+					log.trace("Notify {} about new state {}", o.getClass(), newState);
+					o.onModuliStateChanged(newState);
 				} catch (Exception err)
 				{
-					log.error("Exception while changing moduli state in class " + o.getClass().getName(), err);
+					log.error("Exception while changing moduli state in class {}", o.getClass().getName(), err);
 				}
 			});
 		}