001// Copyright (c) FIRST and other WPILib contributors. 002// Open Source Software; you can modify and/or share it under the terms of 003// the WPILib BSD license file in the root directory of this project. 004 005package edu.wpi.first.math.spline; 006 007import org.ejml.simple.SimpleMatrix; 008 009public class QuinticHermiteSpline extends Spline { 010 private static SimpleMatrix hermiteBasis; 011 private final SimpleMatrix m_coefficients; 012 013 private final ControlVector m_initialControlVector; 014 private final ControlVector m_finalControlVector; 015 016 /** 017 * Constructs a quintic hermite spline with the specified control vectors. Each control vector 018 * contains into about the location of the point, its first derivative, and its second derivative. 019 * 020 * @param xInitialControlVector The control vector for the initial point in the x dimension. 021 * @param xFinalControlVector The control vector for the final point in the x dimension. 022 * @param yInitialControlVector The control vector for the initial point in the y dimension. 023 * @param yFinalControlVector The control vector for the final point in the y dimension. 024 */ 025 public QuinticHermiteSpline( 026 double[] xInitialControlVector, 027 double[] xFinalControlVector, 028 double[] yInitialControlVector, 029 double[] yFinalControlVector) { 030 super(5); 031 032 // Populate the coefficients for the actual spline equations. 033 // Row 0 is x coefficients 034 // Row 1 is y coefficients 035 final var hermite = makeHermiteBasis(); 036 final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector); 037 final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector); 038 039 final var xCoeffs = (hermite.mult(x)).transpose(); 040 final var yCoeffs = (hermite.mult(y)).transpose(); 041 042 m_coefficients = new SimpleMatrix(6, 6); 043 044 for (int i = 0; i < 6; i++) { 045 m_coefficients.set(0, i, xCoeffs.get(0, i)); 046 m_coefficients.set(1, i, yCoeffs.get(0, i)); 047 } 048 for (int i = 0; i < 6; i++) { 049 // Populate Row 2 and Row 3 with the derivatives of the equations above. 050 // Here, we are multiplying by (5 - i) to manually take the derivative. The 051 // power of the term in index 0 is 5, index 1 is 4 and so on. To find the 052 // coefficient of the derivative, we can use the power rule and multiply 053 // the existing coefficient by its power. 054 m_coefficients.set(2, i, m_coefficients.get(0, i) * (5 - i)); 055 m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i)); 056 } 057 for (int i = 0; i < 5; i++) { 058 // Then populate row 4 and 5 with the second derivatives. 059 // Here, we are multiplying by (4 - i) to manually take the derivative. The 060 // power of the term in index 0 is 4, index 1 is 3 and so on. To find the 061 // coefficient of the derivative, we can use the power rule and multiply 062 // the existing coefficient by its power. 063 m_coefficients.set(4, i, m_coefficients.get(2, i) * (4 - i)); 064 m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i)); 065 } 066 067 // Assign member variables. 068 m_initialControlVector = new ControlVector(xInitialControlVector, yInitialControlVector); 069 m_finalControlVector = new ControlVector(xFinalControlVector, yFinalControlVector); 070 } 071 072 /** 073 * Returns the coefficients matrix. 074 * 075 * @return The coefficients matrix. 076 */ 077 @Override 078 public SimpleMatrix getCoefficients() { 079 return m_coefficients; 080 } 081 082 /** 083 * Returns the initial control vector that created this spline. 084 * 085 * @return The initial control vector that created this spline. 086 */ 087 @Override 088 public ControlVector getInitialControlVector() { 089 return m_initialControlVector; 090 } 091 092 /** 093 * Returns the final control vector that created this spline. 094 * 095 * @return The final control vector that created this spline. 096 */ 097 @Override 098 public ControlVector getFinalControlVector() { 099 return m_finalControlVector; 100 } 101 102 /** 103 * Returns the hermite basis matrix for quintic hermite spline interpolation. 104 * 105 * @return The hermite basis matrix for quintic hermite spline interpolation. 106 */ 107 private SimpleMatrix makeHermiteBasis() { 108 if (hermiteBasis == null) { 109 // Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors, 110 // we want to find the coefficients of the spline 111 // P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀. 112 // 113 // P(i) = P(0) = a₀ 114 // P'(i) = P'(0) = a₁ 115 // P''(i) = P"(0) = 2a₂ 116 // P(i+1) = P(1) = a₅ + a₄ + a₃ + a₂ + a₁ + a₀ 117 // P'(i+1) = P'(1) = 5a₅ + 4a₄ + 3a₃ + 2a₂ + a₁ 118 // P"(i+1) = P"(1) = 20a₅ + 12a₄ + 6a₃ + 2a₂ 119 // 120 // [P(i) ] = [ 0 0 0 0 0 1][a₅] 121 // [P'(i) ] = [ 0 0 0 0 1 0][a₄] 122 // [P"(i) ] = [ 0 0 0 2 0 0][a₃] 123 // [P(i+1) ] = [ 1 1 1 1 1 1][a₂] 124 // [P'(i+1)] = [ 5 4 3 2 1 0][a₁] 125 // [P"(i+1)] = [20 12 6 2 0 0][a₀] 126 // 127 // To solve for the coefficients, we can invert the 6x6 matrix and move it 128 // to the other side of the equation. 129 // 130 // [a₅] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5][P(i) ] 131 // [a₄] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0][P'(i) ] 132 // [a₃] = [-10.0 -6.0 -1.5 10.0 -4.0 0.5][P"(i) ] 133 // [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ] 134 // [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)] 135 // [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)] 136 hermiteBasis = 137 new SimpleMatrix( 138 6, 139 6, 140 true, 141 new double[] { 142 -06.0, -03.0, -00.5, +06.0, -03.0, +00.5, +15.0, +08.0, +01.5, -15.0, +07.0, -01.0, 143 -10.0, -06.0, -01.5, +10.0, -04.0, +00.5, +00.0, +00.0, +00.5, +00.0, +00.0, +00.0, 144 +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +01.0, +00.0, +00.0, +00.0, +00.0, +00.0 145 }); 146 } 147 return hermiteBasis; 148 } 149 150 /** 151 * Returns the control vector for each dimension as a matrix from the user-provided arrays in the 152 * constructor. 153 * 154 * @param initialVector The control vector for the initial point. 155 * @param finalVector The control vector for the final point. 156 * @return The control vector matrix for a dimension. 157 */ 158 private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) { 159 if (initialVector.length != 3 || finalVector.length != 3) { 160 throw new IllegalArgumentException("Size of vectors must be 3"); 161 } 162 return new SimpleMatrix( 163 6, 164 1, 165 true, 166 new double[] { 167 initialVector[0], initialVector[1], initialVector[2], 168 finalVector[0], finalVector[1], finalVector[2] 169 }); 170 } 171}