2 * $RCSfile: Quat4d.java,v $
4 * Copyright 1997-2008 Sun Microsystems, Inc. All Rights Reserved.
5 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
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.
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).
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.
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
28 * $Date: 2008/02/28 20:18:50 $
32 package javax.vecmath;
35 * A 4-element quaternion represented by double precision floating
36 * point x,y,z,w coordinates. The quaternion is always normalized.
39 public class Quat4d extends Tuple4d implements java.io.Serializable {
41 // Combatible with 1.1
42 static final long serialVersionUID = 7577479888820201099L;
45 final static double EPS = 1.0e-12;
46 final static double EPS2 = 1.0e-30;
47 final static double PIO2 = 1.57079632679;
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
56 public Quat4d(double x, double y, double z, double w)
59 mag = 1.0/Math.sqrt( x*x + y*y + z*z + w*w );
68 * Constructs and initializes a Quat4d from the array of length 4.
69 * @param q the array of length 4 containing xyzw in order
71 public Quat4d(double[] q)
74 mag = 1.0/Math.sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] );
83 * Constructs and initializes a Quat4d from the specified Quat4d.
84 * @param q1 the Quat4d containing the initialization x y z w data
86 public Quat4d(Quat4d q1)
92 * Constructs and initializes a Quat4d from the specified Quat4f.
93 * @param q1 the Quat4f containing the initialization x y z w data
95 public Quat4d(Quat4f q1)
102 * Constructs and initializes a Quat4d from the specified Tuple4f.
103 * @param t1 the Tuple4f containing the initialization x y z w data
105 public Quat4d(Tuple4f t1)
108 mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
118 * Constructs and initializes a Quat4d from the specified Tuple4d.
119 * @param t1 the Tuple4d containing the initialization x y z w data
121 public Quat4d(Tuple4d t1)
124 mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
133 * Constructs and initializes a Quat4d to (0,0,0,0).
142 * Sets the value of this quaternion to the conjugate of quaternion q1.
143 * @param q1 the source vector
145 public final void conjugate(Quat4d q1)
155 * Negate the value of of each of this quaternion's x,y,z coordinates
158 public final void conjugate()
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
173 public final void mul(Quat4d q1, Quat4d q2)
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;
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;
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
199 public final void mul(Quat4d q1)
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;
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
220 public final void mulInverse(Quat4d q1, Quat4d q2)
222 Quat4d tempQuat = new Quat4d(q2);
225 this.mul(q1, tempQuat);
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
236 public final void mulInverse(Quat4d q1)
238 Quat4d tempQuat = new Quat4d(q1);
246 * Sets the value of this quaternion to quaternion inverse of quaternion q1.
247 * @param q1 the quaternion to be inverted
249 public final void inverse(Quat4d q1)
253 norm = 1.0/(q1.w*q1.w + q1.x*q1.x + q1.y*q1.y + q1.z*q1.z);
262 * Sets the value of this quaternion to the quaternion inverse of itself.
264 public final void inverse()
268 norm = 1.0/(this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z);
277 * Sets the value of this quaternion to the normalized value
279 * @param q1 the quaternion to be normalized.
281 public final void normalize(Quat4d q1)
285 norm = (q1.x*q1.x + q1.y*q1.y + q1.z*q1.z + q1.w*q1.w);
288 norm = 1.0/Math.sqrt(norm);
303 * Normalizes the value of this quaternion in place.
305 public final void normalize()
309 norm = (this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);
312 norm = 1.0 / Math.sqrt(norm);
327 * Sets the value of this quaternion to the rotational component of
329 * @param m1 the matrix4f
331 public final void set(Matrix4f m1)
333 double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
337 this.w = Math.sqrt(ww);
339 this.x = ((m1.m21 - m1.m12)*ww);
340 this.y = ((m1.m02 - m1.m20)*ww);
341 this.z = ((m1.m10 - m1.m01)*ww);
353 ww = -0.5*(m1.m11 + m1.m22);
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);
370 ww = 0.5*(1.0 - m1.m22);
372 this.y = Math.sqrt(ww);
373 this.z = (m1.m21)/(2.0*this.y);
383 * Sets the value of this quaternion to the rotational component of
385 * @param m1 the matrix4d
387 public final void set(Matrix4d m1)
389 double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
393 this.w = Math.sqrt(ww);
395 this.x = (m1.m21 - m1.m12)*ww;
396 this.y = (m1.m02 - m1.m20)*ww;
397 this.z = (m1.m10 - m1.m01)*ww;
409 ww = -0.5*(m1.m11 + m1.m22);
412 this.x = Math.sqrt(ww);
426 ww = 0.5*(1.0 - m1.m22);
428 this.y = Math.sqrt(ww);
429 this.z = m1.m21/(2.0*this.y);
439 * Sets the value of this quaternion to the rotational component of
441 * @param m1 the matrix3f
443 public final void set(Matrix3f m1)
445 double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0);
449 this.w = Math.sqrt(ww);
451 this.x = ((m1.m21 - m1.m12)*ww);
452 this.y = ((m1.m02 - m1.m20)*ww);
453 this.z = ((m1.m10 - m1.m01)*ww);
465 ww = -0.5*(m1.m11 + m1.m22);
468 this.x = Math.sqrt(ww);
470 this.y = (m1.m10*ww);
471 this.z = (m1.m20*ww);
482 ww = 0.5*(1.0 - m1.m22);
484 this.y = Math.sqrt(ww);
485 this.z = (m1.m21/(2.0*this.y));
494 * Sets the value of this quaternion to the rotational component of
496 * @param m1 the matrix3d
498 public final void set(Matrix3d m1)
500 double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0);
504 this.w = Math.sqrt(ww);
506 this.x = (m1.m21 - m1.m12)*ww;
507 this.y = (m1.m02 - m1.m20)*ww;
508 this.z = (m1.m10 - m1.m01)*ww;
520 ww = -0.5*(m1.m11 + m1.m22);
523 this.x = Math.sqrt(ww);
537 ww = 0.5*(1.0 - m1.m22);
539 this.y = Math.sqrt(ww);
540 this.z = m1.m21/(2.0*this.y);
550 * Sets the value of this quaternion to the equivalent rotation
551 * of the AxisAngle argument.
552 * @param a the AxisAngle to be emulated
554 public final void set(AxisAngle4f a)
557 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
559 amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
566 mag = Math.sin(a.angle/2.0);
568 w = Math.cos(a.angle/2.0);
577 * Sets the value of this quaternion to the equivalent rotation
578 * of the AxisAngle argument.
579 * @param a the AxisAngle to be emulated
581 public final void set(AxisAngle4d a)
584 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
586 amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
594 mag = Math.sin(a.angle/2.0);
595 w = Math.cos(a.angle/2.0);
604 * Performs a great circle interpolation between this quaternion
605 * and the quaternion parameter and places the result into this
607 * @param q1 the other quaternion
608 * @param alpha the alpha interpolation parameter
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;
619 dot = x*q1.x + y*q1.y + z*q1.z + w*q1.w;
623 q1.x = -q1.x; q1.y = -q1.y; q1.z = -q1.z; q1.w = -q1.w;
627 if ( (1.0 - dot) > EPS ) {
629 sinom = Math.sin(om);
630 s1 = Math.sin((1.0-alpha)*om)/sinom;
631 s2 = Math.sin( alpha*om)/sinom;
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
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;
659 dot = q2.x*q1.x + q2.y*q1.y + q2.z*q1.z + q2.w*q1.w;
663 q1.x = -q1.x; q1.y = -q1.y; q1.z = -q1.z; q1.w = -q1.w;
667 if ( (1.0 - dot) > EPS ) {
669 sinom = Math.sin(om);
670 s1 = Math.sin((1.0-alpha)*om)/sinom;
671 s2 = Math.sin( alpha*om)/sinom;
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;