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 quintic spline generation bug (#2429)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-04-01 23:31:29 -04:00
committed by GitHub
parent 8f33d21bc2
commit a308dd4471
2 changed files with 10 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -123,7 +123,9 @@ std::vector<QuinticHermiteSpline>
SplineHelper::QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors) {
std::vector<QuinticHermiteSpline> splines;
for (size_t i = 0; i < controlVectors.size() - 1; ++i) {
// There are twice as many control vectors are there are splines,
// so we increment the counter by 2.
for (size_t i = 0; i < controlVectors.size() - 1; i += 2) {
auto& xInitial = controlVectors[i].x;
auto& yInitial = controlVectors[i].y;
auto& xFinal = controlVectors[i + 1].x;
@@ -166,12 +168,7 @@ SplineHelper::QuinticControlVectorsFromWaypoints(
const auto scalar =
1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
// Only add the first control vector if this is the first iteration. The
// last control vector of this iteration is the first control vector of
// the next iteration.
if (i == 0) {
vectors.push_back(QuinticControlVector(scalar, p0));
}
vectors.push_back(QuinticControlVector(scalar, p0));
vectors.push_back(QuinticControlVector(scalar, p1));
}
return vectors;