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

[wpilib] Fix SplineHelper cubic spline bug (#2572)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-07-06 01:13:28 -04:00
committed by GitHub
parent 5bd2dca463
commit b3b2aa6359
2 changed files with 13 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -63,10 +63,14 @@ std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromControlVectors(
yInitial[1]);
if (waypoints.size() > 4) {
for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
waypoints[i - 1].X().to<double>()));
dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
waypoints[i - 1].Y().to<double>()));
// 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().to<double>() -
waypoints[i].X().to<double>()));
dy.emplace_back(3 * (waypoints[i + 2].Y().to<double>() -
waypoints[i].Y().to<double>()));
}
}
dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -