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  protected abstract SimpleMatrix getCoefficients();
031
032  /**
033   * Gets the pose and curvature at some point t on the spline.
034   *
035   * @param t The point t
036   * @return The pose and curvature at that point.
037   */
038  @SuppressWarnings("ParameterName")
039  public PoseWithCurvature getPoint(double t) {
040    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
041    final var coefficients = getCoefficients();
042
043    // Populate the polynomial bases.
044    for (int i = 0; i <= m_degree; i++) {
045      polynomialBases.set(i, 0, Math.pow(t, m_degree - i));
046    }
047
048    // This simply multiplies by the coefficients. We need to divide out t some
049    // n number of times where n is the derivative we want to take.
050    SimpleMatrix combined = coefficients.mult(polynomialBases);
051
052    // Get x and y
053    final double x = combined.get(0, 0);
054    final double y = combined.get(1, 0);
055
056    double dx;
057    double dy;
058    double ddx;
059    double ddy;
060
061    if (t == 0) {
062      dx = coefficients.get(2, m_degree - 1);
063      dy = coefficients.get(3, m_degree - 1);
064      ddx = coefficients.get(4, m_degree - 2);
065      ddy = coefficients.get(5, m_degree - 2);
066    } else {
067      // Divide out t once for first derivative.
068      dx = combined.get(2, 0) / t;
069      dy = combined.get(3, 0) / t;
070
071      // Divide out t twice for second derivative.
072      ddx = combined.get(4, 0) / t / t;
073      ddy = combined.get(5, 0) / t / t;
074    }
075
076    // Find the curvature.
077    final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
078
079    return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
080  }
081
082  /**
083   * Represents a control vector for a spline.
084   *
085   * <p>Each element in each array represents the value of the derivative at the index. For example,
086   * the value of x[2] is the second derivative in the x dimension.
087   */
088  @SuppressWarnings("MemberName")
089  public static class ControlVector {
090    public double[] x;
091    public double[] y;
092
093    /**
094     * Instantiates a control vector.
095     *
096     * @param x The x dimension of the control vector.
097     * @param y The y dimension of the control vector.
098     */
099    @SuppressWarnings("ParameterName")
100    public ControlVector(double[] x, double[] y) {
101      this.x = Arrays.copyOf(x, x.length);
102      this.y = Arrays.copyOf(y, y.length);
103    }
104  }
105}