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 bug in cubic and quintic hermetic spline generation (#2139)

Browse Source 
Add documentation for spline derivatives and explicitly zero matrices.
This commit is contained in:
Matt
2019-12-01 21:29:52 -08:00
committed by Peter Johnson
parent e37ecd33ae
commit 6c8f6cf479
6 changed files with 54 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,10 +27,20 @@ CubicHermiteSpline::CubicHermiteSpline(
// 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) * (3 - i);
m_coefficients.template block<2, 1>(2, i) * (2 - i);
}
}

13
wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
View File

@@ -27,10 +27,19 @@ QuinticHermiteSpline::QuinticHermiteSpline(
// 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) * (5 - i);
m_coefficients.template block<2, 1>(2, i) * (4 - i);
}
}