public void invertTransform(Quat4d q, Vector3d vectorToTransform, Vector3d resultToPack) { qInv.conjugate(q); transform(qInv, vectorToTransform, resultToPack); }
public void inverseMultiply(Quat4d q1, Quat4d q2, Quat4d resultToPack) { qInv.conjugate(q1); resultToPack.mul(qInv, q2); }
public void invertTransform(Quat4d q, Quat4d qToTransform, Quat4d resultToPack) { qInv.conjugate(q); transform(qInv, qToTransform, resultToPack); }
/** * This computes the product: resultToPack = (q0^-1 q1) */ public void computeQuaternionDifference(Quat4d q0, Quat4d q1, Quat4d resultToPack) { resultToPack.conjugate(q0); resultToPack.mul(q1); }
public void computeAngularAcceleration(Quat4d q, Quat4d qDot, Quat4d qDDot, Vector3d angularAccelerationToPack) { qConj.conjugate(q); qDotConj.conjugate(qDot); multiply(qDot, qDotConj, intermediateAngularAcceleration); multiply(qDDot, qConj, angularAccelerationToPack); angularAccelerationToPack.add(intermediateAngularAcceleration); angularAccelerationToPack.scale(2.0); }
public void computeAngularVelocityInBodyFixedFrame(Quat4d q, Quat4d qDot, Vector3d angularVelocityToPack) { qConj.conjugate(q); multiply(qConj, qDot, angularVelocityToPack); angularVelocityToPack.scale(2.0); }
public void computeAngularVelocityInWorldFrame(Quat4d q, Quat4d qDot, Vector3d angularVelocityToPack) { qConj.conjugate(q); multiply(qDot, qConj, angularVelocityToPack); angularVelocityToPack.scale(2.0); }
public void transform(Quat4d q, Vector3d vectorToTransform, Vector3d resultToPack) { setAsPureQuaternion(vectorToTransform, pureQuatForTransform); pureQuatForTransform.mul(q, pureQuatForTransform); qInv.conjugate(q); pureQuatForTransform.mul(qInv); setVectorFromPureQuaternion(pureQuatForTransform, 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); } }
/** * Calculate the relative quaternion orientation of two arrays of points. * * @param fixed * point array, coordinates will not be modified * @param moved * point array, coordinates will not be modified * @return a unit quaternion representing the relative orientation, to * rotate moved to bring it to the same orientation as fixed. */ public static Quat4d relativeOrientation(Point3d[] fixed, Point3d[] moved) { Matrix m = CalcPoint.formMatrix(moved, fixed); // inverse EigenvalueDecomposition eig = m.eig(); double[][] v = eig.getV().getArray(); Quat4d q = new Quat4d(v[1][3], v[2][3], v[3][3], v[0][3]); q.normalize(); q.conjugate(); return q; }