Spline parameterization

.vscode/launch.json

@@ -4,6 +4,13 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
+    {
+      "type": "java",
+      "name": "CodeLens (Launch) - BobTrajectoryApp",
+      "request": "launch",
+      "mainClass": "com.team319.ui.BobTrajectoryApp",
+      "projectName": "BobTrajectory"
+    },
     {
       "type": "java",
       "name": "CodeLens (Launch) - PlotterFrame",

src/main/java/com/team2363/geometry/Pose2d.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package com.team2363.geometry;
+
+public class Pose2d {
+
+    private double x;
+    private double y;
+    private double rotation;
+
+    public Pose2d(double x, double y, double rotation) {
+        this.x = x;
+        this.y = y;
+        this.rotation = rotation;
+    }
+
+    public double x() {
+        return x;
+    }
+
+    public double y() {
+        return y;
+    }
+
+    public double getRotation() {
+        return rotation;
+    }
+
+    public double getDistance(Pose2d p1) {
+        return Math.sqrt((p1.x() - x) * (p1.x() - x) + (p1.y() - y) * (p1.y() - y));
+    }
+}

src/main/java/com/team2363/spline/CartesianCubicSpline.java

@@ -0,0 +1,131 @@
+// /*----------------------------------------------------------------------------*/
+// /* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+// /* Open Source Software - may be modified and shared by FRC teams. The code   */
+// /* must be accompanied by the FIRST BSD license file in the root directory of */
+// /* the project.                                                               */
+// /*----------------------------------------------------------------------------*/
+
+// package com.team2363.spline;
+
+// import com.team2363.geometry.Pose2d;
+
+// public class CartesianCubicSpline extends Spline {
+
+//     private double a,b,c;
+//     private double dydx0, dydx1;
+//     private double x0, y0, thetaOffset, distance;
+
+// 	CartesianCubicSpline(Pose2d p0, Pose2d p1) {
+
+//         x0 = p0.x();
+//         y0 = p0.y();
+//         thetaOffset = Math.atan2(p1.y() - p0.y(), p1.x() - p0.x());
+//         distance = p0.getDistance(p1);
+
+//         dydx0 = Math.tan((p0.getRotation() - thetaOffset) % (2 * Math.PI));
+//         dydx1 = Math.tan((p1.getRotation() - thetaOffset) % (2 * Math.PI));
+
+//         a = ;
+//         b = ;
+//         c = ;
+// 	}
+
+// 	private static boolean almostEqual(double x, double y) {
+// 		return Math.abs(x - y) < 1E-6;
+// 	}
+
+// 	public static boolean reticulateSplines(double x0, double y0, double theta0, double x1, double y1, double theta1,
+// 			Spline result) {
+//         // System.out.println("Reticulating splines...");
+
+// 		// Transform x to the origin
+// 		if (distance == 0) {
+// 			return false;
+// 		}
+
+// 		double theta0_hat = ChezyMath.getDifferenceInAngleRadians(result.theta_offset_, theta0);
+// 		double theta1_hat = ChezyMath.getDifferenceInAngleRadians(result.theta_offset_, theta1);
+// 		// We cannot handle vertical slopes in our rotated, translated basis.
+// 		// This would mean the user wants to end up 90 degrees off of the straight
+// 		// line between p0 and p1.
+// 		if (almostEqual(Math.abs(theta0_hat), Math.PI / 2) || almostEqual(Math.abs(theta1_hat), Math.PI / 2)) {
+// 			return false;
+// 		}
+// 		// We also cannot handle the case that the end angle is facing towards the
+// 		// start angle (total turn > 90 degrees).
+// 		if (Math.abs(ChezyMath.getDifferenceInAngleRadians(theta0_hat, theta1_hat)) >= Math.PI / 2) {
+// 			return false;
+// 		}
+// 		// Turn angles into derivatives (slopes)
+// 		double yp0_hat = Math.tan(theta0_hat);
+// 		double yp1_hat = Math.tan(theta1_hat);
+
+// 		// Calculate the cubic spline coefficients
+// 		result.a_ = 0;
+// 		result.b_ = 0;
+// 		result.c_ = 0;
+
+// 		return true;
+// 	}
+
+// 	public double calculateLength() {
+
+// 		final int kNumSamples = 100000;
+// 		double integral = 0;
+// 		double integrand, last_integrand = Math.sqrt(1 + derivativeAt(0) * derivativeAt(0)) / kNumSamples;
+// 		for (double i = 1; i <= kNumSamples; ++i) {
+// 			double t = i / kNumSamples;
+// 			integrand = Math.sqrt(1 + dydx(t) * dydx(t)) / kNumSamples;
+// 			integral += (integrand + last_integrand) / 2;
+// 			last_integrand = integrand;
+// 		}
+
+// 		return integral;
+// 	}
+
+// 	public double calculateInput(double distance) {
+// 		final int kNumSamples = 100000;
+// 		double arc_length = 0;
+// 		double t = 0;
+// 		double last_arc_length = 0;
+// 		double dydt;
+// 		double integrand, last_integrand = Math.sqrt(1 + dydx(0) * dydx(0)) / kNumSamples;
+// 		distance /= knot_distance_;
+// 		for (double i = 1; i <= kNumSamples; ++i) {
+// 			t = i / kNumSamples;
+// 			integrand = Math.sqrt(1 + dydx(t) * dydx(t)) / kNumSamples;
+// 			arc_length += (integrand + last_integrand) / 2;
+// 			if (arc_length > distance) {
+// 				break;
+// 			}
+// 			last_integrand = integrand;
+// 			last_arc_length = arc_length;
+// 		}
+
+// 		// Interpolate between samples.
+// 		double interpolated = t;
+// 		if (arc_length != last_arc_length) {
+// 			interpolated += ((distance - last_arc_length) / (arc_length - last_arc_length) - 1) / (double) kNumSamples;
+// 		}
+// 		return interpolated;
+//     }
+
+//     public Pose2d getPoint(double t) {
+// 		double x_hat = t * distance;
+// 		double y_hat = (a_ * x_hat + b_) * x_hat * x_hat * x_hat * x_hat + c_ * x_hat * x_hat * x_hat
+// 				+ d_ * x_hat * x_hat + e_ * x_hat;
+
+// 		double cos_theta = Math.cos(thetaOffset);
+//         double sin_theta = Math.sin(thetaOffset);
+
+//         double x = x_hat * cos_theta - y_hat * sin_theta + x0;
+//         double y = x_hat * sin_theta + y_hat * cos_theta + y0;
+//         double theta = ;
+
+// 		return new Pose2d(x, y, theta);
+//     }
+
+//     private double dydx(double t) {
+//         return 3 * a * (t * distance) * (t * distance) + 2 * b * (t * distance) + c;
+//     }
+// }

src/main/java/com/team2363/spline/CartesianQuinticSpline.java

@@ -0,0 +1,29 @@
+// /*----------------------------------------------------------------------------*/
+// /* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+// /* Open Source Software - may be modified and shared by FRC teams. The code   */
+// /* must be accompanied by the FIRST BSD license file in the root directory of */
+// /* the project.                                                               */
+// /*----------------------------------------------------------------------------*/
+
+// package com.team2363.spline;
+
+// import com.team2363.geometry.Pose2d;
+
+// public class CartesianQuinticSpline extends Spline {
+
+//     CartesianQuinticSpline(Pose2d p0, Pose2d p1) {
+
+//     }
+
+//     public Pose2d getPoint(double t) {
+//         return new Pose2d(0, 0, 0);
+//     }
+
+//     public double calculateLength() {
+//         return 0;
+//     }
+
+//     public double calculateInput(double distance) {
+//         return 0;
+//     }
+// }

src/main/java/com/team2363/spline/ParametricCubicSpline.java

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package com.team2363.spline;
+
+import com.team2363.geometry.Pose2d;
+
+public class ParametricCubicSpline extends Spline {
+
+    private double vectorMagnitude;
+    private double dx0,dx1,dy0,dy1;
+    private double ax,bx,cx,dx,ay,by,cy,dy;
+
+    public ParametricCubicSpline(Pose2d p0, Pose2d p1) {
+
+        vectorMagnitude = 1.5 * p0.getDistance(p1);
+
+        dx0 = Math.cos(p0.getRotation()) * vectorMagnitude;
+        dx1 = Math.cos(p1.getRotation()) * vectorMagnitude;
+
+        dy0 = Math.sin(p0.getRotation()) * vectorMagnitude;
+        dy1 = Math.sin(p1.getRotation()) * vectorMagnitude;
+
+        ax = 2 * p0.x() - 2 * p1.x() + dx0 + dx1;
+        bx = -2 * dx0 - dx1 - 3 * p0.x() + 3 * p1.x();
+        cx = dx0;
+        dx = p0.x();
+
+        ay = 2 * p0.y() - 2 * p1.y() + dy0 + dy1;
+        by = -2 * dy0 - dy1 - 3 * p0.y() + 3 * p1.y();
+        cy = dy0;
+        dy = p0.y();
+    }
+
+    public Pose2d getPoint(double t) {
+        return new Pose2d(x(t), y(t), Math.atan2(dy(t), dx(t)));
+    }
+
+    public double arcLengthSegment(double t) {
+        return Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t));
+    }
+
+    private double x(double t) {
+        return ax * t * t * t + bx * t * t + cx * t + dx;
+    }
+
+    private double y(double t) {
+        return ay * t * t * t + by * t * t + cy * t + dy;
+    }
+
+    private double dx(double t) {
+        return 3 * ax * t * t + 2 * bx * t + cx;
+    }
+
+    private double dy(double t) {
+        return 3 * ay * t * t + 2 * by * t + cy;
+    }
+
+    public double calculateLength() {
+
+        final int kNumSamples = 100000;
+		double integral = 0;
+        double integrand, last_integrand = Math.sqrt(dx(0) * dx(0) + dy(0) * dy(0)) / kNumSamples;
+
+		for (double i = 1; i <= kNumSamples; ++i) {
+            double t = i / kNumSamples;
+			integrand = Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t)) / kNumSamples;
+			integral += (integrand + last_integrand) / 2;
+			last_integrand = integrand;
+        }
+
+		return integral;
+    }
+}

