Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add derivation for spline basis matrices

Browse Source
This commit is contained in:
Prateek Machiraju
2021-03-30 18:25:17 -04:00
committed by Peter Johnson
parent f57c188f2e
commit ffb4d38e24
4 changed files with 96 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
View File

@@ -80,6 +80,26 @@ public class CubicHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
// the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P(i+1) = P(1) = a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
//
// [ P(i) ] = [ 0 0 0 1 ][ a3 ]
// [ P'(i) ] = [ 0 0 1 0 ][ a2 ]
// [ P(i+1) ] = [ 1 1 1 1 ][ a1 ]
// [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 4x4 matrix and move it
// to the other side of the equation.
//
// [ a3 ] = [ 2 1 -2 1 ][ P(i) ]
// [ a2 ] = [ -3 -2 3 -1 ][ P'(i) ]
// [ a1 ] = [ 0 1 0 0 ][ P(i+1) ]
// [ a0 ] = [ 1 0 0 0 ][ P'(i+1) ]
hermiteBasis =
new SimpleMatrix(
4,

27
wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
View File

@@ -80,6 +80,33 @@ public class QuinticHermiteSpline extends Spline {
*/
private SimpleMatrix makeHermiteBasis() {
if (hermiteBasis == null) {
// Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
// vectors, we want to find the coefficients of the spline
// P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
//
// P(i) = P(0) = a0
// P'(i) = P'(0) = a1
// P''(i) = P''(0) = 2 * a2
// P(i+1) = P(1) = a5 + a4 + a3 + a2 + a1 + a0
// P'(i+1) = P'(1) = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
// P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
//
// [ P(i) ] = [ 0 0 0 0 0 1 ][ a5 ]
// [ P'(i) ] = [ 0 0 0 0 1 0 ][ a4 ]
// [ P''(i) ] = [ 0 0 0 2 0 0 ][ a3 ]
// [ P(i+1) ] = [ 1 1 1 1 1 1 ][ a2 ]
// [ P'(i+1) ] = [ 5 4 3 2 1 0 ][ a1 ]
// [ P''(i+1) ] = [ 20 12 6 2 0 0 ][ a0 ]
//
// To solve for the coefficients, we can invert the 6x6 matrix and move it
// to the other side of the equation.
//
// [ a5 ] = [ -6.0 -3.0 -0.5 6.0 -3.0 0.5 ][ P(i) ]
// [ a4 ] = [ 15.0 8.0 1.5 -15.0 7.0 -1.0 ][ P'(i) ]
// [ a3 ] = [ -10.0 -6.0 -1.5 10.0 -4.0 0.5 ][ P''(i) ]
// [ a2 ] = [ 0.0 0.0 0.5 0.0 0.0 0.0 ][ P(i+1) ]
// [ a1 ] = [ 0.0 1.0 0.0 0.0 0.0 0.0 ][ P'(i+1) ]
// [ a0 ] = [ 1.0 0.0 0.0 0.0 0.0 0.0 ][ P''(i+1) ]
hermiteBasis =
new SimpleMatrix(
6,