Back to project page gameengine.
The source code is released under:
Apache License
If you think the Android project gameengine listed in this page is inappropriate, such as containing malicious code/tools or violating the copyright, please email info at java2s dot com, thanks.
/******************************************************************************* * Copyright 2011 See AUTHORS file./* w w w. j a va 2 s. c o m*/ * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. ******************************************************************************/ package com.badlogic.gdx.math; import java.io.Serializable; /** * A simple quaternion class. See http://en.wikipedia.org/wiki/Quaternion for * more information. * * @author badlogicgames@gmail.com * @author vesuvio */ public class Quaternion implements Serializable { private static final long serialVersionUID = -7661875440774897168L; private static final float NORMALIZATION_TOLERANCE = 0.00001f; private static Quaternion tmp1 = new Quaternion(0, 0, 0, 0); private static Quaternion tmp2 = new Quaternion(0, 0, 0, 0); public float x; public float y; public float z; public float w; /** * Constructor, sets the four components of the quaternion. * * @param x * The x-component * @param y * The y-component * @param z * The z-component * @param w * The w-component */ public Quaternion(float x, float y, float z, float w) { this.set(x, y, z, w); } public Quaternion() { idt(); } /** * Constructor, sets the quaternion components from the given quaternion. * * @param quaternion * The quaternion to copy. */ public Quaternion(Quaternion quaternion) { this.set(quaternion); } /** * Constructor, sets the quaternion from the given axis vector and the angle * around that axis in degrees. * * @param axis * The axis * @param angle * The angle in degrees. */ public Quaternion(Vector3 axis, float angle) { this.set(axis, angle); } /** * Sets the components of the quaternion * * @param x * The x-component * @param y * The y-component * @param z * The z-component * @param w * The w-component * @return This quaternion for chaining */ public Quaternion set(float x, float y, float z, float w) { this.x = x; this.y = y; this.z = z; this.w = w; return this; } /** * Sets the quaternion components from the given quaternion. * * @param quaternion * The quaternion. * @return This quaternion for chaining. */ public Quaternion set(Quaternion quaternion) { return this.set(quaternion.x, quaternion.y, quaternion.z, quaternion.w); } /** * Sets the quaternion components from the given axis and angle around that * axis. * * @param axis * The axis * @param angle * The angle in degrees * @return This quaternion for chaining. */ public Quaternion set(Vector3 axis, float angle) { float l_ang = (float) Math.toRadians(angle); float l_sin = (float) Math.sin(l_ang / 2); float l_cos = (float) Math.cos(l_ang / 2); return this.set(axis.x * l_sin, axis.y * l_sin, axis.z * l_sin, l_cos) .nor(); } /** @return a copy of this quaternion */ public Quaternion cpy() { return new Quaternion(this); } /** @return the euclidian length of this quaternion */ public float len() { return (float) Math.sqrt(x * x + y * y + z * z + w * w); } /** {@inheritDoc} */ public String toString() { return "[" + x + "|" + y + "|" + z + "|" + w + "]"; } /** * Sets the quaternion to the given euler angles. * * @param yaw * the yaw in degrees * @param pitch * the pitch in degress * @param roll * the roll in degess * @return this quaternion */ public Quaternion setEulerAngles(float yaw, float pitch, float roll) { yaw = (float) Math.toRadians(yaw); pitch = (float) Math.toRadians(pitch); roll = (float) Math.toRadians(roll); float num9 = roll * 0.5f; float num6 = (float) Math.sin(num9); float num5 = (float) Math.cos(num9); float num8 = pitch * 0.5f; float num4 = (float) Math.sin(num8); float num3 = (float) Math.cos(num8); float num7 = yaw * 0.5f; float num2 = (float) Math.sin(num7); float num = (float) Math.cos(num7); x = ((num * num4) * num5) + ((num2 * num3) * num6); y = ((num2 * num3) * num5) - ((num * num4) * num6); z = ((num * num3) * num6) - ((num2 * num4) * num5); w = ((num * num3) * num5) + ((num2 * num4) * num6); return this; } /** @return the length of this quaternion without square root */ public float len2() { return x * x + y * y + z * z + w * w; } /** * Normalizes this quaternion to unit length * * @return the quaternion for chaining */ public Quaternion nor() { float len = len2(); if (len != 0.f && (Math.abs(len - 1.0f) > NORMALIZATION_TOLERANCE)) { len = (float) Math.sqrt(len); w /= len; x /= len; y /= len; z /= len; } return this; } /** * Conjugate the quaternion. * * @return This quaternion for chaining */ public Quaternion conjugate() { x = -x; y = -y; z = -z; return this; } // TODO : this would better fit into the vector3 class /** * Transforms the given vector using this quaternion * * @param v * Vector to transform */ public void transform(Vector3 v) { tmp2.set(this); tmp2.conjugate(); tmp2.mulLeft(tmp1.set(v.x, v.y, v.z, 0)) .mulLeft(this); v.x = tmp2.x; v.y = tmp2.y; v.z = tmp2.z; } /** * Multiplies this quaternion with another one * * @param q * Quaternion to multiply with * @return This quaternion for chaining */ public Quaternion mul(Quaternion q) { float newX = w * q.x + x * q.w + y * q.z - z * q.y; float newY = w * q.y + y * q.w + z * q.x - x * q.z; float newZ = w * q.z + z * q.w + x * q.y - y * q.x; float newW = w * q.w - x * q.x - y * q.y - z * q.z; x = newX; y = newY; z = newZ; w = newW; return this; } /** * Multiplies this quaternion with another one in the form of q * this * * @param q * Quaternion to multiply with * @return This quaternion for chaining */ public Quaternion mulLeft(Quaternion q) { float newX = q.w * x + q.x * w + q.y * z - q.z * y; float newY = q.w * y + q.y * w + q.z * x - q.x * z; float newZ = q.w * z + q.z * w + q.x * y - q.y * x; float newW = q.w * w - q.x * x - q.y * y - q.z * z; x = newX; y = newY; z = newZ; w = newW; return this; } // TODO : the matrix4 set(quaternion) doesnt set the last row+col of the // matrix to 0,0,0,1 so... that's why there is this // method /** * Fills a 4x4 matrix with the rotation matrix represented by this * quaternion. * * @param matrix * Matrix to fill */ public void toMatrix(float[] matrix) { float xx = x * x; float xy = x * y; float xz = x * z; float xw = x * w; float yy = y * y; float yz = y * z; float yw = y * w; float zz = z * z; float zw = z * w; // Set matrix from quaternion matrix[Matrix4.M00] = 1 - 2 * (yy + zz); matrix[Matrix4.M01] = 2 * (xy - zw); matrix[Matrix4.M02] = 2 * (xz + yw); matrix[Matrix4.M03] = 0; matrix[Matrix4.M10] = 2 * (xy + zw); matrix[Matrix4.M11] = 1 - 2 * (xx + zz); matrix[Matrix4.M12] = 2 * (yz - xw); matrix[Matrix4.M13] = 0; matrix[Matrix4.M20] = 2 * (xz - yw); matrix[Matrix4.M21] = 2 * (yz + xw); matrix[Matrix4.M22] = 1 - 2 * (xx + yy); matrix[Matrix4.M23] = 0; matrix[Matrix4.M30] = 0; matrix[Matrix4.M31] = 0; matrix[Matrix4.M32] = 0; matrix[Matrix4.M33] = 1; } /** * Sets the quaternion to an identity Quaternion * * @return this quaternion for chaining */ public Quaternion idt() { this.set(0, 0, 0, 1); return this; } // todo : the setFromAxis(v3,float) method should replace the set(v3,float) // method /** * Sets the quaternion components from the given axis and angle around that * axis. * * @param axis * The axis * @param angle * The angle in degrees * @return This quaternion for chaining. */ public Quaternion setFromAxis(Vector3 axis, float angle) { return setFromAxis(axis.x, axis.y, axis.z, angle); } /** * Sets the quaternion components from the given axis and angle around that * axis. * * @param x * X direction of the axis * @param y * Y direction of the axis * @param z * Z direction of the axis * @param angle * The angle in degrees * @return This quaternion for chaining. */ public Quaternion setFromAxis(float x, float y, float z, float angle) { float l_ang = angle * MathUtils.degreesToRadians; float l_sin = MathUtils.sin(l_ang / 2); float l_cos = MathUtils.cos(l_ang / 2); return this.set(x * l_sin, y * l_sin, z * l_sin, l_cos) .nor(); } // fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y, zAxis.y, // xAxis.z, yAxis.z, zAxis.z); // final float m00, final float m01, final float m02, final float m10, // final float m11, final float m12, final float m20, final float m21, final // float m22 public Quaternion setFromMatrix(Matrix4 matrix) { return setFromAxes(matrix.val[Matrix4.M00], matrix.val[Matrix4.M01], matrix.val[Matrix4.M02], matrix.val[Matrix4.M10], matrix.val[Matrix4.M11], matrix.val[Matrix4.M12], matrix.val[Matrix4.M20], matrix.val[Matrix4.M21], matrix.val[Matrix4.M22]); } /** * <p> * Sets the Quaternion from the given x-, y- and z-axis which have to be * orthonormal. * </p> * * <p> * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ * which in turn took it from Graphics Gem code at * ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z. * </p> * * @param xx * x-axis x-coordinate * @param xy * x-axis y-coordinate * @param xz * x-axis z-coordinate * @param yx * y-axis x-coordinate * @param yy * y-axis y-coordinate * @param yz * y-axis z-coordinate * @param zx * z-axis x-coordinate * @param zy * z-axis y-coordinate * @param zz * z-axis z-coordinate */ public Quaternion setFromAxes(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) { // the trace is the sum of the diagonal elements; see // http://mathworld.wolfram.com/MatrixTrace.html final float m00 = xx, m01 = xy, m02 = xz; final float m10 = yx, m11 = yy, m12 = yz; final float m20 = zx, m21 = zy, m22 = zz; final float t = m00 + m11 + m22; // we protect the division by s by ensuring that s>=1 double x, y, z, w; if (t >= 0) { // |w| >= .5 double s = Math.sqrt(t + 1); // |s|>=1 ... w = 0.5 * s; s = 0.5 / 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)) { double s = Math.sqrt(1.0 + m00 - m11 - m22); // |s|>=1 x = s * 0.5; // |x| >= .5 s = 0.5 / s; y = (m10 + m01) * s; z = (m02 + m20) * s; w = (m21 - m12) * s; } else if (m11 > m22) { double s = Math.sqrt(1.0 + m11 - m00 - m22); // |s|>=1 y = s * 0.5; // |y| >= .5 s = 0.5 / s; x = (m10 + m01) * s; z = (m21 + m12) * s; w = (m02 - m20) * s; } else { double s = Math.sqrt(1.0 + m22 - m00 - m11); // |s|>=1 z = s * 0.5; // |z| >= .5 s = 0.5 / s; x = (m02 + m20) * s; y = (m21 + m12) * s; w = (m10 - m01) * s; } return set((float) x, (float) y, (float) z, (float) w); } /** * Spherical linear interpolation between this quaternion and the other * quaternion, based on the alpha value in the range [0,1]. Taken from. * Taken from Bones framework for JPCT, see http://www.aptalkarga.com/bones/ * * @param end * the end quaternion * @param alpha * alpha in the range [0,1] * @return this quaternion for chaining */ public Quaternion slerp(Quaternion end, float alpha) { if (this.equals(end)) { return this; } float result = dot(end); if (result < 0.0) { // Negate the second quaternion and the result of the dot product end.mul(-1); result = -result; } // Set the first and second scale for the interpolation float scale0 = 1 - alpha; float scale1 = alpha; // Check if the angle between the 2 quaternions was big enough to // warrant such calculations if ((1 - result) > 0.1) {// Get the angle between the 2 quaternions, // and then store the sin() of that angle final double theta = Math.acos(result); final double invSinTheta = 1f / Math.sin(theta); // Calculate the scale for q1 and q2, according to the angle and // it's sine value scale0 = (float) (Math.sin((1 - alpha) * theta) * invSinTheta); scale1 = (float) (Math.sin((alpha * theta)) * invSinTheta); } // Calculate the x, y, z and w values for the quaternion by using a // special form of linear interpolation for quaternions. final float x = (scale0 * this.x) + (scale1 * end.x); final float y = (scale0 * this.y) + (scale1 * end.y); final float z = (scale0 * this.z) + (scale1 * end.z); final float w = (scale0 * this.w) + (scale1 * end.w); set(x, y, z, w); // Return the interpolated quaternion return this; } public boolean equals(final Object o) { if (this == o) { return true; } if (!(o instanceof Quaternion)) { return false; } final Quaternion comp = (Quaternion) o; return this.x == comp.x && this.y == comp.y && this.z == comp.z && this.w == comp.w; } /** * Dot product between this and the other quaternion. * * @param other * the other quaternion. * @return this quaternion for chaining. */ public float dot(Quaternion other) { return x * other.x + y * other.y + z * other.z + w * other.w; } /** * Multiplies the components of this quaternion with the given scalar. * * @param scalar * the scalar. * @return this quaternion for chaining. */ public Quaternion mul(float scalar) { this.x *= scalar; this.y *= scalar; this.z *= scalar; this.w *= scalar; return this; } }