public static void interpolate ( Se2_F64 a , Se2_F64 b , double where , Se2_F64 output ) { double w0 = 1.0 - where ; output . T . x = a . T . x * w0 + b . T . x * where ; output . T . y = a . T . y * w0 + b . T . y * where ; // interpolating rotation is more difficult // This only works well if the difference between the two angles is small double yaw0 = a . getYaw ( ) ; double yaw1 = b . getYaw ( ) ; double cw = UtilAngle . distanceCW ( yaw0 , yaw1 ) ; double ccw = UtilAngle . distanceCCW ( yaw0 , yaw1 ) ; double yaw ; if ( cw > ccw ) { yaw = yaw0 + ccw * where ; } else { yaw = yaw0 - cw * where ; } output . setYaw ( yaw ) ; }
Perform linear interpolation