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

Fix small bug in SplineHelper (#2061)

Browse Source 
Also add comments for clamped cubic solution.
This commit is contained in:
Oblarg
2019-11-11 01:52:24 -05:00
committed by Peter Johnson
parent 7dc7c71b58
commit e3dd1c5d77
3 changed files with 55 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
wpilibc/src/main/native/cpp/spline/SplineHelper.cpp
View File

@@ -28,13 +28,25 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
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);
@@ -42,6 +54,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
}
c.emplace_back(0);
// populate rhs vectors
dx.emplace_back(
3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
xInitial[1]);
@@ -63,6 +76,7 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
waypoints[waypoints.size() - 3].Y().to<double>()) -
yFinal[1]);
// Compute solution to tridiagonal system
ThomasAlgorithm(a, b, c, dx, &fx);
ThomasAlgorithm(a, b, c, dy, &fy);