public void interpolate ( double where , Se3_F64 output ) { rotation . setTheta ( where * rotMagnitude ) ; ConvertRotation3D_F64 . rodriguesToMatrix ( rotation , R ) ; output . T . x = initial . T . x + where * translation . x ; output . T . y = initial . T . y + where * translation . y ; output . T . z = initial . T . z + where * translation . z ; CommonOps_DDRM . mult ( initial . R , R , output . R ) ; }
Interpolates a value between the first and second transform . A value close to 0 will be more similar to the initial and 1 more similar to the end .