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 import gov.nasa.worldwind.util.Logging;
  10
  11 /**
  12  * @author Chris Maxwell
  13  * @version $Id: Quaternion.java 2471 2007-07-31 21:50:57Z tgaskins $
  14  */
  15 public class Quaternion
  16 {
  17     // Multiplicative identity quaternion.
  18     public static final Quaternion IDENTITY = new Quaternion(0, 0, 0, 1);
  19
  20     public final double x;
  21     public final double y;
  22     public final double z;
  23     public final double w;
  24
  25     // 4 values in a quaternion.
  26     private static final int NUM_ELEMENTS = 4;
  27     // Cached computations.
  28     private int hashCode = -1;
  29     private double[] quatArray = null;
  30     private double length = -1;
  31     private double lengthSquared = -1;
  32     private Quaternion normalized = null;
  33     private Quaternion inverse = null;
  34
  35     public Quaternion(double x, double y, double z, double w)
  36     {
  37         this.x = x;
  38         this.y = y;
  39         this.z = z;
  40         this.w = w;
  41     }
  42
  43     public final boolean equals(Object obj)
  44     {
  45         if (this == obj)
  46             return true;
  47         if (obj == null || obj.getClass() != this.getClass())
  48             return false;
  49
  50         Quaternion that = (Quaternion) obj;
  51         return (this.x == that.x)
  52             && (this.y == that.y)
  53             && (this.z == that.z)
  54             && (this.w == that.w);
  55     }
  56
  57     public final int hashCode()
  58     {
  59         if (this.hashCode < 0)
  60         {
  61             int result;
  62             long tmp;
  63             tmp = Double.doubleToLongBits(this.x);
  64             result = (int) (tmp ^ (tmp >>> 32));
  65             tmp = Double.doubleToLongBits(this.y);
  66             result = 31 * result + (int) (tmp ^ (tmp >>> 32));
  67             tmp = Double.doubleToLongBits(this.z);
  68             result = 31 * result + (int) (tmp ^ (tmp >>> 32));
  69             tmp = Double.doubleToLongBits(this.w);
  70             result = 31 * result + (int) (tmp ^ (tmp >>> 32));
  71             this.hashCode = result;
  72         }
  73         return this.hashCode;
  74     }
  75
  76     public static Quaternion fromArray(double[] compArray, int offset)
  77     {
  78         if (compArray == null)
  79         {
  80             String msg = Logging.getMessage("nullValue.ArrayIsNull");
  81             Logging.logger().severe(msg);
  82             throw new IllegalArgumentException(msg);
  83         }
  84         if ((compArray.length + offset) < NUM_ELEMENTS)
  85         {
  86             String msg = Logging.getMessage("generic.ArrayInvalidLength", compArray.length + offset);
  87             Logging.logger().severe(msg);
  88             throw new IllegalArgumentException(msg);
  89         }
  90
  91         return new Quaternion(
  92             compArray[0 + offset],
  93             compArray[1 + offset],
  94             compArray[2 + offset],
  95             compArray[3 + offset]);
  96     }
  97
  98     public final double[] toArray(double[] compArray, int offset)
  99     {
 100         this.ensureQuatArray();
 101         if (compArray == null)
 102             compArray = new double[NUM_ELEMENTS + offset];
 103         System.arraycopy(this.quatArray, 0, compArray, offset, NUM_ELEMENTS);
 104         return compArray;
 105     }
 106
 107     private void ensureQuatArray()
 108     {
 109         if (this.quatArray != null)
 110             return;
 111
 112         this.quatArray = new double[NUM_ELEMENTS];
 113         this.quatArray[0] = this.x;
 114         this.quatArray[1] = this.y;
 115         this.quatArray[2] = this.z;
 116         this.quatArray[3] = this.w;
 117     }
 118
 119     public final String toString()
 120     {
 121         StringBuilder sb = new StringBuilder();
 122         sb.append("(");
 123         sb.append(this.x).append(", ");
 124         sb.append(this.y).append(", ");
 125         sb.append(this.z).append(", ");
 126         sb.append(this.w);
 127         sb.append(")");
 128         return sb.toString();
 129     }
 130
 131     public final double getX()
 132     {
 133         return this.x;
 134     }
 135
 136     public final double getY()
 137     {
 138         return this.y;
 139     }
 140
 141     public final double getZ()
 142     {
 143         return this.z;
 144     }
 145
 146     public final double getW()
 147     {
 148         return this.w;
 149     }
 150
 151     public final double x()
 152     {
 153         return this.x;
 154     }
 155
 156     public final double y()
 157     {
 158         return this.y;
 159     }
 160
 161     public final double z()
 162     {
 163         return this.z;
 164     }
 165
 166     public final double w()
 167     {
 168         return this.w;
 169     }
 170
 171     // ============== Factory Functions ======================= //
 172     // ============== Factory Functions ======================= //
 173     // ============== Factory Functions ======================= //
 174
 175     public static Quaternion fromAxisAngle(Angle angle, Vec4 axis)
 176     {
 177         if (angle == null)
 178         {
 179             String msg = Logging.getMessage("nullValue.AngleIsNull");
 180             Logging.logger().severe(msg);
 181             throw new IllegalArgumentException(msg);
 182         }
 183         if (axis == null)
 184         {
 185             String msg = Logging.getMessage("nullValue.Vec4IsNull");
 186             Logging.logger().severe(msg);
 187             throw new IllegalArgumentException(msg);
 188         }
 189
 190         return fromAxisAngle(angle, axis.x, axis.y, axis.z, true);
 191     }
 192
 193     public static Quaternion fromAxisAngle(Angle angle, double axisX, double axisY, double axisZ)
 194     {
 195         if (angle == null)
 196         {
 197             String msg = Logging.getMessage("nullValue.AngleIsNull");
 198             Logging.logger().severe(msg);
 199             throw new IllegalArgumentException(msg);
 200         }
 201         return fromAxisAngle(angle, axisX, axisY, axisZ, true);
 202     }
 203
 204     private static Quaternion fromAxisAngle(Angle angle, double axisX, double axisY, double axisZ, boolean normalize)
 205     {
 206         if (angle == null)
 207         {
 208             String msg = Logging.getMessage("nullValue.AngleIsNull");
 209             Logging.logger().severe(msg);
 210             throw new IllegalArgumentException(msg);
 211         }
 212
 213         if (normalize)
 214         {
 215             double length = Math.sqrt((axisX * axisX) + (axisY * axisY) + (axisZ * axisZ));
 216             if (!isZero(length) && (length != 1.0))
 217             {
 218                 axisX /= length;
 219                 axisY /= length;
 220                 axisZ /= length;
 221             }
 222         }
 223
 224         double s = angle.sinHalfAngle();
 225         double c = angle.cosHalfAngle();
 226         return new Quaternion(axisX * s, axisY * s, axisZ * s, c);
 227     }
 228
 229     public static Quaternion fromMatrix(Matrix matrix)
 230     {
 231         if (matrix == null)
 232         {
 233             String msg = Logging.getMessage("nullValue.MatrixIsNull");
 234             Logging.logger().severe(msg);
 235             throw new IllegalArgumentException(msg);
 236         }
 237
 238         double t = 1.0 + matrix.m11 + matrix.m22 + matrix.m33;
 239         double x, y, z, w;
 240         double s;
 241         final double EPSILON = 0.00000001;
 242         if (t > EPSILON) {
 243                         s = 2.0 * Math.sqrt(t);
 244                         x = (matrix.m32 - matrix.m23) / s;
 245             y = (matrix.m13 - matrix.m31) / s;
 246             z = (matrix.m21 - matrix.m12) / s;
 247             w = s / 4.0;
 248         } else if ((matrix.m11 > matrix.m22) && (matrix.m11 > matrix.m33)) {
 249                         s = 2.0 * Math.sqrt(1.0 + matrix.m11 - matrix.m22 - matrix.m33);
 250                         x = s / 4.0;
 251                         y = (matrix.m21 + matrix.m12) / s;
 252                         z = (matrix.m13 + matrix.m31) / s;
 253                         w = (matrix.m32 - matrix.m23) / s;
 254                 } else if (matrix.m22 > matrix.m33) {
 255                         s = 2.0 * Math.sqrt(1.0 + matrix.m22 - matrix.m11 - matrix.m33);
 256                         x = (matrix.m21 + matrix.m12) / s;
 257                         y = s / 4.0;
 258                         z = (matrix.m32 + matrix.m23) / s;
 259                         w = (matrix.m13 - matrix.m31) / s;
 260                 } else {
 261                         s = 2.0 * Math.sqrt(1.0 + matrix.m33 - matrix.m11 - matrix.m22);
 262                         x = (matrix.m13 + matrix.m31) / s;
 263                         y = (matrix.m32 + matrix.m23) / s;
 264                         z = s / 4.0;
 265                         w = (matrix.m21 - matrix.m12) / s;
 266                 }
 267         return new Quaternion(x, y, z, w);
 268     }
 269
 270     public static Quaternion fromRotationYPR(Angle yaw, Angle pitch, Angle roll)
 271     {
 272         if ((yaw == null) || (pitch == null) || (roll == null))
 273         {
 274             String msg = Logging.getMessage("nullValue.AngleIsNull");
 275             Logging.logger().severe(msg);
 276             throw new IllegalArgumentException(msg);
 277         }
 278
 279         double cy = yaw.cosHalfAngle();
 280         double cp = pitch.cosHalfAngle();
 281         double cr = roll.cosHalfAngle();
 282         double sy = yaw.sinHalfAngle();
 283         double sp = pitch.sinHalfAngle();
 284         double sr = roll.sinHalfAngle();
 285
 286         double qw = (cy * cp * cr) + (sy * sp * sr);
 287         double qx = (sy * cp * cr) - (cy * sp * sr);
 288         double qy = (cy * sp * cr) + (sy * cp * sr);
 289         double qz = (cy * cp * sr) - (sy * sp * cr);
 290
 291         return new Quaternion(qx, qy, qz, qw);
 292     }
 293
 294     // ============== Arithmetic Functions ======================= //
 295     // ============== Arithmetic Functions ======================= //
 296     // ============== Arithmetic Functions ======================= //
 297
 298     public final Quaternion add(Quaternion quaternion)
 299     {
 300         if (quaternion == null)
 301         {
 302             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 303             Logging.logger().severe(msg);
 304             throw new IllegalArgumentException(msg);
 305         }
 306
 307         return new Quaternion(
 308             this.x + quaternion.x,
 309             this.y + quaternion.y,
 310             this.z + quaternion.z,
 311             this.w + quaternion.w);
 312     }
 313
 314     public final Quaternion subtract(Quaternion quaternion)
 315     {
 316         if (quaternion == null)
 317         {
 318             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 319             Logging.logger().severe(msg);
 320             throw new IllegalArgumentException(msg);
 321         }
 322
 323         return new Quaternion(
 324             this.x - quaternion.x,
 325             this.y - quaternion.y,
 326             this.z - quaternion.z,
 327             this.w - quaternion.w);
 328     }
 329
 330     public final Quaternion multiplyComponents(double value)
 331     {
 332         return new Quaternion(
 333             this.x * value,
 334             this.y * value,
 335             this.z * value,
 336             this.w * value);
 337     }
 338
 339     public final Quaternion multiply(Quaternion quaternion)
 340     {
 341         if (quaternion == null)
 342         {
 343             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 344             Logging.logger().severe(msg);
 345             throw new IllegalArgumentException(msg);
 346         }
 347
 348         return new Quaternion(
 349             (this.w * quaternion.x) + (this.x * quaternion.w) + (this.y * quaternion.z) - (this.z * quaternion.y),
 350             (this.w * quaternion.y) + (this.y * quaternion.w) + (this.z * quaternion.x) - (this.x * quaternion.z),
 351             (this.w * quaternion.z) + (this.z * quaternion.w) + (this.x * quaternion.y) - (this.y * quaternion.x),
 352             (this.w * quaternion.w) - (this.x * quaternion.x) - (this.y * quaternion.y) - (this.z * quaternion.z));
 353     }
 354
 355     public final Quaternion divideComponents(double value)
 356     {
 357         if (isZero(value))
 358         {
 359             String msg = Logging.getMessage("generic.ArgumentOutOfRange", value);
 360             Logging.logger().severe(msg);
 361             throw new IllegalArgumentException(msg);
 362         }
 363
 364         return new Quaternion(
 365             this.x / value,
 366             this.y / value,
 367             this.z / value,
 368             this.w / value);
 369     }
 370
 371     public final Quaternion divideComponents(Quaternion quaternion)
 372     {
 373         if (quaternion == null)
 374         {
 375             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 376             Logging.logger().severe(msg);
 377             throw new IllegalArgumentException(msg);
 378         }
 379
 380         return new Quaternion(
 381             this.x / quaternion.x,
 382             this.y / quaternion.y,
 383             this.z / quaternion.z,
 384             this.w / quaternion.w);
 385     }
 386
 387     public final Quaternion getConjugate()
 388     {
 389         return new Quaternion(
 390             0.0 - this.x,
 391             0.0 - this.y,
 392             0.0 - this.z,
 393             this.w);
 394     }
 395
 396     public final Quaternion getNegative()
 397     {
 398         return new Quaternion(
 399             0.0 - this.x,
 400             0.0 - this.y,
 401             0.0 - this.z,
 402             0.0 - this.w);
 403     }
 404
 405     // ============== Geometric Functions ======================= //
 406     // ============== Geometric Functions ======================= //
 407     // ============== Geometric Functions ======================= //
 408
 409     public final double getLength()
 410     {
 411         this.ensureLength();
 412         return this.length;
 413     }
 414
 415     private void ensureLength()
 416     {
 417         if (this.length >= 0.0)
 418             return;
 419
 420         // Compute cached length.
 421         this.length = Math.sqrt(this.getLengthSquared());
 422     }
 423
 424     public final double getLengthSquared()
 425     {
 426         this.ensureLengthSquared();
 427         return this.lengthSquared;
 428     }
 429
 430     private void ensureLengthSquared()
 431     {
 432         if (this.lengthSquared >= 0.0)
 433             return;
 434
 435         // Compute cached length.
 436         this.lengthSquared = (this.x * this.x)
 437             + (this.y * this.y)
 438             + (this.z * this.z)
 439             + (this.w * this.w);
 440     }
 441
 442     public final Quaternion normalize()
 443     {
 444         this.ensureNormalized();
 445         return this.normalized;
 446     }
 447
 448     private void ensureNormalized()
 449     {
 450         if (this.normalized != null)
 451             return;
 452
 453         // Compute cached normalization.
 454         double length = this.getLength();
 455         // Vector has zero length.
 456         if (isZero(length))
 457         {
 458             this.normalized = this;
 459         }
 460         else
 461         {
 462             this.normalized = new Quaternion(
 463             this.x / length,
 464             this.y / length,
 465             this.z / length,
 466             this.w / length);
 467         }
 468     }
 469
 470     public final double dot(Quaternion quaternion)
 471     {
 472         if (quaternion == null)
 473         {
 474             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 475             Logging.logger().severe(msg);
 476             throw new IllegalArgumentException(msg);
 477         }
 478
 479         return (this.x * quaternion.x) + (this.y * quaternion.y) + (this.z * quaternion.z) + (this.w * quaternion.w);
 480     }
 481
 482     public final Quaternion getInverse()
 483     {
 484         this.ensureInverse();
 485         return this.inverse;
 486     }
 487
 488     private void ensureInverse()
 489     {
 490         if (this.inverse != null)
 491             return;
 492
 493         double length = this.getLength();
 494         this.inverse = new Quaternion(
 495             (0.0 - this.x) / length,
 496             (0.0 - this.y) / length,
 497             (0.0 - this.z) / length,
 498             this.w / length);
 499     }
 500
 501     // ============== Mixing Functions ======================= //
 502     // ============== Mixing Functions ======================= //
 503     // ============== Mixing Functions ======================= //
 504
 505     public static Quaternion mix(double amount, Quaternion value1, Quaternion value2)
 506     {
 507         if ((value1 == null) || (value2 == null))
 508         {
 509             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 510             Logging.logger().severe(msg);
 511             throw new IllegalArgumentException(msg);
 512         }
 513
 514         if (amount < 0.0)
 515             return value1;
 516         else if (amount > 1.0)
 517             return value2;
 518
 519         double t1 = 1.0 - amount;
 520         return new Quaternion(
 521             (value1.x * t1) + (value2.x * amount),
 522             (value1.y * t1) + (value2.y * amount),
 523             (value1.z * t1) + (value2.z * amount),
 524             (value1.w * t1) + (value2.w * amount));
 525     }
 526
 527     public static Quaternion slerp(double amount, Quaternion value1, Quaternion value2)
 528     {
 529         if ((value1 == null) || (value2 == null))
 530         {
 531             String msg = Logging.getMessage("nullValue.QuaternionIsNull");
 532             Logging.logger().severe(msg);
 533             throw new IllegalArgumentException(msg);
 534         }
 535
 536         if (amount < 0.0)
 537             return value1;
 538         else if (amount > 1.0)
 539             return value2;
 540
 541         double dot = value1.dot(value2);
 542         double x2, y2, z2, w2;
 543         if (dot < 0.0)
 544         {
 545             dot = 0.0 - dot;
 546             x2 = 0.0 - value2.x;
 547             y2 = 0.0 - value2.y;
 548             z2 = 0.0 - value2.z;
 549             w2 = 0.0 - value2.w;
 550         }
 551         else
 552         {
 553             x2 = value2.x;
 554             y2 = value2.y;
 555             z2 = value2.z;
 556             w2 = value2.w;
 557         }
 558
 559         double t1, t2;
 560
 561         final double EPSILON = 0.0001;
 562         if ((1.0 - dot) > EPSILON) // standard case (slerp)
 563         {
 564             double angle = Math.acos(dot);
 565             double sinAngle = Math.sin(angle);
 566             t1 = Math.sin((1.0 - amount) * angle) / sinAngle;
 567             t2 = Math.sin(amount * angle) / sinAngle;
 568         }
 569         else // just lerp
 570         {
 571             t1 = 1.0 - amount;
 572             t2 = amount;
 573         }
 574
 575         return new Quaternion(
 576             (value1.x * t1) + (x2 * t2),
 577             (value1.y * t1) + (y2 * t2),
 578             (value1.z * t1) + (z2 * t2),
 579             (value1.w * t1) + (w2 * t2));
 580     }
 581
 582     // ============== Accessor Functions ======================= //
 583     // ============== Accessor Functions ======================= //
 584     // ============== Accessor Functions ======================= //
 585
 586     public final Angle getAngle()
 587     {
 588         double w = this.w;
 589
 590         double length = this.getLength();
 591         if (!isZero(length) && (length != 1.0))
 592             w /= length;
 593
 594         double radians = 2.0 * Math.acos(w);
 595         if (Double.isNaN(radians))
 596             return null;
 597
 598         return Angle.fromRadians(radians);
 599     }
 600
 601     public final Vec4 getAxis()
 602     {
 603         double x = this.x;
 604         double y = this.y;
 605         double z = this.z;
 606
 607         double length = this.getLength();
 608         if (!isZero(length) && (length != 1.0))
 609         {
 610             x /= length;
 611             y /= length;
 612             z /= length;
 613         }
 614
 615         double vecLength = Math.sqrt((x * x) + (y * y) + (z * z));
 616         if (!isZero(vecLength) && (vecLength != 1.0))
 617         {
 618             x /= vecLength;
 619             y /= vecLength;
 620             z /= vecLength;
 621         }
 622
 623         return new Vec4(x, y, z);
 624     }
 625
 626     public final Angle getRotationX()
 627     {
 628         double radians = Math.atan2(
 629             2.0 * (this.y * this.z + this.w * this.x),
 630             (this.w * this.w) - (this.x * this.x) - (this.y * this.y) + (this.z * this.z));
 631         if (Double.isNaN(radians))
 632             return null;
 633
 634         return Angle.fromRadians(radians);
 635     }
 636
 637     public final Angle getRotationY()
 638     {
 639         double radians = Math.asin(-2.0 * (this.x * this.z - this.w * this.y));
 640         if (Double.isNaN(radians))
 641             return null;
 642
 643         return Angle.fromRadians(radians);
 644     }
 645
 646     public final Angle getRotationZ()
 647     {
 648         double radians = Math.atan2(
 649             2.0 * (this.x * this.y + this.w * this.z),
 650             (this.w * this.w) + (this.x * this.x) - (this.y * this.y) - (this.z * this.z));
 651         if (Double.isNaN(radians))
 652             return null;
 653
 654         return Angle.fromRadians(radians);
 655     }
 656
 657     // ============== Helper Functions ======================= //
 658     // ============== Helper Functions ======================= //
 659     // ============== Helper Functions ======================= //
 660
 661     private static final Double PositiveZero = +0.0d;
 662
 663     private static final Double NegativeZero = -0.0d;
 664
 665     private static boolean isZero(double value)
 666     {
 667         return (PositiveZero.compareTo(value) == 0)
 668             || (NegativeZero.compareTo(value) == 0);
 669     }
 670
 671 //    /**
 672 //     * @param q
 673 //     * @return
 674 //     * @throws IllegalArgumentException if <code>q</code> is null
 675 //     */
 676 //    public static double Norm2(Quaternion q)
 677 //    {
 678 //        if (q == null)
 679 //        {
 680 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 681 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 682 //            throw new IllegalArgumentException(msg);
 683 //        }
 684 //        return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
 685 //    }
 686 //
 687 //    /**
 688 //     * @param q
 689 //     * @return
 690 //     * @throws IllegalArgumentException if <code>q</code> is null
 691 //     */
 692 //    public static double Abs(Quaternion q)
 693 //    {
 694 //        if (q == null)
 695 //        {
 696 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 697 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 698 //            throw new IllegalArgumentException(msg);
 699 //        }
 700 //        return Math.sqrt(Norm2(q));
 701 //    }
 702 //
 703 //    /**
 704 //     * @param a
 705 //     * @param b
 706 //     * @return
 707 //     * @throws IllegalArgumentException if <code>a</code> or <code>b</code> is null
 708 //     */
 709 //    public static Quaternion Divide(Quaternion a, Quaternion b)
 710 //    {
 711 //        if (a == null || b == null)
 712 //        {
 713 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 714 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 715 //            throw new IllegalArgumentException(msg);
 716 //        }
 717 //        return Quaternion.Multiply(a, Quaternion.Divide(b.Conjugate(), Abs(b)));
 718 //    }
 719
 720
 721 //    /*****************the below functions have not been certified to work properly ******************/
 722 //
 723 //    public Quaternion Ln()
 724 //    {
 725 //        return Ln(this);
 726 //    }
 727 //
 728 //    /**
 729 //     * @param q
 730 //     * @return
 731 //     * @throws IllegalArgumentException if <code>q</code> is null
 732 //     */
 733 //    public static Quaternion Ln(Quaternion q)
 734 //    {
 735 //        if (q == null)
 736 //        {
 737 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 738 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 739 //            throw new IllegalArgumentException(msg);
 740 //        }
 741 //        double t;
 742 //
 743 //        double s = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
 744 //        double om = Math.atan2(s, q.w);
 745 //
 746 //        if (Math.abs(s) < epsilon)
 747 //            t = 0.0f;
 748 //        else
 749 //            t = om / s;
 750 //
 751 //        return new Quaternion(q.x * t, q.y * t, q.z * t, 0.0f);
 752 //    }
 753 //
 754 //    /**
 755 //     * @param q
 756 //     * @return
 757 //     * @throws IllegalArgumentException if <code>q</code> is null
 758 //     */
 759 //    public static Quaternion Exp(Quaternion q)
 760 //    {
 761 //        if (q == null)
 762 //        {
 763 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 764 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 765 //            throw new IllegalArgumentException(msg);
 766 //        }
 767 //        double sinom;
 768 //        double om = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z);
 769 //
 770 //        if (Math.abs(om) < epsilon)
 771 //            sinom = 1.0;
 772 //        else
 773 //            sinom = Math.sin(om) / om;
 774 //
 775 //        return new Quaternion(q.x * sinom, q.y * sinom, q.z * sinom, Math.cos(om));
 776 //    }
 777 //
 778 //    public Quaternion Exp()
 779 //    {
 780 //        return Ln(this);
 781 //    }
 782 //
 783 //    /**
 784 //     * @param q1
 785 //     * @param a
 786 //     * @param b
 787 //     * @param c
 788 //     * @param t
 789 //     * @return
 790 //     * @throws IllegalArgumentException if any argument is null
 791 //     */
 792 //    public static Quaternion Squad(
 793 //        Quaternion q1,
 794 //        Quaternion a,
 795 //        Quaternion b,
 796 //        Quaternion c,
 797 //        double t)
 798 //    {
 799 //        if (q1 == null || a == null || b == null || c == null)
 800 //        {
 801 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 802 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 803 //            throw new IllegalArgumentException(msg);
 804 //        }
 805 //        return Slerp(
 806 //            Slerp(q1, c, t), Slerp(a, b, t), 2 * t * (1.0 - t));
 807 //    }
 808 //
 809 //    // This needs to be accounted for before Squad() is used
 810 //    public static Quaternion[] SquadSetup(
 811 //        Quaternion4d q0,
 812 //        Quaternion4d q1,
 813 //        Quaternion4d q2,
 814 //        Quaternion4d q3)
 815 //    {
 816 //        if(q0 == null || q1 == null || q2 == null || q3 == null)
 817 //        {
 818 //            String msg = gov.nasa.worldwind.WorldWind.retrieveErrMsg("nullValue.QuaternionIsNull");
 819 //            gov.nasa.worldwind.WorldWind.logger().logger(Level.SEVERE, msg);
 820 //            throw new IllegalArgumentException(msg);
 821 //        }
 822 //
 823 //        q0 = q0 + q1;
 824 //        q0.Normalize();
 825 //
 826 //        q2 = q2 + q1;
 827 //        q2.Normalize();
 828 //
 829 //        q3 = q3 + q1;
 830 //        q3.Normalize();
 831 //
 832 //        q1.Normalize();
 833 //
 834 //        Quaternion[] ret = new Quaternion[3];
 835 //
 836 //        ret[0] = q1 * Exp(-0.25 * (Ln(Exp(q1) * q2) + Ln(Exp(q1) * q0))); // outA
 837 //        ret[1] = q2 * Exp(-0.25 * (Ln(Exp(q2) * q3) + Ln(Exp(q2) * q1))); // outB
 838 //        ret[2] = q2;                                                      // outC
 839 //
 840 //    }
 841 }