Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make controllers and some trajectory classes constexpr (#7343)

Browse Source
This commit is contained in:
Tyler Veness
2024-11-07 13:02:11 -08:00
committed by GitHub
parent 44a45d44e2
commit a66fa339dc
71 changed files with 1512 additions and 1900 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
View File

@@ -33,7 +33,39 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
CubicHermiteSpline(wpi::array<double, 2> xInitialControlVector,
wpi::array<double, 2> xFinalControlVector,
wpi::array<double, 2> yInitialControlVector,
wpi::array<double, 2> yFinalControlVector);
wpi::array<double, 2> yFinalControlVector)
: m_initialControlVector{xInitialControlVector, yInitialControlVector},
m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
const auto y =
ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
// Populate first two rows with coefficients.
m_coefficients.template block<1, 4>(0, 0) = hermite * x;
m_coefficients.template block<1, 4>(1, 0) = hermite * y;
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 4; i++) {
// Here, we are multiplying by (3 - i) to manually take the derivative.
// The power of the term in index 0 is 3, index 1 is 2 and so on. To find
// the coefficient of the derivative, we can use the power rule and
// multiply the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (3 - i);
}
for (int i = 0; i < 3; i++) {
// Here, we are multiplying by (2 - i) to manually take the derivative.
// The power of the term in index 0 is 2, index 1 is 1 and so on. To find
// the coefficient of the derivative, we can use the power rule and
// multiply the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}
/**
* Returns the coefficients matrix.
@@ -69,7 +101,7 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
* Returns the hermite basis matrix for cubic hermite spline interpolation.
* @return The hermite basis matrix for cubic hermite spline interpolation.
*/
static Eigen::Matrix4d MakeHermiteBasis() {
static constexpr Eigen::Matrix4d 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) = a₃t³ + a₂t² + a₁t + a₀.
//
@@ -90,12 +122,10 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
// [a₂] = [-3 -2 3 -1][P'(i) ]
// [a₁] = [ 0 1 0 0][P(i+1) ]
// [a₀] = [ 1 0 0 0][P'(i+1)]
static const Eigen::Matrix4d 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;
return Eigen::Matrix4d{{+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}};
}
/**
@@ -107,10 +137,12 @@ class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
*
* @return The control vector matrix for a dimension.
*/
static Eigen::Vector4d ControlVectorFromArrays(
static constexpr Eigen::Vector4d ControlVectorFromArrays(
wpi::array<double, 2> initialVector, wpi::array<double, 2> finalVector) {
return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0],
finalVector[1]};
return Eigen::Vector4d{{initialVector[0]},
{initialVector[1]},
{finalVector[0]},
{finalVector[1]}};
}
};
} // namespace frc

59
wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
View File

