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.geometry.Pose2d;
008import edu.wpi.first.math.geometry.Rotation2d;
009import java.util.Arrays;
010import org.ejml.simple.SimpleMatrix;
011
012/** Represents a two-dimensional parametric spline that interpolates between two points. */
013public abstract class Spline {
014  private final int m_degree;
015
016  /**
017   * Constructs a spline with the given degree.
018   *
019   * @param degree The degree of the spline.
020   */
021  Spline(int degree) {
022    m_degree = degree;
023  }
024
025  /**
026   * Returns the coefficients of the spline.
027   *
028   * @return The coefficients of the spline.
029   */
030  public abstract SimpleMatrix getCoefficients();
031
032  /**
033   * Returns the initial control vector that created this spline.
034   *
035   * @return The initial control vector that created this spline.
036   */
037  public abstract ControlVector getInitialControlVector();
038
039  /**
040   * Returns the final control vector that created this spline.
041   *
042   * @return The final control vector that created this spline.
043   */
044  public abstract ControlVector getFinalControlVector();
045
046  /**
047   * Gets the pose and curvature at some point t on the spline.
048   *
049   * @param t The point t
050   * @return The pose and curvature at that point.
051   */
052  public PoseWithCurvature getPoint(double t) {
053    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
054    final var coefficients = getCoefficients();
055
056    // Populate the polynomial bases.
057    for (int i = 0; i <= m_degree; i++) {
058      polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
059    }
060
061    // This simply multiplies by the coefficients. We need to divide out t some
062    // n number of times when n is the derivative we want to take.
063    SimpleMatrix combined = coefficients.mult(polynomialBases);
064
065    // Get x and y
066    final double x = combined.get(0, 0);
067    final double y = combined.get(1, 0);
068
069    double dx;
070    double dy;
071    double ddx;
072    double ddy;
073
074    if (t == 0) {
075      dx = coefficients.get(2, m_degree - 1);
076      dy = coefficients.get(3, m_degree - 1);
077      ddx = coefficients.get(4, m_degree - 2);
078      ddy = coefficients.get(5, m_degree - 2);
079    } else {
080      // Divide out t once for first derivative.
081      dx = combined.get(2, 0) / t;
082      dy = combined.get(3, 0) / t;
083
084      // Divide out t twice for second derivative.
085      ddx = combined.get(4, 0) / t / t;
086      ddy = combined.get(5, 0) / t / t;
087    }
088
089    // Find the curvature.
090    final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
091
092    return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
093  }
094
095  /**
096   * Represents a control vector for a spline.
097   *
098   * <p>Each element in each array represents the value of the derivative at the index. For example,
099   * the value of x[2] is the second derivative in the x dimension.
100   */
101  public static class ControlVector {
102    /** The x components of the control vector. */
103    public double[] x;
104
105    /** The y components of the control vector. */
106    public double[] y;
107
108    /**
109     * Instantiates a control vector.
110     *
111     * @param x The x dimension of the control vector.
112     * @param y The y dimension of the control vector.
113     */
114    public ControlVector(double[] x, double[] y) {
115      this.x = Arrays.copyOf(x, x.length);
116      this.y = Arrays.copyOf(y, y.length);
117    }
118  }
119}