main/src/georegression/transform/se/InterpolateLinearSe2_F64.java
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);
	}
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);
	}
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

@param a The first transform
@param b The second transform
@param where from 0 to 1, inclusive.  0 means it will be more similar to 'a'.
@param output The interpolated transform
Perform linear interpolation