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); } }
private void computeAngularVelocity(Vector3D angularVelocityToPack, Quaternion startRotationQuaternion, Quaternion endRotationQuaternion, double alphaDot) { if (startRotationQuaternion.dot(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.multiply(endRotationQuaternion); // R_S_I = R_S_W * R_W_I: orientation of interpolated w.r.t. start quaternionCalculus.log(relativeRotationQuaternion, angularVelocityToPack); angularVelocityToPack.scale(alphaDot); endRotationQuaternion.transform(angularVelocityToPack); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLogAndExpAlgebra() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); Vector4D qLog = new Vector4D(); Quaternion vExp = new Quaternion(); quaternionCalculus.log(q, qLog); Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); quaternionCalculus.exp(v, vExp); assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); } }
previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt; maxVelocityRecorded = Math.max(maxVelocityRecorded, velocityFD);
previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt; maxVelocityRecorded = Math.max(maxVelocityRecorded, velocityFD);
previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt; maxVelocityRecorded = Math.max(maxVelocityRecorded, velocityFD);
previousQuaternion.negate(); currentQuaternion.multiplyConjugateOther(previousQuaternion); quaternionCalculus.log(currentQuaternion, delta); double velocityFD = delta.length() / dt; maxVelocityRecorded = Math.max(maxVelocityRecorded, velocityFD);
quaternionCalculus.log(tempQuatForControlQuats, tempAngularVelocity); controlAngularVelocities[i].set(tempAngularVelocity);