public void transform(Quat4d q, Quat4d qToTransform, Quat4d resultToPack) { resultToPack.mul(q, qToTransform); resultToPack.mulInverse(q); }
/** * 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 multiply(Vector3d v, Quat4d q, Quat4d resultToPack) { setAsPureQuaternion(v, resultToPack); resultToPack.mul(q); }
public void multiply(Quat4d q, Vector3d v, Quat4d resultToPack) { setAsPureQuaternion(v, resultToPack); resultToPack.mul(q, resultToPack); }
public void multiply(Quat4d q1, Quat4d q2, Vector3d resultToPack) { pureQuatForMultiply.mul(q1, q2); setVectorFromPureQuaternion(pureQuatForMultiply, resultToPack); }
public void inverseMultiply(Quat4d q1, Quat4d q2, Quat4d resultToPack) { qInv.conjugate(q1); resultToPack.mul(qInv, q2); }
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); }
/** * Multiplies this quaternion by the inverse of quaternion q1 and places * the value into this quaternion. The value of the argument quaternion * is preserved (this = this * q^-1). * @param q1 the other quaternion */ public final void mulInverse(Quat4d q1) { Quat4d tempQuat = new Quat4d(q1); tempQuat.inverse(); this.mul(tempQuat); }
/** * Multiplies quaternion q1 by the inverse of quaternion q2 and places * the value into this quaternion. The value of both argument quaternions * is preservered (this = q1 * q2^-1). * @param q1 the first quaternion * @param q2 the second quaternion */ public final void mulInverse(Quat4d q1, Quat4d q2) { Quat4d tempQuat = new Quat4d(q2); tempQuat.inverse(); this.mul(q1, tempQuat); }
private void interpolateOrientation(double time, Quat4d qInterpolated) { controlQuaternions[0].get(qInterpolated); updateBezierCoefficients(time); for (int i = 1; i <= 3; i++) { computeBezierQuaternionCurveTerm(i, tempQuatForInterpolation); qInterpolated.mul(tempQuatForInterpolation); } }
@Override public void applyTransform(RigidBodyTransform transform) { if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(angularVelocity); }
public void interpolate(double alpha, Quat4d q0, Quat4d q1, Quat4d qInterpolatedToPack, boolean preventExtraSpin) { tempQ1ForInterpolation.set(q1); if (preventExtraSpin && dot(q0, tempQ1ForInterpolation) < 0.0) { tempQ1ForInterpolation.negate(); } computeQuaternionDifference(q0, tempQ1ForInterpolation, qInterpolatedToPack); pow(qInterpolatedToPack, alpha, qInterpolatedToPack); qInterpolatedToPack.mul(q0, qInterpolatedToPack); }
@Override public void applyTransform(RigidBodyTransform transform) { transform.transform(position); if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(linearVelocity); transform.transform(angularVelocity); }
private void corruptOrientation(Quat4d orientation) { Vector3d axis = RandomTools.generateRandomVector(random); double angle = RandomTools.generateRandomDouble(random, -maxRotationCorruption, maxRotationCorruption); AxisAngle4d axisAngle4d = new AxisAngle4d(); axisAngle4d.set(axis, angle); Quat4d corruption = new Quat4d(); corruption.set(axisAngle4d); orientation.mul(corruption); }
public void correctState(DenseMatrix64F correction) { orientationPort.getData().getQuaternion(quaternion); MatrixTools.extractTuple3dFromEJMLVector(tempRotationVector, correction, 0); RotationTools.convertRotationVectorToAxisAngle(tempRotationVector, tempAxisAngle); quaternionDelta.set(tempAxisAngle); quaternion.mul(quaternionDelta); orientation.set(quaternion); orientationPort.setData(orientation); } }
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); } }
public void startComputation() { frameUsedForPerfectOrientation.getTransformToDesiredFrame(worldFrame).getRotation(rotationMatrix); yoFrameQuaternionPerfect.set(rotationMatrix); corrupt(rotationMatrix); orientationOutputPort.setData(rotationMatrix); yoFrameQuaternionNoisy.set(rotationMatrix); yoFrameQuaternionPerfect.get(tempQuaternionOne); yoFrameQuaternionNoisy.get(tempQuaternionTwo); tempQuaternionTwo.inverse(); tempQuaternionOne.mul(tempQuaternionTwo); tempAxisAngle.set(tempQuaternionOne); double noiseAngle = tempAxisAngle.getAngle(); if (noiseAngle > Math.PI) noiseAngle = noiseAngle - 2.0 * Math.PI; if (noiseAngle < -Math.PI) noiseAngle = noiseAngle + 2.0 * Math.PI; rotationAngleNoise.set(Math.abs(noiseAngle)); }
public void propagateState(double dt) { angularVelocityPort.getData().get(angularVelocity); orientationPort.getData().getQuaternion(quaternion); tempRotationVector.set(angularVelocity); tempRotationVector.scale(dt); RotationTools.convertRotationVectorToAxisAngle(tempRotationVector, tempAxisAngle); quaternionDelta.set(tempAxisAngle); quaternion.mul(quaternionDelta); quaternion.normalize(); // the previous operation should preserve norm, so this might not be necessary every step orientation.set(quaternion); orientationPort.setData(orientation); }
private void setYawPitchRoll() { Quat4d q = new Quat4d(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). RotationTools.convertYawPitchRollToQuaternion(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue(), q); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.mul(qprev); q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }
private void setYawPitchRoll() { Quat4d q = new Quat4d(); //This code has a singularity when yaw and roll line up (e.g. pitch is 90, can't rotate in one direction any more). RotationTools.convertYawPitchRollToQuaternion(q_yaw.getDoubleValue(), q_pitch.getDoubleValue(), q_roll.getDoubleValue(), q); //This code compounds the rotations so that on subsequent frames the ability to rotate in lost rotation directions is regained //This affectively uses global yaw pitch and roll each time. q.mul(qprev); q_qs.set(q.getW()); q_qx.set(q.getX()); q_qy.set(q.getY()); q_qz.set(q.getZ()); }