diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java index 1e64bfd95c..44b549723e 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java @@ -80,6 +80,26 @@ public class CubicHermiteSpline extends Spline { */ private SimpleMatrix makeHermiteBasis() { if (hermiteBasis == null) { + // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find + // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0. + // + // P(i) = P(0) = a0 + // P'(i) = P'(0) = a1 + // P(i+1) = P(1) = a3 + a2 + a1 + a0 + // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1 + // + // [ P(i) ] = [ 0 0 0 1 ][ a3 ] + // [ P'(i) ] = [ 0 0 1 0 ][ a2 ] + // [ P(i+1) ] = [ 1 1 1 1 ][ a1 ] + // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ] + // + // To solve for the coefficients, we can invert the 4x4 matrix and move it + // to the other side of the equation. + // + // [ a3 ] = [ 2 1 -2 1 ][ P(i) ] + // [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ] + // [ a1 ] = [ 0 1 0 0 ][ P(i+1) ] + // [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ] hermiteBasis = new SimpleMatrix( 4, diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java index 4f832103b0..5378c4f9b2 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java @@ -80,6 +80,33 @@ public class QuinticHermiteSpline extends Spline { */ private SimpleMatrix makeHermiteBasis() { if (hermiteBasis == null) { + // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control + // vectors, we want to find the coefficients of the spline + // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0. + // + // P(i) = P(0) = a0 + // P'(i) = P'(0) = a1 + // P''(i) = P''(0) = 2 * a2 + // P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0 + // P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1 + // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2 + // + // [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ] + // [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ] + // [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ] + // [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ] + // [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ] + // [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ] + // + // To solve for the coefficients, we can invert the 6x6 matrix and move it + // to the other side of the equation. + // + // [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ] + // [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ] + // [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ] + // [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ] + // [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ] + // [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ] hermiteBasis = new SimpleMatrix( 6, diff --git a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h index c30a8a89a8..b908bb2e50 100644 --- a/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -52,6 +52,27 @@ class CubicHermiteSpline : public Spline<3> { * @return The hermite basis matrix for cubic hermite spline interpolation. */ static Eigen::Matrix MakeHermiteBasis() { + // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find + // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0. + // + // P(i) = P(0) = a0 + // P'(i) = P'(0) = a1 + // P(i+1) = P(1) = a3 + a2 + a1 + a0 + // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1 + // + // [ P(i) ] = [ 0 0 0 1 ][ a3 ] + // [ P'(i) ] = [ 0 0 1 0 ][ a2 ] + // [ P(i+1) ] = [ 1 1 1 1 ][ a1 ] + // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ] + // + // To solve for the coefficients, we can invert the 4x4 matrix and move it + // to the other side of the equation. + // + // [ a3 ] = [ 2 1 -2 1 ][ P(i) ] + // [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ] + // [ a1 ] = [ 0 1 0 0 ][ P(i+1) ] + // [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ] + // clang-format off static auto basis = (Eigen::Matrix() << +2.0, +1.0, -2.0, +1.0, diff --git a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h index ff9369ce27..66f77991a8 100644 --- a/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -52,6 +52,34 @@ class QuinticHermiteSpline : public Spline<5> { * @return The hermite basis matrix for quintic hermite spline interpolation. */ static Eigen::Matrix MakeHermiteBasis() { + // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control + // vectors, we want to find the coefficients of the spline + // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0. + // + // P(i) = P(0) = a0 + // P'(i) = P'(0) = a1 + // P''(i) = P''(0) = 2 * a2 + // P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0 + // P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1 + // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2 + // + // [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ] + // [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ] + // [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ] + // [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ] + // [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ] + // [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ] + // + // To solve for the coefficients, we can invert the 6x6 matrix and move it + // to the other side of the equation. + // + // [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ] + // [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ] + // [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ] + // [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ] + // [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ] + // [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ] + // clang-format off static const auto basis = (Eigen::Matrix() << -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,