531 lines
12 KiB
Java
531 lines
12 KiB
Java
/*
|
|
* Copyright (c) 2002-2008 LWJGL Project
|
|
* 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 'LWJGL' 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.lwjgl.util.vector;
|
|
|
|
/**
|
|
*
|
|
* Quaternions for LWJGL!
|
|
*
|
|
* @author fbi
|
|
* @version $Revision$
|
|
* $Id$
|
|
*/
|
|
|
|
import java.nio.FloatBuffer;
|
|
|
|
public class Quaternion extends Vector implements ReadableVector4f {
|
|
private static final long serialVersionUID = 1L;
|
|
|
|
public float x, y, z, w;
|
|
|
|
/**
|
|
* C'tor. The quaternion will be initialized to the identity.
|
|
*/
|
|
public Quaternion() {
|
|
super();
|
|
setIdentity();
|
|
}
|
|
|
|
/**
|
|
* C'tor
|
|
*
|
|
* @param src
|
|
*/
|
|
public Quaternion(ReadableVector4f src) {
|
|
set(src);
|
|
}
|
|
|
|
/**
|
|
* C'tor
|
|
*
|
|
*/
|
|
public Quaternion(float x, float y, float z, float w) {
|
|
set(x, y, z, w);
|
|
}
|
|
|
|
/*
|
|
* (non-Javadoc)
|
|
*
|
|
* @see org.lwjgl.util.vector.WritableVector2f#set(float, float)
|
|
*/
|
|
public void set(float x, float y) {
|
|
this.x = x;
|
|
this.y = y;
|
|
}
|
|
|
|
/*
|
|
* (non-Javadoc)
|
|
*
|
|
* @see org.lwjgl.util.vector.WritableVector3f#set(float, float, float)
|
|
*/
|
|
public void set(float x, float y, float z) {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
}
|
|
|
|
/*
|
|
* (non-Javadoc)
|
|
*
|
|
* @see org.lwjgl.util.vector.WritableVector4f#set(float, float, float,
|
|
* float)
|
|
*/
|
|
public void set(float x, float y, float z, float w) {
|
|
this.x = x;
|
|
this.y = y;
|
|
this.z = z;
|
|
this.w = w;
|
|
}
|
|
|
|
/**
|
|
* Load from another Vector4f
|
|
*
|
|
* @param src
|
|
* The source vector
|
|
* @return this
|
|
*/
|
|
public Quaternion set(ReadableVector4f src) {
|
|
x = src.getX();
|
|
y = src.getY();
|
|
z = src.getZ();
|
|
w = src.getW();
|
|
return this;
|
|
}
|
|
|
|
/**
|
|
* Set this quaternion to the multiplication identity.
|
|
* @return this
|
|
*/
|
|
public Quaternion setIdentity() {
|
|
return setIdentity(this);
|
|
}
|
|
|
|
/**
|
|
* Set the given quaternion to the multiplication identity.
|
|
* @param q The quaternion
|
|
* @return q
|
|
*/
|
|
public static Quaternion setIdentity(Quaternion q) {
|
|
q.x = 0;
|
|
q.y = 0;
|
|
q.z = 0;
|
|
q.w = 1;
|
|
return q;
|
|
}
|
|
|
|
/**
|
|
* @return the length squared of the quaternion
|
|
*/
|
|
public float lengthSquared() {
|
|
return x * x + y * y + z * z + w * w;
|
|
}
|
|
|
|
/**
|
|
* Normalise the source quaternion and place the result in another quaternion.
|
|
*
|
|
* @param src
|
|
* The source quaternion
|
|
* @param dest
|
|
* The destination quaternion, or null if a new quaternion is to be
|
|
* created
|
|
* @return The normalised quaternion
|
|
*/
|
|
public static Quaternion normalise(Quaternion src, Quaternion dest) {
|
|
float inv_l = 1f/src.length();
|
|
|
|
if (dest == null)
|
|
dest = new Quaternion();
|
|
|
|
dest.set(src.x * inv_l, src.y * inv_l, src.z * inv_l, src.w * inv_l);
|
|
|
|
return dest;
|
|
}
|
|
|
|
/**
|
|
* Normalise this quaternion and place the result in another quaternion.
|
|
*
|
|
* @param dest
|
|
* The destination quaternion, or null if a new quaternion is to be
|
|
* created
|
|
* @return the normalised quaternion
|
|
*/
|
|
public Quaternion normalise(Quaternion dest) {
|
|
return normalise(this, dest);
|
|
}
|
|
|
|
/**
|
|
* The dot product of two quaternions
|
|
*
|
|
* @param left
|
|
* The LHS quat
|
|
* @param right
|
|
* The RHS quat
|
|
* @return left dot right
|
|
*/
|
|
public static float dot(Quaternion left, Quaternion right) {
|
|
return left.x * right.x + left.y * right.y + left.z * right.z + left.w
|
|
* right.w;
|
|
}
|
|
|
|
/**
|
|
* Calculate the conjugate of this quaternion and put it into the given one
|
|
*
|
|
* @param dest
|
|
* The quaternion which should be set to the conjugate of this
|
|
* quaternion
|
|
*/
|
|
public Quaternion negate(Quaternion dest) {
|
|
return negate(this, dest);
|
|
}
|
|
|
|
/**
|
|
* Calculate the conjugate of this quaternion and put it into the given one
|
|
*
|
|
* @param src
|
|
* The source quaternion
|
|
* @param dest
|
|
* The quaternion which should be set to the conjugate of this
|
|
* quaternion
|
|
*/
|
|
public static Quaternion negate(Quaternion src, Quaternion dest) {
|
|
if (dest == null)
|
|
dest = new Quaternion();
|
|
|
|
dest.x = -src.x;
|
|
dest.y = -src.y;
|
|
dest.z = -src.z;
|
|
dest.w = src.w;
|
|
|
|
return dest;
|
|
}
|
|
|
|
/**
|
|
* Calculate the conjugate of this quaternion
|
|
*/
|
|
public Vector negate() {
|
|
return negate(this, this);
|
|
}
|
|
|
|
/* (non-Javadoc)
|
|
* @see org.lwjgl.util.vector.Vector#load(java.nio.FloatBuffer)
|
|
*/
|
|
public Vector load(FloatBuffer buf) {
|
|
x = buf.get();
|
|
y = buf.get();
|
|
z = buf.get();
|
|
w = buf.get();
|
|
return this;
|
|
}
|
|
|
|
/*
|
|
* (non-Javadoc)
|
|
*
|
|
* @see org.lwjgl.vector.Vector#scale(float)
|
|
*/
|
|
public Vector scale(float scale) {
|
|
return scale(scale, this, this);
|
|
}
|
|
|
|
/**
|
|
* Scale the source quaternion by scale and put the result in the destination
|
|
* @param scale The amount to scale by
|
|
* @param src The source quaternion
|
|
* @param dest The destination quaternion, or null if a new quaternion is to be created
|
|
* @return The scaled quaternion
|
|
*/
|
|
public static Quaternion scale(float scale, Quaternion src, Quaternion dest) {
|
|
if (dest == null)
|
|
dest = new Quaternion();
|
|
dest.x = src.x * scale;
|
|
dest.y = src.y * scale;
|
|
dest.z = src.z * scale;
|
|
dest.w = src.w * scale;
|
|
return dest;
|
|
}
|
|
|
|
/* (non-Javadoc)
|
|
* @see org.lwjgl.util.vector.ReadableVector#store(java.nio.FloatBuffer)
|
|
*/
|
|
public Vector store(FloatBuffer buf) {
|
|
buf.put(x);
|
|
buf.put(y);
|
|
buf.put(z);
|
|
buf.put(w);
|
|
|
|
return this;
|
|
}
|
|
|
|
/**
|
|
* @return x
|
|
*/
|
|
public final float getX() {
|
|
return x;
|
|
}
|
|
|
|
/**
|
|
* @return y
|
|
*/
|
|
public final float getY() {
|
|
return y;
|
|
}
|
|
|
|
/**
|
|
* Set X
|
|
*
|
|
* @param x
|
|
*/
|
|
public final void setX(float x) {
|
|
this.x = x;
|
|
}
|
|
|
|
/**
|
|
* Set Y
|
|
*
|
|
* @param y
|
|
*/
|
|
public final void setY(float y) {
|
|
this.y = y;
|
|
}
|
|
|
|
/**
|
|
* Set Z
|
|
*
|
|
* @param z
|
|
*/
|
|
public void setZ(float z) {
|
|
this.z = z;
|
|
}
|
|
|
|
/*
|
|
* (Overrides)
|
|
*
|
|
* @see org.lwjgl.vector.ReadableVector3f#getZ()
|
|
*/
|
|
public float getZ() {
|
|
return z;
|
|
}
|
|
|
|
/**
|
|
* Set W
|
|
*
|
|
* @param w
|
|
*/
|
|
public void setW(float w) {
|
|
this.w = w;
|
|
}
|
|
|
|
/*
|
|
* (Overrides)
|
|
*
|
|
* @see org.lwjgl.vector.ReadableVector3f#getW()
|
|
*/
|
|
public float getW() {
|
|
return w;
|
|
}
|
|
|
|
public String toString() {
|
|
return "Quaternion: " + x + " " + y + " " + z + " " + w;
|
|
}
|
|
|
|
/**
|
|
* Sets the value of this quaternion to the quaternion product of
|
|
* quaternions left and right (this = left * right). Note that this is safe
|
|
* for aliasing (e.g. this can be left or right).
|
|
*
|
|
* @param left
|
|
* the first quaternion
|
|
* @param right
|
|
* the second quaternion
|
|
*/
|
|
public static Quaternion mul(Quaternion left, Quaternion right,
|
|
Quaternion dest) {
|
|
if (dest == null)
|
|
dest = new Quaternion();
|
|
dest.set(left.x * right.w + left.w * right.x + left.y * right.z
|
|
- left.z * right.y, left.y * right.w + left.w * right.y
|
|
+ left.z * right.x - left.x * right.z, left.z * right.w
|
|
+ left.w * right.z + left.x * right.y - left.y * right.x,
|
|
left.w * right.w - left.x * right.x - left.y * right.y
|
|
- left.z * right.z);
|
|
return dest;
|
|
}
|
|
|
|
/**
|
|
*
|
|
* Multiplies quaternion left by the inverse of quaternion right and places
|
|
* the value into this quaternion. The value of both argument quaternions is
|
|
* preservered (this = left * right^-1).
|
|
*
|
|
* @param left
|
|
* the left quaternion
|
|
* @param right
|
|
* the right quaternion
|
|
*/
|
|
public static Quaternion mulInverse(Quaternion left, Quaternion right,
|
|
Quaternion dest) {
|
|
float n = right.lengthSquared();
|
|
// zero-div may occur.
|
|
n = (n == 0.0 ? n : 1 / n);
|
|
// store on stack once for aliasing-safty
|
|
if (dest == null)
|
|
dest = new Quaternion();
|
|
dest
|
|
.set((left.x * right.w - left.w * right.x - left.y
|
|
* right.z + left.z * right.y)
|
|
* n, (left.y * right.w - left.w * right.y - left.z
|
|
* right.x + left.x * right.z)
|
|
* n, (left.z * right.w - left.w * right.z - left.x
|
|
* right.y + left.y * right.x)
|
|
* n, (left.w * right.w + left.x * right.x + left.y
|
|
* right.y + left.z * right.z)
|
|
* n);
|
|
|
|
return dest;
|
|
}
|
|
|
|
/**
|
|
* Sets the value of this quaternion to the equivalent rotation of the
|
|
* Axis-Angle argument.
|
|
*
|
|
* @param a1
|
|
* the axis-angle: (x,y,z) is the axis and w is the angle
|
|
*/
|
|
public final void setFromAxisAngle(Vector4f a1) {
|
|
x = a1.x;
|
|
y = a1.y;
|
|
z = a1.z;
|
|
float n = (float) Math.sqrt(x * x + y * y + z * z);
|
|
// zero-div may occur.
|
|
float s = (float) (Math.sin(0.5 * a1.w) / n);
|
|
x *= s;
|
|
y *= s;
|
|
z *= s;
|
|
w = (float) Math.cos(0.5 * a1.w);
|
|
}
|
|
|
|
/**
|
|
* Sets the value of this quaternion using the rotational component of the
|
|
* passed matrix.
|
|
*
|
|
* @param m
|
|
* The matrix
|
|
* @return this
|
|
*/
|
|
public final Quaternion setFromMatrix(Matrix4f m) {
|
|
return setFromMatrix(m, this);
|
|
}
|
|
|
|
/**
|
|
* Sets the value of the source quaternion using the rotational component of the
|
|
* passed matrix.
|
|
*
|
|
* @param m
|
|
* The source matrix
|
|
* @param q
|
|
* The destination quaternion, or null if a new quaternion is to be created
|
|
* @return q
|
|
*/
|
|
public final static Quaternion setFromMatrix(Matrix4f m, Quaternion q) {
|
|
return q.setFromMat(m.m00, m.m01, m.m02, m.m10, m.m11, m.m12, m.m20,
|
|
m.m21, m.m22);
|
|
}
|
|
|
|
/**
|
|
* Sets the value of this quaternion using the rotational component of the
|
|
* passed matrix.
|
|
*
|
|
* @param m
|
|
* The source matrix
|
|
*/
|
|
public final Quaternion setFromMatrix(Matrix3f m) {
|
|
return setFromMatrix(m, this);
|
|
}
|
|
|
|
/**
|
|
* Sets the value of the source quaternion using the rotational component of the
|
|
* passed matrix.
|
|
*
|
|
* @param m
|
|
* The source matrix
|
|
* @param q
|
|
* The destination quaternion, or null if a new quaternion is to be created
|
|
* @return q
|
|
*/
|
|
public static final Quaternion setFromMatrix(Matrix3f m, Quaternion q) {
|
|
return q.setFromMat(m.m00, m.m01, m.m02, m.m10, m.m11, m.m12, m.m20,
|
|
m.m21, m.m22);
|
|
}
|
|
|
|
/**
|
|
* Private method to perform the matrix-to-quaternion conversion
|
|
*/
|
|
private Quaternion setFromMat(float m00, float m01, float m02, float m10,
|
|
float m11, float m12, float m20, float m21, float m22) {
|
|
|
|
float s;
|
|
float tr = m00 + m11 + m22;
|
|
if (tr >= 0.0) {
|
|
s = (float) Math.sqrt(tr + 1.0);
|
|
w = s * 0.5f;
|
|
s = 0.5f / s;
|
|
x = (m21 - m12) * s;
|
|
y = (m02 - m20) * s;
|
|
z = (m10 - m01) * s;
|
|
} else {
|
|
float max = Math.max(Math.max(m00, m11), m22);
|
|
if (max == m00) {
|
|
s = (float) Math.sqrt(m00 - (m11 + m22) + 1.0);
|
|
x = s * 0.5f;
|
|
s = 0.5f / s;
|
|
y = (m01 + m10) * s;
|
|
z = (m20 + m02) * s;
|
|
w = (m21 - m12) * s;
|
|
} else if (max == m11) {
|
|
s = (float) Math.sqrt(m11 - (m22 + m00) + 1.0);
|
|
y = s * 0.5f;
|
|
s = 0.5f / s;
|
|
z = (m12 + m21) * s;
|
|
x = (m01 + m10) * s;
|
|
w = (m02 - m20) * s;
|
|
} else {
|
|
s = (float) Math.sqrt(m22 - (m00 + m11) + 1.0);
|
|
z = s * 0.5f;
|
|
s = 0.5f / s;
|
|
x = (m20 + m02) * s;
|
|
y = (m12 + m21) * s;
|
|
w = (m10 - m01) * s;
|
|
}
|
|
}
|
|
return this;
|
|
}
|
|
}
|