5 | Diese App wurde von Alexander Pacha am Human Interface Technology 7 | Laboratory New Zealand entwickelt und demonstriert die 8 | Leistungsfähigkeit von verschiedenen Sensoren und Sensorfusionen. 9 | Messungen von dem Gyroskop, Akzelerometer und Kompass werden in 10 | verschiedenen Weisen kombiniert und das Ergebnis wird als Würfel 11 | visualisiert, der durch rotieren des Gerätes gedreht werden kann. 12 |
13 |14 | Die große Neuheit in dieser Applikation ist die Fusion von zwei 15 | virtuellen Sensoren: Improved Orientation Sensor 1 und Improved 16 | Orientation Sensor 2 nutzen den Android Rotation Vector mit dem 17 | kalibrierten Gyroskopsensor und erreichen eine nie zuvor dagewesenen Präzision 18 | und Reaktionsfähigkeit. 19 |
20 | 21 |Neben diesen beiden Sensorfusionen gibt es noch weitere Sensoren 22 | zum Vergleich:
23 |40 | Die Anwendung wurde entwickelt um die im Rahmer der Masterarbeit "Sensor 42 | fusion for robust outdoor Augmented Reality tracking on mobile devices" 43 | (download) 44 | entwickelte Sensorfusion zu demonstrieren. Die App wurde am Human Interface Technology 46 | Laboratory New Zealand entwickelt. 47 |
48 | 49 |Lange auf den Würfel gedrückt halten, um kurzfristig in den Raummodus zu wechseln.
50 | 51 |52 | Der Quellcode ist öffentlich verfügbar auf Bitbucket 54 | unter der MIT Lizenz. 55 |
56 | 57 |58 | Weitere Mitarbeit: 59 |
This application was developed by Alexander Pacha at the Human Interface 5 | Technology Laboratory New Zealand to demonstrate the 6 | capabilities of various sensors and sensor fusion approaches. Data 7 | from the Gyroscope, Accelerometer and compass are combined in 8 | different ways and the result is shown as a cube that can be rotated 9 | by rotating the device.
10 |11 | The major novelty in this application is the fusion of virtual 12 | sensors: Improved Orientation Sensor 1 and Improved 13 | Orientation Sensor 2 fuse the Android Rotation Vector with the 14 | virtual Gyroscope sensor to achieve a pose estimation with a 15 | previously unknown stability and precision. 16 |
17 | 18 |Apart from these two sensors, the following sensors are 19 | available for comparison:
20 |37 | This application was developed for demonstrating the sensor fusion 38 | approach developed for my Master thesis "Sensor fusion for robust outdoor Augmented Reality tracking on mobile 40 | devices" 41 | (download) at the Human Interface 42 | Technology Laboratory New Zealand 43 |
44 | 45 |Long-click on a cube to temporarily change into the space-mode for this fusion.
46 | 47 |48 | The source-code is publicly available at Bitbucket 50 | and licensed under the MIT license. 51 |
52 | 53 |54 | Contributors: 55 |
11 | * m[offset + 0] m[offset + 4] m[offset + 8] m[offset + 12] 12 | * m[offset + 1] m[offset + 5] m[offset + 9] m[offset + 13] 13 | * m[offset + 2] m[offset + 6] m[offset + 10] m[offset + 14] 14 | * m[offset + 3] m[offset + 7] m[offset + 11] m[offset + 15] 15 | *16 | * 17 | * Vectors are 4 row x 1 column column-vectors stored in order: 18 | * 19 | *
20 | * v[offset + 0] 21 | * v[offset + 1] 22 | * v[offset + 2] 23 | * v[offset + 3] 24 | *25 | * 26 | */ 27 | public class Matrix { 28 | 29 | /** 30 | * Temporary memory for operations that need temporary matrix data. 31 | */ 32 | private static final float[] TEMP_MATRIX_ARRAY = new float[32]; 33 | 34 | /** 35 | * Multiply two 4x4 matrices together and store the result in a third 4x4 36 | * matrix. In matrix notation: result = lhs x rhs. Due to the way 37 | * matrix multiplication works, the result matrix will have the same 38 | * effect as first multiplying by the rhs matrix, then multiplying by 39 | * the lhs matrix. This is the opposite of what you might expect. 40 | * 41 | * The same float array may be passed for result, lhs, and/or rhs. However, 42 | * the result element values are undefined if the result elements overlap 43 | * either the lhs or rhs elements. 44 | * 45 | * @param result The float array that holds the result. 46 | * @param resultOffset The offset into the result array where the result is 47 | * stored. 48 | * @param lhs The float array that holds the left-hand-side matrix. 49 | * @param lhsOffset The offset into the lhs array where the lhs is stored 50 | * @param rhs The float array that holds the right-hand-side matrix. 51 | * @param rhsOffset The offset into the rhs array where the rhs is stored. 52 | * 53 | * @throws IllegalArgumentException if result, lhs, or rhs are null, or if 54 | * resultOffset + 16 > result.length or lhsOffset + 16 > lhs.length or 55 | * rhsOffset + 16 > rhs.length. 56 | */ 57 | /** 58 | * public static void multiplyMM(float[] result, int resultOffset, 59 | * float[] lhs, int lhsOffset, float[] rhs, int rhsOffset){ 60 | * android.opengl.Matrix.multiplyMM(result, resultOffset, lhs, lhsOffset, rhs, rhsOffset); 61 | * } 62 | */ 63 | 64 | public static void multiplyMM(float[] output, int outputOffset, float[] lhs, int lhsOffset, float[] rhs, 65 | int rhsOffset) { 66 | //for(int i = 0; i < 4; i++){ 67 | // for(int j = 0; j < 4; j++){ 68 | 69 | // int k = i * 4; 70 | // output[outputOffset + 0 + j] += lhs[lhsOffset + k + j] * rhs[rhsOffset + 0 * 4 + i]; 71 | // output[outputOffset + 1 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 1 * 4 + i]; 72 | // output[outputOffset + 2 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 2 * 4 + i]; 73 | // output[outputOffset + 3 * 4 + j] += lhs[lhsOffset +k + j] * rhs[rhsOffset + 3 * 4 + i]; 74 | // } 75 | //} 76 | output[outputOffset + 0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 4] * rhs[rhsOffset + 1] 77 | + lhs[lhsOffset + 8] * rhs[rhsOffset + 2] + lhs[lhsOffset + 12] * rhs[rhsOffset + 3]; 78 | output[outputOffset + 1] = lhs[lhsOffset + 1] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] 79 | + lhs[lhsOffset + 9] * rhs[rhsOffset + 2] + lhs[lhsOffset + 13] * rhs[rhsOffset + 3]; 80 | output[outputOffset + 2] = lhs[lhsOffset + 2] * rhs[rhsOffset + 0] + lhs[lhsOffset + 6] * rhs[rhsOffset + 1] 81 | + lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 14] * rhs[rhsOffset + 3]; 82 | output[outputOffset + 3] = lhs[lhsOffset + 3] * rhs[rhsOffset + 0] + lhs[lhsOffset + 7] * rhs[rhsOffset + 1] 83 | + lhs[lhsOffset + 11] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; 84 | 85 | output[outputOffset + 4] = lhs[lhsOffset + 0] * rhs[rhsOffset + 4] + lhs[lhsOffset + 4] * rhs[rhsOffset + 5] 86 | + lhs[lhsOffset + 8] * rhs[rhsOffset + 6] + lhs[lhsOffset + 12] * rhs[rhsOffset + 7]; 87 | output[outputOffset + 5] = lhs[lhsOffset + 1] * rhs[rhsOffset + 4] + lhs[lhsOffset + 5] * rhs[rhsOffset + 5] 88 | + lhs[lhsOffset + 9] * rhs[rhsOffset + 6] + lhs[lhsOffset + 13] * rhs[rhsOffset + 7]; 89 | output[outputOffset + 6] = lhs[lhsOffset + 2] * rhs[rhsOffset + 4] + lhs[lhsOffset + 6] * rhs[rhsOffset + 5] 90 | + lhs[lhsOffset + 10] * rhs[rhsOffset + 6] + lhs[lhsOffset + 14] * rhs[rhsOffset + 7]; 91 | output[outputOffset + 7] = lhs[lhsOffset + 3] * rhs[rhsOffset + 4] + lhs[lhsOffset + 7] * rhs[rhsOffset + 5] 92 | + lhs[lhsOffset + 11] * rhs[rhsOffset + 6] + lhs[lhsOffset + 15] * rhs[rhsOffset + 7]; 93 | 94 | output[outputOffset + 8] = lhs[lhsOffset + 0] * rhs[rhsOffset + 8] + lhs[lhsOffset + 4] * rhs[rhsOffset + 9] 95 | + lhs[lhsOffset + 8] * rhs[rhsOffset + 10] + lhs[lhsOffset + 12] * rhs[rhsOffset + 11]; 96 | output[outputOffset + 9] = lhs[lhsOffset + 1] * rhs[rhsOffset + 8] + lhs[lhsOffset + 5] * rhs[rhsOffset + 9] 97 | + lhs[lhsOffset + 9] * rhs[rhsOffset + 10] + lhs[lhsOffset + 13] * rhs[rhsOffset + 11]; 98 | output[outputOffset + 10] = lhs[lhsOffset + 2] * rhs[rhsOffset + 8] + lhs[lhsOffset + 6] * rhs[rhsOffset + 9] 99 | + lhs[lhsOffset + 10] * rhs[rhsOffset + 10] + lhs[lhsOffset + 14] * rhs[rhsOffset + 11]; 100 | output[outputOffset + 11] = lhs[lhsOffset + 3] * rhs[rhsOffset + 8] + lhs[lhsOffset + 7] * rhs[rhsOffset + 9] 101 | + lhs[lhsOffset + 11] * rhs[rhsOffset + 10] + lhs[lhsOffset + 15] * rhs[rhsOffset + 11]; 102 | 103 | output[outputOffset + 12] = lhs[lhsOffset + 0] * rhs[rhsOffset + 12] + lhs[lhsOffset + 4] * rhs[rhsOffset + 13] 104 | + lhs[lhsOffset + 8] * rhs[rhsOffset + 14] + lhs[lhsOffset + 12] * rhs[rhsOffset + 15]; 105 | output[outputOffset + 13] = lhs[lhsOffset + 1] * rhs[rhsOffset + 12] + lhs[lhsOffset + 5] * rhs[rhsOffset + 13] 106 | + lhs[lhsOffset + 9] * rhs[rhsOffset + 14] + lhs[lhsOffset + 13] * rhs[rhsOffset + 15]; 107 | output[outputOffset + 14] = lhs[lhsOffset + 2] * rhs[rhsOffset + 12] + lhs[lhsOffset + 6] * rhs[rhsOffset + 13] 108 | + lhs[lhsOffset + 10] * rhs[rhsOffset + 14] + lhs[lhsOffset + 14] * rhs[rhsOffset + 15]; 109 | output[outputOffset + 15] = lhs[lhsOffset + 3] * rhs[rhsOffset + 12] + lhs[lhsOffset + 7] * rhs[rhsOffset + 13] 110 | + lhs[lhsOffset + 11] * rhs[rhsOffset + 14] + lhs[lhsOffset + 15] * rhs[rhsOffset + 15]; 111 | } 112 | 113 | public static void multiplyMM(float[] output, float[] lhs, float[] rhs) { 114 | output[0] = lhs[0] * rhs[0] + lhs[4] * rhs[1] + lhs[8] * rhs[2] + lhs[12] * rhs[3]; 115 | output[1] = lhs[1] * rhs[0] + lhs[5] * rhs[1] + lhs[9] * rhs[2] + lhs[13] * rhs[3]; 116 | output[2] = lhs[2] * rhs[0] + lhs[6] * rhs[1] + lhs[10] * rhs[2] + lhs[14] * rhs[3]; 117 | output[3] = lhs[3] * rhs[0] + lhs[7] * rhs[1] + lhs[11] * rhs[2] + lhs[15] * rhs[3]; 118 | 119 | output[4] = lhs[0] * rhs[4] + lhs[4] * rhs[5] + lhs[8] * rhs[6] + lhs[12] * rhs[7]; 120 | output[5] = lhs[1] * rhs[4] + lhs[5] * rhs[5] + lhs[9] * rhs[6] + lhs[13] * rhs[7]; 121 | output[6] = lhs[2] * rhs[4] + lhs[6] * rhs[5] + lhs[10] * rhs[6] + lhs[14] * rhs[7]; 122 | output[7] = lhs[3] * rhs[4] + lhs[7] * rhs[5] + lhs[11] * rhs[6] + lhs[15] * rhs[7]; 123 | 124 | output[8] = lhs[0] * rhs[8] + lhs[4] * rhs[9] + lhs[8] * rhs[10] + lhs[12] * rhs[11]; 125 | output[9] = lhs[1] * rhs[8] + lhs[5] * rhs[9] + lhs[9] * rhs[10] + lhs[13] * rhs[11]; 126 | output[10] = lhs[2] * rhs[8] + lhs[6] * rhs[9] + lhs[10] * rhs[10] + lhs[14] * rhs[11]; 127 | output[11] = lhs[3] * rhs[8] + lhs[7] * rhs[9] + lhs[11] * rhs[10] + lhs[15] * rhs[11]; 128 | 129 | output[12] = lhs[0] * rhs[12] + lhs[4] * rhs[13] + lhs[8] * rhs[14] + lhs[12] * rhs[15]; 130 | output[13] = lhs[1] * rhs[12] + lhs[5] * rhs[13] + lhs[9] * rhs[14] + lhs[13] * rhs[15]; 131 | output[14] = lhs[2] * rhs[12] + lhs[6] * rhs[13] + lhs[10] * rhs[14] + lhs[14] * rhs[15]; 132 | output[15] = lhs[3] * rhs[12] + lhs[7] * rhs[13] + lhs[11] * rhs[14] + lhs[15] * rhs[15]; 133 | } 134 | 135 | /** 136 | * Multiply a 4 element vector by a 4x4 matrix and store the result in a 4 137 | * element column vector. In matrix notation: result = lhs x rhs 138 | * 139 | * The same float array may be passed for resultVec, lhsMat, and/or rhsVec. 140 | * However, the resultVec element values are undefined if the resultVec 141 | * elements overlap either the lhsMat or rhsVec elements. 142 | * 143 | * @param resultVec The float array that holds the result vector. 144 | * @param resultVecOffset The offset into the result array where the result 145 | * vector is stored. 146 | * @param lhsMat The float array that holds the left-hand-side matrix. 147 | * @param lhsMatOffset The offset into the lhs array where the lhs is stored 148 | * @param rhsVec The float array that holds the right-hand-side vector. 149 | * @param rhsVecOffset The offset into the rhs vector where the rhs vector 150 | * is stored. 151 | * 152 | * @throws IllegalArgumentException if resultVec, lhsMat, 153 | * or rhsVec are null, or if resultVecOffset + 4 > resultVec.length 154 | * or lhsMatOffset + 16 > lhsMat.length or 155 | * rhsVecOffset + 4 > rhsVec.length. 156 | */ 157 | /* public static void multiplyMV(float[] resultVec, 158 | * int resultVecOffset, float[] lhsMat, int lhsMatOffset, 159 | * float[] rhsVec, int rhsVecOffset){ 160 | * android.opengl.Matrix.multiplyMV(resultVec, resultVecOffset, lhsMat, lhsMatOffset, rhsVec, rhsVecOffset); 161 | * } */ 162 | public static void multiplyMV(float[] output, int outputOffset, float[] lhs, int lhsOffset, float[] rhs, 163 | int rhsOffset) { 164 | /* wrong implementation (this is for row major matrices) 165 | * output[outputOffset +0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 1] * rhs[rhsOffset + 1] 166 | * + lhs[lhsOffset + 2] * rhs[rhsOffset + 2] + lhs[lhsOffset + 3] * rhs[rhsOffset + 3]; 167 | * output[outputOffset +1] = lhs[lhsOffset + 4] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] + 168 | * lhs[lhsOffset + 6] * rhs[rhsOffset + 2] + lhs[lhsOffset + 7] * rhs[rhsOffset + 3]; 169 | * output[outputOffset +2] = lhs[lhsOffset + 8] * rhs[rhsOffset + 0] + lhs[lhsOffset + 9] * rhs[rhsOffset + 1] + 170 | * lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 11] * rhs[rhsOffset + 3]; 171 | * output[outputOffset +3] = lhs[lhsOffset + 12] * rhs[rhsOffset + 0] + lhs[lhsOffset + 13] * rhs[rhsOffset + 1] 172 | * + lhs[lhsOffset + 14] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; */ 173 | // correct implementation for column major matrices (which is for OpenGL) 174 | output[outputOffset + 0] = lhs[lhsOffset + 0] * rhs[rhsOffset + 0] + lhs[lhsOffset + 4] * rhs[rhsOffset + 1] 175 | + lhs[lhsOffset + 8] * rhs[rhsOffset + 2] + lhs[lhsOffset + 12] * rhs[rhsOffset + 3]; 176 | output[outputOffset + 1] = lhs[lhsOffset + 1] * rhs[rhsOffset + 0] + lhs[lhsOffset + 5] * rhs[rhsOffset + 1] 177 | + lhs[lhsOffset + 9] * rhs[rhsOffset + 2] + lhs[lhsOffset + 13] * rhs[rhsOffset + 3]; 178 | output[outputOffset + 2] = lhs[lhsOffset + 2] * rhs[rhsOffset + 0] + lhs[lhsOffset + 6] * rhs[rhsOffset + 1] 179 | + lhs[lhsOffset + 10] * rhs[rhsOffset + 2] + lhs[lhsOffset + 14] * rhs[rhsOffset + 3]; 180 | output[outputOffset + 3] = lhs[lhsOffset + 3] * rhs[rhsOffset + 0] + lhs[lhsOffset + 7] * rhs[rhsOffset + 1] 181 | + lhs[lhsOffset + 11] * rhs[rhsOffset + 2] + lhs[lhsOffset + 15] * rhs[rhsOffset + 3]; 182 | 183 | } 184 | 185 | public static void multiplyMV(float[] outputV, float[] inputM, float[] inputV) { 186 | outputV[0] = inputM[0] * inputV[0] + inputM[4] * inputV[1] + inputM[8] * inputV[2] + inputM[12] * inputV[3]; 187 | outputV[1] = inputM[1] * inputV[0] + inputM[5] * inputV[1] + inputM[9] * inputV[2] + inputM[13] * inputV[3]; 188 | outputV[2] = inputM[2] * inputV[0] + inputM[6] * inputV[1] + inputM[10] * inputV[2] + inputM[14] * inputV[3]; 189 | outputV[3] = inputM[3] * inputV[0] + inputM[7] * inputV[1] + inputM[11] * inputV[2] + inputM[15] * inputV[3]; 190 | } 191 | 192 | public static void multiplyMV3(float[] outputV, float[] inputM, float[] inputV, float w) { 193 | outputV[0] = inputM[0] * inputV[0] + inputM[4] * inputV[1] + inputM[8] * inputV[2] + inputM[12] * w; 194 | outputV[1] = inputM[1] * inputV[0] + inputM[5] * inputV[1] + inputM[9] * inputV[2] + inputM[13] * w; 195 | outputV[2] = inputM[2] * inputV[0] + inputM[6] * inputV[1] + inputM[10] * inputV[2] + inputM[14] * w; 196 | } 197 | 198 | /** 199 | * Transposes a 4 x 4 matrix. 200 | * 201 | * @param mTrans the array that holds the output inverted matrix 202 | * @param mTransOffset an offset into mInv where the inverted matrix is 203 | * stored. 204 | * @param m the input array 205 | * @param mOffset an offset into m where the matrix is stored. 206 | */ 207 | public static void transposeM(float[] mTrans, int mTransOffset, float[] m, int mOffset) { 208 | for (int i = 0; i < 4; i++) { 209 | int mBase = i * 4 + mOffset; 210 | mTrans[i + mTransOffset] = m[mBase]; 211 | mTrans[i + 4 + mTransOffset] = m[mBase + 1]; 212 | mTrans[i + 8 + mTransOffset] = m[mBase + 2]; 213 | mTrans[i + 12 + mTransOffset] = m[mBase + 3]; 214 | } 215 | } 216 | 217 | /** 218 | * Inverts a 4 x 4 matrix. 219 | * 220 | * @param mInv the array that holds the output inverted matrix 221 | * @param mInvOffset an offset into mInv where the inverted matrix is 222 | * stored. 223 | * @param m the input array 224 | * @param mOffset an offset into m where the matrix is stored. 225 | * @return true if the matrix could be inverted, false if it could not. 226 | */ 227 | public static boolean invertM(float[] mInv, int mInvOffset, float[] m, int mOffset) { 228 | // Invert a 4 x 4 matrix using Cramer's Rule 229 | 230 | // transpose matrix 231 | final float src0 = m[mOffset + 0]; 232 | final float src4 = m[mOffset + 1]; 233 | final float src8 = m[mOffset + 2]; 234 | final float src12 = m[mOffset + 3]; 235 | 236 | final float src1 = m[mOffset + 4]; 237 | final float src5 = m[mOffset + 5]; 238 | final float src9 = m[mOffset + 6]; 239 | final float src13 = m[mOffset + 7]; 240 | 241 | final float src2 = m[mOffset + 8]; 242 | final float src6 = m[mOffset + 9]; 243 | final float src10 = m[mOffset + 10]; 244 | final float src14 = m[mOffset + 11]; 245 | 246 | final float src3 = m[mOffset + 12]; 247 | final float src7 = m[mOffset + 13]; 248 | final float src11 = m[mOffset + 14]; 249 | final float src15 = m[mOffset + 15]; 250 | 251 | // calculate pairs for first 8 elements (cofactors) 252 | final float atmp0 = src10 * src15; 253 | final float atmp1 = src11 * src14; 254 | final float atmp2 = src9 * src15; 255 | final float atmp3 = src11 * src13; 256 | final float atmp4 = src9 * src14; 257 | final float atmp5 = src10 * src13; 258 | final float atmp6 = src8 * src15; 259 | final float atmp7 = src11 * src12; 260 | final float atmp8 = src8 * src14; 261 | final float atmp9 = src10 * src12; 262 | final float atmp10 = src8 * src13; 263 | final float atmp11 = src9 * src12; 264 | 265 | // calculate first 8 elements (cofactors) 266 | final float dst0 = (atmp0 * src5 + atmp3 * src6 + atmp4 * src7) - (atmp1 * src5 + atmp2 * src6 + atmp5 * src7); 267 | final float dst1 = (atmp1 * src4 + atmp6 * src6 + atmp9 * src7) - (atmp0 * src4 + atmp7 * src6 + atmp8 * src7); 268 | final float dst2 = (atmp2 * src4 + atmp7 * src5 + atmp10 * src7) 269 | - (atmp3 * src4 + atmp6 * src5 + atmp11 * src7); 270 | final float dst3 = (atmp5 * src4 + atmp8 * src5 + atmp11 * src6) 271 | - (atmp4 * src4 + atmp9 * src5 + atmp10 * src6); 272 | final float dst4 = (atmp1 * src1 + atmp2 * src2 + atmp5 * src3) - (atmp0 * src1 + atmp3 * src2 + atmp4 * src3); 273 | final float dst5 = (atmp0 * src0 + atmp7 * src2 + atmp8 * src3) - (atmp1 * src0 + atmp6 * src2 + atmp9 * src3); 274 | final float dst6 = (atmp3 * src0 + atmp6 * src1 + atmp11 * src3) 275 | - (atmp2 * src0 + atmp7 * src1 + atmp10 * src3); 276 | final float dst7 = (atmp4 * src0 + atmp9 * src1 + atmp10 * src2) 277 | - (atmp5 * src0 + atmp8 * src1 + atmp11 * src2); 278 | 279 | // calculate pairs for second 8 elements (cofactors) 280 | final float btmp0 = src2 * src7; 281 | final float btmp1 = src3 * src6; 282 | final float btmp2 = src1 * src7; 283 | final float btmp3 = src3 * src5; 284 | final float btmp4 = src1 * src6; 285 | final float btmp5 = src2 * src5; 286 | final float btmp6 = src0 * src7; 287 | final float btmp7 = src3 * src4; 288 | final float btmp8 = src0 * src6; 289 | final float btmp9 = src2 * src4; 290 | final float btmp10 = src0 * src5; 291 | final float btmp11 = src1 * src4; 292 | 293 | // calculate second 8 elements (cofactors) 294 | final float dst8 = (btmp0 * src13 + btmp3 * src14 + btmp4 * src15) 295 | - (btmp1 * src13 + btmp2 * src14 + btmp5 * src15); 296 | final float dst9 = (btmp1 * src12 + btmp6 * src14 + btmp9 * src15) 297 | - (btmp0 * src12 + btmp7 * src14 + btmp8 * src15); 298 | final float dst10 = (btmp2 * src12 + btmp7 * src13 + btmp10 * src15) 299 | - (btmp3 * src12 + btmp6 * src13 + btmp11 * src15); 300 | final float dst11 = (btmp5 * src12 + btmp8 * src13 + btmp11 * src14) 301 | - (btmp4 * src12 + btmp9 * src13 + btmp10 * src14); 302 | final float dst12 = (btmp2 * src10 + btmp5 * src11 + btmp1 * src9) 303 | - (btmp4 * src11 + btmp0 * src9 + btmp3 * src10); 304 | final float dst13 = (btmp8 * src11 + btmp0 * src8 + btmp7 * src10) 305 | - (btmp6 * src10 + btmp9 * src11 + btmp1 * src8); 306 | final float dst14 = (btmp6 * src9 + btmp11 * src11 + btmp3 * src8) 307 | - (btmp10 * src11 + btmp2 * src8 + btmp7 * src9); 308 | final float dst15 = (btmp10 * src10 + btmp4 * src8 + btmp9 * src9) 309 | - (btmp8 * src9 + btmp11 * src10 + btmp5 * src8); 310 | 311 | // calculate determinant 312 | final float det = src0 * dst0 + src1 * dst1 + src2 * dst2 + src3 * dst3; 313 | 314 | if (det == 0.0f) { 315 | return false; 316 | } 317 | 318 | // calculate matrix inverse 319 | final float invdet = 1.0f / det; 320 | mInv[mInvOffset] = dst0 * invdet; 321 | mInv[1 + mInvOffset] = dst1 * invdet; 322 | mInv[2 + mInvOffset] = dst2 * invdet; 323 | mInv[3 + mInvOffset] = dst3 * invdet; 324 | 325 | mInv[4 + mInvOffset] = dst4 * invdet; 326 | mInv[5 + mInvOffset] = dst5 * invdet; 327 | mInv[6 + mInvOffset] = dst6 * invdet; 328 | mInv[7 + mInvOffset] = dst7 * invdet; 329 | 330 | mInv[8 + mInvOffset] = dst8 * invdet; 331 | mInv[9 + mInvOffset] = dst9 * invdet; 332 | mInv[10 + mInvOffset] = dst10 * invdet; 333 | mInv[11 + mInvOffset] = dst11 * invdet; 334 | 335 | mInv[12 + mInvOffset] = dst12 * invdet; 336 | mInv[13 + mInvOffset] = dst13 * invdet; 337 | mInv[14 + mInvOffset] = dst14 * invdet; 338 | mInv[15 + mInvOffset] = dst15 * invdet; 339 | 340 | return true; 341 | } 342 | 343 | /** 344 | * Computes an orthographic projection matrix. 345 | * 346 | * @param m returns the result 347 | * @param mOffset 348 | * @param left 349 | * @param right 350 | * @param bottom 351 | * @param top 352 | * @param near 353 | * @param far 354 | */ 355 | 356 | public static void orthoM(float[] m, int mOffset, float left, float right, float bottom, float top, float near, 357 | float far) { 358 | if (left == right) { 359 | throw new IllegalArgumentException("left == right"); 360 | } 361 | if (bottom == top) { 362 | throw new IllegalArgumentException("bottom == top"); 363 | } 364 | if (near == far) { 365 | throw new IllegalArgumentException("near == far"); 366 | } 367 | 368 | final float r_width = 1.0f / (right - left); 369 | final float r_height = 1.0f / (top - bottom); 370 | final float r_depth = 1.0f / (far - near); 371 | final float x = 2.0f * (r_width); 372 | final float y = 2.0f * (r_height); 373 | final float z = -2.0f * (r_depth); 374 | final float tx = -(right + left) * r_width; 375 | final float ty = -(top + bottom) * r_height; 376 | final float tz = -(far + near) * r_depth; 377 | m[mOffset + 0] = x; 378 | m[mOffset + 5] = y; 379 | m[mOffset + 10] = z; 380 | m[mOffset + 12] = tx; 381 | m[mOffset + 13] = ty; 382 | m[mOffset + 14] = tz; 383 | m[mOffset + 15] = 1.0f; 384 | m[mOffset + 1] = 0.0f; 385 | m[mOffset + 2] = 0.0f; 386 | m[mOffset + 3] = 0.0f; 387 | m[mOffset + 4] = 0.0f; 388 | m[mOffset + 6] = 0.0f; 389 | m[mOffset + 7] = 0.0f; 390 | m[mOffset + 8] = 0.0f; 391 | m[mOffset + 9] = 0.0f; 392 | m[mOffset + 11] = 0.0f; 393 | } 394 | 395 | /** 396 | * Define a projection matrix in terms of six clip planes 397 | * 398 | * @param m the float array that holds the perspective matrix 399 | * @param offset the offset into float array m where the perspective 400 | * matrix data is written 401 | * @param left 402 | * @param right 403 | * @param bottom 404 | * @param top 405 | * @param near 406 | * @param far 407 | */ 408 | 409 | public static void frustumM(float[] m, int offset, float left, float right, float bottom, float top, float near, 410 | float far) { 411 | if (left == right) { 412 | throw new IllegalArgumentException("left == right"); 413 | } 414 | if (top == bottom) { 415 | throw new IllegalArgumentException("top == bottom"); 416 | } 417 | if (near == far) { 418 | throw new IllegalArgumentException("near == far"); 419 | } 420 | if (near <= 0.0f) { 421 | throw new IllegalArgumentException("near <= 0.0f"); 422 | } 423 | if (far <= 0.0f) { 424 | throw new IllegalArgumentException("far <= 0.0f"); 425 | } 426 | final float r_width = 1.0f / (right - left); 427 | final float r_height = 1.0f / (top - bottom); 428 | final float r_depth = 1.0f / (near - far); 429 | final float x = 2.0f * (near * r_width); 430 | final float y = 2.0f * (near * r_height); 431 | final float A = 2.0f * ((right + left) * r_width); 432 | final float B = (top + bottom) * r_height; 433 | final float C = (far + near) * r_depth; 434 | final float D = 2.0f * (far * near * r_depth); 435 | m[offset + 0] = x; 436 | m[offset + 5] = y; 437 | m[offset + 8] = A; 438 | m[offset + 9] = B; 439 | m[offset + 10] = C; 440 | m[offset + 14] = D; 441 | m[offset + 11] = -1.0f; 442 | m[offset + 1] = 0.0f; 443 | m[offset + 2] = 0.0f; 444 | m[offset + 3] = 0.0f; 445 | m[offset + 4] = 0.0f; 446 | m[offset + 6] = 0.0f; 447 | m[offset + 7] = 0.0f; 448 | m[offset + 12] = 0.0f; 449 | m[offset + 13] = 0.0f; 450 | m[offset + 15] = 0.0f; 451 | } 452 | 453 | /** 454 | * Define a projection matrix in terms of a field of view angle, an 455 | * aspect ratio, and z clip planes 456 | * 457 | * @param m the float array that holds the perspective matrix 458 | * @param offset the offset into float array m where the perspective 459 | * matrix data is written 460 | * @param fovy field of view in y direction, in degrees 461 | * @param aspect width to height aspect ratio of the viewport 462 | * @param zNear 463 | * @param zFar 464 | */ 465 | public static void perspectiveM(float[] m, int offset, float fovy, float aspect, float zNear, float zFar) { 466 | float f = 1.0f / (float) Math.tan(fovy * (Math.PI / 360.0)); 467 | float rangeReciprocal = 1.0f / (zNear - zFar); 468 | 469 | m[offset + 0] = f / aspect; 470 | m[offset + 1] = 0.0f; 471 | m[offset + 2] = 0.0f; 472 | m[offset + 3] = 0.0f; 473 | 474 | m[offset + 4] = 0.0f; 475 | m[offset + 5] = f; 476 | m[offset + 6] = 0.0f; 477 | m[offset + 7] = 0.0f; 478 | 479 | m[offset + 8] = 0.0f; 480 | m[offset + 9] = 0.0f; 481 | m[offset + 10] = (zFar + zNear) * rangeReciprocal; 482 | m[offset + 11] = -1.0f; 483 | 484 | m[offset + 12] = 0.0f; 485 | m[offset + 13] = 0.0f; 486 | m[offset + 14] = 2.0f * zFar * zNear * rangeReciprocal; 487 | m[offset + 15] = 0.0f; 488 | } 489 | 490 | /** 491 | * Computes the length of a vector 492 | * 493 | * @param x x coordinate of a vector 494 | * @param y y coordinate of a vector 495 | * @param z z coordinate of a vector 496 | * @return the length of a vector 497 | */ 498 | public static float length(float x, float y, float z) { 499 | return (float) Math.sqrt(x * x + y * y + z * z); 500 | } 501 | 502 | /** 503 | * Sets matrix m to the identity matrix. 504 | * 505 | * @param sm returns the result 506 | * @param smOffset index into sm where the result matrix starts 507 | */ 508 | public static void setIdentityM(float[] sm, int smOffset) { 509 | for (int i = 0; i < 16; i++) { 510 | sm[smOffset + i] = 0; 511 | } 512 | for (int i = 0; i < 16; i += 5) { 513 | sm[smOffset + i] = 1.0f; 514 | } 515 | } 516 | 517 | /** 518 | * Scales matrix m by x, y, and z, putting the result in sm 519 | * 520 | * @param sm returns the result 521 | * @param smOffset index into sm where the result matrix starts 522 | * @param m source matrix 523 | * @param mOffset index into m where the source matrix starts 524 | * @param x scale factor x 525 | * @param y scale factor y 526 | * @param z scale factor z 527 | */ 528 | public static void scaleM(float[] sm, int smOffset, float[] m, int mOffset, float x, float y, float z) { 529 | for (int i = 0; i < 4; i++) { 530 | int smi = smOffset + i; 531 | int mi = mOffset + i; 532 | sm[smi] = m[mi] * x; 533 | sm[4 + smi] = m[4 + mi] * y; 534 | sm[8 + smi] = m[8 + mi] * z; 535 | sm[12 + smi] = m[12 + mi]; 536 | } 537 | } 538 | 539 | /** 540 | * Scales matrix m in place by sx, sy, and sz 541 | * 542 | * @param m matrix to scale 543 | * @param mOffset index into m where the matrix starts 544 | * @param x scale factor x 545 | * @param y scale factor y 546 | * @param z scale factor z 547 | */ 548 | public static void scaleM(float[] m, int mOffset, float x, float y, float z) { 549 | for (int i = 0; i < 4; i++) { 550 | int mi = mOffset + i; 551 | m[mi] *= x; 552 | m[4 + mi] *= y; 553 | m[8 + mi] *= z; 554 | } 555 | } 556 | 557 | /** 558 | * Translates matrix m by x, y, and z, putting the result in tm 559 | * 560 | * @param tm returns the result 561 | * @param tmOffset index into sm where the result matrix starts 562 | * @param m source matrix 563 | * @param mOffset index into m where the source matrix starts 564 | * @param x translation factor x 565 | * @param y translation factor y 566 | * @param z translation factor z 567 | */ 568 | public static void translateM(float[] tm, int tmOffset, float[] m, int mOffset, float x, float y, float z) { 569 | for (int i = 0; i < 12; i++) { 570 | tm[tmOffset + i] = m[mOffset + i]; 571 | } 572 | for (int i = 0; i < 4; i++) { 573 | int tmi = tmOffset + i; 574 | int mi = mOffset + i; 575 | tm[12 + tmi] = m[mi] * x + m[4 + mi] * y + m[8 + mi] * z + m[12 + mi]; 576 | } 577 | } 578 | 579 | /** 580 | * Translates matrix m by x, y, and z in place. 581 | * 582 | * @param m matrix 583 | * @param mOffset index into m where the matrix starts 584 | * @param x translation factor x 585 | * @param y translation factor y 586 | * @param z translation factor z 587 | */ 588 | public static void translateM(float[] m, int mOffset, float x, float y, float z) { 589 | for (int i = 0; i < 4; i++) { 590 | int mi = mOffset + i; 591 | m[12 + mi] += m[mi] * x + m[4 + mi] * y + m[8 + mi] * z; 592 | } 593 | } 594 | 595 | /** 596 | * Rotates matrix m by angle a (in degrees) around the axis (x, y, z) 597 | * 598 | * @param rm returns the result 599 | * @param rmOffset index into rm where the result matrix starts 600 | * @param m source matrix 601 | * @param mOffset index into m where the source matrix starts 602 | * @param a angle to rotate in degrees 603 | * @param x scale factor x 604 | * @param y scale factor y 605 | * @param z scale factor z 606 | */ 607 | public static void rotateM(float[] rm, int rmOffset, float[] m, int mOffset, float a, float x, float y, float z) { 608 | synchronized (TEMP_MATRIX_ARRAY) { 609 | setRotateM(TEMP_MATRIX_ARRAY, 0, a, x, y, z); 610 | multiplyMM(rm, rmOffset, m, mOffset, TEMP_MATRIX_ARRAY, 0); 611 | } 612 | } 613 | 614 | /** 615 | * Rotates matrix m in place by angle a (in degrees) 616 | * around the axis (x, y, z) 617 | * 618 | * @param m source matrix 619 | * @param mOffset index into m where the matrix starts 620 | * @param a angle to rotate in degrees 621 | * @param x scale factor x 622 | * @param y scale factor y 623 | * @param z scale factor z 624 | */ 625 | public static void rotateM(float[] m, int mOffset, float a, float x, float y, float z) { 626 | synchronized (TEMP_MATRIX_ARRAY) { 627 | setRotateM(TEMP_MATRIX_ARRAY, 0, a, x, y, z); 628 | multiplyMM(TEMP_MATRIX_ARRAY, 16, m, mOffset, TEMP_MATRIX_ARRAY, 0); 629 | System.arraycopy(TEMP_MATRIX_ARRAY, 16, m, mOffset, 16); 630 | } 631 | } 632 | 633 | /** 634 | * Rotates matrix m by angle a (in degrees) around the axis (x, y, z) 635 | * 636 | * @param rm returns the result 637 | * @param rmOffset index into rm where the result matrix starts 638 | * @param a angle to rotate in degrees 639 | * @param x scale factor x 640 | * @param y scale factor y 641 | * @param z scale factor z 642 | */ 643 | public static void setRotateM(float[] rm, int rmOffset, float a, float x, float y, float z) { 644 | rm[rmOffset + 3] = 0; 645 | rm[rmOffset + 7] = 0; 646 | rm[rmOffset + 11] = 0; 647 | rm[rmOffset + 12] = 0; 648 | rm[rmOffset + 13] = 0; 649 | rm[rmOffset + 14] = 0; 650 | rm[rmOffset + 15] = 1; 651 | a *= (float) (Math.PI / 180.0f); 652 | float s = (float) Math.sin(a); 653 | float c = (float) Math.cos(a); 654 | if (1.0f == x && 0.0f == y && 0.0f == z) { 655 | rm[rmOffset + 5] = c; 656 | rm[rmOffset + 10] = c; 657 | rm[rmOffset + 6] = s; 658 | rm[rmOffset + 9] = -s; 659 | rm[rmOffset + 1] = 0; 660 | rm[rmOffset + 2] = 0; 661 | rm[rmOffset + 4] = 0; 662 | rm[rmOffset + 8] = 0; 663 | rm[rmOffset + 0] = 1; 664 | } else if (0.0f == x && 1.0f == y && 0.0f == z) { 665 | rm[rmOffset + 0] = c; 666 | rm[rmOffset + 10] = c; 667 | rm[rmOffset + 8] = s; 668 | rm[rmOffset + 2] = -s; 669 | rm[rmOffset + 1] = 0; 670 | rm[rmOffset + 4] = 0; 671 | rm[rmOffset + 6] = 0; 672 | rm[rmOffset + 9] = 0; 673 | rm[rmOffset + 5] = 1; 674 | } else if (0.0f == x && 0.0f == y && 1.0f == z) { 675 | rm[rmOffset + 0] = c; 676 | rm[rmOffset + 5] = c; 677 | rm[rmOffset + 1] = s; 678 | rm[rmOffset + 4] = -s; 679 | rm[rmOffset + 2] = 0; 680 | rm[rmOffset + 6] = 0; 681 | rm[rmOffset + 8] = 0; 682 | rm[rmOffset + 9] = 0; 683 | rm[rmOffset + 10] = 1; 684 | } else { 685 | float len = length(x, y, z); 686 | if (1.0f != len) { 687 | float recipLen = 1.0f / len; 688 | x *= recipLen; 689 | y *= recipLen; 690 | z *= recipLen; 691 | } 692 | float nc = 1.0f - c; 693 | float xy = x * y; 694 | float yz = y * z; 695 | float zx = z * x; 696 | float xs = x * s; 697 | float ys = y * s; 698 | float zs = z * s; 699 | rm[rmOffset + 0] = x * x * nc + c; 700 | rm[rmOffset + 4] = xy * nc - zs; 701 | rm[rmOffset + 8] = zx * nc + ys; 702 | rm[rmOffset + 1] = xy * nc + zs; 703 | rm[rmOffset + 5] = y * y * nc + c; 704 | rm[rmOffset + 9] = yz * nc - xs; 705 | rm[rmOffset + 2] = zx * nc - ys; 706 | rm[rmOffset + 6] = yz * nc + xs; 707 | rm[rmOffset + 10] = z * z * nc + c; 708 | } 709 | } 710 | 711 | /** 712 | * Converts Euler angles to a rotation matrix 713 | * 714 | * @param rm returns the result 715 | * @param rmOffset index into rm where the result matrix starts 716 | * @param x angle of rotation, in degrees 717 | * @param y angle of rotation, in degrees 718 | * @param z angle of rotation, in degrees 719 | */ 720 | public static void setRotateEulerM(float[] rm, int rmOffset, float x, float y, float z) { 721 | x *= (float) (Math.PI / 180.0f); 722 | y *= (float) (Math.PI / 180.0f); 723 | z *= (float) (Math.PI / 180.0f); 724 | float cx = (float) Math.cos(x); 725 | float sx = (float) Math.sin(x); 726 | float cy = (float) Math.cos(y); 727 | float sy = (float) Math.sin(y); 728 | float cz = (float) Math.cos(z); 729 | float sz = (float) Math.sin(z); 730 | float cxsy = cx * sy; 731 | float sxsy = sx * sy; 732 | 733 | rm[rmOffset + 0] = cy * cz; 734 | rm[rmOffset + 1] = -cy * sz; 735 | rm[rmOffset + 2] = sy; 736 | rm[rmOffset + 3] = 0.0f; 737 | 738 | rm[rmOffset + 4] = cxsy * cz + cx * sz; 739 | rm[rmOffset + 5] = -cxsy * sz + cx * cz; 740 | rm[rmOffset + 6] = -sx * cy; 741 | rm[rmOffset + 7] = 0.0f; 742 | 743 | rm[rmOffset + 8] = -sxsy * cz + sx * sz; 744 | rm[rmOffset + 9] = sxsy * sz + sx * cz; 745 | rm[rmOffset + 10] = cx * cy; 746 | rm[rmOffset + 11] = 0.0f; 747 | 748 | rm[rmOffset + 12] = 0.0f; 749 | rm[rmOffset + 13] = 0.0f; 750 | rm[rmOffset + 14] = 0.0f; 751 | rm[rmOffset + 15] = 1.0f; 752 | } 753 | 754 | /** 755 | * Define a viewing transformation in terms of an eye point, a center of 756 | * view, and an up vector. 757 | * 758 | * @param rm returns the result 759 | * @param rmOffset index into rm where the result matrix starts 760 | * @param eyeX eye point X 761 | * @param eyeY eye point Y 762 | * @param eyeZ eye point Z 763 | * @param centerX center of view X 764 | * @param centerY center of view Y 765 | * @param centerZ center of view Z 766 | * @param upX up vector X 767 | * @param upY up vector Y 768 | * @param upZ up vector Z 769 | */ 770 | public static void setLookAtM(float[] rm, int rmOffset, float eyeX, float eyeY, float eyeZ, float centerX, 771 | float centerY, float centerZ, float upX, float upY, float upZ) { 772 | 773 | // See the OpenGL GLUT documentation for gluLookAt for a description 774 | // of the algorithm. We implement it in a straightforward way: 775 | 776 | float fx = centerX - eyeX; 777 | float fy = centerY - eyeY; 778 | float fz = centerZ - eyeZ; 779 | 780 | // Normalize f 781 | float rlf = 1.0f / Matrix.length(fx, fy, fz); 782 | fx *= rlf; 783 | fy *= rlf; 784 | fz *= rlf; 785 | 786 | // compute s = f x up (x means "cross product") 787 | float sx = fy * upZ - fz * upY; 788 | float sy = fz * upX - fx * upZ; 789 | float sz = fx * upY - fy * upX; 790 | 791 | // and normalize s 792 | float rls = 1.0f / Matrix.length(sx, sy, sz); 793 | sx *= rls; 794 | sy *= rls; 795 | sz *= rls; 796 | 797 | // compute u = s x f 798 | float ux = sy * fz - sz * fy; 799 | float uy = sz * fx - sx * fz; 800 | float uz = sx * fy - sy * fx; 801 | 802 | rm[rmOffset + 0] = sx; 803 | rm[rmOffset + 1] = ux; 804 | rm[rmOffset + 2] = -fx; 805 | rm[rmOffset + 3] = 0.0f; 806 | 807 | rm[rmOffset + 4] = sy; 808 | rm[rmOffset + 5] = uy; 809 | rm[rmOffset + 6] = -fy; 810 | rm[rmOffset + 7] = 0.0f; 811 | 812 | rm[rmOffset + 8] = sz; 813 | rm[rmOffset + 9] = uz; 814 | rm[rmOffset + 10] = -fz; 815 | rm[rmOffset + 11] = 0.0f; 816 | 817 | rm[rmOffset + 12] = 0.0f; 818 | rm[rmOffset + 13] = 0.0f; 819 | rm[rmOffset + 14] = 0.0f; 820 | rm[rmOffset + 15] = 1.0f; 821 | 822 | translateM(rm, rmOffset, -eyeX, -eyeY, -eyeZ); 823 | } 824 | } -------------------------------------------------------------------------------- /app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/MatrixF4x4.java: -------------------------------------------------------------------------------- 1 | package org.hitlabnz.sensor_fusion_demo.representation; 2 | 3 | import android.util.Log; 4 | 5 | /** 6 | * The Class MatrixF4x4. 7 | * 8 | * Internal the matrix is structured as 9 | * 10 | * [ x0 , y0 , z0 , w0 ] [ x1 , y1 , z1 , w1 ] [ x2 , y2 , z2 , w2 ] [ x3 , y3 , z3 , w3 ] 11 | * 12 | * it is recommend that when setting the matrix values individually that you use the set{x,#} methods, where 'x' is 13 | * 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 14 | * because it will map directly to that part of the matrix regardless of whether or not the internal matrix is column 15 | * 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. 16 | * 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 17 | * error. 18 | * 19 | */ 20 | public class MatrixF4x4 { 21 | 22 | public static final int[] matIndCol9_3x3 = { 0, 1, 2, 3, 4, 5, 6, 7, 8 }; 23 | public static final int[] matIndCol16_3x3 = { 0, 1, 2, 4, 5, 6, 8, 9, 10 }; 24 | public static final int[] matIndRow9_3x3 = { 0, 3, 6, 1, 4, 7, 3, 5, 8 }; 25 | public static final int[] matIndRow16_3x3 = { 0, 4, 8, 1, 5, 9, 2, 6, 10 }; 26 | 27 | public static final int[] matIndCol16_4x4 = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15 }; 28 | public static final int[] matIndRow16_4x4 = { 0, 4, 8, 12, 1, 5, 9, 13, 2, 6, 10, 14, 3, 7, 11, 15 }; 29 | 30 | private boolean colMaj = true; 31 | 32 | /** The matrix. */ 33 | public float[] matrix; 34 | 35 | /** 36 | * Instantiates a new matrixf4x4. The Matrix is assumed to be Column major, however you can change this by using the 37 | * setColumnMajor function to false and it will operate like a row major matrix. 38 | */ 39 | public MatrixF4x4() { 40 | // The matrix is defined as float[column][row] 41 | this.matrix = new float[16]; 42 | Matrix.setIdentityM(this.matrix, 0); 43 | } 44 | 45 | /** 46 | * Gets the matrix. 47 | * 48 | * @return the matrix, can be null if the matrix is invalid 49 | */ 50 | public float[] getMatrix() { 51 | return this.matrix; 52 | } 53 | 54 | public int size() { 55 | return matrix.length; 56 | } 57 | 58 | /** 59 | * Sets the matrix from a float[16] array. If the matrix you set isn't 16 long then the matrix will be set as 60 | * invalid. 61 | * 62 | * @param matrix the new matrix 63 | */ 64 | public void setMatrix(float[] matrix) { 65 | if (matrix.length == 16 || matrix.length == 9) 66 | this.matrix = matrix; 67 | else { 68 | throw new IllegalArgumentException("Matrix set is invalid, size is " + matrix.length + " expected 9 or 16"); 69 | } 70 | } 71 | 72 | public void set(MatrixF4x4 source) { 73 | System.arraycopy(source.matrix, 0, matrix, 0, matrix.length); 74 | } 75 | 76 | /** 77 | * Set whether the internal data is col major by passing true, or false for a row major matrix. The matrix is column 78 | * major by default. 79 | * 80 | * @param colMajor 81 | */ 82 | public void setColumnMajor(boolean colMajor) { 83 | this.colMaj = colMajor; 84 | } 85 | 86 | /** 87 | * Find out if the stored matrix is column major 88 | * 89 | * @return 90 | */ 91 | public boolean isColumnMajor() { 92 | return colMaj; 93 | } 94 | 95 | /** 96 | * Multiply the given vector by this matrix. This should only be used if the matrix is of size 16 (use the 97 | * matrix.size() method). 98 | * 99 | * @param vector A vector of length 4. 100 | */ 101 | public void multiplyVector4fByMatrix(Vector4f vector) { 102 | 103 | if (matrix.length == 16) { 104 | float x = 0; 105 | float y = 0; 106 | float z = 0; 107 | float w = 0; 108 | 109 | float[] vectorArray = vector.array(); 110 | 111 | if (colMaj) { 112 | for (int i = 0; i < 4; i++) { 113 | 114 | int k = i * 4; 115 | 116 | x += this.matrix[k + 0] * vectorArray[i]; 117 | y += this.matrix[k + 1] * vectorArray[i]; 118 | z += this.matrix[k + 2] * vectorArray[i]; 119 | w += this.matrix[k + 3] * vectorArray[i]; 120 | } 121 | } else { 122 | for (int i = 0; i < 4; i++) { 123 | 124 | x += this.matrix[0 + i] * vectorArray[i]; 125 | y += this.matrix[4 + i] * vectorArray[i]; 126 | z += this.matrix[8 + i] * vectorArray[i]; 127 | w += this.matrix[12 + i] * vectorArray[i]; 128 | } 129 | } 130 | 131 | vector.setX(x); 132 | vector.setY(y); 133 | vector.setZ(z); 134 | vector.setW(w); 135 | } else 136 | Log.e("matrix", "Matrix is invalid, is " + matrix.length + " long, this equation expects a 16 value matrix"); 137 | } 138 | 139 | /** 140 | * Multiply the given vector by this matrix. This should only be used if the matrix is of size 9 (use the 141 | * matrix.size() method). 142 | * 143 | * @param vector A vector of length 3. 144 | */ 145 | public void multiplyVector3fByMatrix(Vector3f vector) { 146 | 147 | if (matrix.length == 9) { 148 | float x = 0; 149 | float y = 0; 150 | float z = 0; 151 | 152 | float[] vectorArray = vector.toArray(); 153 | 154 | if (!colMaj) { 155 | for (int i = 0; i < 3; i++) { 156 | 157 | int k = i * 3; 158 | 159 | x += this.matrix[k + 0] * vectorArray[i]; 160 | y += this.matrix[k + 1] * vectorArray[i]; 161 | z += this.matrix[k + 2] * vectorArray[i]; 162 | } 163 | } else { 164 | for (int i = 0; i < 3; i++) { 165 | 166 | x += this.matrix[0 + i] * vectorArray[i]; 167 | y += this.matrix[3 + i] * vectorArray[i]; 168 | z += this.matrix[6 + i] * vectorArray[i]; 169 | } 170 | } 171 | 172 | vector.setX(x); 173 | vector.setY(y); 174 | vector.setZ(z); 175 | } else 176 | Log.e("matrix", "Matrix is invalid, is " + matrix.length 177 | + " long, this function expects the internal matrix to be of size 9"); 178 | } 179 | 180 | /** 181 | * Multiply matrix4x4 by matrix. 182 | * 183 | * @param matrixf the matrixf 184 | */ 185 | public void multiplyMatrix4x4ByMatrix(MatrixF4x4 matrixf) { 186 | 187 | // TODO implement Strassen Algorithm in place of this slower naive one. 188 | float[] bufferMatrix = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; 189 | float[] matrix = matrixf.getMatrix(); 190 | 191 | /** 192 | * for(int i = 0; i < 4; i++){ for(int j = 0; j < 4; j++){ 193 | * 194 | * int k = i * 4; bufferMatrix[0 + j] += this.matrix[k + j] * matrix[0 * 4 + i]; bufferMatrix[1 * 4 + j] += 195 | * this.matrix[k + j] * matrix[1 * 4 + i]; bufferMatrix[2 * 4 + j] += this.matrix[k + j] * matrix[2 * 4 + 196 | * i]; bufferMatrix[3 * 4 + j] += this.matrix[k + j] * matrix[3 * 4 + i]; } } 197 | */ 198 | 199 | multiplyMatrix(matrix, 0, bufferMatrix, 0); 200 | matrixf.setMatrix(bufferMatrix); 201 | } 202 | 203 | public void multiplyMatrix(float[] input, int inputOffset, float[] output, int outputOffset) { 204 | float[] bufferMatrix = output; 205 | float[] matrix = input; 206 | 207 | for (int i = 0; i < 4; i++) { 208 | for (int j = 0; j < 4; j++) { 209 | 210 | int k = i * 4; 211 | bufferMatrix[outputOffset + 0 + j] += this.matrix[k + j] * matrix[inputOffset + 0 * 4 + i]; 212 | bufferMatrix[outputOffset + 1 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 1 * 4 + i]; 213 | bufferMatrix[outputOffset + 2 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 2 * 4 + i]; 214 | bufferMatrix[outputOffset + 3 * 4 + j] += this.matrix[k + j] * matrix[inputOffset + 3 * 4 + i]; 215 | } 216 | } 217 | } 218 | 219 | /** 220 | * This will rearrange the internal structure of the matrix. Be careful though as this is an expensive operation. 221 | */ 222 | public void transpose() { 223 | if (this.matrix.length == 16) { 224 | float[] newMatrix = new float[16]; 225 | for (int i = 0; i < 4; i++) { 226 | 227 | int k = i * 4; 228 | 229 | newMatrix[k] = matrix[i]; 230 | newMatrix[k + 1] = matrix[4 + i]; 231 | newMatrix[k + 2] = matrix[8 + i]; 232 | newMatrix[k + 3] = matrix[12 + i]; 233 | } 234 | matrix = newMatrix; 235 | 236 | } else { 237 | float[] newMatrix = new float[9]; 238 | for (int i = 0; i < 3; i++) { 239 | 240 | int k = i * 3; 241 | 242 | newMatrix[k] = matrix[i]; 243 | newMatrix[k + 1] = matrix[3 + i]; 244 | newMatrix[k + 2] = matrix[6 + i]; 245 | } 246 | matrix = newMatrix; 247 | } 248 | 249 | } 250 | 251 | public void setX0(float value) { 252 | 253 | if (matrix.length == 16) { 254 | if (colMaj) 255 | matrix[matIndCol16_3x3[0]] = value; 256 | else 257 | matrix[matIndRow16_3x3[0]] = value; 258 | } else { 259 | if (colMaj) 260 | matrix[matIndCol9_3x3[0]] = value; 261 | else 262 | matrix[matIndRow9_3x3[0]] = value; 263 | } 264 | } 265 | 266 | public void setX1(float value) { 267 | 268 | if (matrix.length == 16) { 269 | if (colMaj) 270 | matrix[matIndCol16_3x3[1]] = value; 271 | else 272 | matrix[matIndRow16_3x3[1]] = value; 273 | } else { 274 | if (colMaj) 275 | matrix[matIndCol9_3x3[1]] = value; 276 | else 277 | matrix[matIndRow9_3x3[1]] = value; 278 | } 279 | } 280 | 281 | public void setX2(float value) { 282 | 283 | if (matrix.length == 16) { 284 | if (colMaj) 285 | matrix[matIndCol16_3x3[2]] = value; 286 | else 287 | matrix[matIndRow16_3x3[2]] = value; 288 | } else { 289 | if (colMaj) 290 | matrix[matIndCol9_3x3[2]] = value; 291 | else 292 | matrix[matIndRow9_3x3[2]] = value; 293 | } 294 | } 295 | 296 | public void setY0(float value) { 297 | 298 | if (matrix.length == 16) { 299 | if (colMaj) 300 | matrix[matIndCol16_3x3[3]] = value; 301 | else 302 | matrix[matIndRow16_3x3[3]] = value; 303 | } else { 304 | if (colMaj) 305 | matrix[matIndCol9_3x3[3]] = value; 306 | else 307 | matrix[matIndRow9_3x3[3]] = value; 308 | } 309 | } 310 | 311 | public void setY1(float value) { 312 | 313 | if (matrix.length == 16) { 314 | if (colMaj) 315 | matrix[matIndCol16_3x3[4]] = value; 316 | else 317 | matrix[matIndRow16_3x3[4]] = value; 318 | } else { 319 | if (colMaj) 320 | matrix[matIndCol9_3x3[4]] = value; 321 | else 322 | matrix[matIndRow9_3x3[4]] = value; 323 | } 324 | } 325 | 326 | public void setY2(float value) { 327 | 328 | if (matrix.length == 16) { 329 | if (colMaj) 330 | matrix[matIndCol16_3x3[5]] = value; 331 | else 332 | matrix[matIndRow16_3x3[5]] = value; 333 | } else { 334 | if (colMaj) 335 | matrix[matIndCol9_3x3[5]] = value; 336 | else 337 | matrix[matIndRow9_3x3[5]] = value; 338 | } 339 | } 340 | 341 | public void setZ0(float value) { 342 | 343 | if (matrix.length == 16) { 344 | if (colMaj) 345 | matrix[matIndCol16_3x3[6]] = value; 346 | else 347 | matrix[matIndRow16_3x3[6]] = value; 348 | } else { 349 | if (colMaj) 350 | matrix[matIndCol9_3x3[6]] = value; 351 | else 352 | matrix[matIndRow9_3x3[6]] = value; 353 | } 354 | } 355 | 356 | public void setZ1(float value) { 357 | 358 | if (matrix.length == 16) { 359 | if (colMaj) 360 | matrix[matIndCol16_3x3[7]] = value; 361 | else 362 | matrix[matIndRow16_3x3[7]] = value; 363 | } else { 364 | if (colMaj) 365 | matrix[matIndCol9_3x3[7]] = value; 366 | else 367 | matrix[matIndRow9_3x3[7]] = value; 368 | } 369 | } 370 | 371 | public void setZ2(float value) { 372 | 373 | if (matrix.length == 16) { 374 | if (colMaj) 375 | matrix[matIndCol16_3x3[8]] = value; 376 | else 377 | matrix[matIndRow16_3x3[8]] = value; 378 | } else { 379 | if (colMaj) 380 | matrix[matIndCol9_3x3[8]] = value; 381 | else 382 | matrix[matIndRow9_3x3[8]] = value; 383 | } 384 | } 385 | 386 | public void setX3(float value) { 387 | 388 | if (matrix.length == 16) { 389 | if (colMaj) 390 | matrix[matIndCol16_4x4[3]] = value; 391 | else 392 | matrix[matIndRow16_4x4[3]] = value; 393 | }else 394 | throw new IllegalStateException("length of matrix should be 16"); 395 | } 396 | 397 | public void setY3(float value) { 398 | 399 | if (matrix.length == 16) { 400 | if (colMaj) 401 | matrix[matIndCol16_4x4[7]] = value; 402 | else 403 | matrix[matIndRow16_4x4[7]] = value; 404 | }else 405 | throw new IllegalStateException("length of matrix should be 16"); 406 | } 407 | 408 | public void setZ3(float value) { 409 | 410 | if (matrix.length == 16) { 411 | if (colMaj) 412 | matrix[matIndCol16_4x4[11]] = value; 413 | else 414 | matrix[matIndRow16_4x4[11]] = value; 415 | }else 416 | throw new IllegalStateException("length of matrix should be 16"); 417 | } 418 | 419 | public void setW0(float value) { 420 | 421 | if (matrix.length == 16) { 422 | if (colMaj) 423 | matrix[matIndCol16_4x4[12]] = value; 424 | else 425 | matrix[matIndRow16_4x4[12]] = value; 426 | }else 427 | throw new IllegalStateException("length of matrix should be 16"); 428 | } 429 | 430 | public void setW1(float value) { 431 | 432 | if (matrix.length == 16) { 433 | if (colMaj) 434 | matrix[matIndCol16_4x4[13]] = value; 435 | else 436 | matrix[matIndRow16_4x4[13]] = value; 437 | }else 438 | throw new IllegalStateException("length of matrix should be 16"); 439 | } 440 | 441 | public void setW2(float value) { 442 | 443 | if (matrix.length == 16) { 444 | if (colMaj) 445 | matrix[matIndCol16_4x4[14]] = value; 446 | else 447 | matrix[matIndRow16_4x4[14]] = value; 448 | }else 449 | throw new IllegalStateException("length of matrix should be 16"); 450 | } 451 | 452 | public void setW3(float value) { 453 | 454 | if (matrix.length == 16) { 455 | if (colMaj) 456 | matrix[matIndCol16_4x4[15]] = value; 457 | else 458 | matrix[matIndRow16_4x4[15]] = value; 459 | }else 460 | throw new IllegalStateException("length of matrix should be 16"); 461 | } 462 | 463 | } 464 | -------------------------------------------------------------------------------- /app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Quaternion.java: -------------------------------------------------------------------------------- 1 | package org.hitlabnz.sensor_fusion_demo.representation; 2 | 3 | /** 4 | * The Quaternion class. A Quaternion is a four-dimensional vector that is used to represent rotations of a rigid body 5 | * in the 3D space. It is very similar to a rotation vector; it contains an angle, encoded into the w component 6 | * and three components to describe the rotation-axis (encoded into x, y, z). 7 | * 8 | *
9 | * Quaternions allow for elegant descriptions of 3D rotations, interpolations as well as extrapolations and compared to 10 | * Euler angles, they don't suffer from gimbal lock. Interpolations between two Quaternions are called SLERP (Spherical 11 | * Linear Interpolation). 12 | *
13 | * 14 | *15 | * This class also contains the representation of the same rotation as a Quaternion and 4x4-Rotation-Matrix. 16 | *
17 | * 18 | * @author Leigh Beattie, Alexander Pacha 19 | * 20 | */ 21 | public class Quaternion extends Vector4f { 22 | 23 | /** 24 | * Rotation matrix that contains the same rotation as the Quaternion in a 4x4 homogenised rotation matrix. 25 | * Remember that for performance reasons, this matrix is only updated, when it is accessed and not on every change 26 | * of the quaternion-values. 27 | */ 28 | private MatrixF4x4 matrix; 29 | 30 | /** 31 | * This variable is used to synchronise the rotation matrix with the current quaternion values. If someone has 32 | * changed the 33 | * quaternion numbers then the matrix will need to be updated. To save on processing we only really want to update 34 | * the matrix when someone wants to fetch it, instead of whenever someone sets a quaternion value. 35 | */ 36 | private boolean dirty = false; 37 | 38 | private Vector4f tmpVector = new Vector4f(); 39 | private Quaternion tmpQuaternion; 40 | 41 | /** 42 | * Creates a new Quaternion object and initialises it with the identity Quaternion 43 | */ 44 | public Quaternion() { 45 | super(); 46 | matrix = new MatrixF4x4(); 47 | loadIdentityQuat(); 48 | } 49 | 50 | /** 51 | * Normalise this Quaternion into a unity Quaternion. 52 | */ 53 | public void normalise() { 54 | this.dirty = true; 55 | float mag = (float) Math.sqrt(points[3] * points[3] + points[0] * points[0] + points[1] * points[1] + points[2] 56 | * points[2]); 57 | points[3] = points[3] / mag; 58 | points[0] = points[0] / mag; 59 | points[1] = points[1] / mag; 60 | points[2] = points[2] / mag; 61 | } 62 | 63 | @Override 64 | public void normalize() { 65 | normalise(); 66 | } 67 | 68 | /** 69 | * Copies the values from the given quaternion to this one 70 | * 71 | * @param quat The quaternion to copy from 72 | */ 73 | public void set(Quaternion quat) { 74 | this.dirty = true; 75 | copyVec4(quat); 76 | } 77 | 78 | /** 79 | * Multiply this quaternion by the input quaternion and store the result in the out quaternion 80 | * 81 | * @param input 82 | * @param output 83 | */ 84 | public void multiplyByQuat(Quaternion input, Quaternion output) { 85 | 86 | if (input != output) { 87 | output.points[3] = (points[3] * input.points[3] - points[0] * input.points[0] - points[1] * input.points[1] - points[2] 88 | * input.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2 89 | output.points[0] = (points[3] * input.points[0] + points[0] * input.points[3] + points[1] * input.points[2] - points[2] 90 | * input.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2 91 | output.points[1] = (points[3] * input.points[1] + points[1] * input.points[3] + points[2] * input.points[0] - points[0] 92 | * input.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2 93 | output.points[2] = (points[3] * input.points[2] + points[2] * input.points[3] + points[0] * input.points[1] - points[1] 94 | * input.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2 95 | } else { 96 | tmpVector.points[0] = input.points[0]; 97 | tmpVector.points[1] = input.points[1]; 98 | tmpVector.points[2] = input.points[2]; 99 | tmpVector.points[3] = input.points[3]; 100 | 101 | output.points[3] = (points[3] * tmpVector.points[3] - points[0] * tmpVector.points[0] - points[1] 102 | * tmpVector.points[1] - points[2] * tmpVector.points[2]); //w = w1w2 - x1x2 - y1y2 - z1z2 103 | output.points[0] = (points[3] * tmpVector.points[0] + points[0] * tmpVector.points[3] + points[1] 104 | * tmpVector.points[2] - points[2] * tmpVector.points[1]); //x = w1x2 + x1w2 + y1z2 - z1y2 105 | output.points[1] = (points[3] * tmpVector.points[1] + points[1] * tmpVector.points[3] + points[2] 106 | * tmpVector.points[0] - points[0] * tmpVector.points[2]); //y = w1y2 + y1w2 + z1x2 - x1z2 107 | output.points[2] = (points[3] * tmpVector.points[2] + points[2] * tmpVector.points[3] + points[0] 108 | * tmpVector.points[1] - points[1] * tmpVector.points[0]); //z = w1z2 + z1w2 + x1y2 - y1x2 109 | } 110 | } 111 | 112 | /** 113 | * Multiply this quaternion by the input quaternion and store the result in the out quaternion 114 | * 115 | * @param input 116 | */ 117 | public void multiplyByQuat(Quaternion input) { 118 | this.dirty = true; 119 | if(tmpQuaternion == null) tmpQuaternion = new Quaternion(); 120 | tmpQuaternion.copyVec4(this); 121 | multiplyByQuat(input, tmpQuaternion); 122 | this.copyVec4(tmpQuaternion); 123 | } 124 | 125 | /** 126 | * Multiplies this Quaternion with a scalar 127 | * 128 | * @param scalar the value that the vector should be multiplied with 129 | */ 130 | public void multiplyByScalar(float scalar) { 131 | this.dirty = true; 132 | multiplyByScalar(scalar); 133 | } 134 | 135 | /** 136 | * Add a quaternion to this quaternion 137 | * 138 | * @param input The quaternion that you want to add to this one 139 | */ 140 | public void addQuat(Quaternion input) { 141 | this.dirty = true; 142 | addQuat(input, this); 143 | } 144 | 145 | /** 146 | * Add this quaternion and another quaternion together and store the result in the output quaternion 147 | * 148 | * @param input The quaternion you want added to this quaternion 149 | * @param output The quaternion you want to store the output in. 150 | */ 151 | public void addQuat(Quaternion input, Quaternion output) { 152 | output.setX(getX() + input.getX()); 153 | output.setY(getY() + input.getY()); 154 | output.setZ(getZ() + input.getZ()); 155 | output.setW(getW() + input.getW()); 156 | } 157 | 158 | /** 159 | * Subtract a quaternion to this quaternion 160 | * 161 | * @param input The quaternion that you want to subtracted from this one 162 | */ 163 | public void subQuat(Quaternion input) { 164 | this.dirty = true; 165 | subQuat(input, this); 166 | } 167 | 168 | /** 169 | * Subtract another quaternion from this quaternion and store the result in the output quaternion 170 | * 171 | * @param input The quaternion you want subtracted from this quaternion 172 | * @param output The quaternion you want to store the output in. 173 | */ 174 | public void subQuat(Quaternion input, Quaternion output) { 175 | output.setX(getX() - input.getX()); 176 | output.setY(getY() - input.getY()); 177 | output.setZ(getZ() - input.getZ()); 178 | output.setW(getW() - input.getW()); 179 | } 180 | 181 | /** 182 | * Converts this Quaternion into the Rotation-Matrix representation which can be accessed by 183 | * {@link Quaternion#getMatrix4x4 getMatrix4x4} 184 | */ 185 | private void convertQuatToMatrix() { 186 | float x = points[0]; 187 | float y = points[1]; 188 | float z = points[2]; 189 | float w = points[3]; 190 | 191 | matrix.setX0(1 - 2 * (y * y) - 2 * (z * z)); //1 - 2y2 - 2z2 192 | matrix.setX1(2 * (x * y) + 2 * (w * z)); // 2xy - 2wz 193 | matrix.setX2(2 * (x * z) - 2 * (w * y)); //2xz + 2wy 194 | matrix.setX3(0); 195 | matrix.setY0(2 * (x * y) - 2 * (w * z)); //2xy + 2wz 196 | matrix.setY1(1 - 2 * (x * x) - 2 * (z * z)); //1 - 2x2 - 2z2 197 | matrix.setY2(2 * (y * z) + 2 * (w * x)); // 2yz + 2wx 198 | matrix.setY3(0); 199 | matrix.setZ0(2 * (x * z) + 2 * (w * y)); //2xz + 2wy 200 | matrix.setZ1(2 * (y * z) - 2 * (w * x)); //2yz - 2wx 201 | matrix.setZ2(1 - 2 * (x * x) - 2 * (y * y)); //1 - 2x2 - 2y2 202 | matrix.setZ3(0); 203 | matrix.setW0(0); 204 | matrix.setW1(0); 205 | matrix.setW2(0); 206 | matrix.setW3(1); 207 | } 208 | 209 | /** 210 | * Get an axis angle representation of this quaternion. 211 | * 212 | * @param output Vector4f axis angle. 213 | */ 214 | public void toAxisAngle(Vector4f output) { 215 | if (getW() > 1) { 216 | normalise(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised 217 | } 218 | float angle = 2 * (float) Math.toDegrees(Math.acos(getW())); 219 | float x; 220 | float y; 221 | float z; 222 | 223 | float s = (float) Math.sqrt(1 - getW() * getW()); // assuming quaternion normalised then w is less than 1, so term always positive. 224 | if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt 225 | // if s close to zero then direction of axis not important 226 | x = points[0]; // if it is important that axis is normalised then replace with x=1; y=z=0; 227 | y = points[1]; 228 | z = points[2]; 229 | } else { 230 | x = points[0] / s; // normalise axis 231 | y = points[1] / s; 232 | z = points[2] / s; 233 | } 234 | 235 | output.points[0] = x; 236 | output.points[1] = y; 237 | output.points[2] = z; 238 | output.points[3] = angle; 239 | } 240 | 241 | /** 242 | * Returns the heading, attitude and bank of this quaternion as euler angles in the double array respectively 243 | * 244 | * @return An array of size 3 containing the euler angles for this quaternion 245 | */ 246 | public double[] toEulerAngles() { 247 | double[] ret = new double[3]; 248 | 249 | ret[0] = Math.atan2(2 * points[1] * getW() - 2 * points[0] * points[2], 1 - 2 * (points[1] * points[1]) - 2 250 | * (points[2] * points[2])); // atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2) 251 | ret[1] = Math.asin(2 * points[0] * points[1] + 2 * points[2] * getW()); // asin(2*qx*qy + 2*qz*qw) 252 | ret[2] = Math.atan2(2 * points[0] * getW() - 2 * points[1] * points[2], 1 - 2 * (points[0] * points[0]) - 2 253 | * (points[2] * points[2])); // atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2) 254 | 255 | return ret; 256 | } 257 | 258 | /** 259 | * Sets the quaternion to an identity quaternion of 0,0,0,1. 260 | */ 261 | public void loadIdentityQuat() { 262 | this.dirty = true; 263 | setX(0); 264 | setY(0); 265 | setZ(0); 266 | setW(1); 267 | } 268 | 269 | @Override 270 | public String toString() { 271 | return "{X: " + getX() + ", Y:" + getY() + ", Z:" + getZ() + ", W:" + getW() + "}"; 272 | } 273 | 274 | /** 275 | * This is an internal method used to build a quaternion from a rotation matrix and then sets the current quaternion 276 | * from that matrix. 277 | * 278 | */ 279 | private void generateQuaternionFromMatrix() { 280 | 281 | float qx; 282 | float qy; 283 | float qz; 284 | float qw; 285 | 286 | float[] mat = matrix.getMatrix(); 287 | int[] indices = null; 288 | 289 | if (this.matrix.size() == 16) { 290 | if (this.matrix.isColumnMajor()) { 291 | indices = MatrixF4x4.matIndCol16_3x3; 292 | } else { 293 | indices = MatrixF4x4.matIndRow16_3x3; 294 | } 295 | } else { 296 | if (this.matrix.isColumnMajor()) { 297 | indices = MatrixF4x4.matIndCol9_3x3; 298 | } else { 299 | indices = MatrixF4x4.matIndRow9_3x3; 300 | } 301 | } 302 | 303 | int m00 = indices[0]; 304 | int m01 = indices[1]; 305 | int m02 = indices[2]; 306 | 307 | int m10 = indices[3]; 308 | int m11 = indices[4]; 309 | int m12 = indices[5]; 310 | 311 | int m20 = indices[6]; 312 | int m21 = indices[7]; 313 | int m22 = indices[8]; 314 | 315 | float tr = mat[m00] + mat[m11] + mat[m22]; 316 | if (tr > 0) { 317 | float s = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw 318 | qw = 0.25f * s; 319 | qx = (mat[m21] - mat[m12]) / s; 320 | qy = (mat[m02] - mat[m20]) / s; 321 | qz = (mat[m10] - mat[m01]) / s; 322 | } else if ((mat[m00] > mat[m11]) & (mat[m00] > mat[m22])) { 323 | float s = (float) Math.sqrt(1.0 + mat[m00] - mat[m11] - mat[m22]) * 2; // S=4*qx 324 | qw = (mat[m21] - mat[m12]) / s; 325 | qx = 0.25f * s; 326 | qy = (mat[m01] + mat[m10]) / s; 327 | qz = (mat[m02] + mat[m20]) / s; 328 | } else if (mat[m11] > mat[m22]) { 329 | float s = (float) Math.sqrt(1.0 + mat[m11] - mat[m00] - mat[m22]) * 2; // S=4*qy 330 | qw = (mat[m02] - mat[m20]) / s; 331 | qx = (mat[m01] + mat[m10]) / s; 332 | qy = 0.25f * s; 333 | qz = (mat[m12] + mat[m21]) / s; 334 | } else { 335 | float s = (float) Math.sqrt(1.0 + mat[m22] - mat[m00] - mat[m11]) * 2; // S=4*qz 336 | qw = (mat[m10] - mat[m01]) / s; 337 | qx = (mat[m02] + mat[m20]) / s; 338 | qy = (mat[m12] + mat[m21]) / s; 339 | qz = 0.25f * s; 340 | } 341 | 342 | setX(qx); 343 | setY(qy); 344 | setZ(qz); 345 | setW(qw); 346 | } 347 | 348 | /** 349 | * You can set the values for this quaternion based off a rotation matrix. If the matrix you supply is not a 350 | * rotation matrix this will fail. You MUST provide a 4x4 matrix. 351 | * 352 | * @param matrix A column major rotation matrix 353 | */ 354 | public void setColumnMajor(float[] matrix) { 355 | 356 | this.matrix.setMatrix(matrix); 357 | this.matrix.setColumnMajor(true); 358 | 359 | generateQuaternionFromMatrix(); 360 | } 361 | 362 | /** 363 | * You can set the values for this quaternion based off a rotation matrix. If the matrix you supply is not a 364 | * rotation matrix this will fail. 365 | * 366 | * @param matrix A column major rotation matrix 367 | */ 368 | public void setRowMajor(float[] matrix) { 369 | 370 | this.matrix.setMatrix(matrix); 371 | this.matrix.setColumnMajor(false); 372 | 373 | generateQuaternionFromMatrix(); 374 | } 375 | 376 | /** 377 | * Set this quaternion from axis angle values. All rotations are in degrees. 378 | * 379 | * @param azimuth The rotation around the z axis 380 | * @param pitch The rotation around the y axis 381 | * @param roll The rotation around the x axis 382 | */ 383 | public void setEulerAngle(float azimuth, float pitch, float roll) { 384 | 385 | double heading = Math.toRadians(roll); 386 | double attitude = Math.toRadians(pitch); 387 | double bank = Math.toRadians(azimuth); 388 | 389 | double c1 = Math.cos(heading / 2); 390 | double s1 = Math.sin(heading / 2); 391 | double c2 = Math.cos(attitude / 2); 392 | double s2 = Math.sin(attitude / 2); 393 | double c3 = Math.cos(bank / 2); 394 | double s3 = Math.sin(bank / 2); 395 | double c1c2 = c1 * c2; 396 | double s1s2 = s1 * s2; 397 | setW((float) (c1c2 * c3 - s1s2 * s3)); 398 | setX((float) (c1c2 * s3 + s1s2 * c3)); 399 | setY((float) (s1 * c2 * c3 + c1 * s2 * s3)); 400 | setZ((float) (c1 * s2 * c3 - s1 * c2 * s3)); 401 | 402 | dirty = true; 403 | } 404 | 405 | /** 406 | * Rotation is in degrees. Set this quaternion from the supplied axis angle. 407 | * 408 | * @param vec The vector of rotation 409 | * @param rot The angle of rotation around that vector in degrees. 410 | */ 411 | public void setAxisAngle(Vector3f vec, float rot) { 412 | double s = Math.sin(Math.toRadians(rot / 2)); 413 | setX(vec.getX() * (float) s); 414 | setY(vec.getY() * (float) s); 415 | setZ(vec.getZ() * (float) s); 416 | setW((float) Math.cos(Math.toRadians(rot / 2))); 417 | 418 | dirty = true; 419 | } 420 | 421 | public void setAxisAngleRad(Vector3f vec, double rot) { 422 | double s = rot / 2; 423 | setX(vec.getX() * (float) s); 424 | setY(vec.getY() * (float) s); 425 | setZ(vec.getZ() * (float) s); 426 | setW((float) rot / 2); 427 | 428 | dirty = true; 429 | } 430 | 431 | /** 432 | * @return Returns this Quaternion in the Rotation Matrix representation 433 | */ 434 | public MatrixF4x4 getMatrix4x4() { 435 | //toMatrixColMajor(); 436 | if (dirty) { 437 | convertQuatToMatrix(); 438 | dirty = false; 439 | } 440 | return this.matrix; 441 | } 442 | 443 | public void copyFromVec3(Vector3f vec, float w) { 444 | copyFromV3f(vec, w); 445 | } 446 | 447 | /** 448 | * Get a linear interpolation between this quaternion and the input quaternion, storing the result in the output 449 | * quaternion. 450 | * 451 | * @param input The quaternion to be slerped with this quaternion. 452 | * @param output The quaternion to store the result in. 453 | * @param t The ratio between the two quaternions where 0 <= t <= 1.0 . Increase value of t will bring rotation 454 | * closer to the input quaternion. 455 | */ 456 | public void slerp(Quaternion input, Quaternion output, float t) { 457 | // Calculate angle between them. 458 | //double cosHalftheta = this.dotProduct(input); 459 | Quaternion bufferQuat; 460 | float cosHalftheta = this.dotProduct(input); 461 | 462 | if (cosHalftheta < 0) { 463 | if(tmpQuaternion == null) tmpQuaternion = new Quaternion(); 464 | bufferQuat = tmpQuaternion; 465 | cosHalftheta = -cosHalftheta; 466 | bufferQuat.points[0] = (-input.points[0]); 467 | bufferQuat.points[1] = (-input.points[1]); 468 | bufferQuat.points[2] = (-input.points[2]); 469 | bufferQuat.points[3] = (-input.points[3]); 470 | } else { 471 | bufferQuat = input; 472 | } 473 | /** 474 | * if(dot < 0.95f){ 475 | * double angle = Math.acos(dot); 476 | * double ratioA = Math.sin((1 - t) * angle); 477 | * double ratioB = Math.sin(t * angle); 478 | * double divisor = Math.sin(angle); 479 | * 480 | * //Calculate Quaternion 481 | * output.setW((float)((this.getW() * ratioA + input.getW() * ratioB)/divisor)); 482 | * output.setX((float)((this.getX() * ratioA + input.getX() * ratioB)/divisor)); 483 | * output.setY((float)((this.getY() * ratioA + input.getY() * ratioB)/divisor)); 484 | * output.setZ((float)((this.getZ() * ratioA + input.getZ() * ratioB)/divisor)); 485 | * } 486 | * else{ 487 | * lerp(input, output, t); 488 | * } 489 | */ 490 | // if qa=qb or qa=-qb then theta = 0 and we can return qa 491 | if (Math.abs(cosHalftheta) >= 1.0) { 492 | output.points[0] = (this.points[0]); 493 | output.points[1] = (this.points[1]); 494 | output.points[2] = (this.points[2]); 495 | output.points[3] = (this.points[3]); 496 | } else { 497 | double sinHalfTheta = Math.sqrt(1.0 - cosHalftheta * cosHalftheta); 498 | // if theta = 180 degrees then result is not fully defined 499 | // we could rotate around any axis normal to qa or qb 500 | //if(Math.abs(sinHalfTheta) < 0.001){ 501 | //output.setW(this.getW() * 0.5f + input.getW() * 0.5f); 502 | //output.setX(this.getX() * 0.5f + input.getX() * 0.5f); 503 | //output.setY(this.getY() * 0.5f + input.getY() * 0.5f); 504 | //output.setZ(this.getZ() * 0.5f + input.getZ() * 0.5f); 505 | // lerp(bufferQuat, output, t); 506 | //} 507 | //else{ 508 | double halfTheta = Math.acos(cosHalftheta); 509 | 510 | double ratioA = Math.sin((1 - t) * halfTheta) / sinHalfTheta; 511 | double ratioB = Math.sin(t * halfTheta) / sinHalfTheta; 512 | 513 | //Calculate Quaternion 514 | output.points[3] = ((float) (points[3] * ratioA + bufferQuat.points[3] * ratioB)); 515 | output.points[0] = ((float) (this.points[0] * ratioA + bufferQuat.points[0] * ratioB)); 516 | output.points[1] = ((float) (this.points[1] * ratioA + bufferQuat.points[1] * ratioB)); 517 | output.points[2] = ((float) (this.points[2] * ratioA + bufferQuat.points[2] * ratioB)); 518 | 519 | //} 520 | } 521 | } 522 | 523 | } 524 | -------------------------------------------------------------------------------- /app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Vector3f.java: -------------------------------------------------------------------------------- 1 | package org.hitlabnz.sensor_fusion_demo.representation; 2 | 3 | /** 4 | * 3-dimensional vector with conventient getters and setters. Additionally this class is serializable and 5 | */ 6 | public class Vector3f { 7 | 8 | /** 9 | * A float array was chosen instead of individual variables due to performance concerns. Converting the points into 10 | * an array at run time can cause slowness so instead we use one array and extract the individual variables with get 11 | * methods. 12 | */ 13 | protected float[] points = new float[3]; 14 | 15 | /** 16 | * Initialises the vector with the given values 17 | * 18 | * @param x the x-component 19 | * @param y the y-component 20 | * @param z the z-component 21 | */ 22 | public Vector3f(float x, float y, float z) { 23 | this.points[0] = x; 24 | this.points[1] = y; 25 | this.points[2] = z; 26 | } 27 | 28 | /** 29 | * Initialises all components of this vector with the given same value. 30 | * 31 | * @param value Initialisation value for all components 32 | */ 33 | public Vector3f(float value) { 34 | this.points[0] = value; 35 | this.points[1] = value; 36 | this.points[2] = value; 37 | } 38 | 39 | /** 40 | * Instantiates a new vector3f. 41 | */ 42 | public Vector3f() { 43 | } 44 | 45 | /** 46 | * Copy constructor 47 | */ 48 | public Vector3f(Vector3f vector) { 49 | this.points[0] = vector.points[0]; 50 | this.points[1] = vector.points[1]; 51 | this.points[2] = vector.points[2]; 52 | } 53 | 54 | /** 55 | * Initialises this vector from a 4-dimensional vector. If the fourth component is not zero, a normalisation of all 56 | * components will be performed. 57 | * 58 | * @param vector The 4-dimensional vector that should be used for initialisation 59 | */ 60 | public Vector3f(Vector4f vector) { 61 | if (vector.w() != 0) { 62 | this.points[0] = vector.x() / vector.w(); 63 | this.points[1] = vector.y() / vector.w(); 64 | this.points[2] = vector.z() / vector.w(); 65 | } else { 66 | this.points[0] = vector.x(); 67 | this.points[1] = vector.y(); 68 | this.points[2] = vector.z(); 69 | } 70 | } 71 | 72 | /** 73 | * Returns this vector as float-array. 74 | * 75 | * @return the float[] 76 | */ 77 | public float[] toArray() { 78 | return this.points; 79 | } 80 | 81 | /** 82 | * Adds a vector to this vector 83 | * 84 | * @param summand the vector that should be added component-wise 85 | */ 86 | public void add(Vector3f summand) { 87 | this.points[0] += summand.points[0]; 88 | this.points[1] += summand.points[1]; 89 | this.points[2] += summand.points[2]; 90 | } 91 | 92 | /** 93 | * Adds the value to all components of this vector 94 | * 95 | * @param summand The value that should be added to all components 96 | */ 97 | public void add(float summand) { 98 | this.points[0] += summand; 99 | this.points[1] += summand; 100 | this.points[2] += summand; 101 | } 102 | 103 | /** 104 | * 105 | * @param subtrahend 106 | */ 107 | public void subtract(Vector3f subtrahend) { 108 | this.points[0] -= subtrahend.points[0]; 109 | this.points[1] -= subtrahend.points[1]; 110 | this.points[2] -= subtrahend.points[2]; 111 | } 112 | 113 | /** 114 | * Multiply by scalar. 115 | * 116 | * @param scalar the scalar 117 | */ 118 | public void multiplyByScalar(float scalar) { 119 | this.points[0] *= scalar; 120 | this.points[1] *= scalar; 121 | this.points[2] *= scalar; 122 | } 123 | 124 | /** 125 | * Normalize. 126 | */ 127 | public void normalize() { 128 | 129 | double a = Math.sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]); 130 | this.points[0] = (float) (this.points[0] / a); 131 | this.points[1] = (float) (this.points[1] / a); 132 | this.points[2] = (float) (this.points[2] / a); 133 | 134 | } 135 | 136 | /** 137 | * Gets the x. 138 | * 139 | * @return the x 140 | */ 141 | public float getX() { 142 | return points[0]; 143 | } 144 | 145 | /** 146 | * Gets the y. 147 | * 148 | * @return the y 149 | */ 150 | public float getY() { 151 | return points[1]; 152 | } 153 | 154 | /** 155 | * Gets the z. 156 | * 157 | * @return the z 158 | */ 159 | public float getZ() { 160 | return points[2]; 161 | } 162 | 163 | /** 164 | * Sets the x. 165 | * 166 | * @param x the new x 167 | */ 168 | public void setX(float x) { 169 | this.points[0] = x; 170 | } 171 | 172 | /** 173 | * Sets the y. 174 | * 175 | * @param y the new y 176 | */ 177 | public void setY(float y) { 178 | this.points[1] = y; 179 | } 180 | 181 | /** 182 | * Sets the z. 183 | * 184 | * @param z the new z 185 | */ 186 | public void setZ(float z) { 187 | this.points[2] = z; 188 | } 189 | 190 | /** 191 | * Functions for convenience 192 | */ 193 | 194 | public float x() { 195 | return this.points[0]; 196 | } 197 | 198 | public float y() { 199 | return this.points[1]; 200 | } 201 | 202 | public float z() { 203 | return this.points[2]; 204 | } 205 | 206 | public void x(float x) { 207 | this.points[0] = x; 208 | } 209 | 210 | public void y(float y) { 211 | this.points[1] = y; 212 | } 213 | 214 | public void z(float z) { 215 | this.points[2] = z; 216 | } 217 | 218 | public void setXYZ(float x, float y, float z) { 219 | this.points[0] = x; 220 | this.points[1] = y; 221 | this.points[2] = z; 222 | } 223 | 224 | /** 225 | * Return the dot product of this vector with the input vector 226 | * 227 | * @param inputVec The vector you want to do the dot product with against this vector. 228 | * @return Float value representing the scalar of the dot product operation 229 | */ 230 | public float dotProduct(Vector3f inputVec) { 231 | return points[0] * inputVec.points[0] + points[1] * inputVec.points[1] + points[2] * inputVec.points[2]; 232 | 233 | } 234 | 235 | /** 236 | * Get the cross product of this vector and another vector. The result will be stored in the output vector. 237 | * 238 | * @param inputVec The vector you want to get the dot product of against this vector. 239 | * @param outputVec The vector to store the result in. 240 | */ 241 | public void crossProduct(Vector3f inputVec, Vector3f outputVec) { 242 | outputVec.setX(points[1] * inputVec.points[2] - points[2] * inputVec.points[1]); 243 | outputVec.setY(points[2] * inputVec.points[0] - points[0] * inputVec.points[2]); 244 | outputVec.setZ(points[0] * inputVec.points[1] - points[1] * inputVec.points[0]); 245 | } 246 | 247 | /** 248 | * If you need to get the length of a vector then use this function. 249 | * 250 | * @return The length of the vector 251 | */ 252 | public float getLength() { 253 | return (float) Math.sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]); 254 | } 255 | 256 | @Override 257 | public String toString() { 258 | return "X:" + points[0] + " Y:" + points[1] + " Z:" + points[2]; 259 | } 260 | 261 | /** 262 | * Clone the input vector so that this vector has the same values. 263 | * 264 | * @param source The vector you want to clone. 265 | */ 266 | public void set(Vector3f source) { 267 | set(source.points); 268 | } 269 | 270 | /** 271 | * Clone the input vector so that this vector has the same values. 272 | * 273 | * @param source The vector you want to clone. 274 | */ 275 | public void set(float[] source) { 276 | System.arraycopy(source, 0, points, 0, 3); 277 | } 278 | } 279 | -------------------------------------------------------------------------------- /app/src/main/java/org/hitlabnz/sensor_fusion_demo/representation/Vector4f.java: -------------------------------------------------------------------------------- 1 | package org.hitlabnz.sensor_fusion_demo.representation; 2 | 3 | /** 4 | * Representation of a four-dimensional float-vector 5 | */ 6 | public class Vector4f { 7 | 8 | /** The points. */ 9 | protected float points[] = { 0, 0, 0, 0 }; 10 | 11 | /** 12 | * Instantiates a new vector4f. 13 | * 14 | * @param x the x 15 | * @param y the y 16 | * @param z the z 17 | * @param w the w 18 | */ 19 | public Vector4f(float x, float y, float z, float w) { 20 | this.points[0] = x; 21 | this.points[1] = y; 22 | this.points[2] = z; 23 | this.points[3] = w; 24 | } 25 | 26 | /** 27 | * Instantiates a new vector4f. 28 | */ 29 | public Vector4f() { 30 | this.points[0] = 0; 31 | this.points[1] = 0; 32 | this.points[2] = 0; 33 | this.points[3] = 0; 34 | } 35 | 36 | public Vector4f(Vector3f vector3f, float w) { 37 | this.points[0] = vector3f.x(); 38 | this.points[1] = vector3f.y(); 39 | this.points[2] = vector3f.z(); 40 | this.points[3] = w; 41 | } 42 | 43 | /** 44 | * To array. 45 | * 46 | * @return the float[] 47 | */ 48 | public float[] array() { 49 | return points; 50 | } 51 | 52 | public void copyVec4(Vector4f vec) { 53 | this.points[0] = vec.points[0]; 54 | this.points[1] = vec.points[1]; 55 | this.points[2] = vec.points[2]; 56 | this.points[3] = vec.points[3]; 57 | } 58 | 59 | /** 60 | * Adds the. 61 | * 62 | * @param vector the vector 63 | */ 64 | public void add(Vector4f vector) { 65 | this.points[0] += vector.points[0]; 66 | this.points[1] += vector.points[1]; 67 | this.points[2] += vector.points[2]; 68 | this.points[3] += vector.points[3]; 69 | } 70 | 71 | public void add(Vector3f vector, float w) { 72 | this.points[0] += vector.x(); 73 | this.points[1] += vector.y(); 74 | this.points[2] += vector.z(); 75 | this.points[3] += w; 76 | } 77 | 78 | public void subtract(Vector4f vector) { 79 | this.points[0] -= vector.points[0]; 80 | this.points[1] -= vector.points[1]; 81 | this.points[2] -= vector.points[2]; 82 | this.points[3] -= vector.points[3]; 83 | } 84 | 85 | public void subtract(Vector4f vector, Vector4f output) { 86 | output.setXYZW(this.points[0] - vector.points[0], this.points[1] - vector.points[1], this.points[2] 87 | - vector.points[2], this.points[3] - vector.points[3]); 88 | } 89 | 90 | public void subdivide(Vector4f vector) { 91 | this.points[0] /= vector.points[0]; 92 | this.points[1] /= vector.points[1]; 93 | this.points[2] /= vector.points[2]; 94 | this.points[3] /= vector.points[3]; 95 | } 96 | 97 | /** 98 | * Multiply by scalar. 99 | * 100 | * @param scalar the scalar 101 | */ 102 | public void multiplyByScalar(float scalar) { 103 | this.points[0] *= scalar; 104 | this.points[1] *= scalar; 105 | this.points[2] *= scalar; 106 | this.points[3] *= scalar; 107 | } 108 | 109 | public float dotProduct(Vector4f input) { 110 | return this.points[0] * input.points[0] + this.points[1] * input.points[1] + this.points[2] * input.points[2] 111 | + this.points[3] * input.points[3]; 112 | } 113 | 114 | /** 115 | * Linear interpolation between two vectors storing the result in the output variable. 116 | * 117 | * @param input 118 | * @param output 119 | * @param t 120 | */ 121 | public void lerp(Vector4f input, Vector4f output, float t) { 122 | output.points[0] = (points[0] * (1.0f * t) + input.points[0] * t); 123 | output.points[1] = (points[1] * (1.0f * t) + input.points[1] * t); 124 | output.points[2] = (points[2] * (1.0f * t) + input.points[2] * t); 125 | output.points[3] = (points[3] * (1.0f * t) + input.points[3] * t); 126 | 127 | } 128 | 129 | /** 130 | * Normalize. 131 | */ 132 | public void normalize() { 133 | if (points[3] == 0) 134 | return; 135 | 136 | points[0] /= points[3]; 137 | points[1] /= points[3]; 138 | points[2] /= points[3]; 139 | 140 | double a = Math.sqrt(this.points[0] * this.points[0] + this.points[1] * this.points[1] + this.points[2] 141 | * this.points[2]); 142 | points[0] = (float) (this.points[0] / a); 143 | points[1] = (float) (this.points[1] / a); 144 | points[2] = (float) (this.points[2] / a); 145 | } 146 | 147 | /** 148 | * Gets the x. 149 | * 150 | * @return the x 151 | */ 152 | public float getX() { 153 | return this.points[0]; 154 | } 155 | 156 | /** 157 | * Gets the y. 158 | * 159 | * @return the y 160 | */ 161 | public float getY() { 162 | return this.points[1]; 163 | } 164 | 165 | /** 166 | * Gets the z. 167 | * 168 | * @return the z 169 | */ 170 | public float getZ() { 171 | return this.points[2]; 172 | } 173 | 174 | /** 175 | * Gets the w. 176 | * 177 | * @return the w 178 | */ 179 | public float getW() { 180 | return this.points[3]; 181 | } 182 | 183 | /** 184 | * Sets the x. 185 | * 186 | * @param x the new x 187 | */ 188 | public void setX(float x) { 189 | this.points[0] = x; 190 | } 191 | 192 | /** 193 | * Sets the y. 194 | * 195 | * @param y the new y 196 | */ 197 | public void setY(float y) { 198 | this.points[1] = y; 199 | } 200 | 201 | /** 202 | * Sets the z. 203 | * 204 | * @param z the new z 205 | */ 206 | public void setZ(float z) { 207 | this.points[2] = z; 208 | } 209 | 210 | /** 211 | * Sets the w. 212 | * 213 | * @param w the new w 214 | */ 215 | public void setW(float w) { 216 | this.points[3] = w; 217 | } 218 | 219 | public float x() { 220 | return this.points[0]; 221 | } 222 | 223 | public float y() { 224 | return this.points[1]; 225 | } 226 | 227 | public float z() { 228 | return this.points[2]; 229 | } 230 | 231 | public float w() { 232 | return this.points[3]; 233 | } 234 | 235 | public void x(float x) { 236 | this.points[0] = x; 237 | } 238 | 239 | public void y(float y) { 240 | this.points[1] = y; 241 | } 242 | 243 | public void z(float z) { 244 | this.points[2] = z; 245 | } 246 | 247 | public void w(float w) { 248 | this.points[3] = w; 249 | } 250 | 251 | public void setXYZW(float x, float y, float z, float w) { 252 | this.points[0] = x; 253 | this.points[1] = y; 254 | this.points[2] = z; 255 | this.points[3] = w; 256 | } 257 | 258 | /** 259 | * Compare this vector4f to the supplied one 260 | * 261 | * @param rhs True if they match, false other wise. 262 | * @return 263 | */ 264 | public boolean compareTo(Vector4f rhs) { 265 | boolean ret = false; 266 | if (this.points[0] == rhs.points[0] && this.points[1] == rhs.points[1] && this.points[2] == rhs.points[2] 267 | && this.points[3] == rhs.points[3]) 268 | ret = true; 269 | return ret; 270 | } 271 | 272 | /** 273 | * Copies the data from the supplied vec3 into this vec4 plus the supplied w. 274 | * 275 | * @param input The x y z values to copy in. 276 | * @param w The extra w element to copy in 277 | */ 278 | public void copyFromV3f(Vector3f input, float w) { 279 | points[0] = (input.x()); 280 | points[1] = (input.y()); 281 | points[2] = (input.z()); 282 | points[3] = (w); 283 | } 284 | 285 | @Override 286 | public String toString() { 287 | return "X:" + points[0] + " Y:" + points[1] + " Z:" + points[2] + " W:" + points[3]; 288 | } 289 | 290 | } -------------------------------------------------------------------------------- /app/src/main/res/drawable-hdpi/ic_launcher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/sensor-fusion-demo/d1d23683582c71a3510ec6f256ed0f7d0402e06a/app/src/main/res/drawable-hdpi/ic_launcher.png -------------------------------------------------------------------------------- /app/src/main/res/drawable-mdpi/ic_launcher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/sensor-fusion-demo/d1d23683582c71a3510ec6f256ed0f7d0402e06a/app/src/main/res/drawable-mdpi/ic_launcher.png -------------------------------------------------------------------------------- /app/src/main/res/drawable-xhdpi/ic_launcher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/sensor-fusion-demo/d1d23683582c71a3510ec6f256ed0f7d0402e06a/app/src/main/res/drawable-xhdpi/ic_launcher.png -------------------------------------------------------------------------------- /app/src/main/res/drawable-xxhdpi/ic_launcher.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/sensor-fusion-demo/d1d23683582c71a3510ec6f256ed0f7d0402e06a/app/src/main/res/drawable-xxhdpi/ic_launcher.png -------------------------------------------------------------------------------- /app/src/main/res/layout/activity_about.xml: -------------------------------------------------------------------------------- 1 |