@@ -33,7 +33,38 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
QuinticHermiteSpline(wpi::array<double, 3> xInitialControlVector,
wpi::array<double, 3> xFinalControlVector,
wpi::array<double, 3> yInitialControlVector,
wpi::array<double, 3> yFinalControlVector);
wpi::array<double, 3> yFinalControlVector)
: m_initialControlVector{xInitialControlVector, yInitialControlVector},
m_finalControlVector{xFinalControlVector, yFinalControlVector} {
const auto hermite = MakeHermiteBasis();
const auto x =
ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
const auto y =
ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
// Populate first two rows with coefficients.
m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
// Populate Row 2 and Row 3 with the derivatives of the equations above.
// Then populate row 4 and 5 with the second derivatives.
for (int i = 0; i < 6; i++) {
// Here, we are multiplying by (5 - i) to manually take the derivative.
// The power of the term in index 0 is 5, index 1 is 4 and so on. To find
// the coefficient of the derivative, we can use the power rule and
// multiply the existing coefficient by its power.
m_coefficients.template block<2, 1>(2, i) =
m_coefficients.template block<2, 1>(0, i) * (5 - i);
}
for (int i = 0; i < 5; i++) {
// Here, we are multiplying by (4 - i) to manually take the derivative.
// The power of the term in index 0 is 4, index 1 is 3 and so on. To find
// the coefficient of the derivative, we can use the power rule and
// multiply the existing coefficient by its power.
m_coefficients.template block<2, 1>(4, i) =
m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}
/**
* Returns the coefficients matrix.
@@ -69,7 +100,7 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
* Returns the hermite basis matrix for quintic hermite spline interpolation.
* @return The hermite basis matrix for quintic hermite spline interpolation.
*/
static Matrixd<6, 6> MakeHermiteBasis() {
static constexpr Matrixd<6, 6> MakeHermiteBasis() {
// Given P(i), P'(i), P"(i), P(i+1), P'(i+1), P"(i+1), the control vectors,
// we want to find the coefficients of the spline
// P(t) = a₅t⁵ + a₄t⁴ + a₃t³ + a₂t² + a₁t + a₀.
@@ -97,15 +128,12 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
// [a₂] = [ 0.0 0.0 0.5 0.0 0.0 0.0][P(i+1) ]
// [a₁] = [ 0.0 1.0 0.0 0.0 0.0 0.0][P'(i+1)]
// [a₀] = [ 1.0 0.0 0.0 0.0 0.0 0.0][P"(i+1)]
static const Matrixd<6, 6> basis{
{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
{+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
{-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
{+00.0, +00.0, +00.5, +00.0, +00.0, +00.0},
{+00.0, +01.0, +00.0, +00.0, +00.0, +00.0},
{+01.0, +00.0, +00.0, +00.0, +00.0, +00.0}};
return basis;
return Matrixd<6, 6>{{-06.0, -03.0, -00.5, +06.0, -03.0, +00.5},
{+15.0, +08.0, +01.5, -15.0, +07.0, -01.0},
{-10.0, -06.0, -01.5, +10.0, -04.0, +00.5},
{+00.0, +00.0, +00.5, +00.0, +00.0, +00.0},
{+00.0, +01.0, +00.0, +00.0, +00.0, +00.0},
{+01.0, +00.0, +00.0, +00.0, +00.0, +00.0}};
}
/**
@@ -117,10 +145,11 @@ class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
*
* @return The control vector matrix for a dimension.
*/
static Vectord<6> ControlVectorFromArrays(wpi::array<double, 3> initialVector,
wpi::array<double, 3> finalVector) {
return Vectord<6>{initialVector[0], initialVector[1], initialVector[2],
finalVector[0], finalVector[1], finalVector[2]};
static constexpr Vectord<6> ControlVectorFromArrays(
wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
return Vectord<6>{{initialVector[0]}, {initialVector[1]},
{initialVector[2]}, {finalVector[0]},
{finalVector[1]}, {finalVector[2]}};
}
};
} // namespace frc

34
wpimath/src/main/native/include/frc/spline/Spline.h
View File

@@ -7,6 +7,7 @@
#include <optional>
#include <utility>
#include <gcem.hpp>
#include <wpi/array.h>
#include "frc/EigenCore.h"
@@ -15,6 +16,7 @@
#include "units/length.h"
namespace frc {
/**
* Represents a two-dimensional parametric spline that interpolates between two
* points.
@@ -26,15 +28,15 @@ class Spline {
public:
using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
Spline() = default;
constexpr Spline() = default;
Spline(const Spline&) = default;
Spline& operator=(const Spline&) = default;
constexpr Spline(const Spline&) = default;
constexpr Spline& operator=(const Spline&) = default;
Spline(Spline&&) = default;
Spline& operator=(Spline&&) = default;
constexpr Spline(Spline&&) = default;
constexpr Spline& operator=(Spline&&) = default;
virtual ~Spline() = default;
constexpr virtual ~Spline() = default;
/**
* Represents a control vector for a spline.
@@ -62,7 +64,7 @@ class Spline {
// Populate the polynomial bases
for (int i = 0; i <= Degree; i++) {
polynomialBases(i) = std::pow(t, Degree - i);
polynomialBases(i) = gcem::pow(t, Degree - i);
}
// This simply multiplies by the coefficients. We need to divide out t some
@@ -88,13 +90,13 @@ class Spline {
ddy = combined(5) / t / t;
}
if (std::hypot(dx, dy) < 1e-6) {
if (gcem::hypot(dx, dy) < 1e-6) {
return std::nullopt;
}
// Find the curvature.
const auto curvature =
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
(dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * gcem::hypot(dx, dy));
return PoseWithCurvature{
{FromVector(combined.template block<2, 1>(0, 0)), Rotation2d{dx, dy}},
@@ -106,21 +108,21 @@ class Spline {
*
* @return The coefficients of the spline.
*/
virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
constexpr virtual Matrixd<6, Degree + 1> Coefficients() const = 0;
/**
* Returns the initial control vector that created this spline.
*
* @return The initial control vector that created this spline.
*/
virtual const ControlVector& GetInitialControlVector() const = 0;
constexpr virtual const ControlVector& GetInitialControlVector() const = 0;
/**
* Returns the final control vector that created this spline.
*
* @return The final control vector that created this spline.
*/
virtual const ControlVector& GetFinalControlVector() const = 0;
constexpr virtual const ControlVector& GetFinalControlVector() const = 0;
protected:
/**
@@ -129,8 +131,9 @@ class Spline {
* @param translation The Translation2d to convert.
* @return The vector.
*/
static Eigen::Vector2d ToVector(const Translation2d& translation) {
return Eigen::Vector2d{translation.X().value(), translation.Y().value()};
static constexpr Eigen::Vector2d ToVector(const Translation2d& translation) {
return Eigen::Vector2d{{translation.X().value()},
{translation.Y().value()}};
}
/**
@@ -139,8 +142,9 @@ class Spline {
* @param vector The vector to convert.
* @return The Translation2d.
*/
static Translation2d FromVector(const Eigen::Vector2d& vector) {
static constexpr Translation2d FromVector(const Eigen::Vector2d& vector) {
return Translation2d{units::meter_t{vector(0)}, units::meter_t{vector(1)}};
}
};
} // namespace frc

264
wpimath/src/main/native/include/frc/spline/SplineHelper.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include <utility>
#include <cstddef>
#include <vector>
#include <wpi/SymbolExports.h>
@@ -32,7 +32,22 @@ class WPILIB_DLLEXPORT SplineHelper {
static wpi::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end);
const Pose2d& end) {
double scalar;
if (interiorWaypoints.empty()) {
scalar = 1.2 * start.Translation().Distance(end.Translation()).value();
} else {
scalar =
1.2 * start.Translation().Distance(interiorWaypoints.front()).value();
}
const auto initialCV = CubicControlVector(scalar, start);
if (!interiorWaypoints.empty()) {
scalar =
1.2 * end.Translation().Distance(interiorWaypoints.back()).value();
}
const auto finalCV = CubicControlVector(scalar, end);
return {initialCV, finalCV};
}
/**
* Returns quintic splines from a set of waypoints.
@@ -41,7 +56,24 @@ class WPILIB_DLLEXPORT SplineHelper {
* @return List of quintic splines.
*/
static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints);
const std::vector<Pose2d>& waypoints) {
std::vector<QuinticHermiteSpline> splines;
splines.reserve(waypoints.size() - 1);
for (size_t i = 0; i < waypoints.size() - 1; ++i) {
auto& p0 = waypoints[i];
auto& p1 = waypoints[i + 1];
// This just makes the splines look better.
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).value();
auto controlVectorA = QuinticControlVector(scalar, p0);
auto controlVectorB = QuinticControlVector(scalar, p1);
splines.emplace_back(controlVectorA.x, controlVectorB.x, controlVectorA.y,
controlVectorB.y);
}
return splines;
}
/**
* Returns a set of cubic splines corresponding to the provided control
@@ -63,7 +95,114 @@ class WPILIB_DLLEXPORT SplineHelper {
static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start,
std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end);
const Spline<3>::ControlVector& end) {
std::vector<CubicHermiteSpline> splines;
wpi::array<double, 2> xInitial = start.x;
wpi::array<double, 2> yInitial = start.y;
wpi::array<double, 2> xFinal = end.x;
wpi::array<double, 2> yFinal = end.y;
if (waypoints.size() > 1) {
waypoints.emplace(waypoints.begin(),
Translation2d{units::meter_t{xInitial[0]},
units::meter_t{yInitial[0]}});
waypoints.emplace_back(
Translation2d{units::meter_t{xFinal[0]}, units::meter_t{yFinal[0]}});
// Populate tridiagonal system for clamped cubic
/* See:
https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08
/undervisningsmateriale/chap7alecture.pdf
*/
// Above-diagonal of tridiagonal matrix, zero-padded
std::vector<double> a;
// Diagonal of tridiagonal matrix
std::vector<double> b(waypoints.size() - 2, 4.0);
// Below-diagonal of tridiagonal matrix, zero-padded
std::vector<double> c;
// rhs vectors
std::vector<double> dx, dy;
// solution vectors
std::vector<double> fx(waypoints.size() - 2, 0.0),
fy(waypoints.size() - 2, 0.0);
// populate above-diagonal and below-diagonal vectors
a.emplace_back(0);
for (size_t i = 0; i < waypoints.size() - 3; ++i) {
a.emplace_back(1);
c.emplace_back(1);
}
c.emplace_back(0);
// populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().value() - waypoints[0].X().value()) -
xInitial[1]);
dy.emplace_back(
3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) -
yInitial[1]);
if (waypoints.size() > 4) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
// dx and dy represent the derivatives of the internal waypoints. The
// derivative of the second internal waypoint should involve the third
// and first internal waypoint, which have indices of 1 and 3 in the
// waypoints list (which contains ALL waypoints).
dx.emplace_back(
3 * (waypoints[i + 2].X().value() - waypoints[i].X().value()));
dy.emplace_back(
3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value()));
}
}
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() -
waypoints[waypoints.size() - 3].X().value()) -
xFinal[1]);
dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() -
waypoints[waypoints.size() - 3].Y().value()) -
yFinal[1]);
// Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);
fx.emplace(fx.begin(), xInitial[1]);
fx.emplace_back(xFinal[1]);
fy.emplace(fy.begin(), yInitial[1]);
fy.emplace_back(yFinal[1]);
for (size_t i = 0; i < fx.size() - 1; ++i) {
// Create the spline.
const CubicHermiteSpline spline{
{waypoints[i].X().value(), fx[i]},
{waypoints[i + 1].X().value(), fx[i + 1]},
{waypoints[i].Y().value(), fy[i]},
{waypoints[i + 1].Y().value(), fy[i + 1]}};
splines.push_back(spline);
}
} else if (waypoints.size() == 1) {
const double xDeriv =
(3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
const double yDeriv =
(3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
wpi::array<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
wpi::array<double, 2> midYControlVector{waypoints[0].Y().value(), yDeriv};
splines.emplace_back(xInitial, midXControlVector, yInitial,
midYControlVector);
splines.emplace_back(midXControlVector, xFinal, midYControlVector,
yFinal);
} else {
// Create the spline.
const CubicHermiteSpline spline{xInitial, xFinal, yInitial, yFinal};
splines.push_back(spline);
}
return splines;
}
/**
* Returns a set of quintic splines corresponding to the provided control
@@ -75,7 +214,17 @@ class WPILIB_DLLEXPORT SplineHelper {
* provided waypoints.
*/
static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors);
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> splines;
for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
auto& xInitial = controlVectors[i].x;
auto& yInitial = controlVectors[i].y;
auto& xFinal = controlVectors[i + 1].x;
auto& yFinal = controlVectors[i + 1].y;
splines.emplace_back(xInitial, xFinal, yInitial, yFinal);
}
return splines;
}
/**
* Optimizes the curvature of 2 or more quintic splines at knot points.
@@ -86,7 +235,79 @@ class WPILIB_DLLEXPORT SplineHelper {
* @return A vector of optimized quintic splines.
*/
static std::vector<QuinticHermiteSpline> OptimizeCurvature(
const std::vector<QuinticHermiteSpline>& splines);
const std::vector<QuinticHermiteSpline>& splines) {
// If there's only one spline in the vector, we can't optimize anything so
// just return that.
if (splines.size() < 2) {
return splines;
}
// Implements Section 4.1.2 of
// http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf.
// Cubic splines minimize the integral of the second derivative's absolute
// value. Therefore, we can create cubic splines with the same 0th and 1st
// derivatives and the provided quintic splines, find the second derivative
// of those cubic splines and then use a weighted average for the second
// derivatives of the quintic splines.
std::vector<QuinticHermiteSpline> optimizedSplines;
optimizedSplines.reserve(splines.size());
optimizedSplines.push_back(splines[0]);
for (size_t i = 0; i < splines.size() - 1; ++i) {
const auto& a = splines[i];
const auto& b = splines[i + 1];
// Get the control vectors that created the quintic splines above.
const auto& aInitial = a.GetInitialControlVector();
const auto& aFinal = a.GetFinalControlVector();
const auto& bInitial = b.GetInitialControlVector();
const auto& bFinal = b.GetFinalControlVector();
// Create cubic splines with the same control vectors.
auto Trim = [](const wpi::array<double, 3>& a) {
return wpi::array<double, 2>{a[0], a[1]};
};
CubicHermiteSpline ca{Trim(aInitial.x), Trim(aFinal.x), Trim(aInitial.y),
Trim(aFinal.y)};
CubicHermiteSpline cb{Trim(bInitial.x), Trim(bFinal.x), Trim(bInitial.y),
Trim(bFinal.y)};
// Calculate the second derivatives at the knot points.
frc::Vectord<4> bases{1.0, 1.0, 1.0, 1.0};
frc::Vectord<6> combinedA = ca.Coefficients() * bases;
double ddxA = combinedA(4);
double ddyA = combinedA(5);
double ddxB = cb.Coefficients()(4, 1);
double ddyB = cb.Coefficients()(5, 1);
// Calculate the parameters for weighted average.
double dAB =
std::hypot(aFinal.x[0] - aInitial.x[0], aFinal.y[0] - aInitial.y[0]);
double dBC =
std::hypot(bFinal.x[0] - bInitial.x[0], bFinal.y[0] - bInitial.y[0]);
double alpha = dBC / (dAB + dBC);
double beta = dAB / (dAB + dBC);
// Calculate the weighted average.
double ddx = alpha * ddxA + beta * ddxB;
double ddy = alpha * ddyA + beta * ddyB;
// Create new splines.
optimizedSplines[i] = {aInitial.x,
{aFinal.x[0], aFinal.x[1], ddx},
aInitial.y,
{aFinal.y[0], aFinal.y[1], ddy}};
optimizedSplines.push_back({{bInitial.x[0], bInitial.x[1], ddx},
bFinal.x,
{bInitial.y[0], bInitial.y[1], ddy},
bFinal.y});
}
return optimizedSplines;
}
private:
static Spline<3>::ControlVector CubicControlVector(double scalar,
@@ -114,6 +335,35 @@ class WPILIB_DLLEXPORT SplineHelper {
const std::vector<double>& b,
const std::vector<double>& c,
const std::vector<double>& d,
std::vector<double>* solutionVector);
std::vector<double>* solutionVector) {
auto& f = *solutionVector;
size_t N = d.size();
// Create the temporary vectors
// Note that this is inefficient as it is possible to call
// this function many times. A better implementation would
// pass these temporary matrices by non-const reference to
// save excess allocation and deallocation
std::vector<double> c_star(N, 0.0);
std::vector<double> d_star(N, 0.0);
// This updates the coefficients in the first row
// Note that we should be checking for division by zero here
c_star[0] = c[0] / b[0];
d_star[0] = d[0] / b[0];
// Create the c_star and d_star coefficients in the forward sweep
for (size_t i = 1; i < N; ++i) {
double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
c_star[i] = c[i] * m;
d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
}
f[N - 1] = d_star[N - 1];
// This is the reverse sweep, used to update the solution vector f
for (int i = N - 2; i >= 0; i--) {
f[i] = d_star[i] - c_star[i] * f[i + 1];
}
}
};
} // namespace frc