Modified CustomCurve to allow curves that require data in addition to control points and to fix problem with getClosestPoint.

core/src/main/java/com/pedropathing/geometry/CustomCurve.java

@@ -12,6 +12,9 @@
 /**
  * This is the CustomCurve class. This class can be used to generate custom curves for the robot to follow.
  * @author Havish Sripada - 12808 RevAmped Robotics
+ *
+ * Modified by J Kenney to accomodate a wider variety of custom curves and to prevent
+ * problems that can arise when repeat following a path.
  */
 public abstract class CustomCurve implements Curve {
     protected ArrayList<Pose> controlPoints;
@@ -22,26 +25,58 @@ public abstract class CustomCurve implements Curve {
     private double[][] panelsDrawingPoints;
     protected AbstractBijectiveMap.NumericBijectiveMap completionMap = new AbstractBijectiveMap.NumericBijectiveMap();
 
-    public CustomCurve(List<Pose> controlPoints, PathConstraints pathConstraints) {
+    /*
+     * Set this flag each time the curve is initialized, to indicate that the initialTValueGuess for
+     * getClosestPoint needs to be reset. Otherwise, following the same curve a second time may not work.
+     */
+    private boolean resetClosestPointFlag = true;
+
+    /*
+     * Arbitrary data that may be needed to generate the custom curve
+     */
+    protected Object curveData;
+
+    /*
+     * Add constructors that accept arbitrary data, 'curveData' as parameters. For example, one could pass a
+     * double[2] array that specifies the beginning and ending direction of travel for a spline.
+     */
+
+    public CustomCurve(List<Pose> controlPoints, PathConstraints pathConstraints, Object curveData) {
         this.pathConstraints = pathConstraints;
         this.controlPoints = new ArrayList<>(controlPoints);
 
+        this.curveData = curveData;
+
         // Initialize the curve with the provided control points
         initialize();
     }
 
+    public CustomCurve(List<Pose> controlPoints, PathConstraints pathConstraints){
+        this(controlPoints, pathConstraints, null);
+    }
+
+    public CustomCurve(PathConstraints pathConstraints, Object curveData, Pose... controlPoints){
+        this(new ArrayList<>(Arrays.asList(controlPoints)), pathConstraints, curveData);
+    }
+
     public CustomCurve(Pose... controlPoints) {
-        this(new ArrayList<>(Arrays.asList(controlPoints)), PathConstraints.defaultConstraints);
+        this(PathConstraints.defaultConstraints, null, controlPoints);
+    }
+
+    public CustomCurve(PathConstraints pathConstraints, FuturePose... controlPoints) {
+        this(pathConstraints, null, controlPoints);
     }
 
-    public CustomCurve(FuturePose... controlPoints) {
-        this(PathConstraints.defaultConstraints, controlPoints);
+    public CustomCurve(FuturePose... controlPoints){
+        this(PathConstraints.defaultConstraints, null, controlPoints);
     }
 
-    public CustomCurve(PathConstraints constraints, FuturePose... controlPoints) {
+    public CustomCurve(PathConstraints constraints, Object curveData, FuturePose... controlPoints) {
         this.pathConstraints = constraints;
         this.controlPoints = new ArrayList<>();
 
+        this.curveData = curveData;
+
         boolean lazyInitialize = false;
         ArrayList<Pose> initializedControlPoints = new ArrayList<>();
         for (FuturePose pose : controlPoints) {
@@ -86,17 +121,26 @@ public void initialization() {
 
     @Override
     public void initialize() {
+        /*
+         * Set the flag so that the initialTValueGuess for getClosestPoint will be reset during the next
+         * call to getClosestPoint.
+         */
+        resetClosestPointFlag = true;
+
         if (initialized) return;
+
         if (controlPoints.isEmpty() && !futureControlPoints.isEmpty()) {
             // If control points are not initialized, use future control points
             for (FuturePose futurePose : futureControlPoints) {
                 controlPoints.add(futurePose.getPose());
             }
             futureControlPoints.clear();
         }
+
+        initialization();
+
         initialized = true;
         length = approximateLength();
-        initialization();
         initializePanelsDrawingPoints();
     }
 
@@ -180,4 +224,32 @@ public Vector getNormalVector(double t) {
 
         return aNormal.normalize();
     }
+
+    /*
+     * Modify the default getClosestPoint method so that the initialTValueGuess gets reset
+     * to a rough estimate of closest, if the curve has been initialized since that last call
+     * to the getClosestPoint method.
+     */
+    @Override
+    public double getClosestPoint(Pose pose, int searchLimit, double initialTValueGuess){
+        if (resetClosestPointFlag){
+            resetClosestPointFlag = false;
+            initialTValueGuess = 0.0;
+            Pose pTest = this.getPose(0);
+            double closestSquare = (pTest.getX()-pose.getX())*(pTest.getX()-pose.getX())
+                    + (pTest.getY()-pose.getY())*(pTest.getY()-pose.getY());
+            double[] searchEstimates =
+                    new double[]{0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0};
+            for (int k=0; k<searchEstimates.length; k++){
+                pTest = this.getPose(searchEstimates[k]);
+                double distSquared = (pTest.getX()-pose.getX())*(pTest.getX()-pose.getX())
+                        + (pTest.getY()-pose.getY())*(pTest.getY()-pose.getY());
+                if (distSquared < closestSquare){
+                    initialTValueGuess = searchEstimates[k];
+                    closestSquare = distSquared;
+                }
+            }
+        }
+        return Curve.super.getClosestPoint(pose, searchLimit, initialTValueGuess);
+    }
 }