1/*---------------------------------------------------------------------------* 2 Project: Horizon 3 File: math_Matrix44.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: 13623 $ 14 *---------------------------------------------------------------------------*/ 15 16#include <cmath> 17#include <nn/math/math_Vector3.h> 18 19namespace nn { 20namespace math { 21namespace ARMv6 { 22/*! 23 @name ユーティリティ 24 @{ 25*/ 26 27/*!--------------------------------------------------------------------------* 28 @brief ベクトルを行列で変換します。ベクトルの 4 要素目を 1 として計算します。 29 30 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pV と同じベクトルを指していても構いません。 31 @param[in] pM 変換行列へのポインタ。 32 @param[in] pV 元となるベクトルへのポインタ。 33 34 @return pOut を返します。 35 *---------------------------------------------------------------------------*/ 36NN_MATH_INLINE VEC4* 37VEC3TransformC(VEC4* pOut, const MTX44* pM, const VEC3* pV) 38{ 39 VEC4 tmp; 40 tmp.x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z + pM->f._03; 41 tmp.y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z + pM->f._13; 42 tmp.z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z + pM->f._23; 43 tmp.w = pM->f._30 * pV->x + pM->f._31 * pV->y + pM->f._32 * pV->z + pM->f._33; 44 45 pOut->x = tmp.x; 46 pOut->y = tmp.y; 47 pOut->z = tmp.z; 48 pOut->w = tmp.w; 49 50 return pOut; 51} 52 53/*! 54 @} 55*/ 56 57/* ------------------------------------------------------------------------ 58 MTX44 59 ------------------------------------------------------------------------ */ 60 61/*! 62 @name 行列 63 @{ 64*/ 65 66 67 68/*!--------------------------------------------------------------------------* 69 @brief 行列をコピーします。 70 71 @param[out] pOut コピー先の行列へのポインタ。 72 @param[in] p コピー元の行列へのポインタ 73 74 @return pOut を返します。 75 *---------------------------------------------------------------------------*/ 76NN_MATH_INLINE MTX44* 77MTX44CopyC(MTX44* pOut, const MTX44* p) 78{ 79 if (pOut != p) 80 { 81 *pOut = *p; 82 } 83 84 return pOut; 85} 86 87/*!--------------------------------------------------------------------------* 88 @brief 行列の和を計算します。 89 90 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p1, p2 と同じ行列を指していても構いません。 91 @param[in] p1 左辺値へのポインタ。 92 @param[in] p2 右辺値へのポインタ。 93 94 @return pOut を返します。 95 *---------------------------------------------------------------------------*/ 96NN_MATH_INLINE MTX44* 97MTX44AddC(MTX44* pOut, const MTX44* p1, const MTX44* p2) 98{ 99 pOut->f._00 = p1->f._00 + p2->f._00; 100 pOut->f._01 = p1->f._01 + p2->f._01; 101 pOut->f._02 = p1->f._02 + p2->f._02; 102 pOut->f._03 = p1->f._03 + p2->f._03; 103 104 pOut->f._10 = p1->f._10 + p2->f._10; 105 pOut->f._11 = p1->f._11 + p2->f._11; 106 pOut->f._12 = p1->f._12 + p2->f._12; 107 pOut->f._13 = p1->f._13 + p2->f._13; 108 109 pOut->f._20 = p1->f._20 + p2->f._20; 110 pOut->f._21 = p1->f._21 + p2->f._21; 111 pOut->f._22 = p1->f._22 + p2->f._22; 112 pOut->f._23 = p1->f._23 + p2->f._23; 113 114 pOut->f._30 = p1->f._30 + p2->f._30; 115 pOut->f._31 = p1->f._31 + p2->f._31; 116 pOut->f._32 = p1->f._32 + p2->f._32; 117 pOut->f._33 = p1->f._33 + p2->f._33; 118 119 return pOut; 120} 121 122 123 124/*!--------------------------------------------------------------------------* 125 @brief 行列のスカラー積を計算します。 126 127 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p と同じ行列を指していても構いません。 128 @param[in] p 元の行列のポインタ。 129 @param[in] f 掛ける数 130 131 @return pOut を返します。 132 *---------------------------------------------------------------------------*/ 133NN_MATH_INLINE MTX44* 134MTX44MultC(MTX44* pOut, const MTX44* p, f32 f) 135{ 136 pOut->f._00 = p->f._00 * f; 137 pOut->f._01 = p->f._01 * f; 138 pOut->f._02 = p->f._02 * f; 139 pOut->f._03 = p->f._03 * f; 140 141 pOut->f._10 = p->f._10 * f; 142 pOut->f._11 = p->f._11 * f; 143 pOut->f._12 = p->f._12 * f; 144 pOut->f._13 = p->f._13 * f; 145 146 pOut->f._20 = p->f._20 * f; 147 pOut->f._21 = p->f._21 * f; 148 pOut->f._22 = p->f._22 * f; 149 pOut->f._23 = p->f._23 * f; 150 151 pOut->f._30 = p->f._30 * f; 152 pOut->f._31 = p->f._31 * f; 153 pOut->f._32 = p->f._32 * f; 154 pOut->f._33 = p->f._33 * f; 155 156 return pOut; 157} 158 159 160/*!--------------------------------------------------------------------------* 161 @brief 行列の転置を取得します。 162 163 @param[in] pOut 計算結果を受け取るバッファへのポインタ。pSrcと同じ行列を指していても構いません。 164 @param[in] pSrc 元となる行列へのポインタ。 165 166 @return pOut を返します。 167 *---------------------------------------------------------------------------*/ 168NN_MATH_INLINE MTX44* 169MTX44TransposeC(MTX44* pOut, const MTX44 *pSrc) 170{ 171 MTX44 tmp; 172 const MTX44 *pMtx; 173 174 if (pOut != pSrc) 175 { 176 pMtx = pSrc; 177 pOut->f._00 = pSrc->f._00; 178 pOut->f._11 = pSrc->f._11; 179 pOut->f._22 = pSrc->f._22; 180 pOut->f._33 = pSrc->f._33; 181 } 182 else 183 { 184 pMtx = &tmp; 185 tmp.f._01 = pSrc->f._01; 186 tmp.f._02 = pSrc->f._02; 187 tmp.f._03 = pSrc->f._03; 188 tmp.f._12 = pSrc->f._12; 189 tmp.f._13 = pSrc->f._13; 190 tmp.f._23 = pSrc->f._23; 191 } 192 193 pOut->f._01 = pSrc->f._10; 194 pOut->f._02 = pSrc->f._20; 195 pOut->f._03 = pSrc->f._30; 196 pOut->f._12 = pSrc->f._21; 197 pOut->f._13 = pSrc->f._31; 198 pOut->f._23 = pSrc->f._32; 199 200 pOut->f._10 = pMtx->f._01; 201 pOut->f._20 = pMtx->f._02; 202 pOut->f._30 = pMtx->f._03; 203 pOut->f._21 = pMtx->f._12; 204 pOut->f._31 = pMtx->f._13; 205 pOut->f._32 = pMtx->f._23; 206 207 return pOut; 208} 209 210 211/*!--------------------------------------------------------------------------* 212 @brief 射影行列を視野角とアスペクト比から作成します。 213 214 @param[out] pOut 射影行列を格納する行列へのポインタ。 215 @param[in] fovy 縦方向の視野角（Radian） 216 @param[in] aspect 視野のアスペクト比(幅/高さ) 217 @param[in] n ニアクリッピング面までの距離。 218 @param[in] f ファークリッピング面までの距離。 219 220 @return pOut を返します。 221 *---------------------------------------------------------------------------*/ 222NN_MATH_INLINE MTX44* 223MTX44PerspectiveRadC(MTX44* pOut, f32 fovy, f32 aspect, f32 n, f32 f) 224{ 225 NN_NULL_ASSERT(pOut); 226 227 f32 (*const m)[4] = pOut->m; 228 229 // find the cotangent of half the (YZ) field of view 230 231 const f32 angle = fovy * 0.5f; 232 233 const f32 cot = 1.0f / ::std::tanf(angle); 234 235 m[0][0] = cot / aspect; 236 m[0][1] = 0.0f; 237 m[0][2] = 0.0f; 238 m[0][3] = 0.0f; 239 240 m[1][0] = 0.0f; 241 m[1][1] = cot; 242 m[1][2] = 0.0f; 243 m[1][3] = 0.0f; 244 245 m[2][0] = 0.0f; 246 m[2][1] = 0.0f; 247 248 const f32 tmp = 1.0f / (f - n); 249 m[2][2] = f * tmp; 250 m[2][3] = f * n * tmp; 251 252 m[3][0] = 0.0f; 253 m[3][1] = 0.0f; 254 m[3][2] = -1.0f; 255 m[3][3] = 0.0f; 256 257 return pOut; 258} 259NN_MATH_INLINE MTX44* 260MTX44PerspectiveRadC_FAST(MTX44* pOut, f32 fovy, f32 aspect, f32 n, f32 f) 261{ 262 NN_NULL_ASSERT(pOut); 263 264 f32 (*const m)[4] = pOut->m; 265 266 // find the cotangent of half the (YZ) field of view 267 268 const f32 angle = fovy * 0.5f; 269#if (MTX44PERSPECTIVERAD_CONFIG == D_FAST_C_ALGO) 270 f32 sin, cos; 271 SinCosFIdx(&sin, &cos, NN_MATH_RAD_TO_FIDX(angle)); 272 const f32 cot = cos/sin; 273#else 274 const f32 cot = 1.0f / ::std::tanf(angle); 275#endif 276 const f32 tmp = 1.0f / (f - n); 277 278 register f32 m00, m11, m22, m23; 279 280 m00 = cot / aspect; 281 m11 = cot; 282 m22 = f * tmp; 283 m23 = f * n * tmp; 284 285 m[0][1] = 0.0f; 286 m[0][2] = 0.0f; 287 m[0][3] = 0.0f; 288 m[1][0] = 0.0f; 289 m[1][2] = 0.0f; 290 m[1][3] = 0.0f; 291 m[2][0] = 0.0f; 292 m[2][1] = 0.0f; 293 m[3][0] = 0.0f; 294 m[3][1] = 0.0f; 295 m[3][2] = -1.0f; 296 m[3][3] = 0.0f; 297 298 m[0][0] = m00; 299 m[1][1] = m11; 300 m[2][2] = m22; 301 m[2][3] = m23; 302 303 304 return pOut; 305} 306 307/*!--------------------------------------------------------------------------* 308 @brief 射影行列をニアクリッピング面での視錐台を元に作成します。 309 引数の順序が OpenGL 準拠になっているので注意。 310 311 @param[out] pOut 射影行列を格納する行列へのポインタ。 312 @param[in] l ニアクリッピング面での視錐台左辺の X 座標 313 @param[in] r ニアクリッピング面での視錐台右辺の X 座標 314 @param[in] b ニアクリッピング面での視錐台下辺の Y 座標 315 @param[in] t ニアクリッピング面での視錐台上辺の Y 座標 316 @param[in] n ニアクリッピング面までの距離。 317 @param[in] f ファークリッピング面までの距離。 318 319 @return pOut を返します。 320 *---------------------------------------------------------------------------*/ 321NN_MATH_INLINE MTX44* 322MTX44FrustumC(MTX44* pOut, f32 l, f32 r, f32 b, f32 t, f32 n, f32 f) 323{ 324 NN_NULL_ASSERT( pOut ); 325 326 // NOTE: Be careful about "l" vs. "1" below!!! 327 328 f32 (*const m)[4] = pOut->m; 329 f32 tmp = 1.0f / (r - l); 330 m[0][0] = (2*n) * tmp; 331 m[0][1] = 0.0f; 332 m[0][2] = (r + l) * tmp; 333 m[0][3] = 0.0f; 334 335 tmp = 1.0f / (t - b); 336 m[1][0] = 0.0f; 337 m[1][1] = (2*n) * tmp; 338 m[1][2] = (t + b) * tmp; 339 m[1][3] = 0.0f; 340 341 m[2][0] = 0.0f; 342 m[2][1] = 0.0f; 343 344 tmp = 1.0f / (f - n); 345 346 m[2][2] = f * tmp; 347 m[2][3] = f * n * tmp; 348 349 m[3][0] = 0.0f; 350 m[3][1] = 0.0f; 351 m[3][2] = -1.0f; 352 m[3][3] = 0.0f; 353 354 return pOut; 355} 356NN_MATH_INLINE MTX44* 357MTX44FrustumC_FAST(MTX44* pOut, f32 l, f32 r, f32 b, f32 t, f32 n, f32 f) 358{ 359 NN_NULL_ASSERT( pOut ); 360 361 // NOTE: Be careful about "l" vs. "1" below!!! 362 363 f32 (*const m)[4] = pOut->m; 364 f32 tmp1 = 1.0f / (r - l); 365 f32 tmp3 = 1.0f / (f - n); 366 f32 tmp2 = 1.0f / (t - b); 367 368 register f32 m00, m02, m11, m12, m22, m23; 369 370 m00 = (2*n) * tmp1; 371 m02 = (r + l) * tmp1; 372 373 m11 = (2*n) * tmp2; 374 m12 = (t + b) * tmp2; 375 376 m22 = f * tmp3; 377 m23 = f * n * tmp3; 378 379 m[0][1] = 0.0f; 380 m[0][3] = 0.0f; 381 382 m[1][0] = 0.0f; 383 m[1][3] = 0.0f; 384 385 m[2][0] = 0.0f; 386 m[2][1] = 0.0f; 387 388 m[3][0] = 0.0f; 389 m[3][1] = 0.0f; 390 m[3][2] = -1.0f; 391 m[3][3] = 0.0f; 392 393 m[0][0] = m00; 394 m[0][2] = m02; 395 396 m[1][1] = m11; 397 m[1][2] = m12; 398 399 m[2][2] = m22; 400 m[2][3] = m23; 401 402 return pOut; 403} 404 405 406/*!--------------------------------------------------------------------------* 407 @brief 正射影行列を作成します。 408 引数の順序が OpenGL 準拠になっているので注意。 409 410 @param[out] pOut 射影行列を格納する行列へのポインタ。 411 @param[in] l ニアクリッピング面での視錐台左辺の X 座標 412 @param[in] r ニアクリッピング面での視錐台右辺の X 座標 413 @param[in] b ニアクリッピング面での視錐台下辺の Y 座標 414 @param[in] t ニアクリッピング面での視錐台上辺の Y 座標 415 @param[in] n ニアクリッピング面までの距離。 416 @param[in] f ファークリッピング面までの距離。 417 418 @return pOut を返します。 419 *---------------------------------------------------------------------------*/ 420NN_MATH_INLINE MTX44* 421MTX44OrthoC(MTX44* pOut, f32 l, f32 r, f32 b, f32 t, f32 n, f32 f) 422{ 423 NN_NULL_ASSERT( pOut ); 424 425 // NOTE: Be careful about "l" vs. "1" below!!! 426 427 f32 (*const m)[4] = pOut->m; 428 f32 tmp = 1.0f / (r - l); 429 m[0][0] = 2.0f * tmp; 430 m[0][1] = 0.0f; 431 m[0][2] = 0.0f; 432 m[0][3] = -(r + l) * tmp; 433 434 tmp = 1.0f / (t - b); 435 m[1][0] = 0.0f; 436 m[1][1] = 2.0f * tmp; 437 m[1][2] = 0.0f; 438 m[1][3] = -(t + b) * tmp; 439 440 m[2][0] = 0.0f; 441 m[2][1] = 0.0f; 442 443 tmp = 1.0f / (f - n); 444 445 m[2][2] = tmp; 446 m[2][3] = n * tmp; 447 448 m[3][0] = 0.0f; 449 m[3][1] = 0.0f; 450 m[3][2] = 0.0f; 451 m[3][3] = 1.0f; 452 453 return pOut; 454} 455NN_MATH_INLINE MTX44* 456MTX44OrthoC_FAST(MTX44* pOut, f32 l, f32 r, f32 b, f32 t, f32 n, f32 f) 457{ 458 NN_NULL_ASSERT( pOut ); 459 460 // NOTE: Be careful about "l" vs. "1" below!!! 461 462 f32 (*const m)[4] = pOut->m; 463 register f32 tmp1 = 1.0f / (r - l); 464 register f32 tmp2 = 1.0f / (t - b); 465 register f32 tmp3 = 1.0f / (f - n); 466 register f32 m00, m03, m11, m13, m22, m23; 467 468 m00 = 2.0f * tmp1; 469 m03 = -(r + l) * tmp1; 470 471 m11 = 2.0f * tmp2; 472 m13 = -(t + b) * tmp2; 473 474 m22 = tmp3; 475 m23 = n * tmp3; 476 477 m[0][1] = 0.0f; 478 m[0][2] = 0.0f; 479 480 m[1][0] = 0.0f; 481 m[1][2] = 0.0f; 482 483 m[2][0] = 0.0f; 484 m[2][1] = 0.0f; 485 486 m[3][0] = 0.0f; 487 m[3][1] = 0.0f; 488 m[3][2] = 0.0f; 489 m[3][3] = 1.0f; 490 491 m[0][0] = m00; 492 m[0][3] = m03; 493 m[1][1] = m11; 494 m[1][3] = m13; 495 m[2][2] = m22; 496 m[2][3] = m23; 497 498 return pOut; 499} 500 501 502 /*---------------------------------------------------------------------------* 503 Description: 画面方向に向けて射影行列を回転します。 504 505 Arguments: pOut 回転をおこなう行列へのポインタ 506 pivot 507 508 Returns: 509 *---------------------------------------------------------------------------*/ 510 inline MTX44* 511 MTX44PivotC( MTX44* pOut, PivotDirection pivot ) 512 { 513 // TODO: 処理の最適化が必要。 514 515 const f32 PIVOT_ROTATION_SIN_COS[ PIVOT_NUM ][ 2 ] = 516 { 517 #ifdef NN_PLATFORM_CTR 518 { 0.0f, 1.0f }, // NONE 519 { -1.0f, 0.0f }, // TO_UP 520 { 0.0f, -1.0f }, // TO_RIGHT 521 { 1.0f, 0.0f }, // TO_BOTTOM 522 { 0.0f, 1.0f }, // TO_LEFT 523 #else 524 { 0.0f, 1.0f }, // NONE 525 { 0.0f, 1.0f }, // TO_UP 526 { -1.0f, 0.0f }, // TO_RIGHT 527 { 0.0f, -1.0f }, // TO_BOTTOM 528 { 1.0f, 0.0f }, // TO_LEFT 529 #endif 530 }; 531 532 if ( pivot == PIVOT_NONE ) 533 { 534 return pOut; 535 } 536 537 f32 sin = PIVOT_ROTATION_SIN_COS[ pivot ][ 0 ]; 538 f32 cos = PIVOT_ROTATION_SIN_COS[ pivot ][ 1 ]; 539 540 f32 (*const m)[4] = pOut->m; 541 542 if ( sin == 0.0f ) 543 { 544 m[0][0] = cos * m[0][0]; 545 m[0][1] = cos * m[0][1]; 546 m[0][2] = cos * m[0][2]; 547 m[0][3] = cos * m[0][3]; 548 549 m[1][0] = cos * m[1][0]; 550 m[1][1] = cos * m[1][1]; 551 m[1][2] = cos * m[1][2]; 552 m[1][3] = cos * m[1][3]; 553 } 554 else // if ( cos == 0.0f ) 555 { 556 f32 tmp = m[0][0]; 557 m[0][0] = -sin * m[1][0]; 558 m[1][0] = sin * tmp; 559 560 tmp = m[0][1]; 561 m[0][1] = -sin * m[1][1]; 562 m[1][1] = sin * tmp; 563 564 tmp = m[0][2]; 565 m[0][2] = -sin * m[1][2]; 566 m[1][2] = sin * tmp; 567 568 tmp = m[0][3]; 569 m[0][3] = -sin * m[1][3]; 570 m[1][3] = sin * tmp; 571 } 572 573 return pOut; 574 575 } 576 inline MTX44* 577 MTX44PivotC_FAST( MTX44* pOut, PivotDirection pivot ) 578 { 579 f32 (*const m)[4] = pOut->m; 580 #ifdef NN_PLATFORM_CTR 581 if ( ( pivot == PIVOT_NONE ) || ( pivot == PIVOT_UPSIDE_TO_LEFT ) ) 582 #else 583 if ( ( pivot == PIVOT_NONE ) || ( pivot == PIVOT_UPSIDE_TO_TOP ) ) 584 #endif 585 { 586 return pOut; 587 } 588 589 #ifdef NN_PLATFORM_CTR 590 if ( pivot == PIVOT_UPSIDE_TO_RIGHT ) 591 #else 592 if ( pivot == PIVOT_UPSIDE_TO_BOTTOM ) 593 #endif 594 { 595 register f32 m00, m01, m02, m03, m10, m11, m12, m13; 596 597 m00 = -m[0][0]; 598 m01 = -m[0][1]; 599 m02 = -m[0][2]; 600 m03 = -m[0][3]; 601 602 m10 = -m[1][0]; 603 m11 = -m[1][1]; 604 m12 = -m[1][2]; 605 m13 = -m[1][3]; 606 607 m[0][0] = m00; 608 m[0][1] = m01; 609 m[0][2] = m02; 610 m[0][3] = m03; 611 612 m[1][0] = m10; 613 m[1][1] = m11; 614 m[1][2] = m12; 615 m[1][3] = m13; 616 } 617 #ifdef NN_PLATFORM_CTR 618 else if ( pivot == PIVOT_UPSIDE_TO_BOTTOM ) 619 #else 620 else if ( pivot == PIVOT_UPSIDE_TO_LEFT ) 621 #endif 622 { 623 register f32 m00, m01, m02, m03, m10, m11, m12, m13; 624 625 m10 = m[0][0]; 626 m11 = m[0][1]; 627 m12 = m[0][2]; 628 m13 = m[0][3]; 629 630 m00 = -m[1][0]; 631 m01 = -m[1][1]; 632 m02 = -m[1][2]; 633 m03 = -m[1][3]; 634 635 m[0][0] = m00; 636 m[0][1] = m01; 637 m[0][2] = m02; 638 m[0][3] = m03; 639 640 m[1][0] = m10; 641 m[1][1] = m11; 642 m[1][2] = m12; 643 m[1][3] = m13; 644 } 645 #ifdef NN_PLATFORM_CTR 646 else // if ( pivot == PIVOT_UPSIDE_TO_TOP ) 647 #else 648 else // if ( pivot == PIVOT_UPSIDE_TO_RIGHT ) 649 #endif 650 { 651 register f32 m00, m01, m02, m03, m10, m11, m12, m13; 652 653 m10 = -m[0][0]; 654 m11 = -m[0][1]; 655 m12 = -m[0][2]; 656 m13 = -m[0][3]; 657 658 m00 = m[1][0]; 659 m01 = m[1][1]; 660 m02 = m[1][2]; 661 m03 = m[1][3]; 662 663 m[0][0] = m00; 664 m[0][1] = m01; 665 m[0][2] = m02; 666 m[0][3] = m03; 667 668 m[1][0] = m10; 669 m[1][1] = m11; 670 m[1][2] = m12; 671 m[1][3] = m13; 672 } 673 return pOut; 674 } 675 676 677/*!--------------------------------------------------------------------------* 678 @brief 行列の積を計算します。 679 680 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p1, p2 と同じ行列を指していても構いません。 681 @param[in] p1 左辺値へのポインタ。 682 @param[in] p2 右辺値へのポインタ。 683 684 @return pOut を返します。 685 *---------------------------------------------------------------------------*/ 686NN_MATH_INLINE MTX44* 687MTX44MultC(MTX44* pOut, const MTX44* __restrict p1, const MTX44* __restrict p2) 688{ 689 NN_NULL_ASSERT( pOut ); 690 NN_NULL_ASSERT( p1 ); 691 NN_NULL_ASSERT( p2 ); 692 693 MTX44 mTmp; 694 695 MTX44* __restrict pDst = ( pOut == p1 || pOut == p2 ) ? &mTmp : pOut; 696 697 pDst->f._00 = p1->f._00 * p2->f._00 + p1->f._01 * p2->f._10 + p1->f._02 * p2->f._20 + p1->f._03 * p2->f._30; 698 pDst->f._01 = p1->f._00 * p2->f._01 + p1->f._01 * p2->f._11 + p1->f._02 * p2->f._21 + p1->f._03 * p2->f._31; 699 pDst->f._02 = p1->f._00 * p2->f._02 + p1->f._01 * p2->f._12 + p1->f._02 * p2->f._22 + p1->f._03 * p2->f._32; 700 pDst->f._03 = p1->f._00 * p2->f._03 + p1->f._01 * p2->f._13 + p1->f._02 * p2->f._23 + p1->f._03 * p2->f._33; 701 702 pDst->f._10 = p1->f._10 * p2->f._00 + p1->f._11 * p2->f._10 + p1->f._12 * p2->f._20 + p1->f._13 * p2->f._30; 703 pDst->f._11 = p1->f._10 * p2->f._01 + p1->f._11 * p2->f._11 + p1->f._12 * p2->f._21 + p1->f._13 * p2->f._31; 704 pDst->f._12 = p1->f._10 * p2->f._02 + p1->f._11 * p2->f._12 + p1->f._12 * p2->f._22 + p1->f._13 * p2->f._32; 705 pDst->f._13 = p1->f._10 * p2->f._03 + p1->f._11 * p2->f._13 + p1->f._12 * p2->f._23 + p1->f._13 * p2->f._33; 706 707 pDst->f._20 = p1->f._20 * p2->f._00 + p1->f._21 * p2->f._10 + p1->f._22 * p2->f._20 + p1->f._23 * p2->f._30; 708 pDst->f._21 = p1->f._20 * p2->f._01 + p1->f._21 * p2->f._11 + p1->f._22 * p2->f._21 + p1->f._23 * p2->f._31; 709 pDst->f._22 = p1->f._20 * p2->f._02 + p1->f._21 * p2->f._12 + p1->f._22 * p2->f._22 + p1->f._23 * p2->f._32; 710 pDst->f._23 = p1->f._20 * p2->f._03 + p1->f._21 * p2->f._13 + p1->f._22 * p2->f._23 + p1->f._23 * p2->f._33; 711 712 pDst->f._30 = p1->f._30 * p2->f._00 + p1->f._31 * p2->f._10 + p1->f._32 * p2->f._20 + p1->f._33 * p2->f._30; 713 pDst->f._31 = p1->f._30 * p2->f._01 + p1->f._31 * p2->f._11 + p1->f._32 * p2->f._21 + p1->f._33 * p2->f._31; 714 pDst->f._32 = p1->f._30 * p2->f._02 + p1->f._31 * p2->f._12 + p1->f._32 * p2->f._22 + p1->f._33 * p2->f._32; 715 pDst->f._33 = p1->f._30 * p2->f._03 + p1->f._31 * p2->f._13 + p1->f._32 * p2->f._23 + p1->f._33 * p2->f._33; 716 717 if ( pDst != pOut ) 718 { 719 MTX44Copy( pOut, pDst ); 720 } 721 722 return pOut; 723} 724 725namespace { 726 inline void SwapF(f32 &a, f32 &b) 727 { 728 f32 tmp; 729 tmp = a; 730 a = b; 731 b = tmp; 732 } 733} // namespace (unnamed) 734 735 736/*!--------------------------------------------------------------------------* 737 @brief 行列の逆行列を計算します。 738 739 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p と同じ行列を指していても構いません。 740 @param[in] p 元となる行列へのポインタ。 741 742 @return 逆行列が存在すれば 1 を、存在しなければ 0 を返します。 743 *---------------------------------------------------------------------------*/ 744NN_MATH_INLINE u32 745MTX44InverseC(MTX44* pOut, const MTX44* p) 746{ 747 MTX44 mTmp; 748 f32 (*src)[4]; 749 f32 (*inv)[4]; 750 f32 w; 751 752 NN_NULL_ASSERT( p ); 753 NN_NULL_ASSERT( pOut ); 754 755 MTX44Copy(&mTmp, p); 756 MTX44Identity(pOut); 757 758 src = mTmp.m; 759 inv = pOut->m; 760 761 for (int i = 0; i < 4; ++i) 762 { 763 f32 max = 0.0f; 764 s32 swp = i; 765 766 // ---- partial pivoting ----- 767 for(int k = i ; k < 4 ; k++ ) 768 { 769 f32 ftmp; 770 ftmp = ::std::fabs(src[k][i]); 771 if ( ftmp > max ) 772 { 773 max = ftmp; 774 swp = k; 775 } 776 } 777 778 // check singular matrix 779 //(or can't solve inverse matrix with this algorithm) 780 if ( max == 0.0f ) 781 { 782 return 0; 783 } 784 785 // swap row 786 if ( swp != i ) 787 { 788 for (int k = 0; k < 4; k++) 789 { 790 SwapF(src[i][k], src[swp][k]); 791 SwapF(inv[i][k], inv[swp][k]); 792 } 793 } 794 795 // ---- pivoting end ---- 796 797 w = 1.0f / src[i][i]; 798 for (int j = 0; j < 4; ++j) 799 { 800 src[i][j] *= w; 801 inv[i][j] *= w; 802 } 803 804 for (int k = 0; k < 4; ++k ) 805 { 806 if ( k == i ) 807 continue; 808 809 w = src[k][i]; 810 for (int j = 0; j < 4; ++j) 811 { 812 src[k][j] -= src[i][j] * w; 813 inv[k][j] -= inv[i][j] * w; 814 } 815 } 816 } 817 818 return 1; 819} 820NN_MATH_INLINE u32 821MTX44InverseC_FAST_ALGO(MTX44* pOut, const MTX44* p) 822{ 823 const f32 (*src)[4]; 824 f32 (*inv)[4]; 825 826 NN_NULL_ASSERT( p ); 827 NN_NULL_ASSERT( pOut ); 828 829 src = p->m; 830 inv = pOut->m; 831 832 f32 a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34, a41, a42, a43, a44; 833 f32 b11, b12, b13, b14, b21, b22, b23, b24, b31, b32, b33, b34, b41, b42, b43, b44; 834 f32 det; 835 836 a11 = src[0][0]; 837 a12 = src[0][1]; 838 a13 = src[0][2]; 839 a14 = src[0][3]; 840 841 a21 = src[1][0]; 842 a22 = src[1][1]; 843 a23 = src[1][2]; 844 a24 = src[1][3]; 845 846 a31 = src[2][0]; 847 a32 = src[2][1]; 848 a33 = src[2][2]; 849 a34 = src[2][3]; 850 851 a41 = src[3][0]; 852 a42 = src[3][1]; 853 a43 = src[3][2]; 854 a44 = src[3][3]; 855 856 det = a11*(a22*a33*a44 + a23*a34*a42 + a24*a32*a43) 857 + a12*(a21*a34*a43 + a23*a31*a44 + a24*a33*a41) 858 + a13*(a21*a32*a44 + a22*a34*a41 + a24*a31*a42) 859 + a14*(a21*a33*a42 + a22*a31*a43 + a23*a32*a41) 860 - a11*(a22*a34*a43 + a23*a32*a44 + a24*a33*a42) 861 - a12*(a21*a33*a44 + a23*a34*a41 + a24*a31*a43) 862 - a13*(a21*a34*a42 + a22*a31*a44 + a24*a32*a41) 863 - a14*(a21*a32*a43 + a22*a33*a41 + a23*a31*a42); 864 865 if(det==0.0f) 866 return 0; 867 868 det = 1.0f / det; 869 870 f32 a33xa44_a34xa43, a32xa44_a34xa42, a33xa42_a32xa43, 871 a33xa41_a31xa43, a31xa44_a34xa41, a32xa41_a31xa42; 872 873 a33xa44_a34xa43 = a33*a44 - a34*a43; 874 a32xa44_a34xa42 = a32*a44 - a34*a42; 875 a33xa42_a32xa43 = a33*a42 - a32*a43; 876 a33xa41_a31xa43 = a33*a41 - a31*a43; 877 a31xa44_a34xa41 = a31*a44 - a34*a41; 878 a32xa41_a31xa42 = a32*a41 - a31*a42; 879 880 f32 a23xa44_a24xa43, a24xa33_a23xa34, a24xa42_a22xa44, a22xa43_a23xa42, 881 a22xa34_a24xa32, a23xa32_a22xa33, a21xa44_a24xa41, a23xa41_a21xa43, 882 a24xa31_a21xa34, a21xa33_a23xa31, a21xa42_a22xa41, a22xa31_a21xa32; 883 884 a23xa44_a24xa43 = a23*a44 - a24*a43; 885 a24xa33_a23xa34 = a24*a33 - a23*a34; 886 a24xa42_a22xa44 = a24*a42 - a22*a44; 887 a22xa43_a23xa42 = a22*a43 - a23*a42; 888 a22xa34_a24xa32 = a22*a34 - a24*a32; 889 a23xa32_a22xa33 = a23*a32 - a22*a33; 890 a21xa44_a24xa41 = a21*a44 - a24*a41; 891 a23xa41_a21xa43 = a23*a41 - a21*a43; 892 a24xa31_a21xa34 = a24*a31 - a21*a34; 893 a21xa33_a23xa31 = a21*a33 - a23*a31; 894 a21xa42_a22xa41 = a21*a42 - a22*a41; 895 a22xa31_a21xa32 = a22*a31 - a21*a32; 896 897 b11 =( a22*a33xa44_a34xa43) - (a23*a32xa44_a34xa42) - (a24*a33xa42_a32xa43); 898 b12 =( a13*a32xa44_a34xa42) + (a14*a33xa42_a32xa43) - (a12*a33xa44_a34xa43); 899 b13 =( a12*a23xa44_a24xa43) + (a13*a24xa42_a22xa44) + (a14*a22xa43_a23xa42); 900 b14 =( a12*a24xa33_a23xa34) + (a13*a22xa34_a24xa32) + (a14*a23xa32_a22xa33); 901 b21 =( a23*a31xa44_a34xa41) + (a24*a33xa41_a31xa43) - (a21*a33xa44_a34xa43); 902 b22 =( a11*a33xa44_a34xa43) - (a13*a31xa44_a34xa41) - (a14*a33xa41_a31xa43); 903 b23 =( a13*a21xa44_a24xa41) + (a14*a23xa41_a21xa43) - (a11*a23xa44_a24xa43); 904 b24 =( a13*a24xa31_a21xa34) + (a14*a21xa33_a23xa31) - (a11*a24xa33_a23xa34); 905 b31 =( a21*a32xa44_a34xa42) - (a22*a31xa44_a34xa41) - (a24*a32xa41_a31xa42); 906 b32 =( a12*a31xa44_a34xa41) + (a14*a32xa41_a31xa42) - (a11*a32xa44_a34xa42); 907 b33 =( a14*a21xa42_a22xa41) - (a11*a24xa42_a22xa44) - (a12*a21xa44_a24xa41); 908 b34 =( a14*a22xa31_a21xa32) - (a11*a22xa34_a24xa32) - (a12*a24xa31_a21xa34); 909 b41 =( a21*a33xa42_a32xa43) - (a22*a33xa41_a31xa43) + (a23*a32xa41_a31xa42); 910 b42 =( a12*a33xa41_a31xa43) - (a13*a32xa41_a31xa42) - (a11*a33xa42_a32xa43); 911 b43 =(-a13*a21xa42_a22xa41) - (a11*a22xa43_a23xa42) - (a12*a23xa41_a21xa43); 912 b44 =(-a13*a22xa31_a21xa32) - (a11*a23xa32_a22xa33) - (a12*a21xa33_a23xa31); 913 914 b11 = b11 * det; 915 b12 = b12 * det; 916 b13 = b13 * det; 917 b14 = b14 * det; 918 b21 = b21 * det; 919 b22 = b22 * det; 920 b23 = b23 * det; 921 b24 = b24 * det; 922 b31 = b31 * det; 923 b32 = b32 * det; 924 b33 = b33 * det; 925 b34 = b34 * det; 926 b41 = b41 * det; 927 b42 = b42 * det; 928 b43 = b43 * det; 929 b44 = b44 * det; 930 931 inv[0][0] = b11; 932 inv[0][1] = b12; 933 inv[0][2] = b13; 934 inv[0][3] = b14; 935 936 inv[1][0] = b21; 937 inv[1][1] = b22; 938 inv[1][2] = b23; 939 inv[1][3] = b24; 940 941 inv[2][0] = b31; 942 inv[2][1] = b32; 943 inv[2][2] = b33; 944 inv[2][3] = b34; 945 946 inv[3][0] = b41; 947 inv[3][1] = b42; 948 inv[3][2] = b43; 949 inv[3][3] = b44; 950 951 return 1; 952} 953 954 955/*!--------------------------------------------------------------------------* 956 @brief 指定する軸の周りを回転させる回転行列を作成します。 957 958 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 959 @param[in] pAxis 回転軸を指定するベクトルへのポインタ。 960 @param[in] fRad ラジアン単位での回転量 961 962 @return pOut を返します。 963 *---------------------------------------------------------------------------*/ 964NN_MATH_INLINE MTX44* 965MTX44RotAxisRad_C( MTX44* pOut, const VEC3 *pAxis, f32 fRad ) 966{ 967 VEC3 vN; 968 f32 s, c; // sinTheta, cosTheta 969 f32 t; // ( 1 - cosTheta ) 970 f32 x, y, z; // x, y, z components of normalized axis 971 f32 xSq, ySq, zSq; // x, y, z squared 972 973 974 NN_NULL_ASSERT( pOut ); 975 NN_NULL_ASSERT( pAxis ); 976 977 f32 (*const m)[4] = pOut->m; 978 979 s = ::std::sinf(fRad); 980 c = ::std::cosf(fRad); 981 t = 1.0f - c; 982 983 VEC3Normalize( &vN, pAxis ); 984 985 x = vN.x; 986 y = vN.y; 987 z = vN.z; 988 989 xSq = x * x; 990 ySq = y * y; 991 zSq = z * z; 992 993 m[0][0] = ( t * xSq ) + ( c ); 994 m[0][1] = ( t * x * y ) - ( s * z ); 995 m[0][2] = ( t * x * z ) + ( s * y ); 996 m[0][3] = 0.0f; 997 998 m[1][0] = ( t * x * y ) + ( s * z ); 999 m[1][1] = ( t * ySq ) + ( c ); 1000 m[1][2] = ( t * y * z ) - ( s * x ); 1001 m[1][3] = 0.0f; 1002 1003 m[2][0] = ( t * x * z ) - ( s * y ); 1004 m[2][1] = ( t * y * z ) + ( s * x ); 1005 m[2][2] = ( t * zSq ) + ( c ); 1006 m[2][3] = 0.0f; 1007 1008 m[3][0] = 0.0f; 1009 m[3][1] = 0.0f; 1010 m[3][2] = 0.0f; 1011 m[3][3] = 1.0f; 1012 1013 return pOut; 1014} 1015NN_MATH_INLINE MTX44* 1016MTX44RotAxisRad_C_FAST( MTX44* pOut, const VEC3 *pAxis, f32 fRad ) 1017{ 1018 VEC3 vN; 1019 f32 s, c; // sinTheta, cosTheta 1020 f32 t; // ( 1 - cosTheta ) 1021 f32 x, y, z; // x, y, z components of normalized axis 1022 f32 xSq, ySq, zSq; // x, y, z squared 1023 f32 m00, m01, m02, m10, m11, m12, m20, m21, m22; 1024 1025 1026 NN_NULL_ASSERT( pOut ); 1027 NN_NULL_ASSERT( pAxis ); 1028 1029 f32 (*const m)[4] = pOut->m; 1030 1031#if (MTX44ROTAXISRAD__CONFIG == D_FAST_C_ALGO) 1032 SinCosFIdx(&s, &c, NN_MATH_RAD_TO_FIDX(fRad)); 1033#else 1034 s = ::std::sinf(fRad); 1035 c = ::std::cosf(fRad); 1036#endif 1037 t = 1.0f - c; 1038 1039 VEC3Normalize( &vN, pAxis ); 1040 1041 x = vN.x; 1042 y = vN.y; 1043 z = vN.z; 1044 1045 xSq = x * x; 1046 ySq = y * y; 1047 zSq = z * z; 1048 1049 m00 = ( t * xSq ) + ( c ); 1050 m01 = ( t * x * y ) - ( s * z ); 1051 m02 = ( t * x * z ) + ( s * y ); 1052 1053 m10 = ( t * x * y ) + ( s * z ); 1054 m11 = ( t * ySq ) + ( c ); 1055 m12 = ( t * y * z ) - ( s * x ); 1056 1057 m20 = ( t * x * z ) - ( s * y ); 1058 m21 = ( t * y * z ) + ( s * x ); 1059 m22 = ( t * zSq ) + ( c ); 1060 1061 1062 m[0][0] = m00; 1063 m[0][1] = m01; 1064 m[0][2] = m02; 1065 m[0][3] = 0.0f; 1066 1067 m[1][0] = m10; 1068 m[1][1] = m11; 1069 m[1][2] = m12; 1070 m[1][3] = 0.0f; 1071 1072 m[2][0] = m20; 1073 m[2][1] = m21; 1074 m[2][2] = m22; 1075 m[2][3] = 0.0f; 1076 1077 m[3][0] = 0.0f; 1078 m[3][1] = 0.0f; 1079 m[3][2] = 0.0f; 1080 m[3][3] = 1.0f; 1081 1082 return pOut; 1083} 1084 1085 1086/*!--------------------------------------------------------------------------* 1087 @brief 回転行列を作成します。 1088 1089 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 1090 @param[in] fIdxX 1 円周を 256.0 とする単位での X 軸周りの角度 1091 @param[in] fIdxY 1 円周を 256.0 とする単位での Y 軸周りの角度 1092 @param[in] fIdxZ 1 円周を 256.0 とする単位での Z 軸周りの角度 1093 1094 @return pOut を返します。 1095 *---------------------------------------------------------------------------*/ 1096NN_MATH_INLINE MTX44* 1097MTX44RotXYZFIdxC(MTX44* pOut, f32 fIdxX, f32 fIdxY, f32 fIdxZ) 1098{ 1099 NN_FLOAT_ASSERT(fIdxX); 1100 NN_FLOAT_ASSERT(fIdxY); 1101 NN_FLOAT_ASSERT(fIdxZ); 1102 1103 f32 sinx, cosx; 1104 f32 siny, cosy; 1105 f32 sinz, cosz; 1106 f32 f1, f2; 1107 1108 SinCosFIdx(&sinx, &cosx, fIdxX); 1109 SinCosFIdx(&siny, &cosy, fIdxY); 1110 SinCosFIdx(&sinz, &cosz, fIdxZ); 1111 1112 pOut->f._20 = -siny; 1113 pOut->f._00 = cosz * cosy; 1114 pOut->f._10 = sinz * cosy; 1115 pOut->f._21 = cosy * sinx; 1116 pOut->f._22 = cosy * cosx; 1117 1118 f1 = cosx * sinz; 1119 f2 = sinx * cosz; 1120 1121 pOut->f._01 = f2 * siny - f1; 1122 pOut->f._12 = f1 * siny - f2; 1123 1124 f1 = sinx * sinz; 1125 f2 = cosx * cosz; 1126 pOut->f._02 = f2 * siny + f1; 1127 pOut->f._11 = f1 * siny + f2; 1128 1129 pOut->f._03 = 0.f; 1130 pOut->f._13 = 0.f; 1131 pOut->f._23 = 0.f; 1132 1133 pOut->f._30 = 0.0f; 1134 pOut->f._31 = 0.0f; 1135 pOut->f._32 = 0.0f; 1136 pOut->f._33 = 1.0f; 1137 1138 return pOut; 1139} 1140NN_MATH_INLINE MTX44* 1141MTX44RotXYZFIdxC_FAST(MTX44* pOut, f32 fIdxX, f32 fIdxY, f32 fIdxZ) 1142{ 1143 NN_FLOAT_ASSERT(fIdxX); 1144 NN_FLOAT_ASSERT(fIdxY); 1145 NN_FLOAT_ASSERT(fIdxZ); 1146 1147 f32 sinx, cosx; 1148 f32 siny, cosy; 1149 f32 sinz, cosz; 1150 f32 f1, f2, f3, f4; 1151 f32 f00, f10, f21, f22; 1152 f32 f01, f11, f02, f12; 1153 1154 1155 1156 { 1157 1158 u16 idxx; 1159 f32 abs_fidxx; 1160 f32 rx; 1161 1162 u16 idxy; 1163 f32 abs_fidxy; 1164 f32 ry; 1165 1166 u16 idxz; 1167 f32 abs_fidxz; 1168 f32 rz; 1169 1170 int negx, negy, negz; 1171 1172 negx = (fIdxX < 0.0f) ? 1 : 0; 1173 abs_fidxx = FAbs(fIdxX); 1174 negy = (fIdxY < 0.0f) ? 1 : 0; 1175 abs_fidxy = FAbs(fIdxY); 1176 negz = (fIdxZ < 0.0f) ? 1 : 0; 1177 abs_fidxz = FAbs(fIdxZ); 1178 1179 1180 while ( abs_fidxx >= 65536.0f ) 1181 { 1182 abs_fidxx -= 65536.0f; 1183 } 1184 while ( abs_fidxy >= 65536.0f ) 1185 { 1186 abs_fidxy -= 65536.0f; 1187 } 1188 while ( abs_fidxz >= 65536.0f ) 1189 { 1190 abs_fidxz -= 65536.0f; 1191 } 1192 1193 idxx = F32ToU16(abs_fidxx); 1194 idxy = F32ToU16(abs_fidxy); 1195 idxz = F32ToU16(abs_fidxz); 1196 { 1197 f32 idxxf, idxyf, idxzf; 1198 1199 idxxf = U16ToF32(idxx); 1200 idxyf = U16ToF32(idxy); 1201 idxzf = U16ToF32(idxz); 1202 1203 rx = abs_fidxx - idxxf; 1204 ry = abs_fidxy - idxyf; 1205 rz = abs_fidxz - idxzf; 1206 1207 1208 } 1209 1210 idxx &= 0xff; 1211 idxy &= 0xff; 1212 idxz &= 0xff; 1213 1214 { 1215 f32 sinx_val, sinx_delta, cosx_val, cosx_delta; 1216 f32 siny_val, siny_delta, cosy_val, cosy_delta; 1217 f32 sinz_val, sinz_delta, cosz_val, cosz_delta; 1218 1219 sinx_val = internal::gSinCosTbl[idxx].sin_val; 1220 cosx_val = internal::gSinCosTbl[idxx].cos_val; 1221 sinx_delta = internal::gSinCosTbl[idxx].sin_delta; 1222 cosx_delta = internal::gSinCosTbl[idxx].cos_delta; 1223 1224 sinx = sinx_val + rx * sinx_delta; 1225 cosx = cosx_val + rx * cosx_delta; 1226 1227 siny_val = internal::gSinCosTbl[idxy].sin_val; 1228 cosy_val = internal::gSinCosTbl[idxy].cos_val; 1229 siny_delta = internal::gSinCosTbl[idxy].sin_delta; 1230 cosy_delta = internal::gSinCosTbl[idxy].cos_delta; 1231 1232 siny = siny_val + ry * siny_delta; 1233 cosy = cosy_val + ry * cosy_delta; 1234 1235 sinz_val = internal::gSinCosTbl[idxz].sin_val; 1236 cosz_val = internal::gSinCosTbl[idxz].cos_val; 1237 sinz_delta = internal::gSinCosTbl[idxz].sin_delta; 1238 cosz_delta = internal::gSinCosTbl[idxz].cos_delta; 1239 1240 sinz = sinz_val + rz * sinz_delta; 1241 cosz = cosz_val + rz * cosz_delta; 1242 1243 } 1244 1245 sinx = (negx) ? -sinx : sinx; 1246 siny = (negy) ? -siny : siny; 1247 sinz = (negz) ? -sinz : sinz; 1248 1249 } 1250 1251 f00 = cosz * cosy; 1252 f10 = sinz * cosy; 1253 f21 = sinx * cosy; 1254 f22 = cosx * cosy; 1255 1256 f1 = cosx * sinz; 1257 f2 = sinx * cosz; 1258 1259 f01 = f2 * siny - f1; 1260 f12 = f1 * siny - f2; 1261 1262 f3 = sinx * sinz; 1263 f4 = cosx * cosz; 1264 1265 f02 = f4 * siny + f3; 1266 f11 = f3 * siny + f4; 1267 1268 pOut->f._00 = f00; 1269 pOut->f._10 = f10; 1270 pOut->f._21 = f21; 1271 pOut->f._22 = f22; 1272 1273 pOut->f._01 = f01; 1274 pOut->f._12 = f12; 1275 pOut->f._02 = f02; 1276 pOut->f._11 = f11; 1277 pOut->f._20 = -siny; 1278 1279 *(unsigned int*)&pOut->f._03 = 0x00000000; 1280 *(unsigned int*)&pOut->f._13 = 0x00000000; 1281 *(unsigned int*)&pOut->f._23 = 0x00000000; 1282 1283 *(unsigned int*)&pOut->f._30 = 0x00000000; 1284 *(unsigned int*)&pOut->f._31 = 0x00000000; 1285 *(unsigned int*)&pOut->f._32 = 0x00000000; 1286 *(unsigned int*)&pOut->f._33 = 0x3F800000; 1287 1288 return pOut; 1289} 1290 1291/*!--------------------------------------------------------------------------* 1292 @brief スケール変換用の行列を作成します。 1293 1294 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 1295 @param[in] pS それぞれの軸方向のスケール値が格納されたベクトルへのポインタ。 1296 1297 @return pOut を返します。 1298 *---------------------------------------------------------------------------*/ 1299NN_MATH_INLINE MTX44* 1300MTX44ScaleC(MTX44* pOut, const VEC3* pS) 1301{ 1302 NN_NULL_ASSERT( pOut ); 1303 NN_NULL_ASSERT( pS ); 1304 1305 f32 (*const m)[4] = pOut->m; 1306 1307 m[0][0] = pS->x; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = 0.0f; 1308 m[1][0] = 0.0f; m[1][1] = pS->y; m[1][2] = 0.0f; m[1][3] = 0.0f; 1309 m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = pS->z; m[2][3] = 0.0f; 1310 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 1311 1312 return pOut; 1313} 1314NN_MATH_INLINE MTX44* 1315MTX44ScaleC_FAST(MTX44* pOut, const VEC3* pS) 1316{ 1317 NN_NULL_ASSERT( pOut ); 1318 NN_NULL_ASSERT( pS ); 1319 1320 unsigned int *m = reinterpret_cast<unsigned int *>(pOut->m); 1321 1322 f32 f1 = 1.0f; 1323 f32 f0 = 0.0f; 1324 const unsigned int f32_1 = *(reinterpret_cast<unsigned int*>(&f1)); 1325 const unsigned int f32_0 = *(reinterpret_cast<unsigned int*>(&f0)); 1326 const unsigned int *p = reinterpret_cast<const unsigned int*>(pS); 1327 1328 m[ 0] = p[0]; m[ 1] = f32_0; m[ 2] = f32_0; m[ 3] = f32_0; 1329 m[ 4] = f32_0; m[ 5] = p[1]; m[ 6] = f32_0; m[ 7] = f32_0; 1330 m[ 8] = f32_0; m[ 9] = f32_0; m[10] = p[2]; m[11] = f32_0; 1331 m[12] = f32_0; m[13] = f32_0; m[14] = f32_0; m[15] = f32_1; 1332 1333 return pOut; 1334} 1335 1336 1337/*!--------------------------------------------------------------------------* 1338 @brief 行列にスケール変換を適用します。スケール行列を右から掛けます。 1339 1340 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 1341 @param[in] pM 元となる行列へのポインタ。 1342 @param[in] pS それぞれの軸方向のスケール値が格納されたベクトルへのポインタ。 1343 1344 @return pOut を返します。 1345 *---------------------------------------------------------------------------*/ 1346NN_MATH_INLINE MTX44* 1347MTX44MultScaleC(MTX44* pOut, const MTX44* pM, const VEC3* pS) 1348{ 1349 // スケール行列を右からかけるバージョン 1350 pOut->f._00 = pM->f._00 * pS->x; 1351 pOut->f._10 = pM->f._10 * pS->x; 1352 pOut->f._20 = pM->f._20 * pS->x; 1353 1354 pOut->f._01 = pM->f._01 * pS->y; 1355 pOut->f._11 = pM->f._11 * pS->y; 1356 pOut->f._21 = pM->f._21 * pS->y; 1357 1358 pOut->f._02 = pM->f._02 * pS->z; 1359 pOut->f._12 = pM->f._12 * pS->z; 1360 pOut->f._22 = pM->f._22 * pS->z; 1361 1362 if (pOut != pM) 1363 { 1364 pOut->f._03 = pM->f._03; 1365 pOut->f._13 = pM->f._13; 1366 pOut->f._23 = pM->f._23; 1367 } 1368 1369 return pOut; 1370} 1371 1372 1373/*!--------------------------------------------------------------------------* 1374 @brief 行列にスケール変換を適用します。スケール行列を左から掛けます。 1375 1376 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 1377 @param[in] pS それぞれの軸方向のスケール値が格納されたベクトルへのポインタ。 1378 @param[in] pM 元となる行列へのポインタ。 1379 1380 @return pOut を返します。 1381 *---------------------------------------------------------------------------*/ 1382NN_MATH_INLINE MTX44* 1383MTX44MultScaleC(MTX44* pOut, const VEC3* pS, const MTX44* pM) 1384{ 1385 NN_NULL_ASSERT( pOut ); 1386 NN_NULL_ASSERT( pS ); 1387 NN_NULL_ASSERT( pM ); 1388 1389 const f32 (*const src)[4] = pM->m; 1390 f32 (*const dst)[4] = pOut->m; 1391 1392 dst[0][0] = src[0][0] * pS->x; dst[0][1] = src[0][1] * pS->x; 1393 dst[0][2] = src[0][2] * pS->x; dst[0][3] = src[0][3] * pS->x; 1394 1395 dst[1][0] = src[1][0] * pS->y; dst[1][1] = src[1][1] * pS->y; 1396 dst[1][2] = src[1][2] * pS->y; dst[1][3] = src[1][3] * pS->y; 1397 1398 dst[2][0] = src[2][0] * pS->z; dst[2][1] = src[2][1] * pS->z; 1399 dst[2][2] = src[2][2] * pS->z; dst[2][3] = src[2][3] * pS->z; 1400 1401 return pOut; 1402} 1403 1404 1405/*!--------------------------------------------------------------------------* 1406 @brief 平行移動用の行列を作成します。 1407 1408 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 1409 @param[in] pT それぞれの軸方向の移動量が格納されたベクトルへのポインタ。 1410 1411 @return pOut を返します。 1412 *---------------------------------------------------------------------------*/ 1413NN_MATH_INLINE MTX44* 1414MTX44TranslateC(MTX44* pOut, const VEC3* pT) 1415{ 1416 NN_NULL_ASSERT( pOut ); 1417 NN_NULL_ASSERT( pT ); 1418 1419 f32 (*const m)[4] = pOut->m; 1420 1421 m[0][0] = 1.0f; m[0][1] = 0.0f; m[0][2] = 0.0f; m[0][3] = pT->x; 1422 m[1][0] = 0.0f; m[1][1] = 1.0f; m[1][2] = 0.0f; m[1][3] = pT->y; 1423 m[2][0] = 0.0f; m[2][1] = 0.0f; m[2][2] = 1.0f; m[2][3] = pT->z; 1424 m[3][0] = 0.0f; m[3][1] = 0.0f; m[3][2] = 0.0f; m[3][3] = 1.0f; 1425 1426 return pOut; 1427} 1428NN_MATH_INLINE MTX44* 1429MTX44TranslateC_FAST(MTX44* pOut, const VEC3* pT) 1430{ 1431 NN_NULL_ASSERT( pOut ); 1432 NN_NULL_ASSERT( pT ); 1433 1434 unsigned int *m = reinterpret_cast<unsigned int *>(pOut->m); 1435 1436 f32 f1 = 1.0f; 1437 f32 f0 = 0.0f; 1438 const unsigned int f32_1 = *(reinterpret_cast<unsigned int*>(&f1)); 1439 const unsigned int f32_0 = *(reinterpret_cast<unsigned int*>(&f0)); 1440 const unsigned int *p = reinterpret_cast<const unsigned int*>(pT); 1441 1442 m[ 0] = f32_1; m[ 1] = f32_0; m[ 2] = f32_0; m[ 3] = p[0]; 1443 m[ 4] = f32_0; m[ 5] = f32_1; m[ 6] = f32_0; m[ 7] = p[1]; 1444 m[ 8] = f32_0; m[ 9] = f32_0; m[10] = f32_1; m[11] = p[2]; 1445 m[12] = f32_0; m[13] = f32_0; m[14] = f32_0; m[15] = f32_1; 1446 1447 return pOut; 1448} 1449 1450 1451/*!--------------------------------------------------------------------------* 1452 @brief 行列に平行移動を適用します。移動行列を左から掛けます。 1453 1454 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 1455 @param[in] pT それぞれの軸方向の移動量が格納されたベクトルへのポインタ。 1456 @param[in] pM 元となる行列へのポインタ。 1457 1458 @return pOut を返します。 1459 *---------------------------------------------------------------------------*/ 1460NN_MATH_INLINE MTX44* 1461MTX44MultTranslateC(MTX44* pOut, const VEC3* pT, const MTX44* pM) 1462{ 1463 NN_NULL_ASSERT(pOut); 1464 NN_NULL_ASSERT(pT); 1465 NN_NULL_ASSERT(pM); 1466 1467 const f32 (*const src)[4] = pM->m; 1468 f32 (*const dst)[4] = pOut->m; 1469 1470 if ( src != dst ) 1471 { 1472 dst[0][0] = src[0][0]; dst[0][1] = src[0][1]; dst[0][2] = src[0][2]; 1473 dst[1][0] = src[1][0]; dst[1][1] = src[1][1]; dst[1][2] = src[1][2]; 1474 dst[2][0] = src[2][0]; dst[2][1] = src[2][1]; dst[2][2] = src[2][2]; 1475 } 1476 1477 dst[0][3] = src[0][3] + pT->x; 1478 dst[1][3] = src[1][3] + pT->y; 1479 dst[2][3] = src[2][3] + pT->z; 1480 1481 return pOut; 1482} 1483 1484/*!--------------------------------------------------------------------------* 1485 @brief 行列に平行移動を適用します。移動行列を右から掛けます。 1486 1487 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 1488 @param[in] pM 元となる行列へのポインタ。 1489 @param[in] pT それぞれの軸方向の移動量が格納されたベクトルへのポインタ。 1490 1491 @return pOut を返します。 1492 *---------------------------------------------------------------------------*/ 1493NN_MATH_INLINE MTX44* 1494MTX44MultTranslateC(MTX44* pOut, const MTX44* pM, const VEC3* pT) 1495{ 1496 NN_NULL_ASSERT(pOut); 1497 NN_NULL_ASSERT(pT); 1498 NN_NULL_ASSERT(pM); 1499 1500 // pOut = pM * pT 1501 if (pOut != pM) 1502 { 1503 (void)MTX44Copy(pOut, pM); 1504 } 1505 1506 VEC4 tmp; 1507 VEC3Transform(&tmp, pM, pT); 1508 1509 pOut->f._03 = tmp.x; 1510 pOut->f._13 = tmp.y; 1511 pOut->f._23 = tmp.z; 1512 1513 return pOut; 1514} 1515 1516/*! 1517 @} 1518*/ 1519 1520} // namespace ARMv6 1521} // namespace math 1522} // namespace nn 1523