1/*---------------------------------------------------------------------------* 2 Project: Horizon 3 File: math_Matrix34.ipp 4 5 Copyright (C)2009-2010 Nintendo Co., Ltd. All rights reserved. 6 7 These coded instructions, statements, and computer programs contain 8 proprietary information of Nintendo of America Inc. and/or Nintendo 9 Company Ltd., and are protected by Federal copyright law. They may 10 not be disclosed to third parties or copied or duplicated in any form, 11 in whole or in part, without the prior written consent of Nintendo. 12 13 $Revision: 13664 $ 14 *---------------------------------------------------------------------------*/ 15#include <cmath> 16#include <nn/math/math_Vector3.h> 17 18namespace nn { 19namespace math { 20namespace ARMv6 { 21/* Please see man pages for details 22 23 24*/ 25 26/* 27 28 29 30 31 32 33 34 35*/ 36NN_MATH_INLINE VEC3* 37VEC3TransformC(VEC3* pOut, const MTX34* __restrict pM, const VEC3* __restrict pV) 38{ 39 NN_NULL_ASSERT( pM ); 40 NN_NULL_ASSERT( pV ); 41 NN_NULL_ASSERT( pOut ); 42 43 VEC3 vTmp; 44 VEC3* pDst = (pOut == pV) ? &vTmp : pOut; 45 46 pDst->x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z + pM->f._03; 47 pDst->y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z + pM->f._13; 48 pDst->z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z + pM->f._23; 49 50 if (pDst == &vTmp) 51 { 52 pOut->x = pDst->x; 53 pOut->y = pDst->y; 54 pOut->z = pDst->z; 55 } 56 57 return pOut; 58} 59 60/* ------------------------------------------------------------------------ 61 MTX34 62 ------------------------------------------------------------------------ */ 63 64/* Please see man pages for details 65 66 67*/ 68 69/* 70 71 72 73 74 75 76*/ 77NN_MATH_INLINE MTX34* 78MTX34CopyC(MTX34* pOut, const MTX34* p) 79{ 80 NN_NULL_ASSERT( pOut ); 81 NN_NULL_ASSERT( p ); 82 83 if ( p != pOut ) 84 { 85 *pOut = *p; 86 } 87 88 return pOut; 89} 90 91 92 93/* 94 95 96 97 98 99 100 101 102*/ 103NN_MATH_INLINE MTX34* 104MTX34MultC(MTX34* pOut, const MTX34* __restrict p1, const MTX34* __restrict p2) 105{ 106 MTX34 mTmp; 107 108 NN_NULL_ASSERT( pOut ); 109 NN_NULL_ASSERT( p1 ); 110 NN_NULL_ASSERT( p2 ); 111 112 MTX34* __restrict pDst = (pOut == p1 || pOut == p2) ? &mTmp : pOut; 113 114 pDst->f._00 = p1->f._00 * p2->f._00 + p1->f._01 * p2->f._10 + p1->f._02 * p2->f._20; 115 pDst->f._01 = p1->f._00 * p2->f._01 + p1->f._01 * p2->f._11 + p1->f._02 * p2->f._21; 116 pDst->f._02 = p1->f._00 * p2->f._02 + p1->f._01 * p2->f._12 + p1->f._02 * p2->f._22; 117 pDst->f._03 = p1->f._00 * p2->f._03 + p1->f._01 * p2->f._13 + p1->f._02 * p2->f._23 + p1->f._03; 118 119 pDst->f._10 = p1->f._10 * p2->f._00 + p1->f._11 * p2->f._10 + p1->f._12 * p2->f._20; 120 pDst->f._11 = p1->f._10 * p2->f._01 + p1->f._11 * p2->f._11 + p1->f._12 * p2->f._21; 121 pDst->f._12 = p1->f._10 * p2->f._02 + p1->f._11 * p2->f._12 + p1->f._12 * p2->f._22; 122 pDst->f._13 = p1->f._10 * p2->f._03 + p1->f._11 * p2->f._13 + p1->f._12 * p2->f._23 + p1->f._13; 123 124 pDst->f._20 = p1->f._20 * p2->f._00 + p1->f._21 * p2->f._10 + p1->f._22 * p2->f._20; 125 pDst->f._21 = p1->f._20 * p2->f._01 + p1->f._21 * p2->f._11 + p1->f._22 * p2->f._21; 126 pDst->f._22 = p1->f._20 * p2->f._02 + p1->f._21 * p2->f._12 + p1->f._22 * p2->f._22; 127 pDst->f._23 = p1->f._20 * p2->f._03 + p1->f._21 * p2->f._13 + p1->f._22 * p2->f._23 + p1->f._23; 128 129 // overwrite a or b if needed 130 if ( pDst == &mTmp ) 131 { 132 MTX34Copy( pOut, &mTmp ); 133 } 134 135 return pOut; 136} 137 138/* 139 140 141 142 143 144 145 146*/ 147NN_MATH_INLINE MTX34* 148MTX34AddC(MTX34* pOut, const MTX34* p1, const MTX34* p2) 149{ 150 pOut->f._00 = p1->f._00 + p2->f._00; 151 pOut->f._01 = p1->f._01 + p2->f._01; 152 pOut->f._02 = p1->f._02 + p2->f._02; 153 pOut->f._03 = p1->f._03 + p2->f._03; 154 155 pOut->f._10 = p1->f._10 + p2->f._10; 156 pOut->f._11 = p1->f._11 + p2->f._11; 157 pOut->f._12 = p1->f._12 + p2->f._12; 158 pOut->f._13 = p1->f._13 + p2->f._13; 159 160 pOut->f._20 = p1->f._20 + p2->f._20; 161 pOut->f._21 = p1->f._21 + p2->f._21; 162 pOut->f._22 = p1->f._22 + p2->f._22; 163 pOut->f._23 = p1->f._23 + p2->f._23; 164 165 return pOut; 166} 167 168/* 169 170 171 172 173 174 175 176*/ 177NN_MATH_INLINE MTX34* 178MTX34MultC(MTX34* pOut, const MTX34* p, f32 f) 179{ 180 pOut->f._00 = p->f._00 * f; 181 pOut->f._01 = p->f._01 * f; 182 pOut->f._02 = p->f._02 * f; 183 pOut->f._03 = p->f._03 * f; 184 185 pOut->f._10 = p->f._10 * f; 186 pOut->f._11 = p->f._11 * f; 187 pOut->f._12 = p->f._12 * f; 188 pOut->f._13 = p->f._13 * f; 189 190 pOut->f._20 = p->f._20 * f; 191 pOut->f._21 = p->f._21 * f; 192 pOut->f._22 = p->f._22 * f; 193 pOut->f._23 = p->f._23 * f; 194 195 return pOut; 196} 197 198 199/* 200 201 202 203 204 205 206*/ 207NN_MATH_INLINE MTX34* 208MTX34ScaleC(MTX34* pOut, const VEC3* pS) 209{ 210 NN_NULL_ASSERT( pOut ); 211 NN_NULL_ASSERT( pS ); 212 213 f32 (*const m)[4] = pOut->m; 214 215 m[0][0] = pS->x; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f; 216 m[1][0] = 0.0f; m[1][1] = pS->y; m[1][2] = 0.0f; m[1][3] = 0.0f; 217 m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = pS->z; m[2][3] = 0.0f; 218 219 return pOut; 220} 221NN_MATH_INLINE MTX34* 222MTX34ScaleC_FAST(MTX34* pOut, const VEC3* pS) 223{ 224 NN_NULL_ASSERT( pOut ); 225 NN_NULL_ASSERT( pS ); 226 227 f32 f0 = 0.0f; 228 const unsigned int f32_0 = *(reinterpret_cast<unsigned int*>(&f0)); 229 unsigned int *m = reinterpret_cast<unsigned int *>(pOut->m); 230 const unsigned int *p = reinterpret_cast<const unsigned int*>(pS); 231 232 m[ 0] = p[0]; m[ 1] = f32_0; m[ 2] = f32_0; m[ 3] = f32_0; 233 m[ 4] = f32_0; m[ 5] = p[1]; m[ 6] = f32_0; m[ 7] = f32_0; 234 m[ 8] = f32_0; m[ 9] = f32_0; m[10] = p[2]; m[11] = f32_0; 235 236 return pOut; 237} 238 239/* 240 241 242 243 244 245 246 247*/ 248NN_MATH_INLINE MTX34* 249MTX34MultScaleC(MTX34* pOut, const MTX34* pM, const VEC3* pS) 250{ 251 // Version where the scale matrix is applied from the right. 252 pOut->f._00 = pM->f._00 * pS->x; 253 pOut->f._10 = pM->f._10 * pS->x; 254 pOut->f._20 = pM->f._20 * pS->x; 255 256 pOut->f._01 = pM->f._01 * pS->y; 257 pOut->f._11 = pM->f._11 * pS->y; 258 pOut->f._21 = pM->f._21 * pS->y; 259 260 pOut->f._02 = pM->f._02 * pS->z; 261 pOut->f._12 = pM->f._12 * pS->z; 262 pOut->f._22 = pM->f._22 * pS->z; 263 264 if (pOut != pM) 265 { 266 pOut->f._03 = pM->f._03; 267 pOut->f._13 = pM->f._13; 268 pOut->f._23 = pM->f._23; 269 } 270 271 return pOut; 272} 273 274 275/* 276 277 278 279 280 281 282 283*/ 284NN_MATH_INLINE MTX34* 285MTX34MultScaleC(MTX34* pOut, const VEC3* __restrict pS, const MTX34* pM) 286{ 287 NN_NULL_ASSERT( pOut ); 288 NN_NULL_ASSERT( pS ); 289 NN_NULL_ASSERT( pM ); 290 291 const f32 (*const src)[4] = pM->m; 292 f32 (*const dst)[4] = pOut->m; 293 294 dst[0][0] = src[0][0] * pS->x; dst[0][1] = src[0][1] * pS->x; 295 dst[0][2] = src[0][2] * pS->x; dst[0][3] = src[0][3] * pS->x; 296 297 dst[1][0] = src[1][0] * pS->y; dst[1][1] = src[1][1] * pS->y; 298 dst[1][2] = src[1][2] * pS->y; dst[1][3] = src[1][3] * pS->y; 299 300 dst[2][0] = src[2][0] * pS->z; dst[2][1] = src[2][1] * pS->z; 301 dst[2][2] = src[2][2] * pS->z; dst[2][3] = src[2][3] * pS->z; 302 303 return pOut; 304} 305 306 307/* 308 309 310 311 312 313 314*/ 315NN_MATH_INLINE MTX34* 316MTX34TranslateC(MTX34* pOut, const VEC3* pT) 317{ 318 NN_NULL_ASSERT( pOut ); 319 NN_NULL_ASSERT( pT ); 320 321 f32 (*const m)[4] = pOut->m; 322 323 m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = pT->x; 324 m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = pT->y; 325 m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = pT->z; 326 327 return pOut; 328} 329NN_MATH_INLINE MTX34* 330MTX34TranslateC_FAST(MTX34* pOut, const VEC3* pT) 331{ 332 NN_NULL_ASSERT( pOut ); 333 NN_NULL_ASSERT( pT ); 334 335 unsigned int *m = reinterpret_cast<unsigned int *>(pOut->m); 336 337 f32 f1 = 1.0f; 338 f32 f0 = 0.0f; 339 const unsigned int f32_1 = *(reinterpret_cast<unsigned int*>(&f1)); 340 const unsigned int f32_0 = *(reinterpret_cast<unsigned int*>(&f0)); 341 const unsigned int *p = reinterpret_cast<const unsigned int*>(pT); 342 343 m[ 0] = f32_1; m[ 1] = f32_0; m[ 2] = f32_0; m[ 3] = p[0]; 344 m[ 4] = f32_0; m[ 5] = f32_1; m[ 6] = f32_0; m[ 7] = p[1]; 345 m[ 8] = f32_0; m[ 9] = f32_0; m[10] = f32_1; m[11] = p[2]; 346 347 return pOut; 348} 349 350/* 351 352 353 354 355 356 357 358*/ 359NN_MATH_INLINE MTX34* 360MTX34MultTranslateC(MTX34* pOut, const VEC3* pT, const MTX34* pM) 361{ 362 NN_NULL_ASSERT(pOut); 363 NN_NULL_ASSERT(pT); 364 NN_NULL_ASSERT(pM); 365 366 const f32 (*const src)[4] = pM->m; 367 f32 (*const dst)[4] = pOut->m; 368 369 if ( src != dst ) 370 { 371 dst[0][0] = src[0][0]; dst[0][1] = src[0][1]; dst[0][2] = src[0][2]; 372 dst[1][0] = src[1][0]; dst[1][1] = src[1][1]; dst[1][2] = src[1][2]; 373 dst[2][0] = src[2][0]; dst[2][1] = src[2][1]; dst[2][2] = src[2][2]; 374 } 375 376 dst[0][3] = src[0][3] + pT->x; 377 dst[1][3] = src[1][3] + pT->y; 378 dst[2][3] = src[2][3] + pT->z; 379 380 return pOut; 381} 382 383/* 384 385 386 387 388 389 390 391*/ 392NN_MATH_INLINE MTX34* 393MTX34MultTranslateC(MTX34* pOut, const MTX34* pM, const VEC3* pT) 394{ 395 // pOut = pM * pT 396 if (pOut != pM) 397 { 398 (void)MTX34Copy(pOut, pM); 399 } 400 401 VEC3 tmp; 402 VEC3Transform(&tmp, pM, pT); 403 404 pOut->f._03 = tmp.x; 405 pOut->f._13 = tmp.y; 406 pOut->f._23 = tmp.z; 407 408 return pOut; 409} 410 411 412 413/* 414 415 416 417 418 419 420 421 422*/ 423NN_MATH_INLINE MTX34* 424MTX34MAddC(MTX34* pOut, f32 t, const MTX34* p1, const MTX34* p2) 425{ 426 pOut->f._00 = t * p1->f._00 + p2->f._00; 427 pOut->f._01 = t * p1->f._01 + p2->f._01; 428 pOut->f._02 = t * p1->f._02 + p2->f._02; 429 pOut->f._03 = t * p1->f._03 + p2->f._03; 430 431 pOut->f._10 = t * p1->f._10 + p2->f._10; 432 pOut->f._11 = t * p1->f._11 + p2->f._11; 433 pOut->f._12 = t * p1->f._12 + p2->f._12; 434 pOut->f._13 = t * p1->f._13 + p2->f._13; 435 436 pOut->f._20 = t * p1->f._20 + p2->f._20; 437 pOut->f._21 = t * p1->f._21 + p2->f._21; 438 pOut->f._22 = t * p1->f._22 + p2->f._22; 439 pOut->f._23 = t * p1->f._23 + p2->f._23; 440 441 return pOut; 442} 443 444 445/* 446 447 448 449 450 451 452 453*/ 454NN_MATH_INLINE MTX34* 455MTX34RotAxisRad_C( MTX34* pOut, const VEC3 *pAxis, f32 fRad ) 456{ 457 VEC3 vN; 458 f32 s, c; // sinTheta, cosTheta 459 f32 t; // ( 1 - cosTheta ) 460 f32 x, y, z; // x, y, z components of normalized axis 461 f32 xSq, ySq, zSq; // x, y, z squared 462 463 464 NN_NULL_ASSERT( pOut ); 465 NN_NULL_ASSERT( pAxis ); 466 467 f32 (*const m)[4] = pOut->m; 468 469 s = ::std::sinf(fRad); 470 c = ::std::cosf(fRad); 471 t = 1.0f - c; 472 473 VEC3Normalize( &vN, pAxis ); 474 475 x = vN.x; 476 y = vN.y; 477 z = vN.z; 478 479 xSq = x * x; 480 ySq = y * y; 481 zSq = z * z; 482 483 m[0][0] = ( t * xSq ) + ( c ); 484 m[0][1] = ( t * x * y ) - ( s * z ); 485 m[0][2] = ( t * x * z ) + ( s * y ); 486 m[0][3] = 0.0f; 487 488 m[1][0] = ( t * x * y ) + ( s * z ); 489 m[1][1] = ( t * ySq ) + ( c ); 490 m[1][2] = ( t * y * z ) - ( s * x ); 491 m[1][3] = 0.0f; 492 493 m[2][0] = ( t * x * z ) - ( s * y ); 494 m[2][1] = ( t * y * z ) + ( s * x ); 495 m[2][2] = ( t * zSq ) + ( c ); 496 m[2][3] = 0.0f; 497 498 return pOut; 499} 500NN_MATH_INLINE MTX34* 501MTX34RotAxisRad_C_FAST( MTX34* pOut, const VEC3 *pAxis, f32 fRad ) 502{ 503 VEC3 vN; 504 f32 s, c; // sinTheta, cosTheta 505 f32 t; // ( 1 - cosTheta ) 506 f32 x, y, z; // x, y, z components of normalized axis 507 f32 xSq, ySq, zSq; // x, y, z squared 508 509 510 NN_NULL_ASSERT( pOut ); 511 NN_NULL_ASSERT( pAxis ); 512 513 f32 (*const m)[4] = pOut->m; 514 515#if (MTX34ROTAXISRAD__CONFIG == D_FAST_C_ALGO) 516 SinCosFIdx(&s, &c, NN_MATH_RAD_TO_FIDX(fRad)); 517#else 518 s = ::std::sinf(fRad); 519 c = ::std::cosf(fRad); 520#endif 521 t = 1.0f - c; 522 523 VEC3Normalize( &vN, pAxis ); 524 525 x = vN.x; 526 y = vN.y; 527 z = vN.z; 528 529 xSq = x * x; 530 ySq = y * y; 531 zSq = z * z; 532 533 register f32 m00, m01, m02, m10, m11, m12, m20, m21, m22; 534 m00 = ( t * xSq ) + ( c ); 535 m01 = ( t * x * y ) - ( s * z ); 536 m02 = ( t * x * z ) + ( s * y ); 537 538 m10 = ( t * x * y ) + ( s * z ); 539 m11 = ( t * ySq ) + ( c ); 540 m12 = ( t * y * z ) - ( s * x ); 541 542 m20 = ( t * x * z ) - ( s * y ); 543 m21 = ( t * y * z ) + ( s * x ); 544 m22 = ( t * zSq ) + ( c ); 545 546 m[0][0] = m00; 547 m[0][1] = m01; 548 m[0][2] = m02; 549 m[0][3] = 0.0f; 550 551 m[1][0] = m10; 552 m[1][1] = m11; 553 m[1][2] = m12; 554 m[1][3] = 0.0f; 555 556 m[2][0] = m20; 557 m[2][1] = m21; 558 m[2][2] = m22; 559 m[2][3] = 0.0f; 560 561 562 return pOut; 563} 564 565/* 566 567 568 569 570 571 572 573 574*/ 575NN_MATH_INLINE MTX34* 576MTX34RotXYZFIdxC(MTX34* pOut, f32 fIdxX, f32 fIdxY, f32 fIdxZ) 577{ 578 NN_FLOAT_ASSERT(fIdxX); 579 NN_FLOAT_ASSERT(fIdxY); 580 NN_FLOAT_ASSERT(fIdxZ); 581 582 f32 sinx, cosx; 583 f32 siny, cosy; 584 f32 sinz, cosz; 585 f32 f1, f2; 586 587 SinCosFIdx(&sinx, &cosx, fIdxX); 588 SinCosFIdx(&siny, &cosy, fIdxY); 589 SinCosFIdx(&sinz, &cosz, fIdxZ); 590 591 pOut->f._20 = -siny; 592 pOut->f._00 = cosz * cosy; 593 pOut->f._10 = sinz * cosy; 594 pOut->f._21 = cosy * sinx; 595 pOut->f._22 = cosy * cosx; 596 597 f1 = cosx * sinz; 598 f2 = sinx * cosz; 599 600 pOut->f._01 = f2 * siny - f1; 601 pOut->f._12 = f1 * siny - f2; 602 603 f1 = sinx * sinz; 604 f2 = cosx * cosz; 605 pOut->f._02 = f2 * siny + f1; 606 pOut->f._11 = f1 * siny + f2; 607 608 pOut->f._03 = 0.f; 609 pOut->f._13 = 0.f; 610 pOut->f._23 = 0.f; 611 612 return pOut; 613} 614NN_MATH_INLINE MTX34* 615MTX34RotXYZFIdxC_FAST(MTX34* pOut, f32 fIdxX, f32 fIdxY, f32 fIdxZ) 616{ 617 NN_FLOAT_ASSERT(fIdxX); 618 NN_FLOAT_ASSERT(fIdxY); 619 NN_FLOAT_ASSERT(fIdxZ); 620 621 f32 sinx, cosx; 622 f32 siny, cosy; 623 f32 sinz, cosz; 624 f32 f1, f2, f3, f4; 625 f32 f00, f10, f21, f22; 626 f32 f01, f11, f02, f12; 627 628 629 630 { 631 632 u16 idxx; 633 f32 abs_fidxx; 634 f32 rx; 635 636 u16 idxy; 637 f32 abs_fidxy; 638 f32 ry; 639 640 u16 idxz; 641 f32 abs_fidxz; 642 f32 rz; 643 644 int negx, negy, negz; 645 646 negx = (fIdxX < 0.0f) ? 1 : 0; 647 abs_fidxx = FAbs(fIdxX); 648 negy = (fIdxY < 0.0f) ? 1 : 0; 649 abs_fidxy = FAbs(fIdxY); 650 negz = (fIdxZ < 0.0f) ? 1 : 0; 651 abs_fidxz = FAbs(fIdxZ); 652 653 654 while ( abs_fidxx >= 65536.0f ) 655 { 656 abs_fidxx -= 65536.0f; 657 } 658 while ( abs_fidxy >= 65536.0f ) 659 { 660 abs_fidxy -= 65536.0f; 661 } 662 while ( abs_fidxz >= 65536.0f ) 663 { 664 abs_fidxz -= 65536.0f; 665 } 666 667 idxx = F32ToU16(abs_fidxx); 668 idxy = F32ToU16(abs_fidxy); 669 idxz = F32ToU16(abs_fidxz); 670 { 671 f32 idxxf, idxyf, idxzf; 672 673 idxxf = U16ToF32(idxx); 674 idxyf = U16ToF32(idxy); 675 idxzf = U16ToF32(idxz); 676 677 rx = abs_fidxx - idxxf; 678 ry = abs_fidxy - idxyf; 679 rz = abs_fidxz - idxzf; 680 681 682 } 683 684 idxx &= 0xff; 685 idxy &= 0xff; 686 idxz &= 0xff; 687 688 { 689 f32 sinx_val, sinx_delta, cosx_val, cosx_delta; 690 f32 siny_val, siny_delta, cosy_val, cosy_delta; 691 f32 sinz_val, sinz_delta, cosz_val, cosz_delta; 692 693 sinx_val = internal::gSinCosTbl[idxx].sin_val; 694 cosx_val = internal::gSinCosTbl[idxx].cos_val; 695 sinx_delta = internal::gSinCosTbl[idxx].sin_delta; 696 cosx_delta = internal::gSinCosTbl[idxx].cos_delta; 697 698 sinx = sinx_val + rx * sinx_delta; 699 cosx = cosx_val + rx * cosx_delta; 700 701 siny_val = internal::gSinCosTbl[idxy].sin_val; 702 cosy_val = internal::gSinCosTbl[idxy].cos_val; 703 siny_delta = internal::gSinCosTbl[idxy].sin_delta; 704 cosy_delta = internal::gSinCosTbl[idxy].cos_delta; 705 706 siny = siny_val + ry * siny_delta; 707 cosy = cosy_val + ry * cosy_delta; 708 709 sinz_val = internal::gSinCosTbl[idxz].sin_val; 710 cosz_val = internal::gSinCosTbl[idxz].cos_val; 711 sinz_delta = internal::gSinCosTbl[idxz].sin_delta; 712 cosz_delta = internal::gSinCosTbl[idxz].cos_delta; 713 714 sinz = sinz_val + rz * sinz_delta; 715 cosz = cosz_val + rz * cosz_delta; 716 717 } 718 719 sinx = (negx) ? -sinx : sinx; 720 siny = (negy) ? -siny : siny; 721 sinz = (negz) ? -sinz : sinz; 722 723 } 724 725 f00 = cosz * cosy; 726 f10 = sinz * cosy; 727 f21 = sinx * cosy; 728 f22 = cosx * cosy; 729 730 f1 = cosx * sinz; 731 f2 = sinx * cosz; 732 733 f01 = f2 * siny - f1; 734 f12 = f1 * siny - f2; 735 736 f3 = sinx * sinz; 737 f4 = cosx * cosz; 738 739 f02 = f4 * siny + f3; 740 f11 = f3 * siny + f4; 741 742 pOut->f._00 = f00; 743 pOut->f._10 = f10; 744 pOut->f._21 = f21; 745 pOut->f._22 = f22; 746 747 pOut->f._01 = f01; 748 pOut->f._12 = f12; 749 pOut->f._02 = f02; 750 pOut->f._11 = f11; 751 pOut->f._20 = -siny; 752 753 *(unsigned int*)&pOut->f._03 = 0x00000000; 754 *(unsigned int*)&pOut->f._13 = 0x00000000; 755 *(unsigned int*)&pOut->f._23 = 0x00000000; 756 757 return pOut; 758} 759 760/* 761 762 763 764 765 766 767*/ 768NN_MATH_INLINE u32 769MTX34InverseC(MTX34* pOut, const MTX34* p) 770{ 771 MTX34 mTmp; 772 f32 (*m)[4]; 773 f32 det; 774 775 NN_NULL_ASSERT( p ); 776 NN_NULL_ASSERT( pOut ); 777 778 f32 (*const inv)[4] = pOut->m; 779 const f32 (*const src)[4] = p->m; 780 781 if ( p == pOut ) 782 { 783 m = mTmp.m; 784 } 785 else 786 { 787 m = inv; 788 } 789 790 // compute the determinant of the upper 3x3 submatrix 791 det = src[0][0]*src[1][1]*src[2][2] + src[0][1]*src[1][2]*src[2][0] + src[0][2]*src[1][0]*src[2][1] 792 - src[2][0]*src[1][1]*src[0][2] - src[1][0]*src[0][1]*src[2][2] - src[0][0]*src[2][1]*src[1][2]; 793 794 // check if matrix is singular 795 if ( det == 0.0f ) 796 { 797 return 0; 798 } 799 800 // compute the inverse of the upper submatrix: 801 802 // find the transposed matrix of cofactors of the upper submatrix 803 // and multiply by (1/det) 804 805 det = 1.0f / det; 806 807 m[0][0] = (src[1][1]*src[2][2] - src[2][1]*src[1][2]) * det; 808 m[0][1] = -(src[0][1]*src[2][2] - src[2][1]*src[0][2]) * det; 809 m[0][2] = (src[0][1]*src[1][2] - src[1][1]*src[0][2]) * det; 810 811 m[1][0] = -(src[1][0]*src[2][2] - src[2][0]*src[1][2]) * det; 812 m[1][1] = (src[0][0]*src[2][2] - src[2][0]*src[0][2]) * det; 813 m[1][2] = -(src[0][0]*src[1][2] - src[1][0]*src[0][2]) * det; 814 815 m[2][0] = (src[1][0]*src[2][1] - src[2][0]*src[1][1]) * det; 816 m[2][1] = -(src[0][0]*src[2][1] - src[2][0]*src[0][1]) * det; 817 m[2][2] = (src[0][0]*src[1][1] - src[1][0]*src[0][1]) * det; 818 819 820 // compute (invA)*(-C) 821 m[0][3] = -m[0][0]*src[0][3] - m[0][1]*src[1][3] - m[0][2]*src[2][3]; 822 m[1][3] = -m[1][0]*src[0][3] - m[1][1]*src[1][3] - m[1][2]*src[2][3]; 823 m[2][3] = -m[2][0]*src[0][3] - m[2][1]*src[1][3] - m[2][2]*src[2][3]; 824 825 // copy back if needed 826 if ( m == mTmp.m ) 827 { 828 MTX34Copy( pOut, &mTmp ); 829 } 830 831 return 1; 832} 833 834 835/* 836 837 838 839 840 841 842*/ 843NN_MATH_INLINE MTX34* 844MTX34TransposeC(MTX34* pOut, const MTX34* p) 845{ 846 MTX34 mTmp; 847 848 NN_NULL_ASSERT( p ); 849 NN_NULL_ASSERT( pOut ); 850 851 const f32 (*const src)[4] = p->m; 852 f32 (*m)[4]; 853 854 if (p == pOut) 855 { 856 m = mTmp.m; 857 } 858 else 859 { 860 m = pOut->m; 861 } 862 863 m[0][0] = src[0][0]; m[0][1] = src[1][0]; m[0][2] = src[2][0]; m[0][3] = 0.0f; 864 m[1][0] = src[0][1]; m[1][1] = src[1][1]; m[1][2] = src[2][1]; m[1][3] = 0.0f; 865 m[2][0] = src[0][2]; m[2][1] = src[1][2]; m[2][2] = src[2][2]; m[2][3] = 0.0f; 866 867 // copy back if needed 868 if ( m == mTmp.m ) 869 { 870 MTX34Copy( pOut, &mTmp ); 871 } 872 873 return pOut; 874} 875 876 877/* 878 879 880 881 882 883 884*/ 885NN_MATH_INLINE u32 886MTX34InvTransposeC(MTX34* pOut, const MTX34* __restrict p) 887{ 888 MTX34 mTmp; 889 f32 (*m)[4]; 890 f32 det; 891 892 NN_NULL_ASSERT(p); 893 NN_NULL_ASSERT(pOut); 894 895 f32 (*const invX)[4] = pOut->m; 896 const f32 (*const src)[4] = p->m; 897 898 if ( p == pOut ) 899 { 900 m = mTmp.m; 901 } 902 else 903 { 904 m = invX; 905 } 906 907 // compute the determinant of the upper 3x3 submatrix 908 det = src[0][0]*src[1][1]*src[2][2] + src[0][1]*src[1][2]*src[2][0] + src[0][2]*src[1][0]*src[2][1] 909 - src[2][0]*src[1][1]*src[0][2] - src[1][0]*src[0][1]*src[2][2] - src[0][0]*src[2][1]*src[1][2]; 910 911 // check if matrix is singular 912 if ( det == 0.0f ) 913 { 914 return 0; 915 } 916 917 // compute the inverse-transpose of the upper submatrix: 918 919 // find the transposed matrix of cofactors of the upper submatrix 920 // and multiply by (1/det) 921 922 det = 1.0f / det; 923 924 m[0][0] = (src[1][1]*src[2][2] - src[2][1]*src[1][2]) * det; 925 m[0][1] = -(src[1][0]*src[2][2] - src[2][0]*src[1][2]) * det; 926 m[0][2] = (src[1][0]*src[2][1] - src[2][0]*src[1][1]) * det; 927 928 m[1][0] = -(src[0][1]*src[2][2] - src[2][1]*src[0][2]) * det; 929 m[1][1] = (src[0][0]*src[2][2] - src[2][0]*src[0][2]) * det; 930 m[1][2] = -(src[0][0]*src[2][1] - src[2][0]*src[0][1]) * det; 931 932 m[2][0] = (src[0][1]*src[1][2] - src[1][1]*src[0][2]) * det; 933 m[2][1] = -(src[0][0]*src[1][2] - src[1][0]*src[0][2]) * det; 934 m[2][2] = (src[0][0]*src[1][1] - src[1][0]*src[0][1]) * det; 935 936 937 // the fourth columns should be all zero 938 m[0][3] = 0.0F; 939 m[1][3] = 0.0F; 940 m[2][3] = 0.0F; 941 942 // copy back if needed 943 if ( m == mTmp.m ) 944 { 945 MTX34Copy( pOut, &mTmp ); 946 } 947 948 return 1; 949} 950 951 952/* 953 954 955 956 957 958 959 960 961*/ 962NN_MATH_INLINE MTX34* 963MTX34LookAtC(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamUp, const VEC3* pTarget) 964{ 965 NN_NULL_ASSERT(pOut); 966 NN_NULL_ASSERT(pCamPos); 967 NN_NULL_ASSERT(pCamUp); 968 NN_NULL_ASSERT(pTarget); 969 970 f32 (*const m)[4] = pOut->m; 971 972 // compute unit target vector 973 // use negative value to look down (-Z) axis 974 VEC3 vLook; 975 VEC3Sub(&vLook, pCamPos, pTarget); 976 VEC3Normalize(&vLook, &vLook); 977 978 // vRight = pCamUp x vLook 979 VEC3 vRight; 980 VEC3Cross(&vRight, pCamUp, &vLook); 981 VEC3Normalize(&vRight, &vRight); 982 983 // vUp = vLook x vRight 984 VEC3 vUp; 985 VEC3Cross(&vUp, &vLook, &vRight); 986 // Don't need to normalize vUp since it should already be unit length 987 // VECNormalize( &vUp, &vUp ); 988 989 m[0][0] = vRight.x; 990 m[0][1] = vRight.y; 991 m[0][2] = vRight.z; 992 m[0][3] = -( pCamPos->x * vRight.x + pCamPos->y * vRight.y + pCamPos->z * vRight.z ); 993 994 m[1][0] = vUp.x; 995 m[1][1] = vUp.y; 996 m[1][2] = vUp.z; 997 m[1][3] = -( pCamPos->x * vUp.x + pCamPos->y * vUp.y + pCamPos->z * vUp.z ); 998 999 m[2][0] = vLook.x; 1000 m[2][1] = vLook.y; 1001 m[2][2] = vLook.z; 1002 m[2][3] = -( pCamPos->x * vLook.x + pCamPos->y * vLook.y + pCamPos->z * vLook.z ); 1003 1004 return pOut; 1005} 1006NN_MATH_INLINE MTX34* 1007MTX34LookAtC_FAST(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamUp, const VEC3* pTarget) 1008{ 1009 NN_NULL_ASSERT(pOut); 1010 NN_NULL_ASSERT(pCamPos); 1011 NN_NULL_ASSERT(pCamUp); 1012 NN_NULL_ASSERT(pTarget); 1013 1014 f32 (*const m)[4] = pOut->m; 1015 1016 // compute unit target vector 1017 // use negative value to look down (-Z) axis 1018 f32 vLookx, vLooky, vLookz; 1019 { 1020 vLookx = pCamPos->x - pTarget->x; 1021 vLooky = pCamPos->y - pTarget->y; 1022 vLookz = pCamPos->z - pTarget->z; 1023 } 1024 1025 // vLook Normalize 1026 { 1027 f32 mag = (vLookx * vLookx) + (vLooky * vLooky) + (vLookz * vLookz); 1028 1029 NN_ASSERTMSG(mag != 0, "MATHNormalize3(): zero magnitude vector"); 1030 1031 mag = 1.0f / ::std::sqrtf(mag); 1032 1033 1034 vLookx = vLookx * mag; 1035 vLooky = vLooky * mag; 1036 vLookz = vLookz * mag; 1037 } 1038 1039 // vRight = pCamUp x vLook 1040 f32 vRightx, vRighty, vRightz; 1041 { 1042 vRightx = ( pCamUp->y * vLookz ) - ( pCamUp->z * vLooky ); 1043 vRighty = ( pCamUp->z * vLookx ) - ( pCamUp->x * vLookz ); 1044 vRightz = ( pCamUp->x * vLooky ) - ( pCamUp->y * vLookx ); 1045 } 1046 1047 // vRight Normalize 1048 { 1049 f32 mag = (vRightx * vRightx) + (vRighty * vRighty) + (vRightz * vRightz); 1050 1051 NN_ASSERTMSG(mag != 0, "MATHNormalize3(): zero magnitude vector"); 1052 1053 mag = 1.0f / ::std::sqrtf(mag); 1054 1055 vRightx = vRightx * mag; 1056 vRighty = vRighty * mag; 1057 vRightz = vRightz * mag; 1058 } 1059 1060 // vUp = vLook x vRight 1061 f32 vUpx, vUpy, vUpz; 1062 { 1063 vUpx = ( vLooky * vRightz ) - ( vLookz * vRighty ); 1064 vUpy = ( vLookz * vRightx ) - ( vLookx * vRightz ); 1065 vUpz = ( vLookx * vRighty ) - ( vLooky * vRightx ); 1066 } 1067 1068 1069 // Don't need to normalize vUp since it should already be unit length 1070 // VECNormalize( &vUp, &vUp ); 1071 f32 tmp1, tmp2, tmp3; 1072 1073 tmp1 = -( pCamPos->x * vRightx + pCamPos->y * vRighty + pCamPos->z * vRightz ); 1074 tmp2 = -( pCamPos->x * vUpx + pCamPos->y * vUpy + pCamPos->z * vUpz ); 1075 tmp3 = -( pCamPos->x * vLookx + pCamPos->y * vLooky + pCamPos->z * vLookz ); 1076 1077 m[0][0] = vRightx; 1078 m[0][1] = vRighty; 1079 m[0][2] = vRightz; 1080 1081 m[1][0] = vUpx; 1082 m[1][1] = vUpy; 1083 m[1][2] = vUpz; 1084 1085 m[2][0] = vLookx; 1086 m[2][1] = vLooky; 1087 m[2][2] = vLookz; 1088 1089 m[0][3] = tmp1; 1090 m[1][3] = tmp2; 1091 m[2][3] = tmp3; 1092 1093 return pOut; 1094} 1095 1096/* 1097 1098 1099 1100 1101 1102 1103 1104 1105*/ 1106NN_MATH_INLINE MTX34* 1107MTX34LookAtC(MTX34* pOut, const VEC3* pCamPos, f32 twist, const VEC3* pTarget) 1108{ 1109 NN_NULL_ASSERT(pOut); 1110 NN_NULL_ASSERT(pCamPos); 1111 NN_NULL_ASSERT(pTarget); 1112 1113 f32 (*const m)[4] = pOut->m; 1114 1115 // Z direction of camera coordinates 1116 VEC3 lookReverse(pCamPos->x - pTarget->x, pCamPos->y - pTarget->y, pCamPos->z - pTarget->z); 1117 1118 if ((lookReverse.x == 0.0f) && (lookReverse.z == 0.0f)) 1119 { 1120 // Twist is not defined if the xz-coordinates of the camera and the target are the same. 1121 m[0][0] = 1.0f; 1122 m[0][1] = 0.0f; 1123 m[0][2] = 0.0f; 1124 m[0][3] = -pCamPos->x; 1125 1126 m[1][0] = 0.0f; 1127 m[1][1] = 0.0f; 1128 1129 m[2][0] = 0.0f; 1130 m[2][2] = 0.0f; 1131 1132 if (lookReverse.y <= 0.0f) 1133 { 1134 // When looking straight up 1135 m[1][2] = 1.0f; 1136 m[1][3] = -pCamPos->z; 1137 1138 m[2][1] = -1.0f; 1139 m[2][3] = pCamPos->y; 1140 } 1141 else 1142 { 1143 // When looking straight down 1144 m[1][2] = -1.0f; 1145 m[1][3] = pCamPos->z; 1146 1147 m[2][1] = 1.0f; 1148 m[2][3] = -pCamPos->y; 1149 } 1150 } 1151 else 1152 { 1153 // X direction of camera coordinates 1154 VEC3 r(lookReverse.z, 0.0f, -lookReverse.x); 1155 1156 VEC3Normalize(&lookReverse, &lookReverse); 1157 VEC3Normalize(&r, &r); 1158 1159 // Y direction of camera coordinates 1160 VEC3 u; 1161 VEC3Cross(&u, &lookReverse, &r); 1162 1163 f32 st, ct; 1164 SinCosDeg(&st, &ct, twist); 1165 VEC3 right, up; 1166 1167 // Get 'right' by rotating the r-axis and u-axis cameraTwist values a half rotation clockwise on the ru-plane 1168 // Note that r.y == 0. 1169 right.x = st * u.x + ct * r.x; 1170 right.y = st * u.y; 1171 right.z = st * u.z + ct * r.z; 1172 1173 up.x = ct * u.x - st * r.x; 1174 up.y = ct * u.y; 1175 up.z = ct * u.z - st * r.z; 1176 1177 // Right 1178 m[0][0] = right.x; 1179 m[0][1] = right.y; 1180 m[0][2] = right.z; 1181 m[0][3] = -VEC3Dot(pCamPos, &right); 1182 1183 // Up 1184 m[1][0] = up.x; 1185 m[1][1] = up.y; 1186 m[1][2] = up.z; 1187 m[1][3] = -VEC3Dot(pCamPos, &up); 1188 1189 // look 1190 m[2][0] = lookReverse.x; 1191 m[2][1] = lookReverse.y; 1192 m[2][2] = lookReverse.z; 1193 m[2][3] = -VEC3Dot(pCamPos, &lookReverse); 1194 } 1195 1196 return pOut; 1197} 1198NN_MATH_INLINE MTX34* 1199MTX34LookAtC_FAST(MTX34* pOut, const VEC3* pCamPos, f32 twist, const VEC3* pTarget) 1200{ 1201 NN_NULL_ASSERT(pOut); 1202 NN_NULL_ASSERT(pCamPos); 1203 NN_NULL_ASSERT(pTarget); 1204 1205 f32 (*const m)[4] = pOut->m; 1206 1207 // Z direction of camera coordinates 1208 f32 lookReversex, lookReversey, lookReversez; 1209 1210 lookReversex = pCamPos->x - pTarget->x; 1211 lookReversez = pCamPos->z - pTarget->z; 1212 lookReversey = pCamPos->y - pTarget->y; 1213 1214 if ((lookReversex == 0.0f) && (lookReversez == 0.0f)) 1215 { 1216 // Twist is not defined if the xz-coordinates of the camera and the target are the same. 1217 m[0][0] = 1.0f; 1218 m[0][1] = 0.0f; 1219 m[0][2] = 0.0f; 1220 m[0][3] = -pCamPos->x; 1221 1222 m[1][0] = 0.0f; 1223 m[1][1] = 0.0f; 1224 1225 m[2][0] = 0.0f; 1226 m[2][2] = 0.0f; 1227 1228 if (lookReversey <= 0.0f) 1229 { 1230 // When looking straight up 1231 m[1][2] = 1.0f; 1232 m[1][3] = -pCamPos->z; 1233 1234 m[2][1] = -1.0f; 1235 m[2][3] = pCamPos->y; 1236 } 1237 else 1238 { 1239 // When looking straight down 1240 m[1][2] = -1.0f; 1241 m[1][3] = pCamPos->z; 1242 1243 m[2][1] = 1.0f; 1244 m[2][3] = -pCamPos->y; 1245 } 1246 } 1247 else 1248 { 1249 // X direction of camera coordinates 1250 f32 rx, ry, rz; 1251 f32 mag1, mag2; 1252 f32 ux, uy, uz; 1253 f32 fidx; 1254 1255 mag1 = (lookReversez * lookReversez) + (-lookReversex * -lookReversex); 1256 mag2 = (lookReversex * lookReversex) + (lookReversey * lookReversey) + (lookReversez * lookReversez); 1257 NN_ASSERTMSG(mag1 != 0, "MATHNormalize3(): zero magnitude vector"); 1258 NN_ASSERTMSG(mag2 != 0, "MATHNormalize3(): zero magnitude vector"); 1259 mag1 = ::std::sqrtf(mag1); 1260 mag2 = ::std::sqrtf(mag2); 1261 fidx = NN_MATH_DEG_TO_FIDX(twist); 1262 mag1 = 1.0f / mag1; 1263 mag2 = 1.0f / mag2; 1264 1265 // r Normalize 1266 { 1267 rx = lookReversez * mag1; 1268 ry = 0.0f; 1269 rz = -lookReversex * mag1; 1270 } 1271 // lookReverse Normalize 1272 { 1273 lookReversex = lookReversex * mag2; 1274 lookReversey = lookReversey * mag2; 1275 lookReversez = lookReversez * mag2; 1276 } 1277 1278 // Y direction of camera coordinates 1279 { 1280 ux = ( lookReversey * rz ) - ( lookReversez * ry ); 1281 uy = ( lookReversez * rx ) - ( lookReversex * rz ); 1282 uz = ( lookReversex * ry ) - ( lookReversey * rx ); 1283 } 1284 1285 1286 f32 st, ct; 1287// SinCosDeg(, , twist); 1288 SinCosFIdx(&st, &ct, fidx); 1289 f32 rightx, righty, rightz; 1290 f32 upx, upy, upz; 1291 1292 // Get 'right' by rotating the r-axis and u-axis cameraTwist values a half rotation clockwise on the ru-plane 1293 // Note that r.y == 0. 1294 rightx = st * ux + ct * rx; 1295 righty = st * uy; 1296 rightz = st * uz + ct * rz; 1297 1298 upx = ct * ux - st * rx; 1299 upy = ct * uy; 1300 upz = ct * uz - st * rz; 1301 1302 // Right 1303 f32 tmp1, tmp2, tmp3; 1304 1305 tmp1 = pCamPos->x * rightx + pCamPos->y * righty + pCamPos->z * rightz; 1306 tmp2 = pCamPos->x * upx + pCamPos->y * upy + pCamPos->z * upz; 1307 tmp3 = pCamPos->x * lookReversex + pCamPos->y * lookReversey + pCamPos->z * lookReversez; 1308 1309 m[0][0] = rightx; 1310 m[0][1] = righty; 1311 m[0][2] = rightz; 1312 m[0][3] = -tmp1; 1313 1314 // Up 1315 m[1][0] = upx; 1316 m[1][1] = upy; 1317 m[1][2] = upz; 1318 m[1][3] = -tmp2; 1319 1320 // look 1321 m[2][0] = lookReversex; 1322 m[2][1] = lookReversey; 1323 m[2][2] = lookReversez; 1324 m[2][3] = -tmp3; 1325 } 1326 1327 return pOut; 1328} 1329 1330/* 1331 1332 1333 1334 1335 1336 1337 1338*/ 1339NN_MATH_INLINE MTX34* 1340MTX34CameraRotateC(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamRotate) 1341{ 1342 NN_NULL_ASSERT(pOut); 1343 NN_NULL_ASSERT(pCamPos); 1344 NN_NULL_ASSERT(pCamRotate); 1345 1346 f32 (*const m)[4] = pOut->m; 1347 1348 f32 sx, sy, sz, cx, cy, cz; 1349 SinCosDeg(&sx, &cx, pCamRotate->x); 1350 SinCosDeg(&sy, &cy, pCamRotate->y); 1351 SinCosDeg(&sz, &cz, pCamRotate->z); 1352 1353 // Rotate in the order of z-axis, x-axis, y-axis 1354 VEC3 right, up, back; 1355 1356 right.x = sx * sy * sz + cy * cz; 1357 right.y = cx * sz; 1358 right.z = sx * cy * sz - sy * cz; 1359 1360 up.x = sx * sy * cz - cy * sz; 1361 up.y = cx * cz; 1362 up.z = sx * cy * cz + sy * sz; 1363 1364 back.x = cx * sy; 1365 back.y = - sx; 1366 back.z = cx * cy; 1367 1368 m[0][0] = right.x; 1369 m[0][1] = right.y; 1370 m[0][2] = right.z; 1371 m[0][3] = -VEC3Dot(pCamPos, &right); 1372 1373 m[1][0] = up.x; 1374 m[1][1] = up.y; 1375 m[1][2] = up.z; 1376 m[1][3] = -VEC3Dot(pCamPos, &up); 1377 1378 m[2][0] = back.x; 1379 m[2][1] = back.y; 1380 m[2][2] = back.z; 1381 m[2][3] = -VEC3Dot(pCamPos, &back); 1382 1383 return pOut; 1384} 1385NN_MATH_INLINE MTX34* 1386MTX34CameraRotateC_FAST(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamRotate) 1387{ 1388 NN_NULL_ASSERT(pOut); 1389 NN_NULL_ASSERT(pCamPos); 1390 NN_NULL_ASSERT(pCamRotate); 1391 1392 f32 (*const m)[4] = pOut->m; 1393 1394 f32 sinx, cosx; 1395 f32 siny, cosy; 1396 f32 sinz, cosz; 1397 f32 fIdxX = NN_MATH_DEG_TO_FIDX(pCamRotate->x); 1398 f32 fIdxY = NN_MATH_DEG_TO_FIDX(pCamRotate->y); 1399 f32 fIdxZ = NN_MATH_DEG_TO_FIDX(pCamRotate->z); 1400 1401 { 1402 1403 u16 idxx; 1404 f32 abs_fidxx; 1405 f32 rx; 1406 1407 u16 idxy; 1408 f32 abs_fidxy; 1409 f32 ry; 1410 1411 u16 idxz; 1412 f32 abs_fidxz; 1413 f32 rz; 1414 1415 int negx, negy, negz; 1416 1417 negx = (fIdxX < 0.0f) ? 1 : 0; 1418 abs_fidxx = FAbs(fIdxX); 1419 negy = (fIdxY < 0.0f) ? 1 : 0; 1420 abs_fidxy = FAbs(fIdxY); 1421 negz = (fIdxZ < 0.0f) ? 1 : 0; 1422 abs_fidxz = FAbs(fIdxZ); 1423 1424 1425 while ( abs_fidxx >= 65536.0f ) 1426 { 1427 abs_fidxx -= 65536.0f; 1428 } 1429 while ( abs_fidxy >= 65536.0f ) 1430 { 1431 abs_fidxy -= 65536.0f; 1432 } 1433 while ( abs_fidxz >= 65536.0f ) 1434 { 1435 abs_fidxz -= 65536.0f; 1436 } 1437 1438 idxx = F32ToU16(abs_fidxx); 1439 idxy = F32ToU16(abs_fidxy); 1440 idxz = F32ToU16(abs_fidxz); 1441 { 1442 f32 idxxf, idxyf, idxzf; 1443 1444 idxxf = U16ToF32(idxx); 1445 idxyf = U16ToF32(idxy); 1446 idxzf = U16ToF32(idxz); 1447 1448 rx = abs_fidxx - idxxf; 1449 ry = abs_fidxy - idxyf; 1450 rz = abs_fidxz - idxzf; 1451 1452 1453 } 1454 1455 idxx &= 0xff; 1456 idxy &= 0xff; 1457 idxz &= 0xff; 1458 1459 { 1460 f32 sinx_val, sinx_delta, cosx_val, cosx_delta; 1461 f32 siny_val, siny_delta, cosy_val, cosy_delta; 1462 f32 sinz_val, sinz_delta, cosz_val, cosz_delta; 1463 1464 sinx_val = internal::gSinCosTbl[idxx].sin_val; 1465 cosx_val = internal::gSinCosTbl[idxx].cos_val; 1466 sinx_delta = internal::gSinCosTbl[idxx].sin_delta; 1467 cosx_delta = internal::gSinCosTbl[idxx].cos_delta; 1468 1469 sinx = sinx_val + rx * sinx_delta; 1470 cosx = cosx_val + rx * cosx_delta; 1471 1472 siny_val = internal::gSinCosTbl[idxy].sin_val; 1473 cosy_val = internal::gSinCosTbl[idxy].cos_val; 1474 siny_delta = internal::gSinCosTbl[idxy].sin_delta; 1475 cosy_delta = internal::gSinCosTbl[idxy].cos_delta; 1476 1477 siny = siny_val + ry * siny_delta; 1478 cosy = cosy_val + ry * cosy_delta; 1479 1480 sinz_val = internal::gSinCosTbl[idxz].sin_val; 1481 cosz_val = internal::gSinCosTbl[idxz].cos_val; 1482 sinz_delta = internal::gSinCosTbl[idxz].sin_delta; 1483 cosz_delta = internal::gSinCosTbl[idxz].cos_delta; 1484 1485 sinz = sinz_val + rz * sinz_delta; 1486 cosz = cosz_val + rz * cosz_delta; 1487 1488 } 1489 1490 sinx = (negx) ? -sinx : sinx; 1491 siny = (negy) ? -siny : siny; 1492 sinz = (negz) ? -sinz : sinz; 1493 1494 } 1495 1496 1497 1498 1499 // Rotate in the order of z-axis, x-axis, y-axis 1500 register VEC3 right, up, back; 1501 register f32 m00, m01, m02, m03; 1502 register f32 m10, m11, m12, m13; 1503 register f32 m20, m21, m22, m23; 1504 1505 1506 1507 right.x = sinx * siny * sinz + cosy * cosz; 1508 right.y = cosx * sinz; 1509 right.z = sinx * cosy * sinz - siny * cosz; 1510 1511 up.x = sinx * siny * cosz - cosy * sinz; 1512 up.y = cosx * cosz; 1513 up.z = sinx * cosy * cosz + siny * sinz; 1514 1515 back.x = cosx * siny; 1516 back.y = - sinx; 1517 back.z = cosx * cosy; 1518 1519 m00 = right.x; 1520 m01 = right.y; 1521 m02 = right.z; 1522 m03 = -VEC3Dot(pCamPos, &right); 1523 1524 m10 = up.x; 1525 m11 = up.y; 1526 m12 = up.z; 1527 m13 = -VEC3Dot(pCamPos, &up); 1528 1529 m20 = back.x; 1530 m21 = back.y; 1531 m22 = back.z; 1532 m23 = -VEC3Dot(pCamPos, &back); 1533 1534 m[0][0] = m00; 1535 m[0][1] = m01; 1536 m[0][2] = m02; 1537 m[0][3] = m03; 1538 1539 m[1][0] = m10; 1540 m[1][1] = m11; 1541 m[1][2] = m12; 1542 m[1][3] = m13; 1543 1544 m[2][0] = m20; 1545 m[2][1] = m21; 1546 m[2][2] = m22; 1547 m[2][3] = m23; 1548 1549 return pOut; 1550} 1551 1552/* 1553 1554*/ 1555/* Please see man pages for details 1556 1557 1558*/ 1559 1560/* 1561 1562 1563 1564 1565 1566 1567*/ 1568NN_MATH_INLINE MTX34* 1569QUATToMTX34C(MTX34* pOut, const QUAT* pQ) 1570{ 1571 f32 s, xs, ys, zs; 1572 f32 wx, wy, wz, xx, xy, xz, yy, yz, zz; 1573 1574 NN_NULL_ASSERT(pOut); 1575 NN_NULL_ASSERT(pQ); 1576 NN_ASSERT( pQ->x || pQ->y || pQ->z || pQ->w ); 1577 1578 f32 (*const m)[4] = pOut->m; 1579 1580 s = 2.0f / ( (pQ->x * pQ->x) + (pQ->y * pQ->y) + (pQ->z * pQ->z) + (pQ->w * pQ->w) ); 1581 1582 xs = pQ->x * s; ys = pQ->y * s; zs = pQ->z * s; 1583 wx = pQ->w * xs; wy = pQ->w * ys; wz = pQ->w * zs; 1584 xx = pQ->x * xs; xy = pQ->x * ys; xz = pQ->x * zs; 1585 yy = pQ->y * ys; yz = pQ->y * zs; zz = pQ->z * zs; 1586 1587 m[0][0] = 1.0f - (yy + zz); 1588 m[0][1] = xy - wz; 1589 m[0][2] = xz + wy; 1590 m[0][3] = 0.0f; 1591 1592 m[1][0] = xy + wz; 1593 m[1][1] = 1.0f - (xx + zz); 1594 m[1][2] = yz - wx; 1595 m[1][3] = 0.0f; 1596 1597 m[2][0] = xz - wy; 1598 m[2][1] = yz + wx; 1599 m[2][2] = 1.0f - (xx + yy); 1600 m[2][3] = 0.0f; 1601 1602 return pOut; 1603} 1604NN_MATH_INLINE MTX34* 1605QUATToMTX34C_FAST(MTX34* pOut, const QUAT* pQ) 1606{ 1607 f32 s, xs, ys, zs; 1608 f32 wx, wy, wz, xx, xy, xz, yy, yz, zz; 1609 f32 m00, m01, m02, m10, m11, m12, m20, m21, m22; 1610 f32 pQx, pQy, pQz, pQw; 1611 1612 NN_NULL_ASSERT(pOut); 1613 NN_NULL_ASSERT(pQ); 1614 NN_ASSERT( pQ->x || pQ->y || pQ->z || pQ->w ); 1615 1616 f32 (*const m)[4] = pOut->m; 1617 1618 pQx = pQ->x; 1619 pQy = pQ->y; 1620 pQz = pQ->z; 1621 pQw = pQ->w; 1622 1623 s = 2.0f / ( (pQx * pQx) + (pQy * pQy) + (pQz * pQz) + (pQw * pQw) ); 1624 1625 xs = pQx * s; ys = pQy * s; zs = pQz * s; 1626 wx = pQw * xs; wy = pQw * ys; wz = pQw * zs; 1627 xx = pQx * xs; xy = pQx * ys; xz = pQx * zs; 1628 yy = pQy * ys; yz = pQy * zs; zz = pQz * zs; 1629 1630 m00 = 1.0f - (yy + zz); 1631 m01 = xy - wz; 1632 m02 = xz + wy; 1633 1634 m10 = xy + wz; 1635 m11 = 1.0f - (xx + zz); 1636 m12 = yz - wx; 1637 1638 m20 = xz - wy; 1639 m21 = yz + wx; 1640 m22 = 1.0f - (xx + yy); 1641 1642 m[0][3] = 0.0f; 1643 m[1][3] = 0.0f; 1644 m[2][3] = 0.0f; 1645 1646 m[0][0] = m00; 1647 m[0][1] = m01; 1648 m[0][2] = m02; 1649 1650 m[1][0] = m10; 1651 m[1][1] = m11; 1652 m[1][2] = m12; 1653 1654 m[2][0] = m20; 1655 m[2][1] = m21; 1656 m[2][2] = m22; 1657 1658 1659 return pOut; 1660} 1661 1662/* 1663 1664*/ 1665 1666} // namespace ARMv6 1667} // namespace math 1668} // namespace nn 1669