src/main/java/com/team2363/spline/ParametricQuinticSpline.java

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package com.team2363.spline;
+
+import com.team2363.geometry.Pose2d;
+import com.team254.lib.trajectory.Trajectory;
+import com.team319.ui.DraggableWaypoint;
+
+public class ParametricQuinticSpline extends Spline {
+
+    private double vectorMagnitude;
+    private double x0,x1,dx0,dx1,ddx0,ddx1,y0,y1,dy0,dy1,ddy0,ddy1;
+    private double ax,bx,cx,dx,ex,fx,ay,by,cy,dy,ey,fy;
+
+    public ParametricQuinticSpline(DraggableWaypoint w0, DraggableWaypoint w1) {
+
+        vectorMagnitude = 1.5 * w0.getDistance(w1);
+
+        x0 = w0.getX();
+        x1 = w1.getX();
+        dx0 = Math.cos(w0.getHeading()) * vectorMagnitude;
+        dx1 = Math.cos(w1.getHeading()) * vectorMagnitude;
+        ddy0 = 0;
+        ddy1 = 0;
+
+        y0 = w0.getY();
+        y1 = w1.getY();
+        dy0 = Math.sin(w0.getHeading()) * vectorMagnitude;
+        dy1 = Math.sin(w1.getHeading()) * vectorMagnitude;
+        ddy0 = 0;
+        ddy1 = 0;
+
+        calculateCoefficients();
+    }
+
+    public void calculateCoefficients() {
+        ax = 6 * (x1 - x0) - 3 * (dx0 + dx1) + 0.5 * (ddx1 - ddx0);
+        bx = 15 * x0 + 8 * dx0 + 1.5 * ddx0 - ddx1 + 7 * dx1 - 15 * x1;
+        cx = -10 * x0 - 6 * dx0 - 1.5 * ddx0 + 0.5 * ddx1 - 4 * dx1 + 10 * x1;
+        dx = 0.5 * ddx0;
+        ex = dx0;
+        fx = x0;
+
+        ay = 6 * (y1 - y0) - 3 * (dy0 + dy1) + 0.5 * (ddy1 - ddy0);;
+        by = 15 * y0 + 8 * dy0 + 1.5 * ddy0 - ddy1 + 7 * dy1 - 15 * y1;
+        cy = -10 * y0 - 6 * dy0 - 1.5 * ddy0 + 0.5 * ddy1 - 4 * dy1 + 10 * y1;
+        dy = 0.5 * ddy0;
+        ey = dy0;
+        fy = y0;
+    }
+
+    public Pose2d getPoint(double t) {
+        return new Pose2d(x(t), y(t), Math.atan2(dy(t), dx(t)));
+    }
+
+    public double arcLengthSegment(double t) {
+        return Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t));
+    }
+
+    private double x(double t) {
+        return ax * t * t * t * t * t + bx * t * t * t * t + cx * t * t * t + dx * t * t + ex * t + fx;
+    }
+
+    private double y(double t) {
+        return ay * t * t * t * t * t + by * t * t * t * t + cy * t * t * t + dy * t * t + ey * t + fy;
+    }
+
+    private double dx(double t) {
+        return 5 * ax * t * t * t * t + 4 * bx * t * t * t + 3 * cx * t * t + 2 * dx * t + ex;
+    }
+
+    private double dy(double t) {
+        return 5 * ay * t * t * t * t + 4 * by * t * t * t + 3 * cy * t * t + 2 * dy * t + ey;
+    }
+
+    public double calculateLength() {
+
+        final int kNumSamples = 100000;
+		double integral = 0;
+        double integrand, last_integrand = Math.sqrt(dx(0) * dx(0) + dy(0) * dy(0)) / kNumSamples;
+
+		for (double i = 1; i <= kNumSamples; ++i) {
+            double t = i / kNumSamples;
+			integrand = Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t)) / kNumSamples;
+			integral += (integrand + last_integrand) / 2;
+			last_integrand = integrand;
+        }
+
+		return integral;
+    }
+}