Fix headings to wrap optimally before solver

src-tauri/Cargo.toml

@@ -15,7 +15,7 @@ license = "BSD-3-Clause"
 tauri-build = { version = "1.4", features = [] }
 
 [dependencies]
-tauri = { version = "1.4", features = [ "dialog-confirm", "dialog-save", "dialog-open", "fs-all", "shell-open"] }
+tauri = { version = "1.4", features = [ "window-set-title", "path-all", "dialog-confirm", "dialog-save", "dialog-open", "fs-all", "shell-open", "devtools"] }
 serde = { version = "1.0", features = ["derive"] }
 serde_json = "1.0"
 trajoptlib = { git = "https://github.com/SleipnirGroup/TrajoptLib.git", rev = "20bb8c2ef016ceedab0dc5c0271ad9b69048039a" }

src-tauri/tauri.conf.in.json

@@ -24,6 +24,12 @@
         "confirm": true,
         "open": true,
         "save": true
+      },
+      "window": {
+        "setTitle": true
+      },
+      "path": {
+        "all": true
       }
     },
     "bundle": {

src/components/config/WaypointConfigPanel.tsx

@@ -15,6 +15,7 @@ import { WaypointData } from "../../document/UIStateStore";
 import Waypoint from "../../assets/Waypoint";
 import { Circle, CircleOutlined, Help } from "@mui/icons-material";
 import inputStyles from "../input/InputList.module.css";
+import { angleModulus } from "../../util/MathUtil";
 
 type Props = { waypoint: IHolonomicWaypointStore | null };
 
@@ -61,7 +62,7 @@ class WaypointPanel extends Component<Props, State> {
               showCheckbox={false}
               enabled={waypoint.headingConstrained}
               setEnabled={(_) => {}}
-              number={waypoint.heading}
+              number={angleModulus(waypoint.heading)}
               setNumber={(heading) => waypoint!.setHeading(heading)}
             ></Input>
             <Input

src/document/DocumentModel.tsx

@@ -68,6 +68,7 @@ const StateStore = types
           );
         }
         return new Promise((resolve, reject) => {
+          pathStore.fixWaypointHeadings();
           const controlIntervalOptResult =
             pathStore.optimizeControlIntervalCounts(self.document.robotConfig);
           if (controlIntervalOptResult !== undefined) {

src/document/HolonomicPathStore.ts

@@ -23,6 +23,7 @@ import {
 import { SavedWaypointId } from "./previousSpecs/v0_1";
 import { timeStamp } from "console";
 import { IRobotConfigStore } from "./RobotConfigStore";
+import { angleModulus } from "../util/MathUtil";
 
 export const HolonomicPathStore = types
   .model("HolonomicPathStore", {
@@ -329,6 +330,33 @@ export const HolonomicPathStore = types
           self.generating = generating;
         });
       },
+      fixWaypointHeadings() {
+        let fullRots = 0;
+        let prevHeading = 0;
+        self.waypoints.forEach((point, i, pts) => {
+          if (i == 0) {
+            prevHeading = point.heading;
+          } else {
+            if (point.headingConstrained) {
+              let prevHeadingMod = angleModulus(prevHeading);
+              let heading = pts[i].heading;
+              let headingMod = angleModulus(heading);
+              if (prevHeadingMod < 0 && headingMod > prevHeadingMod + Math.PI) {
+                // negative rollunder
+                fullRots--;
+              } else if (
+                prevHeadingMod > 0 &&
+                headingMod < prevHeadingMod - Math.PI
+              ) {
+                // positive rollover
+                fullRots++;
+              }
+              point.heading = fullRots * 2 * Math.PI + headingMod;
+              prevHeading = point.heading;
+            }
+          }
+        });
+      },
     };
   })
   .actions((self) => {

src/util/MathUtil.ts

@@ -0,0 +1,27 @@
+/**
+ * A port of WPILib's MathUtil.inputModulus
+ */
+export function inputModulus(
+  input: number,
+  maximumInput: number,
+  minimumInput: number
+) {
+  let modulus = maximumInput - minimumInput;
+
+  // Wrap input if it's above the maximum input
+  let numMax = Math.trunc((input - minimumInput) / modulus);
+  input -= numMax * modulus;
+
+  // Wrap input if it's below the minimum input
+  let numMin = Math.trunc((input - maximumInput) / modulus);
+  input -= numMin * modulus;
+
+  return input;
+}
+
+/**
+ * A port of WPILib's MathUtil.angleModulus
+ */
+export function angleModulus(input: number) {
+  return inputModulus(input, Math.PI, -Math.PI);
+}