2 * $RCSfile: Quat4f.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;
34 import java.lang.Math;
37 * A 4 element unit quaternion represented by single precision floating
38 * point x,y,z,w coordinates. The quaternion is always normalized.
41 public class Quat4f extends Tuple4f implements java.io.Serializable {
43 // Combatible with 1.1
44 static final long serialVersionUID = 2675933778405442383L;
46 final static double EPS = 0.000001;
47 final static double EPS2 = 1.0e-30;
48 final static double PIO2 = 1.57079632679;
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
57 public Quat4f(float x, float y, float z, float w)
60 mag = (float)(1.0/Math.sqrt( x*x + y*y + z*z + w*w ));
69 * Constructs and initializes a Quat4f from the array of length 4.
70 * @param q the array of length 4 containing xyzw in order
72 public Quat4f(float[] q)
75 mag = (float)(1.0/Math.sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] ));
85 * Constructs and initializes a Quat4f from the specified Quat4f.
86 * @param q1 the Quat4f containing the initialization x y z w data
88 public Quat4f(Quat4f q1)
94 * Constructs and initializes a Quat4f from the specified Quat4d.
95 * @param q1 the Quat4d containing the initialization x y z w data
97 public Quat4f(Quat4d q1)
104 * Constructs and initializes a Quat4f from the specified Tuple4f.
105 * @param t1 the Tuple4f containing the initialization x y z w data
107 public Quat4f(Tuple4f t1)
110 mag = (float)(1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w ));
120 * Constructs and initializes a Quat4f from the specified Tuple4d.
121 * @param t1 the Tuple4d containing the initialization x y z w data
123 public Quat4f(Tuple4d t1)
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);
135 * Constructs and initializes a Quat4f to (0.0,0.0,0.0,0.0).
144 * Sets the value of this quaternion to the conjugate of quaternion q1.
145 * @param q1 the source vector
147 public final void conjugate(Quat4f q1)
156 * Sets the value of this quaternion to the conjugate of itself.
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(Quat4f q1, Quat4f 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(Quat4f 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(Quat4f q1, Quat4f q2)
222 Quat4f tempQuat = new Quat4f(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(Quat4f q1)
238 Quat4f tempQuat = new Quat4f(q1);
247 * Sets the value of this quaternion to quaternion inverse of quaternion q1.
248 * @param q1 the quaternion to be inverted
250 public final void inverse(Quat4f q1)
254 norm = 1.0f/(q1.w*q1.w + q1.x*q1.x + q1.y*q1.y + q1.z*q1.z);
263 * Sets the value of this quaternion to the quaternion inverse of itself.
265 public final void inverse()
269 norm = 1.0f/(this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z);
278 * Sets the value of this quaternion to the normalized value
280 * @param q1 the quaternion to be normalized.
282 public final void normalize(Quat4f q1)
286 norm = (q1.x*q1.x + q1.y*q1.y + q1.z*q1.z + q1.w*q1.w);
289 norm = 1.0f/(float)Math.sqrt(norm);
295 this.x = (float) 0.0;
296 this.y = (float) 0.0;
297 this.z = (float) 0.0;
298 this.w = (float) 0.0;
304 * Normalizes the value of this quaternion in place.
306 public final void normalize()
310 norm = (this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);
313 norm = 1.0f / (float)Math.sqrt(norm);
319 this.x = (float) 0.0;
320 this.y = (float) 0.0;
321 this.z = (float) 0.0;
322 this.w = (float) 0.0;
328 * Sets the value of this quaternion to the rotational component of
330 * @param m1 the Matrix4f
332 public final void set(Matrix4f m1)
334 float ww = 0.25f*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
338 this.w = (float) Math.sqrt((double)ww);
340 this.x = (m1.m21 - m1.m12)*ww;
341 this.y = (m1.m02 - m1.m20)*ww;
342 this.z = (m1.m10 - m1.m01)*ww;
354 ww = -0.5f*(m1.m11 + m1.m22);
358 this.x = (float) Math.sqrt((double) ww);
359 ww = 1.0f/(2.0f*this.x);
372 ww = 0.5f*(1.0f - m1.m22);
375 this.y = (float) Math.sqrt((double) ww);
376 this.z = m1.m21/(2.0f*this.y);
386 * Sets the value of this quaternion to the rotational component of
388 * @param m1 the Matrix4d
390 public final void set(Matrix4d m1)
392 double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
396 this.w = (float) Math.sqrt(ww);
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);
412 ww = -0.5*(m1.m11 + m1.m22);
415 this.x = (float) Math.sqrt(ww);
417 this.y = (float)(m1.m10*ww);
418 this.z = (float)(m1.m20*ww);
429 ww = 0.5*(1.0 - m1.m22);
431 this.y = (float) Math.sqrt(ww);
432 this.z = (float) (m1.m21/(2.0*(double)(this.y)));
442 * Sets the value of this quaternion to the rotational component of
444 * @param m1 the Matrix3f
446 public final void set(Matrix3f m1)
448 float ww = 0.25f*(m1.m00 + m1.m11 + m1.m22 + 1.0f);
452 this.w = (float) Math.sqrt((double) ww);
454 this.x = (m1.m21 - m1.m12)*ww;
455 this.y = (m1.m02 - m1.m20)*ww;
456 this.z = (m1.m10 - m1.m01)*ww;
468 ww = -0.5f*(m1.m11 + m1.m22);
471 this.x = (float) Math.sqrt((double) ww);
485 ww = 0.5f*(1.0f - m1.m22);
487 this.y = (float) Math.sqrt((double) ww);
488 this.z = m1.m21/(2.0f*this.y);
498 * Sets the value of this quaternion to the rotational component of
500 * @param m1 the Matrix3d
502 public final void set(Matrix3d m1)
504 double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0f);
508 this.w = (float) Math.sqrt(ww);
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);
524 ww = -0.5*(m1.m11 + m1.m22);
527 this.x = (float) Math.sqrt(ww);
529 this.y = (float) (m1.m10*ww);
530 this.z = (float) (m1.m20*ww);
541 ww = 0.5*(1.0 - m1.m22);
543 this.y = (float) Math.sqrt(ww);
544 this.z = (float) (m1.m21/(2.0*(double)(this.y)));
554 * Sets the value of this quaternion to the equivalent rotation
555 * of the AxisAngle argument.
556 * @param a the AxisAngle to be emulated
558 public final void set(AxisAngle4f a)
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);
570 mag = (float)Math.sin(a.angle/2.0);
571 w = (float)Math.cos(a.angle/2.0);
580 * Sets the value of this quaternion to the equivalent rotation
581 * of the AxisAngle argument.
582 * @param a the AxisAngle to be emulated
584 public final void set(AxisAngle4d a)
587 // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
589 amag = (float)(1.0/Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z));
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;
609 * Performs a great circle interpolation between this quaternion
610 * and the quaternion parameter and places the result into this
612 * @param q1 the other quaternion
613 * @param alpha the alpha interpolation parameter
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.
623 double dot,s1,s2,om,sinom;
625 dot = x*q1.x + y*q1.y + z*q1.z + w*q1.w;
629 q1.x = -q1.x; q1.y = -q1.y; q1.z = -q1.z; q1.w = -q1.w;
633 if ( (1.0 - dot) > EPS ) {
635 sinom = Math.sin(om);
636 s1 = Math.sin((1.0-alpha)*om)/sinom;
637 s2 = Math.sin( alpha*om)/sinom;
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);
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
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.
666 double dot,s1,s2,om,sinom;
668 dot = q2.x*q1.x + q2.y*q1.y + q2.z*q1.z + q2.w*q1.w;
672 q1.x = -q1.x; q1.y = -q1.y; q1.z = -q1.z; q1.w = -q1.w;
676 if ( (1.0 - dot) > EPS ) {
678 sinom = Math.sin(om);
679 s1 = Math.sin((1.0-alpha)*om)/sinom;
680 s2 = Math.sin( alpha*om)/sinom;
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);