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/*! 22 @name ユーティリティ 23 @{ 24*/ 25 26/*!--------------------------------------------------------------------------* 27 @brief ベクトルを行列で変換します。ベクトルの 4 要素目を 1 として計算します。 28 29 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 30 pV と同じベクトルを指していても構いません。 31 @param[in] pM 変換行列へのポインタ。 32 @param[in] pV 元となるベクトルへのポインタ。 33 34 @return pOut を返します。 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/*! 65 @name 行列 66 @{ 67*/ 68 69/*!--------------------------------------------------------------------------* 70 @brief 行列をコピーします。 71 72 @param[out] pOut コピー先の行列へのポインタ。 73 @param[in] p コピー元の行列へのポインタ 74 75 @return pOut を返します。 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 @brief 行列の積を計算します。 95 96 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 97 p1, p2 と同じ行列を指していても構いません。 98 @param[in] p1 左辺値へのポインタ。 99 @param[in] p2 右辺値へのポインタ。 100 101 @return pOut を返します。 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 @brief 行列の和を計算します。 140 141 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p1, p2 と同じ行列を指していても構いません。 142 @param[in] p1 左辺値へのポインタ。 143 @param[in] p2 右辺値へのポインタ。 144 145 @return pOut を返します。 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 @brief 行列のスカラー積を計算します。 170 171 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p と同じ行列を指していても構いません。 172 @param[in] p 元の行列のポインタ。 173 @param[in] f 掛ける数 174 175 @return pOut を返します。 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 @brief スケール変換用の行列を作成します。 201 202 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 203 @param[in] pS それぞれの軸方向のスケール値が格納されたベクトルへのポインタ。 204 205 @return pOut を返します。 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 @brief 行列にスケール変換を適用します。スケール行列を右から掛けます。 241 242 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 243 @param[in] pM 元となる行列へのポインタ。 244 @param[in] pS それぞれの軸方向のスケール値が格納されたベクトルへのポインタ。 245 246 @return pOut を返します。 247 *---------------------------------------------------------------------------*/ 248NN_MATH_INLINE MTX34* 249MTX34MultScaleC(MTX34* pOut, const MTX34* pM, const VEC3* pS) 250{ 251 // スケール行列を右からかけるバージョン 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 @brief 行列にスケール変換を適用します。スケール行列を左から掛けます。 277 278 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 279 @param[in] pS それぞれの軸方向のスケール値が格納されたベクトルへのポインタ。 280 @param[in] pM 元となる行列へのポインタ。 281 282 @return pOut を返します。 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 @brief 平行移動用の行列を作成します。 309 310 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 311 @param[in] pT それぞれの軸方向の移動量が格納されたベクトルへのポインタ。 312 313 @return pOut を返します。 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 @brief 行列に平行移動を適用します。移動行列を左から掛けます。 352 353 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 354 @param[in] pT それぞれの軸方向の移動量が格納されたベクトルへのポインタ。 355 @param[in] pM 元となる行列へのポインタ。 356 357 @return pOut を返します。 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 @brief 行列に平行移動を適用します。移動行列を右から掛けます。 385 386 @param[out] pOut 計算結果を受け取るバッファへのポインタ。pM と同じ行列を指していても構いません。 387 @param[in] pM 元となる行列へのポインタ。 388 @param[in] pT それぞれの軸方向の移動量が格納されたベクトルへのポインタ。 389 390 @return pOut を返します。 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 @brief 行列を実数倍して、別の行列を足します。 415 416 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p1, p2 と同じ行列を指していても構いません。 417 @param[in] t 掛ける数。 418 @param[in] p1 元の行列へのポインタ。 419 @param[in] p2 足す行列へのポインタ。 420 421 @return pOut を返します。 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 @brief 指定する軸の周りを回転させる回転行列を作成します。 447 448 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 449 @param[in] pAxis 回転軸を指定するベクトルへのポインタ。 450 @param[in] fRad ラジアン単位での回転量 451 452 @return pOut を返します。 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 @brief 回転行列を作成します。 567 568 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 569 @param[in] fIdxX 1 円周を 256.0 とする単位での X 軸周りの角度 570 @param[in] fIdxY 1 円周を 256.0 とする単位での Y 軸周りの角度 571 @param[in] fIdxZ 1 円周を 256.0 とする単位での Z 軸周りの角度 572 573 @return pOut を返します。 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 @brief 行列の逆行列を計算します。 762 763 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p と同じ行列を指していても構いません。 764 @param[in] p 元となる行列へのポインタ。 765 766 @return 逆行列が存在すれば 1 を、存在しなければ 0 を返します。 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 @brief 行列の転置行列を作成します。 837 838 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p と同じ行列を指していても構いません。 839 @param[in] p 転置する行列へのポインタ。 840 841 @return pOut を返します。 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 @brief 行列の逆転置行列を計算します。 879 880 @param[out] pOut 計算結果を受け取るバッファへのポインタ。p と同じ行列を指していても構いません。 881 @param[in] p 元となる行列へのポインタ。 882 883 @return 逆行列が存在すれば 1 を、存在しなければ 0 を返します。 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 @brief カメラ行列を設定します。 954 955 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 956 @param[in] pCamPos カメラの位置を指定するベクトルへのポインタ。 957 @param[in] pCamUp カメラの上方向を指定するベクトルへのポインタ。 958 @param[in] pTarget カメラの注視点を指定するベクトルへのポインタ。 959 960 @return pOut を返します。 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 @brief カメラ行列を設定します。 1098 1099 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 1100 @param[in] pCamPos カメラの位置を指定するベクトルへのポインタ。 1101 @param[in] twist カメラの視線方向に対する回転角度。単位は Degree です。 1102 @param[in] pTarget カメラの注視点を指定するベクトルへのポインタ。 1103 1104 @return pOut を返します。 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 // カメラ座標系のＺ方向 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 // カメラとターゲットのXZ座標が同じ場合ツイストは定義されない 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 // 真上を見るとき 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 // 真下を見るとき 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 // カメラ座標系のＸ方向 1154 VEC3 r(lookReverse.z, 0.0f, -lookReverse.x); 1155 1156 VEC3Normalize(&lookReverse, &lookReverse); 1157 VEC3Normalize(&r, &r); 1158 1159 // カメラ座標系のＹ方向 1160 VEC3 u; 1161 VEC3Cross(&u, &lookReverse, &r); 1162 1163 f32 st, ct; 1164 SinCosDeg(&st, &ct, twist); 1165 VEC3 right, up; 1166 1167 // r軸, u軸をru平面上でcameraTwistだけ半時計回りに回転させてrightを求める 1168 // 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 // カメラ座標系のＺ方向 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 // カメラとターゲットのXZ座標が同じ場合ツイストは定義されない 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 // 真上を見るとき 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 // 真下を見るとき 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 // カメラ座標系のＸ方向 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 // カメラ座標系のＹ方向 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(&st, &ct, twist); 1288 SinCosFIdx(&st, &ct, fidx); 1289 f32 rightx, righty, rightz; 1290 f32 upx, upy, upz; 1291 1292 // r軸, u軸をru平面上でcameraTwistだけ半時計回りに回転させてrightを求める 1293 // 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 @brief カメラ行列を設定します。 1332 1333 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 1334 @param[in] pCamPos カメラの位置を指定するベクトルへのポインタ。 1335 @param[in] pCamRotate カメラの回転量を指定するベクトルへのポインタ。単位は Degree です。 1336 1337 @return pOut を返します。 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 // Z軸, X軸, Y軸の順番で回転させている 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 // Z軸, X軸, Y軸の順番で回転させている 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/*! 1556 @name クォータニオン 1557 @{ 1558*/ 1559 1560/*!--------------------------------------------------------------------------* 1561 @brief クォータニオンから回転行列を作成します。 1562 1563 @param[out] pOut 計算結果を受け取るバッファへのポインタ。 1564 @param[in] pQ 元となるクォータニオンへのポインタ。 1565 1566 @return pOut を返します。 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