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