git.gag.com Git - debian/openrocket/blob - core/src/net/sf/openrocket/util/Quaternion.java

   1 package net.sf.openrocket.util;
   2
   3 import net.sf.openrocket.logging.LogHelper;
   4 import net.sf.openrocket.startup.Application;
   5
   6 /**
   7  * An immutable quaternion class.
   8  *
   9  * @author Sampo Niskanen <sampo.niskanen@iki.fi>
  10  */
  11 public class Quaternion {
  12         private static final LogHelper log = Application.getLogger();
  13
  14
  15         ////////  Debug section
  16         /*
  17          * Debugging info.  If openrocket.debug.quaternioncount is defined, a line is
  18          * printed every 1000000 instantiations (or as many as defined).
  19          */
  20         private static final boolean COUNT_DEBUG;
  21         private static final int COUNT_DIFF;
  22         static {
  23                 String str = System.getProperty("openrocket.debug.quaternioncount");
  24                 int diff = 0;
  25                 if (str == null) {
  26                         COUNT_DEBUG = false;
  27                         COUNT_DIFF = 0;
  28                 } else {
  29                         COUNT_DEBUG = true;
  30                         try {
  31                                 diff = Integer.parseInt(str);
  32                         } catch (NumberFormatException ignore) {
  33                         }
  34                         if (diff < 1000)
  35                                 diff = 1000000;
  36                         COUNT_DIFF = diff;
  37                 }
  38         }
  39
  40         private static int count = 0;
  41         {
  42                 // Debug count
  43                 if (COUNT_DEBUG) {
  44                         synchronized (Quaternion.class) {
  45                                 count++;
  46                                 if ((count % COUNT_DIFF) == 0) {
  47                                         log.debug("Quaternion instantiated " + count + " times.");
  48                                 }
  49                         }
  50                 }
  51         }
  52
  53         ////////  End debug section
  54
  55
  56
  57         private final double w, x, y, z;
  58         private double norm = -1;
  59
  60
  61         /**
  62          * Construct a new "one" quaternion.  This is equivalent to <code>new Quaternion(1,0,0,0)</code>.
  63          */
  64         public Quaternion() {
  65                 this(1, 0, 0, 0);
  66         }
  67
  68
  69         public Quaternion(double w, double x, double y, double z) {
  70                 this.w = w;
  71                 this.x = x;
  72                 this.y = y;
  73                 this.z = z;
  74         }
  75
  76
  77         /**
  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.
  80          * <p>
  81          * The cost of the operation is approximately that of computing the length of the coordinate
  82          * and computing two trigonometric functions.
  83          *
  84          * @param rotation      the rotation vector
  85          * @return                      the quaternion corresponding to the rotation vector
  86          */
  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);
  91                 }
  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);
  96         }
  97
  98         /**
  99          * Create a rotation quaternion corresponding to the rotation around the provided vector with
 100          * the provided angle.
 101          *
 102          * @param axis          the rotation axis
 103          * @param angle         the rotation angle
 104          * @return                      the corresponding quaternion
 105          */
 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);
 111         }
 112
 113
 114         public double getW() {
 115                 return w;
 116         }
 117
 118         public double getX() {
 119                 return x;
 120         }
 121
 122         public double getY() {
 123                 return y;
 124         }
 125
 126         public double getZ() {
 127                 return z;
 128         }
 129
 130
 131         /**
 132          * Check whether any of the quaternion values is NaN.
 133          *
 134          * @return      true if w, x, y or z is NaN
 135          */
 136         public boolean isNaN() {
 137                 return Double.isNaN(x) || Double.isNaN(y) || Double.isNaN(z) || Double.isNaN(w);
 138         }
 139
 140
 141         /**
 142          * Multiply this quaternion by the other quaternion from the right side.  This
 143          * calculates the product  <code>result = this * other</code>.
 144          *
 145          * @param other   the quaternion to multiply this quaternion by.
 146          * @return                this quaternion.
 147          */
 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);
 153
 154                 return new Quaternion(newW, newX, newY, newZ);
 155         }
 156
 157         /**
 158          * Multiply this quaternion by the other quaternion from the left side.  This
 159          * calculates the product  <code>result = other * this</code>.
 160          *
 161          * @param other   the quaternion to multiply this quaternion by.
 162          * @return                this quaternion.
 163          */
 164         public Quaternion multiplyLeft(Quaternion other) {
 165                 /*  other(abcd) * this(wxyz)  */
 166
 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);
 171
 172                 return new Quaternion(newW, newX, newY, newZ);
 173         }
 174
 175
 176
 177
 178
 179         /**
 180          * Return a normalized version of this quaternion.  If this quaternion is the zero quaternion, throws
 181          * <code>IllegalStateException</code>.
 182          *
 183          * @return   a normalized version of this quaternion.
 184          * @throws   IllegalStateException  if the norm of this quaternion is zero.
 185          */
 186         public Quaternion normalize() {
 187                 double n = norm();
 188                 if (n < 0.0000001) {
 189                         throw new IllegalStateException("attempting to normalize zero-quaternion");
 190                 }
 191                 return new Quaternion(w / n, x / n, y / n, z / n);
 192         }
 193
 194
 195         /**
 196          * Normalize the quaternion if the norm is more than 1ppm from one.
 197          *
 198          * @return      this quaternion or a normalized version of this quaternion.
 199          * @throws   IllegalStateException  if the norm of this quaternion is zero.
 200          */
 201         public Quaternion normalizeIfNecessary() {
 202                 double n2 = norm2();
 203                 if (n2 < 0.999999 || n2 > 1.000001) {
 204                         return normalize();
 205                 } else {
 206                         return this;
 207                 }
 208         }
 209
 210
 211
 212         /**
 213          * Return the norm of this quaternion.
 214          *
 215          * @return   the norm of this quaternion sqrt(w^2 + x^2 + y^2 + z^2).
 216          */
 217         public double norm() {
 218                 if (norm < 0) {
 219                         norm = MathUtil.safeSqrt(x * x + y * y + z * z + w * w);
 220                 }
 221                 return norm;
 222         }
 223
 224         /**
 225          * Return the square of the norm of this quaternion.
 226          *
 227          * @return      the square of the norm of this quaternion (w^2 + x^2 + y^2 + z^2).
 228          */
 229         public double norm2() {
 230                 return x * x + y * y + z * z + w * w;
 231         }
 232
 233
 234         /**
 235          * Perform a coordinate rotation using this unit quaternion.  The result is
 236          * <code>this * coord * this^(-1)</code>.
 237          * <p>
 238          * This method assumes that the norm of this quaternion is one.
 239          *
 240          * @param coord         the coordinate to rotate.
 241          * @return                      the rotated coordinate.
 242          */
 243         public Coordinate rotate(Coordinate coord) {
 244                 double a, b, c, d;
 245
 246                 assert (Math.abs(norm2() - 1) < 0.00001) : "Quaternion not unit length: " + this;
 247
 248
 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
 254
 255
 256                 //  return = (a,b,c,d) * (this)^-1 = (a,b,c,d) * (w,-x,-y,-z)
 257
 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);
 260
 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,
 265                                 coord.weight);
 266         }
 267
 268         /**
 269          * Perform an inverse coordinate rotation using this unit quaternion.  The result is
 270          * <code>this^(-1) * coord * this</code>.
 271          * <p>
 272          * This method assumes that the norm of this quaternion is one.
 273          *
 274          * @param coord         the coordinate to rotate.
 275          * @return                      the rotated coordinate.
 276          */
 277         public Coordinate invRotate(Coordinate coord) {
 278                 double a, b, c, d;
 279
 280                 assert (Math.abs(norm2() - 1) < 0.00001) : "Quaternion not unit length: " + this;
 281
 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;
 287
 288
 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);
 291
 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,
 296                                 coord.weight);
 297         }
 298
 299
 300         /**
 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
 304          * multiplications.
 305          *
 306          * @return      The coordinate (0,0,1) rotated using this quaternion.
 307          */
 308         public Coordinate rotateZ() {
 309                 return new Coordinate(
 310                                 2 * (w * y + x * z),
 311                                 2 * (y * z - w * x),
 312                                 w * w - x * x - y * y + z * z);
 313         }
 314
 315
 316         @Override
 317         public String toString() {
 318                 return String.format("Quaternion[%f,%f,%f,%f,norm=%f]", w, x, y, z, this.norm());
 319         }
 320
 321 }