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 454 */ 455NN_MATH_INLINE MTX34* 456MTX34RotAxisRad_C( MTX34* pOut, const VEC3 *pAxis, f32 fRad, bool isChangeTrans ) 457{ 458 VEC3 vN; 459 f32 s, c; // sinTheta, cosTheta 460 f32 t; // ( 1 - cosTheta ) 461 f32 x, y, z; // x, y, z components of normalized axis 462 f32 xSq, ySq, zSq; // x, y, z squared 463 464 465 NN_NULL_ASSERT( pOut ); 466 NN_NULL_ASSERT( pAxis ); 467 468 f32 (*const m)[4] = pOut->m; 469 470 s = ::std::sinf(fRad); 471 c = ::std::cosf(fRad); 472 t = 1.0f - c; 473 474 VEC3Normalize( &vN, pAxis ); 475 476 x = vN.x; 477 y = vN.y; 478 z = vN.z; 479 480 xSq = x * x; 481 ySq = y * y; 482 zSq = z * z; 483 484 m[0][0] = ( t * xSq ) + ( c ); 485 m[0][1] = ( t * x * y ) - ( s * z ); 486 m[0][2] = ( t * x * z ) + ( s * y ); 487 if ( isChangeTrans ) m[0][3] = 0.0f; 488 489 m[1][0] = ( t * x * y ) + ( s * z ); 490 m[1][1] = ( t * ySq ) + ( c ); 491 m[1][2] = ( t * y * z ) - ( s * x ); 492 if ( isChangeTrans ) m[1][3] = 0.0f; 493 494 m[2][0] = ( t * x * z ) - ( s * y ); 495 m[2][1] = ( t * y * z ) + ( s * x ); 496 m[2][2] = ( t * zSq ) + ( c ); 497 if ( isChangeTrans ) m[2][3] = 0.0f; 498 499 return pOut; 500} 501NN_MATH_INLINE MTX34* 502MTX34RotAxisRad_C_FAST( MTX34* pOut, const VEC3 *pAxis, f32 fRad, bool isChangeTrans ) 503{ 504 VEC3 vN; 505 f32 s, c; // sinTheta, cosTheta 506 f32 t; // ( 1 - cosTheta ) 507 f32 x, y, z; // x, y, z components of normalized axis 508 f32 xSq, ySq, zSq; // x, y, z squared 509 510 511 NN_NULL_ASSERT( pOut ); 512 NN_NULL_ASSERT( pAxis ); 513 514 f32 (*const m)[4] = pOut->m; 515 516#if (MTX34ROTAXISRAD__CONFIG == D_FAST_C_ALGO) 517 SinCosFIdx(&s, &c, NN_MATH_RAD_TO_FIDX(fRad)); 518#else 519 s = ::std::sinf(fRad); 520 c = ::std::cosf(fRad); 521#endif 522 t = 1.0f - c; 523 524 VEC3Normalize( &vN, pAxis ); 525 526 x = vN.x; 527 y = vN.y; 528 z = vN.z; 529 530 xSq = x * x; 531 ySq = y * y; 532 zSq = z * z; 533 534 register f32 m00, m01, m02, m10, m11, m12, m20, m21, m22; 535 m00 = ( t * xSq ) + ( c ); 536 m01 = ( t * x * y ) - ( s * z ); 537 m02 = ( t * x * z ) + ( s * y ); 538 539 m10 = ( t * x * y ) + ( s * z ); 540 m11 = ( t * ySq ) + ( c ); 541 m12 = ( t * y * z ) - ( s * x ); 542 543 m20 = ( t * x * z ) - ( s * y ); 544 m21 = ( t * y * z ) + ( s * x ); 545 m22 = ( t * zSq ) + ( c ); 546 547 m[0][0] = m00; 548 m[0][1] = m01; 549 m[0][2] = m02; 550 if ( isChangeTrans ) m[0][3] = 0.0f; 551 552 m[1][0] = m10; 553 m[1][1] = m11; 554 m[1][2] = m12; 555 if ( isChangeTrans ) m[1][3] = 0.0f; 556 557 m[2][0] = m20; 558 m[2][1] = m21; 559 m[2][2] = m22; 560 if ( isChangeTrans ) m[2][3] = 0.0f; 561 562 563 return pOut; 564} 565 566/* 567 568 569 570 571 572 573 574 575 576 */ 577NN_MATH_INLINE MTX34* 578MTX34RotXYZFIdxC(MTX34* pOut, f32 fIdxX, f32 fIdxY, f32 fIdxZ, bool isChangeTrans) 579{ 580 NN_FLOAT_ASSERT(fIdxX); 581 NN_FLOAT_ASSERT(fIdxY); 582 NN_FLOAT_ASSERT(fIdxZ); 583 584 f32 sinx, cosx; 585 f32 siny, cosy; 586 f32 sinz, cosz; 587 f32 f1, f2; 588 589 SinCosFIdx(&sinx, &cosx, fIdxX); 590 SinCosFIdx(&siny, &cosy, fIdxY); 591 SinCosFIdx(&sinz, &cosz, fIdxZ); 592 593 pOut->f._20 = -siny; 594 pOut->f._00 = cosz * cosy; 595 pOut->f._10 = sinz * cosy; 596 pOut->f._21 = cosy * sinx; 597 pOut->f._22 = cosy * cosx; 598 599 f1 = cosx * sinz; 600 f2 = sinx * cosz; 601 602 pOut->f._01 = f2 * siny - f1; 603 pOut->f._12 = f1 * siny - f2; 604 605 f1 = sinx * sinz; 606 f2 = cosx * cosz; 607 pOut->f._02 = f2 * siny + f1; 608 pOut->f._11 = f1 * siny + f2; 609 610 if ( isChangeTrans ) 611 { 612 pOut->f._03 = 0.f; 613 pOut->f._13 = 0.f; 614 pOut->f._23 = 0.f; 615 } 616 617 return pOut; 618} 619NN_MATH_INLINE MTX34* 620MTX34RotXYZFIdxC_FAST(MTX34* pOut, f32 fIdxX, f32 fIdxY, f32 fIdxZ, bool isChangeTrans) 621{ 622 NN_FLOAT_ASSERT(fIdxX); 623 NN_FLOAT_ASSERT(fIdxY); 624 NN_FLOAT_ASSERT(fIdxZ); 625 626 f32 sinx, cosx; 627 f32 siny, cosy; 628 f32 sinz, cosz; 629 f32 f1, f2, f3, f4; 630 f32 f00, f10, f21, f22; 631 f32 f01, f11, f02, f12; 632 633 { 634 635 u16 idxx; 636 f32 abs_fidxx; 637 f32 rx; 638 639 u16 idxy; 640 f32 abs_fidxy; 641 f32 ry; 642 643 u16 idxz; 644 f32 abs_fidxz; 645 f32 rz; 646 647 int negx, negy, negz; 648 649 negx = (fIdxX < 0.0f) ? 1 : 0; 650 abs_fidxx = FAbs(fIdxX); 651 negy = (fIdxY < 0.0f) ? 1 : 0; 652 abs_fidxy = FAbs(fIdxY); 653 negz = (fIdxZ < 0.0f) ? 1 : 0; 654 abs_fidxz = FAbs(fIdxZ); 655 656 657 while ( abs_fidxx >= 65536.0f ) 658 { 659 abs_fidxx -= 65536.0f; 660 } 661 while ( abs_fidxy >= 65536.0f ) 662 { 663 abs_fidxy -= 65536.0f; 664 } 665 while ( abs_fidxz >= 65536.0f ) 666 { 667 abs_fidxz -= 65536.0f; 668 } 669 670 idxx = F32ToU16(abs_fidxx); 671 idxy = F32ToU16(abs_fidxy); 672 idxz = F32ToU16(abs_fidxz); 673 { 674 f32 idxxf, idxyf, idxzf; 675 676 idxxf = U16ToF32(idxx); 677 idxyf = U16ToF32(idxy); 678 idxzf = U16ToF32(idxz); 679 680 rx = abs_fidxx - idxxf; 681 ry = abs_fidxy - idxyf; 682 rz = abs_fidxz - idxzf; 683 684 685 } 686 687 idxx &= 0xff; 688 idxy &= 0xff; 689 idxz &= 0xff; 690 691 { 692 f32 sinx_val, sinx_delta, cosx_val, cosx_delta; 693 f32 siny_val, siny_delta, cosy_val, cosy_delta; 694 f32 sinz_val, sinz_delta, cosz_val, cosz_delta; 695 696 sinx_val = internal::gSinCosTbl[idxx].sin_val; 697 cosx_val = internal::gSinCosTbl[idxx].cos_val; 698 sinx_delta = internal::gSinCosTbl[idxx].sin_delta; 699 cosx_delta = internal::gSinCosTbl[idxx].cos_delta; 700 701 sinx = sinx_val + rx * sinx_delta; 702 cosx = cosx_val + rx * cosx_delta; 703 704 siny_val = internal::gSinCosTbl[idxy].sin_val; 705 cosy_val = internal::gSinCosTbl[idxy].cos_val; 706 siny_delta = internal::gSinCosTbl[idxy].sin_delta; 707 cosy_delta = internal::gSinCosTbl[idxy].cos_delta; 708 709 siny = siny_val + ry * siny_delta; 710 cosy = cosy_val + ry * cosy_delta; 711 712 sinz_val = internal::gSinCosTbl[idxz].sin_val; 713 cosz_val = internal::gSinCosTbl[idxz].cos_val; 714 sinz_delta = internal::gSinCosTbl[idxz].sin_delta; 715 cosz_delta = internal::gSinCosTbl[idxz].cos_delta; 716 717 sinz = sinz_val + rz * sinz_delta; 718 cosz = cosz_val + rz * cosz_delta; 719 720 } 721 722 sinx = (negx) ? -sinx : sinx; 723 siny = (negy) ? -siny : siny; 724 sinz = (negz) ? -sinz : sinz; 725 726 } 727 728 f00 = cosz * cosy; 729 f10 = sinz * cosy; 730 f21 = sinx * cosy; 731 f22 = cosx * cosy; 732 733 f1 = cosx * sinz; 734 f2 = sinx * cosz; 735 736 f01 = f2 * siny - f1; 737 f12 = f1 * siny - f2; 738 739 f3 = sinx * sinz; 740 f4 = cosx * cosz; 741 742 f02 = f4 * siny + f3; 743 f11 = f3 * siny + f4; 744 745 pOut->f._00 = f00; 746 pOut->f._10 = f10; 747 pOut->f._21 = f21; 748 pOut->f._22 = f22; 749 750 pOut->f._01 = f01; 751 pOut->f._12 = f12; 752 pOut->f._02 = f02; 753 pOut->f._11 = f11; 754 pOut->f._20 = -siny; 755 756 if ( isChangeTrans ) 757 { 758 *(unsigned int*)&pOut->f._03 = 0x00000000; 759 *(unsigned int*)&pOut->f._13 = 0x00000000; 760 *(unsigned int*)&pOut->f._23 = 0x00000000; 761 } 762 763 return pOut; 764} 765 766/* 767 768 769 770 771 772 773 */ 774NN_MATH_INLINE u32 775MTX34InverseC(MTX34* pOut, const MTX34* p) 776{ 777 MTX34 mTmp; 778 f32 (*m)[4]; 779 f32 det; 780 781 NN_NULL_ASSERT( p ); 782 NN_NULL_ASSERT( pOut ); 783 784 f32 (*const inv)[4] = pOut->m; 785 const f32 (*const src)[4] = p->m; 786 787 if ( p == pOut ) 788 { 789 m = mTmp.m; 790 } 791 else 792 { 793 m = inv; 794 } 795 796 // compute the determinant of the upper 3x3 submatrix 797 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] 798 - 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]; 799 800 // check if matrix is singular 801 if ( det == 0.0f ) 802 { 803 return 0; 804 } 805 806 // compute the inverse of the upper submatrix: 807 808 // find the transposed matrix of cofactors of the upper submatrix 809 // and multiply by (1/det) 810 811 det = 1.0f / det; 812 813 m[0][0] = (src[1][1]*src[2][2] - src[2][1]*src[1][2]) * det; 814 m[0][1] = -(src[0][1]*src[2][2] - src[2][1]*src[0][2]) * det; 815 m[0][2] = (src[0][1]*src[1][2] - src[1][1]*src[0][2]) * det; 816 817 m[1][0] = -(src[1][0]*src[2][2] - src[2][0]*src[1][2]) * det; 818 m[1][1] = (src[0][0]*src[2][2] - src[2][0]*src[0][2]) * det; 819 m[1][2] = -(src[0][0]*src[1][2] - src[1][0]*src[0][2]) * det; 820 821 m[2][0] = (src[1][0]*src[2][1] - src[2][0]*src[1][1]) * det; 822 m[2][1] = -(src[0][0]*src[2][1] - src[2][0]*src[0][1]) * det; 823 m[2][2] = (src[0][0]*src[1][1] - src[1][0]*src[0][1]) * det; 824 825 826 // compute (invA)*(-C) 827 m[0][3] = -m[0][0]*src[0][3] - m[0][1]*src[1][3] - m[0][2]*src[2][3]; 828 m[1][3] = -m[1][0]*src[0][3] - m[1][1]*src[1][3] - m[1][2]*src[2][3]; 829 m[2][3] = -m[2][0]*src[0][3] - m[2][1]*src[1][3] - m[2][2]*src[2][3]; 830 831 // copy back if needed 832 if ( m == mTmp.m ) 833 { 834 MTX34Copy( pOut, &mTmp ); 835 } 836 837 return 1; 838} 839 840 841/* 842 843 844 845 846 847 848 */ 849NN_MATH_INLINE MTX34* 850MTX34TransposeC(MTX34* pOut, const MTX34* p) 851{ 852 MTX34 mTmp; 853 854 NN_NULL_ASSERT( p ); 855 NN_NULL_ASSERT( pOut ); 856 857 const f32 (*const src)[4] = p->m; 858 f32 (*m)[4]; 859 860 if (p == pOut) 861 { 862 m = mTmp.m; 863 } 864 else 865 { 866 m = pOut->m; 867 } 868 869 m[0][0] = src[0][0]; m[0][1] = src[1][0]; m[0][2] = src[2][0]; m[0][3] = 0.0f; 870 m[1][0] = src[0][1]; m[1][1] = src[1][1]; m[1][2] = src[2][1]; m[1][3] = 0.0f; 871 m[2][0] = src[0][2]; m[2][1] = src[1][2]; m[2][2] = src[2][2]; m[2][3] = 0.0f; 872 873 // copy back if needed 874 if ( m == mTmp.m ) 875 { 876 MTX34Copy( pOut, &mTmp ); 877 } 878 879 return pOut; 880} 881 882 883/* 884 885 886 887 888 889 890 */ 891NN_MATH_INLINE u32 892MTX34InvTransposeC(MTX34* pOut, const MTX34* __restrict p) 893{ 894 MTX34 mTmp; 895 f32 (*m)[4]; 896 f32 det; 897 898 NN_NULL_ASSERT(p); 899 NN_NULL_ASSERT(pOut); 900 901 f32 (*const invX)[4] = pOut->m; 902 const f32 (*const src)[4] = p->m; 903 904 if ( p == pOut ) 905 { 906 m = mTmp.m; 907 } 908 else 909 { 910 m = invX; 911 } 912 913 // compute the determinant of the upper 3x3 submatrix 914 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] 915 - 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]; 916 917 // check if matrix is singular 918 if ( det == 0.0f ) 919 { 920 return 0; 921 } 922 923 // compute the inverse-transpose of the upper submatrix: 924 925 // find the transposed matrix of cofactors of the upper submatrix 926 // and multiply by (1/det) 927 928 det = 1.0f / det; 929 930 m[0][0] = (src[1][1]*src[2][2] - src[2][1]*src[1][2]) * det; 931 m[0][1] = -(src[1][0]*src[2][2] - src[2][0]*src[1][2]) * det; 932 m[0][2] = (src[1][0]*src[2][1] - src[2][0]*src[1][1]) * det; 933 934 m[1][0] = -(src[0][1]*src[2][2] - src[2][1]*src[0][2]) * det; 935 m[1][1] = (src[0][0]*src[2][2] - src[2][0]*src[0][2]) * det; 936 m[1][2] = -(src[0][0]*src[2][1] - src[2][0]*src[0][1]) * det; 937 938 m[2][0] = (src[0][1]*src[1][2] - src[1][1]*src[0][2]) * det; 939 m[2][1] = -(src[0][0]*src[1][2] - src[1][0]*src[0][2]) * det; 940 m[2][2] = (src[0][0]*src[1][1] - src[1][0]*src[0][1]) * det; 941 942 943 // the fourth columns should be all zero 944 m[0][3] = 0.0F; 945 m[1][3] = 0.0F; 946 m[2][3] = 0.0F; 947 948 // copy back if needed 949 if ( m == mTmp.m ) 950 { 951 MTX34Copy( pOut, &mTmp ); 952 } 953 954 return 1; 955} 956 957 958/* 959 960 961 962 963 964 965 966 967 */ 968NN_MATH_INLINE MTX34* 969MTX34LookAtC(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamUp, const VEC3* pTarget) 970{ 971 NN_NULL_ASSERT(pOut); 972 NN_NULL_ASSERT(pCamPos); 973 NN_NULL_ASSERT(pCamUp); 974 NN_NULL_ASSERT(pTarget); 975 976 f32 (*const m)[4] = pOut->m; 977 978 // compute unit target vector 979 // use negative value to look down (-Z) axis 980 VEC3 vLook; 981 VEC3Sub(&vLook, pCamPos, pTarget); 982 VEC3Normalize(&vLook, &vLook); 983 984 // vRight = pCamUp x vLook 985 VEC3 vRight; 986 VEC3Cross(&vRight, pCamUp, &vLook); 987 VEC3Normalize(&vRight, &vRight); 988 989 // vUp = vLook x vRight 990 VEC3 vUp; 991 VEC3Cross(&vUp, &vLook, &vRight); 992 // Don't need to normalize vUp since it should already be unit length 993 // VECNormalize( &vUp, &vUp ); 994 995 m[0][0] = vRight.x; 996 m[0][1] = vRight.y; 997 m[0][2] = vRight.z; 998 m[0][3] = -( pCamPos->x * vRight.x + pCamPos->y * vRight.y + pCamPos->z * vRight.z ); 999 1000 m[1][0] = vUp.x; 1001 m[1][1] = vUp.y; 1002 m[1][2] = vUp.z; 1003 m[1][3] = -( pCamPos->x * vUp.x + pCamPos->y * vUp.y + pCamPos->z * vUp.z ); 1004 1005 m[2][0] = vLook.x; 1006 m[2][1] = vLook.y; 1007 m[2][2] = vLook.z; 1008 m[2][3] = -( pCamPos->x * vLook.x + pCamPos->y * vLook.y + pCamPos->z * vLook.z ); 1009 1010 return pOut; 1011} 1012NN_MATH_INLINE MTX34* 1013MTX34LookAtC_FAST(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamUp, const VEC3* pTarget) 1014{ 1015 NN_NULL_ASSERT(pOut); 1016 NN_NULL_ASSERT(pCamPos); 1017 NN_NULL_ASSERT(pCamUp); 1018 NN_NULL_ASSERT(pTarget); 1019 1020 f32 (*const m)[4] = pOut->m; 1021 1022 // compute unit target vector 1023 // use negative value to look down (-Z) axis 1024 f32 vLookx, vLooky, vLookz; 1025 { 1026 vLookx = pCamPos->x - pTarget->x; 1027 vLooky = pCamPos->y - pTarget->y; 1028 vLookz = pCamPos->z - pTarget->z; 1029 } 1030 1031 // vLook Normalize 1032 { 1033 f32 mag = (vLookx * vLookx) + (vLooky * vLooky) + (vLookz * vLookz); 1034 1035 NN_ASSERTMSG(mag != 0, "MATHNormalize3(): zero magnitude vector"); 1036 1037 mag = 1.0f / ::std::sqrtf(mag); 1038 1039 1040 vLookx = vLookx * mag; 1041 vLooky = vLooky * mag; 1042 vLookz = vLookz * mag; 1043 } 1044 1045 // vRight = pCamUp x vLook 1046 f32 vRightx, vRighty, vRightz; 1047 { 1048 vRightx = ( pCamUp->y * vLookz ) - ( pCamUp->z * vLooky ); 1049 vRighty = ( pCamUp->z * vLookx ) - ( pCamUp->x * vLookz ); 1050 vRightz = ( pCamUp->x * vLooky ) - ( pCamUp->y * vLookx ); 1051 } 1052 1053 // vRight Normalize 1054 { 1055 f32 mag = (vRightx * vRightx) + (vRighty * vRighty) + (vRightz * vRightz); 1056 1057 NN_ASSERTMSG(mag != 0, "MATHNormalize3(): zero magnitude vector"); 1058 1059 mag = 1.0f / ::std::sqrtf(mag); 1060 1061 vRightx = vRightx * mag; 1062 vRighty = vRighty * mag; 1063 vRightz = vRightz * mag; 1064 } 1065 1066 // vUp = vLook x vRight 1067 f32 vUpx, vUpy, vUpz; 1068 { 1069 vUpx = ( vLooky * vRightz ) - ( vLookz * vRighty ); 1070 vUpy = ( vLookz * vRightx ) - ( vLookx * vRightz ); 1071 vUpz = ( vLookx * vRighty ) - ( vLooky * vRightx ); 1072 } 1073 1074 1075 // Don't need to normalize vUp since it should already be unit length 1076 // VECNormalize( &vUp, &vUp ); 1077 f32 tmp1, tmp2, tmp3; 1078 1079 tmp1 = -( pCamPos->x * vRightx + pCamPos->y * vRighty + pCamPos->z * vRightz ); 1080 tmp2 = -( pCamPos->x * vUpx + pCamPos->y * vUpy + pCamPos->z * vUpz ); 1081 tmp3 = -( pCamPos->x * vLookx + pCamPos->y * vLooky + pCamPos->z * vLookz ); 1082 1083 m[0][0] = vRightx; 1084 m[0][1] = vRighty; 1085 m[0][2] = vRightz; 1086 1087 m[1][0] = vUpx; 1088 m[1][1] = vUpy; 1089 m[1][2] = vUpz; 1090 1091 m[2][0] = vLookx; 1092 m[2][1] = vLooky; 1093 m[2][2] = vLookz; 1094 1095 m[0][3] = tmp1; 1096 m[1][3] = tmp2; 1097 m[2][3] = tmp3; 1098 1099 return pOut; 1100} 1101 1102/* 1103 1104 1105 1106 1107 1108 1109 1110 1111 */ 1112NN_MATH_INLINE MTX34* 1113MTX34LookAtC(MTX34* pOut, const VEC3* pCamPos, f32 twist, const VEC3* pTarget) 1114{ 1115 NN_NULL_ASSERT(pOut); 1116 NN_NULL_ASSERT(pCamPos); 1117 NN_NULL_ASSERT(pTarget); 1118 1119 f32 (*const m)[4] = pOut->m; 1120 1121 // Z-direction for camera coordinate system 1122 VEC3 lookReverse(pCamPos->x - pTarget->x, pCamPos->y - pTarget->y, pCamPos->z - pTarget->z); 1123 1124 if ((lookReverse.x == 0.0f) && (lookReverse.z == 0.0f)) 1125 { 1126 // Twist is not defined if the xz-coordinates of the camera and the target are the same. 1127 m[0][0] = 1.0f; 1128 m[0][1] = 0.0f; 1129 m[0][2] = 0.0f; 1130 m[0][3] = -pCamPos->x; 1131 1132 m[1][0] = 0.0f; 1133 m[1][1] = 0.0f; 1134 1135 m[2][0] = 0.0f; 1136 m[2][2] = 0.0f; 1137 1138 if (lookReverse.y <= 0.0f) 1139 { 1140 // When looking straight up 1141 m[1][2] = 1.0f; 1142 m[1][3] = -pCamPos->z; 1143 1144 m[2][1] = -1.0f; 1145 m[2][3] = pCamPos->y; 1146 } 1147 else 1148 { 1149 // When looking straight down 1150 m[1][2] = -1.0f; 1151 m[1][3] = pCamPos->z; 1152 1153 m[2][1] = 1.0f; 1154 m[2][3] = -pCamPos->y; 1155 } 1156 } 1157 else 1158 { 1159 // X-direction for camera coordinate system 1160 VEC3 r(lookReverse.z, 0.0f, -lookReverse.x); 1161 1162 VEC3Normalize(&lookReverse, &lookReverse); 1163 VEC3Normalize(&r, &r); 1164 1165 // Y-direction for camera coordinate system 1166 VEC3 u; 1167 VEC3Cross(&u, &lookReverse, &r); 1168 1169 f32 st, ct; 1170 SinCosDeg(&st, &ct, twist); 1171 VEC3 right, up; 1172 1173 // Get 'right' by rotating the r-axis and u-axis cameraTwist values a half rotation clockwise on the ru-plane 1174 // Note that r.y == 0. 1175 right.x = st * u.x + ct * r.x; 1176 right.y = st * u.y; 1177 right.z = st * u.z + ct * r.z; 1178 1179 up.x = ct * u.x - st * r.x; 1180 up.y = ct * u.y; 1181 up.z = ct * u.z - st * r.z; 1182 1183 // Right 1184 m[0][0] = right.x; 1185 m[0][1] = right.y; 1186 m[0][2] = right.z; 1187 m[0][3] = -VEC3Dot(pCamPos, &right); 1188 1189 // Up 1190 m[1][0] = up.x; 1191 m[1][1] = up.y; 1192 m[1][2] = up.z; 1193 m[1][3] = -VEC3Dot(pCamPos, &up); 1194 1195 // look 1196 m[2][0] = lookReverse.x; 1197 m[2][1] = lookReverse.y; 1198 m[2][2] = lookReverse.z; 1199 m[2][3] = -VEC3Dot(pCamPos, &lookReverse); 1200 } 1201 1202 return pOut; 1203} 1204NN_MATH_INLINE MTX34* 1205MTX34LookAtC_FAST(MTX34* pOut, const VEC3* pCamPos, f32 twist, const VEC3* pTarget) 1206{ 1207 NN_NULL_ASSERT(pOut); 1208 NN_NULL_ASSERT(pCamPos); 1209 NN_NULL_ASSERT(pTarget); 1210 1211 f32 (*const m)[4] = pOut->m; 1212 1213 // Z-direction for camera coordinate system 1214 f32 lookReversex, lookReversey, lookReversez; 1215 1216 lookReversex = pCamPos->x - pTarget->x; 1217 lookReversez = pCamPos->z - pTarget->z; 1218 lookReversey = pCamPos->y - pTarget->y; 1219 1220 if ((lookReversex == 0.0f) && (lookReversez == 0.0f)) 1221 { 1222 // Twist is not defined if the xz-coordinates of the camera and the target are the same. 1223 m[0][0] = 1.0f; 1224 m[0][1] = 0.0f; 1225 m[0][2] = 0.0f; 1226 m[0][3] = -pCamPos->x; 1227 1228 m[1][0] = 0.0f; 1229 m[1][1] = 0.0f; 1230 1231 m[2][0] = 0.0f; 1232 m[2][2] = 0.0f; 1233 1234 if (lookReversey <= 0.0f) 1235 { 1236 // When looking straight up 1237 m[1][2] = 1.0f; 1238 m[1][3] = -pCamPos->z; 1239 1240 m[2][1] = -1.0f; 1241 m[2][3] = pCamPos->y; 1242 } 1243 else 1244 { 1245 // When looking straight down 1246 m[1][2] = -1.0f; 1247 m[1][3] = pCamPos->z; 1248 1249 m[2][1] = 1.0f; 1250 m[2][3] = -pCamPos->y; 1251 } 1252 } 1253 else 1254 { 1255 // X-direction for camera coordinate system 1256 f32 rx, ry, rz; 1257 f32 mag1, mag2; 1258 f32 ux, uy, uz; 1259 f32 fidx; 1260 1261 mag1 = (lookReversez * lookReversez) + (-lookReversex * -lookReversex); 1262 mag2 = (lookReversex * lookReversex) + (lookReversey * lookReversey) + (lookReversez * lookReversez); 1263 NN_ASSERTMSG(mag1 != 0, "MATHNormalize3(): zero magnitude vector"); 1264 NN_ASSERTMSG(mag2 != 0, "MATHNormalize3(): zero magnitude vector"); 1265 mag1 = ::std::sqrtf(mag1); 1266 mag2 = ::std::sqrtf(mag2); 1267 fidx = NN_MATH_DEG_TO_FIDX(twist); 1268 mag1 = 1.0f / mag1; 1269 mag2 = 1.0f / mag2; 1270 1271 // r Normalize 1272 { 1273 rx = lookReversez * mag1; 1274 ry = 0.0f; 1275 rz = -lookReversex * mag1; 1276 } 1277 // lookReverse Normalize 1278 { 1279 lookReversex = lookReversex * mag2; 1280 lookReversey = lookReversey * mag2; 1281 lookReversez = lookReversez * mag2; 1282 } 1283 1284 // Y-direction for camera coordinate system 1285 { 1286 ux = ( lookReversey * rz ) - ( lookReversez * ry ); 1287 uy = ( lookReversez * rx ) - ( lookReversex * rz ); 1288 uz = ( lookReversex * ry ) - ( lookReversey * rx ); 1289 } 1290 1291 1292 f32 st, ct; 1293// SinCosDeg(&st, &ct, twist); 1294 SinCosFIdx(&st, &ct, fidx); 1295 f32 rightx, righty, rightz; 1296 f32 upx, upy, upz; 1297 1298 // Get 'right' by rotating the r-axis and u-axis cameraTwist values a half rotation clockwise on the ru-plane 1299 // Note that r.y == 0. 1300 rightx = st * ux + ct * rx; 1301 righty = st * uy; 1302 rightz = st * uz + ct * rz; 1303 1304 upx = ct * ux - st * rx; 1305 upy = ct * uy; 1306 upz = ct * uz - st * rz; 1307 1308 // Right 1309 f32 tmp1, tmp2, tmp3; 1310 1311 tmp1 = pCamPos->x * rightx + pCamPos->y * righty + pCamPos->z * rightz; 1312 tmp2 = pCamPos->x * upx + pCamPos->y * upy + pCamPos->z * upz; 1313 tmp3 = pCamPos->x * lookReversex + pCamPos->y * lookReversey + pCamPos->z * lookReversez; 1314 1315 m[0][0] = rightx; 1316 m[0][1] = righty; 1317 m[0][2] = rightz; 1318 m[0][3] = -tmp1; 1319 1320 // Up 1321 m[1][0] = upx; 1322 m[1][1] = upy; 1323 m[1][2] = upz; 1324 m[1][3] = -tmp2; 1325 1326 // look 1327 m[2][0] = lookReversex; 1328 m[2][1] = lookReversey; 1329 m[2][2] = lookReversez; 1330 m[2][3] = -tmp3; 1331 } 1332 1333 return pOut; 1334} 1335 1336/* 1337 1338 1339 1340 1341 1342 1343 1344 */ 1345NN_MATH_INLINE MTX34* 1346MTX34CameraRotateC(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamRotate) 1347{ 1348 NN_NULL_ASSERT(pOut); 1349 NN_NULL_ASSERT(pCamPos); 1350 NN_NULL_ASSERT(pCamRotate); 1351 1352 f32 (*const m)[4] = pOut->m; 1353 1354 f32 sx, sy, sz, cx, cy, cz; 1355 SinCosDeg(&sx, &cx, pCamRotate->x); 1356 SinCosDeg(&sy, &cy, pCamRotate->y); 1357 SinCosDeg(&sz, &cz, pCamRotate->z); 1358 1359 // Rotate in the order of z-axis, x-axis, y-axis 1360 VEC3 right, up, back; 1361 1362 right.x = sx * sy * sz + cy * cz; 1363 right.y = cx * sz; 1364 right.z = sx * cy * sz - sy * cz; 1365 1366 up.x = sx * sy * cz - cy * sz; 1367 up.y = cx * cz; 1368 up.z = sx * cy * cz + sy * sz; 1369 1370 back.x = cx * sy; 1371 back.y = - sx; 1372 back.z = cx * cy; 1373 1374 m[0][0] = right.x; 1375 m[0][1] = right.y; 1376 m[0][2] = right.z; 1377 m[0][3] = -VEC3Dot(pCamPos, &right); 1378 1379 m[1][0] = up.x; 1380 m[1][1] = up.y; 1381 m[1][2] = up.z; 1382 m[1][3] = -VEC3Dot(pCamPos, &up); 1383 1384 m[2][0] = back.x; 1385 m[2][1] = back.y; 1386 m[2][2] = back.z; 1387 m[2][3] = -VEC3Dot(pCamPos, &back); 1388 1389 return pOut; 1390} 1391NN_MATH_INLINE MTX34* 1392MTX34CameraRotateC_FAST(MTX34* pOut, const VEC3* pCamPos, const VEC3* pCamRotate) 1393{ 1394 NN_NULL_ASSERT(pOut); 1395 NN_NULL_ASSERT(pCamPos); 1396 NN_NULL_ASSERT(pCamRotate); 1397 1398 f32 (*const m)[4] = pOut->m; 1399 1400 f32 sinx, cosx; 1401 f32 siny, cosy; 1402 f32 sinz, cosz; 1403 f32 fIdxX = NN_MATH_DEG_TO_FIDX(pCamRotate->x); 1404 f32 fIdxY = NN_MATH_DEG_TO_FIDX(pCamRotate->y); 1405 f32 fIdxZ = NN_MATH_DEG_TO_FIDX(pCamRotate->z); 1406 1407 { 1408 1409 u16 idxx; 1410 f32 abs_fidxx; 1411 f32 rx; 1412 1413 u16 idxy; 1414 f32 abs_fidxy; 1415 f32 ry; 1416 1417 u16 idxz; 1418 f32 abs_fidxz; 1419 f32 rz; 1420 1421 int negx, negy, negz; 1422 1423 negx = (fIdxX < 0.0f) ? 1 : 0; 1424 abs_fidxx = FAbs(fIdxX); 1425 negy = (fIdxY < 0.0f) ? 1 : 0; 1426 abs_fidxy = FAbs(fIdxY); 1427 negz = (fIdxZ < 0.0f) ? 1 : 0; 1428 abs_fidxz = FAbs(fIdxZ); 1429 1430 1431 while ( abs_fidxx >= 65536.0f ) 1432 { 1433 abs_fidxx -= 65536.0f; 1434 } 1435 while ( abs_fidxy >= 65536.0f ) 1436 { 1437 abs_fidxy -= 65536.0f; 1438 } 1439 while ( abs_fidxz >= 65536.0f ) 1440 { 1441 abs_fidxz -= 65536.0f; 1442 } 1443 1444 idxx = F32ToU16(abs_fidxx); 1445 idxy = F32ToU16(abs_fidxy); 1446 idxz = F32ToU16(abs_fidxz); 1447 { 1448 f32 idxxf, idxyf, idxzf; 1449 1450 idxxf = U16ToF32(idxx); 1451 idxyf = U16ToF32(idxy); 1452 idxzf = U16ToF32(idxz); 1453 1454 rx = abs_fidxx - idxxf; 1455 ry = abs_fidxy - idxyf; 1456 rz = abs_fidxz - idxzf; 1457 1458 1459 } 1460 1461 idxx &= 0xff; 1462 idxy &= 0xff; 1463 idxz &= 0xff; 1464 1465 { 1466 f32 sinx_val, sinx_delta, cosx_val, cosx_delta; 1467 f32 siny_val, siny_delta, cosy_val, cosy_delta; 1468 f32 sinz_val, sinz_delta, cosz_val, cosz_delta; 1469 1470 sinx_val = internal::gSinCosTbl[idxx].sin_val; 1471 cosx_val = internal::gSinCosTbl[idxx].cos_val; 1472 sinx_delta = internal::gSinCosTbl[idxx].sin_delta; 1473 cosx_delta = internal::gSinCosTbl[idxx].cos_delta; 1474 1475 sinx = sinx_val + rx * sinx_delta; 1476 cosx = cosx_val + rx * cosx_delta; 1477 1478 siny_val = internal::gSinCosTbl[idxy].sin_val; 1479 cosy_val = internal::gSinCosTbl[idxy].cos_val; 1480 siny_delta = internal::gSinCosTbl[idxy].sin_delta; 1481 cosy_delta = internal::gSinCosTbl[idxy].cos_delta; 1482 1483 siny = siny_val + ry * siny_delta; 1484 cosy = cosy_val + ry * cosy_delta; 1485 1486 sinz_val = internal::gSinCosTbl[idxz].sin_val; 1487 cosz_val = internal::gSinCosTbl[idxz].cos_val; 1488 sinz_delta = internal::gSinCosTbl[idxz].sin_delta; 1489 cosz_delta = internal::gSinCosTbl[idxz].cos_delta; 1490 1491 sinz = sinz_val + rz * sinz_delta; 1492 cosz = cosz_val + rz * cosz_delta; 1493 1494 } 1495 1496 sinx = (negx) ? -sinx : sinx; 1497 siny = (negy) ? -siny : siny; 1498 sinz = (negz) ? -sinz : sinz; 1499 1500 } 1501 1502 1503 1504 1505 // Rotate in the order of z-axis, x-axis, y-axis 1506 register VEC3 right, up, back; 1507 register f32 m00, m01, m02, m03; 1508 register f32 m10, m11, m12, m13; 1509 register f32 m20, m21, m22, m23; 1510 1511 1512 1513 right.x = sinx * siny * sinz + cosy * cosz; 1514 right.y = cosx * sinz; 1515 right.z = sinx * cosy * sinz - siny * cosz; 1516 1517 up.x = sinx * siny * cosz - cosy * sinz; 1518 up.y = cosx * cosz; 1519 up.z = sinx * cosy * cosz + siny * sinz; 1520 1521 back.x = cosx * siny; 1522 back.y = - sinx; 1523 back.z = cosx * cosy; 1524 1525 m00 = right.x; 1526 m01 = right.y; 1527 m02 = right.z; 1528 m03 = -VEC3Dot(pCamPos, &right); 1529 1530 m10 = up.x; 1531 m11 = up.y; 1532 m12 = up.z; 1533 m13 = -VEC3Dot(pCamPos, &up); 1534 1535 m20 = back.x; 1536 m21 = back.y; 1537 m22 = back.z; 1538 m23 = -VEC3Dot(pCamPos, &back); 1539 1540 m[0][0] = m00; 1541 m[0][1] = m01; 1542 m[0][2] = m02; 1543 m[0][3] = m03; 1544 1545 m[1][0] = m10; 1546 m[1][1] = m11; 1547 m[1][2] = m12; 1548 m[1][3] = m13; 1549 1550 m[2][0] = m20; 1551 m[2][1] = m21; 1552 m[2][2] = m22; 1553 m[2][3] = m23; 1554 1555 return pOut; 1556} 1557 1558/* 1559 1560*/ 1561/* Please see man pages for details 1562 1563 1564*/ 1565 1566/* 1567 1568 1569 1570 1571 1572 1573 */ 1574NN_MATH_INLINE MTX34* 1575QUATToMTX34C(MTX34* pOut, const QUAT* pQ, bool isChangeTrans) 1576{ 1577 f32 s, xs, ys, zs; 1578 f32 wx, wy, wz, xx, xy, xz, yy, yz, zz; 1579 1580 NN_NULL_ASSERT(pOut); 1581 NN_NULL_ASSERT(pQ); 1582 NN_ASSERT( pQ->x || pQ->y || pQ->z || pQ->w ); 1583 1584 f32 (*const m)[4] = pOut->m; 1585 1586 s = 2.0f / ( (pQ->x * pQ->x) + (pQ->y * pQ->y) + (pQ->z * pQ->z) + (pQ->w * pQ->w) ); 1587 1588 xs = pQ->x * s; ys = pQ->y * s; zs = pQ->z * s; 1589 wx = pQ->w * xs; wy = pQ->w * ys; wz = pQ->w * zs; 1590 xx = pQ->x * xs; xy = pQ->x * ys; xz = pQ->x * zs; 1591 yy = pQ->y * ys; yz = pQ->y * zs; zz = pQ->z * zs; 1592 1593 m[0][0] = 1.0f - (yy + zz); 1594 m[0][1] = xy - wz; 1595 m[0][2] = xz + wy; 1596 if ( isChangeTrans ) m[0][3] = 0.0f; 1597 1598 m[1][0] = xy + wz; 1599 m[1][1] = 1.0f - (xx + zz); 1600 m[1][2] = yz - wx; 1601 if ( isChangeTrans ) m[1][3] = 0.0f; 1602 1603 m[2][0] = xz - wy; 1604 m[2][1] = yz + wx; 1605 m[2][2] = 1.0f - (xx + yy); 1606 if ( isChangeTrans ) m[2][3] = 0.0f; 1607 1608 return pOut; 1609} 1610NN_MATH_INLINE MTX34* 1611QUATToMTX34C_FAST(MTX34* pOut, const QUAT* pQ, bool isChangeTrans) 1612{ 1613 f32 s, xs, ys, zs; 1614 f32 wx, wy, wz, xx, xy, xz, yy, yz, zz; 1615 f32 m00, m01, m02, m10, m11, m12, m20, m21, m22; 1616 f32 pQx, pQy, pQz, pQw; 1617 1618 NN_NULL_ASSERT(pOut); 1619 NN_NULL_ASSERT(pQ); 1620 NN_ASSERT( pQ->x || pQ->y || pQ->z || pQ->w ); 1621 1622 f32 (*const m)[4] = pOut->m; 1623 1624 pQx = pQ->x; 1625 pQy = pQ->y; 1626 pQz = pQ->z; 1627 pQw = pQ->w; 1628 1629 s = 2.0f / ( (pQx * pQx) + (pQy * pQy) + (pQz * pQz) + (pQw * pQw) ); 1630 1631 xs = pQx * s; ys = pQy * s; zs = pQz * s; 1632 wx = pQw * xs; wy = pQw * ys; wz = pQw * zs; 1633 xx = pQx * xs; xy = pQx * ys; xz = pQx * zs; 1634 yy = pQy * ys; yz = pQy * zs; zz = pQz * zs; 1635 1636 m00 = 1.0f - (yy + zz); 1637 m01 = xy - wz; 1638 m02 = xz + wy; 1639 1640 m10 = xy + wz; 1641 m11 = 1.0f - (xx + zz); 1642 m12 = yz - wx; 1643 1644 m20 = xz - wy; 1645 m21 = yz + wx; 1646 m22 = 1.0f - (xx + yy); 1647 1648 if ( isChangeTrans ) 1649 { 1650 m[0][3] = 0.0f; 1651 m[1][3] = 0.0f; 1652 m[2][3] = 0.0f; 1653 } 1654 1655 m[0][0] = m00; 1656 m[0][1] = m01; 1657 m[0][2] = m02; 1658 1659 m[1][0] = m10; 1660 m[1][1] = m11; 1661 m[1][2] = m12; 1662 1663 m[2][0] = m20; 1664 m[2][1] = m21; 1665 m[2][2] = m22; 1666 1667 1668 return pOut; 1669} 1670 1671/* 1672 1673*/ 1674 1675} // namespace ARMv6 1676} // namespace math 1677} // namespace nn 1678