gov/nasa/worldwind/geom/Quaternion.java

   1 /*
   2 Copyright (C) 2001, 2006 United States Government
   3 as represented by the Administrator of the
   4 National Aeronautics and Space Administration.
   5 All Rights Reserved.
   6 */
   7 package gov.nasa.worldwind.geom;
   8
   9 /**
  10  * @author Chris Maxwell
  11  * @version $Id: Quaternion.java 1749 2007-05-06 19:48:14Z tgaskins $
  12  */
  13 public class Quaternion
  14 {
  15     private final double x;
  16     private final double y;
  17     private final double z;
  18     private final double w;
  19     private final static double epsilon = 0.0001;
  20
  21     public Quaternion(double x, double y, double z, double w)
  22     {
  23         this.x = x;
  24         this.y = y;
  25         this.z = z;
  26         this.w = w;
  27     }
  28
  29     @Override
  30     public boolean equals(Object o)
  31     {
  32         if (this == o)
  33             return true;
  34         if (o == null || getClass() != o.getClass())
  35             return false;
  36
  37         final gov.nasa.worldwind.geom.Quaternion quaternion = (gov.nasa.worldwind.geom.Quaternion) o;
  38
  39         if (Double.compare(quaternion.w, w) != 0)
  40             return false;
  41         if (Double.compare(quaternion.x, x) != 0)
  42             return false;
  43         if (Double.compare(quaternion.y, y) != 0)
  44             return false;
  45         if (Double.compare(quaternion.z, z) != 0)
  46             return false;
  47
  48         return true;
  49     }
  50
  51     /**
  52      * Generates an integer that is always the same for identical objects, but usually different for different objects.
  53      * This method overrides the one in <code>Object</code>.
  54      * <p/>
  55      * This method makes comparisons on private fields; overriding implementations should include a call to
  56      * <code>super.hashCode()</code>.
  57      *
  58      * @return the hashCode for this <code>Point</code>.
  59      */
  60     @Override
  61     public int hashCode()
  62     {
  63         int result;
  64         long temp;
  65         temp = x != +0.0d ? Double.doubleToLongBits(x) : 0L;
  66         result = (int) (temp ^ (temp >>> 32));
  67         temp = y != +0.0d ? Double.doubleToLongBits(y) : 0L;
  68         result = 29 * result + (int) (temp ^ (temp >>> 32));
  69         temp = z != +0.0d ? Double.doubleToLongBits(z) : 0L;
  70         result = 29 * result + (int) (temp ^ (temp >>> 32));
  71         temp = w != +0.0d ? Double.doubleToLongBits(w) : 0L;
  72         result = 29 * result + (int) (temp ^ (temp >>> 32));
  73         return result;
  74     }
  75
  76     @Override
  77     public final String toString()
  78     {
  79         return "(" + Double.toString(this.x) + ", " + Double.toString(this.y) + ", " + Double.toString(
  80             this.z) + ", " + Double.toString(this.w) + ")";
  81     }
  82
  83     public static Quaternion EulerToQuaternion(double yaw, double pitch, double roll)
  84     {
  85         double cy = Math.cos(yaw * 0.5);
  86         double cp = Math.cos(pitch * 0.5);
  87         double cr = Math.cos(roll * 0.5);
  88         double sy = Math.sin(yaw * 0.5);
  89         double sp = Math.sin(pitch * 0.5);
  90         double sr = Math.sin(roll * 0.5);
  91
  92         double qw = cy * cp * cr + sy * sp * sr;
  93         double qx = sy * cp * cr - cy * sp * sr;
  94         double qy = cy * sp * cr + sy * cp * sr;
  95         double qz = cy * cp * sr - sy * sp * cr;
  96
  97         return new Quaternion(qx, qy, qz, qw);
  98     }
  99
 100     /**
 101      * Transforms a rotation in quaternion form to a set of Euler angles
 102      *
 103      * @param q
 104      * @return The rotation transformed to Euler angles, X=Yaw, Y=Pitch, Z=Roll (radians)
 105      * @throws IllegalArgumentException if <code>q</code> is null
 106      */
 107     public static Point QuaternionToEuler(Quaternion q)
 108     {
 109         if (q == null)
 110         {
 111             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 112             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 113             throw new IllegalArgumentException(msg);
 114         }
 115
 116         double q0 = q.w;
 117         double q1 = q.x;
 118         double q2 = q.y;
 119         double q3 = q.z;
 120
 121         double x = Math.atan2(2 * (q2 * q3 + q0 * q1), (q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3));
 122         double y = Math.asin(-2 * (q1 * q3 - q0 * q2));
 123         double z = Math.atan2(2 * (q1 * q2 + q0 * q3), (q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3));
 124
 125         return new Point(x, y, z);
 126     }
 127
 128     public static Quaternion AxisAngleToQuaternion(Angle angle, double x, double y, double z)
 129     {
 130         if (angle == null)
 131         {
 132             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.AngleIsNull");
 133             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 134             throw new IllegalArgumentException(msg);
 135         }
 136         double length = Math.sqrt(x * x + y * y + z * z);
 137         if (length > 0 && length != 1)
 138         {
 139             x = x / length;
 140             y = y / length;
 141             z = z / length;
 142         }
 143         double sinAngle = angle.sinHalfAngle();
 144         double cosAngle = angle.cosHalfAngle();
 145         return new Quaternion(x * sinAngle, y * sinAngle, z * sinAngle, cosAngle);
 146     }
 147
 148     public static Point QuaternionToAxisAngle(Quaternion q)
 149     {
 150         if (q == null)
 151         {
 152             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 153             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 154             throw new IllegalArgumentException(msg);
 155         }
 156         double x, y, z;
 157         q = q.Normalize();
 158         double s = Math.sqrt(q.x * q.x + q.y * q.y + q.x * q.z);
 159         if (s > 0)
 160         {
 161             x = q.x / s;
 162             y = q.y / s;
 163             z = q.z / s;
 164
 165         }
 166         else
 167         {
 168             x = q.x;
 169             y = q.y;
 170             z = q.z;
 171         }
 172         double angle = 2 * Math.acos(q.w);
 173         return new Point(x, y, z, angle);
 174     }
 175
 176     /**
 177      * @param a
 178      * @param b
 179      * @return
 180      * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
 181      */
 182     public static Quaternion Add(Quaternion a, Quaternion b)
 183     {
 184         if (a == null || b == null)
 185         {
 186             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 187             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 188             throw new IllegalArgumentException(msg);
 189         }
 190         return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
 191     }
 192
 193     /**
 194      * @param a
 195      * @param b
 196      * @return
 197      * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
 198      */
 199     public static Quaternion Subtract(Quaternion a, Quaternion b)
 200     {
 201         if (a == null || b == null)
 202         {
 203             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 204             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 205             throw new IllegalArgumentException(msg);
 206         }
 207         return new Quaternion(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
 208     }
 209
 210     /**
 211      * @param a
 212      * @param b
 213      * @return
 214      * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
 215      */
 216     public static Quaternion Multiply(Quaternion a, Quaternion b)
 217     {
 218         if (a == null || b == null)
 219         {
 220             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 221             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 222             throw new IllegalArgumentException(msg);
 223         }
 224         return new Quaternion(
 225             a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
 226             a.w * b.y + a.y * b.w + a.z * b.x - a.x * b.z,
 227             a.w * b.z + a.z * b.w + a.x * b.y - a.y * b.x,
 228             a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);
 229     }
 230
 231     /**
 232      * @param q
 233      * @return
 234      * @throws IllegalArgumentException if <code>q</code> is null
 235      */
 236     public static Quaternion Multiply(double s, Quaternion q)
 237     {
 238         if (q == null)
 239         {
 240             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 241             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 242             throw new IllegalArgumentException(msg);
 243         }
 244         return new Quaternion(s * q.x, s * q.y, s * q.z, s * q.w);
 245     }
 246
 247     /**
 248      * @param q
 249      * @param s
 250      * @return
 251      * @throws IllegalArgumentException if <code>q</code> is null
 252      */
 253     public static Quaternion Multiply(Quaternion q, double s)
 254     {
 255         if (q == null)
 256         {
 257             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 258             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 259             throw new IllegalArgumentException(msg);
 260         }
 261         return new Quaternion(s * q.x, s * q.y, s * q.z, s * q.w);
 262     }
 263
 264     /**
 265      * equivalent to multiplying by the quaternion (0, v)
 266      *
 267      * @param v
 268      * @param q
 269      * @return
 270      * @throws IllegalArgumentException if <code>q</code> or <code>v</code> is null
 271      */
 272     public static Quaternion Multiply(Point v, Quaternion q)
 273     {
 274         if (v == null)
 275         {
 276             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.VectorIsNull");
 277             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 278             throw new IllegalArgumentException(msg);
 279         }
 280         if (q == null)
 281         {
 282             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 283             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 284             throw new IllegalArgumentException(msg);
 285         }
 286         return new Quaternion(
 287             v.x() * q.w + v.y() * q.z - v.z() * q.y,
 288             v.y() * q.w + v.z() * q.x - v.x() * q.z,
 289             v.z() * q.w + v.x() * q.y - v.y() * q.x,
 290             -v.x() * q.x - v.y() * q.y - v.z() * q.z);
 291     }
 292
 293     /**
 294      * @param q
 295      * @param s
 296      * @return
 297      * @throws IllegalArgumentException if <code>q</code> is null
 298      */
 299     public static Quaternion Divide(Quaternion q, double s)
 300     {
 301         if (q == null)
 302         {
 303             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 304             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 305             throw new IllegalArgumentException(msg);
 306         }
 307         return Quaternion.Multiply(q, (1 / s));
 308     }
 309
 310     // conjugate operator
 311     public Quaternion Conjugate()
 312     {
 313         return new Quaternion(-x, -y, -z, w);
 314     }
 315
 316     /**
 317      * @param q
 318      * @return
 319      * @throws IllegalArgumentException if <code>q</code> is null
 320      */
 321     public static double Norm2(Quaternion q)
 322     {
 323         if (q == null)
 324         {
 325             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 326             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 327             throw new IllegalArgumentException(msg);
 328         }
 329         return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
 330     }
 331
 332     /**
 333      * @param q
 334      * @return
 335      * @throws IllegalArgumentException if <code>q</code> is null
 336      */
 337     public static double Abs(Quaternion q)
 338     {
 339         if (q == null)
 340         {
 341             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 342             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 343             throw new IllegalArgumentException(msg);
 344         }
 345         return Math.sqrt(Norm2(q));
 346     }
 347
 348     /**
 349      * @param a
 350      * @param b
 351      * @return
 352      * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
 353      */
 354     public static Quaternion Divide(Quaternion a, Quaternion b)
 355     {
 356         if (a == null || b == null)
 357         {
 358             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 359             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 360             throw new IllegalArgumentException(msg);
 361         }
 362         return Quaternion.Multiply(a, Quaternion.Divide(b.Conjugate(), Abs(b)));
 363     }
 364
 365     /**
 366      * @param a
 367      * @param b
 368      * @return
 369      * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
 370      */
 371     public static double Dot(Quaternion a, Quaternion b)
 372     {
 373         if (a == null || b == null)
 374         {
 375             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 376             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 377             throw new IllegalArgumentException(msg);
 378         }
 379         return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
 380     }
 381
 382     public Quaternion Normalize()
 383     {
 384         double L = this.Length();
 385
 386         return new Quaternion(
 387             x / L,
 388             y / L,
 389             z / L,
 390             w / L);
 391     }
 392
 393     public double Length()
 394     {
 395         return Math.sqrt(
 396             x * x + y * y + z * z + w * w);
 397     }
 398
 399     /**
 400      * @param q0
 401      * @param q1
 402      * @param t
 403      * @return
 404      * @throws IllegalArgumentException if either supplied <code>Quaternion</code> is null
 405      */
 406     public static Quaternion Slerp(Quaternion q0, Quaternion q1, double t)
 407     {
 408         if (q0 == null || q1 == null)
 409         {
 410             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 411             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 412             throw new IllegalArgumentException(msg);
 413         }
 414         double cosom = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
 415         double tmp0, tmp1, tmp2, tmp3;
 416         if (cosom < 0.0)
 417         {
 418             cosom = -cosom;
 419             tmp0 = -q1.x;
 420             tmp1 = -q1.y;
 421             tmp2 = -q1.z;
 422             tmp3 = -q1.w;
 423         }
 424         else
 425         {
 426             tmp0 = q1.x;
 427             tmp1 = q1.y;
 428             tmp2 = q1.z;
 429             tmp3 = q1.w;
 430         }
 431
 432         /* calc coeffs */
 433         double scale0, scale1;
 434
 435         if ((1.0 - cosom) > epsilon)
 436         {
 437             // standard case (slerp)
 438             double omega = Math.acos(cosom);
 439             double sinom = Math.sin(omega);
 440             scale0 = Math.sin((1.0 - t) * omega) / sinom;
 441             scale1 = Math.sin(t * omega) / sinom;
 442         }
 443         else
 444         {
 445             /* just lerp */
 446             scale0 = 1.0 - t;
 447             scale1 = t;
 448         }
 449
 450         return new Quaternion(
 451             scale0 * q0.x + scale1 * tmp0,
 452             scale0 * q0.y + scale1 * tmp1,
 453             scale0 * q0.z + scale1 * tmp2,
 454             scale0 * q0.w + scale1 * tmp3);
 455     }
 456
 457     public Quaternion Ln()
 458     {
 459         return Ln(this);
 460     }
 461
 462     /**
 463      * @param q
 464      * @return
 465      * @throws IllegalArgumentException if <code>q</code> is null
 466      */
 467     public static Quaternion Ln(Quaternion q)
 468     {
 469         if (q == null)
 470         {
 471             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 472             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 473             throw new IllegalArgumentException(msg);
 474         }
 475         double t;
 476
 477         double s = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
 478         double om = Math.atan2(s, q.w);
 479
 480         if (Math.abs(s) < epsilon)
 481             t = 0.0f;
 482         else
 483             t = om / s;
 484
 485         return new Quaternion(q.x * t, q.y * t, q.z * t, 0.0f);
 486     }
 487
 488     /*****************the below functions have not been certified to work properly ******************/
 489
 490     /**
 491      * @param q
 492      * @return
 493      * @throws IllegalArgumentException if <code>q</code> is null
 494      */
 495     public static Quaternion Exp(Quaternion q)
 496     {
 497         if (q == null)
 498         {
 499             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 500             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 501             throw new IllegalArgumentException(msg);
 502         }
 503         double sinom;
 504         double om = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
 505
 506         if (Math.abs(om) < epsilon)
 507             sinom = 1.0;
 508         else
 509             sinom = Math.sin(om) / om;
 510
 511         return new Quaternion(q.x * sinom, q.y * sinom, q.z * sinom, Math.cos(om));
 512     }
 513
 514     public Quaternion Exp()
 515     {
 516         return Ln(this);
 517     }
 518
 519     /**
 520      * @param q1
 521      * @param a
 522      * @param b
 523      * @param c
 524      * @param t
 525      * @return
 526      * @throws IllegalArgumentException if any argument is null
 527      */
 528     public static Quaternion Squad(
 529         Quaternion q1,
 530         Quaternion a,
 531         Quaternion b,
 532         Quaternion c,
 533         double t)
 534     {
 535         if (q1 == null || a == null || b == null || c == null)
 536         {
 537             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 538             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 539             throw new IllegalArgumentException(msg);
 540         }
 541         return Slerp(
 542             Slerp(q1, c, t), Slerp(a, b, t), 2 * t * (1.0 - t));
 543     }
 544
 545     //TODO: this needs to be accounted for before Squad() is used
 546     /*public static Quaternion[] SquadSetup(
 547         Quaternion4d q0,
 548         Quaternion4d q1,
 549         Quaternion4d q2,
 550         Quaternion4d q3)
 551     {
 552         if(q0 == null || q1 == null || q2 == null || q3 == null)
 553         {
 554             String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 555             gov.nasa.worldwind.WorldWind.logger().log(java.util.logging.Level.FINE, msg);
 556             throw new IllegalArgumentException(msg);
 557         }
 558
 559         q0 = q0 + q1;
 560         q0.Normalize();
 561
 562         q2 = q2 + q1;
 563         q2.Normalize();
 564
 565         q3 = q3 + q1;
 566         q3.Normalize();
 567
 568         q1.Normalize();
 569
 570         Quaternion[] ret = new Quaternion[3];
 571
 572         ret[0] = q1 * Exp(-0.25 * (Ln(Exp(q1) * q2) + Ln(Exp(q1) * q0))); // outA
 573         ret[1] = q2 * Exp(-0.25 * (Ln(Exp(q2) * q3) + Ln(Exp(q2) * q1))); // outB
 574         ret[2] = q2;                                                      // outC
 575
 576     }*/
 577 }