/**
* Copyright 2010 JogAmp Community. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification, are
* permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this list of
* conditions and the following disclaimer.
*
* 2. 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.
*
* THIS SOFTWARE IS PROVIDED BY JogAmp Community ``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 JogAmp Community 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.
*
* The views and conclusions contained in the software and documentation are those of the
* authors and should not be interpreted as representing official policies, either expressed
* or implied, of JogAmp Community.
*/
package com.jogamp.opengl.math;
public class Quaternion {
protected float x,y,z,w;
public Quaternion(){
}
public Quaternion(float x, float y, float z, float w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
/** Constructor to create a rotation based quaternion from two vectors
* @param vector1
* @param vector2
*/
public Quaternion(float[] vector1, float[] vector2)
{
float theta = (float)FloatUtil.acos(dot(vector1, vector2));
float[] cross = cross(vector1,vector2);
cross = normalizeVec(cross);
this.x = (float)FloatUtil.sin(theta/2)*cross[0];
this.y = (float)FloatUtil.sin(theta/2)*cross[1];
this.z = (float)FloatUtil.sin(theta/2)*cross[2];
this.w = (float)FloatUtil.cos(theta/2);
this.normalize();
}
/** Transform the rotational quaternion to axis based rotation angles
* @return new float[4] with ,theta,Rx,Ry,Rz
*/
public float[] toAxis()
{
float[] vec = new float[4];
float scale = (float)FloatUtil.sqrt(x * x + y * y + z * z);
vec[0] =(float) FloatUtil.acos(w) * 2.0f;
vec[1] = x / scale;
vec[2] = y / scale;
vec[3] = z / scale;
return vec;
}
/** Normalize a vector
* @param vector input vector
* @return normalized vector
*/
private float[] normalizeVec(float[] vector)
{
float[] newVector = new float[3];
float d = FloatUtil.sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
if(d> 0.0f)
{
newVector[0] = vector[0]/d;
newVector[1] = vector[1]/d;
newVector[2] = vector[2]/d;
}
return newVector;
}
/** compute the dot product of two points
* @param vec1 vector 1
* @param vec2 vector 2
* @return the dot product as float
*/
private float dot(float[] vec1, float[] vec2)
{
return (vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]);
}
/** cross product vec1 x vec2
* @param vec1 vector 1
* @param vec2 vecttor 2
* @return the resulting vector
*/
private float[] cross(float[] vec1, float[] vec2)
{
float[] out = new float[3];
out[0] = vec2[2]*vec1[1] - vec2[1]*vec1[2];
out[1] = vec2[0]*vec1[2] - vec2[2]*vec1[0];
out[2] = vec2[1]*vec1[0] - vec2[0]*vec1[1];
return out;
}
public float getW() {
return w;
}
public void setW(float w) {
this.w = w;
}
public float getX() {
return x;
}
public void setX(float x) {
this.x = x;
}
public float getY() {
return y;
}
public void setY(float y) {
this.y = y;
}
public float getZ() {
return z;
}
public void setZ(float z) {
this.z = z;
}
/** Add a quaternion
* @param q quaternion
*/
public void add(Quaternion q)
{
x+=q.x;
y+=q.y;
z+=q.z;
}
/** Subtract a quaternion
* @param q quaternion
*/
public void subtract(Quaternion q)
{
x-=q.x;
y-=q.y;
z-=q.z;
}
/** Divide a quaternion by a constant
* @param n a float to divide by
*/
public void divide(float n)
{
x/=n;
y/=n;
z/=n;
}
/** Multiply this quaternion by
* the param quaternion
* @param q a quaternion to multiply with
*/
public void mult(Quaternion q)
{
float w1 = w*q.w - x*q.x - y*q.y - z*q.z;
float x1 = w*q.x + x*q.w + y*q.z - z*q.y;
float y1 = w*q.y - x*q.z + y*q.w + z*q.x;
float z1 = w*q.z + x*q.y - y*q.x + z*q.w;
w = w1;
x = x1;
y = y1;
z = z1;
}
/** Multiply a quaternion by a constant
* @param n a float constant
*/
public void mult(float n)
{
x*=n;
y*=n;
z*=n;
}
/** Normalize a quaternion required if
* to be used as a rotational quaternion
*/
public void normalize()
{
float norme = (float)FloatUtil.sqrt(w*w + x*x + y*y + z*z);
if (norme == 0.0f)
{
w = 1.0f;
x = y = z = 0.0f;
}
else
{
float recip = 1.0f/norme;
w *= recip;
x *= recip;
y *= recip;
z *= recip;
}
}
/** Invert the quaternion If rotational,
* will produce a the inverse rotation
*/
public void inverse()
{
float norm = w*w + x*x + y*y + z*z;
float recip = 1.0f/norm;
w *= recip;
x = -1*x*recip;
y = -1*y*recip;
z = -1*z*recip;
}
/** Transform this quaternion to a
* 4x4 column matrix representing the rotation
* @return new float[16] column matrix 4x4
*/
public float[] toMatrix()
{
float[] matrix = new float[16];
matrix[0] = 1.0f - 2*y*y - 2*z*z;
matrix[1] = 2*x*y + 2*w*z;
matrix[2] = 2*x*z - 2*w*y;
matrix[3] = 0;
matrix[4] = 2*x*y - 2*w*z;
matrix[5] = 1.0f - 2*x*x - 2*z*z;
matrix[6] = 2*y*z + 2*w*x;
matrix[7] = 0;
matrix[8] = 2*x*z + 2*w*y;
matrix[9] = 2*y*z - 2*w*x;
matrix[10] = 1.0f - 2*x*x - 2*y*y;
matrix[11] = 0;
matrix[12] = 0;
matrix[13] = 0;
matrix[14] = 0;
matrix[15] = 1;
return matrix;
}
/** Set this quaternion from a Sphereical interpolation
* of two param quaternion, used mostly for rotational animation.
* <p>
* Note: Method does not normalize this quaternion!
* </p>
* <p>
* See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
* </p>
* @param a initial quaternion
* @param b target quaternion
* @param t float between 0 and 1 representing interp.
*/
public void slerp(Quaternion a, Quaternion b, float t) {
final float cosom = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
final float t1 = 1.0f - t;
// if the two quaternions are close, just use linear interpolation
if (cosom >= 0.95f) {
x = a.x * t1 + b.x * t;
y = a.y * t1 + b.y * t;
z = a.z * t1 + b.z * t;
w = a.w * t1 + b.w * t;
return;
}
// the quaternions are nearly opposite, we can pick any axis normal to a,b
// to do the rotation
if (cosom <= -0.99f) {
x = 0.5f * (a.x + b.x);
y = 0.5f * (a.y + b.y);
z = 0.5f * (a.z + b.z);
w = 0.5f * (a.w + b.w);
return;
}
// cosom is now withion range of acos, do a SLERP
final float sinom = FloatUtil.sqrt(1.0f - cosom * cosom);
final float omega = FloatUtil.acos(cosom);
final float scla = FloatUtil.sin(t1 * omega) / sinom;
final float sclb = FloatUtil.sin( t * omega) / sinom;
x = a.x * scla + b.x * sclb;
y = a.y * scla + b.y * sclb;
z = a.z * scla + b.z * sclb;
w = a.w * scla + b.w * sclb;
}
/** Check if this quaternion is empty, ie (0,0,0,1)
* @return true if empty, false otherwise
*/
public boolean isEmpty()
{
if (w==1 && x==0 && y==0 && z==0)
return true;
return false;
}
/** Check if this quaternion represents an identity
* matrix, for rotation.
* @return true if it is an identity rep., false otherwise
*/
public boolean isIdentity()
{
if (w==0 && x==0 && y==0 && z==0)
return true;
return false;
}
/** compute the quaternion from a 3x3 column matrix
* @param m 3x3 column matrix
*/
public void setFromMatrix(float[] m) {
float T= m[0] + m[4] + m[8] + 1;
if (T>0){
float S = 0.5f / (float)FloatUtil.sqrt(T);
w = 0.25f / S;
x = ( m[5] - m[7]) * S;
y = ( m[6] - m[2]) * S;
z = ( m[1] - m[3] ) * S;
}
else{
if ((m[0] > m[4])&(m[0] > m[8])) {
float S = FloatUtil.sqrt( 1.0f + m[0] - m[4] - m[8] ) * 2f; // S=4*qx
w = (m[7] - m[5]) / S;
x = 0.25f * S;
y = (m[3] + m[1]) / S;
z = (m[6] + m[2]) / S;
}
else if (m[4] > m[8]) {
float S = FloatUtil.sqrt( 1.0f + m[4] - m[0] - m[8] ) * 2f; // S=4*qy
w = (m[6] - m[2]) / S;
x = (m[3] + m[1]) / S;
y = 0.25f * S;
z = (m[7] + m[5]) / S;
}
else {
float S = FloatUtil.sqrt( 1.0f + m[8] - m[0] - m[4] ) * 2f; // S=4*qz
w = (m[3] - m[1]) / S;
x = (m[6] + m[2]) / S;
y = (m[7] + m[5]) / S;
z = 0.25f * S;
}
}
}
/** Check if the the 3x3 matrix (param) is in fact
* an affine rotational matrix
* @param m 3x3 column matrix
* @return true if representing a rotational matrix, false otherwise
*/
public boolean isRotationMatrix(float[] m) {
double epsilon = 0.01; // margin to allow for rounding errors
if (FloatUtil.abs(m[0]*m[3] + m[3]*m[4] + m[6]*m[7]) > epsilon) return false;
if (FloatUtil.abs(m[0]*m[2] + m[3]*m[5] + m[6]*m[8]) > epsilon) return false;
if (FloatUtil.abs(m[1]*m[2] + m[4]*m[5] + m[7]*m[8]) > epsilon) return false;
if (FloatUtil.abs(m[0]*m[0] + m[3]*m[3] + m[6]*m[6] - 1) > epsilon) return false;
if (FloatUtil.abs(m[1]*m[1] + m[4]*m[4] + m[7]*m[7] - 1) > epsilon) return false;
if (FloatUtil.abs(m[2]*m[2] + m[5]*m[5] + m[8]*m[8] - 1) > epsilon) return false;
return (FloatUtil.abs(determinant(m)-1) < epsilon);
}
private float determinant(float[] m) {
return m[0]*m[4]*m[8] + m[3]*m[7]*m[2] + m[6]*m[1]*m[5] - m[0]*m[7]*m[5] - m[3]*m[1]*m[8] - m[6]*m[4]*m[2];
}
}