javax.vecmath/src/javax/vecmath/Quat4d.java

   1 /*
   2  * $RCSfile: Quat4d.java,v $
   3  *
   4  * Copyright 1997-2008 Sun Microsystems, Inc.  All Rights Reserved.
   5  * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
   6  *
   7  * This code is free software; you can redistribute it and/or modify it
   8  * under the terms of the GNU General Public License version 2 only, as
   9  * published by the Free Software Foundation.  Sun designates this
  10  * particular file as subject to the "Classpath" exception as provided
  11  * by Sun in the LICENSE file that accompanied this code.
  12  *
  13  * This code is distributed in the hope that it will be useful, but WITHOUT
  14  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  15  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  16  * version 2 for more details (a copy is included in the LICENSE file that
  17  * accompanied this code).
  18  *
  19  * You should have received a copy of the GNU General Public License version
  20  * 2 along with this work; if not, write to the Free Software Foundation,
  21  * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
  22  *
  23  * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
  24  * CA 95054 USA or visit www.sun.com if you need additional information or
  25  * have any questions.
  26  *
  27  * $Revision: 1.7 $
  28  * $Date: 2008/02/28 20:18:50 $
  29  * $State: Exp $
  30  */
  31
  32 package javax.vecmath;
  33
  34 /**
  35  * A 4-element quaternion represented by double precision floating
  36  * point x,y,z,w coordinates.  The quaternion is always normalized.
  37  *
  38  */
  39 public class Quat4d extends Tuple4d implements java.io.Serializable {
  40
  41   // Combatible with 1.1
  42   static final long serialVersionUID = 7577479888820201099L;
  43
  44   // Fixed to issue 538
  45   final static double EPS = 1.0e-12;
  46   final static double EPS2 = 1.0e-30;
  47   final static double PIO2 = 1.57079632679;
  48
  49   /**
  50    * Constructs and initializes a Quat4d from the specified xyzw coordinates.
  51    * @param x the x coordinate
  52    * @param y the y coordinate
  53    * @param z the z coordinate
  54    * @param w the w scalar component
  55    */
  56   public Quat4d(double x, double y, double z, double w)
  57   {
  58       double mag;
  59       mag = 1.0/Math.sqrt( x*x + y*y + z*z + w*w );
  60       this.x =  x*mag;
  61       this.y =  y*mag;
  62       this.z =  z*mag;
  63       this.w =  w*mag;
  64
  65   }
  66
  67   /**
  68    * Constructs and initializes a Quat4d from the array of length 4.
  69    * @param q the array of length 4 containing xyzw in order
  70    */
  71   public Quat4d(double[] q)
  72   {
  73       double mag;
  74       mag = 1.0/Math.sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] );
  75       x =  q[0]*mag;
  76       y =  q[1]*mag;
  77       z =  q[2]*mag;
  78       w =  q[3]*mag;
  79
  80   }
  81
  82   /**
  83    * Constructs and initializes a Quat4d from the specified Quat4d.
  84    * @param q1 the Quat4d containing the initialization x y z w data
  85    */
  86   public Quat4d(Quat4d q1)
  87   {
  88        super(q1);
  89   }
  90
  91   /**
  92    * Constructs and initializes a Quat4d from the specified Quat4f.
  93    * @param q1 the Quat4f containing the initialization x y z w data
  94    */
  95   public Quat4d(Quat4f q1)
  96   {
  97      super(q1);
  98   }
  99
 100
 101     /**
 102      * Constructs and initializes a Quat4d from the specified Tuple4f.
 103      * @param t1 the Tuple4f containing the initialization x y z w data
 104      */
 105     public Quat4d(Tuple4f t1)
 106     {
 107       double mag;
 108       mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
 109       x =  t1.x*mag;
 110       y =  t1.y*mag;
 111       z =  t1.z*mag;
 112       w =  t1.w*mag;
 113
 114     }
 115
 116
 117     /**
 118      * Constructs and initializes a Quat4d from the specified Tuple4d.
 119      * @param t1 the Tuple4d containing the initialization x y z w data
 120      */
 121     public Quat4d(Tuple4d t1)
 122     {
 123       double mag;
 124       mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
 125       x =  t1.x*mag;
 126       y =  t1.y*mag;
 127       z =  t1.z*mag;
 128       w =  t1.w*mag;
 129     }
 130
 131
 132   /**
 133    * Constructs and initializes a Quat4d to (0,0,0,0).
 134    */
 135   public Quat4d()
 136   {
 137      super();
 138   }
 139
 140
 141   /**
 142    * Sets the value of this quaternion to the conjugate of quaternion q1.
 143    * @param q1 the source vector
 144    */
 145   public final void conjugate(Quat4d q1)
 146   {
 147     this.x = -q1.x;
 148     this.y = -q1.y;
 149     this.z = -q1.z;
 150     this.w = q1.w;
 151   }
 152
 153
 154   /**
 155    * Negate the value of of each of this quaternion's x,y,z coordinates
 156    *  in place.
 157    */
 158   public final void conjugate()
 159   {
 160     this.x = -this.x;
 161     this.y = -this.y;
 162     this.z = -this.z;
 163   }
 164
 165
 166   /**
 167    * Sets the value of this quaternion to the quaternion product of
 168    * quaternions q1 and q2 (this = q1 * q2).
 169    * Note that this is safe for aliasing (e.g. this can be q1 or q2).
 170    * @param q1 the first quaternion
 171    * @param q2 the second quaternion
 172    */
 173   public final void mul(Quat4d q1, Quat4d q2)
 174   {
 175     if (this != q1 && this != q2) {
 176       this.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
 177       this.x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
 178       this.y = q1.w*q2.y + q2.w*q1.y - q1.x*q2.z + q1.z*q2.x;
 179       this.z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
 180     } else {
 181       double    x, y, w;
 182
 183       w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
 184       x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
 185       y = q1.w*q2.y + q2.w*q1.y - q1.x*q2.z + q1.z*q2.x;
 186       this.z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
 187       this.w = w;
 188       this.x = x;
 189       this.y = y;
 190     }
 191   }
 192
 193
 194  /**
 195    * Sets the value of this quaternion to the quaternion product of
 196    * itself and q1 (this = this * q1).
 197    * @param q1 the other quaternion
 198    */
 199   public final void mul(Quat4d q1)
 200   {
 201       double     x, y, w;
 202
 203        w = this.w*q1.w - this.x*q1.x - this.y*q1.y - this.z*q1.z;
 204        x = this.w*q1.x + q1.w*this.x + this.y*q1.z - this.z*q1.y;
 205        y = this.w*q1.y + q1.w*this.y - this.x*q1.z + this.z*q1.x;
 206        this.z = this.w*q1.z + q1.w*this.z + this.x*q1.y - this.y*q1.x;
 207        this.w = w;
 208        this.x = x;
 209        this.y = y;
 210   }
 211
 212
 213  /**
 214    * Multiplies quaternion q1 by the inverse of quaternion q2 and places
 215    * the value into this quaternion.  The value of both argument quaternions
 216    * is preservered (this = q1 * q2^-1).
 217    * @param q1 the first quaternion
 218    * @param q2 the second quaternion
 219    */
 220   public final void mulInverse(Quat4d q1, Quat4d q2)
 221   {
 222       Quat4d  tempQuat = new Quat4d(q2);
 223
 224       tempQuat.inverse();
 225       this.mul(q1, tempQuat);
 226   }
 227
 228
 229
 230  /**
 231    * Multiplies this quaternion by the inverse of quaternion q1 and places
 232    * the value into this quaternion.  The value of the argument quaternion
 233    * is preserved (this = this * q^-1).
 234    * @param q1 the other quaternion
 235    */
 236   public final void mulInverse(Quat4d q1)
 237   {
 238       Quat4d  tempQuat = new Quat4d(q1);
 239
 240       tempQuat.inverse();
 241       this.mul(tempQuat);
 242   }
 243
 244
 245   /**
 246    * Sets the value of this quaternion to quaternion inverse of quaternion q1.
 247    * @param q1 the quaternion to be inverted
 248    */
 249   public final void inverse(Quat4d q1)
 250   {
 251     double norm;
 252
 253     norm = 1.0/(q1.w*q1.w + q1.x*q1.x + q1.y*q1.y + q1.z*q1.z);
 254     this.w =  norm*q1.w;
 255     this.x = -norm*q1.x;
 256     this.y = -norm*q1.y;
 257     this.z = -norm*q1.z;
 258   }
 259
 260
 261   /**
 262    * Sets the value of this quaternion to the quaternion inverse of itself.
 263    */
 264   public final void inverse()
 265   {
 266     double norm;
 267
 268     norm = 1.0/(this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z);
 269     this.w *=  norm;
 270     this.x *= -norm;
 271     this.y *= -norm;
 272     this.z *= -norm;
 273   }
 274
 275
 276   /**
 277    * Sets the value of this quaternion to the normalized value
 278    * of quaternion q1.
 279    * @param q1 the quaternion to be normalized.
 280    */
 281   public final void normalize(Quat4d q1)
 282   {
 283     double norm;
 284
 285     norm = (q1.x*q1.x + q1.y*q1.y + q1.z*q1.z + q1.w*q1.w);
 286
 287     if (norm > 0.0) {
 288       norm = 1.0/Math.sqrt(norm);
 289       this.x = norm*q1.x;
 290       this.y = norm*q1.y;
 291       this.z = norm*q1.z;
 292       this.w = norm*q1.w;
 293     } else {
 294       this.x =  0.0;
 295       this.y =  0.0;
 296       this.z =  0.0;
 297       this.w =  0.0;
 298     }
 299   }
 300
 301
 302   /**
 303    * Normalizes the value of this quaternion in place.
 304    */
 305   public final void normalize()
 306   {
 307     double norm;
 308
 309     norm = (this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);
 310
 311     if (norm > 0.0) {
 312       norm = 1.0 / Math.sqrt(norm);
 313       this.x *= norm;
 314       this.y *= norm;
 315       this.z *= norm;
 316       this.w *= norm;
 317     } else {
 318       this.x =  0.0;
 319       this.y =  0.0;
 320       this.z =  0.0;
 321       this.w =  0.0;
 322     }
 323   }
 324
 325
 326     /**
 327      * Sets the value of this quaternion to the rotational component of
 328      * the passed matrix.
 329      * @param m1 the matrix4f
 330      */
 331     public final void set(Matrix4f m1)
 332     {
 333         double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
 334
 335         if (ww >= 0) {
 336             if (ww >= EPS2) {
 337                 this.w = Math.sqrt(ww);
 338                 ww =  0.25/this.w;
 339                 this.x =  ((m1.m21 - m1.m12)*ww);
 340                 this.y =  ((m1.m02 - m1.m20)*ww);
 341                 this.z =  ((m1.m10 - m1.m01)*ww);
 342                 return;
 343             }
 344         } else {
 345             this.w = 0;
 346             this.x = 0;
 347             this.y = 0;
 348             this.z = 1;
 349             return;
 350         }
 351
 352         this.w = 0;
 353         ww = -0.5*(m1.m11 + m1.m22);
 354         if (ww >= 0) {
 355             if (ww >= EPS2) {
 356                 this.x =  Math.sqrt(ww);
 357                 ww = 1.0/(2.0*this.x);
 358                 this.y = (m1.m10*ww);
 359                 this.z = (m1.m20*ww);
 360                 return;
 361             }
 362         } else {
 363             this.x = 0;
 364             this.y = 0;
 365             this.z = 1;
 366             return;
 367         }
 368
 369         this.x = 0;
 370         ww = 0.5*(1.0 - m1.m22);
 371         if (ww >= EPS2) {
 372             this.y = Math.sqrt(ww);
 373             this.z = (m1.m21)/(2.0*this.y);
 374             return;
 375         }
 376
 377         this.y = 0;
 378         this.z = 1;
 379     }
 380
 381
 382     /**
 383      * Sets the value of this quaternion to the rotational component of
 384      * the passed matrix.
 385      * @param m1 the matrix4d
 386      */
 387     public final void set(Matrix4d m1)
 388     {
 389         double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
 390
 391         if (ww >= 0) {
 392             if (ww >= EPS2) {
 393                 this.w = Math.sqrt(ww);
 394                 ww = 0.25/this.w;
 395                 this.x = (m1.m21 - m1.m12)*ww;
 396                 this.y = (m1.m02 - m1.m20)*ww;
 397                 this.z = (m1.m10 - m1.m01)*ww;
 398                 return;
 399             }
 400         } else {
 401             this.w = 0;
 402             this.x = 0;
 403             this.y = 0;
 404             this.z = 1;
 405             return;
 406         }
 407
 408         this.w = 0;
 409         ww = -0.5*(m1.m11 + m1.m22);
 410         if (ww >= 0) {
 411             if (ww >= EPS2){
 412                 this.x =  Math.sqrt(ww);
 413                 ww = 0.5/this.x;
 414                 this.y = m1.m10*ww;
 415                 this.z = m1.m20*ww;
 416                 return;
 417             }
 418         } else {
 419             this.x = 0;
 420             this.y = 0;
 421             this.z = 1;
 422             return;
 423         }
 424
 425         this.x = 0.0;
 426         ww = 0.5*(1.0 - m1.m22);
 427         if (ww >= EPS2) {
 428             this.y =  Math.sqrt(ww);
 429             this.z = m1.m21/(2.0*this.y);
 430             return;
 431         }
 432
 433         this.y =  0;
 434         this.z =  1;
 435     }
 436
 437
 438     /**
 439      * Sets the value of this quaternion to the rotational component of
 440      * the passed matrix.
 441      * @param m1 the matrix3f
 442      */
 443     public final void set(Matrix3f m1)
 444     {
 445         double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0);
 446
 447         if (ww >= 0) {
 448             if (ww >= EPS2) {
 449                 this.w = Math.sqrt(ww);
 450                 ww = 0.25/this.w;
 451                 this.x = ((m1.m21 - m1.m12)*ww);
 452                 this.y = ((m1.m02 - m1.m20)*ww);
 453                 this.z = ((m1.m10 - m1.m01)*ww);
 454                 return;
 455             }
 456         } else {
 457             this.w = 0;
 458             this.x = 0;
 459             this.y = 0;
 460             this.z = 1;
 461             return;
 462         }
 463
 464         this.w = 0;
 465         ww = -0.5*(m1.m11 + m1.m22);
 466         if (ww >= 0) {
 467             if (ww >= EPS2) {
 468                 this.x = Math.sqrt(ww);
 469                 ww = 0.5/this.x;
 470                 this.y = (m1.m10*ww);
 471                 this.z = (m1.m20*ww);
 472                 return;
 473             }
 474         } else {
 475             this.x = 0;
 476             this.y = 0;
 477             this.z = 1;
 478             return;
 479         }
 480
 481         this.x = 0;
 482         ww =  0.5*(1.0 - m1.m22);
 483         if (ww >= EPS2) {
 484             this.y = Math.sqrt(ww);
 485             this.z = (m1.m21/(2.0*this.y));
 486         }
 487
 488         this.y =  0;
 489         this.z =  1;
 490     }
 491
 492
 493     /**
 494      * Sets the value of this quaternion to the rotational component of
 495      * the passed matrix.
 496      * @param m1 the matrix3d
 497      */
 498     public final void set(Matrix3d m1)
 499     {
 500         double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0);
 501
 502         if (ww >= 0) {
 503             if (ww >= EPS2) {
 504                 this.w = Math.sqrt(ww);
 505                 ww = 0.25/this.w;
 506                 this.x = (m1.m21 - m1.m12)*ww;
 507                 this.y = (m1.m02 - m1.m20)*ww;
 508                 this.z = (m1.m10 - m1.m01)*ww;
 509                 return;
 510             }
 511         } else {
 512             this.w = 0;
 513             this.x = 0;
 514             this.y = 0;
 515             this.z = 1;
 516             return;
 517         }
 518
 519         this.w = 0;
 520         ww = -0.5*(m1.m11 + m1.m22);
 521         if (ww >= 0) {
 522             if (ww >= EPS2) {
 523                 this.x =  Math.sqrt(ww);
 524                 ww = 0.5/this.x;
 525                 this.y = m1.m10*ww;
 526                 this.z = m1.m20*ww;
 527                 return;
 528             }
 529         } else {
 530             this.x = 0;
 531             this.y = 0;
 532             this.z = 1;
 533             return;
 534         }
 535
 536         this.x = 0;
 537         ww = 0.5*(1.0 - m1.m22);
 538         if (ww >= EPS2) {
 539             this.y =  Math.sqrt(ww);
 540             this.z = m1.m21/(2.0*this.y);
 541             return;
 542         }
 543
 544         this.y = 0;
 545         this.z = 1;
 546     }
 547
 548
 549     /**
 550      * Sets the value of this quaternion to the equivalent rotation
 551      * of the AxisAngle argument.
 552      * @param a  the AxisAngle to be emulated
 553      */
 554     public final void set(AxisAngle4f a)
 555     {
 556         double mag,amag;
 557         // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 558
 559         amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
 560         if( amag < EPS ) {
 561             w = 0.0;
 562             x = 0.0;
 563             y = 0.0;
 564             z = 0.0;
 565         } else {
 566             mag = Math.sin(a.angle/2.0);
 567             amag = 1.0/amag;
 568             w = Math.cos(a.angle/2.0);
 569             x = a.x*amag*mag;
 570             y = a.y*amag*mag;
 571             z = a.z*amag*mag;
 572         }
 573
 574     }
 575
 576     /**
 577      * Sets the value of this quaternion to the equivalent rotation
 578      * of the AxisAngle argument.
 579      * @param a  the AxisAngle to be emulated
 580      */
 581     public final void set(AxisAngle4d a)
 582     {
 583         double mag,amag;
 584         // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 585
 586         amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
 587         if( amag < EPS ) {
 588             w = 0.0;
 589             x = 0.0;
 590             y = 0.0;
 591             z = 0.0;
 592         } else {
 593             amag = 1.0/amag;
 594             mag = Math.sin(a.angle/2.0);
 595             w = Math.cos(a.angle/2.0);
 596        x = a.x*amag*mag;
 597        y = a.y*amag*mag;
 598        z = a.z*amag*mag;
 599         }
 600
 601     }
 602
 603  /**
 604    *  Performs a great circle interpolation between this quaternion
 605    *  and the quaternion parameter and places the result into this
 606    *  quaternion.
 607    *  @param q1  the other quaternion
 608    *  @param alpha  the alpha interpolation parameter
 609    */
 610   public final void interpolate(Quat4d q1, double alpha) {
 611       // From "Advanced Animation and Rendering Techniques"
 612       // by Watt and Watt pg. 364, function as implemented appeared to be
 613       // incorrect.  Fails to choose the same quaternion for the double
 614       // covering. Resulting in change of direction for rotations.
 615       // Fixed function to negate the first quaternion in the case that the
 616       // dot product of q1 and this is negative. Second case was not needed.
 617       double dot,s1,s2,om,sinom;
 618
 619       dot = x*q1.x + y*q1.y + z*q1.z + w*q1.w;
 620
 621       if ( dot < 0 ) {
 622         // negate quaternion
 623         q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
 624         dot = -dot;
 625       }
 626
 627       if ( (1.0 - dot) > EPS ) {
 628         om = Math.acos(dot);
 629         sinom = Math.sin(om);
 630         s1 = Math.sin((1.0-alpha)*om)/sinom;
 631         s2 = Math.sin( alpha*om)/sinom;
 632       } else{
 633         s1 = 1.0 - alpha;
 634         s2 = alpha;
 635       }
 636
 637       w = s1*w + s2*q1.w;
 638       x = s1*x + s2*q1.x;
 639       y = s1*y + s2*q1.y;
 640       z = s1*z + s2*q1.z;
 641     }
 642
 643 /**
 644    *  Performs a great circle interpolation between quaternion q1
 645    *  and quaternion q2 and places the result into this quaternion.
 646    *  @param q1  the first quaternion
 647    *  @param q2  the second quaternion
 648    *  @param alpha  the alpha interpolation parameter
 649    */
 650   public final void interpolate(Quat4d q1, Quat4d q2, double alpha) {
 651       // From "Advanced Animation and Rendering Techniques"
 652       // by Watt and Watt pg. 364, function as implemented appeared to be
 653       // incorrect.  Fails to choose the same quaternion for the double
 654       // covering. Resulting in change of direction for rotations.
 655       // Fixed function to negate the first quaternion in the case that the
 656       // dot product of q1 and this is negative. Second case was not needed.
 657       double dot,s1,s2,om,sinom;
 658
 659       dot = q2.x*q1.x + q2.y*q1.y + q2.z*q1.z + q2.w*q1.w;
 660
 661       if ( dot < 0 ) {
 662         // negate quaternion
 663         q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
 664         dot = -dot;
 665       }
 666
 667       if ( (1.0 - dot) > EPS ) {
 668         om = Math.acos(dot);
 669         sinom = Math.sin(om);
 670         s1 = Math.sin((1.0-alpha)*om)/sinom;
 671         s2 = Math.sin( alpha*om)/sinom;
 672       } else{
 673         s1 = 1.0 - alpha;
 674         s2 = alpha;
 675       }
 676       w = s1*q1.w + s2*q2.w;
 677       x = s1*q1.x + s2*q2.x;
 678       y = s1*q1.y + s2*q2.y;
 679       z = s1*q1.z + s2*q2.z;
 680     }
 681
 682 }