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

   1 package net.sf.openrocket.util;
   2
   3
   4 public class Quaternion implements Cloneable {
   5
   6         protected double w, x, y, z;
   7         protected int modCount = 0;
   8
   9         public Quaternion() {
  10                 this(1,0,0,0);
  11         }
  12
  13         public Quaternion(double w, double x, double y, double z) {
  14                 this.w = w;
  15                 this.x = x;
  16                 this.y = y;
  17                 this.z = z;
  18         }
  19
  20
  21         public static Quaternion rotation(Coordinate rotation) {
  22                 double length = rotation.length();
  23                 if (length < 0.000001) {
  24                         return new Quaternion(1,0,0,0);
  25                 }
  26                 double sin = Math.sin(length/2);
  27                 double cos = Math.cos(length/2);
  28                 return new Quaternion(cos,
  29                                 sin*rotation.x/length, sin*rotation.y/length, sin*rotation.z/length);
  30         }
  31
  32         public static Quaternion rotation(Coordinate axis, double angle) {
  33                 Coordinate a = axis.normalize();
  34                 double sin = Math.sin(angle);
  35                 double cos = Math.cos(angle);
  36                 return new Quaternion(cos, sin*a.x, sin*a.y, sin*a.z);
  37         }
  38
  39
  40         public double getW() {
  41                 return w;
  42         }
  43
  44         public void setW(double w) {
  45                 this.w = w;
  46                 modCount++;
  47         }
  48
  49         public double getX() {
  50                 return x;
  51         }
  52
  53         public void setX(double x) {
  54                 this.x = x;
  55                 modCount++;
  56         }
  57
  58         public double getY() {
  59                 return y;
  60         }
  61
  62         public void setY(double y) {
  63                 this.y = y;
  64                 modCount++;
  65         }
  66
  67         public double getZ() {
  68                 return z;
  69         }
  70
  71         public void setZ(double z) {
  72                 this.z = z;
  73                 modCount++;
  74         }
  75
  76
  77         public void setAll(double w, double x, double y, double z) {
  78                 this.w = w;
  79                 this.x = x;
  80                 this.y = y;
  81                 this.z = z;
  82                 modCount++;
  83         }
  84
  85
  86         /**
  87          * Multiply this quaternion by the other quaternion from the right side.  This
  88          * calculates the product  <code>this = this * other</code>.
  89          *
  90          * @param other   the quaternion to multiply this quaternion by.
  91          * @return                this quaternion.
  92          */
  93         public Quaternion multiplyRight(Quaternion other) {
  94                 double w = (this.w*other.w - this.x*other.x - this.y*other.y - this.z*other.z);
  95                 double x = (this.w*other.x + this.x*other.w + this.y*other.z - this.z*other.y);
  96                 double y = (this.w*other.y + this.y*other.w + this.z*other.x - this.x*other.z);
  97                 double z = (this.w*other.z + this.z*other.w + this.x*other.y - this.y*other.x);
  98
  99                 this.w = w;
 100                 this.x = x;
 101                 this.y = y;
 102                 this.z = z;
 103                 return this;
 104         }
 105
 106         /**
 107          * Multiply this quaternion by the other quaternion from the left side.  This
 108          * calculates the product  <code>this = other * this</code>.
 109          *
 110          * @param other   the quaternion to multiply this quaternion by.
 111          * @return                this quaternion.
 112          */
 113         public Quaternion multiplyLeft(Quaternion other) {
 114                 /*  other(abcd) * this(wxyz)  */
 115
 116                 double w = (other.w*this.w - other.x*this.x - other.y*this.y - other.z*this.z);
 117                 double x = (other.w*this.x + other.x*this.w + other.y*this.z - other.z*this.y);
 118                 double y = (other.w*this.y + other.y*this.w + other.z*this.x - other.x*this.z);
 119                 double z = (other.w*this.z + other.z*this.w + other.x*this.y - other.y*this.x);
 120
 121                 this.w = w;
 122                 this.x = x;
 123                 this.y = y;
 124                 this.z = z;
 125                 return this;
 126         }
 127
 128
 129
 130
 131
 132
 133         @Override
 134         public Quaternion clone() {
 135                 try {
 136                         return (Quaternion) super.clone();
 137                 } catch (CloneNotSupportedException e) {
 138                         throw new BugException("CloneNotSupportedException encountered");
 139                 }
 140         }
 141
 142
 143
 144         /**
 145          * Normalize this quaternion.  After the call the norm of the quaternion is exactly
 146          * one.  If this quaternion is the zero quaternion, throws
 147          * <code>IllegalStateException</code>.  Returns this quaternion.
 148          *
 149          * @return   this quaternion.
 150          * @throws   IllegalStateException  if the norm of this quaternion is zero.
 151          */
 152         public Quaternion normalize() {
 153                 double norm = norm();
 154                 if (norm < 0.0000001) {
 155                         throw new IllegalStateException("attempting to normalize zero-quaternion");
 156                 }
 157                 x /= norm;
 158                 y /= norm;
 159                 z /= norm;
 160                 w /= norm;
 161                 return this;
 162         }
 163
 164
 165         /**
 166          * Normalize this quaternion if the norm is more than 1ppm from one.
 167          *
 168          * @return      this quaternion.
 169          * @throws   IllegalStateException  if the norm of this quaternion is zero.
 170          */
 171         public Quaternion normalizeIfNecessary() {
 172                 double n2 = norm2();
 173                 if (n2 < 0.999999 || n2 > 1.000001)
 174                         normalize();
 175                 return this;
 176         }
 177
 178
 179
 180         /**
 181          * Return the norm of this quaternion.
 182          *
 183          * @return   the norm of this quaternion sqrt(w^2 + x^2 + y^2 + z^2).
 184          */
 185         public double norm() {
 186                 return Math.sqrt(x*x + y*y + z*z + w*w);
 187         }
 188
 189         /**
 190          * Return the square of the norm of this quaternion.
 191          *
 192          * @return      the square of the norm of this quaternion (w^2 + x^2 + y^2 + z^2).
 193          */
 194         public double norm2() {
 195                 return x*x + y*y + z*z + w*w;
 196         }
 197
 198
 199         public Coordinate rotate(Coordinate coord) {
 200                 double a,b,c,d;
 201
 202                 assert(Math.abs(norm2()-1) < 0.00001) : "Quaternion not unit length: "+this;
 203
 204
 205                 //  (a,b,c,d) = this * coord = (w,x,y,z) * (0,cx,cy,cz)
 206                 a = - x * coord.x - y * coord.y - z * coord.z;  // w
 207                 b =   w * coord.x + y * coord.z - z * coord.y;  // x i
 208                 c =   w * coord.y - x * coord.z + z * coord.x;  // y j
 209                 d =   w * coord.z + x * coord.y - y * coord.x;  // z k
 210
 211
 212                 //  return = (a,b,c,d) * (this)^-1 = (a,b,c,d) * (w,-x,-y,-z)
 213
 214                 // Assert that the w-value is zero
 215                 assert(Math.abs(a*w + b*x + c*y + d*z) < coord.max() * MathUtil.EPSILON):
 216                         ("Should be zero: " + (a*w + b*x + c*y + d*z) + " in " + this + " c=" + coord);
 217
 218                 return new Coordinate(
 219                                 - a*x + b*w - c*z + d*y,
 220                                 - a*y + b*z + c*w - d*x,
 221                                 - a*z - b*y + c*x + d*w,
 222                                 coord.weight
 223                 );
 224         }
 225
 226         public Coordinate invRotate(Coordinate coord) {
 227                 double a,b,c,d;
 228
 229                 assert(Math.abs(norm2()-1) < 0.00001) : "Quaternion not unit length: "+this;
 230
 231                 //  (a,b,c,d) = (this)^-1 * coord = (w,-x,-y,-z) * (0,cx,cy,cz)
 232                 a = + x * coord.x + y * coord.y + z * coord.z;
 233                 b =   w * coord.x - y * coord.z + z * coord.y;
 234                 c =   w * coord.y + x * coord.z - z * coord.x;
 235                 d =   w * coord.z - x * coord.y + y * coord.x;
 236
 237
 238                 //  return = (a,b,c,d) * this = (a,b,c,d) * (w,x,y,z)
 239                 assert(Math.abs(a*w - b*x - c*y - d*z) < Math.max(coord.max(), 1) * MathUtil.EPSILON):
 240                         ("Should be zero: " + (a*w - b*x - c*y - d*z) + " in " + this + " c=" + coord);
 241
 242                 return new Coordinate(
 243                                 a*x + b*w + c*z - d*y,
 244                                 a*y - b*z + c*w + d*x,
 245                                 a*z + b*y - c*x + d*w,
 246                                 coord.weight
 247                 );
 248         }
 249
 250
 251         /**
 252          * Rotate the coordinate (0,0,1) using this quaternion.  The result is returned
 253          * as a Coordinate.  This method is equivalent to calling
 254          * <code>q.rotate(new Coordinate(0,0,1))</code> but requires only about half of the
 255          * multiplications.
 256          *
 257          * @return      The coordinate (0,0,1) rotated using this quaternion.
 258          */
 259         public Coordinate rotateZ() {
 260                 return new Coordinate(
 261                                 2*(w*y + x*z),
 262                                 2*(y*z - w*x),
 263                                 w*w - x*x - y*y + z*z
 264                 );
 265         }
 266
 267
 268         @Override
 269         public String toString() {
 270                 return String.format("Quaternion[%f,%f,%f,%f,norm=%f]",w,x,y,z,this.norm());
 271         }
 272
 273         public static void main(String[] arg) {
 274
 275                 Quaternion q = new Quaternion(Math.random()-0.5,Math.random()-0.5,
 276                                 Math.random()-0.5,Math.random()-0.5);
 277                 q.normalize();
 278
 279                 q = new Quaternion(-0.998717,0.000000,0.050649,-0.000000);
 280
 281                 Coordinate coord = new Coordinate(10*(Math.random()-0.5),
 282                                 10*(Math.random()-0.5), 10*(Math.random()-0.5));
 283
 284                 System.out.println("Quaternion: "+q);
 285                 System.out.println("Coordinate: "+coord);
 286                 coord = q.invRotate(coord);
 287                 System.out.println("Rotated: "+ coord);
 288                 coord = q.rotate(coord);
 289                 System.out.println("Back:       "+coord);
 290
 291
 292                 q = new Quaternion(0.237188,0.570190,-0.514542,0.594872);
 293                 q.normalize();
 294                 Coordinate c = new Coordinate(148578428.914,8126778.954,-607.741);
 295
 296                 System.out.println("Rotated: " + q.rotate(c));
 297
 298 //              Coordinate c = new Coordinate(0,1,0);
 299 //              Coordinate rot = new Coordinate(Math.PI/4,0,0);
 300 //
 301 //              System.out.println("Before: "+c);
 302 //              c = rotation(rot).invRotate(c);
 303 //              System.out.println("After: "+c);
 304
 305
 306         }
 307
 308 }