Mathematically, quaternions are an extension of complex numbers. In - * mathematics texts, W often appears first, but in JME it always comes last. - * - * @author Mark Powell - * @author Joshua Slack - */ -public final class Quaternion implements Savable, Cloneable, java.io.Serializable { - - static final long serialVersionUID = 1; - - private static final Logger logger = Logger.getLogger(Quaternion.class.getName()); - /** - * Shared instance of the identity quaternion (0, 0, 0, 1). Do not modify! - * - *
This is the usual representation for a null rotation. - */ - public static final Quaternion IDENTITY = new Quaternion(); - /** - * Another shared instance of the identity quaternion (0, 0, 0, 1). Do not - * modify! - */ - public static final Quaternion DIRECTION_Z = new Quaternion(); - /** - * Shared instance of the zero quaternion (0, 0, 0, 0). Do not modify! - * - *
The zero quaternion doesn't represent any valid rotation. - */ - public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0); - - static { - DIRECTION_Z.fromAxes(Vector3f.UNIT_X, Vector3f.UNIT_Y, Vector3f.UNIT_Z); - } - /** - * The first imaginary (X) component. Not an angle! - */ - protected float x; - /** - * The 2nd imaginary (Y) component. Not an angle! - */ - protected float y; - /** - * The 3rd imaginary (Z) component. Not an angle! - */ - protected float z; - /** - * The real (W) component. Not an angle! - */ - protected float w; - - /** - * Instantiates an identity quaternion: all components zeroed except - * {@code w}, which is set to 1. - */ - public Quaternion() { - x = 0; - y = 0; - z = 0; - w = 1; - } - - /** - * Instantiates a quaternion with the specified components. - * - * @param x the desired X component - * @param y the desired Y component - * @param z the desired Z component - * @param w the desired W component - */ - public Quaternion(float x, float y, float z, float w) { - this.x = x; - this.y = y; - this.z = z; - this.w = w; - } - - /** - * Returns the X component. The quaternion is unaffected. - * - * @return the value of the {@link #x} component - */ - public float getX() { - return x; - } - - /** - * Returns the Y component. The quaternion is unaffected. - * - * @return the value of the {@link #y} component - */ - public float getY() { - return y; - } - - /** - * Returns the Z component. The quaternion is unaffected. - * - * @return the value of the {@link #z} component - */ - public float getZ() { - return z; - } - - /** - * Returns the W (real) component. The quaternion is unaffected. - * - * @return the value of the {@link #w} component - */ - public float getW() { - return w; - } - - /** - * Sets all 4 components to specified values. - * - * @param x the desired X component - * @param y the desired Y component - * @param z the desired Z component - * @param w the desired W component - * @return the (modified) current instance (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; - } - - /** - * Copies all 4 components from the argument. - * - * @param q the quaternion to copy (not null, unaffected) - * @return the (modified) current instance (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; - } - - /** - * Instantiates a quaternion from Tait-Bryan angles, applying the rotations - * in x-z-y extrinsic order or y-z'-x" intrinsic order. - * - * @param angles an array of Tait-Bryan angles (in radians, exactly 3 - * elements, the X angle in {@code angles[0]}, the Y angle in {@code - * angles[1]}, and the Z angle in {@code angles[2]}, not null, - * unaffected) - */ - public Quaternion(float[] angles) { - fromAngles(angles); - } - - /** - * Instantiates a quaternion by interpolating between the specified - * quaternions. - * - *
Uses
- * {@link #slerp(com.jme3.math.Quaternion, com.jme3.math.Quaternion, float)},
- * which is fast but inaccurate.
- *
- * @param q1 the desired value when interp=0 (not null, unaffected)
- * @param q2 the desired value when interp=1 (not null, may be modified)
- * @param interp the fractional change amount
- */
- public Quaternion(Quaternion q1, Quaternion q2, float interp) {
- slerp(q1, q2, interp);
- }
-
- /**
- * Instantiates a copy of the argument.
- *
- * @param q the quaternion to copy (not null, unaffected)
- */
- public Quaternion(Quaternion q) {
- this.x = q.x;
- this.y = q.y;
- this.z = q.z;
- this.w = q.w;
- }
-
- /**
- * Sets all components to zero except {@code w}, which is set to 1.
- */
- public void loadIdentity() {
- x = y = z = 0;
- w = 1;
- }
-
- /**
- * Compares with the identity quaternion, without distinguishing -0 from 0.
- * The current instance is unaffected.
- *
- * @return true if the current quaternion equals the identity, otherwise
- * false
- */
- public boolean isIdentity() {
- if (x == 0 && y == 0 && z == 0 && w == 1) {
- return true;
- } else {
- return false;
- }
- }
-
- /**
- * Sets the quaternion from the specified Tait-Bryan angles, applying the
- * rotations in x-z-y extrinsic order or y-z'-x" intrinsic order.
- *
- * @param angles an array of Tait-Bryan angles (in radians, exactly 3
- * elements, the X angle in {@code angles[0]}, the Y angle in {@code
- * angles[1]}, and the Z angle in {@code angles[2]}, not null,
- * unaffected)
- * @return the (modified) current instance (for chaining)
- * @throws IllegalArgumentException if {@code angles.length != 3}
- */
- public Quaternion fromAngles(float[] angles) {
- if (angles.length != 3) {
- throw new IllegalArgumentException(
- "Angles array must have three elements");
- }
-
- return fromAngles(angles[0], angles[1], angles[2]);
- }
-
- /**
- * Sets the quaternion from the specified Tait-Bryan angles, applying the
- * rotations in x-z-y extrinsic order or y-z'-x" intrinsic order.
- *
- * @see
- * http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
- *
- * @param xAngle the X angle (in radians)
- * @param yAngle the Y angle (in radians)
- * @param zAngle the Z angle (in radians)
- * @return the (modified) current instance (for chaining)
- */
- public Quaternion fromAngles(float xAngle, float yAngle, float zAngle) {
- float angle;
- float sinY, sinZ, sinX, cosY, cosZ, cosX;
- angle = zAngle * 0.5f;
- sinZ = FastMath.sin(angle);
- cosZ = FastMath.cos(angle);
- angle = yAngle * 0.5f;
- sinY = FastMath.sin(angle);
- cosY = FastMath.cos(angle);
- angle = xAngle * 0.5f;
- sinX = FastMath.sin(angle);
- cosX = FastMath.cos(angle);
-
- // variables used to reduce multiplication calls.
- float cosYXcosZ = cosY * cosZ;
- float sinYXsinZ = sinY * sinZ;
- float cosYXsinZ = cosY * sinZ;
- float sinYXcosZ = sinY * cosZ;
-
- w = (cosYXcosZ * cosX - sinYXsinZ * sinX);
- x = (cosYXcosZ * sinX + sinYXsinZ * cosX);
- y = (sinYXcosZ * cosX + cosYXsinZ * sinX);
- z = (cosYXsinZ * cosX - sinYXcosZ * sinX);
-
- normalizeLocal();
- return this;
- }
-
- /**
- * Converts to equivalent Tait-Bryan angles, to be applied in x-z-y
- * intrinsic order or y-z'-x" extrinsic order, for instance by
- * {@link #fromAngles(float[])}. The current instance is unaffected.
- *
- * @see
- *
- * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
- *
- *
- * @param angles storage for the result, or null for a new float[3]
- * @return an array of 3 angles (in radians, either angles or a
- * new float[3], the X angle in angles[0], the Y angle in angles[1], and
- * the Z angle in angles[2])
- * @throws IllegalArgumentException if {@code angles.length != 3}
- */
- 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 * FastMath.atan2(x, w);
- angles[2] = FastMath.HALF_PI;
- angles[0] = 0;
- } else if (test < -0.499 * unit) { // singularity at south pole
- angles[1] = -2 * FastMath.atan2(x, w);
- angles[2] = -FastMath.HALF_PI;
- angles[0] = 0;
- } else {
- angles[1] = FastMath.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // yaw or heading
- angles[2] = FastMath.asin(2 * test / unit); // roll or bank
- angles[0] = FastMath.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // pitch or attitude
- }
- return angles;
- }
-
- /**
- * Sets the quaternion from the specified rotation matrix.
- *
- *
Does not verify that the argument is a valid rotation matrix. - * Positive scaling is compensated for, but not reflection or shear. - * - * @param matrix the input matrix (not null, unaffected) - * @return the (modified) current instance (for chaining) - */ - public Quaternion fromRotationMatrix(Matrix3f matrix) { - return fromRotationMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10, - matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22); - } - - /** - * Sets the quaternion from a rotation matrix with the specified elements. - * - *
Does not verify that the arguments form a valid rotation matrix. - * Positive scaling is compensated for, but not reflection or shear. - * - * @param m00 the matrix element in row 0, column 0 - * @param m01 the matrix element in row 0, column 1 - * @param m02 the matrix element in row 0, column 2 - * @param m10 the matrix element in row 1, column 0 - * @param m11 the matrix element in row 1, column 1 - * @param m12 the matrix element in row 1, column 2 - * @param m20 the matrix element in row 2, column 0 - * @param m21 the matrix element in row 2, column 1 - * @param m22 the matrix element in row 2, column 2 - * @return the (modified) current instance (for chaining) - */ - public Quaternion fromRotationMatrix(float m00, float m01, float m02, - float m10, float m11, float m12, float m20, float m21, float m22) { - // first normalize the forward (F), up (U) and side (S) vectors of the rotation matrix - // so that positive scaling does not affect the rotation - float lengthSquared = m00 * m00 + m10 * m10 + m20 * m20; - if (lengthSquared != 1f && lengthSquared != 0f) { - lengthSquared = 1.0f / FastMath.sqrt(lengthSquared); - m00 *= lengthSquared; - m10 *= lengthSquared; - m20 *= lengthSquared; - } - lengthSquared = m01 * m01 + m11 * m11 + m21 * m21; - if (lengthSquared != 1f && lengthSquared != 0f) { - lengthSquared = 1.0f / FastMath.sqrt(lengthSquared); - m01 *= lengthSquared; - m11 *= lengthSquared; - m21 *= lengthSquared; - } - lengthSquared = m02 * m02 + m12 * m12 + m22 * m22; - if (lengthSquared != 1f && lengthSquared != 0f) { - lengthSquared = 1.0f / FastMath.sqrt(lengthSquared); - m02 *= lengthSquared; - m12 *= lengthSquared; - m22 *= lengthSquared; - } - - // 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 = FastMath.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 = FastMath.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 = FastMath.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 = FastMath.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; - } - - /** - * Converts to an equivalent rotation matrix. The current instance is - * unaffected. - * - *
Note: the result is created from a normalized version of the current - * instance. - * - * @return a new 3x3 rotation matrix - */ - public Matrix3f toRotationMatrix() { - Matrix3f matrix = new Matrix3f(); - return toRotationMatrix(matrix); - } - - /** - * Converts to an equivalent rotation matrix. The current instance is - * unaffected. - * - *
Note: the result is created from a normalized version of the current - * instance. - * - * @param result storage for the result (not null) - * @return {@code result}, configured as a 3x3 rotation matrix - */ - 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; - } - - /** - * Sets the rotation component of the specified transform matrix. The - * current instance is unaffected. - * - *
Note: preserves the translation component of {@code store} but not - * its scaling component. - * - *
Note: the result is created from a normalized version of the current - * instance. - * - * @param store storage for the result (not null) - * @return {@code store}, with 9 of its 16 elements modified - */ - public Matrix4f toTransformMatrix(Matrix4f store) { - - 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 - store.m00 = 1 - (yy + zz); - store.m01 = (xy - zw); - store.m02 = (xz + yw); - store.m10 = (xy + zw); - store.m11 = 1 - (xx + zz); - store.m12 = (yz - xw); - store.m20 = (xz - yw); - store.m21 = (yz + xw); - store.m22 = 1 - (xx + yy); - - return store; - } - - /** - * Sets the rotation component of the specified transform matrix. The - * current instance is unaffected. - * - *
Note: preserves the translation and scaling components of - * {@code result} unless {@code result} includes reflection. - * - *
Note: the result is created from a normalized version of the current - * instance. - * - * @param result storage for the result (not null) - * @return {@code result}, with 9 of its 16 elements modified - */ - public Matrix4f toRotationMatrix(Matrix4f result) { - TempVars tempv = TempVars.get(); - Vector3f originalScale = tempv.vect1; - - result.toScaleVector(originalScale); - result.setScale(1, 1, 1); - 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); - - result.setScale(originalScale); - - tempv.release(); - - return result; - } - - /** - * Calculates one of the basis vectors of the rotation. The current instance - * is unaffected. - * - *
Note: the result is created from a normalized version of the current - * instance. - * - * @param i which basis vector to retrieve (≥0, <3, 0→X-axis, - * 1→Y-axis, 2→Z-axis) - * @return the basis vector (a new Vector3f) - */ - public Vector3f getRotationColumn(int i) { - return getRotationColumn(i, null); - } - - /** - * Calculates one of the basis vectors of the rotation. The current instance - * is unaffected. - * - *
Note: the result is created from a normalized version of the current
- * instance.
- *
- * @param i which basis vector to retrieve (≥0, <3, 0→X-axis,
- * 1→Y-axis, 2→Z-axis)
- * @param store storage for the result, or null for a new Vector3f
- * @return the basis vector (either store or a new Vector3f)
- * @throws IllegalArgumentException if index is not 0, 1, or 2
- */
- public Vector3f getRotationColumn(int i, Vector3f store) {
- if (store == null) {
- store = new Vector3f();
- }
-
- float norm = norm();
- if (norm != 1.0f) {
- norm = 1.0f / 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 IllegalArgumentException("Invalid column index. " + i);
- }
-
- return store;
- }
-
- /**
- * Sets the quaternion from the specified rotation angle and axis of
- * rotation. This method creates garbage, so use
- * {@link #fromAngleNormalAxis(float, com.jme3.math.Vector3f)} if the axis
- * is known to be normalized.
- *
- * @param angle the desired rotation angle (in radians)
- * @param axis the desired axis of rotation (not null, unaffected)
- * @return the (modified) current instance (for chaining)
- */
- public Quaternion fromAngleAxis(float angle, Vector3f axis) {
- Vector3f normAxis = axis.normalize();
- fromAngleNormalAxis(angle, normAxis);
- return this;
- }
-
- /**
- * Sets the quaternion from the specified rotation angle and normalized axis
- * of rotation. If the axis might not be normalized, use
- * {@link #fromAngleAxis(float, com.jme3.math.Vector3f)} instead.
- *
- * @param angle the desired rotation angle (in radians)
- * @param axis the desired axis of rotation (not null, length=1, unaffected)
- * @return the (modified) current instance (for chaining)
- */
- public Quaternion fromAngleNormalAxis(float angle, Vector3f axis) {
- if (axis.x == 0 && axis.y == 0 && axis.z == 0) {
- loadIdentity();
- } else {
- float halfAngle = 0.5f * angle;
- float sin = FastMath.sin(halfAngle);
- w = FastMath.cos(halfAngle);
- x = sin * axis.x;
- y = sin * axis.y;
- z = sin * axis.z;
- }
- return this;
- }
-
- /**
- * Converts the quaternion to a rotation angle and axis of rotation, storing
- * the axis in the argument (if it's non-null) and returning the angle.
- *
- *
If the quaternion has {@code x*x + y*y + z*z == 0}, then (1,0,0) is - * stored and 0 is returned. (This might happen if the rotation angle is - * very close to 0.) - * - *
Otherwise, the quaternion is assumed to be normalized (norm=1). No - * error checking is performed; the caller must ensure that the quaternion - * is normalized. - * - *
In all cases, the current instance is unaffected. - * - * @param axisStore storage for the axis (modified if not null) - * @return the rotation angle (in radians) - */ - public float toAngleAxis(Vector3f 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 * FastMath.acos(w)); - if (axisStore != null) { - float invLength = (1.0f / FastMath.sqrt(sqrLength)); - axisStore.x = x * invLength; - axisStore.y = y * invLength; - axisStore.z = z * invLength; - } - } - - return angle; - } - - /** - * Interpolates between the specified quaternions and stores the result in - * the current instance. - * - * @param q1 the desired value when interp=0 (not null, unaffected) - * @param q2 the desired value when interp=1 (not null, may be modified) - * @param t the fractional change amount - * @return the (modified) current instance (for chaining) - */ - 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 = FastMath.acos(result); - float invSinTheta = 1f / FastMath.sin(theta); - - // Calculate the scale for q1 and q2, according to the angle and - // its sine - scale0 = FastMath.sin((1 - t) * theta) * invSinTheta; - scale1 = FastMath.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; - } - - /** - * Interpolates between the current instance and {@code q2} and stores the - * result in the current instance. - * - *
This method is often more accurate than - * {@link #nlerp(com.jme3.math.Quaternion, float)}, but slower. - - * @param q2 the desired value when changeAmnt=1 (not null, may be modified) - * @param changeAmount the fractional change amount - */ - public void slerp(Quaternion q2, float changeAmount) { - 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 - changeAmount; - float scale1 = changeAmount; - - // 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 = FastMath.acos(result); - float invSinTheta = 1f / FastMath.sin(theta); - - // Calculate the scale for q1 and q2, according to the angle and - // its sine - scale0 = FastMath.sin((1 - changeAmount) * theta) * invSinTheta; - scale1 = FastMath.sin((changeAmount * 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); - } - - /** - * Interpolates quickly between the current instance and {@code q2} using - * nlerp, and stores the result in the current instance. - * - *
This method is often faster than - * {@link #slerp(com.jme3.math.Quaternion, float)}, but less accurate. - * - * @param q2 the desired value when blend=1 (not null, unaffected) - * @param blend the fractional change amount - */ - public void nlerp(Quaternion q2, float blend) { - float dot = dot(q2); - float blendI = 1.0f - blend; - if (dot < 0.0f) { - x = blendI * x - blend * q2.x; - y = blendI * y - blend * q2.y; - z = blendI * z - blend * q2.z; - w = blendI * w - blend * q2.w; - } else { - x = blendI * x + blend * q2.x; - y = blendI * y + blend * q2.y; - z = blendI * z + blend * q2.z; - w = blendI * w + blend * q2.w; - } - normalizeLocal(); - } - - /** - * Adds the argument and returns the sum as a new instance. The current - * instance is unaffected. - * - *
Seldom used. To combine rotations, use - * {@link #mult(com.jme3.math.Quaternion)} instead of this method. - * - * @param q the quaternion to add (not null, unaffected) - * @return a new Quaternion - */ - public Quaternion add(Quaternion q) { - return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w); - } - - /** - * Adds the argument and returns the (modified) current instance. - * - *
Seldom used. To combine rotations, use - * {@link #multLocal(com.jme3.math.Quaternion)} or - * {@link #mult(com.jme3.math.Quaternion, com.jme3.math.Quaternion)} - * instead of this method. - * - * @param q the quaternion to add (not null, unaffected unless it's - * {@code this}) - * @return the (modified) current instance (for chaining) - */ - public Quaternion addLocal(Quaternion q) { - this.x += q.x; - this.y += q.y; - this.z += q.z; - this.w += q.w; - return this; - } - - /** - * Subtracts the argument and returns difference as a new instance. The - * current instance is unaffected. - * - * @param q the quaternion to subtract (not null, unaffected) - * @return a new Quaternion - */ - public Quaternion subtract(Quaternion q) { - return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w); - } - - /** - * Subtracts the argument and returns the (modified) current instance. - * - *
To quantify the similarity of 2 normalized quaternions, use - * {@link #dot(com.jme3.math.Quaternion)}. - * - * @param q the quaternion to subtract (not null, unaffected unless it's - * {@code this}) - * @return the (modified) current instance - */ - public Quaternion subtractLocal(Quaternion q) { - this.x -= q.x; - this.y -= q.y; - this.z -= q.z; - this.w -= q.w; - return this; - } - - /** - * Multiplies by the argument and returns the product as a new instance. - * The current instance is unaffected. - * - *
This method is used to combine rotations. Note that quaternion - * multiplication is noncommutative, so generally q * p != p * q. - * - * @param q the right factor (not null, unaffected) - * @return {@code this * q} (a new Quaternion) - */ - public Quaternion mult(Quaternion q) { - return mult(q, null); - } - - /** - * Multiplies by the specified quaternion and returns the product in a 3rd - * quaternion. The current instance is unaffected, unless it's {@code storeResult}. - * - *
This method is used to combine rotations. Note that quaternion - * multiplication is noncommutative, so generally q * p != p * q. - * - *
It is safe for {@code q} and {@code storeResult} to be the same object. - * However, if {@code this} and {@code storeResult} are the same object, the result - * is undefined. - * - * @param q the right factor (not null, unaffected unless it's {@code storeResult}) - * @param storeResult storage for the product, or null for a new Quaternion - * @return {@code this * q} (either {@code storeResult} or a new Quaternion) - */ - public Quaternion mult(Quaternion q, Quaternion storeResult) { - if (storeResult == null) { - storeResult = new Quaternion(); - } - float qw = q.w, qx = q.x, qy = q.y, qz = q.z; - storeResult.x = x * qw + y * qz - z * qy + w * qx; - storeResult.y = -x * qz + y * qw + z * qx + w * qy; - storeResult.z = x * qy - y * qx + z * qw + w * qz; - storeResult.w = -x * qx - y * qy - z * qz + w * qw; - return storeResult; - } - - /** - * Applies the rotation represented by the argument to the current instance. - * - *
Does not verify that {@code matrix} is a valid rotation matrix. - * Positive scaling is compensated for, but not reflection or shear. - * - * @param matrix the rotation matrix to apply (not null, unaffected) - */ - public void apply(Matrix3f 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; - } - - /** - * Sets the quaternion from the specified orthonormal basis. - * - *
The 3 basis vectors describe the axes of a rotated coordinate system. - * They are assumed to be normalized, mutually orthogonal, and in right-hand - * order. No error checking is performed; the caller must ensure that the - * specified vectors represent a right-handed coordinate system. - * - * @param axis the array of desired basis vectors (not null, array length=3, - * each vector having length=1, unaffected) - * @return the (modified) current instance (for chaining) - * @throws IllegalArgumentException if {@code axis.length != 3} - */ - public Quaternion fromAxes(Vector3f[] axis) { - if (axis.length != 3) { - throw new IllegalArgumentException( - "Axis array must have three elements"); - } - return fromAxes(axis[0], axis[1], axis[2]); - } - - /** - * Sets the quaternion from the specified orthonormal basis. - * - *
The 3 basis vectors describe the axes of a rotated coordinate system. - * They are assumed to be normalized, mutually orthogonal, and in right-hand - * order. No error checking is performed; the caller must ensure that the - * specified vectors represent a right-handed coordinate system. - * - * @param xAxis the X axis of the desired coordinate system (not null, - * length=1, unaffected) - * @param yAxis the Y axis of the desired coordinate system (not null, - * length=1, unaffected) - * @param zAxis the Z axis of the desired coordinate system (not null, - * length=1, unaffected) - * @return the (modified) current instance (for chaining) - */ - public Quaternion fromAxes(Vector3f xAxis, Vector3f yAxis, Vector3f zAxis) { - return fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y, - zAxis.y, xAxis.z, yAxis.z, zAxis.z); - } - - /** - * Converts the quaternion to a rotated coordinate system and stores the - * resulting axes in the argument. The current instance is unaffected. - * - *
The resulting vectors form the basis of a rotated coordinate system. - * They will be normalized, mutually orthogonal, and in right-hand order. - * - * @param axes storage for the results (not null, length=3, each element - * non-null, elements modified) - * @throws IllegalArgumentException if {@code axes.length != 3} - */ - public void toAxes(Vector3f axes[]) { - if (axes.length != 3) { - throw new IllegalArgumentException( - "Axes array must have three elements"); - } - - Matrix3f tempMat = toRotationMatrix(); - axes[0] = tempMat.getColumn(0, axes[0]); - axes[1] = tempMat.getColumn(1, axes[1]); - axes[2] = tempMat.getColumn(2, axes[2]); - } - - /** - * Rotates the argument vector and returns the result as a new vector. The - * current instance is unaffected. - * - *
The quaternion is assumed to be normalized (norm=1). No error checking - * is performed; the caller must ensure that the norm is approximately equal - * to 1. - * - *
Despite the name, the result differs from the mathematical definition - * of vector-quaternion multiplication. - * - * @param v the vector to rotate (not null, unaffected) - * @return a new Vector3f - */ - public Vector3f mult(Vector3f v) { - return mult(v, null); - } - - /** - * Rotates the argument vector. Despite the name, the current instance is - * unaffected. - * - *
The quaternion is assumed to be normalized (norm=1). No error checking - * is performed; the caller must ensure that the norm is approximately equal - * to 1. - * - *
Despite the name, the result differs from the mathematical definition - * of vector-quaternion multiplication. - * - * @param v the vector to rotate (not null) - * @return the (modified) vector {@code v} - */ - public Vector3f multLocal(Vector3f 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 by the argument and returns the (modified) current instance. - * - *
This method is used to combine rotations. Note that quaternion - * multiplication is noncommutative, so generally q * p != p * q. - * - * @param q the right factor (not null, unaffected unless it's {@code this}) - * @return the (modified) current instance (for chaining) - */ - 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 by a quaternion with the specified components and returns the - * (modified) current instance. - * - *
This method is used to combine rotations. Note that quaternion - * multiplication is noncommutative, so generally q * p != p * q. - * - * @param qx the X component of the right factor - * @param qy the Y component of the right factor - * @param qz the Z component of the right factor - * @param qw the W component of the right factor - * @return the (modified) current instance (for chaining) - */ - 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; - } - - /** - * Rotates a specified vector and returns the result in another vector. The - * current instance is unaffected. - * - *
The quaternion is assumed to be normalized (norm=1). No error checking - * is performed; the caller must ensure that the norm is approximately equal - * to 1. - * - *
It is safe for {@code v} and {@code store} to be the same object. - * - *
Despite the name, the result differs from the mathematical definition - * of vector-quaternion multiplication. - * - * @param v the vector to rotate (not null, unaffected unless it's - * {@code store}) - * @param store storage for the result, or null for a new Vector3f - * @return the rotated vector (either {@code store} or a new Vector3f) - */ - public Vector3f mult(Vector3f v, Vector3f store) { - if (store == null) { - store = new Vector3f(); - } - if (v.x == 0 && v.y == 0 && v.z == 0) { - store.set(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; - } - - /** - * Multiplies with the scalar argument and returns the product as a new - * instance. The current instance is unaffected. - * - * @param scalar the scaling factor - * @return a new Quaternion - */ - public Quaternion mult(float scalar) { - return new Quaternion(scalar * x, scalar * y, scalar * z, scalar * w); - } - - /** - * Multiplies by the scalar argument and returns the (modified) current - * instance. - * - * @param scalar the scaling factor - * @return the (modified) current instance (for chaining) - */ - public Quaternion multLocal(float scalar) { - w *= scalar; - x *= scalar; - y *= scalar; - z *= scalar; - return this; - } - - /** - * Returns the dot product with the argument. The current instance is - * unaffected. - * - *
This method can be used to quantify the similarity of 2 normalized
- * quaternions.
- *
- * @param q the quaternion to multiply (not null, unaffected)
- * @return the dot product
- */
- public float dot(Quaternion q) {
- return w * q.w + x * q.x + y * q.y + z * q.z;
- }
-
- /**
- * Returns the norm, defined as the dot product of the quaternion with
- * itself. The current instance is unaffected.
- *
- * @return the sum of the squared components (not negative)
- */
- public float norm() {
- return w * w + x * x + y * y + z * z;
- }
-
-// /**
-// * normalize normalizes the current Quaternion
-// * @deprecated The naming of this method doesn't follow convention.
-// * Please use {@link Quaternion#normalizeLocal() } instead.
-// */
-// @Deprecated
-// public void normalize() {
-// float n = FastMath.invSqrt(norm());
-// x *= n;
-// y *= n;
-// z *= n;
-// w *= n;
-// }
-
- /**
- * Scales the quaternion to have norm=1 and returns the (modified) current
- * instance. For a quaternion with norm=0, the result is undefined.
- *
- * @return the (modified) current instance (for chaining)
- */
- public Quaternion normalizeLocal() {
- float n = FastMath.invSqrt(norm());
- x *= n;
- y *= n;
- z *= n;
- w *= n;
- return this;
- }
-
- /**
- * Returns the multiplicative inverse. For a quaternion with norm=0, null is
- * returned. Either way, the current instance is unaffected.
- *
- * @return a new Quaternion or null
- */
- 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;
- }
-
- /**
- * Inverts the quaternion and returns the (modified) current instance. For
- * a quaternion with norm=0, the current instance is unchanged.
- *
- * @return the current instance (for chaining)
- */
- 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;
- }
-
- /**
- * Negates all 4 components.
- *
- * @deprecated The naming of this method doesn't follow convention. Please
- * use {@link #negateLocal()} instead.
- */
- @Deprecated
- public void negate() {
- negateLocal();
- }
-
- /**
- * Negates all 4 components and returns the (modified) current instance.
- *
- * @return the (modified) current instance (for chaining)
- */
- public Quaternion negateLocal() {
- x = -x;
- y = -y;
- z = -z;
- w = -w;
-
- return this;
- }
-
- /**
- * Returns a string representation of the quaternion, which is unaffected.
- * For example, the identity quaternion is represented by:
- *
- * (0.0, 0.0, 0.0, 1.0) - *- * - * @return the string representation (not null, not empty) - */ - @Override - public String toString() { - return "(" + x + ", " + y + ", " + z + ", " + w + ")"; - } - - /** - * Tests for exact equality with the argument, distinguishing -0 from 0. If - * {@code o} is null, false is returned. Either way, the current instance is - * unaffected. - * - * @param o the object to compare (may be null, unaffected) - * @return true if {@code this} and {@code o} have identical values, - * otherwise false - */ - @Override - 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; - } - - /** - * Tests for approximate equality with the specified quaternion, using the - * specified tolerance. The current instance is unaffected. - * - *
To quantify the similarity of 2 normalized quaternions, use - * {@link #dot(com.jme3.math.Quaternion)}. - * - * @param other the quaternion to compare (not null, unaffected) - * @param epsilon the tolerance for each component - * @return true if all 4 components are within tolerance, otherwise false - */ - public boolean isSimilar(Quaternion other, float epsilon) { - if (other == null) { - return false; - } - if (Float.compare(Math.abs(other.x - x), epsilon) > 0) { - return false; - } - if (Float.compare(Math.abs(other.y - y), epsilon) > 0) { - return false; - } - if (Float.compare(Math.abs(other.z - z), epsilon) > 0) { - return false; - } - if (Float.compare(Math.abs(other.w - w), epsilon) > 0) { - return false; - } - return true; - } - - /** - * Returns a hash code. If two quaternions have identical values, they - * will have the same hash code. The current instance is unaffected. - * - * @return a 32-bit value for use in hashing - * @see java.lang.Object#hashCode() - */ - @Override - 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; - - } - - /** - * Sets the quaternion from an {@code ObjectInput} object. - * - *
Used with serialization. Should not be invoked directly by application - * code. - * - * @param in the object to read from (not null) - * @throws IOException if the ObjectInput cannot read 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(); - } - - /** - * Writes the quaternion to an {@code ObjectOutput} object. - * - *
Used with serialization. Should not be invoked directly by application - * code. - * - * @param out the object to write to (not null) - * @throws IOException if the ObjectOutput cannot write a float - * @see java.io.Externalizable - */ - public void writeExternal(ObjectOutput out) throws IOException { - out.writeFloat(x); - out.writeFloat(y); - out.writeFloat(z); - out.writeFloat(w); - } - - /** - * Convenience method to set the quaternion based on a "look" (Z-axis) - * direction and an "up" (Y-axis) direction. - * - *
If either vector has length=0, the result is undefined. - * - *
If the vectors are parallel, the result is undefined. - * - * @param direction the desired Z-axis direction (in local coordinates, not - * null, length>0, unaffected) - * @param up the desired Y-axis direction (in local coordinates, not null, - * length>0, unaffected, typically (0,1,0) ) - * @return the (modified) current instance (for chaining) - */ - public Quaternion lookAt(Vector3f direction, Vector3f up) { - TempVars vars = TempVars.get(); - vars.vect3.set(direction).normalizeLocal(); - vars.vect1.set(up).crossLocal(direction).normalizeLocal(); - vars.vect2.set(direction).crossLocal(vars.vect1).normalizeLocal(); - fromAxes(vars.vect1, vars.vect2, vars.vect3); - vars.release(); - return this; - } - - /** - * Serializes to the specified exporter, for example when saving to a J3O - * file. The current instance is unaffected. - * - * @param e the exporter to use (not null) - * @throws IOException from the exporter - */ - @Override - 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); - } - - /** - * De-serializes from the specified importer, for example when loading from - * a J3O file. - * - * @param importer the importer to use (not null) - * @throws IOException from the importer - */ - @Override - public void read(JmeImporter importer) throws IOException { - InputCapsule cap = importer.getCapsule(this); - x = cap.readFloat("x", 0); - y = cap.readFloat("y", 0); - z = cap.readFloat("z", 0); - w = cap.readFloat("w", 1); - } - - /** - * @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); - } - - /** - * Returns a rotation with the same axis and the angle increased by 180 - * degrees. If the quaternion isn't normalized, or if the rotation angle is - * very small, the result is undefined. - * - *
The current instance is unaffected, unless {@code store} is - * {@code this}. - * - * @param store storage for the result, or null for a new Quaternion - * @return either {@code store} or a new Quaternion - */ - public Quaternion opposite(Quaternion store) { - if (store == null) { - store = new Quaternion(); - } - - Vector3f axis = new Vector3f(); - float angle = toAngleAxis(axis); - - store.fromAngleAxis(FastMath.PI + angle, axis); - return store; - } - - /** - * Changes the quaternion to a rotation with the same axis and the angle - * increased by 180 degrees. If the quaternion isn't normalized, or if the - * rotation angle is very small, the result is undefined. - * - * @return the (modified) current instance - */ - public Quaternion oppositeLocal() { - return opposite(this); - } - - /** - * Creates a copy. The current instance is unaffected. - * - * @return a new instance, equivalent to the current one - */ - @Override - public Quaternion clone() { - try { - return (Quaternion) super.clone(); - } catch (CloneNotSupportedException e) { - throw new AssertionError(); // can not happen - } - } - - /** - * Tests whether the argument is a valid quaternion, returning false if it's - * null or if any component is NaN or infinite. - * - * @param quaternion the quaternion to test (unaffected) - * @return true if non-null and finite, otherwise false - */ - public static boolean isValidQuaternion(Quaternion quaternion) { - if (quaternion == null) { - return false; - } - if (Float.isNaN(quaternion.x) - || Float.isNaN(quaternion.y) - || Float.isNaN(quaternion.z) - || Float.isNaN(quaternion.w)) { - return false; - } - return !Float.isInfinite(quaternion.x) - && !Float.isInfinite(quaternion.y) - && !Float.isInfinite(quaternion.z) - && !Float.isInfinite(quaternion.w); - } -} +/* + * Copyright (c) 2009-2023 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 com.jme3.math; + +import com.jme3.export.*; +import com.jme3.util.TempVars; + +import java.io.*; +import java.util.logging.Logger; + +/** + * Used to efficiently represent rotations and orientations in 3-dimensional + * space, without risk of gimbal lock. Each instance has 4 single-precision + * components: 3 imaginary components (X, Y, and Z) and a real component (W). + * + *
Mathematically, quaternions are an extension of complex numbers. In + * mathematics texts, W often appears first, but in JME it always comes last. + * + * @author Mark Powell + * @author Joshua Slack + */ +public final class Quaternion implements Savable, Cloneable, java.io.Serializable { + + static final long serialVersionUID = 1; + + private static final Logger logger = Logger.getLogger(Quaternion.class.getName()); + /** + * Shared instance of the identity quaternion (0, 0, 0, 1). Do not modify! + * + *
This is the usual representation for a null rotation. + */ + public static final Quaternion IDENTITY = new Quaternion(); + /** + * Another shared instance of the identity quaternion (0, 0, 0, 1). Do not + * modify! + */ + public static final Quaternion DIRECTION_Z = new Quaternion(); + /** + * Shared instance of the zero quaternion (0, 0, 0, 0). Do not modify! + * + *
The zero quaternion doesn't represent any valid rotation. + */ + public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0); + + static { + DIRECTION_Z.fromAxes(Vector3f.UNIT_X, Vector3f.UNIT_Y, Vector3f.UNIT_Z); + } + /** + * The first imaginary (X) component. Not an angle! + */ + protected float x; + /** + * The 2nd imaginary (Y) component. Not an angle! + */ + protected float y; + /** + * The 3rd imaginary (Z) component. Not an angle! + */ + protected float z; + /** + * The real (W) component. Not an angle! + */ + protected float w; + + /** + * Instantiates an identity quaternion: all components zeroed except + * {@code w}, which is set to 1. + */ + public Quaternion() { + x = 0; + y = 0; + z = 0; + w = 1; + } + + /** + * Instantiates a quaternion with the specified components. + * + * @param x the desired X component + * @param y the desired Y component + * @param z the desired Z component + * @param w the desired W component + */ + public Quaternion(float x, float y, float z, float w) { + this.x = x; + this.y = y; + this.z = z; + this.w = w; + } + + /** + * Returns the X component. The quaternion is unaffected. + * + * @return the value of the {@link #x} component + */ + public float getX() { + return x; + } + + /** + * Returns the Y component. The quaternion is unaffected. + * + * @return the value of the {@link #y} component + */ + public float getY() { + return y; + } + + /** + * Returns the Z component. The quaternion is unaffected. + * + * @return the value of the {@link #z} component + */ + public float getZ() { + return z; + } + + /** + * Returns the W (real) component. The quaternion is unaffected. + * + * @return the value of the {@link #w} component + */ + public float getW() { + return w; + } + + /** + * Sets all 4 components to specified values. + * + * @param x the desired X component + * @param y the desired Y component + * @param z the desired Z component + * @param w the desired W component + * @return the (modified) current instance (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; + } + + /** + * Copies all 4 components from the argument. + * + * @param q the quaternion to copy (not null, unaffected) + * @return the (modified) current instance (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; + } + + /** + * Instantiates a quaternion from Tait-Bryan angles, applying the rotations + * in x-z-y extrinsic order or y-z'-x" intrinsic order. + * + * @param angles an array of Tait-Bryan angles (in radians, exactly 3 + * elements, the X angle in {@code angles[0]}, the Y angle in {@code + * angles[1]}, and the Z angle in {@code angles[2]}, not null, + * unaffected) + */ + public Quaternion(float[] angles) { + fromAngles(angles); + } + + /** + * Instantiates a quaternion by interpolating between the specified + * quaternions. + * + *
Uses
+ * {@link #slerp(com.jme3.math.Quaternion, com.jme3.math.Quaternion, float)},
+ * which is fast but inaccurate.
+ *
+ * @param q1 the desired value when interp=0 (not null, unaffected)
+ * @param q2 the desired value when interp=1 (not null, may be modified)
+ * @param interp the fractional change amount
+ */
+ public Quaternion(Quaternion q1, Quaternion q2, float interp) {
+ slerp(q1, q2, interp);
+ }
+
+ /**
+ * Instantiates a copy of the argument.
+ *
+ * @param q the quaternion to copy (not null, unaffected)
+ */
+ public Quaternion(Quaternion q) {
+ this.x = q.x;
+ this.y = q.y;
+ this.z = q.z;
+ this.w = q.w;
+ }
+
+ /**
+ * Sets all components to zero except {@code w}, which is set to 1.
+ */
+ public void loadIdentity() {
+ x = y = z = 0;
+ w = 1;
+ }
+
+ /**
+ * Compares with the identity quaternion, without distinguishing -0 from 0.
+ * The current instance is unaffected.
+ *
+ * @return true if the current quaternion equals the identity, otherwise
+ * false
+ */
+ public boolean isIdentity() {
+ if (x == 0 && y == 0 && z == 0 && w == 1) {
+ return true;
+ } else {
+ return false;
+ }
+ }
+
+ /**
+ * Sets the quaternion from the specified Tait-Bryan angles, applying the
+ * rotations in x-z-y extrinsic order or y-z'-x" intrinsic order.
+ *
+ * @param angles an array of Tait-Bryan angles (in radians, exactly 3
+ * elements, the X angle in {@code angles[0]}, the Y angle in {@code
+ * angles[1]}, and the Z angle in {@code angles[2]}, not null,
+ * unaffected)
+ * @return the (modified) current instance (for chaining)
+ * @throws IllegalArgumentException if {@code angles.length != 3}
+ */
+ public Quaternion fromAngles(float[] angles) {
+ if (angles.length != 3) {
+ throw new IllegalArgumentException(
+ "Angles array must have three elements");
+ }
+
+ return fromAngles(angles[0], angles[1], angles[2]);
+ }
+
+ /**
+ * Sets the quaternion from the specified Tait-Bryan angles, applying the
+ * rotations in x-z-y extrinsic order or y-z'-x" intrinsic order.
+ *
+ * @see
+ * http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
+ *
+ * @param xAngle the X angle (in radians)
+ * @param yAngle the Y angle (in radians)
+ * @param zAngle the Z angle (in radians)
+ * @return the (modified) current instance (for chaining)
+ */
+ public Quaternion fromAngles(float xAngle, float yAngle, float zAngle) {
+ float angle;
+ float sinY, sinZ, sinX, cosY, cosZ, cosX;
+ angle = zAngle * 0.5f;
+ sinZ = FastMath.sin(angle);
+ cosZ = FastMath.cos(angle);
+ angle = yAngle * 0.5f;
+ sinY = FastMath.sin(angle);
+ cosY = FastMath.cos(angle);
+ angle = xAngle * 0.5f;
+ sinX = FastMath.sin(angle);
+ cosX = FastMath.cos(angle);
+
+ // variables used to reduce multiplication calls.
+ float cosYXcosZ = cosY * cosZ;
+ float sinYXsinZ = sinY * sinZ;
+ float cosYXsinZ = cosY * sinZ;
+ float sinYXcosZ = sinY * cosZ;
+
+ w = (cosYXcosZ * cosX - sinYXsinZ * sinX);
+ x = (cosYXcosZ * sinX + sinYXsinZ * cosX);
+ y = (sinYXcosZ * cosX + cosYXsinZ * sinX);
+ z = (cosYXsinZ * cosX - sinYXcosZ * sinX);
+
+ normalizeLocal();
+ return this;
+ }
+
+ /**
+ * Converts to equivalent Tait-Bryan angles, to be applied in x-z-y
+ * intrinsic order or y-z'-x" extrinsic order, for instance by
+ * {@link #fromAngles(float[])}. The current instance is unaffected.
+ *
+ * @see
+ *
+ * http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
+ *
+ *
+ * @param angles storage for the result, or null for a new float[3]
+ * @return an array of 3 angles (in radians, either angles or a
+ * new float[3], the X angle in angles[0], the Y angle in angles[1], and
+ * the Z angle in angles[2])
+ * @throws IllegalArgumentException if {@code angles.length != 3}
+ */
+ 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 * FastMath.atan2(x, w);
+ angles[2] = FastMath.HALF_PI;
+ angles[0] = 0;
+ } else if (test < -0.499 * unit) { // singularity at south pole
+ angles[1] = -2 * FastMath.atan2(x, w);
+ angles[2] = -FastMath.HALF_PI;
+ angles[0] = 0;
+ } else {
+ angles[1] = FastMath.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // yaw or heading
+ angles[2] = FastMath.asin(2 * test / unit); // roll or bank
+ angles[0] = FastMath.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // pitch or attitude
+ }
+ return angles;
+ }
+
+ /**
+ * Sets the quaternion from the specified rotation matrix.
+ *
+ *
Does not verify that the argument is a valid rotation matrix. + * Positive scaling is compensated for, but not reflection or shear. + * + * @param matrix the input matrix (not null, unaffected) + * @return the (modified) current instance (for chaining) + */ + public Quaternion fromRotationMatrix(Matrix3f matrix) { + return fromRotationMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10, + matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22); + } + + /** + * Sets the quaternion from a rotation matrix with the specified elements. + * + *
Does not verify that the arguments form a valid rotation matrix. + * Positive scaling is compensated for, but not reflection or shear. + * + * @param m00 the matrix element in row 0, column 0 + * @param m01 the matrix element in row 0, column 1 + * @param m02 the matrix element in row 0, column 2 + * @param m10 the matrix element in row 1, column 0 + * @param m11 the matrix element in row 1, column 1 + * @param m12 the matrix element in row 1, column 2 + * @param m20 the matrix element in row 2, column 0 + * @param m21 the matrix element in row 2, column 1 + * @param m22 the matrix element in row 2, column 2 + * @return the (modified) current instance (for chaining) + */ + public Quaternion fromRotationMatrix(float m00, float m01, float m02, + float m10, float m11, float m12, float m20, float m21, float m22) { + // first normalize the forward (F), up (U) and side (S) vectors of the rotation matrix + // so that positive scaling does not affect the rotation + float lengthSquared = m00 * m00 + m10 * m10 + m20 * m20; + if (lengthSquared != 1f && lengthSquared != 0f) { + lengthSquared = 1.0f / FastMath.sqrt(lengthSquared); + m00 *= lengthSquared; + m10 *= lengthSquared; + m20 *= lengthSquared; + } + lengthSquared = m01 * m01 + m11 * m11 + m21 * m21; + if (lengthSquared != 1f && lengthSquared != 0f) { + lengthSquared = 1.0f / FastMath.sqrt(lengthSquared); + m01 *= lengthSquared; + m11 *= lengthSquared; + m21 *= lengthSquared; + } + lengthSquared = m02 * m02 + m12 * m12 + m22 * m22; + if (lengthSquared != 1f && lengthSquared != 0f) { + lengthSquared = 1.0f / FastMath.sqrt(lengthSquared); + m02 *= lengthSquared; + m12 *= lengthSquared; + m22 *= lengthSquared; + } + + // 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 = FastMath.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 = FastMath.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 = FastMath.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 = FastMath.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; + } + + /** + * Converts to an equivalent rotation matrix. The current instance is + * unaffected. + * + *
Note: the result is created from a normalized version of the current + * instance. + * + * @return a new 3x3 rotation matrix + */ + public Matrix3f toRotationMatrix() { + Matrix3f matrix = new Matrix3f(); + return toRotationMatrix(matrix); + } + + /** + * Converts to an equivalent rotation matrix. The current instance is + * unaffected. + * + *
Note: the result is created from a normalized version of the current + * instance. + * + * @param result storage for the result (not null) + * @return {@code result}, configured as a 3x3 rotation matrix + */ + 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; + } + + /** + * Sets the rotation component of the specified transform matrix. The + * current instance is unaffected. + * + *
Note: preserves the translation component of {@code store} but not + * its scaling component. + * + *
Note: the result is created from a normalized version of the current + * instance. + * + * @param store storage for the result (not null) + * @return {@code store}, with 9 of its 16 elements modified + */ + public Matrix4f toTransformMatrix(Matrix4f store) { + + 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 + store.m00 = 1 - (yy + zz); + store.m01 = (xy - zw); + store.m02 = (xz + yw); + store.m10 = (xy + zw); + store.m11 = 1 - (xx + zz); + store.m12 = (yz - xw); + store.m20 = (xz - yw); + store.m21 = (yz + xw); + store.m22 = 1 - (xx + yy); + + return store; + } + + /** + * Sets the rotation component of the specified transform matrix. The + * current instance is unaffected. + * + *
Note: preserves the translation and scaling components of + * {@code result} unless {@code result} includes reflection. + * + *
Note: the result is created from a normalized version of the current + * instance. + * + * @param result storage for the result (not null) + * @return {@code result}, with 9 of its 16 elements modified + */ + public Matrix4f toRotationMatrix(Matrix4f result) { + TempVars tempv = TempVars.get(); + Vector3f originalScale = tempv.vect1; + + result.toScaleVector(originalScale); + result.setScale(1, 1, 1); + 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); + + result.setScale(originalScale); + + tempv.release(); + + return result; + } + + /** + * Calculates one of the basis vectors of the rotation. The current instance + * is unaffected. + * + *
Note: the result is created from a normalized version of the current + * instance. + * + * @param i which basis vector to retrieve (≥0, <3, 0→X-axis, + * 1→Y-axis, 2→Z-axis) + * @return the basis vector (a new Vector3f) + */ + public Vector3f getRotationColumn(int i) { + return getRotationColumn(i, null); + } + + /** + * Calculates one of the basis vectors of the rotation. The current instance + * is unaffected. + * + *
Note: the result is created from a normalized version of the current
+ * instance.
+ *
+ * @param i which basis vector to retrieve (≥0, <3, 0→X-axis,
+ * 1→Y-axis, 2→Z-axis)
+ * @param store storage for the result, or null for a new Vector3f
+ * @return the basis vector (either store or a new Vector3f)
+ * @throws IllegalArgumentException if index is not 0, 1, or 2
+ */
+ public Vector3f getRotationColumn(int i, Vector3f store) {
+ if (store == null) {
+ store = new Vector3f();
+ }
+
+ float norm = norm();
+ if (norm != 1.0f) {
+ norm = 1.0f / 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 IllegalArgumentException("Invalid column index. " + i);
+ }
+
+ return store;
+ }
+
+ /**
+ * Sets the quaternion from the specified rotation angle and axis of
+ * rotation. This method creates garbage, so use
+ * {@link #fromAngleNormalAxis(float, com.jme3.math.Vector3f)} if the axis
+ * is known to be normalized.
+ *
+ * @param angle the desired rotation angle (in radians)
+ * @param axis the desired axis of rotation (not null, unaffected)
+ * @return the (modified) current instance (for chaining)
+ */
+ public Quaternion fromAngleAxis(float angle, Vector3f axis) {
+ Vector3f normAxis = axis.normalize();
+ fromAngleNormalAxis(angle, normAxis);
+ return this;
+ }
+
+ /**
+ * Sets the quaternion from the specified rotation angle and normalized axis
+ * of rotation. If the axis might not be normalized, use
+ * {@link #fromAngleAxis(float, com.jme3.math.Vector3f)} instead.
+ *
+ * @param angle the desired rotation angle (in radians)
+ * @param axis the desired axis of rotation (not null, length=1, unaffected)
+ * @return the (modified) current instance (for chaining)
+ */
+ public Quaternion fromAngleNormalAxis(float angle, Vector3f axis) {
+ if (axis.x == 0 && axis.y == 0 && axis.z == 0) {
+ loadIdentity();
+ } else {
+ float halfAngle = 0.5f * angle;
+ float sin = FastMath.sin(halfAngle);
+ w = FastMath.cos(halfAngle);
+ x = sin * axis.x;
+ y = sin * axis.y;
+ z = sin * axis.z;
+ }
+ return this;
+ }
+
+ /**
+ * Converts the quaternion to a rotation angle and axis of rotation, storing
+ * the axis in the argument (if it's non-null) and returning the angle.
+ *
+ *
If the quaternion has {@code x*x + y*y + z*z == 0}, then (1,0,0) is + * stored and 0 is returned. (This might happen if the rotation angle is + * very close to 0.) + * + *
Otherwise, the quaternion is assumed to be normalized (norm=1). No + * error checking is performed; the caller must ensure that the quaternion + * is normalized. + * + *
In all cases, the current instance is unaffected. + * + * @param axisStore storage for the axis (modified if not null) + * @return the rotation angle (in radians) + */ + public float toAngleAxis(Vector3f 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 * FastMath.acos(w)); + if (axisStore != null) { + float invLength = (1.0f / FastMath.sqrt(sqrLength)); + axisStore.x = x * invLength; + axisStore.y = y * invLength; + axisStore.z = z * invLength; + } + } + + return angle; + } + + /** + * Interpolates between the specified quaternions and stores the result in + * the current instance. + * + * @param q1 the desired value when interp=0 (not null, unaffected) + * @param q2 the desired value when interp=1 (not null, may be modified) + * @param t the fractional change amount + * @return the (modified) current instance (for chaining) + */ + 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 = FastMath.acos(result); + float invSinTheta = 1f / FastMath.sin(theta); + + // Calculate the scale for q1 and q2, according to the angle and + // its sine + scale0 = FastMath.sin((1 - t) * theta) * invSinTheta; + scale1 = FastMath.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; + } + + /** + * Interpolates between the current instance and {@code q2} and stores the + * result in the current instance. + * + *
This method is often more accurate than + * {@link #nlerp(com.jme3.math.Quaternion, float)}, but slower. + + * @param q2 the desired value when changeAmnt=1 (not null, may be modified) + * @param changeAmount the fractional change amount + */ + public void slerp(Quaternion q2, float changeAmount) { + 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 - changeAmount; + float scale1 = changeAmount; + + // 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 = FastMath.acos(result); + float invSinTheta = 1f / FastMath.sin(theta); + + // Calculate the scale for q1 and q2, according to the angle and + // its sine + scale0 = FastMath.sin((1 - changeAmount) * theta) * invSinTheta; + scale1 = FastMath.sin((changeAmount * 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); + } + + /** + * Interpolates quickly between the current instance and {@code q2} using + * nlerp, and stores the result in the current instance. + * + *
This method is often faster than + * {@link #slerp(com.jme3.math.Quaternion, float)}, but less accurate. + * + * @param q2 the desired value when blend=1 (not null, unaffected) + * @param blend the fractional change amount + */ + public void nlerp(Quaternion q2, float blend) { + float dot = dot(q2); + float blendI = 1.0f - blend; + if (dot < 0.0f) { + x = blendI * x - blend * q2.x; + y = blendI * y - blend * q2.y; + z = blendI * z - blend * q2.z; + w = blendI * w - blend * q2.w; + } else { + x = blendI * x + blend * q2.x; + y = blendI * y + blend * q2.y; + z = blendI * z + blend * q2.z; + w = blendI * w + blend * q2.w; + } + normalizeLocal(); + } + + /** + * Adds the argument and returns the sum as a new instance. The current + * instance is unaffected. + * + *
Seldom used. To combine rotations, use + * {@link #mult(com.jme3.math.Quaternion)} instead of this method. + * + * @param q the quaternion to add (not null, unaffected) + * @return a new Quaternion + */ + public Quaternion add(Quaternion q) { + return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w); + } + + /** + * Adds the argument and returns the (modified) current instance. + * + *
Seldom used. To combine rotations, use + * {@link #multLocal(com.jme3.math.Quaternion)} or + * {@link #mult(com.jme3.math.Quaternion, com.jme3.math.Quaternion)} + * instead of this method. + * + * @param q the quaternion to add (not null, unaffected unless it's + * {@code this}) + * @return the (modified) current instance (for chaining) + */ + public Quaternion addLocal(Quaternion q) { + this.x += q.x; + this.y += q.y; + this.z += q.z; + this.w += q.w; + return this; + } + + /** + * Subtracts the argument and returns difference as a new instance. The + * current instance is unaffected. + * + * @param q the quaternion to subtract (not null, unaffected) + * @return a new Quaternion + */ + public Quaternion subtract(Quaternion q) { + return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w); + } + + /** + * Subtracts the argument and returns the (modified) current instance. + * + *
To quantify the similarity of 2 normalized quaternions, use + * {@link #dot(com.jme3.math.Quaternion)}. + * + * @param q the quaternion to subtract (not null, unaffected unless it's + * {@code this}) + * @return the (modified) current instance + */ + public Quaternion subtractLocal(Quaternion q) { + this.x -= q.x; + this.y -= q.y; + this.z -= q.z; + this.w -= q.w; + return this; + } + + /** + * Multiplies by the argument and returns the product as a new instance. + * The current instance is unaffected. + * + *
This method is used to combine rotations. Note that quaternion + * multiplication is noncommutative, so generally q * p != p * q. + * + * @param q the right factor (not null, unaffected) + * @return {@code this * q} (a new Quaternion) + */ + public Quaternion mult(Quaternion q) { + return mult(q, null); + } + + /** + * Multiplies by the specified quaternion and returns the product in a 3rd + * quaternion. The current instance is unaffected, unless it's {@code storeResult}. + * + *
This method is used to combine rotations. Note that quaternion + * multiplication is noncommutative, so generally q * p != p * q. + * + *
It is safe for {@code q} and {@code storeResult} to be the same object. + * However, if {@code this} and {@code storeResult} are the same object, the result + * is undefined. + * + * @param q the right factor (not null, unaffected unless it's {@code storeResult}) + * @param storeResult storage for the product, or null for a new Quaternion + * @return {@code this * q} (either {@code storeResult} or a new Quaternion) + */ + public Quaternion mult(Quaternion q, Quaternion storeResult) { + if (storeResult == null) { + storeResult = new Quaternion(); + } + float qw = q.w, qx = q.x, qy = q.y, qz = q.z; + storeResult.x = x * qw + y * qz - z * qy + w * qx; + storeResult.y = -x * qz + y * qw + z * qx + w * qy; + storeResult.z = x * qy - y * qx + z * qw + w * qz; + storeResult.w = -x * qx - y * qy - z * qz + w * qw; + return storeResult; + } + + /** + * Applies the rotation represented by the argument to the current instance. + * + *
Does not verify that {@code matrix} is a valid rotation matrix. + * Positive scaling is compensated for, but not reflection or shear. + * + * @param matrix the rotation matrix to apply (not null, unaffected) + */ + public void apply(Matrix3f 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; + } + + /** + * Sets the quaternion from the specified orthonormal basis. + * + *
The 3 basis vectors describe the axes of a rotated coordinate system. + * They are assumed to be normalized, mutually orthogonal, and in right-hand + * order. No error checking is performed; the caller must ensure that the + * specified vectors represent a right-handed coordinate system. + * + * @param axis the array of desired basis vectors (not null, array length=3, + * each vector having length=1, unaffected) + * @return the (modified) current instance (for chaining) + * @throws IllegalArgumentException if {@code axis.length != 3} + */ + public Quaternion fromAxes(Vector3f[] axis) { + if (axis.length != 3) { + throw new IllegalArgumentException( + "Axis array must have three elements"); + } + return fromAxes(axis[0], axis[1], axis[2]); + } + + /** + * Sets the quaternion from the specified orthonormal basis. + * + *
The 3 basis vectors describe the axes of a rotated coordinate system. + * They are assumed to be normalized, mutually orthogonal, and in right-hand + * order. No error checking is performed; the caller must ensure that the + * specified vectors represent a right-handed coordinate system. + * + * @param xAxis the X axis of the desired coordinate system (not null, + * length=1, unaffected) + * @param yAxis the Y axis of the desired coordinate system (not null, + * length=1, unaffected) + * @param zAxis the Z axis of the desired coordinate system (not null, + * length=1, unaffected) + * @return the (modified) current instance (for chaining) + */ + public Quaternion fromAxes(Vector3f xAxis, Vector3f yAxis, Vector3f zAxis) { + return fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y, + zAxis.y, xAxis.z, yAxis.z, zAxis.z); + } + + /** + * Converts the quaternion to a rotated coordinate system and stores the + * resulting axes in the argument. The current instance is unaffected. + * + *
The resulting vectors form the basis of a rotated coordinate system. + * They will be normalized, mutually orthogonal, and in right-hand order. + * + * @param axes storage for the results (not null, length=3, each element + * non-null, elements modified) + * @throws IllegalArgumentException if {@code axes.length != 3} + */ + public void toAxes(Vector3f axes[]) { + if (axes.length != 3) { + throw new IllegalArgumentException( + "Axes array must have three elements"); + } + + Matrix3f tempMat = toRotationMatrix(); + axes[0] = tempMat.getColumn(0, axes[0]); + axes[1] = tempMat.getColumn(1, axes[1]); + axes[2] = tempMat.getColumn(2, axes[2]); + } + + /** + * Rotates the argument vector and returns the result as a new vector. The + * current instance is unaffected. + * + *
The quaternion is assumed to be normalized (norm=1). No error checking + * is performed; the caller must ensure that the norm is approximately equal + * to 1. + * + *
Despite the name, the result differs from the mathematical definition + * of vector-quaternion multiplication. + * + * @param v the vector to rotate (not null, unaffected) + * @return a new Vector3f + */ + public Vector3f mult(Vector3f v) { + return mult(v, null); + } + + /** + * Rotates the argument vector. Despite the name, the current instance is + * unaffected. + * + *
The quaternion is assumed to be normalized (norm=1). No error checking + * is performed; the caller must ensure that the norm is approximately equal + * to 1. + * + *
Despite the name, the result differs from the mathematical definition + * of vector-quaternion multiplication. + * + * @param v the vector to rotate (not null) + * @return the (modified) vector {@code v} + */ + public Vector3f multLocal(Vector3f 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 by the argument and returns the (modified) current instance. + * + *
This method is used to combine rotations. Note that quaternion + * multiplication is noncommutative, so generally q * p != p * q. + * + * @param q the right factor (not null, unaffected unless it's {@code this}) + * @return the (modified) current instance (for chaining) + */ + 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 by a quaternion with the specified components and returns the + * (modified) current instance. + * + *
This method is used to combine rotations. Note that quaternion + * multiplication is noncommutative, so generally q * p != p * q. + * + * @param qx the X component of the right factor + * @param qy the Y component of the right factor + * @param qz the Z component of the right factor + * @param qw the W component of the right factor + * @return the (modified) current instance (for chaining) + */ + 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; + } + + /** + * Rotates a specified vector and returns the result in another vector. The + * current instance is unaffected. + * + *
The quaternion is assumed to be normalized (norm=1). No error checking + * is performed; the caller must ensure that the norm is approximately equal + * to 1. + * + *
It is safe for {@code v} and {@code store} to be the same object. + * + *
Despite the name, the result differs from the mathematical definition + * of vector-quaternion multiplication. + * + * @param v the vector to rotate (not null, unaffected unless it's + * {@code store}) + * @param store storage for the result, or null for a new Vector3f + * @return the rotated vector (either {@code store} or a new Vector3f) + */ + public Vector3f mult(Vector3f v, Vector3f store) { + if (store == null) { + store = new Vector3f(); + } + if (v.x == 0 && v.y == 0 && v.z == 0) { + store.set(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; + } + + /** + * Multiplies with the scalar argument and returns the product as a new + * instance. The current instance is unaffected. + * + * @param scalar the scaling factor + * @return a new Quaternion + */ + public Quaternion mult(float scalar) { + return new Quaternion(scalar * x, scalar * y, scalar * z, scalar * w); + } + + /** + * Multiplies by the scalar argument and returns the (modified) current + * instance. + * + * @param scalar the scaling factor + * @return the (modified) current instance (for chaining) + */ + public Quaternion multLocal(float scalar) { + w *= scalar; + x *= scalar; + y *= scalar; + z *= scalar; + return this; + } + + /** + * Returns the dot product with the argument. The current instance is + * unaffected. + * + *
This method can be used to quantify the similarity of 2 normalized
+ * quaternions.
+ *
+ * @param q the quaternion to multiply (not null, unaffected)
+ * @return the dot product
+ */
+ public float dot(Quaternion q) {
+ return w * q.w + x * q.x + y * q.y + z * q.z;
+ }
+
+ /**
+ * Returns the norm, defined as the dot product of the quaternion with
+ * itself. The current instance is unaffected.
+ *
+ * @return the sum of the squared components (not negative)
+ */
+ public float norm() {
+ return w * w + x * x + y * y + z * z;
+ }
+
+// /**
+// * normalize normalizes the current Quaternion
+// * @deprecated The naming of this method doesn't follow convention.
+// * Please use {@link Quaternion#normalizeLocal() } instead.
+// */
+// @Deprecated
+// public void normalize() {
+// float n = FastMath.invSqrt(norm());
+// x *= n;
+// y *= n;
+// z *= n;
+// w *= n;
+// }
+
+ /**
+ * Scales the quaternion to have norm=1 and returns the (modified) current
+ * instance. For a quaternion with norm=0, the result is undefined.
+ *
+ * @return the (modified) current instance (for chaining)
+ */
+ public Quaternion normalizeLocal() {
+ float n = FastMath.invSqrt(norm());
+ x *= n;
+ y *= n;
+ z *= n;
+ w *= n;
+ return this;
+ }
+
+ /**
+ * Returns the multiplicative inverse. For a quaternion with norm=0, null is
+ * returned. Either way, the current instance is unaffected.
+ *
+ * @return a new Quaternion or null
+ */
+ 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;
+ }
+
+ /**
+ * Inverts the quaternion and returns the (modified) current instance. For
+ * a quaternion with norm=0, the current instance is unchanged.
+ *
+ * @return the current instance (for chaining)
+ */
+ 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;
+ }
+
+ /**
+ * Negates all 4 components.
+ *
+ * @deprecated The naming of this method doesn't follow convention. Please
+ * use {@link #negateLocal()} instead.
+ */
+ @Deprecated
+ public void negate() {
+ negateLocal();
+ }
+
+ /**
+ * Negates all 4 components and returns the (modified) current instance.
+ *
+ * @return the (modified) current instance (for chaining)
+ */
+ public Quaternion negateLocal() {
+ x = -x;
+ y = -y;
+ z = -z;
+ w = -w;
+
+ return this;
+ }
+
+ /**
+ * Returns a string representation of the quaternion, which is unaffected.
+ * For example, the identity quaternion is represented by:
+ *
+ * (0.0, 0.0, 0.0, 1.0) + *+ * + * @return the string representation (not null, not empty) + */ + @Override + public String toString() { + return "(" + x + ", " + y + ", " + z + ", " + w + ")"; + } + + /** + * Tests for exact equality with the argument, distinguishing -0 from 0. If + * {@code o} is null, false is returned. Either way, the current instance is + * unaffected. + * + * @param o the object to compare (may be null, unaffected) + * @return true if {@code this} and {@code o} have identical values, + * otherwise false + */ + @Override + 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; + } + + /** + * Tests for approximate equality with the specified quaternion, using the + * specified tolerance. The current instance is unaffected. + * + *
To quantify the similarity of 2 normalized quaternions, use + * {@link #dot(com.jme3.math.Quaternion)}. + * + * @param other the quaternion to compare (not null, unaffected) + * @param epsilon the tolerance for each component + * @return true if all 4 components are within tolerance, otherwise false + */ + public boolean isSimilar(Quaternion other, float epsilon) { + if (other == null) { + return false; + } + if (Float.compare(Math.abs(other.x - x), epsilon) > 0) { + return false; + } + if (Float.compare(Math.abs(other.y - y), epsilon) > 0) { + return false; + } + if (Float.compare(Math.abs(other.z - z), epsilon) > 0) { + return false; + } + if (Float.compare(Math.abs(other.w - w), epsilon) > 0) { + return false; + } + return true; + } + + /** + * Returns a hash code. If two quaternions have identical values, they + * will have the same hash code. The current instance is unaffected. + * + * @return a 32-bit value for use in hashing + * @see java.lang.Object#hashCode() + */ + @Override + 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; + + } + + /** + * Sets the quaternion from an {@code ObjectInput} object. + * + *
Used with serialization. Should not be invoked directly by application + * code. + * + * @param in the object to read from (not null) + * @throws IOException if the ObjectInput cannot read 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(); + } + + /** + * Writes the quaternion to an {@code ObjectOutput} object. + * + *
Used with serialization. Should not be invoked directly by application + * code. + * + * @param out the object to write to (not null) + * @throws IOException if the ObjectOutput cannot write a float + * @see java.io.Externalizable + */ + public void writeExternal(ObjectOutput out) throws IOException { + out.writeFloat(x); + out.writeFloat(y); + out.writeFloat(z); + out.writeFloat(w); + } + + /** + * Convenience method to set the quaternion based on a "look" (Z-axis) + * direction and an "up" (Y-axis) direction. + * + *
If either vector has length=0, the result is undefined. + * + *
If the vectors are parallel, the result is undefined. + * + * @param direction the desired Z-axis direction (in local coordinates, not + * null, length>0, unaffected) + * @param up the desired Y-axis direction (in local coordinates, not null, + * length>0, unaffected, typically (0,1,0) ) + * @return the (modified) current instance (for chaining) + */ + public Quaternion lookAt(Vector3f direction, Vector3f up) { + TempVars vars = TempVars.get(); + vars.vect3.set(direction).normalizeLocal(); + vars.vect1.set(up).crossLocal(direction).normalizeLocal(); + vars.vect2.set(direction).crossLocal(vars.vect1).normalizeLocal(); + fromAxes(vars.vect1, vars.vect2, vars.vect3); + vars.release(); + return this; + } + + /** + * Serializes to the specified exporter, for example when saving to a J3O + * file. The current instance is unaffected. + * + * @param e the exporter to use (not null) + * @throws IOException from the exporter + */ + @Override + 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); + } + + /** + * De-serializes from the specified importer, for example when loading from + * a J3O file. + * + * @param importer the importer to use (not null) + * @throws IOException from the importer + */ + @Override + public void read(JmeImporter importer) throws IOException { + InputCapsule cap = importer.getCapsule(this); + x = cap.readFloat("x", 0); + y = cap.readFloat("y", 0); + z = cap.readFloat("z", 0); + w = cap.readFloat("w", 1); + } + + /** + * @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); + } + + /** + * Returns a rotation with the same axis and the angle increased by 180 + * degrees. If the quaternion isn't normalized, or if the rotation angle is + * very small, the result is undefined. + * + *
The current instance is unaffected, unless {@code store} is
+ * {@code this}.
+ *
+ * @param store storage for the result, or null for a new Quaternion
+ * @return either {@code store} or a new Quaternion
+ */
+ public Quaternion opposite(Quaternion store) {
+ if (store == null) {
+ store = new Quaternion();
+ }
+
+ Vector3f axis = new Vector3f();
+ float angle = toAngleAxis(axis);
+
+ store.fromAngleAxis(FastMath.PI + angle, axis);
+ return store;
+ }
+
+ /**
+ * Changes the quaternion to a rotation with the same axis and the angle
+ * increased by 180 degrees. If the quaternion isn't normalized, or if the
+ * rotation angle is very small, the result is undefined.
+ *
+ * @return the (modified) current instance
+ */
+ public Quaternion oppositeLocal() {
+ return opposite(this);
+ }
+
+ /**
+ * Creates a copy. The current instance is unaffected.
+ *
+ * @return a new instance, equivalent to the current one
+ */
+ @Override
+ public Quaternion clone() {
+ try {
+ return (Quaternion) super.clone();
+ } catch (CloneNotSupportedException e) {
+ throw new AssertionError(); // can not happen
+ }
+ }
+
+ /**
+ * Tests whether the argument is a valid quaternion, returning false if it's
+ * null or if any component is NaN or infinite.
+ *
+ * @param quaternion the quaternion to test (unaffected)
+ * @return true if non-null and finite, otherwise false
+ */
+ public static boolean isValidQuaternion(Quaternion quaternion) {
+ if (quaternion == null) {
+ return false;
+ }
+ if (Float.isNaN(quaternion.x)
+ || Float.isNaN(quaternion.y)
+ || Float.isNaN(quaternion.z)
+ || Float.isNaN(quaternion.w)) {
+ return false;
+ }
+ return !Float.isInfinite(quaternion.x)
+ && !Float.isInfinite(quaternion.y)
+ && !Float.isInfinite(quaternion.z)
+ && !Float.isInfinite(quaternion.w);
+ }
+}
diff --git a/jme3-core/src/main/java/com/jme3/util/BufferUtils.java b/jme3-core/src/main/java/com/jme3/util/BufferUtils.java
index 8ed6d9eb78..a6406cd6fd 100644
--- a/jme3-core/src/main/java/com/jme3/util/BufferUtils.java
+++ b/jme3-core/src/main/java/com/jme3/util/BufferUtils.java
@@ -1,1367 +1,1367 @@
-/*
- * Copyright (c) 2009-2021 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 com.jme3.util;
-
-import com.jme3.math.ColorRGBA;
-import com.jme3.math.Quaternion;
-import com.jme3.math.Vector2f;
-import com.jme3.math.Vector3f;
-import com.jme3.math.Vector4f;
-
-import java.io.UnsupportedEncodingException;
-import java.lang.ref.PhantomReference;
-import java.lang.ref.Reference;
-import java.lang.ref.ReferenceQueue;
-import java.nio.Buffer;
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-import java.nio.DoubleBuffer;
-import java.nio.FloatBuffer;
-import java.nio.IntBuffer;
-import java.nio.ShortBuffer;
-import java.util.concurrent.ConcurrentHashMap;
-
-/**
- * BufferUtils is a helper class for generating nio buffers from
- * jME data classes such as Vectors and ColorRGBA.
- *
- * @author Joshua Slack
- * @version $Id: BufferUtils.java,v 1.16 2007/10/29 16:56:18 nca Exp $
- */
-public final class BufferUtils {
-
- /**
- * Should be final for thread safety.
- */
- private static final BufferAllocator allocator = BufferAllocatorFactory.create();
-
- private static boolean trackDirectMemory = false;
- private static final ReferenceQueuebyte
- *
- * @param array
- * The array
- * @return The buffer
- */
- public static Buffer createByteBuffer(int[] array) {
- ByteBuffer buffer = BufferUtils.createByteBuffer(array.length);
- for (int i = 0; i < array.length; i++) {
- buffer.put(i, (byte) array[i]);
- }
- return buffer;
- }
-
- /**
- * Create a short buffer containing the given values, cast to short
- *
- * @param array
- * The array
- * @return The buffer
- */
- public static Buffer createShortBuffer(int[] array) {
- ShortBuffer buffer = BufferUtils.createShortBuffer(array.length);
- for (int i = 0; i < array.length; i++) {
- buffer.put(i, (short) array[i]);
- }
- return buffer;
- }
-
- /**
- * Ensures there is at least the required number of entries
- * left after the current position of the buffer. If the buffer is too small
- * a larger one is created and the old one copied to the new buffer.
- *
- * @param buffer
- * buffer that should be checked/copied (may be null)
- * @param required
- * minimum number of elements that should be remaining in the
- * returned buffer
- * @return a buffer large enough to receive at least the
- * required number of entries, same position as the
- * input buffer, not null
- */
- public static FloatBuffer ensureLargeEnough(FloatBuffer buffer, int required) {
- if (buffer != null) {
- buffer.limit(buffer.capacity());
- }
- if (buffer == null || (buffer.remaining() < required)) {
- int position = (buffer != null ? buffer.position() : 0);
- FloatBuffer newVerts = createFloatBuffer(position + required);
- if (buffer != null) {
- buffer.flip();
- newVerts.put(buffer);
- newVerts.position(position);
- }
- buffer = newVerts;
- }
- return buffer;
- }
-
- public static IntBuffer ensureLargeEnough(IntBuffer buffer, int required) {
- if (buffer != null) {
- buffer.limit(buffer.capacity());
- }
- if (buffer == null || (buffer.remaining() < required)) {
- int position = (buffer != null ? buffer.position() : 0);
- IntBuffer newVerts = createIntBuffer(position + required);
- if (buffer != null) {
- buffer.flip();
- newVerts.put(buffer);
- newVerts.position(position);
- }
- buffer = newVerts;
- }
- return buffer;
- }
-
- public static ShortBuffer ensureLargeEnough(ShortBuffer buffer, int required) {
- if (buffer != null) {
- buffer.limit(buffer.capacity());
- }
- if (buffer == null || (buffer.remaining() < required)) {
- int position = (buffer != null ? buffer.position() : 0);
- ShortBuffer newVerts = createShortBuffer(position + required);
- if (buffer != null) {
- buffer.flip();
- newVerts.put(buffer);
- newVerts.position(position);
- }
- buffer = newVerts;
- }
- return buffer;
- }
-
- public static ByteBuffer ensureLargeEnough(ByteBuffer buffer, int required) {
- if (buffer != null) {
- buffer.limit(buffer.capacity());
- }
- if (buffer == null || (buffer.remaining() < required)) {
- int position = (buffer != null ? buffer.position() : 0);
- ByteBuffer newVerts = createByteBuffer(position + required);
- if (buffer != null) {
- buffer.flip();
- newVerts.put(buffer);
- newVerts.position(position);
- }
- buffer = newVerts;
- }
- return buffer;
- }
-
- public static void printCurrentDirectMemory(StringBuilder store) {
- long totalHeld = 0;
- long heapMem = Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();
-
- boolean printStout = store == null;
- if (store == null) {
- store = new StringBuilder();
- }
- if (trackDirectMemory) {
- // make a new set to hold the keys to prevent concurrency issues.
- int fBufs = 0, bBufs = 0, iBufs = 0, sBufs = 0, dBufs = 0;
- int fBufsM = 0, bBufsM = 0, iBufsM = 0, sBufsM = 0, dBufsM = 0;
- for (BufferInfo b : BufferUtils.trackedBuffers.values()) {
- if (b.type == ByteBuffer.class) {
- totalHeld += b.size;
- bBufsM += b.size;
- bBufs++;
- } else if (b.type == FloatBuffer.class) {
- totalHeld += b.size;
- fBufsM += b.size;
- fBufs++;
- } else if (b.type == IntBuffer.class) {
- totalHeld += b.size;
- iBufsM += b.size;
- iBufs++;
- } else if (b.type == ShortBuffer.class) {
- totalHeld += b.size;
- sBufsM += b.size;
- sBufs++;
- } else if (b.type == DoubleBuffer.class) {
- totalHeld += b.size;
- dBufsM += b.size;
- dBufs++;
- }
- }
-
- store.append("Existing buffers: ").append(BufferUtils.trackedBuffers.size()).append("\n");
- store.append("(b: ").append(bBufs).append(" f: ").append(fBufs).append(" i: ").append(iBufs)
- .append(" s: ").append(sBufs).append(" d: ").append(dBufs).append(")").append("\n");
- store.append("Total heap memory held: ").append(heapMem / 1024).append("kb\n");
- store.append("Total direct memory held: ").append(totalHeld / 1024).append("kb\n");
- store.append("(b: ").append(bBufsM / 1024).append("kb f: ").append(fBufsM / 1024).append("kb i: ")
- .append(iBufsM / 1024).append("kb s: ").append(sBufsM / 1024).append("kb d: ")
- .append(dBufsM / 1024).append("kb)").append("\n");
- } else {
- store.append("Total heap memory held: ").append(heapMem / 1024).append("kb\n");
- store.append(
- "Only heap memory available, if you want to monitor direct memory use BufferUtils.setTrackDirectMemoryEnabled(true) during initialization.")
- .append("\n");
- }
- if (printStout) {
- System.out.println(store.toString());
- }
- }
-
- /**
- * Direct buffers are garbage collected by using a phantom reference and a
- * reference queue. Every once a while, the JVM checks the reference queue
- * and cleans the direct buffers. However, as this doesn't happen
- * immediately after discarding all references to a direct buffer, it's easy
- * to OutOfMemoryError yourself using direct buffers.
- *
- * @param toBeDestroyed the buffer to de-allocate (not null)
- */
- public static void destroyDirectBuffer(Buffer toBeDestroyed) {
- if (!isDirect(toBeDestroyed)) {
- return;
- }
- allocator.destroyDirectBuffer(toBeDestroyed);
- }
-
- /**
- * Test whether the specified buffer is direct.
- *
- * @param buf the buffer to test (not null, unaffected)
- * @return true if direct, otherwise false
- */
- private static boolean isDirect(Buffer buf) {
- return buf.isDirect();
- }
-
- private static class BufferInfo extends PhantomReferenceBufferUtils is a helper class for generating nio buffers from
+ * jME data classes such as Vectors and ColorRGBA.
+ *
+ * @author Joshua Slack
+ * @version $Id: BufferUtils.java,v 1.16 2007/10/29 16:56:18 nca Exp $
+ */
+public final class BufferUtils {
+
+ /**
+ * Should be final for thread safety.
+ */
+ private static final BufferAllocator allocator = BufferAllocatorFactory.create();
+
+ private static boolean trackDirectMemory = false;
+ private static final ReferenceQueuebyte
+ *
+ * @param array
+ * The array
+ * @return The buffer
+ */
+ public static Buffer createByteBuffer(int[] array) {
+ ByteBuffer buffer = BufferUtils.createByteBuffer(array.length);
+ for (int i = 0; i < array.length; i++) {
+ buffer.put(i, (byte) array[i]);
+ }
+ return buffer;
+ }
+
+ /**
+ * Create a short buffer containing the given values, cast to short
+ *
+ * @param array
+ * The array
+ * @return The buffer
+ */
+ public static Buffer createShortBuffer(int[] array) {
+ ShortBuffer buffer = BufferUtils.createShortBuffer(array.length);
+ for (int i = 0; i < array.length; i++) {
+ buffer.put(i, (short) array[i]);
+ }
+ return buffer;
+ }
+
+ /**
+ * Ensures there is at least the required number of entries
+ * left after the current position of the buffer. If the buffer is too small
+ * a larger one is created and the old one copied to the new buffer.
+ *
+ * @param buffer
+ * buffer that should be checked/copied (may be null)
+ * @param required
+ * minimum number of elements that should be remaining in the
+ * returned buffer
+ * @return a buffer large enough to receive at least the
+ * required number of entries, same position as the
+ * input buffer, not null
+ */
+ public static FloatBuffer ensureLargeEnough(FloatBuffer buffer, int required) {
+ if (buffer != null) {
+ buffer.limit(buffer.capacity());
+ }
+ if (buffer == null || (buffer.remaining() < required)) {
+ int position = (buffer != null ? buffer.position() : 0);
+ FloatBuffer newVerts = createFloatBuffer(position + required);
+ if (buffer != null) {
+ buffer.flip();
+ newVerts.put(buffer);
+ newVerts.position(position);
+ }
+ buffer = newVerts;
+ }
+ return buffer;
+ }
+
+ public static IntBuffer ensureLargeEnough(IntBuffer buffer, int required) {
+ if (buffer != null) {
+ buffer.limit(buffer.capacity());
+ }
+ if (buffer == null || (buffer.remaining() < required)) {
+ int position = (buffer != null ? buffer.position() : 0);
+ IntBuffer newVerts = createIntBuffer(position + required);
+ if (buffer != null) {
+ buffer.flip();
+ newVerts.put(buffer);
+ newVerts.position(position);
+ }
+ buffer = newVerts;
+ }
+ return buffer;
+ }
+
+ public static ShortBuffer ensureLargeEnough(ShortBuffer buffer, int required) {
+ if (buffer != null) {
+ buffer.limit(buffer.capacity());
+ }
+ if (buffer == null || (buffer.remaining() < required)) {
+ int position = (buffer != null ? buffer.position() : 0);
+ ShortBuffer newVerts = createShortBuffer(position + required);
+ if (buffer != null) {
+ buffer.flip();
+ newVerts.put(buffer);
+ newVerts.position(position);
+ }
+ buffer = newVerts;
+ }
+ return buffer;
+ }
+
+ public static ByteBuffer ensureLargeEnough(ByteBuffer buffer, int required) {
+ if (buffer != null) {
+ buffer.limit(buffer.capacity());
+ }
+ if (buffer == null || (buffer.remaining() < required)) {
+ int position = (buffer != null ? buffer.position() : 0);
+ ByteBuffer newVerts = createByteBuffer(position + required);
+ if (buffer != null) {
+ buffer.flip();
+ newVerts.put(buffer);
+ newVerts.position(position);
+ }
+ buffer = newVerts;
+ }
+ return buffer;
+ }
+
+ public static void printCurrentDirectMemory(StringBuilder store) {
+ long totalHeld = 0;
+ long heapMem = Runtime.getRuntime().totalMemory() - Runtime.getRuntime().freeMemory();
+
+ boolean printStout = store == null;
+ if (store == null) {
+ store = new StringBuilder();
+ }
+ if (trackDirectMemory) {
+ // make a new set to hold the keys to prevent concurrency issues.
+ int fBufs = 0, bBufs = 0, iBufs = 0, sBufs = 0, dBufs = 0;
+ int fBufsM = 0, bBufsM = 0, iBufsM = 0, sBufsM = 0, dBufsM = 0;
+ for (BufferInfo b : BufferUtils.trackedBuffers.values()) {
+ if (b.type == ByteBuffer.class) {
+ totalHeld += b.size;
+ bBufsM += b.size;
+ bBufs++;
+ } else if (b.type == FloatBuffer.class) {
+ totalHeld += b.size;
+ fBufsM += b.size;
+ fBufs++;
+ } else if (b.type == IntBuffer.class) {
+ totalHeld += b.size;
+ iBufsM += b.size;
+ iBufs++;
+ } else if (b.type == ShortBuffer.class) {
+ totalHeld += b.size;
+ sBufsM += b.size;
+ sBufs++;
+ } else if (b.type == DoubleBuffer.class) {
+ totalHeld += b.size;
+ dBufsM += b.size;
+ dBufs++;
+ }
+ }
+
+ store.append("Existing buffers: ").append(BufferUtils.trackedBuffers.size()).append("\n");
+ store.append("(b: ").append(bBufs).append(" f: ").append(fBufs).append(" i: ").append(iBufs)
+ .append(" s: ").append(sBufs).append(" d: ").append(dBufs).append(")").append("\n");
+ store.append("Total heap memory held: ").append(heapMem / 1024).append("kb\n");
+ store.append("Total direct memory held: ").append(totalHeld / 1024).append("kb\n");
+ store.append("(b: ").append(bBufsM / 1024).append("kb f: ").append(fBufsM / 1024).append("kb i: ")
+ .append(iBufsM / 1024).append("kb s: ").append(sBufsM / 1024).append("kb d: ")
+ .append(dBufsM / 1024).append("kb)").append("\n");
+ } else {
+ store.append("Total heap memory held: ").append(heapMem / 1024).append("kb\n");
+ store.append(
+ "Only heap memory available, if you want to monitor direct memory use BufferUtils.setTrackDirectMemoryEnabled(true) during initialization.")
+ .append("\n");
+ }
+ if (printStout) {
+ System.out.println(store.toString());
+ }
+ }
+
+ /**
+ * Direct buffers are garbage collected by using a phantom reference and a
+ * reference queue. Every once a while, the JVM checks the reference queue
+ * and cleans the direct buffers. However, as this doesn't happen
+ * immediately after discarding all references to a direct buffer, it's easy
+ * to OutOfMemoryError yourself using direct buffers.
+ *
+ * @param toBeDestroyed the buffer to de-allocate (not null)
+ */
+ public static void destroyDirectBuffer(Buffer toBeDestroyed) {
+ if (!isDirect(toBeDestroyed)) {
+ return;
+ }
+ allocator.destroyDirectBuffer(toBeDestroyed);
+ }
+
+ /**
+ * Test whether the specified buffer is direct.
+ *
+ * @param buf the buffer to test (not null, unaffected)
+ * @return true if direct, otherwise false
+ */
+ private static boolean isDirect(Buffer buf) {
+ return buf.isDirect();
+ }
+
+ private static class BufferInfo extends PhantomReferenceTempVarsStack contains a stack of TempVars.
- * Every time TempVars.get() is called, a new entry is added to the stack,
- * and the index incremented.
- * When TempVars.release() is called, the entry is checked against
- * the current instance and then the index is decremented.
- */
- private static class TempVarsStack {
-
- int index = 0;
- TempVars[] tempVars = new TempVars[STACK_SIZE];
- }
- /**
- * ThreadLocal to store a TempVarsStack for each thread.
- * This ensures each thread has a single TempVarsStack that is
- * used only in method calls in that thread.
- */
- private static final ThreadLocalTempVarsStack contains a stack of TempVars.
+ * Every time TempVars.get() is called, a new entry is added to the stack,
+ * and the index incremented.
+ * When TempVars.release() is called, the entry is checked against
+ * the current instance and then the index is decremented.
+ */
+ private static class TempVarsStack {
+
+ int index = 0;
+ TempVars[] tempVars = new TempVars[STACK_SIZE];
+ }
+ /**
+ * ThreadLocal to store a TempVarsStack for each thread.
+ * This ensures each thread has a single TempVarsStack that is
+ * used only in method calls in that thread.
+ */
+ private static final ThreadLocal