@Override public String getOrientation(int index) { StringBuilder s = new StringBuilder(); s.append(setCentroid()); // calculate orientation Quat4d q = new Quat4d(); q.set(polyhedron.getViewMatrix(index)); q.normalize(); Quat4d r = new Quat4d(); r.set(rotationAxisAligner.getRotationMatrix()); r.normalize(); q.mul(r); q.normalize(); // set orientation s.append("moveto 4 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("}"); s.append(";"); return s.toString(); }
@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(); }
public static void packJMEQuaterionInVecMathQuat4d(Quaternion original, Quat4d target) { target.set(original.getX(), original.getY(), original.getZ(), original.getW()); // do not remove the normalization. // The conversion from float to double generates very tiny differences which make the // quaternion SLIGHTLY not normal. target.normalize(); }
/** * Normalize the quaternion and also limits the described angle magnitude in [-Pi, Pi]. * The latter prevents some controllers to poop their pants. */ public void normalizeAndLimitToPiMinusPi() { // Quat4d.normalize() turns it into zero if it contains NaN. This needs to be fixed. Should stay NaN... if (this.containsNaN()) return; if (normSquared() < 1.0e-7) setToZero(); else { super.normalize(); if (this.getW() < 0.0) this.negate(); } }
/** * 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; }
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); }
quatFDDelta.normalize();