working adaptable path

Author: liqyn

src/main/java/com/github/mittyrobotics/pathfollowing/Parametric.java

@@ -543,17 +543,12 @@ public Vector2D getAbsoluteMaxCoordinates(double steps) {
 
     /**
      * Return a new {@link Parametric} path to this parametric's setpoint from a position, velocity, and acceleration
-     * @param newPos {@Pose2D} to start from
-     * @param newVel {@Vector2D} velocity to start from
-     * @param newAcc {@Vector2D} acceleration to start from
+     * @param newPos {@link Pose2D} to start from
+     * @param newVel {@link Vector2D} velocity to start from
+     * @param newAcc {@link Vector2D} acceleration to start from
      * @return a new {@link Parametric} path to this parametric's setpoint from a position, velocity, and acceleration
      */
     public Parametric getNewPath(Pose2D newPos, Vector2D newVel, Vector2D newAcc) {
-        if(this instanceof QuinticHermiteSpline) {
-            //for quintic hermite splines
-            return new QuinticHermiteSpline(newPos, ((QuinticHermiteSpline) this).getPose1(), newVel,
-                    ((QuinticHermiteSpline) this).getVelocity1(), newAcc, ((QuinticHermiteSpline) this).getAcceleration1());
-        }
         return new Parametric();
     }
 

src/main/java/com/github/mittyrobotics/pathfollowing/QuinticHermiteSpline.java

@@ -255,4 +255,17 @@ public void setAcceleration1(Vector2D acceleration) {
         this.length = getGaussianQuadratureLength(17);
     }
 
+    /**
+     * Return a new {@link QuinticHermiteSpline} path to this spline's setpoint from a position, velocity, and acceleration
+     * @param newPos {@link Pose2D} to start from
+     * @param newVel {@link Vector2D} velocity to start from
+     * @param newAcc {@link Vector2D} acceleration to start from
+     * @return a new {@link QuinticHermiteSpline} path to this spline's setpoint from a position, velocity, and acceleration
+     */
+    @Override
+    public QuinticHermiteSpline getNewPath(Pose2D newPos, Vector2D newVel, Vector2D newAcc) {
+        //for quintic hermite splines
+        return new QuinticHermiteSpline(newPos, pose1, newVel, velocity1, newAcc, acceleration1);
+    }
+
 }

src/main/java/com/github/mittyrobotics/pathfollowing/QuinticHermiteSplineGroup.java

@@ -147,6 +147,13 @@ public Point2D getDerivative(double t, int n) {
         return splines.get(index).getDerivative(getSplineTFromT(t, index), n);
     }
 
+    /**
+     * Returns the closest associated t value on the spline from a {@link Point2D} using Newton's method on the distance function
+     * @param point the {@link Point2D} that to get closest point from
+     * @param steps the number of steps to start Newton's method from
+     * @param iterations the number of iterations to run Newton's method on a single step
+     * @return the closest associated t value on the spline from a {@link Point2D} using Newton's method on the distance function
+     */
     @Override
     public double findClosestPointOnSpline(Point2D point, int steps, int iterations) {
         Vector2D cur_min = new Vector2D(Double.POSITIVE_INFINITY, 0);
@@ -186,6 +193,11 @@ public double getGaussianQuadratureLength(double start, double end, int steps) {
         return length;
     }
 
+    /**
+     * Returns the t parameter associated with a certain length from the beginning
+     * @param length length to get the t parameter of
+     * @return the t parameter associated with a certain length from the beginning
+     */
     @Override
     public double getTFromLength(double length) {
         double t = 0;
@@ -202,11 +214,30 @@ public double getTFromLength(double length) {
         return t;
     }
 
+    /**
+     * Returns the index of the spline closest to a {@link Point2D}
+     * @param point {@link Point2D} to get closest spline to
+     * @param newtonsSteps the number of steps to start Newton's method from
+     * @return the index of the spline closest to a {@link Point2D}
+     */
     public int getIndexOfSplineFromPoint(Point2D point, int newtonsSteps) {
         return getSplineFromT(findClosestPointOnSpline(point, newtonsSteps, 5));
     }
 
-    public double getIndexOfSplineFromPointI(Point2D point, int newtonsSteps) {
-        return findClosestPointOnSpline(point, newtonsSteps, 5);
+    /**
+     * Return a new {@link QuinticHermiteSplineGroup} path to this spline group's setpoint from a position, velocity, and acceleration
+     * @param newPos {@link Pose2D} to start from
+     * @param newVel {@link Vector2D} velocity to start from
+     * @param newAcc {@link Vector2D} acceleration to start from
+     * @return a new {@link QuinticHermiteSplineGroup} path to this spline group's setpoint from a position, velocity, and acceleration
+     */
+    @Override
+    public QuinticHermiteSplineGroup getNewPath(Pose2D newPos, Vector2D newVel, Vector2D newAcc) {
+        int index = getIndexOfSplineFromPoint(newPos.getPosition(), 100);
+        QuinticHermiteSplineGroup group = new QuinticHermiteSplineGroup(new QuinticHermiteSpline(newPos, splines.get(index).getPose1()));
+        for(int i = index + 1; i < splines.size(); i++) {
+            group.addSpline(splines.get(i));
+        }
+        return group;
     }
 }