@Override public Quat4d initialValue() { return new Quat4d(); } };
/** * 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); }
public void transform(Quat4d q, Quat4d qToTransform, Quat4d resultToPack) { resultToPack.mul(q, qToTransform); resultToPack.mulInverse(q); }
@Override public String getDefaultOrientation() { StringBuilder s = new StringBuilder(); s.append(setCentroid()); // calculate orientation Quat4d q = new Quat4d(); q.set(polyhedron.getViewMatrix(0)); q.normalize(); Quat4d r = new Quat4d(); r.set(rotationAxisAligner.getRotationMatrix()); r.normalize(); q.mul(r); q.normalize(); // set orientation s.append("moveto 0 quaternion{"); s.append(jMolFloat(q.x)); s.append(","); s.append(jMolFloat(q.y)); s.append(","); s.append(jMolFloat(q.z)); s.append(","); s.append(jMolFloat(q.w)); s.append("};"); return s.toString(); }
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); }
private void computeOrientationError() { orientationEstimator.getEstimatedOrientation(estimatedOrientation); Quat4d estimatedOrientationQuat4d = new Quat4d(); estimatedOrientation.getQuaternion(estimatedOrientationQuat4d); Quat4d actualOrientation = new Quat4d(); estimationJoint.getRotationToWorld(actualOrientation); if (((estimatedOrientationQuat4d.getW() > 0.0) && (actualOrientation.getW() < 0.0)) || ((estimatedOrientationQuat4d.getW() < 0.0) && (actualOrientation.getW() > 0.0))) { actualOrientation.negate(); } perfectOrientation.set(actualOrientation); Quat4d orientationErrorQuat4d = new Quat4d(actualOrientation); orientationErrorQuat4d.mulInverse(estimatedOrientationQuat4d); AxisAngle4d orientationErrorAxisAngle = new AxisAngle4d(); orientationErrorAxisAngle.set(orientationErrorQuat4d); double errorAngle = AngleTools.trimAngleMinusPiToPi(orientationErrorAxisAngle.getAngle()); orientationError.set(Math.abs(errorAngle)); }
@Override public void applyTransform(RigidBodyTransform transform) { if (tempQuaternionForTransform == null) tempQuaternionForTransform = new Quat4d(); transform.getRotation(tempQuaternionForTransform); orientation.mul(tempQuaternionForTransform, orientation); transform.transform(angularVelocity); }
/** * 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; }
rotationError.setW(1); rotationError.setX(rand.nextGaussian() * quaternionNoiseStd); rotationError.setY(rand.nextGaussian() * quaternionNoiseStd); rotationError.setZ(rand.nextGaussian() * quaternionNoiseStd); rotationError.normalize(); rotationFilter.scale(1 - alpha); rotationError.scale(alpha); rotationFilter.add(rotationError); rotationFilter.normalize();
@Override public void setToNaN() { super.set(Double.NaN, Double.NaN, Double.NaN, Double.NaN); }
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 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); }
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); }
public void multiply(Quat4d q, Vector3d v, Quat4d resultToPack) { setAsPureQuaternion(v, resultToPack); resultToPack.mul(q, resultToPack); }