/*
* $RCSfile: Quat4f.java,v $
*
* Copyright 1997-2008 Sun Microsystems, Inc. All Rights Reserved.
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
*
* This code is free software; you can redistribute it and/or modify it
* under the terms of the GNU General Public License version 2 only, as
* published by the Free Software Foundation. Sun designates this
* particular file as subject to the "Classpath" exception as provided
* by Sun in the LICENSE file that accompanied this code.
*
* This code is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* version 2 for more details (a copy is included in the LICENSE file that
* accompanied this code).
*
* You should have received a copy of the GNU General Public License version
* 2 along with this work; if not, write to the Free Software Foundation,
* Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
*
* Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
* CA 95054 USA or visit www.sun.com if you need additional information or
* have any questions.
*
* $Revision: 1.6 $
* $Date: 2008/02/28 20:18:50 $
* $State: Exp $
*/
package javax.vecmath;
import java.lang.Math;
/**
* A 4 element unit quaternion represented by single precision floating point
* x,y,z,w coordinates. The quaternion is always normalized.
*
*/
public class Quat4f extends Tuple4f implements java.io.Serializable {
// Combatible with 1.1
static final long serialVersionUID = 2675933778405442383L;
final static double EPS = 0.000001;
final static double EPS2 = 1.0e-30;
final static double PIO2 = 1.57079632679;
/**
* Constructs and initializes a Quat4f from the specified xyzw coordinates.
*
* @param x
* the x coordinate
* @param y
* the y coordinate
* @param z
* the z coordinate
* @param w
* the w scalar component
*/
public Quat4f(float x, float y, float z, float w) {
float mag;
mag = (float) (1.0 / Math.sqrt(x * x + y * y + z * z + w * w));
this.x = x * mag;
this.y = y * mag;
this.z = z * mag;
this.w = w * mag;
}
/**
* Constructs and initializes a Quat4f from the array of length 4.
*
* @param q
* the array of length 4 containing xyzw in order
*/
public Quat4f(float[] q) {
float mag;
mag = (float) (1.0 / Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]
+ q[3] * q[3]));
x = q[0] * mag;
y = q[1] * mag;
z = q[2] * mag;
w = q[3] * mag;
}
/**
* Constructs and initializes a Quat4f from the specified Quat4f.
*
* @param q1
* the Quat4f containing the initialization x y z w data
*/
public Quat4f(Quat4f q1) {
super(q1);
}
/**
* Constructs and initializes a Quat4f from the specified Quat4d.
*
* @param q1
* the Quat4d containing the initialization x y z w data
*/
public Quat4f(Quat4d q1) {
super(q1);
}
/**
* Constructs and initializes a Quat4f from the specified Tuple4f.
*
* @param t1
* the Tuple4f containing the initialization x y z w data
*/
public Quat4f(Tuple4f t1) {
float mag;
mag = (float) (1.0 / Math.sqrt(t1.x * t1.x + t1.y * t1.y + t1.z * t1.z
+ t1.w * t1.w));
x = t1.x * mag;
y = t1.y * mag;
z = t1.z * mag;
w = t1.w * mag;
}
/**
* Constructs and initializes a Quat4f from the specified Tuple4d.
*
* @param t1
* the Tuple4d containing the initialization x y z w data
*/
public Quat4f(Tuple4d t1) {
double mag;
mag = 1.0 / Math
.sqrt(t1.x * t1.x + t1.y * t1.y + t1.z * t1.z + t1.w * t1.w);
x = (float) (t1.x * mag);
y = (float) (t1.y * mag);
z = (float) (t1.z * mag);
w = (float) (t1.w * mag);
}
/**
* Constructs and initializes a Quat4f to (0.0,0.0,0.0,0.0).
*/
public Quat4f() {
super();
}
/**
* Sets the value of this quaternion to the conjugate of quaternion q1.
*
* @param q1
* the source vector
*/
public final void conjugate(Quat4f q1) {
this.x = -q1.x;
this.y = -q1.y;
this.z = -q1.z;
this.w = q1.w;
}
/**
* Sets the value of this quaternion to the conjugate of itself.
*/
public final void conjugate() {
this.x = -this.x;
this.y = -this.y;
this.z = -this.z;
}
/**
* Sets the value of this quaternion to the quaternion product of quaternions
* q1 and q2 (this = q1 * q2). Note that this is safe for aliasing (e.g. this
* can be q1 or q2).
*
* @param q1
* the first quaternion
* @param q2
* the second quaternion
*/
public final void mul(Quat4f q1, Quat4f q2) {
if (this != q1 && this != q2) {
this.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
this.x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
this.y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
} else {
float x, y, w;
w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
x = q1.w * q2.x + q2.w * q1.x + q1.y * q2.z - q1.z * q2.y;
y = q1.w * q2.y + q2.w * q1.y - q1.x * q2.z + q1.z * q2.x;
this.z = q1.w * q2.z + q2.w * q1.z + q1.x * q2.y - q1.y * q2.x;
this.w = w;
this.x = x;
this.y = y;
}
}
/**
* Sets the value of this quaternion to the quaternion product of itself and
* q1 (this = this * q1).
*
* @param q1
* the other quaternion
*/
public final void mul(Quat4f q1) {
float x, y, w;
w = this.w * q1.w - this.x * q1.x - this.y * q1.y - this.z * q1.z;
x = this.w * q1.x + q1.w * this.x + this.y * q1.z - this.z * q1.y;
y = this.w * q1.y + q1.w * this.y - this.x * q1.z + this.z * q1.x;
this.z = this.w * q1.z + q1.w * this.z + this.x * q1.y - this.y * q1.x;
this.w = w;
this.x = x;
this.y = y;
}
/**
* Multiplies quaternion q1 by the inverse of quaternion q2 and places the
* value into this quaternion. The value of both argument quaternions is
* preservered (this = q1 * q2^-1).
*
* @param q1
* the first quaternion
* @param q2
* the second quaternion
*/
public final void mulInverse(Quat4f q1, Quat4f q2) {
Quat4f tempQuat = new Quat4f(q2);
tempQuat.inverse();
this.mul(q1, tempQuat);
}
/**
* Multiplies this quaternion by the inverse of quaternion q1 and places the
* value into this quaternion. The value of the argument quaternion is
* preserved (this = this * q^-1).
*
* @param q1
* the other quaternion
*/
public final void mulInverse(Quat4f q1) {
Quat4f tempQuat = new Quat4f(q1);
tempQuat.inverse();
this.mul(tempQuat);
}
/**
* Sets the value of this quaternion to quaternion inverse of quaternion q1.
*
* @param q1
* the quaternion to be inverted
*/
public final void inverse(Quat4f q1) {
float norm;
norm = 1.0f / (q1.w * q1.w + q1.x * q1.x + q1.y * q1.y + q1.z * q1.z);
this.w = norm * q1.w;
this.x = -norm * q1.x;
this.y = -norm * q1.y;
this.z = -norm * q1.z;
}
/**
* Sets the value of this quaternion to the quaternion inverse of itself.
*/
public final void inverse() {
float norm;
norm = 1.0f / (this.w * this.w + this.x * this.x + this.y * this.y + this.z
* this.z);
this.w *= norm;
this.x *= -norm;
this.y *= -norm;
this.z *= -norm;
}
/**
* Sets the value of this quaternion to the normalized value of quaternion q1.
*
* @param q1
* the quaternion to be normalized.
*/
public final void normalize(Quat4f q1) {
float norm;
norm = (q1.x * q1.x + q1.y * q1.y + q1.z * q1.z + q1.w * q1.w);
if (norm > 0.0f) {
norm = 1.0f / (float) Math.sqrt(norm);
this.x = norm * q1.x;
this.y = norm * q1.y;
this.z = norm * q1.z;
this.w = norm * q1.w;
} else {
this.x = (float) 0.0;
this.y = (float) 0.0;
this.z = (float) 0.0;
this.w = (float) 0.0;
}
}
/**
* Normalizes the value of this quaternion in place.
*/
public final void normalize() {
float norm;
norm = (this.x * this.x + this.y * this.y + this.z * this.z + this.w
* this.w);
if (norm > 0.0f) {
norm = 1.0f / (float) Math.sqrt(norm);
this.x *= norm;
this.y *= norm;
this.z *= norm;
this.w *= norm;
} else {
this.x = (float) 0.0;
this.y = (float) 0.0;
this.z = (float) 0.0;
this.w = (float) 0.0;
}
}
/**
* Sets the value of this quaternion to the rotational component of the passed
* matrix.
*
* @param m1
* the Matrix4f
*/
public final void set(Matrix4f m1) {
float ww = 0.25f * (m1.m00 + m1.m11 + m1.m22 + m1.m33);
if (ww >= 0) {
if (ww >= EPS2) {
this.w = (float) Math.sqrt((double) ww);
ww = 0.25f / this.w;
this.x = (m1.m21 - m1.m12) * ww;
this.y = (m1.m02 - m1.m20) * ww;
this.z = (m1.m10 - m1.m01) * ww;
return;
}
} else {
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5f * (m1.m11 + m1.m22);
if (ww >= 0) {
if (ww >= EPS2) {
this.x = (float) Math.sqrt((double) ww);
ww = 1.0f / (2.0f * this.x);
this.y = m1.m10 * ww;
this.z = m1.m20 * ww;
return;
}
} else {
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5f * (1.0f - m1.m22);
if (ww >= EPS2) {
this.y = (float) Math.sqrt((double) ww);
this.z = m1.m21 / (2.0f * this.y);
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the rotational component of the passed
* matrix.
*
* @param m1
* the Matrix4d
*/
public final void set(Matrix4d m1) {
double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + m1.m33);
if (ww >= 0) {
if (ww >= EPS2) {
this.w = (float) Math.sqrt(ww);
ww = 0.25 / this.w;
this.x = (float) ((m1.m21 - m1.m12) * ww);
this.y = (float) ((m1.m02 - m1.m20) * ww);
this.z = (float) ((m1.m10 - m1.m01) * ww);
return;
}
} else {
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5 * (m1.m11 + m1.m22);
if (ww >= 0) {
if (ww >= EPS2) {
this.x = (float) Math.sqrt(ww);
ww = 0.5 / this.x;
this.y = (float) (m1.m10 * ww);
this.z = (float) (m1.m20 * ww);
return;
}
} else {
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5 * (1.0 - m1.m22);
if (ww >= EPS2) {
this.y = (float) Math.sqrt(ww);
this.z = (float) (m1.m21 / (2.0 * (double) (this.y)));
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the rotational component of the passed
* matrix.
*
* @param m1
* the Matrix3f
*/
public final void set(Matrix3f m1) {
float ww = 0.25f * (m1.m00 + m1.m11 + m1.m22 + 1.0f);
if (ww >= 0) {
if (ww >= EPS2) {
this.w = (float) Math.sqrt((double) ww);
ww = 0.25f / this.w;
this.x = (m1.m21 - m1.m12) * ww;
this.y = (m1.m02 - m1.m20) * ww;
this.z = (m1.m10 - m1.m01) * ww;
return;
}
} else {
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5f * (m1.m11 + m1.m22);
if (ww >= 0) {
if (ww >= EPS2) {
this.x = (float) Math.sqrt((double) ww);
ww = 0.5f / this.x;
this.y = m1.m10 * ww;
this.z = m1.m20 * ww;
return;
}
} else {
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5f * (1.0f - m1.m22);
if (ww >= EPS2) {
this.y = (float) Math.sqrt((double) ww);
this.z = m1.m21 / (2.0f * this.y);
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the rotational component of the passed
* matrix.
*
* @param m1
* the Matrix3d
*/
public final void set(Matrix3d m1) {
double ww = 0.25 * (m1.m00 + m1.m11 + m1.m22 + 1.0f);
if (ww >= 0) {
if (ww >= EPS2) {
this.w = (float) Math.sqrt(ww);
ww = 0.25 / this.w;
this.x = (float) ((m1.m21 - m1.m12) * ww);
this.y = (float) ((m1.m02 - m1.m20) * ww);
this.z = (float) ((m1.m10 - m1.m01) * ww);
return;
}
} else {
this.w = 0;
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.w = 0;
ww = -0.5 * (m1.m11 + m1.m22);
if (ww >= 0) {
if (ww >= EPS2) {
this.x = (float) Math.sqrt(ww);
ww = 0.5 / this.x;
this.y = (float) (m1.m10 * ww);
this.z = (float) (m1.m20 * ww);
return;
}
} else {
this.x = 0;
this.y = 0;
this.z = 1;
return;
}
this.x = 0;
ww = 0.5 * (1.0 - m1.m22);
if (ww >= EPS2) {
this.y = (float) Math.sqrt(ww);
this.z = (float) (m1.m21 / (2.0 * (double) (this.y)));
return;
}
this.y = 0;
this.z = 1;
}
/**
* Sets the value of this quaternion to the equivalent rotation of the
* AxisAngle argument.
*
* @param a
* the AxisAngle to be emulated
*/
public final void set(AxisAngle4f a) {
float mag, amag;
// Quat = cos(theta/2) + sin(theta/2)(roation_axis)
amag = (float) Math.sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
if (amag < EPS) {
w = 0.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
} else {
amag = 1.0f / amag;
mag = (float) Math.sin(a.angle / 2.0);
w = (float) Math.cos(a.angle / 2.0);
x = a.x * amag * mag;
y = a.y * amag * mag;
z = a.z * amag * mag;
}
}
/**
* Sets the value of this quaternion to the equivalent rotation of the
* AxisAngle argument.
*
* @param a
* the AxisAngle to be emulated
*/
public final void set(AxisAngle4d a) {
float mag, amag;
// Quat = cos(theta/2) + sin(theta/2)(roation_axis)
amag = (float) (1.0 / Math.sqrt(a.x * a.x + a.y * a.y + a.z * a.z));
if (amag < EPS) {
w = 0.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
} else {
amag = 1.0f / amag;
mag = (float) Math.sin(a.angle / 2.0);
w = (float) Math.cos(a.angle / 2.0);
x = (float) a.x * amag * mag;
y = (float) a.y * amag * mag;
z = (float) a.z * amag * mag;
}
}
/**
* Performs a great circle interpolation between this quaternion and the
* quaternion parameter and places the result into this quaternion.
*
* @param q1
* the other quaternion
* @param alpha
* the alpha interpolation parameter
*/
public final void interpolate(Quat4f q1, float alpha) {
// From "Advanced Animation and Rendering Techniques"
// by Watt and Watt pg. 364, function as implemented appeared to be
// incorrect. Fails to choose the same quaternion for the double
// covering. Resulting in change of direction for rotations.
// Fixed function to negate the first quaternion in the case that the
// dot product of q1 and this is negative. Second case was not needed.
double dot, s1, s2, om, sinom;
dot = x * q1.x + y * q1.y + z * q1.z + w * q1.w;
if (dot < 0) {
// negate quaternion
q1.x = -q1.x;
q1.y = -q1.y;
q1.z = -q1.z;
q1.w = -q1.w;
dot = -dot;
}
if ((1.0 - dot) > EPS) {
om = Math.acos(dot);
sinom = Math.sin(om);
s1 = Math.sin((1.0 - alpha) * om) / sinom;
s2 = Math.sin(alpha * om) / sinom;
} else {
s1 = 1.0 - alpha;
s2 = alpha;
}
w = (float) (s1 * w + s2 * q1.w);
x = (float) (s1 * x + s2 * q1.x);
y = (float) (s1 * y + s2 * q1.y);
z = (float) (s1 * z + s2 * q1.z);
}
/**
* Performs a great circle interpolation between quaternion q1 and quaternion
* q2 and places the result into this quaternion.
*
* @param q1
* the first quaternion
* @param q2
* the second quaternion
* @param alpha
* the alpha interpolation parameter
*/
public final void interpolate(Quat4f q1, Quat4f q2, float alpha) {
// From "Advanced Animation and Rendering Techniques"
// by Watt and Watt pg. 364, function as implemented appeared to be
// incorrect. Fails to choose the same quaternion for the double
// covering. Resulting in change of direction for rotations.
// Fixed function to negate the first quaternion in the case that the
// dot product of q1 and this is negative. Second case was not needed.
double dot, s1, s2, om, sinom;
dot = q2.x * q1.x + q2.y * q1.y + q2.z * q1.z + q2.w * q1.w;
if (dot < 0) {
// negate quaternion
q1.x = -q1.x;
q1.y = -q1.y;
q1.z = -q1.z;
q1.w = -q1.w;
dot = -dot;
}
if ((1.0 - dot) > EPS) {
om = Math.acos(dot);
sinom = Math.sin(om);
s1 = Math.sin((1.0 - alpha) * om) / sinom;
s2 = Math.sin(alpha * om) / sinom;
} else {
s1 = 1.0 - alpha;
s2 = alpha;
}
w = (float) (s1 * q1.w + s2 * q2.w);
x = (float) (s1 * q1.x + s2 * q2.x);
y = (float) (s1 * q1.y + s2 * q2.y);
z = (float) (s1 * q1.z + s2 * q2.z);
}
}