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