1 package net.sf.openrocket.util;
3 import net.sf.openrocket.logging.LogHelper;
4 import net.sf.openrocket.startup.Application;
7 * An immutable quaternion class.
9 * @author Sampo Niskanen <sampo.niskanen@iki.fi>
11 public class Quaternion {
12 private static final LogHelper log = Application.getLogger();
15 //////// Debug section
17 * Debugging info. If openrocket.debug.quaternioncount is defined, a line is
18 * printed every 1000000 instantiations (or as many as defined).
20 private static final boolean COUNT_DEBUG;
21 private static final int COUNT_DIFF;
23 String str = System.getProperty("openrocket.debug.quaternioncount");
31 diff = Integer.parseInt(str);
32 } catch (NumberFormatException ignore) {
40 private static int count = 0;
44 synchronized (Quaternion.class) {
46 if ((count % COUNT_DIFF) == 0) {
47 log.debug("Quaternion instantiated " + count + " times.");
53 //////// End debug section
57 private final double w, x, y, z;
58 private double norm = -1;
62 * Construct a new "one" quaternion. This is equivalent to <code>new Quaternion(1,0,0,0)</code>.
69 public Quaternion(double w, double x, double y, double z) {
78 * Create a rotation quaternion corresponding to the rotation vector. The rotation axis is
79 * the direction of vector, and rotation angle is the length of the vector.
81 * The cost of the operation is approximately that of computing the length of the coordinate
82 * and computing two trigonometric functions.
84 * @param rotation the rotation vector
85 * @return the quaternion corresponding to the rotation vector
87 public static Quaternion rotation(Coordinate rotation) {
88 double length = rotation.length();
89 if (length < 0.000001) {
90 return new Quaternion(1, 0, 0, 0);
92 double sin = Math.sin(length / 2);
93 double cos = Math.cos(length / 2);
94 return new Quaternion(cos,
95 sin * rotation.x / length, sin * rotation.y / length, sin * rotation.z / length);
99 * Create a rotation quaternion corresponding to the rotation around the provided vector with
100 * the provided angle.
102 * @param axis the rotation axis
103 * @param angle the rotation angle
104 * @return the corresponding quaternion
106 public static Quaternion rotation(Coordinate axis, double angle) {
107 Coordinate a = axis.normalize();
108 double sin = Math.sin(angle);
109 double cos = Math.cos(angle);
110 return new Quaternion(cos, sin * a.x, sin * a.y, sin * a.z);
114 public double getW() {
118 public double getX() {
122 public double getY() {
126 public double getZ() {
132 * Check whether any of the quaternion values is NaN.
134 * @return true if w, x, y or z is NaN
136 public boolean isNaN() {
137 return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z) || Double.isNaN(w);
142 * Multiply this quaternion by the other quaternion from the right side. This
143 * calculates the product <code>result = this * other</code>.
145 * @param other the quaternion to multiply this quaternion by.
146 * @return this quaternion.
148 public Quaternion multiplyRight(Quaternion other) {
149 double newW = (this.w * other.w - this.x * other.x - this.y * other.y - this.z * other.z);
150 double newX = (this.w * other.x + this.x * other.w + this.y * other.z - this.z * other.y);
151 double newY = (this.w * other.y + this.y * other.w + this.z * other.x - this.x * other.z);
152 double newZ = (this.w * other.z + this.z * other.w + this.x * other.y - this.y * other.x);
154 return new Quaternion(newW, newX, newY, newZ);
158 * Multiply this quaternion by the other quaternion from the left side. This
159 * calculates the product <code>result = other * this</code>.
161 * @param other the quaternion to multiply this quaternion by.
162 * @return this quaternion.
164 public Quaternion multiplyLeft(Quaternion other) {
165 /* other(abcd) * this(wxyz) */
167 double newW = (other.w * this.w - other.x * this.x - other.y * this.y - other.z * this.z);
168 double newX = (other.w * this.x + other.x * this.w + other.y * this.z - other.z * this.y);
169 double newY = (other.w * this.y + other.y * this.w + other.z * this.x - other.x * this.z);
170 double newZ = (other.w * this.z + other.z * this.w + other.x * this.y - other.y * this.x);
172 return new Quaternion(newW, newX, newY, newZ);
180 * Return a normalized version of this quaternion. If this quaternion is the zero quaternion, throws
181 * <code>IllegalStateException</code>.
183 * @return a normalized version of this quaternion.
184 * @throws IllegalStateException if the norm of this quaternion is zero.
186 public Quaternion normalize() {
189 throw new IllegalStateException("attempting to normalize zero-quaternion");
191 return new Quaternion(w / n, x / n, y / n, z / n);
196 * Normalize the quaternion if the norm is more than 1ppm from one.
198 * @return this quaternion or a normalized version of this quaternion.
199 * @throws IllegalStateException if the norm of this quaternion is zero.
201 public Quaternion normalizeIfNecessary() {
203 if (n2 < 0.999999 || n2 > 1.000001) {
213 * Return the norm of this quaternion.
215 * @return the norm of this quaternion sqrt(w^2 + x^2 + y^2 + z^2).
217 public double norm() {
219 norm = MathUtil.safeSqrt(x * x + y * y + z * z + w * w);
225 * Return the square of the norm of this quaternion.
227 * @return the square of the norm of this quaternion (w^2 + x^2 + y^2 + z^2).
229 public double norm2() {
230 return x * x + y * y + z * z + w * w;
235 * Perform a coordinate rotation using this unit quaternion. The result is
236 * <code>this * coord * this^(-1)</code>.
238 * This method assumes that the norm of this quaternion is one.
240 * @param coord the coordinate to rotate.
241 * @return the rotated coordinate.
243 public Coordinate rotate(Coordinate coord) {
246 assert (Math.abs(norm2() - 1) < 0.00001) : "Quaternion not unit length: " + this;
249 // (a,b,c,d) = this * coord = (w,x,y,z) * (0,cx,cy,cz)
250 a = -x * coord.x - y * coord.y - z * coord.z; // w
251 b = w * coord.x + y * coord.z - z * coord.y; // x i
252 c = w * coord.y - x * coord.z + z * coord.x; // y j
253 d = w * coord.z + x * coord.y - y * coord.x; // z k
256 // return = (a,b,c,d) * (this)^-1 = (a,b,c,d) * (w,-x,-y,-z)
258 // Assert that the w-value is zero
259 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);
261 return new Coordinate(
262 -a * x + b * w - c * z + d * y,
263 -a * y + b * z + c * w - d * x,
264 -a * z - b * y + c * x + d * w,
269 * Perform an inverse coordinate rotation using this unit quaternion. The result is
270 * <code>this^(-1) * coord * this</code>.
272 * This method assumes that the norm of this quaternion is one.
274 * @param coord the coordinate to rotate.
275 * @return the rotated coordinate.
277 public Coordinate invRotate(Coordinate coord) {
280 assert (Math.abs(norm2() - 1) < 0.00001) : "Quaternion not unit length: " + this;
282 // (a,b,c,d) = (this)^-1 * coord = (w,-x,-y,-z) * (0,cx,cy,cz)
283 a = +x * coord.x + y * coord.y + z * coord.z;
284 b = w * coord.x - y * coord.z + z * coord.y;
285 c = w * coord.y + x * coord.z - z * coord.x;
286 d = w * coord.z - x * coord.y + y * coord.x;
289 // return = (a,b,c,d) * this = (a,b,c,d) * (w,x,y,z)
290 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);
292 return new Coordinate(
293 a * x + b * w + c * z - d * y,
294 a * y - b * z + c * w + d * x,
295 a * z + b * y - c * x + d * w,
301 * Rotate the coordinate (0,0,1) using this quaternion. The result is returned
302 * as a Coordinate. This method is equivalent to calling
303 * <code>q.rotate(new Coordinate(0,0,1))</code> but requires only about half of the
306 * @return The coordinate (0,0,1) rotated using this quaternion.
308 public Coordinate rotateZ() {
309 return new Coordinate(
312 w * w - x * x - y * y + z * z);
317 public String toString() {
318 return String.format("Quaternion[%f,%f,%f,%f,norm=%f]", w, x, y, z, this.norm());