javax.vecmath/src/javax/vecmath/Quat4f.java

   1 /*
   2  * $RCSfile: Quat4f.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.6 $
  28  * $Date: 2008/02/28 20:18:50 $
  29  * $State: Exp $
  30  */
  31
  32 package javax.vecmath;
  33
  34 import java.lang.Math;
  35
  36 /**
  37  * A 4 element unit quaternion represented by single precision floating
  38  * point x,y,z,w coordinates.  The quaternion is always normalized.
  39  *
  40  */
  41 public class Quat4f extends Tuple4f implements java.io.Serializable {
  42
  43   // Combatible with 1.1
  44   static final long serialVersionUID = 2675933778405442383L;
  45
  46   final static double EPS = 0.000001;
  47   final static double EPS2 = 1.0e-30;
  48   final static double PIO2 = 1.57079632679;
  49
  50   /**
  51    * Constructs and initializes a Quat4f from the specified xyzw coordinates.
  52    * @param x the x coordinate
  53    * @param y the y coordinate
  54    * @param z the z coordinate
  55    * @param w the w scalar component
  56    */
  57   public Quat4f(float x, float y, float z, float w)
  58   {
  59       float mag;
  60       mag = (float)(1.0/Math.sqrt( x*x + y*y + z*z + w*w ));
  61       this.x =  x*mag;
  62       this.y =  y*mag;
  63       this.z =  z*mag;
  64       this.w =  w*mag;
  65
  66   }
  67
  68   /**
  69    * Constructs and initializes a Quat4f from the array of length 4.
  70    * @param q the array of length 4 containing xyzw in order
  71    */
  72   public Quat4f(float[] q)
  73   {
  74       float mag;
  75       mag = (float)(1.0/Math.sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] ));
  76       x =  q[0]*mag;
  77       y =  q[1]*mag;
  78       z =  q[2]*mag;
  79       w =  q[3]*mag;
  80
  81   }
  82
  83
  84   /**
  85    * Constructs and initializes a Quat4f from the specified Quat4f.
  86    * @param q1 the Quat4f containing the initialization x y z w data
  87    */
  88   public Quat4f(Quat4f q1)
  89   {
  90        super(q1);
  91   }
  92
  93   /**
  94    * Constructs and initializes a Quat4f from the specified Quat4d.
  95    * @param q1 the Quat4d containing the initialization x y z w data
  96    */
  97   public Quat4f(Quat4d q1)
  98   {
  99       super(q1);
 100   }
 101
 102
 103     /**
 104      * Constructs and initializes a Quat4f from the specified Tuple4f.
 105      * @param t1 the Tuple4f containing the initialization x y z w data
 106      */
 107     public Quat4f(Tuple4f t1)
 108     {
 109       float mag;
 110       mag = (float)(1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w ));
 111       x =  t1.x*mag;
 112       y =  t1.y*mag;
 113       z =  t1.z*mag;
 114       w =  t1.w*mag;
 115
 116     }
 117
 118
 119     /**
 120      * Constructs and initializes a Quat4f from the specified Tuple4d.
 121      * @param t1 the Tuple4d containing the initialization x y z w data
 122      */
 123     public Quat4f(Tuple4d t1)
 124     {
 125       double mag;
 126       mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
 127       x =  (float)(t1.x*mag);
 128       y =  (float)(t1.y*mag);
 129       z =  (float)(t1.z*mag);
 130       w =  (float)(t1.w*mag);
 131     }
 132
 133
 134   /**
 135    * Constructs and initializes a Quat4f to (0.0,0.0,0.0,0.0).
 136    */
 137   public Quat4f()
 138   {
 139       super();
 140   }
 141
 142
 143   /**
 144    * Sets the value of this quaternion to the conjugate of quaternion q1.
 145    * @param q1 the source vector
 146    */
 147   public final void conjugate(Quat4f q1)
 148   {
 149     this.x = -q1.x;
 150     this.y = -q1.y;
 151     this.z = -q1.z;
 152     this.w = q1.w;
 153   }
 154
 155   /**
 156    * Sets the value of this quaternion to the conjugate of itself.
 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(Quat4f q1, Quat4f 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       float     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(Quat4f q1)
 200   {
 201       float     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(Quat4f q1, Quat4f q2)
 221   {
 222       Quat4f  tempQuat = new Quat4f(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(Quat4f q1)
 237   {
 238       Quat4f  tempQuat = new Quat4f(q1);
 239
 240       tempQuat.inverse();
 241       this.mul(tempQuat);
 242   }
 243
 244
 245
 246   /**
 247    * Sets the value of this quaternion to quaternion inverse of quaternion q1.
 248    * @param q1 the quaternion to be inverted
 249    */
 250   public final void inverse(Quat4f q1)
 251   {
 252     float norm;
 253
 254     norm = 1.0f/(q1.w*q1.w + q1.x*q1.x + q1.y*q1.y + q1.z*q1.z);
 255     this.w =  norm*q1.w;
 256     this.x = -norm*q1.x;
 257     this.y = -norm*q1.y;
 258     this.z = -norm*q1.z;
 259   }
 260
 261
 262   /**
 263    * Sets the value of this quaternion to the quaternion inverse of itself.
 264    */
 265   public final void inverse()
 266   {
 267     float norm;
 268
 269     norm = 1.0f/(this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z);
 270     this.w *=  norm;
 271     this.x *= -norm;
 272     this.y *= -norm;
 273     this.z *= -norm;
 274   }
 275
 276
 277   /**
 278    * Sets the value of this quaternion to the normalized value
 279    * of quaternion q1.
 280    * @param q1 the quaternion to be normalized.
 281    */
 282   public final void normalize(Quat4f q1)
 283   {
 284     float norm;
 285
 286     norm = (q1.x*q1.x + q1.y*q1.y + q1.z*q1.z + q1.w*q1.w);
 287
 288     if (norm > 0.0f) {
 289       norm = 1.0f/(float)Math.sqrt(norm);
 290       this.x = norm*q1.x;
 291       this.y = norm*q1.y;
 292       this.z = norm*q1.z;
 293       this.w = norm*q1.w;
 294     } else {
 295       this.x = (float) 0.0;
 296       this.y = (float) 0.0;
 297       this.z = (float) 0.0;
 298       this.w = (float) 0.0;
 299     }
 300   }
 301
 302
 303   /**
 304    * Normalizes the value of this quaternion in place.
 305    */
 306   public final void normalize()
 307   {
 308     float norm;
 309
 310     norm = (this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);
 311
 312     if (norm > 0.0f) {
 313       norm = 1.0f / (float)Math.sqrt(norm);
 314       this.x *= norm;
 315       this.y *= norm;
 316       this.z *= norm;
 317       this.w *= norm;
 318     } else {
 319       this.x = (float) 0.0;
 320       this.y = (float) 0.0;
 321       this.z = (float) 0.0;
 322       this.w = (float) 0.0;
 323     }
 324   }
 325
 326
 327   /**
 328    * Sets the value of this quaternion to the rotational component of
 329    * the passed matrix.
 330    * @param m1 the Matrix4f
 331    */
 332    public final void set(Matrix4f m1)
 333    {
 334        float ww = 0.25f*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
 335
 336        if (ww >= 0) {
 337            if (ww >= EPS2) {
 338                this.w = (float) Math.sqrt((double)ww);
 339                ww =  0.25f/this.w;
 340                this.x = (m1.m21 - m1.m12)*ww;
 341                this.y = (m1.m02 - m1.m20)*ww;
 342                this.z = (m1.m10 - m1.m01)*ww;
 343                return;
 344            }
 345        } else {
 346            this.w = 0;
 347            this.x = 0;
 348            this.y = 0;
 349            this.z = 1;
 350            return;
 351        }
 352
 353        this.w = 0;
 354        ww = -0.5f*(m1.m11 + m1.m22);
 355
 356        if (ww >= 0) {
 357            if (ww >= EPS2) {
 358                this.x = (float) Math.sqrt((double) ww);
 359                ww = 1.0f/(2.0f*this.x);
 360                this.y = m1.m10*ww;
 361                this.z = m1.m20*ww;
 362                return;
 363            }
 364        } else {
 365            this.x = 0;
 366            this.y = 0;
 367            this.z = 1;
 368            return;
 369        }
 370
 371        this.x = 0;
 372        ww = 0.5f*(1.0f - m1.m22);
 373
 374        if (ww >= EPS2) {
 375            this.y = (float) Math.sqrt((double) ww);
 376            this.z = m1.m21/(2.0f*this.y);
 377            return;
 378        }
 379
 380        this.y = 0;
 381        this.z = 1;
 382    }
 383
 384
 385     /**
 386      * Sets the value of this quaternion to the rotational component of
 387      * the passed matrix.
 388      * @param m1 the Matrix4d
 389      */
 390     public final void set(Matrix4d m1)
 391     {
 392         double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
 393
 394         if (ww >= 0) {
 395             if (ww >= EPS2) {
 396                 this.w = (float) Math.sqrt(ww);
 397                 ww = 0.25/this.w;
 398                 this.x = (float) ((m1.m21 - m1.m12)*ww);
 399                 this.y = (float) ((m1.m02 - m1.m20)*ww);
 400                 this.z = (float) ((m1.m10 - m1.m01)*ww);
 401                 return;
 402             }
 403         } else {
 404             this.w = 0;
 405             this.x = 0;
 406             this.y = 0;
 407             this.z = 1;
 408             return;
 409         }
 410
 411         this.w = 0;
 412         ww = -0.5*(m1.m11 + m1.m22);
 413         if (ww >= 0) {
 414             if (ww >= EPS2) {
 415                 this.x = (float) Math.sqrt(ww);
 416                 ww = 0.5/this.x;
 417                 this.y = (float)(m1.m10*ww);
 418                 this.z = (float)(m1.m20*ww);
 419                 return;
 420             }
 421         } else {
 422             this.x = 0;
 423             this.y = 0;
 424             this.z = 1;
 425             return;
 426         }
 427
 428         this.x = 0;
 429         ww = 0.5*(1.0 - m1.m22);
 430         if (ww >= EPS2) {
 431             this.y = (float) Math.sqrt(ww);
 432             this.z = (float) (m1.m21/(2.0*(double)(this.y)));
 433             return;
 434         }
 435
 436         this.y = 0;
 437         this.z = 1;
 438     }
 439
 440
 441     /**
 442      * Sets the value of this quaternion to the rotational component of
 443      * the passed matrix.
 444      * @param m1 the Matrix3f
 445      */
 446     public final void set(Matrix3f m1)
 447     {
 448         float ww = 0.25f*(m1.m00 + m1.m11 + m1.m22 + 1.0f);
 449
 450         if (ww >= 0) {
 451             if (ww >= EPS2) {
 452                 this.w = (float) Math.sqrt((double) ww);
 453                 ww = 0.25f/this.w;
 454                 this.x = (m1.m21 - m1.m12)*ww;
 455                 this.y = (m1.m02 - m1.m20)*ww;
 456                 this.z = (m1.m10 - m1.m01)*ww;
 457                 return;
 458             }
 459         } else {
 460             this.w = 0;
 461             this.x = 0;
 462             this.y = 0;
 463             this.z = 1;
 464             return;
 465         }
 466
 467         this.w = 0;
 468         ww = -0.5f*(m1.m11 + m1.m22);
 469         if (ww >= 0) {
 470             if (ww >= EPS2) {
 471                 this.x = (float) Math.sqrt((double) ww);
 472                 ww = 0.5f/this.x;
 473                 this.y = m1.m10*ww;
 474                 this.z = m1.m20*ww;
 475                 return;
 476             }
 477         } else {
 478             this.x = 0;
 479             this.y = 0;
 480             this.z = 1;
 481             return;
 482         }
 483
 484         this.x = 0;
 485         ww =  0.5f*(1.0f - m1.m22);
 486         if (ww >= EPS2) {
 487             this.y = (float) Math.sqrt((double) ww);
 488             this.z = m1.m21/(2.0f*this.y);
 489             return;
 490         }
 491
 492         this.y = 0;
 493         this.z = 1;
 494     }
 495
 496
 497     /**
 498      * Sets the value of this quaternion to the rotational component of
 499      * the passed matrix.
 500      * @param m1 the Matrix3d
 501      */
 502     public final void set(Matrix3d m1)
 503     {
 504         double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0f);
 505
 506         if (ww >= 0) {
 507             if (ww >= EPS2) {
 508                 this.w = (float) Math.sqrt(ww);
 509                 ww = 0.25/this.w;
 510                 this.x = (float) ((m1.m21 - m1.m12)*ww);
 511                 this.y = (float) ((m1.m02 - m1.m20)*ww);
 512                 this.z = (float) ((m1.m10 - m1.m01)*ww);
 513                 return;
 514             }
 515         } else {
 516             this.w = 0;
 517             this.x = 0;
 518             this.y = 0;
 519             this.z = 1;
 520             return;
 521         }
 522
 523         this.w = 0;
 524         ww = -0.5*(m1.m11 + m1.m22);
 525         if (ww >= 0) {
 526             if (ww >= EPS2) {
 527                 this.x = (float) Math.sqrt(ww);
 528                 ww = 0.5/this.x;
 529                 this.y = (float) (m1.m10*ww);
 530                 this.z = (float) (m1.m20*ww);
 531                 return;
 532             }
 533         } else {
 534             this.x = 0;
 535             this.y = 0;
 536             this.z = 1;
 537             return;
 538         }
 539
 540         this.x = 0;
 541         ww = 0.5*(1.0 - m1.m22);
 542         if (ww >= EPS2) {
 543             this.y = (float) Math.sqrt(ww);
 544             this.z = (float) (m1.m21/(2.0*(double)(this.y)));
 545             return;
 546         }
 547
 548         this.y = 0;
 549         this.z = 1;
 550     }
 551
 552
 553     /**
 554      * Sets the value of this quaternion to the equivalent rotation
 555      * of the AxisAngle argument.
 556      * @param a  the AxisAngle to be emulated
 557      */
 558     public final void set(AxisAngle4f a)
 559     {
 560         float mag,amag;
 561         // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 562         amag = (float)Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
 563         if (amag < EPS ) {
 564             w = 0.0f;
 565             x = 0.0f;
 566             y = 0.0f;
 567             z = 0.0f;
 568         } else {
 569             amag = 1.0f/amag;
 570             mag = (float)Math.sin(a.angle/2.0);
 571             w = (float)Math.cos(a.angle/2.0);
 572             x = a.x*amag*mag;
 573             y = a.y*amag*mag;
 574             z = a.z*amag*mag;
 575         }
 576     }
 577
 578
 579     /**
 580      * Sets the value of this quaternion to the equivalent rotation
 581      * of the AxisAngle argument.
 582      * @param a  the AxisAngle to be emulated
 583      */
 584     public final void set(AxisAngle4d a)
 585     {
 586         float mag,amag;
 587         // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
 588
 589         amag = (float)(1.0/Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z));
 590
 591         if (amag < EPS ) {
 592             w = 0.0f;
 593             x = 0.0f;
 594             y = 0.0f;
 595             z = 0.0f;
 596         } else {
 597             amag = 1.0f/amag;
 598             mag = (float)Math.sin(a.angle/2.0);
 599             w = (float)Math.cos(a.angle/2.0);
 600             x = (float)a.x*amag*mag;
 601             y = (float)a.y*amag*mag;
 602             z = (float)a.z*amag*mag;
 603         }
 604
 605     }
 606
 607
 608     /**
 609      *  Performs a great circle interpolation between this quaternion
 610      *  and the quaternion parameter and places the result into this
 611      *  quaternion.
 612      *  @param q1  the other quaternion
 613      *  @param alpha  the alpha interpolation parameter
 614      */
 615     public final void interpolate(Quat4f q1, float alpha) {
 616         // From "Advanced Animation and Rendering Techniques"
 617         // by Watt and Watt pg. 364, function as implemented appeared to be
 618         // incorrect.  Fails to choose the same quaternion for the double
 619         // covering. Resulting in change of direction for rotations.
 620         // Fixed function to negate the first quaternion in the case that the
 621         // dot product of q1 and this is negative. Second case was not needed.
 622
 623      double dot,s1,s2,om,sinom;
 624
 625      dot = x*q1.x + y*q1.y + z*q1.z + w*q1.w;
 626
 627      if ( dot < 0 ) {
 628         // negate quaternion
 629        q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
 630        dot = -dot;
 631      }
 632
 633      if ( (1.0 - dot) > EPS ) {
 634        om = Math.acos(dot);
 635        sinom = Math.sin(om);
 636        s1 = Math.sin((1.0-alpha)*om)/sinom;
 637        s2 = Math.sin( alpha*om)/sinom;
 638      } else{
 639        s1 = 1.0 - alpha;
 640        s2 = alpha;
 641      }
 642
 643      w = (float)(s1*w + s2*q1.w);
 644      x = (float)(s1*x + s2*q1.x);
 645      y = (float)(s1*y + s2*q1.y);
 646      z = (float)(s1*z + s2*q1.z);
 647   }
 648
 649
 650
 651   /**
 652     *  Performs a great circle interpolation between quaternion q1
 653     *  and quaternion q2 and places the result into this quaternion.
 654     *  @param q1  the first quaternion
 655     *  @param q2  the second quaternion
 656     *  @param alpha  the alpha interpolation parameter
 657     */
 658     public final void interpolate(Quat4f q1, Quat4f q2, float alpha) {
 659         // From "Advanced Animation and Rendering Techniques"
 660         // by Watt and Watt pg. 364, function as implemented appeared to be
 661         // incorrect.  Fails to choose the same quaternion for the double
 662         // covering. Resulting in change of direction for rotations.
 663         // Fixed function to negate the first quaternion in the case that the
 664         // dot product of q1 and this is negative. Second case was not needed.
 665
 666      double dot,s1,s2,om,sinom;
 667
 668      dot = q2.x*q1.x + q2.y*q1.y + q2.z*q1.z + q2.w*q1.w;
 669
 670      if ( dot < 0 ) {
 671         // negate quaternion
 672        q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
 673        dot = -dot;
 674      }
 675
 676      if ( (1.0 - dot) > EPS ) {
 677        om = Math.acos(dot);
 678        sinom = Math.sin(om);
 679        s1 = Math.sin((1.0-alpha)*om)/sinom;
 680        s2 = Math.sin( alpha*om)/sinom;
 681      } else{
 682        s1 = 1.0 - alpha;
 683        s2 = alpha;
 684      }
 685      w = (float)(s1*q1.w + s2*q2.w);
 686      x = (float)(s1*q1.x + s2*q2.x);
 687      y = (float)(s1*q1.y + s2*q2.y);
 688      z = (float)(s1*q1.z + s2*q2.z);
 689   }
 690
 691 }
 692
 693
 694
 695