Added different representation as alternative to a plain float-vector: Matrix, Vector and Quaternion. These representations provide getters, setters and conversion-functions.
This commit is contained in:
Alexander Pacha
824
src/org/hitlabnz/sensor_fusion_demo/representation/Matrix.java
Normal file
824
src/org/hitlabnz/sensor_fusion_demo/representation/Matrix.java
Normal file
@@ -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:
|
||||
*
|
||||
* <pre>
|
||||
* 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]
|
||||
* </pre>
|
||||
*
|
||||
* Vectors are 4 row x 1 column column-vectors stored in order:
|
||||
*
|
||||
* <pre>
|
||||
* v[offset + 0]
|
||||
* v[offset + 1]
|
||||
* v[offset + 2]
|
||||
* v[offset + 3]
|
||||
* </pre>
|
||||
*
|
||||
*/
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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).
|
||||
*
|
||||
* <p>
|
||||
* 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).
|
||||
* </p>
|
||||
*
|
||||
* <p>
|
||||
* This class also contains the representation of the same rotation as a Quaternion and 4x4-Rotation-Matrix.
|
||||
* </p>
|
||||
*
|
||||
* @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));
|
||||
|
||||
//}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
295
src/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java
Normal file
295
src/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
296
src/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java
Normal file
296
src/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java
Normal file
@@ -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];
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user