public void computeQDDot(Quat4d q, Quat4d qDot, Vector3d angularVelocity, Vector3d angularAcceleration, Quat4d qDDotToPack) { multiply(angularAcceleration, q, intermediateQDDot); multiply(angularVelocity, qDot, qDDotToPack); qDDotToPack.add(intermediateQDDot); qDDotToPack.scale(0.5); }
public void computeQDDotByFiniteDifferenceCentral(Quat4d qPrevious, Quat4d q, Quat4d qNext, double dt, Quat4d qDDotToPack) { qDDotToPack.set(qNext); qDDotToPack.sub(q); qDDotToPack.sub(q); qDDotToPack.add(qPrevious); qDDotToPack.scale(1.0 / MathTools.square(dt)); }
rotationFilter.scale(1 - alpha); rotationError.scale(alpha); rotationFilter.add(rotationError); rotationFilter.normalize();