+ * Matrices are 4 x 4 column-vector matrices stored in column-major + * order: + *
+ * m[offset + 0] m[offset + 4] m[offset + 8] m[offset + 12] + * m[offset + 1] m[offset + 5] m[offset + 9] m[offset + 13] + * m[offset + 2] m[offset + 6] m[offset + 10] m[offset + 14] + * m[offset + 3] m[offset + 7] m[offset + 11] m[offset + 15]+ * + * Vectors are 4 x 1 column vectors stored in order: + *
+ * v[offset + 0] + * v[offset + 1] + * v[offset + 2] + * v[offset + 3]+ */ +public class Matrix { + /** Temporary memory for operations that need temporary matrix data. */ + private final static float[] sTemp = new float[32]; + /** + * @deprecated All methods are static, do not instantiate this class. + */ + @Deprecated + public Matrix() {} + /** + * Multiplies two 4x4 matrices together and stores the result in a third 4x4 + * matrix. In matrix notation: result = lhs x rhs. Due to the way + * matrix multiplication works, the result matrix will have the same + * effect as first multiplying by the rhs matrix, then multiplying by + * the lhs matrix. This is the opposite of what you might expect. + *
+ * The same float array may be passed for result, lhs, and/or rhs. However, + * the result element values are undefined if the result elements overlap + * either the lhs or rhs elements. + * + * @param result The float array that holds the result. + * @param resultOffset The offset into the result array where the result is + * stored. + * @param lhs The float array that holds the left-hand-side matrix. + * @param lhsOffset The offset into the lhs array where the lhs is stored + * @param rhs The float array that holds the right-hand-side matrix. + * @param rhsOffset The offset into the rhs array where the rhs is stored. + * + * @throws IllegalArgumentException if result, lhs, or rhs are null, or if + * resultOffset + 16 > result.length or lhsOffset + 16 > lhs.length or + * rhsOffset + 16 > rhs.length. + */ + public static native void multiplyMM(float[] result, int resultOffset, + float[] lhs, int lhsOffset, float[] rhs, int rhsOffset); + /** + * Multiplies a 4 element vector by a 4x4 matrix and stores the result in a + * 4-element column vector. In matrix notation: result = lhs x rhs + *
+ * The same float array may be passed for resultVec, lhsMat, and/or rhsVec. + * However, the resultVec element values are undefined if the resultVec + * elements overlap either the lhsMat or rhsVec elements. + * + * @param resultVec The float array that holds the result vector. + * @param resultVecOffset The offset into the result array where the result + * vector is stored. + * @param lhsMat The float array that holds the left-hand-side matrix. + * @param lhsMatOffset The offset into the lhs array where the lhs is stored + * @param rhsVec The float array that holds the right-hand-side vector. + * @param rhsVecOffset The offset into the rhs vector where the rhs vector + * is stored. + * + * @throws IllegalArgumentException if resultVec, lhsMat, + * or rhsVec are null, or if resultVecOffset + 4 > resultVec.length + * or lhsMatOffset + 16 > lhsMat.length or + * rhsVecOffset + 4 > rhsVec.length. + */ + public static native void multiplyMV(float[] resultVec, + int resultVecOffset, float[] lhsMat, int lhsMatOffset, + float[] rhsVec, int rhsVecOffset); + /** + * Transposes a 4 x 4 matrix. + *
+ * mTrans and m must not overlap. + * + * @param mTrans the array that holds the output transposed matrix + * @param mTransOffset an offset into mTrans where the transposed matrix is + * stored. + * @param m the input array + * @param mOffset an offset into m where the input matrix is stored. + */ + public static void transposeM(float[] mTrans, int mTransOffset, float[] m, + int mOffset) { + for (int i = 0; i < 4; i++) { + int mBase = i * 4 + mOffset; + mTrans[i + mTransOffset] = m[mBase]; + mTrans[i + 4 + mTransOffset] = m[mBase + 1]; + mTrans[i + 8 + mTransOffset] = m[mBase + 2]; + mTrans[i + 12 + mTransOffset] = m[mBase + 3]; + } + } + /** + * Inverts a 4 x 4 matrix. + *
+ * mInv and m must not overlap. + * + * @param mInv the array that holds the output inverted matrix + * @param mInvOffset an offset into mInv where the inverted matrix is + * stored. + * @param m the input array + * @param mOffset an offset into m where the input matrix is stored. + * @return true if the matrix could be inverted, false if it could not. + */ + public static boolean invertM(float[] mInv, int mInvOffset, float[] m, + int mOffset) { + // Invert a 4 x 4 matrix using Cramer's Rule + // transpose matrix + final float src0 = m[mOffset + 0]; + final float src4 = m[mOffset + 1]; + final float src8 = m[mOffset + 2]; + final float src12 = m[mOffset + 3]; + final float src1 = m[mOffset + 4]; + final float src5 = m[mOffset + 5]; + final float src9 = m[mOffset + 6]; + final float src13 = m[mOffset + 7]; + final float src2 = m[mOffset + 8]; + final float src6 = m[mOffset + 9]; + final float src10 = m[mOffset + 10]; + final float src14 = m[mOffset + 11]; + final float src3 = m[mOffset + 12]; + final float src7 = m[mOffset + 13]; + final float src11 = m[mOffset + 14]; + final float src15 = m[mOffset + 15]; + // calculate pairs for first 8 elements (cofactors) + final float atmp0 = src10 * src15; + final float atmp1 = src11 * src14; + final float atmp2 = src9 * src15; + final float atmp3 = src11 * src13; + final float atmp4 = src9 * src14; + final float atmp5 = src10 * src13; + final float atmp6 = src8 * src15; + final float atmp7 = src11 * src12; + final float atmp8 = src8 * src14; + final float atmp9 = src10 * src12; + final float atmp10 = src8 * src13; + final float atmp11 = src9 * src12; + // calculate first 8 elements (cofactors) + final float dst0 = (atmp0 * src5 + atmp3 * src6 + atmp4 * src7) + - (atmp1 * src5 + atmp2 * src6 + atmp5 * src7); + final float dst1 = (atmp1 * src4 + atmp6 * src6 + atmp9 * src7) + - (atmp0 * src4 + atmp7 * src6 + atmp8 * src7); + final float dst2 = (atmp2 * src4 + atmp7 * src5 + atmp10 * src7) + - (atmp3 * src4 + atmp6 * src5 + atmp11 * src7); + final float dst3 = (atmp5 * src4 + atmp8 * src5 + atmp11 * src6) + - (atmp4 * src4 + atmp9 * src5 + atmp10 * src6); + final float dst4 = (atmp1 * src1 + atmp2 * src2 + atmp5 * src3) + - (atmp0 * src1 + atmp3 * src2 + atmp4 * src3); + final float dst5 = (atmp0 * src0 + atmp7 * src2 + atmp8 * src3) + - (atmp1 * src0 + atmp6 * src2 + atmp9 * src3); + final float dst6 = (atmp3 * src0 + atmp6 * src1 + atmp11 * src3) + - (atmp2 * src0 + atmp7 * src1 + atmp10 * src3); + final float dst7 = (atmp4 * src0 + atmp9 * src1 + atmp10 * src2) + - (atmp5 * src0 + atmp8 * src1 + atmp11 * src2); + // calculate pairs for second 8 elements (cofactors) + final float btmp0 = src2 * src7; + final float btmp1 = src3 * src6; + final float btmp2 = src1 * src7; + final float btmp3 = src3 * src5; + final float btmp4 = src1 * src6; + final float btmp5 = src2 * src5; + final float btmp6 = src0 * src7; + final float btmp7 = src3 * src4; + final float btmp8 = src0 * src6; + final float btmp9 = src2 * src4; + final float btmp10 = src0 * src5; + final float btmp11 = src1 * src4; + // calculate second 8 elements (cofactors) + final float dst8 = (btmp0 * src13 + btmp3 * src14 + btmp4 * src15) + - (btmp1 * src13 + btmp2 * src14 + btmp5 * src15); + final float dst9 = (btmp1 * src12 + btmp6 * src14 + btmp9 * src15) + - (btmp0 * src12 + btmp7 * src14 + btmp8 * src15); + final float dst10 = (btmp2 * src12 + btmp7 * src13 + btmp10 * src15) + - (btmp3 * src12 + btmp6 * src13 + btmp11 * src15); + final float dst11 = (btmp5 * src12 + btmp8 * src13 + btmp11 * src14) + - (btmp4 * src12 + btmp9 * src13 + btmp10 * src14); + final float dst12 = (btmp2 * src10 + btmp5 * src11 + btmp1 * src9 ) + - (btmp4 * src11 + btmp0 * src9 + btmp3 * src10); + final float dst13 = (btmp8 * src11 + btmp0 * src8 + btmp7 * src10) + - (btmp6 * src10 + btmp9 * src11 + btmp1 * src8 ); + final float dst14 = (btmp6 * src9 + btmp11 * src11 + btmp3 * src8 ) + - (btmp10 * src11 + btmp2 * src8 + btmp7 * src9 ); + final float dst15 = (btmp10 * src10 + btmp4 * src8 + btmp9 * src9 ) + - (btmp8 * src9 + btmp11 * src10 + btmp5 * src8 ); + // calculate determinant + final float det = + src0 * dst0 + src1 * dst1 + src2 * dst2 + src3 * dst3; + if (det == 0.0f) { + return false; + } + // calculate matrix inverse + final float invdet = 1.0f / det; + mInv[ mInvOffset] = dst0 * invdet; + mInv[ 1 + mInvOffset] = dst1 * invdet; + mInv[ 2 + mInvOffset] = dst2 * invdet; + mInv[ 3 + mInvOffset] = dst3 * invdet; + mInv[ 4 + mInvOffset] = dst4 * invdet; + mInv[ 5 + mInvOffset] = dst5 * invdet; + mInv[ 6 + mInvOffset] = dst6 * invdet; + mInv[ 7 + mInvOffset] = dst7 * invdet; + mInv[ 8 + mInvOffset] = dst8 * invdet; + mInv[ 9 + mInvOffset] = dst9 * invdet; + mInv[10 + mInvOffset] = dst10 * invdet; + mInv[11 + mInvOffset] = dst11 * invdet; + mInv[12 + mInvOffset] = dst12 * invdet; + mInv[13 + mInvOffset] = dst13 * invdet; + mInv[14 + mInvOffset] = dst14 * invdet; + mInv[15 + mInvOffset] = dst15 * invdet; + return true; + } + /** + * Computes an orthographic projection matrix. + * + * @param m returns the result + * @param mOffset + * @param left + * @param right + * @param bottom + * @param top + * @param near + * @param far + */ + public static void orthoM(float[] m, int mOffset, + float left, float right, float bottom, float top, + float near, float far) { + if (left == right) { + throw new IllegalArgumentException("left == right"); + } + if (bottom == top) { + throw new IllegalArgumentException("bottom == top"); + } + if (near == far) { + throw new IllegalArgumentException("near == far"); + } + final float r_width = 1.0f / (right - left); + final float r_height = 1.0f / (top - bottom); + final float r_depth = 1.0f / (far - near); + final float x = 2.0f * (r_width); + final float y = 2.0f * (r_height); + final float z = -2.0f * (r_depth); + final float tx = -(right + left) * r_width; + final float ty = -(top + bottom) * r_height; + final float tz = -(far + near) * r_depth; + m[mOffset + 0] = x; + m[mOffset + 5] = y; + m[mOffset +10] = z; + m[mOffset +12] = tx; + m[mOffset +13] = ty; + m[mOffset +14] = tz; + m[mOffset +15] = 1.0f; + m[mOffset + 1] = 0.0f; + m[mOffset + 2] = 0.0f; + m[mOffset + 3] = 0.0f; + m[mOffset + 4] = 0.0f; + m[mOffset + 6] = 0.0f; + m[mOffset + 7] = 0.0f; + m[mOffset + 8] = 0.0f; + m[mOffset + 9] = 0.0f; + m[mOffset + 11] = 0.0f; + } + /** + * Defines a projection matrix in terms of six clip planes. + * + * @param m the float array that holds the output perspective matrix + * @param offset the offset into float array m where the perspective + * matrix data is written + * @param left + * @param right + * @param bottom + * @param top + * @param near + * @param far + */ + public static void frustumM(float[] m, int offset, + float left, float right, float bottom, float top, + float near, float far) { + if (left == right) { + throw new IllegalArgumentException("left == right"); + } + if (top == bottom) { + throw new IllegalArgumentException("top == bottom"); + } + if (near == far) { + throw new IllegalArgumentException("near == far"); + } + if (near <= 0.0f) { + throw new IllegalArgumentException("near <= 0.0f"); + } + if (far <= 0.0f) { + throw new IllegalArgumentException("far <= 0.0f"); + } + final float r_width = 1.0f / (right - left); + final float r_height = 1.0f / (top - bottom); + final float r_depth = 1.0f / (near - far); + final float x = 2.0f * (near * r_width); + final float y = 2.0f * (near * r_height); + final float A = (right + left) * r_width; + final float B = (top + bottom) * r_height; + final float C = (far + near) * r_depth; + final float D = 2.0f * (far * near * r_depth); + m[offset + 0] = x; + m[offset + 5] = y; + m[offset + 8] = A; + m[offset + 9] = B; + m[offset + 10] = C; + m[offset + 14] = D; + m[offset + 11] = -1.0f; + m[offset + 1] = 0.0f; + m[offset + 2] = 0.0f; + m[offset + 3] = 0.0f; + m[offset + 4] = 0.0f; + m[offset + 6] = 0.0f; + m[offset + 7] = 0.0f; + m[offset + 12] = 0.0f; + m[offset + 13] = 0.0f; + m[offset + 15] = 0.0f; + } + /** + * Defines a projection matrix in terms of a field of view angle, an + * aspect ratio, and z clip planes. + * + * @param m the float array that holds the perspective matrix + * @param offset the offset into float array m where the perspective + * matrix data is written + * @param fovy field of view in y direction, in degrees + * @param aspect width to height aspect ratio of the viewport + * @param zNear + * @param zFar + */ + public static void perspectiveM(float[] m, int offset, + float fovy, float aspect, float zNear, float zFar) { + float f = 1.0f / (float) Math.tan(fovy * (Math.PI / 360.0)); + float rangeReciprocal = 1.0f / (zNear - zFar); + m[offset + 0] = f / aspect; + m[offset + 1] = 0.0f; + m[offset + 2] = 0.0f; + m[offset + 3] = 0.0f; + m[offset + 4] = 0.0f; + m[offset + 5] = f; + m[offset + 6] = 0.0f; + m[offset + 7] = 0.0f; + m[offset + 8] = 0.0f; + m[offset + 9] = 0.0f; + m[offset + 10] = (zFar + zNear) * rangeReciprocal; + m[offset + 11] = -1.0f; + m[offset + 12] = 0.0f; + m[offset + 13] = 0.0f; + m[offset + 14] = 2.0f * zFar * zNear * rangeReciprocal; + m[offset + 15] = 0.0f; + } + /** + * Computes the length of a vector. + * + * @param x x coordinate of a vector + * @param y y coordinate of a vector + * @param z z coordinate of a vector + * @return the length of a vector + */ + public static float length(float x, float y, float z) { + return (float) Math.sqrt(x * x + y * y + z * z); + } + /** + * Sets matrix m to the identity matrix. + * + * @param sm returns the result + * @param smOffset index into sm where the result matrix starts + */ + public static void setIdentityM(float[] sm, int smOffset) { + for (int i=0 ; i<16 ; i++) { + sm[smOffset + i] = 0; + } + for(int i = 0; i < 16; i += 5) { + sm[smOffset + i] = 1.0f; + } + } + /** + * Scales matrix m by x, y, and z, putting the result in sm. + *
+ * m and sm must not overlap. + * + * @param sm returns the result + * @param smOffset index into sm where the result matrix starts + * @param m source matrix + * @param mOffset index into m where the source matrix starts + * @param x scale factor x + * @param y scale factor y + * @param z scale factor z + */ + public static void scaleM(float[] sm, int smOffset, + float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<4 ; i++) { + int smi = smOffset + i; + int mi = mOffset + i; + sm[ smi] = m[ mi] * x; + sm[ 4 + smi] = m[ 4 + mi] * y; + sm[ 8 + smi] = m[ 8 + mi] * z; + sm[12 + smi] = m[12 + mi]; + } + } + /** + * Scales matrix m in place by sx, sy, and sz. + * + * @param m matrix to scale + * @param mOffset index into m where the matrix starts + * @param x scale factor x + * @param y scale factor y + * @param z scale factor z + */ + public static void scaleM(float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<4 ; i++) { + int mi = mOffset + i; + m[ mi] *= x; + m[ 4 + mi] *= y; + m[ 8 + mi] *= z; + } + } + /** + * Translates matrix m by x, y, and z, putting the result in tm. + *
+ * m and tm must not overlap. + * + * @param tm returns the result + * @param tmOffset index into sm where the result matrix starts + * @param m source matrix + * @param mOffset index into m where the source matrix starts + * @param x translation factor x + * @param y translation factor y + * @param z translation factor z + */ + public static void translateM(float[] tm, int tmOffset, + float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<12 ; i++) { + tm[tmOffset + i] = m[mOffset + i]; + } + for (int i=0 ; i<4 ; i++) { + int tmi = tmOffset + i; + int mi = mOffset + i; + tm[12 + tmi] = m[mi] * x + m[4 + mi] * y + m[8 + mi] * z + + m[12 + mi]; + } + } + /** + * Translates matrix m by x, y, and z in place. + * + * @param m matrix + * @param mOffset index into m where the matrix starts + * @param x translation factor x + * @param y translation factor y + * @param z translation factor z + */ + public static void translateM( + float[] m, int mOffset, + float x, float y, float z) { + for (int i=0 ; i<4 ; i++) { + int mi = mOffset + i; + m[12 + mi] += m[mi] * x + m[4 + mi] * y + m[8 + mi] * z; + } + } + /** + * Rotates matrix m by angle a (in degrees) around the axis (x, y, z). + *
+ * m and rm must not overlap. + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param m source matrix + * @param mOffset index into m where the source matrix starts + * @param a angle to rotate in degrees + * @param x X axis component + * @param y Y axis component + * @param z Z axis component + */ + public static void rotateM(float[] rm, int rmOffset, + float[] m, int mOffset, + float a, float x, float y, float z) { + synchronized(sTemp) { + setRotateM(sTemp, 0, a, x, y, z); + multiplyMM(rm, rmOffset, m, mOffset, sTemp, 0); + } + } + /** + * Rotates matrix m in place by angle a (in degrees) + * around the axis (x, y, z). + * + * @param m source matrix + * @param mOffset index into m where the matrix starts + * @param a angle to rotate in degrees + * @param x X axis component + * @param y Y axis component + * @param z Z axis component + */ + public static void rotateM(float[] m, int mOffset, + float a, float x, float y, float z) { + synchronized(sTemp) { + setRotateM(sTemp, 0, a, x, y, z); + multiplyMM(sTemp, 16, m, mOffset, sTemp, 0); + System.arraycopy(sTemp, 16, m, mOffset, 16); + } + } + /** + * Creates a matrix for rotation by angle a (in degrees) + * around the axis (x, y, z). + *
+ * An optimized path will be used for rotation about a major axis + * (e.g. x=1.0f y=0.0f z=0.0f). + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param a angle to rotate in degrees + * @param x X axis component + * @param y Y axis component + * @param z Z axis component + */ + public static void setRotateM(float[] rm, int rmOffset, + float a, float x, float y, float z) { + rm[rmOffset + 3] = 0; + rm[rmOffset + 7] = 0; + rm[rmOffset + 11]= 0; + rm[rmOffset + 12]= 0; + rm[rmOffset + 13]= 0; + rm[rmOffset + 14]= 0; + rm[rmOffset + 15]= 1; + a *= (float) (Math.PI / 180.0f); + float s = (float) Math.sin(a); + float c = (float) Math.cos(a); + if (1.0f == x && 0.0f == y && 0.0f == z) { + rm[rmOffset + 5] = c; rm[rmOffset + 10]= c; + rm[rmOffset + 6] = s; rm[rmOffset + 9] = -s; + rm[rmOffset + 1] = 0; rm[rmOffset + 2] = 0; + rm[rmOffset + 4] = 0; rm[rmOffset + 8] = 0; + rm[rmOffset + 0] = 1; + } else if (0.0f == x && 1.0f == y && 0.0f == z) { + rm[rmOffset + 0] = c; rm[rmOffset + 10]= c; + rm[rmOffset + 8] = s; rm[rmOffset + 2] = -s; + rm[rmOffset + 1] = 0; rm[rmOffset + 4] = 0; + rm[rmOffset + 6] = 0; rm[rmOffset + 9] = 0; + rm[rmOffset + 5] = 1; + } else if (0.0f == x && 0.0f == y && 1.0f == z) { + rm[rmOffset + 0] = c; rm[rmOffset + 5] = c; + rm[rmOffset + 1] = s; rm[rmOffset + 4] = -s; + rm[rmOffset + 2] = 0; rm[rmOffset + 6] = 0; + rm[rmOffset + 8] = 0; rm[rmOffset + 9] = 0; + rm[rmOffset + 10]= 1; + } else { + float len = length(x, y, z); + if (1.0f != len) { + float recipLen = 1.0f / len; + x *= recipLen; + y *= recipLen; + z *= recipLen; + } + float nc = 1.0f - c; + float xy = x * y; + float yz = y * z; + float zx = z * x; + float xs = x * s; + float ys = y * s; + float zs = z * s; + rm[rmOffset + 0] = x*x*nc + c; + rm[rmOffset + 4] = xy*nc - zs; + rm[rmOffset + 8] = zx*nc + ys; + rm[rmOffset + 1] = xy*nc + zs; + rm[rmOffset + 5] = y*y*nc + c; + rm[rmOffset + 9] = yz*nc - xs; + rm[rmOffset + 2] = zx*nc - ys; + rm[rmOffset + 6] = yz*nc + xs; + rm[rmOffset + 10] = z*z*nc + c; + } + } + /** + * Converts Euler angles to a rotation matrix. + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param x angle of rotation, in degrees + * @param y angle of rotation, in degrees + * @param z angle of rotation, in degrees + */ + public static void setRotateEulerM(float[] rm, int rmOffset, + float x, float y, float z) { + x *= (float) (Math.PI / 180.0f); + y *= (float) (Math.PI / 180.0f); + z *= (float) (Math.PI / 180.0f); + float cx = (float) Math.cos(x); + float sx = (float) Math.sin(x); + float cy = (float) Math.cos(y); + float sy = (float) Math.sin(y); + float cz = (float) Math.cos(z); + float sz = (float) Math.sin(z); + float cxsy = cx * sy; + float sxsy = sx * sy; + rm[rmOffset + 0] = cy * cz; + rm[rmOffset + 1] = -cy * sz; + rm[rmOffset + 2] = sy; + rm[rmOffset + 3] = 0.0f; + rm[rmOffset + 4] = cxsy * cz + cx * sz; + rm[rmOffset + 5] = -cxsy * sz + cx * cz; + rm[rmOffset + 6] = -sx * cy; + rm[rmOffset + 7] = 0.0f; + rm[rmOffset + 8] = -sxsy * cz + sx * sz; + rm[rmOffset + 9] = sxsy * sz + sx * cz; + rm[rmOffset + 10] = cx * cy; + rm[rmOffset + 11] = 0.0f; + rm[rmOffset + 12] = 0.0f; + rm[rmOffset + 13] = 0.0f; + rm[rmOffset + 14] = 0.0f; + rm[rmOffset + 15] = 1.0f; + } + /** + * Defines a viewing transformation in terms of an eye point, a center of + * view, and an up vector. + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param eyeX eye point X + * @param eyeY eye point Y + * @param eyeZ eye point Z + * @param centerX center of view X + * @param centerY center of view Y + * @param centerZ center of view Z + * @param upX up vector X + * @param upY up vector Y + * @param upZ up vector Z + */ + public static void setLookAtM(float[] rm, int rmOffset, + float eyeX, float eyeY, float eyeZ, + float centerX, float centerY, float centerZ, float upX, float upY, + float upZ) { + // See the OpenGL GLUT documentation for gluLookAt for a description + // of the algorithm. We implement it in a straightforward way: + float fx = centerX - eyeX; + float fy = centerY - eyeY; + float fz = centerZ - eyeZ; + // Normalize f + float rlf = 1.0f / Matrix.length(fx, fy, fz); + fx *= rlf; + fy *= rlf; + fz *= rlf; + // compute s = f x up (x means "cross product") + float sx = fy * upZ - fz * upY; + float sy = fz * upX - fx * upZ; + float sz = fx * upY - fy * upX; + // and normalize s + float rls = 1.0f / Matrix.length(sx, sy, sz); + sx *= rls; + sy *= rls; + sz *= rls; + // compute u = s x f + float ux = sy * fz - sz * fy; + float uy = sz * fx - sx * fz; + float uz = sx * fy - sy * fx; + rm[rmOffset + 0] = sx; + rm[rmOffset + 1] = ux; + rm[rmOffset + 2] = -fx; + rm[rmOffset + 3] = 0.0f; + rm[rmOffset + 4] = sy; + rm[rmOffset + 5] = uy; + rm[rmOffset + 6] = -fy; + rm[rmOffset + 7] = 0.0f; + rm[rmOffset + 8] = sz; + rm[rmOffset + 9] = uz; + rm[rmOffset + 10] = -fz; + rm[rmOffset + 11] = 0.0f; + rm[rmOffset + 12] = 0.0f; + rm[rmOffset + 13] = 0.0f; + rm[rmOffset + 14] = 0.0f; + rm[rmOffset + 15] = 1.0f; + translateM(rm, rmOffset, -eyeX, -eyeY, -eyeZ); + } +} diff --git a/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/matrices /OpenGLMatrix.java b/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/matrices /OpenGLMatrix.java new file mode 100644 index 0000000..679b441 --- /dev/null +++ b/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/matrices /OpenGLMatrix.java @@ -0,0 +1,271 @@ +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) 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 Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcore.external.matrices; + +import android.opengl.Matrix; + +import com.vuforia.Matrix44F; + +import org.firstinspires.ftc.robotcore.external.Const; +import org.firstinspires.ftc.robotcore.external.NonConst; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +/** + * An {@link OpenGLMatrix} is a 4x4 matrix commonly used as a transformation matrix for 3D + * homogeneous coordinates. The data layout of an {@link OpenGLMatrix} is used heavily in the + * OpenGL high performance graphics standard. + * + * @see Homogenous coordinates + * @see Transformation Matrix + * @see android.opengl.Matrix + * @see Matrix + */ +public class OpenGLMatrix extends ColumnMajorMatrixF + { + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + float[] data; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public OpenGLMatrix() + { + super(4,4); + this.data = new float[4*4]; + Matrix.setIdentityM(this.data, 0); + } + + public OpenGLMatrix(float[] data) + { + super(4,4); + this.data = data; + if (this.data.length != 4*4) throw dimensionsError(); + } + + public OpenGLMatrix(Matrix44F matrix) + { + this(matrix.getData()); + } + + /** + * Constructs an OpenGL matrix whose values are initialized from the other matrix. + * The other matrix must have dimensions at most 4x4. + * @param him the matrix from which to initialize our own data + */ + public OpenGLMatrix(MatrixF him) + { + this(); + if (him.numRows > 4 || him.numCols > 4) throw him.dimensionsError(); + for (int i = 0; i < Math.min(4,him.numRows); i++) + { + for (int j = 0; j < Math.min(4,him.numCols); j++) + { + this.put(i,j, him.get(i,j)); + } + } + } + + @Override public MatrixF emptyMatrix(int numRows, int numCols) + { + if (numRows==4 && numCols==4) + return new OpenGLMatrix(); + else + return new GeneralMatrixF(numRows, numCols); + } + + /** + * Creates a matrix for rotation by the indicated angle around the indicated vector. + */ + public static OpenGLMatrix rotation(AngleUnit angleUnit, float angle, float dx, float dy, float dz) + { + float[] data = new float[16]; + Matrix.setRotateM(data, 0, angleUnit.toDegrees(angle), dx, dy, dz); + return new OpenGLMatrix(data); + } + + /** + * Creates a matrix for a rotation specified by three successive rotation angles. + * @see Orientation#getRotationMatrix(AxesReference, AxesOrder, AngleUnit, float, float, float) + */ + public static OpenGLMatrix rotation(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float first, float second, float third) + { + OpenGLMatrix rotation = Orientation.getRotationMatrix(axesReference, axesOrder, angleUnit, first, second, third); + return identityMatrix().multiplied(rotation); + } + public static OpenGLMatrix translation(float dx, float dy, float dz) + { + OpenGLMatrix result = new OpenGLMatrix(); + result.translate(dx, dy, dz); + return result; + } + public static OpenGLMatrix identityMatrix() + { + return new OpenGLMatrix(); + } + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + @Override public float[] getData() + { + return this.data; + } + + //---------------------------------------------------------------------------------------------- + // Transformation matrix operations (in-place). These methods all return the receiver + // in order to facilitate chaining. + // + // Note that these are some of the very view matrix operations that update-in-place rather than + // returning a new matrix and leaving the receiver unmodified. Care must thus be taken to avoid + // sharing the data of this matrix (using getData()) with other matrix-related objects and then + // subsequently modifying this matrix. + //---------------------------------------------------------------------------------------------- + + @NonConst public void scale(float scaleX, float scaleY, float scaleZ) + { + Matrix.scaleM(this.data, 0, scaleX, scaleY, scaleZ); + } + @NonConst public void scale(float scale) + { + this.scale(scale, scale, scale); + } + @NonConst public void translate(float dx, float dy, float dz) + { + Matrix.translateM(this.data, 0, dx, dy, dz); + } + @NonConst public void rotate(AngleUnit angleUnit, float angle, float dx, float dy, float dz) + { + Matrix.rotateM(this.data, 0, angleUnit.toDegrees(angle), dx, dy, dz); + } + @NonConst public void rotate(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float first, float second, float third) + { + OpenGLMatrix rotation = Orientation.getRotationMatrix(axesReference, axesOrder, angleUnit, first, second, third); + this.data = this.multiplied(rotation).getData(); + } + + //---------------------------------------------------------------------------------------------- + // Transformation matrix operations + //---------------------------------------------------------------------------------------------- + + @Const public OpenGLMatrix scaled(float scaleX, float scaleY, float scaleZ) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.scaleM(result.data, 0, this.data, 0, scaleX, scaleY, scaleZ); + return result; + } + @Const public OpenGLMatrix scaled(float scale) + { + return scaled(scale, scale, scale); + } + @Const public OpenGLMatrix translated(float dx, float dy, float dz) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.translateM(result.data, 0, this.data, 0, dx, dy, dz); + return result; + } + @Const public OpenGLMatrix rotated(AngleUnit angleUnit, float angle, float dx, float dy, float dz) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.rotateM(result.data, 0, this.data, 0, angleUnit.toDegrees(angle), dx, dy, dz); + return result; + } + @Const public OpenGLMatrix rotated(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float first, float second, float third) + { + OpenGLMatrix rotation = Orientation.getRotationMatrix(axesReference, axesOrder, angleUnit, first, second, third); + return this.multiplied(rotation); + } + + //---------------------------------------------------------------------------------------------- + // Matrix operations + //---------------------------------------------------------------------------------------------- + + @Override @Const public OpenGLMatrix inverted() + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.invertM(result.data, 0, this.data, 0); + return result; + } + + @Override @Const public OpenGLMatrix transposed() + { + return (OpenGLMatrix)super.transposed(); + } + + @Const public OpenGLMatrix multiplied(OpenGLMatrix him) + { + OpenGLMatrix result = new OpenGLMatrix(); + Matrix.multiplyMM(result.data, 0, this.data, 0, him.getData(), 0); + return result; + } + + @Override @Const public MatrixF multiplied(MatrixF him) + { + if (him instanceof OpenGLMatrix) + { + return this.multiplied((OpenGLMatrix)him); + } + else + return super.multiplied(him); + } + + /** + * Updates the receiver to be the product of itself and another matrix. + * @param him the matrix with which the receiver is to be multiplied. + */ + @NonConst public void multiply(OpenGLMatrix him) + { + this.data = this.multiplied(him).getData(); + } + + /** + * Updates the receiver to be the product of itself and another matrix. + * @param him the matrix with which the receiver is to be multiplied. + */ + @Override @NonConst public void multiply(MatrixF him) + { + if (him instanceof OpenGLMatrix) + { + this.multiply((OpenGLMatrix)him); + } + else + super.multiply(him); + } + } diff --git a/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java b/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java index 46988ef..7ea8684 100644 --- a/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java +++ b/RobotCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Orientation.java @@ -1,39 +1,39 @@ /* -Copyright (c) 2016 Robert Atkinson - -All rights reserved. - -Redistribution and use in source and binary forms, with or without modification, -are permitted (subject to the limitations in the disclaimer below) 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 Robert Atkinson nor the names of his contributors may be used to -endorse or promote products derived from this software without specific prior -written permission. - -NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS -LICENSE. 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. -*/ + Copyright (c) 2016 Robert Atkinson + + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted (subject to the limitations in the disclaimer below) 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 Robert Atkinson nor the names of his contributors may be used to + endorse or promote products derived from this software without specific prior + written permission. + + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ package org.firstinspires.ftc.robotcore.external.navigation; import org.firstinspires.ftc.robotcore.external.matrices.MatrixF; -// import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; +import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.internal.system.Assert; @@ -102,30 +102,30 @@ are permitted (subject to the limitations in the disclaimer below) provided that * @see Rotation Matrix */ public class Orientation - { +{ //---------------------------------------------------------------------------------------------- // State //---------------------------------------------------------------------------------------------- - + /** * whether we have extrinsic or intrinsic rotations * * @see #axesOrder */ public AxesReference axesReference; - + /** * the order of axes around which our three rotations occur * * @see #axesReference */ public AxesOrder axesOrder; - + /** * the unit in which the angles are expressed */ public AngleUnit angleUnit; - + /** * the chronologically first rotation made in the {@link AxesOrder} */ @@ -138,24 +138,24 @@ public class Orientation * the chronologically third rotation made in the {@link AxesOrder} */ public float thirdAngle; - + /** * the time on the System.nanoTime() clock at which the data was acquired. If no * timestamp is associated with this particular set of data, this value is zero. */ public long acquisitionTime; - + //---------------------------------------------------------------------------------------------- // Construction //---------------------------------------------------------------------------------------------- - + public Orientation() - { + { this(AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.RADIANS, 0, 0, 0, 0); - } - + } + public Orientation(AxesReference axesReference, AxesOrder axesOrder, AngleUnit angleUnit, float firstAngle, float secondAngle, float thirdAngle, long acquisitionTime) - { + { this.axesReference = axesReference; this.axesOrder = axesOrder; this.angleUnit = angleUnit; @@ -163,8 +163,8 @@ public Orientation(AxesReference axesReference, AxesOrder axesOrder, AngleUnit a this.secondAngle = secondAngle; this.thirdAngle = thirdAngle; this.acquisitionTime = acquisitionTime; - } - + } + /** * Converts this {@link Orientation} to one with the indicated angular units. * @@ -172,19 +172,19 @@ public Orientation(AxesReference axesReference, AxesOrder axesOrder, AngleUnit a * @return a new [@link Orientation} with the same data but in the indicated units */ public Orientation toAngleUnit(AngleUnit angleUnit) - { + { if (angleUnit != this.angleUnit) - { + { return new Orientation(this.axesReference, this.axesOrder, angleUnit, - angleUnit.fromUnit(this.angleUnit, firstAngle), - angleUnit.fromUnit(this.angleUnit, secondAngle), - angleUnit.fromUnit(this.angleUnit, thirdAngle), - this.acquisitionTime); - } + angleUnit.fromUnit(this.angleUnit, firstAngle), + angleUnit.fromUnit(this.angleUnit, secondAngle), + angleUnit.fromUnit(this.angleUnit, thirdAngle), + this.acquisitionTime); + } else return this; - } - + } + /** * Converts the {@link Orientation} to an equivalent one with the indicted point of view. * @@ -192,9 +192,9 @@ public Orientation toAngleUnit(AngleUnit angleUnit) * @return an equivalent orientation but with the indicated point of view. */ public Orientation toAxesReference(AxesReference axesReference) - { + { if (this.axesReference != axesReference) - { + { /** * Theorem: Any extrinsic rotation is equivalent to an intrinsic rotation by * the same angles but with inverted order of elemental orientations, and vice versa. @@ -202,43 +202,43 @@ public Orientation toAxesReference(AxesReference axesReference) */ Assert.assertTrue(axesReference == this.axesReference.reverse()); return new Orientation(this.axesReference.reverse(), this.axesOrder.reverse(), this.angleUnit, - this.thirdAngle, this.secondAngle, this.firstAngle, this.acquisitionTime); - } + this.thirdAngle, this.secondAngle, this.firstAngle, this.acquisitionTime); + } else return this; - } - + } + /** * Converts the {@link Orientation} to an equivalent one with the indicated ordering of axes * @param axesOrder the desired ordering of axes * @return an equivalent orientation with the indicated axes order */ public Orientation toAxesOrder(AxesOrder axesOrder) - { + { if (this.axesOrder != axesOrder) - { + { return Orientation.getOrientation(this.getRotationMatrix(), this.axesReference, axesOrder, this.angleUnit); - } + } else return this; - } - + } + //---------------------------------------------------------------------------------------------- // Accessing //---------------------------------------------------------------------------------------------- - + @Override public String toString() - { + { if (this.angleUnit == AngleUnit.DEGREES) return String.format("{%s %s %.0f %.0f %.0f}", this.axesReference.toString(), this.axesOrder.toString(), this.firstAngle, this.secondAngle, this.thirdAngle); else return String.format("{%s %s %.3f %.3f %.3f}", this.axesReference.toString(), this.axesOrder.toString(), this.firstAngle, this.secondAngle, this.thirdAngle); - } - + } + //---------------------------------------------------------------------------------------------- // Rotation Matrices //---------------------------------------------------------------------------------------------- - + /** * Returns the rotation matrix associated with the receiver {@link Orientation}. * @@ -247,10 +247,10 @@ public Orientation toAxesOrder(AxesOrder axesOrder) * @see Rotation Matrix */ public OpenGLMatrix getRotationMatrix() - { - throw RuntimeException("Stub!"); - } - + { + return getRotationMatrix(this.axesReference, this.axesOrder, this.angleUnit, this.firstAngle, this.secondAngle, this.thirdAngle); + } + /** * Returns the rotation matrix associated with a particular set of three rotational angles. * @@ -259,10 +259,189 @@ public OpenGLMatrix getRotationMatrix() * @see Rotation Matrix */ public static OpenGLMatrix getRotationMatrix(AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit, float firstAngle, float secondAngle, float thirdAngle) + { + if (axesReference == AxesReference.INTRINSIC) { - throw RuntimeException("Stub!"); + /** + * Theorem: Any extrinsic rotation is equivalent to an intrinsic rotation by the same + * angles but with inverted order of elemental orientations, and vice versa. + * @see Euler Angles + */ + return getRotationMatrix(axesReference.reverse(), axesOrder.reverse(), unit, thirdAngle, secondAngle, firstAngle); } - + + /** + * The extrinsic case takes some work. + * + * Implementation note: these computations were created automatically from symbolic Mathematica expressions. + * Each computes the intrinsic rotation matrix of a given {@link AxesOrder}, where the axes in the {@link AxesOrder} + * are used left to right chronologically and the angles are applied chronologically as well. + * + * For example, the entry for YXZ is rotation matrix of the extrinsic rotation which rotates first + * Y(firstAngle), then X(secondAngle), and finally Z(thirdAngle). The rotation matrix in this case is + * Z(thirdAngle).X(secondAngle).Y(firstAngle) + * as the matrix order (when *post*-multiplying vectors, as is always done in modern computer graphics systems), + * which you will notice has the matrices in reverse order from the chronological sequence of extrinsic rotations. + */ + + firstAngle = unit.toRadians(firstAngle); + secondAngle = unit.toRadians(secondAngle); + thirdAngle = unit.toRadians(thirdAngle); + + float m00, m01, m02; + float m10, m11, m12; + float m20, m21, m22; + + switch (axesOrder) + { + default: + case XZX: + m00 = ((float) (cos(secondAngle))); + m01 = ((float) (-(cos(firstAngle) * sin(secondAngle)))); + m02 = ((float) (sin(firstAngle) * sin(secondAngle))); + m10 = ((float) (cos(thirdAngle) * sin(secondAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(firstAngle) * sin(thirdAngle)) - cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m20 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m21 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * cos(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + break; + case XYX: + m00 = ((float) (cos(secondAngle))); + m01 = ((float) (sin(firstAngle) * sin(secondAngle))); + m02 = ((float) (cos(firstAngle) * sin(secondAngle))); + m10 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(firstAngle) * cos(secondAngle) * sin(thirdAngle)) - cos(thirdAngle) * sin(firstAngle))); + m20 = ((float) (-(cos(thirdAngle) * sin(secondAngle)))); + m21 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + break; + case YXY: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m02 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * cos(secondAngle) * sin(thirdAngle))); + m10 = ((float) (sin(firstAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle))); + m12 = ((float) (-(cos(firstAngle) * sin(secondAngle)))); + m20 = ((float) (-(cos(firstAngle) * sin(thirdAngle)) - cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m21 = ((float) (cos(thirdAngle) * sin(secondAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + break; + case YZY: + m00 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(thirdAngle) * sin(secondAngle)))); + m02 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m10 = ((float) (cos(firstAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle))); + m12 = ((float) (sin(firstAngle) * sin(secondAngle))); + m20 = ((float) (-(cos(firstAngle) * cos(secondAngle) * sin(thirdAngle)) - cos(thirdAngle) * sin(firstAngle))); + m21 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + break; + case ZYZ: + m00 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(firstAngle) * sin(thirdAngle)) - cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m02 = ((float) (cos(thirdAngle) * sin(secondAngle))); + m10 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * cos(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m20 = ((float) (-(cos(firstAngle) * sin(secondAngle)))); + m21 = ((float) (sin(firstAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle))); + break; + case ZXZ: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) - cos(secondAngle) * sin(firstAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(firstAngle) * cos(secondAngle) * sin(thirdAngle)) - cos(thirdAngle) * sin(firstAngle))); + m02 = ((float) (sin(secondAngle) * sin(thirdAngle))); + m10 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(secondAngle) * cos(thirdAngle) * sin(firstAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle) * cos(thirdAngle) - sin(firstAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(thirdAngle) * sin(secondAngle)))); + m20 = ((float) (sin(firstAngle) * sin(secondAngle))); + m21 = ((float) (cos(firstAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle))); + break; + case XZY: + m00 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m01 = ((float) (sin(firstAngle) * sin(thirdAngle) - cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m02 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(thirdAngle) * sin(firstAngle) * sin(secondAngle))); + m10 = ((float) (sin(secondAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle))); + m12 = ((float) (-(cos(secondAngle) * sin(firstAngle)))); + m20 = ((float) (-(cos(secondAngle) * sin(thirdAngle)))); + m21 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) - sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + break; + case XYZ: + m00 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m01 = ((float) (cos(thirdAngle) * sin(firstAngle) * sin(secondAngle) - cos(firstAngle) * sin(thirdAngle))); + m02 = ((float) (sin(firstAngle) * sin(thirdAngle) + cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m10 = ((float) (cos(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) + sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m12 = ((float) (cos(firstAngle) * sin(secondAngle) * sin(thirdAngle) - cos(thirdAngle) * sin(firstAngle))); + m20 = ((float) (-sin(secondAngle))); + m21 = ((float) (cos(secondAngle) * sin(firstAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle))); + break; + case YXZ: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) - sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m01 = ((float) (-(cos(secondAngle) * sin(thirdAngle)))); + m02 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m10 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(thirdAngle) * sin(firstAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m12 = ((float) (sin(firstAngle) * sin(thirdAngle) - cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m20 = ((float) (-(cos(secondAngle) * sin(firstAngle)))); + m21 = ((float) (sin(secondAngle))); + m22 = ((float) (cos(firstAngle) * cos(secondAngle))); + break; + case YZX: + m00 = ((float) (cos(firstAngle) * cos(secondAngle))); + m01 = ((float) (-sin(secondAngle))); + m02 = ((float) (cos(secondAngle) * sin(firstAngle))); + m10 = ((float) (sin(firstAngle) * sin(thirdAngle) + cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m11 = ((float) (cos(secondAngle) * cos(thirdAngle))); + m12 = ((float) (cos(thirdAngle) * sin(firstAngle) * sin(secondAngle) - cos(firstAngle) * sin(thirdAngle))); + m20 = ((float) (cos(firstAngle) * sin(secondAngle) * sin(thirdAngle) - cos(thirdAngle) * sin(firstAngle))); + m21 = ((float) (cos(secondAngle) * sin(thirdAngle))); + m22 = ((float) (cos(firstAngle) * cos(thirdAngle) + sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + break; + case ZYX: + m00 = ((float) (cos(firstAngle) * cos(secondAngle))); + m01 = ((float) (-(cos(secondAngle) * sin(firstAngle)))); + m02 = ((float) (sin(secondAngle))); + m10 = ((float) (cos(thirdAngle) * sin(firstAngle) + cos(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m11 = ((float) (cos(firstAngle) * cos(thirdAngle) - sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m12 = ((float) (-(cos(secondAngle) * sin(thirdAngle)))); + m20 = ((float) (sin(firstAngle) * sin(thirdAngle) - cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m21 = ((float) (cos(firstAngle) * sin(thirdAngle) + cos(thirdAngle) * sin(firstAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle) * cos(thirdAngle))); + break; + case ZXY: + m00 = ((float) (cos(firstAngle) * cos(thirdAngle) + sin(firstAngle) * sin(secondAngle) * sin(thirdAngle))); + m01 = ((float) (cos(firstAngle) * sin(secondAngle) * sin(thirdAngle) - cos(thirdAngle) * sin(firstAngle))); + m02 = ((float) (cos(secondAngle) * sin(thirdAngle))); + m10 = ((float) (cos(secondAngle) * sin(firstAngle))); + m11 = ((float) (cos(firstAngle) * cos(secondAngle))); + m12 = ((float) (-sin(secondAngle))); + m20 = ((float) (cos(thirdAngle) * sin(firstAngle) * sin(secondAngle) - cos(firstAngle) * sin(thirdAngle))); + m21 = ((float) (sin(firstAngle) * sin(thirdAngle) + cos(firstAngle) * cos(thirdAngle) * sin(secondAngle))); + m22 = ((float) (cos(secondAngle) * cos(thirdAngle))); + break; + } + + OpenGLMatrix result = new OpenGLMatrix(); + result.put(0, 0, m00); + result.put(0, 1, m01); + result.put(0, 2, m02); + result.put(1, 0, m10); + result.put(1, 1, m11); + result.put(1, 2, m12); + result.put(2, 0, m20); + result.put(2, 1, m21); + result.put(2, 2, m22); + return result; + } + /** * Given a rotation matrix, and an {@link AxesReference} and {@link AxesOrder}, returns an orientation * that would produce that rotation matrix. @@ -277,26 +456,26 @@ public static OpenGLMatrix getRotationMatrix(AxesReference axesReference, AxesOr * @see Rotation Matrix */ public static Orientation getOrientation(MatrixF rot, AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit) - { + { /** * Run both choices and return the one that uses smaller sets of angles. This is just a heuristic * to choose which angle set is the most aesthetically pleasing. Both angle sets are equally valid. */ Orientation one = getOrientation(rot, axesReference, axesOrder, unit, AngleSet.THEONE); Orientation theOther = getOrientation(rot, axesReference, axesOrder, unit, AngleSet.THEOTHER); - + VectorF vOne = new VectorF(one.firstAngle, one.secondAngle, one.thirdAngle); VectorF vOther = new VectorF(theOther.firstAngle, theOther.secondAngle, theOther.thirdAngle); - + return vOne.magnitude() <= vOther.magnitude() ? one : theOther; - } - + } + /** * {@link AngleSet} is used to distinguish between the two sets of angles that will produce * a given rotation in a given axes reference and a given axes order */ public enum AngleSet { THEONE, THEOTHER }; - + /** * Given a rotation matrix, and an {@link AxesReference} and {@link AxesOrder}, returns an orientation * that would produce that rotation matrix. @@ -311,14 +490,14 @@ public enum AngleSet { THEONE, THEOTHER }; * @see Rotation Matrix */ public static Orientation getOrientation(MatrixF rot, AxesReference axesReference, AxesOrder axesOrder, AngleUnit unit, AngleSet angleSet) - { + { float firstAngle, secondAngle, thirdAngle; - + if (axesReference == AxesReference.INTRINSIC) - { + { return getOrientation(rot, axesReference.reverse(), axesOrder.reverse(), unit, angleSet).toAxesReference(axesReference); - } - + } + /** * The extrinsic case takes some work. * @@ -336,28 +515,28 @@ public static Orientation getOrientation(MatrixF rot, AxesReference axesReferenc */ float test; switch (axesOrder) - { + { default: case XZX: test = rot.get(0, 0); /* cos(secondAngle) */ if (test == 1) - { + { secondAngle = (float) 0; firstAngle = (float) 0; /* arbitrary */ /* rot.get(2, 1) == sin(firstAngle + thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(2, 1), rot.get(1, 1)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) PI; firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 2) == sin(firstAngle - thirdAngle) */ /* rot.get(2, 2) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(1, 2), rot.get(2, 2))); - } + } else - { + { /* rot.get(0, 0) == cos(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(0, 0)) : -acos(rot.get(0, 0))); /* rot.get(0, 2) == sin(firstAngle) * sin(secondAngle) */ @@ -366,28 +545,28 @@ else if (test == -1) /* rot.get(2, 0) == sin(secondAngle) * sin(thirdAngle) */ /* rot.get(1, 0) == cos(thirdAngle) * sin(secondAngle) */ thirdAngle = (float) atan2(rot.get(2, 0) / sin(secondAngle), rot.get(1, 0) / sin(secondAngle)); - } + } break; case XYX: test = rot.get(0, 0); /* cos(secondAngle) */ if (test == 1) - { + { secondAngle = (float) 0; firstAngle = (float) 0; /* arbitrary */ /* rot.get(2, 1) == sin(firstAngle + thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(2, 1), rot.get(1, 1)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) PI; firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 2) == -sin(firstAngle - thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(-rot.get(1, 2), rot.get(1, 1))); - } + } else - { + { /* rot.get(0, 0) == cos(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(0, 0)) : -acos(rot.get(0, 0))); /* rot.get(0, 1) == sin(firstAngle) * sin(secondAngle) */ @@ -396,28 +575,28 @@ else if (test == -1) /* rot.get(1, 0) == sin(secondAngle) * sin(thirdAngle) */ /* rot.get(2, 0) == -(cos(thirdAngle) * sin(secondAngle)) */ thirdAngle = (float) atan2(rot.get(1, 0) / sin(secondAngle), -rot.get(2, 0) / sin(secondAngle)); - } + } break; case YXY: test = rot.get(1, 1); /* cos(secondAngle) */ if (test == 1) - { + { secondAngle = (float) 0; firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(0, 0)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) PI; firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == sin(firstAngle - thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(0, 2), rot.get(0, 0))); - } + } else - { + { /* rot.get(1, 1) == cos(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(1, 1)) : -acos(rot.get(1, 1))); /* rot.get(1, 0) == sin(firstAngle) * sin(secondAngle) */ @@ -426,28 +605,28 @@ else if (test == -1) /* rot.get(0, 1) == sin(secondAngle) * sin(thirdAngle) */ /* rot.get(2, 1) == cos(thirdAngle) * sin(secondAngle) */ thirdAngle = (float) atan2(rot.get(0, 1) / sin(secondAngle), rot.get(2, 1) / sin(secondAngle)); - } + } break; case YZY: test = rot.get(1, 1); /* cos(secondAngle) */ if (test == 1) - { + { secondAngle = (float) 0; firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(0, 0)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) PI; firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == -sin(firstAngle - thirdAngle) */ /* rot.get(2, 2) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(-rot.get(0, 2), rot.get(2, 2))); - } + } else - { + { /* rot.get(1, 1) == cos(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(1, 1)) : -acos(rot.get(1, 1))); /* rot.get(1, 2) == sin(firstAngle) * sin(secondAngle) */ @@ -456,28 +635,28 @@ else if (test == -1) /* rot.get(2, 1) == sin(secondAngle) * sin(thirdAngle) */ /* rot.get(0, 1) == -(cos(thirdAngle) * sin(secondAngle)) */ thirdAngle = (float) atan2(rot.get(2, 1) / sin(secondAngle), -rot.get(0, 1) / sin(secondAngle)); - } + } break; case ZYZ: test = rot.get(2, 2); /* cos(secondAngle) */ if (test == 1) - { + { secondAngle = (float) 0; firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 0) == sin(firstAngle + thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(1, 0), rot.get(0, 0)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) PI; firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 1) == sin(firstAngle - thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(0, 1), rot.get(1, 1))); - } + } else - { + { /* rot.get(2, 2) == cos(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(2, 2)) : -acos(rot.get(2, 2))); /* rot.get(2, 1) == sin(firstAngle) * sin(secondAngle) */ @@ -486,28 +665,28 @@ else if (test == -1) /* rot.get(1, 2) == sin(secondAngle) * sin(thirdAngle) */ /* rot.get(0, 2) == cos(thirdAngle) * sin(secondAngle) */ thirdAngle = (float) atan2(rot.get(1, 2) / sin(secondAngle), rot.get(0, 2) / sin(secondAngle)); - } + } break; case ZXZ: test = rot.get(2, 2); /* cos(secondAngle) */ if (test == 1) - { + { secondAngle = (float) 0; firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 0) == sin(firstAngle + thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(1, 0), rot.get(0, 0)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) PI; firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 1) == -sin(firstAngle - thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(-rot.get(0, 1), rot.get(0, 0))); - } + } else - { + { /* rot.get(2, 2) == cos(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? acos(rot.get(2, 2)) : -acos(rot.get(2, 2))); /* rot.get(2, 0) == sin(firstAngle) * sin(secondAngle) */ @@ -516,28 +695,28 @@ else if (test == -1) /* rot.get(0, 2) == sin(secondAngle) * sin(thirdAngle) */ /* rot.get(1, 2) == -(cos(thirdAngle) * sin(secondAngle)) */ thirdAngle = (float) atan2(rot.get(0, 2) / sin(secondAngle), -rot.get(1, 2) / sin(secondAngle)); - } + } break; case XZY: test = rot.get(1, 0); /* sin(secondAngle) */ if (test == 1) - { + { secondAngle = (float) (PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ /* rot.get(2, 2) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(2, 2)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) (-PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(2, 1) == sin(firstAngle - thirdAngle) */ /* rot.get(0, 1) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(2, 1), rot.get(0, 1))); - } + } else - { + { /* rot.get(1, 0) == sin(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? asin(rot.get(1, 0)) : PI - asin(rot.get(1, 0))); /* rot.get(1, 2) == -(cos(secondAngle) * sin(firstAngle)) */ @@ -546,28 +725,28 @@ else if (test == -1) /* rot.get(2, 0) == -(cos(secondAngle) * sin(thirdAngle)) */ /* rot.get(0, 0) == cos(secondAngle) * cos(thirdAngle) */ thirdAngle = (float) atan2(-rot.get(2, 0) / cos(secondAngle), rot.get(0, 0) / cos(secondAngle)); - } + } break; case XYZ: test = rot.get(2, 0); /* -sin(secondAngle) */ if (test == -1) - { + { secondAngle = (float) (PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 1) == sin(firstAngle - thirdAngle) */ /* rot.get(0, 2) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(0, 1), rot.get(0, 2))); - } + } else if (test == 1) - { + { secondAngle = (float) (-PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 1) == -sin(firstAngle + thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(-rot.get(0, 1), rot.get(1, 1)) - firstAngle); - } + } else - { + { /* rot.get(2, 0) == -sin(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? -asin(rot.get(2, 0)) : PI + asin(rot.get(2, 0))); /* rot.get(2, 1) == cos(secondAngle) * sin(firstAngle) */ @@ -576,28 +755,28 @@ else if (test == 1) /* rot.get(1, 0) == cos(secondAngle) * sin(thirdAngle) */ /* rot.get(0, 0) == cos(secondAngle) * cos(thirdAngle) */ thirdAngle = (float) atan2(rot.get(1, 0) / cos(secondAngle), rot.get(0, 0) / cos(secondAngle)); - } + } break; case YXZ: test = rot.get(2, 1); /* sin(secondAngle) */ if (test == 1) - { + { secondAngle = (float) (PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == sin(firstAngle + thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(0, 2), rot.get(0, 0)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) (-PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 2) == sin(firstAngle - thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(0, 2), rot.get(0, 0))); - } + } else - { + { /* rot.get(2, 1) == sin(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? asin(rot.get(2, 1)) : PI - asin(rot.get(2, 1))); /* rot.get(2, 0) == -(cos(secondAngle) * sin(firstAngle)) */ @@ -606,28 +785,28 @@ else if (test == -1) /* rot.get(0, 1) == -(cos(secondAngle) * sin(thirdAngle)) */ /* rot.get(1, 1) == cos(secondAngle) * cos(thirdAngle) */ thirdAngle = (float) atan2(-rot.get(0, 1) / cos(secondAngle), rot.get(1, 1) / cos(secondAngle)); - } + } break; case YZX: test = rot.get(0, 1); /* -sin(secondAngle) */ if (test == -1) - { + { secondAngle = (float) (PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 2) == sin(firstAngle - thirdAngle) */ /* rot.get(1, 0) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(1, 2), rot.get(1, 0))); - } + } else if (test == 1) - { + { secondAngle = (float) (-PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 2) == -sin(firstAngle + thirdAngle) */ /* rot.get(2, 2) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(-rot.get(1, 2), rot.get(2, 2)) - firstAngle); - } + } else - { + { /* rot.get(0, 1) == -sin(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? -asin(rot.get(0, 1)) : PI + asin(rot.get(0, 1))); /* rot.get(0, 2) == cos(secondAngle) * sin(firstAngle) */ @@ -636,28 +815,28 @@ else if (test == 1) /* rot.get(2, 1) == cos(secondAngle) * sin(thirdAngle) */ /* rot.get(1, 1) == cos(secondAngle) * cos(thirdAngle) */ thirdAngle = (float) atan2(rot.get(2, 1) / cos(secondAngle), rot.get(1, 1) / cos(secondAngle)); - } + } break; case ZYX: test = rot.get(0, 2); /* sin(secondAngle) */ if (test == 1) - { + { secondAngle = (float) (PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 0) == sin(firstAngle + thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(rot.get(1, 0), rot.get(1, 1)) - firstAngle); - } + } else if (test == -1) - { + { secondAngle = (float) (-PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(1, 0) == sin(firstAngle - thirdAngle) */ /* rot.get(1, 1) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(1, 0), rot.get(1, 1))); - } + } else - { + { /* rot.get(0, 2) == sin(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? asin(rot.get(0, 2)) : PI - asin(rot.get(0, 2))); /* rot.get(0, 1) == -(cos(secondAngle) * sin(firstAngle)) */ @@ -666,28 +845,28 @@ else if (test == -1) /* rot.get(1, 2) == -(cos(secondAngle) * sin(thirdAngle)) */ /* rot.get(2, 2) == cos(secondAngle) * cos(thirdAngle) */ thirdAngle = (float) atan2(-rot.get(1, 2) / cos(secondAngle), rot.get(2, 2) / cos(secondAngle)); - } + } break; case ZXY: test = rot.get(1, 2); /* -sin(secondAngle) */ if (test == -1) - { + { secondAngle = (float) (PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(2, 0) == sin(firstAngle - thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle - thirdAngle) */ thirdAngle = (float) (firstAngle - atan2(rot.get(2, 0), rot.get(0, 0))); - } + } else if (test == 1) - { + { secondAngle = (float) (-PI / 2); firstAngle = (float) 0; /* arbitrary */ /* rot.get(0, 1) == -sin(firstAngle + thirdAngle) */ /* rot.get(0, 0) == cos(firstAngle + thirdAngle) */ thirdAngle = (float) (atan2(-rot.get(0, 1), rot.get(0, 0)) - firstAngle); - } + } else - { + { /* rot.get(1, 2) == -sin(secondAngle) */ secondAngle = (float) (angleSet == AngleSet.THEONE ? -asin(rot.get(1, 2)) : PI + asin(rot.get(1, 2))); /* rot.get(1, 0) == cos(secondAngle) * sin(firstAngle) */ @@ -696,58 +875,13 @@ else if (test == 1) /* rot.get(0, 2) == cos(secondAngle) * sin(thirdAngle) */ /* rot.get(2, 2) == cos(secondAngle) * cos(thirdAngle) */ thirdAngle = (float) atan2(rot.get(0, 2) / cos(secondAngle), rot.get(2, 2) / cos(secondAngle)); - } + } break; - } - - return new Orientation(axesReference, axesOrder, unit, - unit.fromRadians(firstAngle), unit.fromRadians(secondAngle), unit.fromRadians(thirdAngle), - 0); } - + + return new Orientation(axesReference, axesOrder, unit, + unit.fromRadians(firstAngle), unit.fromRadians(secondAngle), unit.fromRadians(thirdAngle), + 0); + } + } - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -