From 02aa4c85df098ec865458c457d1fdde8d6bf148b Mon Sep 17 00:00:00 2001 From: Alexander Pacha Date: Sun, 15 Dec 2013 15:51:41 +0100 Subject: [PATCH] Added different representation as alternative to a plain float-vector: Matrix, Vector and Quaternion. These representations provide getters, setters and conversion-functions. --- .../representation/Matrix.java | 824 ++++++++++++++++++ .../representation/Matrixf4x4.java | 501 +++++++++++ .../representation/Quaternion.java | 536 ++++++++++++ .../representation/Renderable.java | 39 + .../representation/Vector3f.java | 295 +++++++ .../representation/Vector4f.java | 296 +++++++ 6 files changed, 2491 insertions(+) create mode 100644 src/org/hitlabnz/sensor_fusion_demo/representation/Matrix.java create mode 100644 src/org/hitlabnz/sensor_fusion_demo/representation/Matrixf4x4.java create mode 100644 src/org/hitlabnz/sensor_fusion_demo/representation/Quaternion.java create mode 100644 src/org/hitlabnz/sensor_fusion_demo/representation/Renderable.java create mode 100644 src/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java create mode 100644 src/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java diff --git a/src/org/hitlabnz/sensor_fusion_demo/representation/Matrix.java b/src/org/hitlabnz/sensor_fusion_demo/representation/Matrix.java new file mode 100644 index 0000000..1ca250f --- /dev/null +++ b/src/org/hitlabnz/sensor_fusion_demo/representation/Matrix.java @@ -0,0 +1,824 @@ +package org.hitlabnz.sensor_fusion_demo.representation; + +/** + * Matrix math utilities. These methods operate on OpenGL ES format + * matrices and vectors stored in float arrays. + * + * 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 row x 1 column 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 static final float[] TEMP_MATRIX_ARRAY = new float[32]; + + /** + * Multiply two 4x4 matrices together and store 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 void multiplyMM(float[] result, int resultOffset, + * float[] lhs, int lhsOffset, float[] rhs, int rhsOffset){ + * android.opengl.Matrix.multiplyMM(result, resultOffset, lhs, lhsOffset, rhs, rhsOffset); + * } + */ + + public static void multiplyMM(float[] output, int outputOffset, float[] lhs, int lhsOffset, float[] rhs, + int rhsOffset) { + //for(int i = 0; i < 4; i++){ + // for(int j = 0; j < 4; j++){ + + // int k = i * 4; + // output[outputOffset + 0 + j] += lhs[lhsOffset + k + j] * rhs[rhsOffset + 0 * 4 + i]; + // output[outputOffset + 1 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 1 * 4 + i]; + // output[outputOffset + 2 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 2 * 4 + i]; + // output[outputOffset + 3 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 3 * 4 + i]; + // } + //} + output[outputOffset + 0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 4] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 8] * rhs[rhsOffset + 2] + lhs[lhsOffset + 12] * rhs[rhsOffset + 3]; + output[outputOffset + 1] = lhs[lhsOffset + 1] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 9] * rhs[rhsOffset + 2] + lhs[lhsOffset + 13] * rhs[rhsOffset + 3]; + output[outputOffset + 2] = lhs[lhsOffset + 2] * rhs[rhsOffset + 0] + lhs[lhsOffset + 6] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 14] * rhs[rhsOffset + 3]; + output[outputOffset + 3] = lhs[lhsOffset + 3] * rhs[rhsOffset + 0] + lhs[lhsOffset + 7] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 11] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; + + output[outputOffset + 4] = lhs[lhsOffset + 0] * rhs[rhsOffset + 4] + lhs[lhsOffset + 4] * rhs[rhsOffset + 5] + + lhs[lhsOffset + 8] * rhs[rhsOffset + 6] + lhs[lhsOffset + 12] * rhs[rhsOffset + 7]; + output[outputOffset + 5] = lhs[lhsOffset + 1] * rhs[rhsOffset + 4] + lhs[lhsOffset + 5] * rhs[rhsOffset + 5] + + lhs[lhsOffset + 9] * rhs[rhsOffset + 6] + lhs[lhsOffset + 13] * rhs[rhsOffset + 7]; + output[outputOffset + 6] = lhs[lhsOffset + 2] * rhs[rhsOffset + 4] + lhs[lhsOffset + 6] * rhs[rhsOffset + 5] + + lhs[lhsOffset + 10] * rhs[rhsOffset + 6] + lhs[lhsOffset + 14] * rhs[rhsOffset + 7]; + output[outputOffset + 7] = lhs[lhsOffset + 3] * rhs[rhsOffset + 4] + lhs[lhsOffset + 7] * rhs[rhsOffset + 5] + + lhs[lhsOffset + 11] * rhs[rhsOffset + 6] + lhs[lhsOffset + 15] * rhs[rhsOffset + 7]; + + output[outputOffset + 8] = lhs[lhsOffset + 0] * rhs[rhsOffset + 8] + lhs[lhsOffset + 4] * rhs[rhsOffset + 9] + + lhs[lhsOffset + 8] * rhs[rhsOffset + 10] + lhs[lhsOffset + 12] * rhs[rhsOffset + 11]; + output[outputOffset + 9] = lhs[lhsOffset + 1] * rhs[rhsOffset + 8] + lhs[lhsOffset + 5] * rhs[rhsOffset + 9] + + lhs[lhsOffset + 9] * rhs[rhsOffset + 10] + lhs[lhsOffset + 13] * rhs[rhsOffset + 11]; + output[outputOffset + 10] = lhs[lhsOffset + 2] * rhs[rhsOffset + 8] + lhs[lhsOffset + 6] * rhs[rhsOffset + 9] + + lhs[lhsOffset + 10] * rhs[rhsOffset + 10] + lhs[lhsOffset + 14] * rhs[rhsOffset + 11]; + output[outputOffset + 11] = lhs[lhsOffset + 3] * rhs[rhsOffset + 8] + lhs[lhsOffset + 7] * rhs[rhsOffset + 9] + + lhs[lhsOffset + 11] * rhs[rhsOffset + 10] + lhs[lhsOffset + 15] * rhs[rhsOffset + 11]; + + output[outputOffset + 12] = lhs[lhsOffset + 0] * rhs[rhsOffset + 12] + lhs[lhsOffset + 4] * rhs[rhsOffset + 13] + + lhs[lhsOffset + 8] * rhs[rhsOffset + 14] + lhs[lhsOffset + 12] * rhs[rhsOffset + 15]; + output[outputOffset + 13] = lhs[lhsOffset + 1] * rhs[rhsOffset + 12] + lhs[lhsOffset + 5] * rhs[rhsOffset + 13] + + lhs[lhsOffset + 9] * rhs[rhsOffset + 14] + lhs[lhsOffset + 13] * rhs[rhsOffset + 15]; + output[outputOffset + 14] = lhs[lhsOffset + 2] * rhs[rhsOffset + 12] + lhs[lhsOffset + 6] * rhs[rhsOffset + 13] + + lhs[lhsOffset + 10] * rhs[rhsOffset + 14] + lhs[lhsOffset + 14] * rhs[rhsOffset + 15]; + output[outputOffset + 15] = lhs[lhsOffset + 3] * rhs[rhsOffset + 12] + lhs[lhsOffset + 7] * rhs[rhsOffset + 13] + + lhs[lhsOffset + 11] * rhs[rhsOffset + 14] + lhs[lhsOffset + 15] * rhs[rhsOffset + 15]; + } + + public static void multiplyMM(float[] output, float[] lhs, float[] rhs) { + output[0] = lhs[0] * rhs[0] + lhs[4] * rhs[1] + lhs[8] * rhs[2] + lhs[12] * rhs[3]; + output[1] = lhs[1] * rhs[0] + lhs[5] * rhs[1] + lhs[9] * rhs[2] + lhs[13] * rhs[3]; + output[2] = lhs[2] * rhs[0] + lhs[6] * rhs[1] + lhs[10] * rhs[2] + lhs[14] * rhs[3]; + output[3] = lhs[3] * rhs[0] + lhs[7] * rhs[1] + lhs[11] * rhs[2] + lhs[15] * rhs[3]; + + output[4] = lhs[0] * rhs[4] + lhs[4] * rhs[5] + lhs[8] * rhs[6] + lhs[12] * rhs[7]; + output[5] = lhs[1] * rhs[4] + lhs[5] * rhs[5] + lhs[9] * rhs[6] + lhs[13] * rhs[7]; + output[6] = lhs[2] * rhs[4] + lhs[6] * rhs[5] + lhs[10] * rhs[6] + lhs[14] * rhs[7]; + output[7] = lhs[3] * rhs[4] + lhs[7] * rhs[5] + lhs[11] * rhs[6] + lhs[15] * rhs[7]; + + output[8] = lhs[0] * rhs[8] + lhs[4] * rhs[9] + lhs[8] * rhs[10] + lhs[12] * rhs[11]; + output[9] = lhs[1] * rhs[8] + lhs[5] * rhs[9] + lhs[9] * rhs[10] + lhs[13] * rhs[11]; + output[10] = lhs[2] * rhs[8] + lhs[6] * rhs[9] + lhs[10] * rhs[10] + lhs[14] * rhs[11]; + output[11] = lhs[3] * rhs[8] + lhs[7] * rhs[9] + lhs[11] * rhs[10] + lhs[15] * rhs[11]; + + output[12] = lhs[0] * rhs[12] + lhs[4] * rhs[13] + lhs[8] * rhs[14] + lhs[12] * rhs[15]; + output[13] = lhs[1] * rhs[12] + lhs[5] * rhs[13] + lhs[9] * rhs[14] + lhs[13] * rhs[15]; + output[14] = lhs[2] * rhs[12] + lhs[6] * rhs[13] + lhs[10] * rhs[14] + lhs[14] * rhs[15]; + output[15] = lhs[3] * rhs[12] + lhs[7] * rhs[13] + lhs[11] * rhs[14] + lhs[15] * rhs[15]; + } + + /** + * Multiply a 4 element vector by a 4x4 matrix and store 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 void multiplyMV(float[] resultVec, + * int resultVecOffset, float[] lhsMat, int lhsMatOffset, + * float[] rhsVec, int rhsVecOffset){ + * android.opengl.Matrix.multiplyMV(resultVec, resultVecOffset, lhsMat, lhsMatOffset, rhsVec, rhsVecOffset); + * } */ + public static void multiplyMV(float[] output, int outputOffset, float[] lhs, int lhsOffset, float[] rhs, + int rhsOffset) { + /* wrong implementation (this is for row major matrices) + * output[outputOffset +0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 1] * rhs[rhsOffset + 1] + * + lhs[lhsOffset + 2] * rhs[rhsOffset + 2] + lhs[lhsOffset + 3] * rhs[rhsOffset + 3]; + * output[outputOffset +1] = lhs[lhsOffset + 4] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] + + * lhs[lhsOffset + 6] * rhs[rhsOffset + 2] + lhs[lhsOffset + 7] * rhs[rhsOffset + 3]; + * output[outputOffset +2] = lhs[lhsOffset + 8] * rhs[rhsOffset + 0] + lhs[lhsOffset + 9] * rhs[rhsOffset + 1] + + * lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 11] * rhs[rhsOffset + 3]; + * output[outputOffset +3] = lhs[lhsOffset + 12] * rhs[rhsOffset + 0] + lhs[lhsOffset + 13] * rhs[rhsOffset + 1] + * + lhs[lhsOffset + 14] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; */ + // correct implementation for column major matrices (which is for OpenGL) + output[outputOffset + 0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 4] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 8] * rhs[rhsOffset + 2] + lhs[lhsOffset + 12] * rhs[rhsOffset + 3]; + output[outputOffset + 1] = lhs[lhsOffset + 1] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 9] * rhs[rhsOffset + 2] + lhs[lhsOffset + 13] * rhs[rhsOffset + 3]; + output[outputOffset + 2] = lhs[lhsOffset + 2] * rhs[rhsOffset + 0] + lhs[lhsOffset + 6] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 14] * rhs[rhsOffset + 3]; + output[outputOffset + 3] = lhs[lhsOffset + 3] * rhs[rhsOffset + 0] + lhs[lhsOffset + 7] * rhs[rhsOffset + 1] + + lhs[lhsOffset + 11] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; + + } + + public static void multiplyMV(float[] outputV, float[] inputM, float[] inputV) { + outputV[0] = inputM[0] * inputV[0] + inputM[4] * inputV[1] + inputM[8] * inputV[2] + inputM[12] * inputV[3]; + outputV[1] = inputM[1] * inputV[0] + inputM[5] * inputV[1] + inputM[9] * inputV[2] + inputM[13] * inputV[3]; + outputV[2] = inputM[2] * inputV[0] + inputM[6] * inputV[1] + inputM[10] * inputV[2] + inputM[14] * inputV[3]; + outputV[3] = inputM[3] * inputV[0] + inputM[7] * inputV[1] + inputM[11] * inputV[2] + inputM[15] * inputV[3]; + } + + public static void multiplyMV3(float[] outputV, float[] inputM, float[] inputV, float w) { + outputV[0] = inputM[0] * inputV[0] + inputM[4] * inputV[1] + inputM[8] * inputV[2] + inputM[12] * w; + outputV[1] = inputM[1] * inputV[0] + inputM[5] * inputV[1] + inputM[9] * inputV[2] + inputM[13] * w; + outputV[2] = inputM[2] * inputV[0] + inputM[6] * inputV[1] + inputM[10] * inputV[2] + inputM[14] * w; + } + + /** + * Transposes a 4 x 4 matrix. + * + * @param mTrans the array that holds the output inverted matrix + * @param mTransOffset an offset into mInv where the inverted matrix is + * stored. + * @param m the input array + * @param mOffset an offset into m where the 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. + * + * @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 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; + } + + /** + * Define a projection matrix in terms of six 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 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 = 2.0f * ((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; + } + + /** + * Define 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 + * + * @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 + * + * @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) + * + * @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 scale factor x + * @param y scale factor y + * @param z scale factor z + */ + public static void rotateM(float[] rm, int rmOffset, float[] m, int mOffset, float a, float x, float y, float z) { + synchronized (TEMP_MATRIX_ARRAY) { + setRotateM(TEMP_MATRIX_ARRAY, 0, a, x, y, z); + multiplyMM(rm, rmOffset, m, mOffset, TEMP_MATRIX_ARRAY, 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 scale factor x + * @param y scale factor y + * @param z scale factor z + */ + public static void rotateM(float[] m, int mOffset, float a, float x, float y, float z) { + synchronized (TEMP_MATRIX_ARRAY) { + setRotateM(TEMP_MATRIX_ARRAY, 0, a, x, y, z); + multiplyMM(TEMP_MATRIX_ARRAY, 16, m, mOffset, TEMP_MATRIX_ARRAY, 0); + System.arraycopy(TEMP_MATRIX_ARRAY, 16, m, mOffset, 16); + } + } + + /** + * Rotates matrix m by angle a (in degrees) around the axis (x, y, z) + * + * @param rm returns the result + * @param rmOffset index into rm where the result matrix starts + * @param a angle to rotate in degrees + * @param x scale factor x + * @param y scale factor y + * @param z scale factor z + */ + 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; + } + + /** + * Define 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); + } +} \ No newline at end of file diff --git a/src/org/hitlabnz/sensor_fusion_demo/representation/Matrixf4x4.java b/src/org/hitlabnz/sensor_fusion_demo/representation/Matrixf4x4.java new file mode 100644 index 0000000..9d0966c --- /dev/null +++ b/src/org/hitlabnz/sensor_fusion_demo/representation/Matrixf4x4.java @@ -0,0 +1,501 @@ +package org.hitlabnz.sensor_fusion_demo.representation; + +import android.util.Log; + +/** + * The Class Matrixf4x4. + * + * Internal the matrix is structured as + * + * [ x0 , y0 , z0 , w0 ] [ x1 , y1 , z1 , w1 ] [ x2 , y2 , z2 , w2 ] [ x3 , y3 , z3 , w3 ] + * + * it is recommend that when setting the matrix values individually that you use the set{x,#} methods, where 'x' is + * either x, y, z or w and # is either 0, 1, 2 or 3, setY1 for example. The reason you should use these functions is + * because it will map directly to that part of the matrix regardless of whether or not the internal matrix is column + * major or not. If the matrix is either or length 9 or 16 it will be able to determine if it can set the value or not. + * If the matrix is of size 9 but you set say w2, the value will not be set and the set method will return without any + * error. + * + */ +public class Matrixf4x4 { + + public static final int[] matIndCol9_3x3 = { 0, 1, 2, 3, 4, 5, 6, 7, 8 }; + public static final int[] matIndCol16_3x3 = { 0, 1, 2, 4, 5, 6, 8, 9, 10 }; + public static final int[] matIndRow9_3x3 = { 0, 3, 6, 1, 4, 7, 3, 5, 8 }; + public static final int[] matIndRow16_3x3 = { 0, 4, 8, 1, 5, 9, 2, 6, 10 }; + + public static final int[] matIndCol16_4x4 = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; + public static final int[] matIndRow16_4x4 = { 0, 4, 8, 12, 1, 5, 9, 13, 2, 6, 10, 14, 3, 7, 11, 15 }; + + private boolean colMaj = true; + private boolean matrixValid = false; + + /** The matrix. */ + public float[] matrix; + + /** + * Instantiates a new matrixf4x4. The Matrix is assumed to be Column major, however you can change this by using the + * setColumnMajor function to false and it will operate like a row major matrix. + */ + public Matrixf4x4() { + // The matrix is defined as float[column][row] + this.matrix = new float[16]; + Matrix.setIdentityM(this.matrix, 0); + matrixValid = true; + } + + /** + * Gets the matrix. + * + * @return the matrix, can be null if the matrix is invalid + */ + public float[] getMatrix() { + return this.matrix; + } + + public int size() { + return matrix.length; + } + + /** + * Sets the matrix from a float[16] array. If the matrix you set isn't 16 long then the matrix will be set as + * invalid. + * + * @param matrix the new matrix + */ + public void setMatrix(float[] matrix) { + this.matrix = matrix; + + if (matrix.length == 16 || matrix.length == 9) + this.matrixValid = true; + else { + this.matrixValid = false; + Log.e("matrix", "Matrix set is invalid, size is " + matrix.length + " expected 9 or 16"); + } + } + + /** + * Set whether the internal data is col major by passing true, or false for a row major matrix. The matrix is column + * major by default. + * + * @param colMajor + */ + public void setColumnMajor(boolean colMajor) { + this.colMaj = colMajor; + } + + /** + * Find out if the stored matrix is column major + * + * @return + */ + public boolean isColumnMajor() { + return colMaj; + } + + /** + * Multiply the given vector by this matrix. This should only be used if the matrix is of size 16 (use the + * matrix.size() method). + * + * @param vector A vector of length 4. + */ + public void multiplyVector4fByMatrix(Vector4f vector) { + + if (matrixValid && matrix.length == 16) { + float x = 0; + float y = 0; + float z = 0; + float w = 0; + + float[] vectorArray = vector.ToArray(); + + if (colMaj) { + for (int i = 0; i < 4; i++) { + + int k = i * 4; + + x += this.matrix[k + 0] * vectorArray[i]; + y += this.matrix[k + 1] * vectorArray[i]; + z += this.matrix[k + 2] * vectorArray[i]; + w += this.matrix[k + 3] * vectorArray[i]; + } + } else { + for (int i = 0; i < 4; i++) { + + x += this.matrix[0 + i] * vectorArray[i]; + y += this.matrix[4 + i] * vectorArray[i]; + z += this.matrix[8 + i] * vectorArray[i]; + w += this.matrix[12 + i] * vectorArray[i]; + } + } + + vector.setX(x); + vector.setY(y); + vector.setZ(z); + vector.setW(w); + } else + Log.e("matrix", "Matrix is invalid, is " + matrix.length + " long, this equation expects a 16 value matrix"); + } + + /** + * Multiply the given vector by this matrix. This should only be used if the matrix is of size 9 (use the + * matrix.size() method). + * + * @param vector A vector of length 3. + */ + public void multiplyVector3fByMatrix(Vector3f vector) { + + if (matrixValid && matrix.length == 9) { + float x = 0; + float y = 0; + float z = 0; + + float[] vectorArray = vector.toArray(); + + if (!colMaj) { + for (int i = 0; i < 3; i++) { + + int k = i * 3; + + x += this.matrix[k + 0] * vectorArray[i]; + y += this.matrix[k + 1] * vectorArray[i]; + z += this.matrix[k + 2] * vectorArray[i]; + } + } else { + for (int i = 0; i < 3; i++) { + + x += this.matrix[0 + i] * vectorArray[i]; + y += this.matrix[3 + i] * vectorArray[i]; + z += this.matrix[6 + i] * vectorArray[i]; + } + } + + vector.setX(x); + vector.setY(y); + vector.setZ(z); + } else + Log.e("matrix", "Matrix is invalid, is " + matrix.length + + " long, this function expects the internal matrix to be of size 9"); + } + + public boolean isMatrixValid() { + return matrixValid; + } + + /** + * Multiply matrix4x4 by matrix. + * + * @param matrixf the matrixf + */ + public void multiplyMatrix4x4ByMatrix(Matrixf4x4 matrixf) { + + // TODO implement Strassen Algorithm in place of this slower naive one. + if (matrixValid && matrixf.isMatrixValid()) { + float[] bufferMatrix = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + float[] matrix = matrixf.getMatrix(); + + /** + * for(int i = 0; i < 4; i++){ for(int j = 0; j < 4; j++){ + * + * int k = i * 4; bufferMatrix[0 + j] += this.matrix[k + j] * matrix[0 * 4 + i]; bufferMatrix[1 * 4 + j] += + * this.matrix[k + j] * matrix[1 * 4 + i]; bufferMatrix[2 * 4 + j] += this.matrix[k + j] * matrix[2 * 4 + + * i]; bufferMatrix[3 * 4 + j] += this.matrix[k + j] * matrix[3 * 4 + i]; } } + */ + + multiplyMatrix(matrix, 0, bufferMatrix, 0); + + matrixf.setMatrix(bufferMatrix); + } else + Log.e("matrix", "Matrix is invalid, internal is " + matrix.length + " long" + " , input matrix is " + + matrixf.getMatrix().length + " long"); + + } + + public void multiplyMatrix(float[] input, int inputOffset, float[] output, int outputOffset) { + float[] bufferMatrix = output; + float[] matrix = input; + + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + + int k = i * 4; + bufferMatrix[outputOffset + 0 + j] += this.matrix[k + j] * matrix[inputOffset + 0 * 4 + i]; + bufferMatrix[outputOffset + 1 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 1 * 4 + i]; + bufferMatrix[outputOffset + 2 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 2 * 4 + i]; + bufferMatrix[outputOffset + 3 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 3 * 4 + i]; + } + } + } + + /** + * This will rearrange the internal structure of the matrix. Be careful though as this is an expensive operation. + */ + public void transpose() { + if (matrixValid) { + if (this.matrix.length == 16) { + float[] newMatrix = new float[16]; + for (int i = 0; i < 4; i++) { + + int k = i * 4; + + newMatrix[k] = matrix[i]; + newMatrix[k + 1] = matrix[4 + i]; + newMatrix[k + 2] = matrix[8 + i]; + newMatrix[k + 3] = matrix[12 + i]; + } + matrix = newMatrix; + + } else { + float[] newMatrix = new float[9]; + for (int i = 0; i < 3; i++) { + + int k = i * 3; + + newMatrix[k] = matrix[i]; + newMatrix[k + 1] = matrix[3 + i]; + newMatrix[k + 2] = matrix[6 + i]; + } + matrix = newMatrix; + } + } + + } + + public void setX0(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[0]] = value; + else + matrix[matIndRow16_3x3[0]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[0]] = value; + else + matrix[matIndRow9_3x3[0]] = value; + } + } + } + + public void setX1(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[1]] = value; + else + matrix[matIndRow16_3x3[1]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[1]] = value; + else + matrix[matIndRow9_3x3[1]] = value; + } + } + } + + public void setX2(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[2]] = value; + else + matrix[matIndRow16_3x3[2]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[2]] = value; + else + matrix[matIndRow9_3x3[2]] = value; + } + } + } + + public void setY0(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[3]] = value; + else + matrix[matIndRow16_3x3[3]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[3]] = value; + else + matrix[matIndRow9_3x3[3]] = value; + } + } + } + + public void setY1(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[4]] = value; + else + matrix[matIndRow16_3x3[4]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[4]] = value; + else + matrix[matIndRow9_3x3[4]] = value; + } + } + } + + public void setY2(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[5]] = value; + else + matrix[matIndRow16_3x3[5]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[5]] = value; + else + matrix[matIndRow9_3x3[5]] = value; + } + } + } + + public void setZ0(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[6]] = value; + else + matrix[matIndRow16_3x3[6]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[6]] = value; + else + matrix[matIndRow9_3x3[6]] = value; + } + } + } + + public void setZ1(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[7]] = value; + else + matrix[matIndRow16_3x3[7]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[7]] = value; + else + matrix[matIndRow9_3x3[7]] = value; + } + } + } + + public void setZ2(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_3x3[8]] = value; + else + matrix[matIndRow16_3x3[8]] = value; + } else { + if (colMaj) + matrix[matIndCol9_3x3[8]] = value; + else + matrix[matIndRow9_3x3[8]] = value; + } + } + } + + public void setX3(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[3]] = value; + else + matrix[matIndRow16_4x4[3]] = value; + } + } + } + + public void setY3(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[7]] = value; + else + matrix[matIndRow16_4x4[7]] = value; + } + } + } + + public void setZ3(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[11]] = value; + else + matrix[matIndRow16_4x4[11]] = value; + } + } + } + + public void setW0(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[12]] = value; + else + matrix[matIndRow16_4x4[12]] = value; + } + } + } + + public void setW1(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[13]] = value; + else + matrix[matIndRow16_4x4[13]] = value; + } + } + } + + public void setW2(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[14]] = value; + else + matrix[matIndRow16_4x4[14]] = value; + } + } + } + + public void setW3(float value) { + + if (matrixValid) { + if (matrix.length == 16) { + if (colMaj) + matrix[matIndCol16_4x4[15]] = value; + else + matrix[matIndRow16_4x4[15]] = value; + } + } + } + +} diff --git a/src/org/hitlabnz/sensor_fusion_demo/representation/Quaternion.java b/src/org/hitlabnz/sensor_fusion_demo/representation/Quaternion.java new file mode 100644 index 0000000..7c613e0 --- /dev/null +++ b/src/org/hitlabnz/sensor_fusion_demo/representation/Quaternion.java @@ -0,0 +1,536 @@ +package org.hitlabnz.sensor_fusion_demo.representation; + +/** + * The Quaternion class. A Quaternion is a four-dimensional vector that is used to represent rotations of a rigid body + * in the 3D space. It is very similar to a rotation vector; it contains an angle, encoded into the w component + * and three components to describe the rotation-axis (encoded into x, y, z). + * + *

+ * Quaternions allow for elegant descriptions of 3D rotations, interpolations as well as extrapolations and compared to + * Euler angles, they don't suffer from gimbal lock. Interpolations between two Quaternions are called SLERP (Spherical + * Linear Interpolation). + *

+ * + *

+ * This class also contains the representation of the same rotation as a Quaternion and 4x4-Rotation-Matrix. + *

+ * + * @author Leigh Beattie, Alexander Pacha + * + */ +public class Quaternion extends Vector4f { + + /** + * A randomly generated UID to make the Quaternion object serialisable. + */ + private static final long serialVersionUID = -7148812599404359073L; + + /** + * Rotation matrix that contains the same rotation as the Quaternion in a 4x4 homogenised rotation matrix. + * Remember that for performance reasons, this matrix is only updated, when it is accessed and not on every change + * of the quaternion-values. + */ + private Matrixf4x4 matrix; + + /** + * This variable is used to synchronise the rotation matrix with the current quaternion values. If someone has + * changed the + * quaternion numbers then the matrix will need to be updated. To save on processing we only really want to update + * the matrix when someone wants to fetch it, instead of whenever someone sets a quaternion value. + */ + private boolean dirty = false; + + /** + * Creates a new Quaternion object and initialises it with the identity Quaternion + */ + public Quaternion() { + super(); + matrix = new Matrixf4x4(); + loadIdentityQuat(); + } + + @Override + public Quaternion clone() { + Quaternion clone = new Quaternion(); + clone.copyVec4(this); + return clone; + } + + /** + * Normalise this Quaternion into a unity Quaternion. + */ + public void normalise() { + this.dirty = true; + float mag = (float) Math.sqrt(points[3] * points[3] + points[0] * points[0] + points[1] * points[1] + points[2] + * points[2]); + points[3] = points[3] / mag; + points[0] = points[0] / mag; + points[1] = points[1] / mag; + points[2] = points[2] / mag; + } + + @Override + public void normalize() { + normalise(); + } + + /** + * Copies the values from the given quaternion to this one + * + * @param quat The quaternion to copy from + */ + public void set(Quaternion quat) { + this.dirty = true; + copyVec4(quat); + } + + /** + * Multiply this quaternion by the input quaternion and store the result in the out quaternion + * + * @param input + * @param output + */ + public void multiplyByQuat(Quaternion input, Quaternion output) { + Vector4f inputCopy = new Vector4f(); + if (input != output) { + output.points[3] = (points[3] * input.points[3] - points[0] * input.points[0] - points[1] * input.points[1] - points[2] + * input.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2 + output.points[0] = (points[3] * input.points[0] + points[0] * input.points[3] + points[1] * input.points[2] - points[2] + * input.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2 + output.points[1] = (points[3] * input.points[1] + points[1] * input.points[3] + points[2] * input.points[0] - points[0] + * input.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2 + output.points[2] = (points[3] * input.points[2] + points[2] * input.points[3] + points[0] * input.points[1] - points[1] + * input.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2 + } else { + inputCopy.points[0] = input.points[0]; + inputCopy.points[1] = input.points[1]; + inputCopy.points[2] = input.points[2]; + inputCopy.points[3] = input.points[3]; + + output.points[3] = (points[3] * inputCopy.points[3] - points[0] * inputCopy.points[0] - points[1] + * inputCopy.points[1] - points[2] * inputCopy.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2 + output.points[0] = (points[3] * inputCopy.points[0] + points[0] * inputCopy.points[3] + points[1] + * inputCopy.points[2] - points[2] * inputCopy.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2 + output.points[1] = (points[3] * inputCopy.points[1] + points[1] * inputCopy.points[3] + points[2] + * inputCopy.points[0] - points[0] * inputCopy.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2 + output.points[2] = (points[3] * inputCopy.points[2] + points[2] * inputCopy.points[3] + points[0] + * inputCopy.points[1] - points[1] * inputCopy.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2 + } + } + + /** + * Multiply this quaternion by the input quaternion and store the result in the out quaternion + * + * @param input + * @param output + */ + Quaternion bufferQuaternion; + + public void multiplyByQuat(Quaternion input) { + if (bufferQuaternion == null) { + bufferQuaternion = new Quaternion(); + } + this.dirty = true; + bufferQuaternion.copyVec4(this); + multiplyByQuat(input, bufferQuaternion); + this.copyVec4(bufferQuaternion); + } + + /** + * Multiplies this Quaternion with a scalar + * + * @param scalar the value that the vector should be multiplied with + */ + public void multiplyByScalar(float scalar) { + this.dirty = true; + multiplyByScalar(scalar); + } + + /** + * Add a quaternion to this quaternion + * + * @param input The quaternion that you want to add to this one + */ + public void addQuat(Quaternion input) { + this.dirty = true; + addQuat(input, this); + } + + /** + * Add this quaternion and another quaternion together and store the result in the output quaternion + * + * @param input The quaternion you want added to this quaternion + * @param output The quaternion you want to store the output in. + */ + public void addQuat(Quaternion input, Quaternion output) { + output.setX(getX() + input.getX()); + output.setY(getY() + input.getY()); + output.setZ(getZ() + input.getZ()); + output.setW(getW() + input.getW()); + } + + /** + * Subtract a quaternion to this quaternion + * + * @param input The quaternion that you want to subtracted from this one + */ + public void subQuat(Quaternion input) { + this.dirty = true; + subQuat(input, this); + } + + /** + * Subtract another quaternion from this quaternion and store the result in the output quaternion + * + * @param input The quaternion you want subtracted from this quaternion + * @param output The quaternion you want to store the output in. + */ + public void subQuat(Quaternion input, Quaternion output) { + output.setX(getX() - input.getX()); + output.setY(getY() - input.getY()); + output.setZ(getZ() - input.getZ()); + output.setW(getW() - input.getW()); + } + + /** + * Converts this Quaternion into the Rotation-Matrix representation which can be accessed by + * {@link Quaternion#getMatrix4x4 getMatrix4x4} + */ + private void convertQuatToMatrix() { + float x = points[0]; + float y = points[1]; + float z = points[2]; + float w = points[3]; + + matrix.setX0(1 - 2 * (y * y) - 2 * (z * z)); //1 - 2y2 - 2z2 + matrix.setX1(2 * (x * y) + 2 * (w * z)); // 2xy - 2wz + matrix.setX2(2 * (x * z) - 2 * (w * y)); //2xz + 2wy + matrix.setX3(0); + matrix.setY0(2 * (x * y) - 2 * (w * z)); //2xy + 2wz + matrix.setY1(1 - 2 * (x * x) - 2 * (z * z)); //1 - 2x2 - 2z2 + matrix.setY2(2 * (y * z) + 2 * (w * x)); // 2yz + 2wx + matrix.setY3(0); + matrix.setZ0(2 * (x * z) + 2 * (w * y)); //2xz + 2wy + matrix.setZ1(2 * (y * z) - 2 * (w * x)); //2yz - 2wx + matrix.setZ2(1 - 2 * (x * x) - 2 * (y * y)); //1 - 2x2 - 2y2 + matrix.setZ3(0); + matrix.setW0(0); + matrix.setW1(0); + matrix.setW2(0); + matrix.setW3(1); + } + + /** + * Get an axis angle representation of this quaternion. + * + * @param output Vector4f axis angle. + */ + public void toAxisAngle(Vector4f output) { + if (getW() > 1) { + normalise(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised + } + float angle = 2 * (float) Math.toDegrees(Math.acos(getW())); + float x; + float y; + float z; + + float s = (float) Math.sqrt(1 - getW() * getW()); // assuming quaternion normalised then w is less than 1, so term always positive. + if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt + // if s close to zero then direction of axis not important + x = points[0]; // if it is important that axis is normalised then replace with x=1; y=z=0; + y = points[1]; + z = points[2]; + } else { + x = points[0] / s; // normalise axis + y = points[1] / s; + z = points[2] / s; + } + + output.points[0] = x; + output.points[1] = y; + output.points[2] = z; + output.points[3] = angle; + } + + /** + * Returns the heading, attitude and bank of this quaternion as euler angles in the double array respectively + * + * @return An array of size 3 containing the euler angles for this quaternion + */ + public double[] toEulerAngles() { + double[] ret = new double[3]; + + ret[0] = Math.atan2(2 * points[1] * getW() - 2 * points[0] * points[2], 1 - 2 * (points[1] * points[1]) - 2 + * (points[2] * points[2])); // atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2) + ret[1] = Math.asin(2 * points[0] * points[1] + 2 * points[2] * getW()); // asin(2*qx*qy + 2*qz*qw) + ret[2] = Math.atan2(2 * points[0] * getW() - 2 * points[1] * points[2], 1 - 2 * (points[0] * points[0]) - 2 + * (points[2] * points[2])); // atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2) + + return ret; + } + + /** + * Sets the quaternion to an identity quaternion of 0,0,0,1. + */ + public void loadIdentityQuat() { + this.dirty = true; + setX(0); + setY(0); + setZ(0); + setW(1); + } + + @Override + public String toString() { + return "{X: " + getX() + ", Y:" + getY() + ", Z:" + getZ() + ", W:" + getW() + "}"; + } + + /** + * This is an internal method used to build a quaternion from a rotation matrix and then sets the current quaternion + * from that matrix. + * + */ + private void generateQuaternionFromMatrix() { + + float qx; + float qy; + float qz; + float qw; + + float[] mat = matrix.getMatrix(); + int[] indices = null; + + if (this.matrix.size() == 16) { + if (this.matrix.isColumnMajor()) { + indices = Matrixf4x4.matIndCol16_3x3; + } else { + indices = Matrixf4x4.matIndRow16_3x3; + } + } else { + if (this.matrix.isColumnMajor()) { + indices = Matrixf4x4.matIndCol9_3x3; + } else { + indices = Matrixf4x4.matIndRow9_3x3; + } + } + + int m00 = indices[0]; + int m01 = indices[1]; + int m02 = indices[2]; + + int m10 = indices[3]; + int m11 = indices[4]; + int m12 = indices[5]; + + int m20 = indices[6]; + int m21 = indices[7]; + int m22 = indices[8]; + + float tr = mat[m00] + mat[m11] + mat[m22]; + if (tr > 0) { + float s = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw + qw = 0.25f * s; + qx = (mat[m21] - mat[m12]) / s; + qy = (mat[m02] - mat[m20]) / s; + qz = (mat[m10] - mat[m01]) / s; + } else if ((mat[m00] > mat[m11]) & (mat[m00] > mat[m22])) { + float s = (float) Math.sqrt(1.0 + mat[m00] - mat[m11] - mat[m22]) * 2; // S=4*qx + qw = (mat[m21] - mat[m12]) / s; + qx = 0.25f * s; + qy = (mat[m01] + mat[m10]) / s; + qz = (mat[m02] + mat[m20]) / s; + } else if (mat[m11] > mat[m22]) { + float s = (float) Math.sqrt(1.0 + mat[m11] - mat[m00] - mat[m22]) * 2; // S=4*qy + qw = (mat[m02] - mat[m20]) / s; + qx = (mat[m01] + mat[m10]) / s; + qy = 0.25f * s; + qz = (mat[m12] + mat[m21]) / s; + } else { + float s = (float) Math.sqrt(1.0 + mat[m22] - mat[m00] - mat[m11]) * 2; // S=4*qz + qw = (mat[m10] - mat[m01]) / s; + qx = (mat[m02] + mat[m20]) / s; + qy = (mat[m12] + mat[m21]) / s; + qz = 0.25f * s; + } + + setX(qx); + setY(qy); + setZ(qz); + setW(qw); + } + + /** + * You can set the values for this quaternion based off a rotation matrix. If the matrix you supply is not a + * rotation matrix this will fail. You MUST provide a 4x4 matrix. + * + * @param matrix A column major rotation matrix + */ + public void setColumnMajor(float[] matrix) { + + this.matrix.setMatrix(matrix); + this.matrix.setColumnMajor(true); + + generateQuaternionFromMatrix(); + } + + /** + * You can set the values for this quaternion based off a rotation matrix. If the matrix you supply is not a + * rotation matrix this will fail. + * + * @param matrix A column major rotation matrix + */ + public void setRowMajor(float[] matrix) { + + this.matrix.setMatrix(matrix); + this.matrix.setColumnMajor(false); + + generateQuaternionFromMatrix(); + } + + /** + * Set this quaternion from axis angle values. All rotations are in degrees. + * + * @param azimuth The rotation around the z axis + * @param pitch The rotation around the y axis + * @param roll The rotation around the x axis + */ + public void setEulerAngle(float azimuth, float pitch, float roll) { + + double heading = Math.toRadians(roll); + double attitude = Math.toRadians(pitch); + double bank = Math.toRadians(azimuth); + + double c1 = Math.cos(heading / 2); + double s1 = Math.sin(heading / 2); + double c2 = Math.cos(attitude / 2); + double s2 = Math.sin(attitude / 2); + double c3 = Math.cos(bank / 2); + double s3 = Math.sin(bank / 2); + double c1c2 = c1 * c2; + double s1s2 = s1 * s2; + setW((float) (c1c2 * c3 - s1s2 * s3)); + setX((float) (c1c2 * s3 + s1s2 * c3)); + setY((float) (s1 * c2 * c3 + c1 * s2 * s3)); + setZ((float) (c1 * s2 * c3 - s1 * c2 * s3)); + + dirty = true; + } + + /** + * Rotation is in degrees. Set this quaternion from the supplied axis angle. + * + * @param vec The vector of rotation + * @param rot The angle of rotation around that vector in degrees. + */ + public void setAxisAngle(Vector3f vec, float rot) { + double s = Math.sin(Math.toRadians(rot / 2)); + setX(vec.getX() * (float) s); + setY(vec.getY() * (float) s); + setZ(vec.getZ() * (float) s); + setW((float) Math.cos(Math.toRadians(rot / 2))); + + dirty = true; + } + + public void setAxisAngleRad(Vector3f vec, double rot) { + double s = rot / 2; + setX(vec.getX() * (float) s); + setY(vec.getY() * (float) s); + setZ(vec.getZ() * (float) s); + setW((float) rot / 2); + + dirty = true; + } + + /** + * @return Returns this Quaternion in the + */ + public Matrixf4x4 getMatrix4x4() { + //toMatrixColMajor(); + if (dirty) { + convertQuatToMatrix(); + dirty = false; + } + return this.matrix; + } + + public void copyFromVec3(Vector3f vec, float w) { + copyFromV3f(vec, w); + } + + /** + * Get a linear interpolation between this quaternion and the input quaternion, storing the result in the output + * quaternion. + * + * @param input The quaternion to be slerped with this quaternion. + * @param output The quaternion to store the result in. + * @param t The ratio between the two quaternions where 0 <= t <= 1.0 . Increase value of t will bring rotation + * closer to the input quaternion. + */ + public void slerp(Quaternion input, Quaternion output, float t) { + // Calculate angle between them. + //double cosHalftheta = this.dotProduct(input); + Quaternion bufferQuat = null; + float cosHalftheta = this.dotProduct(input); + + if (cosHalftheta < 0) { + bufferQuat = new Quaternion(); + cosHalftheta = -cosHalftheta; + bufferQuat.points[0] = (-input.points[0]); + bufferQuat.points[1] = (-input.points[1]); + bufferQuat.points[2] = (-input.points[2]); + bufferQuat.points[3] = (-input.points[3]); + } else { + bufferQuat = input; + } + /** + * if(dot < 0.95f){ + * double angle = Math.acos(dot); + * double ratioA = Math.sin((1 - t) * angle); + * double ratioB = Math.sin(t * angle); + * double divisor = Math.sin(angle); + * + * //Calculate Quaternion + * output.setW((float)((this.getW() * ratioA + input.getW() * ratioB)/divisor)); + * output.setX((float)((this.getX() * ratioA + input.getX() * ratioB)/divisor)); + * output.setY((float)((this.getY() * ratioA + input.getY() * ratioB)/divisor)); + * output.setZ((float)((this.getZ() * ratioA + input.getZ() * ratioB)/divisor)); + * } + * else{ + * lerp(input, output, t); + * } + */ + // if qa=qb or qa=-qb then theta = 0 and we can return qa + if (Math.abs(cosHalftheta) >= 1.0) { + output.points[0] = (this.points[0]); + output.points[1] = (this.points[1]); + output.points[2] = (this.points[2]); + output.points[3] = (this.points[3]); + } else { + double sinHalfTheta = Math.sqrt(1.0 - cosHalftheta * cosHalftheta); + // if theta = 180 degrees then result is not fully defined + // we could rotate around any axis normal to qa or qb + //if(Math.abs(sinHalfTheta) < 0.001){ + //output.setW(this.getW() * 0.5f + input.getW() * 0.5f); + //output.setX(this.getX() * 0.5f + input.getX() * 0.5f); + //output.setY(this.getY() * 0.5f + input.getY() * 0.5f); + //output.setZ(this.getZ() * 0.5f + input.getZ() * 0.5f); + // lerp(bufferQuat, output, t); + //} + //else{ + double halfTheta = Math.acos(cosHalftheta); + + double ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta; + double ratioB = Math.sin(t * halfTheta) / sinHalfTheta; + + //Calculate Quaternion + output.points[3] = ((float) (points[3] * ratioA + bufferQuat.points[3] * ratioB)); + output.points[0] = ((float) (this.points[0] * ratioA + bufferQuat.points[0] * ratioB)); + output.points[1] = ((float) (this.points[1] * ratioA + bufferQuat.points[1] * ratioB)); + output.points[2] = ((float) (this.points[2] * ratioA + bufferQuat.points[2] * ratioB)); + + //} + } + } + +} diff --git a/src/org/hitlabnz/sensor_fusion_demo/representation/Renderable.java b/src/org/hitlabnz/sensor_fusion_demo/representation/Renderable.java new file mode 100644 index 0000000..9ca9e7e --- /dev/null +++ b/src/org/hitlabnz/sensor_fusion_demo/representation/Renderable.java @@ -0,0 +1,39 @@ +package org.hitlabnz.sensor_fusion_demo.representation; + +import java.io.Serializable; +import java.util.concurrent.locks.ReentrantLock; + +/** + * @author Leigh Beattie + * + * At the moment this is a place holder for objects that can be put in the scene graph. There may be some + * requirements later specified. + */ +public class Renderable implements Serializable { + + /** + * ID for serialisation + */ + private static final long serialVersionUID = 6701586807666461858L; + + //Used in data managemenst and synchronisation. If you make a renderable then you should change this boolean to true. + protected boolean dirty = true; + protected ReentrantLock lock = new ReentrantLock(); + + public boolean dirty() { + return dirty; + } + + public void setClean() { + this.dirty = false; + } + + public void setDirty() { + this.dirty = true; + } + + public ReentrantLock getLock() { + return this.lock; + } + +} diff --git a/src/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java b/src/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java new file mode 100644 index 0000000..6ef5aec --- /dev/null +++ b/src/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java @@ -0,0 +1,295 @@ +package org.hitlabnz.sensor_fusion_demo.representation; + +/** + * 3-dimensional vector with conventient getters and setters. Additionally this class is serializable and + */ +public class Vector3f extends Renderable { + + /** + * ID for serialisation + */ + private static final long serialVersionUID = -4565578579900616220L; + + /** + * A float array was chosen instead of individual variables due to performance concerns. Converting the points into + * an array at run time can cause slowness so instead we use one array and extract the individual variables with get + * methods. + */ + protected float[] points = new float[3]; + + /** + * Initialises the vector with the given values + * + * @param x the x-component + * @param y the y-component + * @param z the z-component + */ + public Vector3f(float x, float y, float z) { + this.points[0] = x; + this.points[1] = y; + this.points[2] = z; + } + + /** + * Initialises all components of this vector with the given same value. + * + * @param value Initialisation value for all components + */ + public Vector3f(float value) { + this.points[0] = value; + this.points[1] = value; + this.points[2] = value; + } + + /** + * Instantiates a new vector3f. + */ + public Vector3f() { + } + + /** + * Copy constructor + */ + public Vector3f(Vector3f vector) { + this.points[0] = vector.points[0]; + this.points[1] = vector.points[1]; + this.points[2] = vector.points[2]; + } + + /** + * Initialises this vector from a 4-dimensional vector. If the fourth component is not zero, a normalisation of all + * components will be performed. + * + * @param vector The 4-dimensional vector that should be used for initialisation + */ + public Vector3f(Vector4f vector) { + if (vector.w() != 0) { + this.points[0] = vector.x() / vector.w(); + this.points[1] = vector.y() / vector.w(); + this.points[2] = vector.z() / vector.w(); + } else { + this.points[0] = vector.x(); + this.points[1] = vector.y(); + this.points[2] = vector.z(); + } + } + + /** + * Returns this vector as float-array. + * + * @return the float[] + */ + public float[] toArray() { + return this.points; + } + + /** + * Adds a vector to this vector + * + * @param summand the vector that should be added component-wise + */ + public void add(Vector3f summand) { + this.points[0] += summand.points[0]; + this.points[1] += summand.points[1]; + this.points[2] += summand.points[2]; + } + + /** + * Adds the value to all components of this vector + * + * @param summand The value that should be added to all components + */ + public void add(float summand) { + this.points[0] += summand; + this.points[1] += summand; + this.points[2] += summand; + } + + /** + * + * @param subtrahend + */ + public void subtract(Vector3f subtrahend) { + this.points[0] -= subtrahend.points[0]; + this.points[1] -= subtrahend.points[1]; + this.points[2] -= subtrahend.points[2]; + } + + /** + * Multiply by scalar. + * + * @param scalar the scalar + */ + public void multiplyByScalar(float scalar) { + this.points[0] *= scalar; + this.points[1] *= scalar; + this.points[2] *= scalar; + } + + /** + * Normalize. + */ + public void normalize() { + + double a = Math.sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]); + this.points[0] = (float) (this.points[0] / a); + this.points[1] = (float) (this.points[1] / a); + this.points[2] = (float) (this.points[2] / a); + + } + + /** + * Gets the x. + * + * @return the x + */ + public float getX() { + return points[0]; + } + + /** + * Gets the y. + * + * @return the y + */ + public float getY() { + return points[1]; + } + + /** + * Gets the z. + * + * @return the z + */ + public float getZ() { + return points[2]; + } + + /** + * Sets the x. + * + * @param x the new x + */ + public void setX(float x) { + this.points[0] = x; + } + + /** + * Sets the y. + * + * @param y the new y + */ + public void setY(float y) { + this.points[1] = y; + } + + /** + * Sets the z. + * + * @param z the new z + */ + public void setZ(float z) { + this.points[2] = z; + } + + /** + * Functions for convenience + */ + + public float x() { + return this.points[0]; + } + + public float y() { + return this.points[1]; + } + + public float z() { + return this.points[2]; + } + + public void x(float x) { + this.points[0] = x; + } + + public void y(float y) { + this.points[1] = y; + } + + public void z(float z) { + this.points[2] = z; + } + + public void setXYZ(float x, float y, float z) { + this.points[0] = x; + this.points[1] = y; + this.points[2] = z; + } + + /** + * Return the dot product of this vector with the input vector + * + * @param inputVec The vector you want to do the dot product with against this vector. + * @return Float value representing the scalar of the dot product operation + */ + public float dotProduct(Vector3f inputVec) { + return points[0] * inputVec.points[0] + points[1] * inputVec.points[1] + points[2] * inputVec.points[2]; + + } + + /** + * Get the cross product of this vector and another vector. The result will be stored in the output vector. + * + * @param inputVec The vector you want to get the dot product of against this vector. + * @param outputVec The vector to store the result in. + */ + public void crossProduct(Vector3f inputVec, Vector3f outputVec) { + outputVec.setX(points[1] * inputVec.points[2] - points[2] * inputVec.points[1]); + outputVec.setY(points[2] * inputVec.points[0] - points[0] * inputVec.points[2]); + outputVec.setZ(points[0] * inputVec.points[1] - points[1] * inputVec.points[0]); + } + + public Vector3f crossProduct(Vector3f in) { + Vector3f out = new Vector3f(); + crossProduct(in, out); + return out; + } + + /** + * If you need to get the length of a vector then use this function. + * + * @return The length of the vector + */ + public float getLength() { + return (float) Math.sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]); + } + + @Override + public String toString() { + return "X:" + points[0] + " Y:" + points[1] + " Z:" + points[2]; + } + + /** + * Clone the input vector so that this vector has the same values. + * + * @param source The vector you want to clone. + */ + public void clone(Vector3f source) { + // this.points[0] = source.points[0]; + // this.points[1] = source.points[1]; + // this.points[2] = source.points[2]; + System.arraycopy(source.points, 0, points, 0, 3); + } + + /** + * Clone the input vector so that this vector has the same values. + * + * @param source The vector you want to clone. + */ + public void clone(float[] source) { + // this.points[0] = source[0]; + // this.points[1] = source[1]; + // this.points[2] = source[2]; + System.arraycopy(source, 0, points, 0, 3); + } +} diff --git a/src/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java b/src/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java new file mode 100644 index 0000000..dd214ff --- /dev/null +++ b/src/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java @@ -0,0 +1,296 @@ +package org.hitlabnz.sensor_fusion_demo.representation; + +import java.io.Serializable; + +/** + * Representation of a four-dimensional float-vector + */ +public class Vector4f extends Renderable implements Serializable { + + /** + * ID for Serialisation + */ + private static final long serialVersionUID = 1L; + /** The points. */ + protected float points[] = { 0, 0, 0, 0 }; + + /** + * Instantiates a new vector4f. + * + * @param x the x + * @param y the y + * @param z the z + * @param w the w + */ + public Vector4f(float x, float y, float z, float w) { + this.points[0] = x; + this.points[1] = y; + this.points[2] = z; + this.points[3] = w; + } + + /** + * Instantiates a new vector4f. + */ + public Vector4f() { + this.points[0] = 0; + this.points[1] = 0; + this.points[2] = 0; + this.points[3] = 0; + } + + public Vector4f(Vector3f vector3f, float w) { + this.points[0] = vector3f.x(); + this.points[1] = vector3f.y(); + this.points[2] = vector3f.z(); + this.points[3] = w; + } + + /** + * To array. + * + * @return the float[] + */ + public float[] ToArray() { + return points; + } + + public void copyVec4(Vector4f vec) { + this.points[0] = vec.points[0]; + this.points[1] = vec.points[1]; + this.points[2] = vec.points[2]; + this.points[3] = vec.points[3]; + } + + /** + * Adds the. + * + * @param vector the vector + */ + public void add(Vector4f vector) { + this.points[0] += vector.points[0]; + this.points[1] += vector.points[1]; + this.points[2] += vector.points[2]; + this.points[3] += vector.points[3]; + } + + public void add(Vector3f vector, float w) { + this.points[0] += vector.x(); + this.points[1] += vector.y(); + this.points[2] += vector.z(); + this.points[3] += w; + } + + public void subtract(Vector4f vector) { + this.points[0] -= vector.points[0]; + this.points[1] -= vector.points[1]; + this.points[2] -= vector.points[2]; + this.points[3] -= vector.points[3]; + } + + public void subtract(Vector4f vector, Vector4f output) { + output.setXYZW(this.points[0] - vector.points[0], this.points[1] - vector.points[1], this.points[2] + - vector.points[2], this.points[3] - vector.points[3]); + } + + public void subdivide(Vector4f vector) { + this.points[0] /= vector.points[0]; + this.points[1] /= vector.points[1]; + this.points[2] /= vector.points[2]; + this.points[3] /= vector.points[3]; + } + + /** + * Multiply by scalar. + * + * @param scalar the scalar + */ + public void multiplyByScalar(float scalar) { + this.points[0] *= scalar; + this.points[1] *= scalar; + this.points[2] *= scalar; + this.points[3] *= scalar; + } + + public float dotProduct(Vector4f input) { + return this.points[0] * input.points[0] + this.points[1] * input.points[1] + this.points[2] * input.points[2] + + this.points[3] * input.points[3]; + } + + /** + * Linear interpolation between two vectors storing the result in the output variable. + * + * @param input + * @param output + * @param t + */ + public void lerp(Vector4f input, Vector4f output, float t) { + output.points[0] = (points[0] * (1.0f * t) + input.points[0] * t); + output.points[1] = (points[1] * (1.0f * t) + input.points[1] * t); + output.points[2] = (points[2] * (1.0f * t) + input.points[2] * t); + output.points[3] = (points[3] * (1.0f * t) + input.points[3] * t); + + } + + /** + * Normalize. + */ + public void normalize() { + if (points[3] == 0) + return; + + points[0] /= points[3]; + points[1] /= points[3]; + points[2] /= points[3]; + + double a = Math.sqrt(this.points[0] * this.points[0] + this.points[1] * this.points[1] + this.points[2] + * this.points[2]); + points[0] = (float) (this.points[0] / a); + points[1] = (float) (this.points[1] / a); + points[2] = (float) (this.points[2] / a); + } + + /** + * Gets the x. + * + * @return the x + */ + public float getX() { + return this.points[0]; + } + + /** + * Gets the y. + * + * @return the y + */ + public float getY() { + return this.points[1]; + } + + /** + * Gets the z. + * + * @return the z + */ + public float getZ() { + return this.points[2]; + } + + /** + * Gets the w. + * + * @return the w + */ + public float getW() { + return this.points[3]; + } + + /** + * Sets the x. + * + * @param x the new x + */ + public void setX(float x) { + this.points[0] = x; + } + + /** + * Sets the y. + * + * @param y the new y + */ + public void setY(float y) { + this.points[1] = y; + } + + /** + * Sets the z. + * + * @param z the new z + */ + public void setZ(float z) { + this.points[2] = z; + } + + /** + * Sets the w. + * + * @param w the new w + */ + public void setW(float w) { + this.points[3] = w; + } + + public float x() { + return this.points[0]; + } + + public float y() { + return this.points[1]; + } + + public float z() { + return this.points[2]; + } + + public float w() { + return this.points[3]; + } + + public void x(float x) { + this.points[0] = x; + } + + public void y(float y) { + this.points[1] = y; + } + + public void z(float z) { + this.points[2] = z; + } + + public void w(float w) { + this.points[3] = w; + } + + public void setXYZW(float x, float y, float z, float w) { + this.points[0] = x; + this.points[1] = y; + this.points[2] = z; + this.points[3] = w; + } + + /** + * Compare this vector4f to the supplied one + * + * @param rhs True if they match, false other wise. + * @return + */ + public boolean compareTo(Vector4f rhs) { + boolean ret = false; + if (this.points[0] == rhs.points[0] && this.points[1] == rhs.points[1] && this.points[2] == rhs.points[2] + && this.points[3] == rhs.points[3]) + ret = true; + return ret; + } + + /** + * Copies the data from the supplied vec3 into this vec4 plus the supplied w. + * + * @param input The x y z values to copy in. + * @param w The extra w element to copy in + */ + public void copyFromV3f(Vector3f input, float w) { + points[0] = (input.x()); + points[1] = (input.y()); + points[2] = (input.z()); + points[3] = (w); + } + + @Override + public String toString() { + return "X:" + points[0] + " Y:" + points[1] + " Z:" + points[2] + " W:" + points[3]; + } + +} \ No newline at end of file