X-Git-Url: https://git.gag.com/?a=blobdiff_plain;ds=sidebyside;f=src%2Fnet%2Fsf%2Fopenrocket%2Futil%2FQuaternion.java;h=5f2776626c26fcd0764be7a33a8e8fee717f72cd;hb=8320c04afa30e2aa0150adc870d02abeedb01066;hp=3c457caba5aa7c85508c9fe6969f6d31762cd1b5;hpb=720d4935bc6bec453e6478ad5227356c626610a2;p=debian%2Fopenrocket diff --git a/src/net/sf/openrocket/util/Quaternion.java b/src/net/sf/openrocket/util/Quaternion.java index 3c457cab..5f277662 100644 --- a/src/net/sf/openrocket/util/Quaternion.java +++ b/src/net/sf/openrocket/util/Quaternion.java @@ -1,15 +1,71 @@ package net.sf.openrocket.util; +import net.sf.openrocket.logging.LogHelper; +import net.sf.openrocket.startup.Application; + +/** + * An immutable quaternion class. + * + * @author Sampo Niskanen + */ +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 new Quaternion(1,0,0,0). + */ 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; @@ -18,172 +74,151 @@ public class Quaternion implements Cloneable { } + /** + * 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. + *

+ * 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 this = this * other. + * calculates the product result = this * other. * * @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 this = other * this. + * calculates the product result = other * this. * * @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 - * IllegalStateException. Returns this quaternion. + * Return a normalized version of this quaternion. If this quaternion is the zero quaternion, throws + * IllegalStateException. * - * @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; } /** @@ -192,50 +227,73 @@ public class Quaternion implements Cloneable { * @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 + * this * coord * this^(-1). + *

+ * 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 + * this^(-1) * coord * this. + *

+ * 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); } @@ -249,44 +307,50 @@ public class Quaternion implements Cloneable { */ 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); + + } }