Add robot outline and left/right paths

src/main/java/edu/wpi/first/pathweaver/CreateProjectController.java

@@ -54,6 +54,10 @@ public class CreateProjectController {
 	@FXML
 	private TextField trackWidth;
 	@FXML
+	private TextField robotWidth;
+	@FXML
+	private TextField robotLength;
+	@FXML
 	private ChoiceBox<Game> game;
 	@FXML
 	private ChoiceBox<Unit<Length>> length;
@@ -70,19 +74,27 @@ public class CreateProjectController {
 	@FXML
 	private Label trackWidthLabel;
 	@FXML
+	private Label robotWidthLabel;
+	@FXML
+	private Label robotLengthLabel;
+	@FXML
 	private Label velocityUnits;
 	@FXML
 	private Label accelerationUnits;
 	@FXML
 	private Label trackWidthUnits;
+	@FXML
+	private Label robotWidthUnits;
+	@FXML
+	private Label robotLengthUnits;
 
 	private boolean editing = false;
 
 	@FXML
 
 	private void initialize() {
 		ObservableList<TextField> numericFields = FXCollections.observableArrayList(maxVelocity,
-				maxAcceleration, trackWidth);
+				maxAcceleration, trackWidth, robotWidth, robotLength);
 		ObservableList<TextField> allFields = FXCollections.observableArrayList(numericFields);
 		allFields.add(directory);
 
@@ -91,6 +103,8 @@ private void initialize() {
 		var velocityControls = List.of(velocityLabel, maxVelocity, velocityUnits);
 		var accelerationControls = List.of(accelerationLabel, maxAcceleration, accelerationUnits);
 		var trackWidthControls = List.of(trackWidthLabel, trackWidth, trackWidthUnits);
+		var robotWidthControls = List.of(robotWidthLabel, robotWidth, robotWidthUnits);
+		var robotLengthControls = List.of(robotLengthLabel, robotLength, robotLengthUnits);
 
 		BooleanBinding bind = new SimpleBooleanProperty(true).not();
 		for (TextField field : allFields) {
@@ -167,12 +181,18 @@ public ProjectPreferences.ExportUnit fromString(String string) {
 		trackWidthControls.forEach(
 				control -> control.setTooltip(new Tooltip("The width between the center of each tire of the " +
 						"drivebase.  Even better would be a calculated track width from robot characterization.")));
+		robotWidthControls.forEach(
+			control -> control.setTooltip(new Tooltip("The width of the robot - including bumpers.")));
+		robotLengthControls.forEach(
+			control -> control.setTooltip(new Tooltip("The length of the robot - including bumpers.")));
 		trackWidthUnits.textProperty().bind(lengthUnit.map(SimpleUnitFormat.getInstance()::format));
-		// Show longer text for an extended period of time
+		robotWidthUnits.textProperty().bind(lengthUnit.map(SimpleUnitFormat.getInstance()::format));
+		robotLengthUnits.textProperty().bind(lengthUnit.map(SimpleUnitFormat.getInstance()::format));
+			// Show longer text for an extended period of time
 		Stream.of(directoryControls, outputControls).flatMap(List::stream)
 				.forEach(control -> control.getTooltip().setShowDuration(Duration.seconds(10)));
 		Stream.of(directoryControls, outputControls, velocityControls, accelerationControls,
-				trackWidthControls).flatMap(List::stream)
+				trackWidthControls, robotWidthControls, robotLengthControls).flatMap(List::stream)
 				.forEach(control -> control.getTooltip().setShowDelay(Duration.millis(150)));
 
 		// We are editing a project
@@ -197,6 +217,8 @@ private void setupCreateProject() {
 		maxVelocity.setText("");
 		maxAcceleration.setText("");
 		trackWidth.setText("");
+		robotWidth.setText("");
+		robotLength.setText("");
 		editing = false;
 	}
 
@@ -222,8 +244,11 @@ private void createProject() {
 		double velocityMax = Double.parseDouble(maxVelocity.getText());
 		double accelerationMax = Double.parseDouble(maxAcceleration.getText());
 		double trackWidthDistance = Double.parseDouble(trackWidth.getText());
+		double robotWidthDistance = Double.parseDouble(robotWidth.getText());
+		double robotLengthDistance = Double.parseDouble(robotLength.getText());
 		ProjectPreferences.Values values = new ProjectPreferences.Values(lengthUnit, exportUnit, velocityMax,
-				accelerationMax, trackWidthDistance, game.getValue().getName(), outputPath);
+				accelerationMax, trackWidthDistance, game.getValue().getName(), outputPath, robotWidthDistance,
+				robotLengthDistance);
 		ProjectPreferences prefs = ProjectPreferences.getInstance(directory.getAbsolutePath());
 		prefs.setValues(values);
 		editing = false;
@@ -270,6 +295,8 @@ private void setupEditProject() {
 		maxVelocity.setText(String.valueOf(values.getMaxVelocity()));
 		maxAcceleration.setText(String.valueOf(values.getMaxAcceleration()));
 		trackWidth.setText(String.valueOf(values.getTrackWidth()));
+		robotWidth.setText(String.valueOf(values.getRobotWidth()));
+		robotLength.setText(String.valueOf(values.getRobotLength()));
 		editing = true;
 	}
 }

src/main/java/edu/wpi/first/pathweaver/EditWaypointController.java

@@ -167,13 +167,13 @@ private void enableSaving(ObservableValue<Waypoint> wp) {
 
     lockedTangent.selectedProperty()
             .addListener((listener, oldValue, newValue) -> {
-              if (wp.getValue().isLockTangent() != newValue) {
+              if (wp.getValue() != null && wp.getValue().isLockTangent() != newValue) {
                 SaveManager.getInstance().addChange(CurrentSelections.getCurPath());
               }
             });
     reverseSpline.selectedProperty()
             .addListener((listener, oldValue, newValue) -> {
-              if (wp.getValue().isReversed() != newValue) {
+              if (wp.getValue() != null && wp.getValue().isReversed() != newValue) {
                 SaveManager.getInstance().addChange(CurrentSelections.getCurPath());
               }
             });

src/main/java/edu/wpi/first/pathweaver/FieldDisplayController.java

@@ -60,6 +60,7 @@ private void initialize() {
         this.drawPane.setOnMouseClicked(e -> {
             if (CurrentSelections.getCurWaypoint() != null) {
                 CurrentSelections.getCurWaypoint().getIcon().pseudoClassStateChanged(SELECTED_CLASS, false);
+                CurrentSelections.getCurWaypoint().getRobotOutline().pseudoClassStateChanged(SELECTED_CLASS, false);
                 CurrentSelections.setCurWaypoint(null);
             }
 

src/main/java/edu/wpi/first/pathweaver/MainController.java

@@ -364,7 +364,7 @@ private void buildPaths() {
 
       java.nio.file.Path pathNameFile = output.resolve(path.getPathNameNoExtension());
 
-      if(!path.getSpline().writeToFile(pathNameFile)) {
+      if(!path.getCenterSpline().writeToFile(pathNameFile)) {
         Alert alert = new Alert(Alert.AlertType.WARNING);
         FxUtils.applyDarkMode(alert);
         alert.setTitle("Path export failure!");

src/main/java/edu/wpi/first/pathweaver/ProjectPreferences.java

@@ -103,7 +103,7 @@ private ProjectPreferences(String directory) {
 	}
 
 	private void setDefaults() {
-		values = new Values("FOOT", "Always Meters", 10.0, 60.0, 2.0, Game.DEFAULT_GAME.getName(), null);
+		values = new Values("FOOT", "Always Meters", 10.0, 60.0, 2.0, Game.DEFAULT_GAME.getName(), null, 3.0, 3.0);
 		updateValues();
 	}
 
@@ -244,6 +244,8 @@ public static class Values {
 		private final double trackWidth;
 		private String gameName;
 		private final String outputDir;
+		private final double robotWidth;
+		private final double robotLength;
 
 		/**
 		 * Constructor for Values of ProjectPreferences.
@@ -261,16 +263,22 @@ public static class Values {
 		 *            The year/FRC game
 		 * @param outputDir
 		 *            The directory for the output files
+		 * @param robotWidth
+		 * 	          The width of the robot - including bumpers
+		 * @param robotLength
+		 * 	          The length of the robot - including bumpers
 		 */
 		public Values(String lengthUnit, String exportUnit, double maxVelocity, double maxAcceleration,
-				double trackWidth, String gameName, String outputDir) {
+				double trackWidth, String gameName, String outputDir, double robotWidth, double robotLength) {
 			this.lengthUnit = lengthUnit;
 			this.exportUnit = exportUnit;
 			this.maxVelocity = maxVelocity;
 			this.maxAcceleration = maxAcceleration;
 			this.trackWidth = trackWidth;
 			this.gameName = gameName;
 			this.outputDir = outputDir;
+			this.robotWidth = robotWidth;
+			this.robotLength = robotLength;
 		}
 
 		public Unit<Length> getLengthUnit() {
@@ -300,5 +308,13 @@ public String getGameName() {
 		public String getOutputDir() {
 			return outputDir;
 		}
+
+		public double getRobotWidth() {
+			return robotWidth;
+		}
+
+		public double getRobotLength() {
+			return robotLength;
+		}
 	}
 }

src/main/java/edu/wpi/first/pathweaver/Waypoint.java

@@ -36,6 +36,7 @@ public class Waypoint {
 
 	private final Line tangentLine;
 	private final Polygon icon;
+	private final Polygon robotOutline;
 
 	/**
 	 * Creates Waypoint object containing javafx circle.
@@ -53,6 +54,15 @@ public Waypoint(Point2D position, Point2D tangentVector, boolean fixedAngle, boo
 		reversed.set(reverse);
 		setCoords(position);
 
+		double scale = ProjectPreferences.getInstance().getField().getScale();
+		double robotWidth = ProjectPreferences.getInstance().getValues().getRobotWidth() * scale;
+		double robotLength = ProjectPreferences.getInstance().getValues().getRobotLength() * scale;
+		robotOutline = new Polygon(-robotLength / 2, -robotWidth / 2,
+			robotLength / 2, -robotWidth / 2,
+			robotLength / 2, robotWidth / 2,
+			-robotLength / 2, robotWidth / 2);
+		setupRobotOutline();
+
 		icon = new Polygon(0.0, SIZE / 3, SIZE, 0.0, 0.0, -SIZE / 3);
 		setupIcon();
 
@@ -71,6 +81,8 @@ public Waypoint(Point2D position, Point2D tangentVector, boolean fixedAngle, boo
 	public void enableSubchildSelector(int i) {
 		FxUtils.enableSubchildSelector(this.icon, i);
 		getIcon().applyCss();
+		FxUtils.enableSubchildSelector(this.robotOutline, i);
+		getRobotOutline().applyCss();
 	}
 
 	private void setupIcon() {
@@ -88,6 +100,21 @@ private void setupIcon() {
 		icon.getStyleClass().add("waypoint");
 	}
 
+	private void setupRobotOutline() {
+		robotOutline.setLayoutX(-(robotOutline.getLayoutBounds().getMaxX() + robotOutline.getLayoutBounds().getMinX()) / 2);
+		robotOutline.setLayoutY(-(robotOutline.getLayoutBounds().getMaxY() + robotOutline.getLayoutBounds().getMinY()) / 2);
+
+		robotOutline.translateXProperty().bind(x);
+		//Convert from WPILib to JavaFX coords
+		robotOutline.translateYProperty().bind(y.negate());
+		FxUtils.applySubchildClasses(this.robotOutline);
+		this.robotOutline.rotateProperty()
+				.bind(Bindings.createObjectBinding(
+						() -> getTangent() == null ? 0.0 : Math.toDegrees(Math.atan2(-getTangentY(), getTangentX())),
+						tangentX, tangentY));
+		robotOutline.getStyleClass().add("robot");
+	}
+
 	/**
 	 * Convenience function for math purposes.
 	 *
@@ -158,6 +185,10 @@ public Polygon getIcon() {
 		return icon;
 	}
 
+	public Polygon getRobotOutline() {
+		return robotOutline;
+	}
+
 	public double getX() {
 		return x.get();
 	}

src/main/java/edu/wpi/first/pathweaver/path/Path.java

@@ -47,13 +47,16 @@ public abstract class Path {
     protected static final PseudoClass SELECTED_CLASS = PseudoClass.getPseudoClass("selected");
     protected static final double DEFAULT_SPLINE_SCALE = 6;
     protected static final double DEFAULT_CIRCLE_SCALE = .75;
+    protected static final double DEFAULT_ROBOT_SCALE = 1;
     protected static final double DEFAULT_LINE_SCALE = 4;
 
     protected final Field field = ProjectPreferences.getInstance().getField();
     protected final ObservableList<Waypoint> waypoints = new ObservableListWrapper<>(new ArrayList<>());
     protected Group mainGroup = new Group();
 
-    protected final Spline spline;
+    protected final Spline centerSpline;
+    protected final Spline leftSpline;
+    protected final Spline rightSpline;
     protected final String pathName;
     protected int subchildIdx = 0;
 
@@ -63,7 +66,9 @@ public abstract class Path {
      * @param pathName the name of the path
      */
     protected Path(SplineFactory splineFactory, String pathName) {
-        this.spline = splineFactory.makeSpline(waypoints, this);
+        this.centerSpline = splineFactory.makeSpline(waypoints, this);
+        this.leftSpline = splineFactory.makeSpline(waypoints, this);
+        this.rightSpline = splineFactory.makeSpline(waypoints, this);
         this.pathName = Objects.requireNonNull(pathName);
     }
 
@@ -96,7 +101,7 @@ public final void convertUnit(Unit<Length> from, Unit<Length> to) {
      * Updates this path to reflect new waypoint data.
      */
     public void update() {
-        spline.update();
+        centerSpline.update();
     }
 
     public final Waypoint getStart() {
@@ -107,8 +112,8 @@ public final Waypoint getEnd() {
         return waypoints.get(waypoints.size() - 1);
     }
 
-    public final Spline getSpline() {
-        return spline;
+    public final Spline getCenterSpline() {
+        return centerSpline;
     }
 
     public final Group getMainGroup() {
@@ -180,7 +185,7 @@ public void enableSubchildSelector(int i) {
         for (Waypoint wp : waypoints) {
             wp.enableSubchildSelector(subchildIdx);
         }
-        spline.enableSubchildSelector(subchildIdx);
+        centerSpline.enableSubchildSelector(subchildIdx);
     }
 
     /**
@@ -209,6 +214,9 @@ public void selectWaypoint(Waypoint waypoint) {
         waypoint.getIcon().pseudoClassStateChanged(SELECTED_CLASS, true);
         waypoint.getIcon().requestFocus();
         waypoint.getIcon().toFront();
+        waypoint.getRobotOutline().pseudoClassStateChanged(SELECTED_CLASS, true);
+        waypoint.getRobotOutline().requestFocus();
+        waypoint.getRobotOutline().toFront();
         CurrentSelections.setCurWaypoint(waypoint);
         CurrentSelections.setCurPath(this);
     }
@@ -222,6 +230,7 @@ public void deselectWaypoint(Waypoint waypoint) {
         Waypoint curWaypoint = CurrentSelections.getCurWaypoint();
         if (CurrentSelections.getCurWaypoint() == waypoint) {
             curWaypoint.getIcon().pseudoClassStateChanged(SELECTED_CLASS, false);
+            curWaypoint.getRobotOutline().pseudoClassStateChanged(SELECTED_CLASS, false);
             mainGroup.requestFocus();
             CurrentSelections.setCurWaypoint(null);
         }