mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
e767605e94
This makes complex code significantly easier to read. frc::Vectord<Size> = Eigen::Vector<double, Size> frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
97 lines
3.4 KiB
C++
97 lines
3.4 KiB
C++
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
#pragma once
|
|
|
|
#include <wpi/SymbolExports.h>
|
|
#include <wpi/array.h>
|
|
|
|
#include "frc/EigenCore.h"
|
|
#include "frc/spline/Spline.h"
|
|
|
|
namespace frc {
|
|
/**
|
|
* Represents a hermite spline of degree 3.
|
|
*/
|
|
class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
|
|
public:
|
|
/**
|
|
* Constructs a cubic hermite spline with the specified control vectors. Each
|
|
* control vector contains info about the location of the point and its first
|
|
* derivative.
|
|
*
|
|
* @param xInitialControlVector The control vector for the initial point in
|
|
* the x dimension.
|
|
* @param xFinalControlVector The control vector for the final point in
|
|
* the x dimension.
|
|
* @param yInitialControlVector The control vector for the initial point in
|
|
* the y dimension.
|
|
* @param yFinalControlVector The control vector for the final point in
|
|
* the y dimension.
|
|
*/
|
|
CubicHermiteSpline(wpi::array<double, 2> xInitialControlVector,
|
|
wpi::array<double, 2> xFinalControlVector,
|
|
wpi::array<double, 2> yInitialControlVector,
|
|
wpi::array<double, 2> yFinalControlVector);
|
|
|
|
protected:
|
|
/**
|
|
* Returns the coefficients matrix.
|
|
* @return The coefficients matrix.
|
|
*/
|
|
Matrixd<6, 3 + 1> Coefficients() const override { return m_coefficients; }
|
|
|
|
private:
|
|
Matrixd<6, 4> m_coefficients = Matrixd<6, 4>::Zero();
|
|
|
|
/**
|
|
* Returns the hermite basis matrix for cubic hermite spline interpolation.
|
|
* @return The hermite basis matrix for cubic hermite spline interpolation.
|
|
*/
|
|
static Matrixd<4, 4> MakeHermiteBasis() {
|
|
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
|
|
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
|
|
//
|
|
// P(i) = P(0) = a0
|
|
// P'(i) = P'(0) = a1
|
|
// P(i+1) = P(1) = a3 + a2 + a1 + a0
|
|
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
|
|
//
|
|
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
|
|
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
|
|
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
|
|
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
|
|
//
|
|
// To solve for the coefficients, we can invert the 4x4 matrix and move it
|
|
// to the other side of the equation.
|
|
//
|
|
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
|
|
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
|
|
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
|
|
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
|
|
|
|
static const Matrixd<4, 4> basis{{+2.0, +1.0, -2.0, +1.0},
|
|
{-3.0, -2.0, +3.0, -1.0},
|
|
{+0.0, +1.0, +0.0, +0.0},
|
|
{+1.0, +0.0, +0.0, +0.0}};
|
|
return basis;
|
|
}
|
|
|
|
/**
|
|
* Returns the control vector for each dimension as a matrix from the
|
|
* user-provided arrays in the constructor.
|
|
*
|
|
* @param initialVector The control vector for the initial point.
|
|
* @param finalVector The control vector for the final point.
|
|
*
|
|
* @return The control vector matrix for a dimension.
|
|
*/
|
|
static Eigen::Vector4d ControlVectorFromArrays(
|
|
wpi::array<double, 2> initialVector, wpi::array<double, 2> finalVector) {
|
|
return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0],
|
|
finalVector[1]};
|
|
}
|
|
};
|
|
} // namespace frc
|