public void update(Quat4d qMeasured) { if (hasBeenCalled.getBooleanValue()) { get(qPreviousFiltered); qNewFiltered.interpolate(qMeasured, qPreviousFiltered, alpha.getDoubleValue()); // qPreviousFiltered 'gets multiplied by alpha' set(qNewFiltered); } else { set(qMeasured); hasBeenCalled.set(true); } }
/** * Computes the interpolation between the two transforms using the alpha parameter to control the blend. * Note that the transforms must have a proper rotation matrix, meaning it satisfies: R'R = I and det(R) = 1 * @param transform1 * @param transform2 * @param alpha Ranges from [0, 1], where return = (1- alpha) * tansform1 + (alpha) * transform2 * @return return = (1- alpha) * tansform1 + alpha * transform2 */ public void computeInterpolation(RigidBodyTransform transform1, RigidBodyTransform transform2, RigidBodyTransform result, double alpha) { alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); transform1.get(transform1Quaternion, transform1Translation); transform2.get(transform2Quaternion, transform2Translation); interpolatedTranslation.interpolate(transform1Translation, transform2Translation, alpha); interpolatedQuaternion.interpolate(transform1Quaternion, transform2Quaternion, alpha); result.setRotationAndZeroTranslation(interpolatedQuaternion); result.setTranslation(interpolatedTranslation); }