/** * Rotates qToTransform by the rotation described by q: qToTransform = q qToTransform q^-1 * Assumes that q is a unit-quaternion and describes a orientation. * Assumes that qToTransform is a pure-quaternion. */ public void transform(Quat4d q, Quat4d qToTransform) { transform(q, qToTransform, qToTransform); }
public void transform(Quat4d q, Vector3d vectorToTransform) { transform(q, vectorToTransform, vectorToTransform); }
public void invertTransform(Quat4d q, Vector3d vectorToTransform, Vector3d resultToPack) { qInv.conjugate(q); transform(qInv, vectorToTransform, resultToPack); }
public void invertTransform(Quat4d q, Quat4d qToTransform, Quat4d resultToPack) { qInv.conjugate(q); transform(qInv, qToTransform, resultToPack); }
private void computeAngularVelocity(Vector3d angularVelocityToPack, Quat4d startRotationQuaternion, Quat4d endRotationQuaternion, double alphaDot) { if (quaternionCalculus.dot(startRotationQuaternion, endRotationQuaternion) < 0.0) endRotationQuaternion.negate(); // compute relative orientation: orientation of interpolated frame w.r.t. start frame relativeRotationQuaternion.set(startRotationQuaternion); // R_W_S: orientation of start w.r.t. world relativeRotationQuaternion.conjugate(); // R_S_W: orientation of world w.r.t. start relativeRotationQuaternion.mul(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); quaternionCalculus.transform(endRotationQuaternion, angularVelocityToPack); } }
linearVelocity.scale(maximumSixDoFJointLinearVelocity / linearVelocityMagnitude); translationIntegrated.scale(controlDT, linearVelocity); quaternionCalculus.transform(rotation, translationIntegrated);