diff --git a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp index 3578b1daec..3991fec4d2 100644 --- a/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp +++ b/wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp @@ -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); } } diff --git a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp index f714c6f617..bb8ad3c320 100644 --- a/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp +++ b/wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp @@ -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); } } diff --git a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h index a8979c1651..1e08a942e8 100644 --- a/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h +++ b/wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h @@ -48,7 +48,8 @@ class CubicHermiteSpline : public Spline<3> { } private: - Eigen::Matrix m_coefficients; + Eigen::Matrix m_coefficients = + Eigen::Matrix::Zero(); /** * Returns the hermite basis matrix for cubic hermite spline interpolation. diff --git a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h index 730eab42f3..632d3adcf7 100644 --- a/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h +++ b/wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h @@ -48,7 +48,8 @@ class QuinticHermiteSpline : public Spline<5> { } private: - Eigen::Matrix m_coefficients; + Eigen::Matrix m_coefficients = + Eigen::Matrix::Zero(); /** * Returns the hermite basis matrix for quintic hermite spline interpolation. diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java index 5674231d96..578787c4f3 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java @@ -50,10 +50,21 @@ public class CubicHermiteSpline extends Spline { // Populate Row 2 and Row 3 with the derivatives of the equations above. // Then populate row 4 and 5 with the second derivatives. + // 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.set(2, i, m_coefficients.get(0, i) * (3 - i)); m_coefficients.set(3, i, m_coefficients.get(1, i) * (3 - i)); - m_coefficients.set(4, i, m_coefficients.get(2, i) * (3 - i)); - m_coefficients.set(5, i, m_coefficients.get(3, 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.set(4, i, m_coefficients.get(2, i) * (2 - i)); + m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i)); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java index 562134167d..3b2d3ebebd 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java @@ -47,13 +47,24 @@ public class QuinticHermiteSpline extends Spline { for (int i = 0; i < 6; i++) { m_coefficients.set(0, i, xCoeffs.get(0, i)); m_coefficients.set(1, i, yCoeffs.get(0, i)); - + } + for (int i = 0; i < 6; i++) { // Populate Row 2 and Row 3 with the derivatives of the equations above. - // Then populate row 4 and 5 with the second derivatives. + // 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.set(2, i, m_coefficients.get(0, i) * (5 - i)); m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i)); - m_coefficients.set(4, i, m_coefficients.get(2, i) * (5 - i)); - m_coefficients.set(5, i, m_coefficients.get(3, i) * (5 - i)); + } + for (int i = 0; i < 5; i++) { + // Then populate row 4 and 5 with the second derivatives. + // 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.set(4, i, m_coefficients.get(2, i) * (4 - i)); + m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i)); } }