WPILibC++  2020.3.2
CubicHermiteSpline.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <array>
11 
12 #include <Eigen/Core>
13 
14 #include "frc/spline/Spline.h"
15 
16 namespace frc {
20 class CubicHermiteSpline : public Spline<3> {
21  public:
36  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
37  std::array<double, 2> xFinalControlVector,
38  std::array<double, 2> yInitialControlVector,
39  std::array<double, 2> yFinalControlVector);
40 
41  protected:
46  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
47  return m_coefficients;
48  }
49 
50  private:
51  Eigen::Matrix<double, 6, 4> m_coefficients =
52  Eigen::Matrix<double, 6, 4>::Zero();
53 
58  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
59  // clang-format off
60  static auto basis = (Eigen::Matrix<double, 4, 4>() <<
61  +2.0, +1.0, -2.0, +1.0,
62  -3.0, -2.0, +3.0, -1.0,
63  +0.0, +1.0, +0.0, +0.0,
64  +1.0, +0.0, +0.0, +0.0).finished();
65  // clang-format on
66  return basis;
67  }
68 
78  static Eigen::Vector4d ControlVectorFromArrays(
79  std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
80  return (Eigen::Vector4d() << initialVector[0], initialVector[1],
81  finalVector[0], finalVector[1])
82  .finished();
83  }
84 };
85 } // namespace frc
frc::Spline
Represents a two-dimensional parametric spline that interpolates between two points.
Definition: Spline.h:33
frc::CubicHermiteSpline::CubicHermiteSpline
CubicHermiteSpline(std::array< double, 2 > xInitialControlVector, std::array< double, 2 > xFinalControlVector, std::array< double, 2 > yInitialControlVector, std::array< double, 2 > yFinalControlVector)
Constructs a cubic hermite spline with the specified control vectors.
frc::CubicHermiteSpline::Coefficients
Eigen::Matrix< double, 6, 3+1 > Coefficients() const override
Returns the coefficients matrix.
Definition: CubicHermiteSpline.h:46
frc::CubicHermiteSpline
Represents a hermite spline of degree 3.
Definition: CubicHermiteSpline.h:20
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16