Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +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>() -

7
wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
View File

@@ -145,8 +145,11 @@ public final class SplineHelper {
if (newWaypts.length > 4) {
for (int i = 1; i <= newWaypts.length - 4; i++) {
dx[i] = 3 * (newWaypts[i + 1].getX() - newWaypts[i - 1].getX());
dy[i] = 3 * (newWaypts[i + 1].getY() - newWaypts[i - 1].getY());
// 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 newWaypts list (which contains ALL waypoints).
dx[i] = 3 * (newWaypts[i + 2].getX() - newWaypts[i].getX());
dy[i] = 3 * (newWaypts[i + 2].getY() - newWaypts[i].getY());
}
}