1 /* 2 * Copyright (C) 2008 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 package android.hardware.cts; 18 19 import junit.framework.Assert; 20 21 import android.content.Context; 22 import android.hardware.SensorManager; 23 import android.os.PowerManager; 24 25 import java.util.Random; 26 27 public class SensorManagerStaticTest extends SensorTestCase { 28 private static final String TAG = "SensorManagerTest"; 29 30 // local float version of PI 31 private static final float FLOAT_PI = (float) Math.PI; 32 33 34 private PowerManager.WakeLock mWakeLock; 35 36 @Override setUp()37 protected void setUp() throws Exception { 38 Context context = getContext(); 39 PowerManager pm = (PowerManager) context.getSystemService(Context.POWER_SERVICE); 40 mWakeLock = pm.newWakeLock(PowerManager.PARTIAL_WAKE_LOCK, TAG); 41 42 mWakeLock.acquire(); 43 } 44 45 @Override tearDown()46 protected void tearDown(){ 47 if (mWakeLock != null && mWakeLock.isHeld()) { 48 mWakeLock.release(); 49 } 50 } 51 52 // SensorManager Tests testGetAltitude()53 public void testGetAltitude() throws Exception { 54 float r, q; 55 float altitude; 56 57 // identity property 58 for (r = 0.5f; r < 1.3f; r += 0.1f) { 59 60 altitude = SensorManager.getAltitude(r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE, 61 r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE); 62 assertRoughlyEqual("getAltitude identity property violated.", altitude, 0.0f, 0.1f); 63 } 64 65 // uniform increasing as pressure decreases property 66 float prevAltitude = 1e5f; // 100km ceiling 67 for (r = 0.5f; r < 1.3f; r += 0.01f) { 68 altitude = SensorManager.getAltitude(SensorManager.PRESSURE_STANDARD_ATMOSPHERE, 69 r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE); 70 71 assertTrue("getAltitude result has to decrease as p increase.", prevAltitude > altitude); 72 prevAltitude = altitude; 73 } 74 75 // compare to a reference algorithm 76 final float coef = 1.0f / 5.255f; 77 for (r = 0.8f; r < 1.3f; r += 0.1f) { 78 for (q = 1.1f * r; q > 0.5f * r; q -= 0.1f * r) { 79 float p0 = r * SensorManager.PRESSURE_STANDARD_ATMOSPHERE; 80 float p = q * SensorManager.PRESSURE_STANDARD_ATMOSPHERE; 81 82 float t1 = SensorManager.getAltitude(p0, p); 83 float t2 = 44330.f*(1.0f- (float) Math.pow(p/p0, coef)); 84 85 assertRoughlyEqual( 86 String.format("getAltitude comparing to reference algorithm failed. " + 87 "Detail: getAltitude(%f, %f) => %f, reference => %f", 88 p0, p, t1, t2), 89 t1, t2, 100.f); 90 } 91 } 92 93 } 94 testGetAngleChange()95 public void testGetAngleChange() throws Exception { 96 TestDataGenerator data = new TestDataGenerator(); 97 98 int i; 99 float [] rotv = new float[3]; 100 float [] rotv2 = new float[3]; 101 102 // test many instances 103 for (i=0; i<100; ++i) { 104 float [] R1, R12, R2; 105 // azimuth(yaw) pitch roll 106 data.nextRotationAngles(rotv); 107 R1 = mat9VRot(rotv); // random base 108 109 // azimuth(yaw) pitch roll 110 data.nextRotationAngles(rotv); 111 R12 = mat9VRot(rotv); 112 R2 = mat9Mul(R1, R12); // apply another random rotation 113 114 // test different variations of input matrix format 115 switch(i & 3) { 116 case 0: 117 SensorManager.getAngleChange(rotv2, R2, R1); 118 break; 119 case 1: 120 SensorManager.getAngleChange(rotv2, mat9to16(R2), R1); 121 break; 122 case 2: 123 SensorManager.getAngleChange(rotv2, R2, mat9to16(R1)); 124 break; 125 case 3: 126 SensorManager.getAngleChange(rotv2, mat9to16(R2), mat9to16(R1)); 127 break; 128 } 129 130 // check range 131 assertRotationAnglesValid("getAngleChange result out of range.", rotv2); 132 133 // avoid directly checking the rotation angles to avoid corner cases 134 float [] R12rt = mat9T(mat9VRot(rotv2)); 135 float [] RI = mat9Mul(R12rt, R12); 136 137 assertRoughlyEqual( 138 String.format("getAngleChange result is incorrect. Details: case %d, " + 139 "truth = [%f, %f, %f], result = [%f, %f, %f]", i, rotv[0], rotv[1], rotv[2], 140 rotv2[0], rotv2[1], rotv2[2]), 141 RI[0] + RI[4] + RI[8], 3.f, 1e-4f); 142 } 143 } 144 testGetInclination()145 public void testGetInclination() throws Exception { 146 TestDataGenerator data = new TestDataGenerator(); 147 148 int i; 149 float [] rotv = new float[3]; 150 float [] rotv2 = new float[3]; 151 float [] rotv3; 152 153 // test many instances 154 for (i = 0; i < 100; ++i) { 155 float [] R; 156 float angle; 157 angle = (data.nextFloat()-0.5f) * FLOAT_PI; 158 R = mat9Rot(SensorManager.AXIS_X, -angle); 159 160 float angler = ((i&1) != 0) ? 161 SensorManager.getInclination(mat9to16(R)) : SensorManager.getInclination(R); 162 assertRoughlyEqual( 163 String.format( 164 "getInclination return incorrect result. Detail: case %d, truth %f, result %f.", 165 i, angle, angler), 166 angle, angler, 1e-4f); 167 } 168 } 169 testGetOrientation()170 public void testGetOrientation() throws Exception { 171 TestDataGenerator data = new TestDataGenerator(); 172 173 int i; 174 float [] rotv = new float[3]; 175 float [] rotv2 = new float[3]; 176 float [] rotv3; 177 178 // test many instances 179 for (i=0; i<100; ++i) { 180 float [] R; 181 // yaw pitch roll 182 data.nextRotationAngles(rotv); 183 R = mat9VRot(rotv); 184 185 rotv3 = SensorManager.getOrientation( ((i&1) != 0) ? R : mat9to16(R), rotv2); 186 assertTrue("getOrientaion has to return the array passed in argument", rotv3 == rotv2); 187 188 // check range 189 assertRotationAnglesValid("getOrientation result out of range.", rotv2); 190 191 // Avoid directly comparing rotation angles. Instead, compare the rotation matrix. 192 float [] Rr = mat9T(mat9VRot(rotv2)); 193 float [] RI = mat9Mul(Rr, R); 194 195 assertRoughlyEqual( 196 String.format("getOrientation result is incorrect. Details: case %d, " + 197 "truth = [%f, %f, %f], result = [%f, %f, %f]", i, rotv[0], rotv[1], rotv[2], 198 rotv2[0], rotv2[1], rotv2[2]), 199 RI[0] + RI[4] + RI[8], 3.f, 1e-4f); 200 } 201 } 202 testGetQuaternionFromVector()203 public void testGetQuaternionFromVector() throws Exception { 204 TestDataGenerator data = new TestDataGenerator(); 205 206 int i; 207 float [] v; 208 float [] q = new float[4]; 209 float [] q2 = new float[4]; 210 float [] v3 = new float[3]; 211 float [] v4 = new float[4]; 212 float [] v5 = new float[5]; 213 float [][] vs = new float[][] {v3, v4, v5}; 214 215 float [] xyzth = new float[4]; 216 for (i = 0; i < 100; ++i) { 217 float c, s; 218 219 data.nextRotationAxisAngle(xyzth); 220 221 c = (float) Math.cos(xyzth[3]); 222 s = (float) Math.sin(xyzth[3]); 223 if (c < 0.f) { 224 c = -c; 225 s = -s; 226 } 227 228 v = vs[i%3]; 229 switch(i%3) { 230 case 2: 231 v[4] = data.nextBoolean() ? data.nextFloat() : -1.f; 232 case 1: 233 v[3] = c; 234 case 0: 235 v[0] = s * xyzth[0]; 236 v[1] = s * xyzth[1]; 237 v[2] = s * xyzth[2]; 238 } 239 240 q2[0] = c; 241 q2[1] = v[0]; 242 q2[2] = v[1]; 243 q2[3] = v[2]; 244 245 SensorManager.getQuaternionFromVector(q, v); 246 assertVectorRoughlyEqual( 247 String.format("getQuaternionFromVector returns wrong results, Details: case %d, " + 248 "truth = (%f, %f, %f, %f), result = (%f, %f, %f, %f).", 249 i, q2[0], q2[1], q2[2], q2[3], q[0], q[1], q[2], q[3]), 250 q, q2, 1e-4f); 251 } 252 } 253 testGetRotationMatrix()254 public void testGetRotationMatrix() throws Exception { 255 TestDataGenerator data = new TestDataGenerator(); 256 final float gravity = 9.81f; 257 final float magStrength = 50.f; 258 259 int i; 260 float [] gm = new float[9]; 261 float [] rotv = new float[3]; 262 float [] gI = null; 263 float [] mI = null; 264 float [] Rr = new float[9]; 265 float [] Ir = new float[9]; 266 267 gm[6] = gravity; // m/s^2, first column gravity 268 269 // test many instances 270 for (i=0; i<100; ++i) { 271 float [] Rt; 272 float incline; 273 // yaw pitch roll 274 data.nextRotationAngles(rotv); 275 Rt = mat9T(mat9VRot(rotv)); // from world frame to phone frame 276 //Rt = mat9I(); 277 278 incline = -0.9f * (data.nextFloat() - 0.5f) * FLOAT_PI; // ~ +-80 degrees 279 //incline = 0.f; 280 gm[4] = magStrength * (float) Math.cos(-incline); // positive means rotate downwards 281 gm[7] = magStrength * (float) Math.sin(-incline); 282 283 float [] gmb = mat9Mul(Rt, gm); // do not care about right most column 284 gI = mat9Axis(gmb, SensorManager.AXIS_X); 285 mI = mat9Axis(gmb, SensorManager.AXIS_Y); 286 287 assertTrue("getRotationMatrix returns false on valid inputs", 288 SensorManager.getRotationMatrix(Rr, Ir, gI, mI)); 289 290 float [] n = mat9Mul(Rr, Rt); 291 assertRoughlyEqual( 292 String.format("getRotationMatrix returns incorrect R matrix. " + 293 "Details: case %d, truth R = %s, result R = %s.", 294 i, mat9ToStr(mat9T(Rt)), mat9ToStr(Rr)), 295 n[0] + n[4] + n[8], 3.f, 1e-4f); 296 297 298 // Magnetic incline is defined so that it means the magnetic field lines is formed 299 // by rotate local y axis around -x axis by incline angle. However, I matrix is 300 // defined as (according to document): 301 // [0 m 0] = I * R * geomagnetic, 302 // which means, 303 // I' * [0 m 0] = R * geomagnetic. 304 // Thus, I' = Rot(-x, incline) and I = Rot(-x, incline)' = Rot(x, incline) 305 float [] Ix = mat9Rot(SensorManager.AXIS_X, incline); 306 assertVectorRoughlyEqual( 307 String.format("getRotationMatrix returns incorrect I matrix. " + 308 "Details: case %d, truth I = %s, result I = %s.", 309 i, mat9ToStr(Ix), mat9ToStr(Ir)), 310 Ix, Ir, 1e-4f); 311 } 312 313 // test 16 element inputs 314 float [] Rr2 = new float[16]; 315 float [] Ir2 = new float[16]; 316 317 assertTrue("getRotationMatrix returns false on valid inputs", 318 SensorManager.getRotationMatrix(Rr2, Ir2, gI, mI)); 319 320 assertVectorRoughlyEqual( 321 "getRotationMatrix acts inconsistent with 9- and 16- elements matrix buffer", 322 mat16to9(Rr2), Rr, 1e-4f); 323 324 assertVectorRoughlyEqual( 325 "getRotationMatrix acts inconsistent with 9- and 16- elements matrix buffer", 326 mat16to9(Ir2), Ir, 1e-4f); 327 328 // test null inputs 329 assertTrue("getRotationMatrix does not handle null inputs", 330 SensorManager.getRotationMatrix(Rr, null, gI, mI)); 331 332 assertTrue("getRotationMatrix does not handle null inputs", 333 SensorManager.getRotationMatrix(null, Ir, gI, mI)); 334 335 assertTrue("getRotationMatrix does not handle null inputs", 336 SensorManager.getRotationMatrix(null, null, gI, mI)); 337 338 // test fail cases 339 // free fall, if the acc reading is less than 10% of gravity 340 gI[0] = gI[1] = gI[2] = data.nextFloat() * gravity * 0.05f; // sqrt(3) * 0.05 < 0.1 341 assertFalse("getRotationMatrix does not fail when it supposed to fail (gravity too small)", 342 SensorManager.getRotationMatrix(Rr, Ir, gI, mI)); 343 344 // wrong input 345 assertFalse("getRotationMatrix does not fail when it supposed to fail (singular axis)", 346 SensorManager.getRotationMatrix(Rr, Ir, gI, gI)); 347 } 348 testGetRotationMatrixFromVector()349 public void testGetRotationMatrixFromVector() throws Exception { 350 TestDataGenerator data = new TestDataGenerator(); 351 352 int i; 353 float [] v; 354 float [] q = new float[4]; 355 356 float [] v3 = new float[3]; 357 float [] v4 = new float[4]; 358 float [] v5 = new float[5]; 359 float [][] vs = new float[][]{v3, v4, v5}; 360 361 float [] m9 = new float[9]; 362 float [] m16 = new float[16]; 363 364 // format: x y z theta/2 365 float [] xyzth = new float[4]; 366 // test the orthogonal property of returned matrix 367 for (i=0; i<20; ++i) { 368 float c, s; 369 data.nextRotationAxisAngle(xyzth); 370 371 c = (float) Math.cos(xyzth[3]); 372 s = (float) Math.sin(xyzth[3]); 373 if (c < 0.f) { 374 c = -c; 375 s = -s; 376 } 377 378 v = vs[i%3]; 379 switch(i%3) { 380 case 2: 381 v[4] = data.nextBoolean() ? data.nextFloat() : -1.f; 382 case 1: 383 v[3] = c; 384 case 0: 385 v[0] = s * xyzth[0]; 386 v[1] = s * xyzth[1]; 387 v[2] = s * xyzth[2]; 388 } 389 390 if ((i % 1) != 0) { 391 SensorManager.getRotationMatrixFromVector(m16, v); 392 m9 = mat16to9(m16); 393 }else { 394 SensorManager.getRotationMatrixFromVector(m9, v); 395 } 396 397 float [] n = mat9Mul(m9, mat9T(m9)); 398 assertRoughlyEqual("getRotationMatrixFromVector do not return proper matrix", 399 n[0]+ n[4] + n[8], 3.f, 1e-4f); 400 } 401 402 // test if multiple rotation (total 2pi) about an axis result in identity 403 v = v3; 404 float [] Rr = new float[9]; 405 406 for (i=0; i<20; ++i) { 407 float j, halfTheta, residualHalfTheta = FLOAT_PI; 408 float [] R = mat9I(); 409 float c, s; 410 411 data.nextRotationAxisAngle(xyzth); // half theta is ignored 412 413 j = data.nextInt(5) + 2; // 2 ~ 6 rotations 414 415 while(j-- > 0) { 416 if (j == 0) { 417 halfTheta = residualHalfTheta; 418 } else { 419 halfTheta = data.nextFloat() * FLOAT_PI; 420 } 421 422 c = (float) Math.cos(halfTheta); 423 s = (float) Math.sin(halfTheta); 424 if (c < 0.f) { 425 c = -c; 426 s = -s; 427 } 428 429 v[0] = s * xyzth[0]; 430 v[1] = s * xyzth[1]; 431 v[2] = s * xyzth[2]; 432 433 SensorManager.getRotationMatrixFromVector(Rr, v); 434 R = mat9Mul(Rr, R); 435 436 residualHalfTheta -= halfTheta; 437 } 438 439 assertRoughlyEqual("getRotationMatrixFromVector returns incorrect matrix", 440 R[0] + R[4] + R[8], 3.f, 1e-4f); 441 } 442 443 // test if rotation about trival axis works 444 v = v3; 445 for (i=0; i<20; ++i) { 446 int axis = (i % 3) + 1; 447 float theta = data.nextFloat() * 2.f * FLOAT_PI; 448 float [] R; 449 450 v[0] = v[1] = v[2] = 0.f; 451 v[axis - 1] = (float) Math.sin(theta / 2.f); 452 if ( (float) Math.cos(theta / 2.f) < 0.f) { 453 v[axis-1] = -v[axis-1]; 454 } 455 456 SensorManager.getRotationMatrixFromVector(m9, v); 457 R = mat9Rot(axis, theta); 458 459 assertVectorRoughlyEqual( 460 String.format("getRotationMatrixFromVector returns incorrect matrix with "+ 461 "simple rotation. Details: case %d, truth R = %s, result R = %s.", 462 i, mat9ToStr(R), mat9ToStr(m9)), 463 R, m9, 1e-4f); 464 } 465 } 466 testRemapCoordinateSystem()467 public void testRemapCoordinateSystem() throws Exception { 468 TestDataGenerator data = new TestDataGenerator(); 469 470 int i, j, k; 471 float [] rotv = new float[3]; 472 float [] Rout = new float[9]; 473 float [] Rout2 = new float[16]; 474 int a1, a2; // AXIS_X/Y/Z 475 int b1, b2, b3; // AXIS_X/Y/Z w/ or w/o MINUS 476 477 // test a few instances 478 for (i=0; i<10; ++i) { 479 float [] R; 480 // yaw pitch roll 481 data.nextRotationAngles(rotv); 482 R = mat9VRot(rotv); 483 484 // total of 6*4 = 24 variations 485 // 6 = A(3,2) 486 for (j=0; j<9; ++j) { 487 // axis without minus 488 a1 = j/3 + 1; 489 a2 = j%3 + 1; 490 491 // skip cases when two axis are the same 492 if (a1 == a2) continue; 493 494 for (k=0; k<3; ++k) { 495 // test all minus axis combination: ++, +-, -+, -- 496 b1 = a1 | (((k & 2) != 0) ? 0x80 : 0); 497 b2 = a2 | (((k & 1) != 0) ? 0x80 : 0); 498 // the third axis 499 b3 = (6 - a1 -a2) | 500 ( (((a2 + 3 - a1) % 3 == 2) ? 0x80 : 0) ^ (b1 & 0x80) ^ (b2 & 0x80)); 501 502 // test both input formats 503 if ( (i & 1) != 0 ) { 504 assertTrue(SensorManager.remapCoordinateSystem(R, b1, b2, Rout)); 505 } else { 506 assertTrue(SensorManager.remapCoordinateSystem(mat9to16(R), b1, b2, Rout2)); 507 Rout = mat16to9(Rout2); 508 } 509 510 float [] v1, v2; 511 512 String detail = String.format( 513 "Details: case %d (%x %x %x), original R = %s, result R = %s.", 514 i, b1, b2, b3, mat9ToStr(R), mat9ToStr(Rout)); 515 516 v1 = mat9Axis(R, SensorManager.AXIS_X); 517 v2 = mat9Axis(Rout, b1); 518 assertVectorRoughlyEqual( 519 "remapCoordinateSystem gives incorrect result (x)." + detail, 520 v1, v2, 1e-4f); 521 522 v1 = mat9Axis(R, SensorManager.AXIS_Y); 523 v2 = mat9Axis(Rout, b2); 524 assertVectorRoughlyEqual( 525 "remapCoordinateSystem gives incorrect result (y)." + detail, 526 v1, v2, 1e-4f); 527 528 v1 = mat9Axis(R, SensorManager.AXIS_Z); 529 v2 = mat9Axis(Rout, b3); 530 assertVectorRoughlyEqual( 531 "remapCoordinateSystem gives incorrect result (z)." + detail, 532 v1, v2, 1e-4f); 533 } 534 } 535 536 } 537 538 // test cases when false should be returned 539 assertTrue("remapCoordinateSystem should return false with mismatch size input and output", 540 !SensorManager.remapCoordinateSystem(Rout, 541 SensorManager.AXIS_Y, SensorManager.AXIS_Z, Rout2)); 542 assertTrue("remapCoordinateSystem should return false with invalid axis setting", 543 !SensorManager.remapCoordinateSystem(Rout, 544 SensorManager.AXIS_X, SensorManager.AXIS_X, Rout)); 545 assertTrue("remapCoordinateSystem should return false with invalid axis setting", 546 !SensorManager.remapCoordinateSystem(Rout, 547 SensorManager.AXIS_X, SensorManager.AXIS_MINUS_X, Rout)); 548 549 } 550 551 // Utilities class & functions 552 553 private class TestDataGenerator { 554 // carry out test deterministically without manually picking numbers 555 private final long DEFAULT_SEED = 0xFEDCBA9876543210l; 556 557 private Random mRandom; 558 TestDataGenerator(long seed)559 TestDataGenerator(long seed) { 560 mRandom = new Random(seed); 561 } 562 TestDataGenerator()563 TestDataGenerator() { 564 mRandom = new Random(DEFAULT_SEED); 565 } 566 nextRotationAngles(float [] rotv)567 void nextRotationAngles(float [] rotv) { 568 assertTrue(rotv.length == 3); 569 570 rotv[0] = (mRandom.nextFloat()-0.5f) * 2.0f * FLOAT_PI; // azimuth(yaw) -pi ~ pi 571 rotv[1] = (mRandom.nextFloat()-0.5f) * FLOAT_PI; // pitch -pi/2 ~ +pi/2 572 rotv[2] = (mRandom.nextFloat()-0.5f) * 2.f * FLOAT_PI; // roll -pi ~ +pi 573 } 574 nextRotationAxisAngle(float [] aa)575 void nextRotationAxisAngle(float [] aa) { 576 assertTrue(aa.length == 4); 577 578 aa[0] = (mRandom.nextFloat() - 0.5f) * 2.f; 579 aa[1] = (mRandom.nextFloat() - 0.5f ) * 2.f * (float) Math.sqrt(1.f - aa[0] * aa[0]); 580 aa[2] = (mRandom.nextBoolean() ? 1.f : -1.f) * 581 (float) Math.sqrt(1.f - aa[0] * aa[0] - aa[1] * aa[1]); 582 aa[3] = mRandom.nextFloat() * FLOAT_PI; 583 } 584 nextInt(int i)585 int nextInt(int i) { 586 return mRandom.nextInt(i); 587 } 588 nextFloat()589 float nextFloat() { 590 return mRandom.nextFloat(); 591 } 592 nextBoolean()593 boolean nextBoolean() { 594 return mRandom.nextBoolean(); 595 } 596 } 597 assertRotationAnglesValid(String message, float[] ra)598 private static void assertRotationAnglesValid(String message, float[] ra) { 599 600 assertTrue(message, ra.length == 3 && 601 ra[0] >= -FLOAT_PI && ra[0] <= FLOAT_PI && // azimuth 602 ra[1] >= -FLOAT_PI / 2.f && ra[1] <= FLOAT_PI / 2.f && // pitch 603 ra[2] >= -FLOAT_PI && ra[2] <= FLOAT_PI); // roll 604 } 605 assertRoughlyEqual(String message, float a, float b, float bound)606 private static void assertRoughlyEqual(String message, float a, float b, float bound) { 607 assertTrue(message, Math.abs(a-b) < bound); 608 } 609 assertVectorRoughlyEqual(String message, float [] v1, float [] v2, float bound)610 private static void assertVectorRoughlyEqual(String message, float [] v1, float [] v2, 611 float bound) { 612 assertTrue(message, v1.length == v2.length); 613 int i; 614 float sum = 0.f; 615 for (i=0; i<v1.length; ++i) { 616 sum += (v1[i] - v2[i]) * (v1[i] - v2[i]); 617 } 618 assertRoughlyEqual(message, (float)Math.sqrt(sum), 0.f, bound); 619 } 620 mat9to16(float [] m)621 private static float [] mat9to16(float [] m) { 622 assertTrue(m.length == 9); 623 624 float [] n = new float[16]; 625 int i; 626 for (i=0; i<9; ++i) { 627 n[i+i/3] = m[i]; 628 } 629 n[15] = 1.f; 630 return n; 631 } 632 mat16to9(float [] m)633 private static float [] mat16to9(float [] m) { 634 assertTrue(m.length == 16); 635 636 float [] n = new float[9]; 637 int i; 638 for (i=0; i<9; ++i) { 639 n[i] = m[i + i/3]; 640 } 641 return n; 642 } 643 mat9Mul(float [] m, float [] n)644 private static float [] mat9Mul(float [] m, float [] n) { 645 assertTrue(m.length == 9 && n.length == 9); 646 647 float [] r = new float[9]; 648 int i, j, k; 649 650 for (i = 0; i < 3; ++i) 651 for (j = 0; j < 3; ++j) 652 for (k = 0; k < 3; ++k) 653 r[i * 3 + j] += m[i * 3 + k] * n[k * 3 + j]; 654 655 return r; 656 } 657 mat9T(float [] m)658 private static float [] mat9T(float [] m) { 659 assertTrue(m.length == 9); 660 661 int i, j; 662 float [] n = new float[9]; 663 664 for (i = 0; i < 3; ++i) 665 for (j = 0; j < 3; ++j) 666 n[i * 3 + j] = m[j * 3 + i]; 667 668 return n; 669 } 670 mat9I()671 private static float [] mat9I() { 672 float [] m = new float[9]; 673 m[0] = m[4] = m[8] = 1.f; 674 return m; 675 } 676 mat9Rot(int axis, float angle)677 private static float [] mat9Rot(int axis, float angle) { 678 float [] m = new float[9]; 679 switch (axis) { 680 case SensorManager.AXIS_X: 681 m[0] = 1.f; 682 m[4] = m[8] = (float) Math.cos(angle); 683 m[5] = - (m[7] = (float) Math.sin(angle)); 684 break; 685 case SensorManager.AXIS_Y: 686 m[4] = 1.f; 687 m[0] = m[8] = (float) Math.cos(angle); 688 m[6] = - (m[2] = (float) Math.sin(angle)); 689 break; 690 case SensorManager.AXIS_Z: 691 m[8] = 1.f; 692 m[0] = m[4] = (float) Math.cos(angle); 693 m[1] = - (m[3] = (float) Math.sin(angle)); 694 break; 695 default: 696 // should never be here 697 assertTrue(false); 698 } 699 return m; 700 } 701 mat9VRot(float [] angles)702 private static float [] mat9VRot(float [] angles) { 703 assertTrue(angles.length == 3); 704 // yaw, android yaw rotate to -z 705 float [] R = mat9Rot(SensorManager.AXIS_Z, -angles[0]); 706 // pitch, android pitch rotate to -x 707 R = mat9Mul(R, mat9Rot(SensorManager.AXIS_X, -angles[1])); 708 // roll 709 R = mat9Mul(R, mat9Rot(SensorManager.AXIS_Y, angles[2])); 710 711 return R; 712 } 713 mat9Axis(float m[], int axis)714 private static float [] mat9Axis(float m[], int axis) { 715 assertTrue(m.length == 9); 716 717 boolean negative = (axis & 0x80) != 0; 718 float [] v = new float[3]; 719 int offset; 720 721 offset = (axis & ~0x80) - 1; 722 v[0] = negative ? -m[offset] : m[offset]; 723 v[1] = negative ? -m[offset+3] : m[offset+3]; 724 v[2] = negative ? -m[offset+6] : m[offset+6]; 725 return v; 726 } 727 vecInner(float u[], float v[])728 private static float vecInner(float u[], float v[]) { 729 assertTrue(u.length == v.length); 730 731 int i; 732 float sum = 0.f; 733 734 for (i=0; i < v.length; ++i) { 735 sum += u[i]*v[i]; 736 } 737 return (float)Math.sqrt(sum); 738 } 739 vecToStr(float u[])740 private static String vecToStr(float u[]) { 741 int i; 742 String s; 743 switch (u.length) { 744 case 3: 745 return String.format("[%f, %f, %f]", u[0], u[1], u[2]); 746 case 4: 747 return String.format("(%f, %f, %f, %f)", u[0], u[1], u[2], u[3]); 748 default: 749 s = "["; 750 for (i = 0; i < u.length-1; ++i) { 751 s += String.format("%f, ", u[i]); 752 } 753 s += String.format("%f]", u[i]); 754 return s; 755 } 756 } 757 mat9ToStr(float m[])758 private static String mat9ToStr(float m[]) { 759 assertTrue(m.length == 9); 760 return String.format("[%f, %f, %f; %f, %f, %f; %f, %f, %f]", 761 m[0], m[1], m[2], 762 m[3], m[4], m[5], 763 m[6], m[7], m[8]); 764 } 765 mat16ToStr(float m[])766 private static String mat16ToStr(float m[]) { 767 assertTrue(m.length == 16); 768 return String.format("[%f, %f, %f, %f; %f, %f, %f, %f; %f, %f, %f, %f; %f, %f, %f, %f]", 769 m[0], m[1], m[2], m[3], 770 m[4], m[5], m[6], m[7], 771 m[8], m[9], m[10], m[11], 772 m[12], m[13], m[14], m[15]); 773 } 774 775 } 776 777