package net.sf.openrocket.util;
+import net.sf.openrocket.logging.LogHelper;
+import net.sf.openrocket.startup.Application;
+
+/**
+ * An immutable quaternion class.
+ *
+ * @author Sampo Niskanen <sampo.niskanen@iki.fi>
+ */
+public class Quaternion {
+ private static final LogHelper log = Application.getLogger();
+
+
+ //////// Debug section
+ /*
+ * Debugging info. If openrocket.debug.quaternioncount is defined, a line is
+ * printed every 1000000 instantiations (or as many as defined).
+ */
+ private static final boolean COUNT_DEBUG;
+ private static final int COUNT_DIFF;
+ static {
+ String str = System.getProperty("openrocket.debug.quaternioncount");
+ int diff = 0;
+ if (str == null) {
+ COUNT_DEBUG = false;
+ COUNT_DIFF = 0;
+ } else {
+ COUNT_DEBUG = true;
+ try {
+ diff = Integer.parseInt(str);
+ } catch (NumberFormatException ignore) {
+ }
+ if (diff < 1000)
+ diff = 1000000;
+ COUNT_DIFF = diff;
+ }
+ }
+
+ private static int count = 0;
+ {
+ // Debug count
+ if (COUNT_DEBUG) {
+ synchronized (Quaternion.class) {
+ count++;
+ if ((count % COUNT_DIFF) == 0) {
+ log.debug("Quaternion instantiated " + count + " times.");
+ }
+ }
+ }
+ }
+
+ //////// End debug section
+
-public class Quaternion implements Cloneable {
- protected double w, x, y, z;
- protected int modCount = 0;
+ private final double w, x, y, z;
+ private double norm = -1;
+
+ /**
+ * Construct a new "one" quaternion. This is equivalent to <code>new Quaternion(1,0,0,0)</code>.
+ */
public Quaternion() {
- this(1,0,0,0);
+ this(1, 0, 0, 0);
}
+
public Quaternion(double w, double x, double y, double z) {
this.w = w;
this.x = x;
}
+ /**
+ * Create a rotation quaternion corresponding to the rotation vector. The rotation axis is
+ * the direction of vector, and rotation angle is the length of the vector.
+ * <p>
+ * The cost of the operation is approximately that of computing the length of the coordinate
+ * and computing two trigonometric functions.
+ *
+ * @param rotation the rotation vector
+ * @return the quaternion corresponding to the rotation vector
+ */
public static Quaternion rotation(Coordinate rotation) {
double length = rotation.length();
if (length < 0.000001) {
- return new Quaternion(1,0,0,0);
+ return new Quaternion(1, 0, 0, 0);
}
- double sin = Math.sin(length/2);
- double cos = Math.cos(length/2);
- return new Quaternion(cos,
- sin*rotation.x/length, sin*rotation.y/length, sin*rotation.z/length);
+ double sin = Math.sin(length / 2);
+ double cos = Math.cos(length / 2);
+ return new Quaternion(cos,
+ sin * rotation.x / length, sin * rotation.y / length, sin * rotation.z / length);
}
+ /**
+ * Create a rotation quaternion corresponding to the rotation around the provided vector with
+ * the provided angle.
+ *
+ * @param axis the rotation axis
+ * @param angle the rotation angle
+ * @return the corresponding quaternion
+ */
public static Quaternion rotation(Coordinate axis, double angle) {
Coordinate a = axis.normalize();
double sin = Math.sin(angle);
double cos = Math.cos(angle);
- return new Quaternion(cos, sin*a.x, sin*a.y, sin*a.z);
+ return new Quaternion(cos, sin * a.x, sin * a.y, sin * a.z);
}
-
+
public double getW() {
return w;
}
-
- public void setW(double w) {
- this.w = w;
- modCount++;
- }
-
+
public double getX() {
return x;
}
-
- public void setX(double x) {
- this.x = x;
- modCount++;
- }
-
+
public double getY() {
return y;
}
-
- public void setY(double y) {
- this.y = y;
- modCount++;
- }
-
+
public double getZ() {
return z;
}
-
- public void setZ(double z) {
- this.z = z;
- modCount++;
- }
- public void setAll(double w, double x, double y, double z) {
- this.w = w;
- this.x = x;
- this.y = y;
- this.z = z;
- modCount++;
+ /**
+ * Check whether any of the quaternion values is NaN.
+ *
+ * @return true if w, x, y or z is NaN
+ */
+ public boolean isNaN() {
+ return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z) || Double.isNaN(w);
}
-
+
/**
* Multiply this quaternion by the other quaternion from the right side. This
- * calculates the product <code>this = this * other</code>.
+ * calculates the product <code>result = this * other</code>.
*
* @param other the quaternion to multiply this quaternion by.
* @return this quaternion.
*/
public Quaternion multiplyRight(Quaternion other) {
- double w = (this.w*other.w - this.x*other.x - this.y*other.y - this.z*other.z);
- double x = (this.w*other.x + this.x*other.w + this.y*other.z - this.z*other.y);
- double y = (this.w*other.y + this.y*other.w + this.z*other.x - this.x*other.z);
- double z = (this.w*other.z + this.z*other.w + this.x*other.y - this.y*other.x);
+ double newW = (this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z);
+ double newX = (this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y);
+ double newY = (this.w * other.y + this.y * other.w + this.z * other.x - this.x * other.z);
+ double newZ = (this.w * other.z + this.z * other.w + this.x * other.y - this.y * other.x);
- this.w = w;
- this.x = x;
- this.y = y;
- this.z = z;
- return this;
+ return new Quaternion(newW, newX, newY, newZ);
}
/**
* Multiply this quaternion by the other quaternion from the left side. This
- * calculates the product <code>this = other * this</code>.
+ * calculates the product <code>result = other * this</code>.
*
* @param other the quaternion to multiply this quaternion by.
* @return this quaternion.
*/
public Quaternion multiplyLeft(Quaternion other) {
/* other(abcd) * this(wxyz) */
+
+ double newW = (other.w * this.w - other.x * this.x - other.y * this.y - other.z * this.z);
+ double newX = (other.w * this.x + other.x * this.w + other.y * this.z - other.z * this.y);
+ double newY = (other.w * this.y + other.y * this.w + other.z * this.x - other.x * this.z);
+ double newZ = (other.w * this.z + other.z * this.w + other.x * this.y - other.y * this.x);
- double w = (other.w*this.w - other.x*this.x - other.y*this.y - other.z*this.z);
- double x = (other.w*this.x + other.x*this.w + other.y*this.z - other.z*this.y);
- double y = (other.w*this.y + other.y*this.w + other.z*this.x - other.x*this.z);
- double z = (other.w*this.z + other.z*this.w + other.x*this.y - other.y*this.x);
-
- this.w = w;
- this.x = x;
- this.y = y;
- this.z = z;
- return this;
+ return new Quaternion(newW, newX, newY, newZ);
}
-
-
-
- @Override
- public Quaternion clone() {
- try {
- return (Quaternion) super.clone();
- } catch (CloneNotSupportedException e) {
- throw new RuntimeException("CloneNotSupportedException encountered");
- }
- }
-
/**
- * Normalize this quaternion. After the call the norm of the quaternion is exactly
- * one. If this quaternion is the zero quaternion, throws
- * <code>IllegalStateException</code>. Returns this quaternion.
+ * Return a normalized version of this quaternion. If this quaternion is the zero quaternion, throws
+ * <code>IllegalStateException</code>.
*
- * @return this quaternion.
+ * @return a normalized version of this quaternion.
* @throws IllegalStateException if the norm of this quaternion is zero.
*/
public Quaternion normalize() {
- double norm = norm();
- if (norm < 0.0000001) {
+ double n = norm();
+ if (n < 0.0000001) {
throw new IllegalStateException("attempting to normalize zero-quaternion");
}
- x /= norm;
- y /= norm;
- z /= norm;
- w /= norm;
- return this;
+ return new Quaternion(w / n, x / n, y / n, z / n);
}
/**
- * Normalize this quaternion if the norm is more than 1ppm from one.
+ * Normalize the quaternion if the norm is more than 1ppm from one.
*
- * @return this quaternion.
+ * @return this quaternion or a normalized version of this quaternion.
* @throws IllegalStateException if the norm of this quaternion is zero.
*/
public Quaternion normalizeIfNecessary() {
double n2 = norm2();
- if (n2 < 0.999999 || n2 > 1.000001)
- normalize();
- return this;
+ if (n2 < 0.999999 || n2 > 1.000001) {
+ return normalize();
+ } else {
+ return this;
+ }
}
-
+
/**
* Return the norm of this quaternion.
*
* @return the norm of this quaternion sqrt(w^2 + x^2 + y^2 + z^2).
*/
public double norm() {
- return Math.sqrt(x*x + y*y + z*z + w*w);
+ if (norm < 0) {
+ norm = MathUtil.safeSqrt(x * x + y * y + z * z + w * w);
+ }
+ return norm;
}
/**
* @return the square of the norm of this quaternion (w^2 + x^2 + y^2 + z^2).
*/
public double norm2() {
- return x*x + y*y + z*z + w*w;
+ return x * x + y * y + z * z + w * w;
}
+ /**
+ * Perform a coordinate rotation using this unit quaternion. The result is
+ * <code>this * coord * this^(-1)</code>.
+ * <p>
+ * This method assumes that the norm of this quaternion is one.
+ *
+ * @param coord the coordinate to rotate.
+ * @return the rotated coordinate.
+ */
public Coordinate rotate(Coordinate coord) {
- double a,b,c,d;
+ double a, b, c, d;
- assert(Math.abs(norm2()-1) < 0.00001) : "Quaternion not unit length: "+this;
+ assert (Math.abs(norm2() - 1) < 0.00001) : "Quaternion not unit length: " + this;
- a = - x * coord.x - y * coord.y - z * coord.z; // w
- b = w * coord.x + y * coord.z - z * coord.y; // x i
- c = w * coord.y - x * coord.z + z * coord.x; // y j
- d = w * coord.z + x * coord.y - y * coord.x; // z k
+
+ // (a,b,c,d) = this * coord = (w,x,y,z) * (0,cx,cy,cz)
+ a = -x * coord.x - y * coord.y - z * coord.z; // w
+ b = w * coord.x + y * coord.z - z * coord.y; // x i
+ c = w * coord.y - x * coord.z + z * coord.x; // y j
+ d = w * coord.z + x * coord.y - y * coord.x; // z k
+
+
+ // return = (a,b,c,d) * (this)^-1 = (a,b,c,d) * (w,-x,-y,-z)
+
+ // Assert that the w-value is zero
+ assert (Math.abs(a * w + b * x + c * y + d * z) < coord.max() * MathUtil.EPSILON) : ("Should be zero: " + (a * w + b * x + c * y + d * z) + " in " + this + " c=" + coord);
- assert(MathUtil.equals(a*w + b*x + c*y + d*z, 0)) :
- ("Should be zero: " + (a*w - b*x - c*y - d*z) + " in " + this + " c=" + coord);
-
return new Coordinate(
- - a*x + b*w - c*z + d*y,
- - a*y + b*z + c*w - d*x,
- - a*z - b*y + c*x + d*w,
- coord.weight
- );
+ -a * x + b * w - c * z + d * y,
+ -a * y + b * z + c * w - d * x,
+ -a * z - b * y + c * x + d * w,
+ coord.weight);
}
+ /**
+ * Perform an inverse coordinate rotation using this unit quaternion. The result is
+ * <code>this^(-1) * coord * this</code>.
+ * <p>
+ * This method assumes that the norm of this quaternion is one.
+ *
+ * @param coord the coordinate to rotate.
+ * @return the rotated coordinate.
+ */
public Coordinate invRotate(Coordinate coord) {
- double a,b,c,d;
-
- assert(Math.abs(norm2()-1) < 0.00001) : "Quaternion not unit length: "+this;
-
- a = + x * coord.x + y * coord.y + z * coord.z;
- b = w * coord.x - y * coord.z + z * coord.y;
- c = w * coord.y + x * coord.z - z * coord.x;
- d = w * coord.z - x * coord.y + y * coord.x;
+ double a, b, c, d;
+
+ assert (Math.abs(norm2() - 1) < 0.00001) : "Quaternion not unit length: " + this;
- assert(MathUtil.equals(a*w - b*x - c*y - d*z, 0)):
- ("Should be zero: " + (a*w - b*x - c*y - d*z) + " in " + this + " c=" + coord);
+ // (a,b,c,d) = (this)^-1 * coord = (w,-x,-y,-z) * (0,cx,cy,cz)
+ a = +x * coord.x + y * coord.y + z * coord.z;
+ b = w * coord.x - y * coord.z + z * coord.y;
+ c = w * coord.y + x * coord.z - z * coord.x;
+ d = w * coord.z - x * coord.y + y * coord.x;
+
+
+ // return = (a,b,c,d) * this = (a,b,c,d) * (w,x,y,z)
+ assert (Math.abs(a * w - b * x - c * y - d * z) < Math.max(coord.max(), 1) * MathUtil.EPSILON) : ("Should be zero: " + (a * w - b * x - c * y - d * z) + " in " + this + " c=" + coord);
return new Coordinate(
- a*x + b*w + c*z - d*y,
- a*y - b*z + c*w + d*x,
- a*z + b*y - c*x + d*w,
- coord.weight
- );
+ a * x + b * w + c * z - d * y,
+ a * y - b * z + c * w + d * x,
+ a * z + b * y - c * x + d * w,
+ coord.weight);
}
*/
public Coordinate rotateZ() {
return new Coordinate(
- 2*(w*y + x*z),
- 2*(y*z - w*x),
- w*w - x*x - y*y + z*z
- );
+ 2 * (w * y + x * z),
+ 2 * (y * z - w * x),
+ w * w - x * x - y * y + z * z);
}
@Override
public String toString() {
- return String.format("Quaternion[%f,%f,%f,%f,norm=%f]",w,x,y,z,this.norm());
+ return String.format("Quaternion[%f,%f,%f,%f,norm=%f]", w, x, y, z, this.norm());
}
public static void main(String[] arg) {
- Quaternion q = new Quaternion(Math.random()-0.5,Math.random()-0.5,
- Math.random()-0.5,Math.random()-0.5);
+ Quaternion q = new Quaternion(Math.random() - 0.5, Math.random() - 0.5,
+ Math.random() - 0.5, Math.random() - 0.5);
q.normalize();
- q = new Quaternion(-0.998717,0.000000,0.050649,-0.000000);
+ q = new Quaternion(-0.998717, 0.000000, 0.050649, -0.000000);
- Coordinate coord = new Coordinate(10*(Math.random()-0.5),
- 10*(Math.random()-0.5), 10*(Math.random()-0.5));
+ Coordinate coord = new Coordinate(10 * (Math.random() - 0.5),
+ 10 * (Math.random() - 0.5), 10 * (Math.random() - 0.5));
- System.out.println("Quaternion: "+q);
- System.out.println("Coordinate: "+coord);
+ System.out.println("Quaternion: " + q);
+ System.out.println("Coordinate: " + coord);
coord = q.invRotate(coord);
- System.out.println("Rotated: "+ coord);
+ System.out.println("Rotated: " + coord);
coord = q.rotate(coord);
- System.out.println("Back: "+coord);
+ System.out.println("Back: " + coord);
-// Coordinate c = new Coordinate(0,1,0);
-// Coordinate rot = new Coordinate(Math.PI/4,0,0);
-//
-// System.out.println("Before: "+c);
-// c = rotation(rot).invRotate(c);
-// System.out.println("After: "+c);
+
+ q = new Quaternion(0.237188, 0.570190, -0.514542, 0.594872);
+ q.normalize();
+ Coordinate c = new Coordinate(148578428.914, 8126778.954, -607.741);
+ System.out.println("Rotated: " + q.rotate(c));
+ // Coordinate c = new Coordinate(0,1,0);
+ // Coordinate rot = new Coordinate(Math.PI/4,0,0);
+ //
+ // System.out.println("Before: "+c);
+ // c = rotation(rot).invRotate(c);
+ // System.out.println("After: "+c);
+
+
}
}
projects / debian / openrocket / blobdiff