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

```