public void computeQDot(Quat4d q, Vector3d angularVelocity, Quat4d qDotToPack) { multiply(angularVelocity, q, qDotToPack); qDotToPack.scale(0.5); }
public void computeQDotByFiniteDifferenceCentral(Quat4d qPrevious, Quat4d qNext, double dt, Quat4d qDotToPack) { qDotToPack.set(qNext); qDotToPack.sub(qPrevious); qDotToPack.scale(0.5 / dt); }
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)); }
/** * This computes: resultToPack = log(q) * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result and is a pure quaternion (w = 0.0). */ public void log(Quat4d q, Quat4d resultToPack) { axisAngleForLog.set(q); resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0); resultToPack.scale(axisAngleForLog.getAngle()); }