diff --git a/modules/common-gui/src/main/resources/reset.png b/modules/common-gui/src/main/resources/reset.png new file mode 100644 index 00000000..828c978a Binary files /dev/null and b/modules/common-gui/src/main/resources/reset.png differ diff --git a/modules/common/src/main/java/edu/tigers/sumatra/drawable/ColorPickerFactory.java b/modules/common/src/main/java/edu/tigers/sumatra/drawable/ColorPickerFactory.java index 3585db2d..0ea83757 100644 --- a/modules/common/src/main/java/edu/tigers/sumatra/drawable/ColorPickerFactory.java +++ b/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 colors = new ArrayList<>(); + private static final List 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)); } diff --git a/modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableAnnotation.java b/modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableAnnotation.java index c95ed492..3a00f24b 100644 --- a/modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableAnnotation.java +++ b/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; diff --git a/modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableGrid.java b/modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableGrid.java index d5121a5b..1cd7b612 100644 --- a/modules/common/src/main/java/edu/tigers/sumatra/drawable/DrawableGrid.java +++ b/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); } } diff --git a/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AMovingRobot.java b/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AMovingRobot.java index 4d34a7c2..9ec01736 100644 --- a/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AMovingRobot.java +++ b/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); } diff --git a/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AcceleratingRobot.java b/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AcceleratingRobot.java index c5600663..b0536820 100644 --- a/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/AcceleratingRobot.java +++ b/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) ); } } diff --git a/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/StoppingRobot.java b/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/StoppingRobot.java index 396acc0b..3c9251c9 100644 --- a/modules/common/src/main/java/edu/tigers/sumatra/movingrobot/StoppingRobot.java +++ b/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 ); } diff --git a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/BorderVisCalc.java b/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/BorderVisCalc.java index ae013311..5ede9f07 100644 --- a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/BorderVisCalc.java +++ b/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))); diff --git a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/EWpShapesLayer.java b/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/EWpShapesLayer.java index a490cf09..69b4a3f9 100644 --- a/modules/moduli-wp/src/main/java/edu/tigers/sumatra/wp/vis/EWpShapesLayer.java +++ b/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( diff --git a/modules/sumatra-model/src/main/java/edu/tigers/sumatra/model/ModuliStateAdapter.java b/modules/sumatra-model/src/main/java/edu/tigers/sumatra/model/ModuliStateAdapter.java index 02a1da03..ae18d3a1 100644 --- a/modules/sumatra-model/src/main/java/edu/tigers/sumatra/model/ModuliStateAdapter.java +++ b/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); } }); }