/*
* Copyright (c) 2003-2009 jMonkeyEngine
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are
* met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of 'jMonkeyEngine' nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
* TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.mt4j.util.math;
import java.io.Externalizable;
import java.io.IOException;
import java.io.ObjectInput;
import java.io.ObjectOutput;
import java.util.logging.Logger;
/**
* <code>Quaternion</code> defines a single example of a more general class of
* hypercomplex numbers. Quaternions extends a rotation in three dimensions to a
* rotation in four dimensions. This avoids "gimbal lock" and allows for smooth
* continuous rotation.
*
* <code>Quaternion</code> is defined by four floating point numbers: {x y z
* w}.
* <br>Copyright (c) by JMonkeyEngine.
*
* @author Mark Powell
* @author Joshua Slack
*/
public class Quaternion implements Externalizable, Cloneable {
/** The Constant logger. */
private static final Logger logger = Logger.getLogger(Quaternion.class.getName());
/** The Constant serialVersionUID. */
private static final long serialVersionUID = 1L;
/** The w. */
public float x, y, z, w;
/**
* Constructor instantiates a new <code>Quaternion</code> object
* initializing all values to zero, except w which is initialized to 1.
*/
public Quaternion() {
x = 0;
y = 0;
z = 0;
w = 1;
}
/**
* Constructor instantiates a new <code>Quaternion</code> object from the
* given list of parameters.
*
* @param x the x value of the quaternion.
* @param y the y value of the quaternion.
* @param z the z value of the quaternion.
* @param w the w value of the quaternion.
*/
public Quaternion(float x, float y, float z, float w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
/**
* sets the data in a <code>Quaternion</code> object from the given list
* of parameters.
*
* @param x the x value of the quaternion.
* @param y the y value of the quaternion.
* @param z the z value of the quaternion.
* @param w the w value of the quaternion.
*/
public void set(float x, float y, float z, float w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
/**
* Sets the data in this <code>Quaternion</code> object to be equal to the
* passed <code>Quaternion</code> object. The values are copied producing
* a new object.
*
* @param q The Quaternion to copy values from.
*
* @return this for chaining
*/
public Quaternion set(Quaternion q) {
this.x = q.x;
this.y = q.y;
this.z = q.z;
this.w = q.w;
return this;
}
/**
* Constructor instantiates a new <code>Quaternion</code> object from a
* collection of rotation angles.
*
* @param angles the angles of rotation (x, y, z) that will define the
* <code>Quaternion</code>.
*/
public Quaternion(float[] angles) {
fromAngles(angles);
}
/**
* Constructor instantiates a new <code>Quaternion</code> object from an
* interpolation between two other quaternions.
*
* @param q1 the first quaternion.
* @param q2 the second quaternion.
* @param interp the amount to interpolate between the two quaternions.
*/
public Quaternion(Quaternion q1, Quaternion q2, float interp) {
slerp(q1, q2, interp);
}
/**
* Constructor instantiates a new <code>Quaternion</code> object from an
* existing quaternion, creating a copy.
*
* @param q the quaternion to copy.
*/
public Quaternion(Quaternion q) {
this.x = q.x;
this.y = q.y;
this.z = q.z;
this.w = q.w;
}
/**
* Sets this Quaternion to {0, 0, 0, 1}. Same as calling set(0,0,0,1).
*/
public void loadIdentity() {
x = y = z = 0;
w = 1;
}
/**
* Checks if is identity.
*
* @return true if this Quaternion is {0,0,0,1}
*/
public boolean isIdentity() {
if (x == 0 && y == 0 && z == 0 && w == 1)
return true;
else
return false;
}
/**
* <code>fromAngles</code> builds a quaternion from the Euler rotation
* angles (y,r,p).
*
* @param angles the Euler angles of rotation (in radians).
*/
public void fromAngles(float[] angles) {
if (angles.length != 3)
throw new IllegalArgumentException(
"Angles array must have three elements");
fromAngles(angles[0], angles[1], angles[2]);
}
/**
* <code>fromAngles</code> builds a Quaternion from the Euler rotation
* angles (y,r,p). Note that we are applying in order: roll, pitch, yaw but
* we've ordered them in x, y, and z for convenience.
* See: http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
*
* @param yaw the Euler yaw of rotation (in radians). (aka Bank, often rot
* around x)
* @param roll the Euler roll of rotation (in radians). (aka Heading, often
* rot around y)
* @param pitch the Euler pitch of rotation (in radians). (aka Attitude, often
* rot around z)
*
* @return the quaternion
*/
public Quaternion fromAngles(float yaw, float roll, float pitch) {
float angle;
float sinRoll, sinPitch, sinYaw, cosRoll, cosPitch, cosYaw;
angle = pitch * 0.5f;
sinPitch = ToolsMath.sin(angle);
cosPitch = ToolsMath.cos(angle);
angle = roll * 0.5f;
sinRoll = ToolsMath.sin(angle);
cosRoll = ToolsMath.cos(angle);
angle = yaw * 0.5f;
sinYaw = ToolsMath.sin(angle);
cosYaw = ToolsMath.cos(angle);
// variables used to reduce multiplication calls.
float cosRollXcosPitch = cosRoll * cosPitch;
float sinRollXsinPitch = sinRoll * sinPitch;
float cosRollXsinPitch = cosRoll * sinPitch;
float sinRollXcosPitch = sinRoll * cosPitch;
w = (cosRollXcosPitch * cosYaw - sinRollXsinPitch * sinYaw);
x = (cosRollXcosPitch * sinYaw + sinRollXsinPitch * cosYaw);
y = (sinRollXcosPitch * cosYaw + cosRollXsinPitch * sinYaw);
z = (cosRollXsinPitch * cosYaw - sinRollXcosPitch * sinYaw);
normalize();
return this;
}
/**
* <code>toAngles</code> returns this quaternion converted to Euler
* rotation angles (yaw,roll,pitch).<br/>
* See http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
*
* @param angles the float[] in which the angles should be stored, or null if
* you want a new float[] to be created
*
* @return the float[] in which the angles are stored.
*/
public float[] toAngles(float[] angles) {
if (angles == null)
angles = new float[3];
else if (angles.length != 3)
throw new IllegalArgumentException("Angles array must have three elements");
float sqw = w * w;
float sqx = x * x;
float sqy = y * y;
float sqz = z * z;
float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise
// is correction factor
float test = x * y + z * w;
if (test > 0.499 * unit) { // singularity at north pole
angles[1] = 2 * ToolsMath.atan2(x, w);
angles[2] = ToolsMath.HALF_PI;
angles[0] = 0;
} else if (test < -0.499 * unit) { // singularity at south pole
angles[1] = -2 * ToolsMath.atan2(x, w);
angles[2] = -ToolsMath.HALF_PI;
angles[0] = 0;
} else {
angles[1] = ToolsMath.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // roll or heading
angles[2] = ToolsMath.asin(2 * test / unit); // pitch or attitude
angles[0] = ToolsMath.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // yaw or bank
}
return angles;
}
/**
* <code>fromRotationMatrix</code> generates a quaternion from a supplied
* matrix. This matrix is assumed to be a rotational matrix.
*
* @param matrix the matrix that defines the rotation.
*
* @return the quaternion
*/
public Quaternion fromRotationMatrix(Matrix matrix) {
return fromRotationMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10,
matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22);
}
/**
* From rotation matrix.
*
* @param m00 the m00
* @param m01 the m01
* @param m02 the m02
* @param m10 the m10
* @param m11 the m11
* @param m12 the m12
* @param m20 the m20
* @param m21 the m21
* @param m22 the m22
*
* @return the quaternion
*/
public Quaternion fromRotationMatrix(float m00, float m01, float m02,
float m10, float m11, float m12,
float m20, float m21, float m22) {
// Use the Graphics Gems code, from
// ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
// *NOT* the "Matrix and Quaternions FAQ", which has errors!
// the trace is the sum of the diagonal elements; see
// http://mathworld.wolfram.com/MatrixTrace.html
float t = m00 + m11 + m22;
// we protect the division by s by ensuring that s>=1
if (t >= 0) { // |w| >= .5
float s = ToolsMath.sqrt(t+1); // |s|>=1 ...
w = 0.5f * s;
s = 0.5f / s; // so this division isn't bad
x = (m21 - m12) * s;
y = (m02 - m20) * s;
z = (m10 - m01) * s;
} else if ((m00 > m11) && (m00 > m22)) {
float s = ToolsMath
.sqrt(1.0f + m00 - m11 - m22); // |s|>=1
x = s * 0.5f; // |x| >= .5
s = 0.5f / s;
y = (m10 + m01) * s;
z = (m02 + m20) * s;
w = (m21 - m12) * s;
} else if (m11 > m22) {
float s = ToolsMath
.sqrt(1.0f + m11 - m00 - m22); // |s|>=1
y = s * 0.5f; // |y| >= .5
s = 0.5f / s;
x = (m10 + m01) * s;
z = (m21 + m12) * s;
w = (m02 - m20) * s;
} else {
float s = ToolsMath
.sqrt(1.0f + m22 - m00 - m11); // |s|>=1
z = s * 0.5f; // |z| >= .5
s = 0.5f / s;
x = (m02 + m20) * s;
y = (m21 + m12) * s;
w = (m10 - m01) * s;
}
return this;
}
/**
* <code>toRotationMatrix</code> converts this quaternion to a rotational
* matrix. Note: the result is created from a normalized version of this quat.
*
* @return the rotation matrix representation of this quaternion.
*/
public Matrix toRotationMatrix() {
Matrix matrix = new Matrix();
return toRotationMatrix(matrix);
}
// /**
// * <code>toRotationMatrix</code> converts this quaternion to a rotational
// * matrix. The result is stored in result.
// *
// * @param result
// * The Matrix3f to store the result in.
// * @return the rotation matrix representation of this quaternion.
// */
// public Matrix3f toRotationMatrix(Matrix3f result) {
//
// float norm = norm();
// // we explicitly test norm against one here, saving a division
// // at the cost of a test and branch. Is it worth it?
// float s = (norm==1f) ? 2f : (norm > 0f) ? 2f/norm : 0;
//
// // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
// // will be used 2-4 times each.
// float xs = x * s;
// float ys = y * s;
// float zs = z * s;
// float xx = x * xs;
// float xy = x * ys;
// float xz = x * zs;
// float xw = w * xs;
// float yy = y * ys;
// float yz = y * zs;
// float yw = w * ys;
// float zz = z * zs;
// float zw = w * zs;
//
// // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
// result.m00 = 1 - ( yy + zz );
// result.m01 = ( xy - zw );
// result.m02 = ( xz + yw );
// result.m10 = ( xy + zw );
// result.m11 = 1 - ( xx + zz );
// result.m12 = ( yz - xw );
// result.m20 = ( xz - yw );
// result.m21 = ( yz + xw );
// result.m22 = 1 - ( xx + yy );
//
// return result;
// }
/**
* <code>toRotationMatrix</code> converts this quaternion to a rotational
* matrix. The result is stored in result. 4th row and 4th column values are
* untouched. Note: the result is created from a normalized version of this quat.
*
* @param result The Matrix4f to store the result in.
*
* @return the rotation matrix representation of this quaternion.
*/
public Matrix toRotationMatrix(Matrix result) {
float norm = norm();
// we explicitly test norm against one here, saving a division
// at the cost of a test and branch. Is it worth it?
float s = (norm==1f) ? 2f : (norm > 0f) ? 2f/norm : 0;
// compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
// will be used 2-4 times each.
float xs = x * s;
float ys = y * s;
float zs = z * s;
float xx = x * xs;
float xy = x * ys;
float xz = x * zs;
float xw = w * xs;
float yy = y * ys;
float yz = y * zs;
float yw = w * ys;
float zz = z * zs;
float zw = w * zs;
// using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
result.m00 = 1 - ( yy + zz );
result.m01 = ( xy - zw );
result.m02 = ( xz + yw );
result.m10 = ( xy + zw );
result.m11 = 1 - ( xx + zz );
result.m12 = ( yz - xw );
result.m20 = ( xz - yw );
result.m21 = ( yz + xw );
result.m22 = 1 - ( xx + yy );
return result;
}
/**
* <code>getRotationColumn</code> returns one of three columns specified
* by the parameter. This column is returned as a <code>Vector3f</code>
* object.
*
* @param i the column to retrieve. Must be between 0 and 2.
*
* @return the column specified by the index.
*
* @throws Exception the exception
*/
public Vector3D getRotationColumn(int i) throws Exception {
return getRotationColumn(i, null);
}
/**
* <code>getRotationColumn</code> returns one of three columns specified
* by the parameter. This column is returned as a <code>Vector3D</code>
* object. The value is retrieved as if this quaternion was first normalized.
*
* @param i the column to retrieve. Must be between 0 and 2.
* @param store the vector object to store the result in. if null, a new one
* is created.
*
* @return the column specified by the index.
*
* @throws Exception the exception
*/
public Vector3D getRotationColumn(int i, Vector3D store) throws Exception {
if (store == null)
store = new Vector3D();
float norm = norm();
if (norm != 1.0f) {
norm = ToolsMath.invSqrt(norm);
}
float xx = x * x * norm;
float xy = x * y * norm;
float xz = x * z * norm;
float xw = x * w * norm;
float yy = y * y * norm;
float yz = y * z * norm;
float yw = y * w * norm;
float zz = z * z * norm;
float zw = z * w * norm;
switch (i) {
case 0:
store.x = 1 - 2 * ( yy + zz );
store.y = 2 * ( xy + zw );
store.z = 2 * ( xz - yw );
break;
case 1:
store.x = 2 * ( xy - zw );
store.y = 1 - 2 * ( xx + zz );
store.z = 2 * ( yz + xw );
break;
case 2:
store.x = 2 * ( xz + yw );
store.y = 2 * ( yz - xw );
store.z = 1 - 2 * ( xx + yy );
break;
default:
logger.warning("Invalid column index.");
throw new Exception("Invalid column index. " + i);
}
return store;
}
/**
* <code>fromAngleAxis</code> sets this quaternion to the values specified
* by an angle and an axis of rotation. This method creates an object, so
* use fromAngleNormalAxis if your axis is already normalized.
*
* @param angle the angle to rotate (in radians).
* @param axis the axis of rotation.
*
* @return this quaternion
*/
public Quaternion fromAngleAxis(float angle, Vector3D axis) {
axis.normalizeLocal();
fromAngleNormalAxis(angle, axis);
return this;
}
/**
* <code>fromAngleNormalAxis</code> sets this quaternion to the values
* specified by an angle and a normalized axis of rotation.
*
* @param angle the angle to rotate (in radians).
* @param axis the axis of rotation (already normalized).
*
* @return the quaternion
*/
public Quaternion fromAngleNormalAxis(float angle, Vector3D axis) {
if (axis.x == 0 && axis.y == 0 && axis.z == 0) {
loadIdentity();
} else {
float halfAngle = 0.5f * angle;
float sin = ToolsMath.sin(halfAngle);
w = ToolsMath.cos(halfAngle);
x = sin * axis.x;
y = sin * axis.y;
z = sin * axis.z;
}
return this;
}
/**
* <code>toAngleAxis</code> sets a given angle and axis to that
* represented by the current quaternion. The values are stored as
* following: The axis is provided as a parameter and built by the method,
* the angle is returned as a float.
*
* @param axisStore the object we'll store the computed axis in.
*
* @return the angle of rotation in radians.
*/
public float toAngleAxis(Vector3D axisStore) {
float sqrLength = x * x + y * y + z * z;
float angle;
if (sqrLength == 0.0f) {
angle = 0.0f;
if (axisStore != null) {
axisStore.x = 1.0f;
axisStore.y = 0.0f;
axisStore.z = 0.0f;
}
} else {
angle = (2.0f * ToolsMath.acos(w));
if (axisStore != null) {
float invLength = (1.0f / ToolsMath.sqrt(sqrLength));
axisStore.x = x * invLength;
axisStore.y = y * invLength;
axisStore.z = z * invLength;
}
}
return angle;
}
/**
* <code>slerp</code> sets this quaternion's value as an interpolation
* between two other quaternions.
*
* @param q1 the first quaternion.
* @param q2 the second quaternion.
* @param t the amount to interpolate between the two quaternions.
*
* @return the quaternion
*/
public Quaternion slerp(Quaternion q1, Quaternion q2, float t) {
// Create a local quaternion to store the interpolated quaternion
if (q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w) {
this.set(q1);
return this;
}
float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z)
+ (q1.w * q2.w);
if (result < 0.0f) {
// Negate the second quaternion and the result of the dot product
q2.x = -q2.x;
q2.y = -q2.y;
q2.z = -q2.z;
q2.w = -q2.w;
result = -result;
}
// Set the first and second scale for the interpolation
float scale0 = 1 - t;
float scale1 = t;
// Check if the angle between the 2 quaternions was big enough to
// warrant such calculations
if ((1 - result) > 0.1f) {// Get the angle between the 2 quaternions,
// and then store the sin() of that angle
float theta = ToolsMath.acos(result);
float invSinTheta = 1f / ToolsMath.sin(theta);
// Calculate the scale for q1 and q2, according to the angle and
// it's sine value
scale0 = ToolsMath.sin((1 - t) * theta) * invSinTheta;
scale1 = ToolsMath.sin((t * theta)) * invSinTheta;
}
// Calculate the x, y, z and w values for the quaternion by using a
// special
// form of linear interpolation for quaternions.
this.x = (scale0 * q1.x) + (scale1 * q2.x);
this.y = (scale0 * q1.y) + (scale1 * q2.y);
this.z = (scale0 * q1.z) + (scale1 * q2.z);
this.w = (scale0 * q1.w) + (scale1 * q2.w);
// Return the interpolated quaternion
return this;
}
/**
* Sets the values of this quaternion to the slerp from itself to q2 by
* changeAmnt.
*
* @param q2 Final interpolation value
* @param changeAmnt The amount diffrence
*/
public void slerp(Quaternion q2, float changeAmnt) {
if (this.x == q2.x && this.y == q2.y && this.z == q2.z
&& this.w == q2.w) {
return;
}
float result = (this.x * q2.x) + (this.y * q2.y) + (this.z * q2.z)
+ (this.w * q2.w);
if (result < 0.0f) {
// Negate the second quaternion and the result of the dot product
q2.x = -q2.x;
q2.y = -q2.y;
q2.z = -q2.z;
q2.w = -q2.w;
result = -result;
}
// Set the first and second scale for the interpolation
float scale0 = 1 - changeAmnt;
float scale1 = changeAmnt;
// Check if the angle between the 2 quaternions was big enough to
// warrant such calculations
if ((1 - result) > 0.1f) {
// Get the angle between the 2 quaternions, and then store the sin()
// of that angle
float theta = ToolsMath.acos(result);
float invSinTheta = 1f / ToolsMath.sin(theta);
// Calculate the scale for q1 and q2, according to the angle and
// it's sine value
scale0 = ToolsMath.sin((1 - changeAmnt) * theta) * invSinTheta;
scale1 = ToolsMath.sin((changeAmnt * theta)) * invSinTheta;
}
// Calculate the x, y, z and w values for the quaternion by using a
// special
// form of linear interpolation for quaternions.
this.x = (scale0 * this.x) + (scale1 * q2.x);
this.y = (scale0 * this.y) + (scale1 * q2.y);
this.z = (scale0 * this.z) + (scale1 * q2.z);
this.w = (scale0 * this.w) + (scale1 * q2.w);
}
/**
* <code>add</code> adds the values of this quaternion to those of the
* parameter quaternion. The result is returned as a new quaternion.
*
* @param q the quaternion to add to this.
*
* @return the new quaternion.
*/
public Quaternion add(Quaternion q) {
return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w);
}
/**
* <code>add</code> adds the values of this quaternion to those of the
* parameter quaternion. The result is stored in this Quaternion.
*
* @param q the quaternion to add to this.
*
* @return This Quaternion after addition.
*/
public Quaternion addLocal(Quaternion q) {
this.x += q.x;
this.y += q.y;
this.z += q.z;
this.w += q.w;
return this;
}
/**
* <code>subtract</code> subtracts the values of the parameter quaternion
* from those of this quaternion. The result is returned as a new
* quaternion.
*
* @param q the quaternion to subtract from this.
*
* @return the new quaternion.
*/
public Quaternion subtract(Quaternion q) {
return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w);
}
/**
* <code>subtract</code> subtracts the values of the parameter quaternion
* from those of this quaternion. The result is stored in this Quaternion.
*
* @param q the quaternion to subtract from this.
*
* @return This Quaternion after subtraction.
*/
public Quaternion subtractLocal(Quaternion q) {
this.x -= q.x;
this.y -= q.y;
this.z -= q.z;
this.w -= q.w;
return this;
}
/**
* <code>mult</code> multiplies this quaternion by a parameter quaternion.
* The result is returned as a new quaternion. It should be noted that
* quaternion multiplication is not cummulative so q * p != p * q.
*
* @param q the quaternion to multiply this quaternion by.
*
* @return the new quaternion.
*/
public Quaternion mult(Quaternion q) {
return mult(q, null);
}
/**
* <code>mult</code> multiplies this quaternion by a parameter quaternion.
* The result is returned as a new quaternion. It should be noted that
* quaternion multiplication is not cummulative so q * p != p * q.
*
* It IS safe for q and res to be the same object.
*
* @param q the quaternion to multiply this quaternion by.
* @param res the quaternion to store the result in.
*
* @return the new quaternion.
*/
public Quaternion mult(Quaternion q, Quaternion res) {
if (res == null)
res = new Quaternion();
float qw = q.w, qx = q.x, qy = q.y, qz = q.z;
res.x = x * qw + y * qz - z * qy + w * qx;
res.y = -x * qz + y * qw + z * qx + w * qy;
res.z = x * qy - y * qx + z * qw + w * qz;
res.w = -x * qx - y * qy - z * qz + w * qw;
return res;
}
/**
* <code>apply</code> multiplies this quaternion by a parameter matrix
* internally.
*
* @param matrix the matrix to apply to this quaternion.
*/
public void apply(Matrix matrix) {
float oldX = x, oldY = y, oldZ = z, oldW = w;
fromRotationMatrix(matrix);
float tempX = x, tempY = y, tempZ = z, tempW = w;
x = oldX * tempW + oldY * tempZ - oldZ * tempY + oldW * tempX;
y = -oldX * tempZ + oldY * tempW + oldZ * tempX + oldW * tempY;
z = oldX * tempY - oldY * tempX + oldZ * tempW + oldW * tempZ;
w = -oldX * tempX - oldY * tempY - oldZ * tempZ + oldW * tempW;
}
/**
* <code>fromAxes</code> creates a <code>Quaternion</code> that
* represents the coordinate system defined by three axes. These axes are
* assumed to be orthogonal and no error checking is applied. Thus, the user
* must insure that the three axes being provided indeed represents a proper
* right handed coordinate system.
*
* @param axis the array containing the three vectors representing the
* coordinate system.
*
* @return the quaternion
*/
public Quaternion fromAxes(Vector3D[] axis) {
if (axis.length != 3)
throw new IllegalArgumentException(
"Axis array must have three elements");
return fromAxes(axis[0], axis[1], axis[2]);
}
/**
* <code>fromAxes</code> creates a <code>Quaternion</code> that
* represents the coordinate system defined by three axes. These axes are
* assumed to be orthogonal and no error checking is applied. Thus, the user
* must insure that the three axes being provided indeed represents a proper
* right handed coordinate system.
*
* @param xAxis vector representing the x-axis of the coordinate system.
* @param yAxis vector representing the y-axis of the coordinate system.
* @param zAxis vector representing the z-axis of the coordinate system.
*
* @return the quaternion
*/
public Quaternion fromAxes(Vector3D xAxis, Vector3D yAxis, Vector3D zAxis) {
return fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y,
zAxis.y, xAxis.z, yAxis.z, zAxis.z);
}
/**
* <code>toAxes</code> takes in an array of three vectors. Each vector
* corresponds to an axis of the coordinate system defined by the quaternion
* rotation.
*
* @param axis the array of vectors to be filled.
*/
public void toAxes(Vector3D axis[]) {
Matrix tempMat = toRotationMatrix();
float[] a;
try {
a = tempMat.getColumn(0, null);
axis[0] = new Vector3D(a[0],a[1],a[2]);
float[] b = tempMat.getColumn(1, null);
axis[1] = new Vector3D(b[0],b[1],b[2]);
float[] c = tempMat.getColumn(2, null);
axis[2] = new Vector3D(c[0],c[1],c[2]);
} catch (Exception e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
// axis[0] = tempMat.getColumn(0, axis[0]);
// axis[1] = tempMat.getColumn(1, axis[1]);
// axis[2] = tempMat.getColumn(2, axis[2]);
}
/**
* <code>mult</code> multiplies this quaternion by a parameter vector. The
* result is returned as a new vector.
*
* @param v the vector to multiply this quaternion by.
*
* @return the new vector.
*/
public Vector3D mult(Vector3D v) {
return mult(v, null);
}
/**
* <code>mult</code> multiplies this quaternion by a parameter vector. The
* result is stored in the supplied vector
*
* @param v the vector to multiply this quaternion by.
*
* @return v
*/
public Vector3D multLocal(Vector3D v) {
float tempX, tempY;
tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x
+ 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x;
tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z
* v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x
* v.y;
v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x
- y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z;
v.x = tempX;
v.y = tempY;
return v;
}
/**
* Multiplies this Quaternion by the supplied quaternion. The result is
* stored in this Quaternion, which is also returned for chaining. Similar
* to this *= q.
*
* @param q The Quaternion to multiply this one by.
*
* @return This Quaternion, after multiplication.
*/
public Quaternion multLocal(Quaternion q) {
float x1 = x * q.w + y * q.z - z * q.y + w * q.x;
float y1 = -x * q.z + y * q.w + z * q.x + w * q.y;
float z1 = x * q.y - y * q.x + z * q.w + w * q.z;
w = -x * q.x - y * q.y - z * q.z + w * q.w;
x = x1;
y = y1;
z = z1;
return this;
}
/**
* Multiplies this Quaternion by the supplied quaternion. The result is
* stored in this Quaternion, which is also returned for chaining. Similar
* to this *= q.
*
* @param qx -
* quat x value
* @param qy -
* quat y value
* @param qz -
* quat z value
* @param qw -
* quat w value
*
* @return This Quaternion, after multiplication.
*/
public Quaternion multLocal(float qx, float qy, float qz, float qw) {
float x1 = x * qw + y * qz - z * qy + w * qx;
float y1 = -x * qz + y * qw + z * qx + w * qy;
float z1 = x * qy - y * qx + z * qw + w * qz;
w = -x * qx - y * qy - z * qz + w * qw;
x = x1;
y = y1;
z = z1;
return this;
}
/**
* <code>mult</code> multiplies this quaternion by a parameter vector. The
* result is returned as a new vector.
*
* @param v the vector to multiply this quaternion by.
* @param store the vector to store the result in. It IS safe for v and store
* to be the same object.
*
* @return the result vector.
*/
public Vector3D mult(Vector3D v, Vector3D store) {
if (store == null)
store = new Vector3D();
if (v.x == 0 && v.y == 0 && v.z == 0) {
store.setXYZ(0, 0, 0);
} else {
float vx = v.x, vy = v.y, vz = v.z;
store.x = w * w * vx + 2 * y * w * vz - 2 * z * w * vy + x * x
* vx + 2 * y * x * vy + 2 * z * x * vz - z * z * vx - y
* y * vx;
store.y = 2 * x * y * vx + y * y * vy + 2 * z * y * vz + 2 * w
* z * vx - z * z * vy + w * w * vy - 2 * x * w * vz - x
* x * vy;
store.z = 2 * x * z * vx + 2 * y * z * vy + z * z * vz - 2 * w
* y * vx - y * y * vz + 2 * w * x * vy - x * x * vz + w
* w * vz;
}
return store;
}
/**
* <code>mult</code> multiplies this quaternion by a parameter scalar. The
* result is returned as a new quaternion.
*
* @param scalar the quaternion to multiply this quaternion by.
*
* @return the new quaternion.
*/
public Quaternion mult(float scalar) {
return new Quaternion(scalar * x, scalar * y, scalar * z, scalar * w);
}
/**
* <code>mult</code> multiplies this quaternion by a parameter scalar. The
* result is stored locally.
*
* @param scalar the quaternion to multiply this quaternion by.
*
* @return this.
*/
public Quaternion multLocal(float scalar) {
w *= scalar;
x *= scalar;
y *= scalar;
z *= scalar;
return this;
}
/**
* <code>dot</code> calculates and returns the dot product of this
* quaternion with that of the parameter quaternion.
*
* @param q the quaternion to calculate the dot product of.
*
* @return the dot product of this and the parameter quaternion.
*/
public float dot(Quaternion q) {
return w * q.w + x * q.x + y * q.y + z * q.z;
}
/**
* <code>norm</code> returns the norm of this quaternion. This is the dot
* product of this quaternion with itself.
*
* @return the norm of the quaternion.
*/
public float norm() {
return w * w + x * x + y * y + z * z;
}
/**
* <code>normalize</code> normalizes the current <code>Quaternion</code>.
*/
public void normalize() {
float n = ToolsMath.invSqrt(norm());
x *= n;
y *= n;
z *= n;
w *= n;
}
/**
* <code>inverse</code> returns the inverse of this quaternion as a new
* quaternion. If this quaternion does not have an inverse (if its normal is
* 0 or less), then null is returned.
*
* @return the inverse of this quaternion or null if the inverse does not
* exist.
*/
public Quaternion inverse() {
float norm = norm();
if (norm > 0.0) {
float invNorm = 1.0f / norm;
return new Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w
* invNorm);
}
// return an invalid result to flag the error
return null;
}
/**
* <code>inverse</code> calculates the inverse of this quaternion and
* returns this quaternion after it is calculated. If this quaternion does
* not have an inverse (if it's norma is 0 or less), nothing happens
*
* @return the inverse of this quaternion
*/
public Quaternion inverseLocal() {
float norm = norm();
if (norm > 0.0) {
float invNorm = 1.0f / norm;
x *= -invNorm;
y *= -invNorm;
z *= -invNorm;
w *= invNorm;
}
return this;
}
/**
* <code>negate</code> inverts the values of the quaternion.
*/
public void negate() {
x *= -1;
y *= -1;
z *= -1;
w *= -1;
}
/**
* <code>toString</code> creates the string representation of this
* <code>Quaternion</code>. The values of the quaternion are displace (x,
* y, z, w), in the following manner: <br>
* com.jme.math.Quaternion: [x=1" y=2 z=3 w=1]
*
* @return the string representation of this object.
*
* @see java.lang.Object#toString()
*/
public String toString() {
return "com.jme.math.Quaternion: [x=" + x + " y=" + y + " z=" + z
+ " w=" + w + "]";
}
/**
* <code>equals</code> determines if two quaternions are logically equal,
* that is, if the values of (x, y, z, w) are the same for both quaternions.
*
* @param o the object to compare for equality
*
* @return true if they are equal, false otherwise.
*/
public boolean equals(Object o) {
if (!(o instanceof Quaternion) ) {
return false;
}
if (this == o) {
return true;
}
Quaternion comp = (Quaternion) o;
if (Float.compare(x,comp.x) != 0) return false;
if (Float.compare(y,comp.y) != 0) return false;
if (Float.compare(z,comp.z) != 0) return false;
if (Float.compare(w,comp.w) != 0) return false;
return true;
}
/**
* <code>hashCode</code> returns the hash code value as an integer and is
* supported for the benefit of hashing based collection classes such as
* Hashtable, HashMap, HashSet etc.
*
* @return the hashcode for this instance of Quaternion.
*
* @see java.lang.Object#hashCode()
*/
public int hashCode() {
int hash = 37;
hash = 37 * hash + Float.floatToIntBits(x);
hash = 37 * hash + Float.floatToIntBits(y);
hash = 37 * hash + Float.floatToIntBits(z);
hash = 37 * hash + Float.floatToIntBits(w);
return hash;
}
/**
* <code>readExternal</code> builds a quaternion from an
* <code>ObjectInput</code> object. <br>
* NOTE: Used with serialization. Not to be called manually.
*
* @param in the ObjectInput value to read from.
*
* @throws IOException if the ObjectInput value has problems reading a float.
*
* @see java.io.Externalizable
*/
public void readExternal(ObjectInput in) throws IOException {
x = in.readFloat();
y = in.readFloat();
z = in.readFloat();
w = in.readFloat();
}
/**
* <code>writeExternal</code> writes this quaternion out to a
* <code>ObjectOutput</code> object. NOTE: Used with serialization. Not to
* be called manually.
*
* @param out the object to write to.
*
* @throws IOException if writing to the ObjectOutput fails.
*
* @see java.io.Externalizable
*/
public void writeExternal(ObjectOutput out) throws IOException {
out.writeFloat(x);
out.writeFloat(y);
out.writeFloat(z);
out.writeFloat(w);
}
/** The Constant tmpYaxis. */
private static final Vector3D tmpYaxis = new Vector3D();
/** The Constant tmpZaxis. */
private static final Vector3D tmpZaxis = new Vector3D();
/** The Constant tmpXaxis. */
private static final Vector3D tmpXaxis = new Vector3D();
/**
* <code>lookAt</code> is a convienence method for auto-setting the
* quaternion based on a direction and an up vector. It computes
* the rotation to transform the z-axis to point into 'direction'
* and the y-axis to 'up'.
*
* @param direction where to look at in terms of local coordinates
* @param up a vector indicating the local up direction.
* (typically {0, 1, 0} in jME.)
*/
public void lookAt(Vector3D direction, Vector3D up ) {
tmpZaxis.setXYZ(direction.x, direction.y, direction.z);
tmpZaxis.normalizeLocal();
// tmpZaxis.set( direction ).normalizeLocal();
tmpXaxis.setXYZ(up.x, up.y, up.z);
tmpXaxis.crossLocal(direction);
tmpXaxis.normalizeLocal();
// tmpXaxis.set( up ).crossLocal( direction ).normalizeLocal();
tmpYaxis.setXYZ(direction.x, direction.y, direction.z);
tmpYaxis.crossLocal( tmpXaxis );
tmpYaxis.normalizeLocal();
// tmpYaxis.set( direction ).crossLocal( tmpXaxis ).normalizeLocal();
fromAxes( tmpXaxis, tmpYaxis, tmpZaxis );
}
// public void write(JMEExporter e) throws IOException {
// OutputCapsule cap = e.getCapsule(this);
// cap.write(x, "x", 0);
// cap.write(y, "y", 0);
// cap.write(z, "z", 0);
// cap.write(w, "w", 1);
// }
//
// public void read(JMEImporter e) throws IOException {
// InputCapsule cap = e.getCapsule(this);
// x = cap.readFloat("x", 0);
// y = cap.readFloat("y", 0);
// z = cap.readFloat("z", 0);
// w = cap.readFloat("w", 1);
// }
/**
* Gets the class tag.
*
* @return the class tag
*/
public Class<? extends Quaternion> getClassTag() {
return this.getClass();
}
/**
* Opposite.
*
* @return A new quaternion that describes a rotation that would point you
* in the exact opposite direction of this Quaternion.
*/
public Quaternion opposite() {
return opposite(null);
}
/**
* FIXME: This seems to have singularity type issues with angle == 0, possibly others such as PI.
*
* @param store A Quaternion to store our result in. If null, a new one is
* created.
*
* @return The store quaternion (or a new Quaterion, if store is null) that
* describes a rotation that would point you in the exact opposite
* direction of this Quaternion.
*/
public Quaternion opposite(Quaternion store) {
if (store == null)
store = new Quaternion();
Vector3D axis = new Vector3D();
float angle = toAngleAxis(axis);
store.fromAngleAxis(ToolsMath.PI + angle, axis);
return store;
}
/**
* Opposite local.
*
* @return This Quaternion, altered to describe a rotation that would point
* you in the exact opposite direction of where it is pointing
* currently.
*/
public Quaternion oppositeLocal() {
return opposite(this);
}
/* (non-Javadoc)
* @see java.lang.Object#clone()
*/
@Override
public Quaternion clone() {
try {
return (Quaternion) super.clone();
} catch (CloneNotSupportedException e) {
throw new AssertionError(); // can not happen
}
}
}