1#pragma once 2 3//******************************************************************************************** 4//* 5//* This file is part of Egoboo. 6//* 7//* Egoboo is free software: you can redistribute it and/or modify it 8//* under the terms of the GNU General Public License as published by 9//* the Free Software Foundation, either version 3 of the License, or 10//* (at your option) any later version. 11//* 12//* Egoboo is distributed in the hope that it will be useful, but 13//* WITHOUT ANY WARRANTY; without even the implied warranty of 14//* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 15//* General Public License for more details. 16//* 17//* You should have received a copy of the GNU General Public License 18//* along with Egoboo. If not, see <http://www.gnu.org/licenses/>. 19//* 20//******************************************************************************************** 21 22/// @file egoboo_math.inl 23/// @brief 24/// @details Almost all of the math functions are intended to be inlined for maximum speed 25 26#include "egoboo_math.h" 27#include "log.h" 28#include "ogl_include.h" 29#include "ogl_debug.h" 30 31#include <float.h> 32 33//-------------------------------------------------------------------------------------------- 34// FORWARD DECLARATIONS 35//-------------------------------------------------------------------------------------------- 36 37// conversion functions 38static INLINE FACING_T vec_to_facing( const float dx, const float dy ); 39static INLINE void facing_to_vec( const FACING_T facing, float * dx, float * dy ); 40 41// rotation functions 42static INLINE int terp_dir( const FACING_T majordir, const FACING_T minordir, const int weight ); 43 44// limiting functions 45static INLINE void getadd( const int min, const int value, const int max, int* valuetoadd ); 46static INLINE void fgetadd( const float min, const float value, const float max, float* valuetoadd ); 47 48// random functions 49static INLINE int generate_irand_pair( const IPair num ); 50static INLINE int generate_irand_range( const FRange num ); 51static INLINE int generate_randmask( const int base, const int mask ); 52 53// vector functions 54static INLINE bool_t fvec2_self_clear( fvec2_base_t A ); 55static INLINE bool_t fvec2_base_copy( fvec2_base_t A, const fvec2_base_t B ); 56static INLINE bool_t fvec2_base_assign( fvec2_base_t A, const fvec2_t B ); 57static INLINE float fvec2_length( const fvec2_base_t A ); 58static INLINE float fvec2_length_abs( const fvec2_base_t A ); 59static INLINE float fvec2_length_2( const fvec2_base_t A ); 60static INLINE bool_t fvec2_self_scale( fvec2_base_t A, const float B ); 61static INLINE fvec2_t fvec2_sub( const fvec2_base_t A, const fvec2_base_t B ); 62static INLINE fvec2_t fvec2_normalize( const fvec2_base_t vec ); 63static INLINE bool_t fvec2_self_normalize( fvec2_base_t A ); 64static INLINE float fvec2_cross_product( const fvec2_base_t A, const fvec2_base_t B ); 65static INLINE float fvec2_dot_product( const fvec2_base_t A, const fvec2_base_t B ); 66static INLINE float fvec2_dist_abs( const fvec2_base_t A, const fvec2_base_t B ); 67 68static INLINE bool_t fvec3_self_clear( fvec3_base_t A ); 69static INLINE bool_t fvec3_base_copy( fvec3_base_t A, const fvec3_base_t B ); 70static INLINE bool_t fvec3_base_assign( fvec3_base_t A, const fvec3_t B ); 71static INLINE bool_t fvec3_self_scale( fvec3_base_t A, const float B ); 72static INLINE bool_t fvec3_self_sum( fvec3_base_t A, const fvec3_base_t B ); 73static INLINE bool_t fvec3_self_normalize( fvec3_base_t A ); 74static INLINE bool_t fvec3_self_normalize_to( fvec3_base_t A, const float B ); 75static INLINE float fvec3_length_2( const fvec3_base_t A ); 76static INLINE float fvec3_length( const fvec3_base_t A ); 77static INLINE float fvec3_length_abs( const fvec3_base_t A ); 78static INLINE float fvec3_dot_product( const fvec3_base_t A, const fvec3_base_t B ); 79static INLINE float fvec3_dist_abs( const fvec3_base_t A, const fvec3_base_t B ); 80static INLINE fvec3_t fvec3_scale( const fvec3_base_t A, const float B ); 81static INLINE fvec3_t fvec3_normalize( const fvec3_base_t A ); 82static INLINE fvec3_t fvec3_add( const fvec3_base_t A, const fvec3_base_t B ); 83static INLINE fvec3_t fvec3_sub( const fvec3_base_t A, const fvec3_base_t B ); 84static INLINE fvec3_t fvec3_cross_product( const fvec3_base_t A, const fvec3_base_t B ); 85static INLINE float fvec3_decompose( const fvec3_base_t A, const fvec3_base_t vnrm, fvec3_base_t vpara, fvec3_base_t vperp ); 86 87static INLINE bool_t fvec4_self_clear( fvec4_base_t A ); 88 89// matrix functions 90static INLINE fmat_4x4_t IdentityMatrix( void ); 91static INLINE fmat_4x4_t ZeroMatrix( void ); 92static INLINE fmat_4x4_t MatrixMult( const fmat_4x4_t a, const fmat_4x4_t b ); 93static INLINE fmat_4x4_t Translate( const float dx, const float dy, const float dz ); 94static INLINE fmat_4x4_t RotateX( const float rads ); 95static INLINE fmat_4x4_t RotateY( const float rads ); 96static INLINE fmat_4x4_t RotateZ( const float rads ); 97static INLINE fmat_4x4_t ScaleXYZ( const float sizex, const float sizey, const float sizez ); 98static INLINE fmat_4x4_t FourPoints( const fvec4_base_t ori, const fvec4_base_t wid, const fvec4_base_t frw, const fvec4_base_t upx, const float scale ); 99static INLINE fmat_4x4_t ViewMatrix( const fvec3_base_t from, const fvec3_base_t at, const fvec3_base_t world_up, const float roll ); 100static INLINE fmat_4x4_t ProjectionMatrix( const float near_plane, const float far_plane, const float fov ); 101static INLINE void TransformVertices( const fmat_4x4_t *pMatrix, const fvec4_t *pSourceV, fvec4_t *pDestV, const Uint32 NumVertor ); 102 103static INLINE bool_t mat_getChrUp( const fmat_4x4_base_t mat, fvec3_base_t vec ); 104static INLINE bool_t mat_getChrForward( const fmat_4x4_base_t mat, fvec3_base_t vec ); 105static INLINE bool_t mat_getChrRight( const fmat_4x4_base_t mat, fvec3_base_t vec ); 106static INLINE bool_t mat_getCamUp( const fmat_4x4_base_t mat, fvec3_base_t vec ); 107static INLINE bool_t mat_getCamRight( const fmat_4x4_base_t mat, fvec3_base_t vec ); 108static INLINE bool_t mat_getCamForward( const fmat_4x4_base_t mat, fvec3_base_t vec ); 109static INLINE bool_t mat_getTranslate( const fmat_4x4_base_t mat, fvec3_base_t vec ); 110static INLINE float * mat_getTranslate_v( const fmat_4x4_base_t mat ); 111 112//-------------------------------------------------------------------------------------------- 113// CONVERSION FUNCTIONS 114//-------------------------------------------------------------------------------------------- 115static INLINE FACING_T vec_to_facing( const float dx, const float dy ) 116{ 117 return ( FACING_T )(( ATAN2( dy, dx ) + PI ) * RAD_TO_TURN ); 118} 119 120//-------------------------------------------------------------------------------------------- 121static INLINE void facing_to_vec( const FACING_T facing, float * dx, float * dy ) 122{ 123 TURN_T turn = TO_TURN( facing - 0x8000 ); 124 125 if ( NULL != dx ) 126 { 127 *dx = turntocos[turn]; 128 } 129 130 if ( NULL != dy ) 131 { 132 *dy = turntosin[turn]; 133 } 134} 135 136//-------------------------------------------------------------------------------------------- 137// ROTATION FUNCTIONS 138//-------------------------------------------------------------------------------------------- 139static INLINE int terp_dir( const FACING_T majordir, const FACING_T minordir, const int weight ) 140{ 141 /// @details ZZ@> This function returns a direction between the major and minor ones, closer 142 /// to the major. 143 144 int diff; 145 146 // Align major direction with 0 147 diff = ( int )minordir - ( int )majordir; 148 149 if ( diff <= -( int )0x8000L ) 150 { 151 diff += ( int )0x00010000L; 152 } 153 else if ( diff >= ( int )0x8000L ) 154 { 155 diff -= ( int )0x00010000L; 156 } 157 158 return diff / weight; 159} 160 161//-------------------------------------------------------------------------------------------- 162// LIMITING FUNCTIONS 163//-------------------------------------------------------------------------------------------- 164static INLINE void getadd( const int min, const int value, const int max, int* valuetoadd ) 165{ 166 /// @details ZZ@> This function figures out what value to add should be in order 167 /// to not overflow the min and max bounds 168 169 int newvalue; 170 171 newvalue = value + ( *valuetoadd ); 172 if ( newvalue < min ) 173 { 174 // Increase valuetoadd to fit 175 *valuetoadd = min - value; 176 if ( *valuetoadd > 0 ) *valuetoadd = 0; 177 178 return; 179 } 180 if ( newvalue > max ) 181 { 182 // Decrease valuetoadd to fit 183 *valuetoadd = max - value; 184 if ( *valuetoadd < 0 ) *valuetoadd = 0; 185 } 186} 187 188//-------------------------------------------------------------------------------------------- 189static INLINE void fgetadd( const float min, const float value, const float max, float* valuetoadd ) 190{ 191 /// @details ZZ@> This function figures out what value to add should be in order 192 /// to not overflow the min and max bounds 193 194 float newvalue; 195 196 newvalue = value + ( *valuetoadd ); 197 if ( newvalue < min ) 198 { 199 // Increase valuetoadd to fit 200 *valuetoadd = min - value; 201 if ( *valuetoadd > 0 ) *valuetoadd = 0; 202 203 return; 204 } 205 if ( newvalue > max ) 206 { 207 // Decrease valuetoadd to fit 208 *valuetoadd = max - value; 209 if ( *valuetoadd < 0 ) *valuetoadd = 0; 210 } 211} 212 213//-------------------------------------------------------------------------------------------- 214// RANDOM FUNCTIONS 215//-------------------------------------------------------------------------------------------- 216static INLINE int generate_irand_pair( const IPair num ) 217{ 218 /// @details ZZ@> This function generates a random number 219 220 int tmp = num.base; 221 if ( num.rand > 1 ) 222 { 223 int irand = RANDIE; 224 tmp += irand % num.rand; 225 } 226 227 return tmp; 228} 229 230//-------------------------------------------------------------------------------------------- 231static INLINE int generate_irand_range( const FRange num ) 232{ 233 /// @details ZZ@> This function generates a random number 234 235 IPair loc_pair; 236 237 range_to_pair( num, &loc_pair ); 238 239 return generate_irand_pair( loc_pair ); 240} 241 242//-------------------------------------------------------------------------------------------- 243static INLINE int generate_randmask( const int base, const int mask ) 244{ 245 /// @details ZZ@> This function generates a random number 246 int tmp; 247 int irand = RANDIE; 248 249 tmp = base; 250 if ( mask > 0 ) 251 { 252 tmp += irand & mask; 253 } 254 255 return tmp; 256} 257 258//-------------------------------------------------------------------------------------------- 259// VECTOR FUNCTIONS 260//-------------------------------------------------------------------------------------------- 261static INLINE bool_t fvec2_self_clear( fvec2_base_t A ) 262{ 263 if ( NULL == A ) return bfalse; 264 265 A[kX] = A[kY] = 0.0f; 266 267 return btrue; 268} 269 270//-------------------------------------------------------------------------------------------- 271static INLINE bool_t fvec2_base_copy( fvec2_base_t A, const fvec2_base_t B ) 272{ 273 if ( NULL == A ) return bfalse; 274 275 if ( NULL == B ) return fvec2_self_clear( A ); 276 277 A[kX] = B[kX]; 278 A[kY] = B[kY]; 279 280 return btrue; 281} 282 283//-------------------------------------------------------------------------------------------- 284static INLINE bool_t fvec2_base_assign( fvec2_base_t A, const fvec2_t B ) 285{ 286 if ( NULL == A ) return bfalse; 287 288 A[kX] = B.v[kX]; 289 A[kY] = B.v[kY]; 290 291 return btrue; 292} 293 294//-------------------------------------------------------------------------------------------- 295static INLINE bool_t fvec2_self_scale( fvec2_base_t A, const float B ) 296{ 297 if ( NULL == A ) return bfalse; 298 299 A[kX] *= B; 300 A[kY] *= B; 301 302 return btrue; 303} 304 305//-------------------------------------------------------------------------------------------- 306static INLINE float fvec2_length_abs( const fvec2_base_t A ) 307{ 308 if ( NULL == A ) return 0.0f; 309 310 return ABS( A[kX] ) + ABS( A[kY] ); 311} 312 313//-------------------------------------------------------------------------------------------- 314static INLINE float fvec2_length_2( const fvec2_base_t A ) 315{ 316 float A2; 317 318 if ( NULL == A ) return 0.0f; 319 320 A2 = A[kX] * A[kX] + A[kY] * A[kY]; 321 322 return A2; 323} 324 325//-------------------------------------------------------------------------------------------- 326static INLINE float fvec2_length( const fvec2_base_t A ) 327{ 328 float A2; 329 330 if ( NULL == A ) return 0.0f; 331 332 A2 = A[kX] * A[kX] + A[kY] * A[kY]; 333 334 return SQRT( A2 ); 335} 336 337//-------------------------------------------------------------------------------------------- 338static INLINE fvec2_t fvec2_sub( const fvec2_base_t A, const fvec2_base_t B ) 339{ 340 fvec2_t tmp; 341 342 tmp.x = A[kX] - B[kX]; 343 tmp.y = A[kY] - B[kY]; 344 345 return tmp; 346} 347 348//-------------------------------------------------------------------------------------------- 349static INLINE float fvec2_dist_abs( const fvec2_base_t A, const fvec2_base_t B ) 350{ 351 return ABS( A[kX] - B[kX] ) + ABS( A[kY] - B[kY] ); 352} 353 354//-------------------------------------------------------------------------------------------- 355static INLINE fvec2_t fvec2_scale( const fvec2_base_t A, const float B ) 356{ 357 fvec2_t tmp = ZERO_VECT2; 358 359 if ( NULL == A || 0.0f == B ) return tmp; 360 361 tmp.v[kX] = A[kX] * B; 362 tmp.v[kY] = A[kY] * B; 363 364 return tmp; 365} 366 367//-------------------------------------------------------------------------------------------- 368static INLINE fvec2_t fvec2_normalize( const fvec2_base_t vec ) 369{ 370 fvec2_t tmp = ZERO_VECT2; 371 372 if ( ABS( vec[kX] ) + ABS( vec[kY] ) > 0 ) 373 { 374 float len2 = vec[kX] * vec[kX] + vec[kY] * vec[kY]; 375 float inv_len = 1.0f / SQRT( len2 ); 376 LOG_NAN( inv_len ); 377 378 tmp.x = vec[kX] * inv_len; 379 LOG_NAN( tmp.x ); 380 381 tmp.y = vec[kY] * inv_len; 382 LOG_NAN( tmp.y ); 383 } 384 385 return tmp; 386} 387 388//-------------------------------------------------------------------------------------------- 389static INLINE bool_t fvec2_self_normalize( fvec2_base_t A ) 390{ 391 float len2; 392 float inv_len; 393 394 if ( NULL == A ) return bfalse; 395 396 if ( 0.0f == fvec2_length_abs( A ) ) return bfalse; 397 398 len2 = A[kX] * A[kX] + A[kY] * A[kY]; 399 inv_len = 1.0f / SQRT( len2 ); 400 401 A[kX] *= inv_len; 402 A[kY] *= inv_len; 403 404 return btrue; 405} 406 407//-------------------------------------------------------------------------------------------- 408static INLINE float fvec2_cross_product( const fvec2_base_t A, const fvec2_base_t B ) 409{ 410 return A[kX] * B[kY] - A[kY] * B[kX]; 411} 412 413//-------------------------------------------------------------------------------------------- 414static INLINE float fvec2_dot_product( const fvec2_base_t A, const fvec2_base_t B ) 415{ 416 return A[kX]*B[kX] + A[kY]*B[kY]; 417} 418 419//-------------------------------------------------------------------------------------------- 420//-------------------------------------------------------------------------------------------- 421static INLINE bool_t fvec3_self_clear( fvec3_base_t A ) 422{ 423 if ( NULL == A ) return bfalse; 424 425 A[kX] = A[kY] = A[kZ] = 0.0f; 426 427 return btrue; 428} 429 430//-------------------------------------------------------------------------------------------- 431static INLINE bool_t fvec3_base_copy( fvec3_base_t A, const fvec3_base_t B ) 432{ 433 if ( NULL == A ) return bfalse; 434 435 if ( NULL == B ) return fvec3_self_clear( A ); 436 437 A[kX] = B[kX]; 438 A[kY] = B[kY]; 439 A[kZ] = B[kZ]; 440 441 return btrue; 442} 443 444//-------------------------------------------------------------------------------------------- 445static INLINE bool_t fvec3_base_assign( fvec3_base_t A, const fvec3_t B ) 446{ 447 if ( NULL == A ) return bfalse; 448 449 A[kX] = B.v[kX]; 450 A[kY] = B.v[kY]; 451 A[kZ] = B.v[kZ]; 452 453 return btrue; 454} 455 456//-------------------------------------------------------------------------------------------- 457static INLINE bool_t fvec3_self_scale( fvec3_base_t A, const float B ) 458{ 459 if ( NULL == A ) return bfalse; 460 461 A[kX] *= B; 462 A[kY] *= B; 463 A[kZ] *= B; 464 465 return btrue; 466} 467 468//-------------------------------------------------------------------------------------------- 469static INLINE bool_t fvec3_self_sum( fvec3_base_t A, const fvec3_base_t B ) 470{ 471 if ( NULL == A || NULL == B ) return bfalse; 472 473 A[kX] += B[kX]; 474 A[kY] += B[kY]; 475 A[kZ] += B[kZ]; 476 477 return btrue; 478} 479 480//-------------------------------------------------------------------------------------------- 481static INLINE float fvec3_length_abs( const fvec3_base_t A ) 482{ 483 if ( NULL == A ) return 0.0f; 484 485 return ABS( A[kX] ) + ABS( A[kY] ) + ABS( A[kZ] ); 486} 487 488//-------------------------------------------------------------------------------------------- 489static INLINE float fvec3_length_2( const fvec3_base_t A ) 490{ 491 float A2; 492 493 if ( NULL == A ) return 0.0f; 494 495 A2 = A[kX] * A[kX] + A[kY] * A[kY] + A[kZ] * A[kZ]; 496 497 return A2; 498} 499 500//-------------------------------------------------------------------------------------------- 501static INLINE float fvec3_length( const fvec3_base_t A ) 502{ 503 float A2; 504 505 if ( NULL == A ) return 0.0f; 506 507 A2 = A[kX] * A[kX] + A[kY] * A[kY] + A[kZ] * A[kZ]; 508 509 return SQRT( A2 ); 510} 511 512//-------------------------------------------------------------------------------------------- 513static INLINE fvec3_t fvec3_add( const fvec3_base_t A, const fvec3_base_t B ) 514{ 515 fvec3_t tmp; 516 517 tmp.x = A[kX] + B[kX]; 518 tmp.y = A[kY] + B[kY]; 519 tmp.z = A[kZ] + B[kZ]; 520 521 return tmp; 522} 523 524//-------------------------------------------------------------------------------------------- 525static INLINE fvec3_t fvec3_sub( const fvec3_base_t A, const fvec3_base_t B ) 526{ 527 fvec3_t tmp; 528 529 tmp.x = A[kX] - B[kX]; 530 tmp.y = A[kY] - B[kY]; 531 tmp.z = A[kZ] - B[kZ]; 532 533 return tmp; 534} 535 536//-------------------------------------------------------------------------------------------- 537static INLINE fvec3_t fvec3_scale( const fvec3_base_t A, const float B ) 538{ 539 fvec3_t tmp = ZERO_VECT3; 540 541 if ( NULL == A || 0.0f == B ) return tmp; 542 543 tmp.v[kX] = A[kX] * B; 544 tmp.v[kY] = A[kY] * B; 545 tmp.v[kZ] = A[kZ] * B; 546 547 return tmp; 548} 549 550//-------------------------------------------------------------------------------------------- 551static INLINE fvec3_t fvec3_normalize( const fvec3_base_t vec ) 552{ 553 float len2, inv_len; 554 fvec3_t tmp = ZERO_VECT3; 555 556 if ( NULL == vec ) return tmp; 557 558 if ( 0.0f == fvec3_length_abs( vec ) ) return tmp; 559 560 len2 = vec[kX] * vec[kX] + vec[kY] * vec[kY] + vec[kZ] * vec[kZ]; 561 inv_len = 1.0f / SQRT( len2 ); 562 LOG_NAN( inv_len ); 563 564 tmp.x = vec[kX] * inv_len; 565 tmp.y = vec[kY] * inv_len; 566 tmp.z = vec[kZ] * inv_len; 567 568 return tmp; 569} 570 571//-------------------------------------------------------------------------------------------- 572static INLINE bool_t fvec3_self_normalize( fvec3_base_t A ) 573{ 574 if ( NULL == A ) return bfalse; 575 576 if ( 0.0f != fvec3_length_abs( A ) ) 577 { 578 float len2 = A[kX] * A[kX] + A[kY] * A[kY] + A[kZ] * A[kZ]; 579 float inv_len = 1.0f / SQRT( len2 ); 580 LOG_NAN( inv_len ); 581 582 A[kX] *= inv_len; 583 A[kY] *= inv_len; 584 A[kZ] *= inv_len; 585 } 586 587 return btrue; 588} 589 590//-------------------------------------------------------------------------------------------- 591static INLINE bool_t fvec3_self_normalize_to( fvec3_base_t vec, const float B ) 592{ 593 if ( NULL == vec ) return bfalse; 594 595 if ( 0.0f == B ) 596 { 597 fvec3_self_clear( vec ); 598 return btrue; 599 } 600 601 if ( 0.0f != fvec3_length_abs( vec ) ) 602 { 603 float len2 = vec[kX] * vec[kX] + vec[kY] * vec[kY] + vec[kZ] * vec[kZ]; 604 float inv_len = B / SQRT( len2 ); 605 LOG_NAN( inv_len ); 606 607 vec[kX] *= inv_len; 608 vec[kY] *= inv_len; 609 vec[kZ] *= inv_len; 610 } 611 612 return btrue; 613} 614 615//-------------------------------------------------------------------------------------------- 616static INLINE fvec3_t fvec3_cross_product( const fvec3_base_t A, const fvec3_base_t B ) 617{ 618 fvec3_t tmp; 619 620 tmp.x = A[kY] * B[kZ] - A[kZ] * B[kY]; 621 tmp.y = A[kZ] * B[kX] - A[kX] * B[kZ]; 622 tmp.z = A[kX] * B[kY] - A[kY] * B[kX]; 623 624 return tmp; 625} 626 627//-------------------------------------------------------------------------------------------- 628static INLINE float fvec3_decompose( const fvec3_base_t A, const fvec3_base_t vnrm, fvec3_base_t vpara, fvec3_base_t vperp ) 629{ 630 /// BB@> the normal (vnrm) is assumed to be normalized. Try to get this as optimized as possible. 631 632 float dot; 633 634 // error trapping 635 if ( NULL == A || NULL == vnrm ) return 0.0f; 636 637 // if this is true, there is no reason to run this function 638 dot = fvec3_dot_product( A, vnrm ); 639 640 if ( 0.0f == dot ) 641 { 642 // handle optional parameters 643 if ( NULL == vpara && NULL == vperp ) 644 { 645 // no point in doing anything 646 return 0.0f; 647 } 648 else if ( NULL == vpara ) 649 { 650 vperp[kX] = A[kX]; 651 vperp[kY] = A[kY]; 652 vperp[kZ] = A[kZ]; 653 } 654 else if ( NULL == vperp ) 655 { 656 vpara[kX] = 0.0f; 657 vpara[kY] = 0.0f; 658 vpara[kZ] = 0.0f; 659 } 660 else 661 { 662 vpara[kX] = 0.0f; 663 vpara[kY] = 0.0f; 664 vpara[kZ] = 0.0f; 665 666 vperp[kX] = A[kX]; 667 vperp[kY] = A[kY]; 668 vperp[kZ] = A[kZ]; 669 } 670 } 671 else 672 { 673 // handle optional parameters 674 if ( NULL == vpara && NULL == vperp ) 675 { 676 // no point in doing anything 677 return 0.0f; 678 } 679 else if ( NULL == vpara ) 680 { 681 vperp[kX] = A[kX] - dot * vnrm[kX]; 682 vperp[kY] = A[kY] - dot * vnrm[kY]; 683 vperp[kZ] = A[kZ] - dot * vnrm[kZ]; 684 } 685 else if ( NULL == vperp ) 686 { 687 vpara[kX] = dot * vnrm[kX]; 688 vpara[kY] = dot * vnrm[kY]; 689 vpara[kZ] = dot * vnrm[kZ]; 690 } 691 else 692 { 693 vpara[kX] = dot * vnrm[kX]; 694 vpara[kY] = dot * vnrm[kY]; 695 vpara[kZ] = dot * vnrm[kZ]; 696 697 vperp[kX] = A[kX] - vpara[kX]; 698 vperp[kY] = A[kY] - vpara[kY]; 699 vperp[kZ] = A[kZ] - vpara[kZ]; 700 } 701 } 702 703 return dot; 704} 705 706//-------------------------------------------------------------------------------------------- 707static INLINE float fvec3_dist_abs( const fvec3_base_t A, const fvec3_base_t B ) 708{ 709 return ABS( A[kX] - B[kX] ) + ABS( A[kY] - B[kY] ) + ABS( A[kZ] - B[kZ] ); 710} 711 712//-------------------------------------------------------------------------------------------- 713static INLINE float fvec3_dot_product( const fvec3_base_t A, const fvec3_base_t B ) 714{ 715 return A[kX]*B[kX] + A[kY]*B[kY] + A[kZ]*B[kZ]; 716} 717 718//-------------------------------------------------------------------------------------------- 719//-------------------------------------------------------------------------------------------- 720static INLINE bool_t fvec4_self_clear( fvec4_base_t A ) 721{ 722 if ( NULL == A ) return bfalse; 723 724 A[kX] = A[kY] = A[kZ] = 0.0f; 725 A[kW] = 1.0f; 726 727 return btrue; 728} 729 730//-------------------------------------------------------------------------------------------- 731// MATIX FUNCTIONS 732//-------------------------------------------------------------------------------------------- 733static INLINE bool_t mat_getTranslate( const fmat_4x4_base_t mat, fvec3_base_t vpos ) 734{ 735 if ( NULL == mat || NULL == vpos ) return bfalse; 736 737 vpos[kX] = mat[MAT_IDX( 3, 0 )]; 738 vpos[kY] = mat[MAT_IDX( 3, 1 )]; 739 vpos[kZ] = mat[MAT_IDX( 3, 2 )]; 740 741 return btrue; 742} 743 744//-------------------------------------------------------------------------------------------- 745static INLINE bool_t mat_getChrUp( const fmat_4x4_base_t mat, fvec3_base_t vup ) 746{ 747 if ( NULL == mat || NULL == vup ) return bfalse; 748 749 // for a character 750 vup[kX] = mat[MAT_IDX( 2, 0 )]; 751 vup[kY] = mat[MAT_IDX( 2, 1 )]; 752 vup[kZ] = mat[MAT_IDX( 2, 2 )]; 753 754 return btrue; 755} 756 757//-------------------------------------------------------------------------------------------- 758static INLINE bool_t mat_getChrForward( const fmat_4x4_base_t mat, fvec3_base_t vright ) 759{ 760 if ( NULL == mat || NULL == vright ) return bfalse; 761 762 // for a character 763 vright[kX] = -mat[MAT_IDX( 0, 0 )]; 764 vright[kY] = -mat[MAT_IDX( 0, 1 )]; 765 vright[kZ] = -mat[MAT_IDX( 0, 2 )]; 766 767 return btrue; 768} 769 770//-------------------------------------------------------------------------------------------- 771static INLINE bool_t mat_getChrRight( const fmat_4x4_base_t mat, fvec3_base_t vfrw ) 772{ 773 if ( NULL == mat || NULL == vfrw ) return bfalse; 774 775 // for a character's matrix 776 vfrw[kX] = mat[MAT_IDX( 1, 0 )]; 777 vfrw[kY] = mat[MAT_IDX( 1, 1 )]; 778 vfrw[kZ] = mat[MAT_IDX( 1, 2 )]; 779 780 return btrue; 781} 782 783//-------------------------------------------------------------------------------------------- 784static INLINE bool_t mat_getCamUp( const fmat_4x4_base_t mat, fvec3_base_t vup ) 785{ 786 if ( NULL == mat || NULL == vup ) return bfalse; 787 788 // for the camera 789 vup[kX] = -mat[MAT_IDX( 0, 1 )]; 790 vup[kY] = -mat[MAT_IDX( 1, 1 )]; 791 vup[kZ] = -mat[MAT_IDX( 2, 1 )]; 792 793 return btrue; 794} 795 796//-------------------------------------------------------------------------------------------- 797static INLINE bool_t mat_getCamRight( const fmat_4x4_base_t mat, fvec3_base_t vright ) 798{ 799 if ( NULL == mat || NULL == vright ) return bfalse; 800 801 // for the camera 802 vright[kX] = mat[MAT_IDX( 0, 0 )]; 803 vright[kY] = mat[MAT_IDX( 1, 0 )]; 804 vright[kZ] = mat[MAT_IDX( 2, 0 )]; 805 806 return btrue; 807} 808 809//-------------------------------------------------------------------------------------------- 810static INLINE bool_t mat_getCamForward( const fmat_4x4_base_t mat, fvec3_base_t vfrw ) 811{ 812 if ( NULL == mat || NULL == vfrw ) return bfalse; 813 814 // for the camera 815 vfrw[kX] = mat[MAT_IDX( 0, 2 )]; 816 vfrw[kY] = mat[MAT_IDX( 1, 2 )]; 817 vfrw[kZ] = mat[MAT_IDX( 2, 2 )]; 818 819 return btrue; 820} 821 822//-------------------------------------------------------------------------------------------- 823static INLINE float * mat_getTranslate_v( const fmat_4x4_base_t mat ) 824{ 825 static fvec3_t pos; 826 827 pos.x = mat[MAT_IDX( 3, 0 )]; 828 pos.y = mat[MAT_IDX( 3, 1 )]; 829 pos.z = mat[MAT_IDX( 3, 2 )]; 830 831 return pos.v; 832} 833 834//-------------------------------------------------------------------------------------------- 835static INLINE fmat_4x4_t IdentityMatrix() 836{ 837 fmat_4x4_t tmp; 838 839 tmp.CNV( 0, 0 ) = 1; tmp.CNV( 1, 0 ) = 0; tmp.CNV( 2, 0 ) = 0; tmp.CNV( 3, 0 ) = 0; 840 tmp.CNV( 0, 1 ) = 0; tmp.CNV( 1, 1 ) = 1; tmp.CNV( 2, 1 ) = 0; tmp.CNV( 3, 1 ) = 0; 841 tmp.CNV( 0, 2 ) = 0; tmp.CNV( 1, 2 ) = 0; tmp.CNV( 2, 2 ) = 1; tmp.CNV( 3, 2 ) = 0; 842 tmp.CNV( 0, 3 ) = 0; tmp.CNV( 1, 3 ) = 0; tmp.CNV( 2, 3 ) = 0; tmp.CNV( 3, 3 ) = 1; 843 844 return( tmp ); 845} 846 847//-------------------------------------------------------------------------------------------- 848static INLINE fmat_4x4_t ZeroMatrix( void ) 849{ 850 // initializes matrix to zero 851 852 fmat_4x4_t ret; 853 int i, j; 854 855 for ( i = 0; i < 4; i++ ) 856 { 857 for ( j = 0; j < 4; j++ ) 858 { 859 ret.CNV( i, j ) = 0; 860 } 861 } 862 863 return ret; 864} 865 866//-------------------------------------------------------------------------------------------- 867static INLINE fmat_4x4_t MatrixMult( const fmat_4x4_t a, const fmat_4x4_t b ) 868{ 869 fmat_4x4_t ret = ZERO_MAT_4X4; 870 int i, j, k; 871 872 for ( i = 0; i < 4; i++ ) 873 { 874 for ( j = 0; j < 4; j++ ) 875 { 876 for ( k = 0; k < 4; k++ ) 877 { 878 ret.CNV( i, j ) += a.CNV( k, j ) * b.CNV( i, k ); 879 } 880 } 881 } 882 883 return ret; 884} 885 886//-------------------------------------------------------------------------------------------- 887static INLINE fmat_4x4_t Translate( const float dx, const float dy, const float dz ) 888{ 889 fmat_4x4_t ret = IdentityMatrix(); 890 891 ret.CNV( 3, 0 ) = dx; 892 ret.CNV( 3, 1 ) = dy; 893 ret.CNV( 3, 2 ) = dz; 894 895 return ret; 896} 897 898//-------------------------------------------------------------------------------------------- 899static INLINE fmat_4x4_t RotateX( const float rads ) 900{ 901 float cosine = COS( rads ); 902 float sine = SIN( rads ); 903 904 fmat_4x4_t ret = IdentityMatrix(); 905 906 ret.CNV( 1, 1 ) = cosine; 907 ret.CNV( 2, 2 ) = cosine; 908 ret.CNV( 1, 2 ) = -sine; 909 ret.CNV( 2, 1 ) = sine; 910 911 return ret; 912} 913 914//-------------------------------------------------------------------------------------------- 915static INLINE fmat_4x4_t RotateY( const float rads ) 916{ 917 float cosine = COS( rads ); 918 float sine = SIN( rads ); 919 920 fmat_4x4_t ret = IdentityMatrix(); 921 922 ret.CNV( 0, 0 ) = cosine; // 0,0 923 ret.CNV( 2, 2 ) = cosine; // 2,2 924 ret.CNV( 0, 2 ) = sine; // 0,2 925 ret.CNV( 2, 0 ) = -sine; // 2,0 926 927 return ret; 928} 929 930//-------------------------------------------------------------------------------------------- 931static INLINE fmat_4x4_t RotateZ( const float rads ) 932{ 933 float cosine = COS( rads ); 934 float sine = SIN( rads ); 935 936 fmat_4x4_t ret = IdentityMatrix(); 937 938 ret.CNV( 0, 0 ) = cosine; // 0,0 939 ret.CNV( 1, 1 ) = cosine; // 1,1 940 ret.CNV( 0, 1 ) = -sine; // 0,1 941 ret.CNV( 1, 0 ) = sine; // 1,0 942 943 return ret; 944} 945 946//-------------------------------------------------------------------------------------------- 947static INLINE fmat_4x4_t ScaleXYZ( const float sizex, const float sizey, const float sizez ) 948{ 949 fmat_4x4_t ret = IdentityMatrix(); 950 951 ret.CNV( 0, 0 ) = sizex; // 0,0 952 ret.CNV( 1, 1 ) = sizey; // 1,1 953 ret.CNV( 2, 2 ) = sizez; // 2,2 954 955 return ret; 956} 957 958//-------------------------------------------------------------------------------------------- 959static INLINE fmat_4x4_t ScaleXYZRotateXYZTranslate_SpaceFixed( const float scale_x, const float scale_y, const float scale_z, const Uint16 turn_z, const Uint16 turn_x, const Uint16 turn_y, const float translate_x, const float translate_y, const float translate_z ) 960{ 961 fmat_4x4_t ret; 962 963 float cx = turntocos[turn_x & TRIG_TABLE_MASK]; 964 float sx = turntosin[turn_x & TRIG_TABLE_MASK]; 965 float cy = turntocos[turn_y & TRIG_TABLE_MASK]; 966 float sy = turntosin[turn_y & TRIG_TABLE_MASK]; 967 float cz = turntocos[turn_z & TRIG_TABLE_MASK]; 968 float sz = turntosin[turn_z & TRIG_TABLE_MASK]; 969 970 ret.CNV( 0, 0 ) = scale_x * ( cz * cy ); 971 ret.CNV( 0, 1 ) = scale_x * ( cz * sy * sx + sz * cx ); 972 ret.CNV( 0, 2 ) = scale_x * ( sz * sx - cz * sy * cx ); 973 ret.CNV( 0, 3 ) = 0.0f; 974 975 ret.CNV( 1, 0 ) = scale_y * ( -sz * cy ); 976 ret.CNV( 1, 1 ) = scale_y * ( -sz * sy * sx + cz * cx ); 977 ret.CNV( 1, 2 ) = scale_y * ( sz * sy * cx + cz * sx ); 978 ret.CNV( 1, 3 ) = 0.0f; 979 980 ret.CNV( 2, 0 ) = scale_z * ( sy ); 981 ret.CNV( 2, 1 ) = scale_z * ( -cy * sx ); 982 ret.CNV( 2, 2 ) = scale_z * ( cy * cx ); 983 ret.CNV( 2, 3 ) = 0.0f; 984 985 ret.CNV( 3, 0 ) = translate_x; 986 ret.CNV( 3, 1 ) = translate_y; 987 ret.CNV( 3, 2 ) = translate_z; 988 ret.CNV( 3, 3 ) = 1.0f; 989 990 return ret; 991} 992 993//-------------------------------------------------------------------------------------------- 994static INLINE fmat_4x4_t ScaleXYZRotateXYZTranslate_BodyFixed( const float scale_x, const float scale_y, const float scale_z, const Uint16 turn_z, const Uint16 turn_x, const Uint16 turn_y, const float translate_x, const float translate_y, const float translate_z ) 995{ 996 /// @details BB@> Transpose the SpaceFixed representation and invert the angles to get the BodyFixed representation 997 998 fmat_4x4_t ret; 999 1000 float cx = turntocos[turn_x & TRIG_TABLE_MASK]; 1001 float sx = turntosin[turn_x & TRIG_TABLE_MASK]; 1002 float cy = turntocos[turn_y & TRIG_TABLE_MASK]; 1003 float sy = turntosin[turn_y & TRIG_TABLE_MASK]; 1004 float cz = turntocos[turn_z & TRIG_TABLE_MASK]; 1005 float sz = turntosin[turn_z & TRIG_TABLE_MASK]; 1006 1007 //ret.CNV( 0, 0 ) = scale_x * ( cz * cy); 1008 //ret.CNV( 0, 1 ) = scale_x * ( sz * cy); 1009 //ret.CNV( 0, 2 ) = scale_x * (-sy); 1010 //ret.CNV( 0, 3 ) = 0.0f; 1011 1012 //ret.CNV( 1, 0 ) = scale_y * (-sz * cx + cz * sy * sx); 1013 //ret.CNV( 1, 1 ) = scale_y * ( cz * cx + sz * sy * sx); 1014 //ret.CNV( 1, 2 ) = scale_y * ( cy * sx); 1015 //ret.CNV( 1, 3 ) = 0.0f; 1016 1017 //ret.CNV( 2, 0 ) = scale_z * ( sz * sx + cz * sy * cx); 1018 //ret.CNV( 2, 1 ) = scale_z * (-cz * sx + sz * sy * cx); 1019 //ret.CNV( 2, 2 ) = scale_z * ( cy * cx); 1020 //ret.CNV( 2, 3 ) = 0.0f; 1021 1022 ret.CNV( 0, 0 ) = scale_x * ( cz * cy - sz * sy * sx ); 1023 ret.CNV( 0, 1 ) = scale_x * ( sz * cy + cz * sy * sx ); 1024 ret.CNV( 0, 2 ) = scale_x * ( -cx * sy ); 1025 ret.CNV( 0, 3 ) = 0.0f; 1026 1027 ret.CNV( 1, 0 ) = scale_y * ( -sz * cx ); 1028 ret.CNV( 1, 1 ) = scale_y * ( cz * cx ); 1029 ret.CNV( 1, 2 ) = scale_y * ( sx ); 1030 ret.CNV( 1, 3 ) = 0.0f; 1031 1032 ret.CNV( 2, 0 ) = scale_z * ( cz * sy + sz * sx * cy ); 1033 ret.CNV( 2, 1 ) = scale_z * ( sz * sy - cz * sx * cy ); 1034 ret.CNV( 2, 2 ) = scale_z * ( cy * cx ); 1035 ret.CNV( 2, 3 ) = 0.0f; 1036 1037 ret.CNV( 3, 0 ) = translate_x; 1038 ret.CNV( 3, 1 ) = translate_y; 1039 ret.CNV( 3, 2 ) = translate_z; 1040 ret.CNV( 3, 3 ) = 1.0f; 1041 1042 return ret; 1043} 1044 1045//-------------------------------------------------------------------------------------------- 1046static INLINE fmat_4x4_t FourPoints( const fvec4_base_t ori, const fvec4_base_t wid, const fvec4_base_t frw, const fvec4_base_t up, const float scale ) 1047{ 1048 fmat_4x4_t tmp; 1049 1050 fvec3_t vWid, vFor, vUp; 1051 1052 vWid.x = wid[kX] - ori[kX]; 1053 vWid.y = wid[kY] - ori[kY]; 1054 vWid.z = wid[kZ] - ori[kZ]; 1055 1056 vUp.x = up[kX] - ori[kX]; 1057 vUp.y = up[kY] - ori[kY]; 1058 vUp.z = up[kZ] - ori[kZ]; 1059 1060 vFor.x = frw[kX] - ori[kX]; 1061 vFor.y = frw[kY] - ori[kY]; 1062 vFor.z = frw[kZ] - ori[kZ]; 1063 1064 fvec3_self_normalize( vWid.v ); 1065 fvec3_self_normalize( vUp.v ); 1066 fvec3_self_normalize( vFor.v ); 1067 1068 tmp.CNV( 0, 0 ) = -scale * vWid.x; // HUK 1069 tmp.CNV( 0, 1 ) = -scale * vWid.y; // HUK 1070 tmp.CNV( 0, 2 ) = -scale * vWid.z; // HUK 1071 tmp.CNV( 0, 3 ) = 0.0f; 1072 1073 tmp.CNV( 1, 0 ) = scale * vFor.x; 1074 tmp.CNV( 1, 1 ) = scale * vFor.y; 1075 tmp.CNV( 1, 2 ) = scale * vFor.z; 1076 tmp.CNV( 1, 3 ) = 0.0f; 1077 1078 tmp.CNV( 2, 0 ) = scale * vUp.x; 1079 tmp.CNV( 2, 1 ) = scale * vUp.y; 1080 tmp.CNV( 2, 2 ) = scale * vUp.z; 1081 tmp.CNV( 2, 3 ) = 0.0f; 1082 1083 tmp.CNV( 3, 0 ) = ori[kX]; 1084 tmp.CNV( 3, 1 ) = ori[kY]; 1085 tmp.CNV( 3, 2 ) = ori[kZ]; 1086 tmp.CNV( 3, 3 ) = 1.0f; 1087 1088 return tmp; 1089} 1090 1091//-------------------------------------------------------------------------------------------- 1092static INLINE fmat_4x4_t ViewMatrix( const fvec3_base_t from, // camera location 1093 const fvec3_base_t at, // camera look-at target 1094 const fvec3_base_t world_up, // world’s up, usually 0, 0, 1 1095 const float roll ) // clockwise roll around 1096// viewing direction, 1097// in radians 1098{ 1099 /// @details MN@> This probably should be replaced by a call to gluLookAt(), 1100 /// don't see why we need to make our own... 1101 1102 fmat_4x4_t view = IdentityMatrix(); 1103 fvec3_t up, right, view_dir, temp; 1104 1105 temp = fvec3_sub( at, from ); 1106 view_dir = fvec3_normalize( temp.v ); 1107 right = fvec3_cross_product( world_up, view_dir.v ); 1108 up = fvec3_cross_product( view_dir.v, right.v ); 1109 fvec3_self_normalize( right.v ); 1110 fvec3_self_normalize( up.v ); 1111 1112 view.CNV( 0, 0 ) = right.x; 1113 view.CNV( 1, 0 ) = right.y; 1114 view.CNV( 2, 0 ) = right.z; 1115 view.CNV( 0, 1 ) = up.x; 1116 view.CNV( 1, 1 ) = up.y; 1117 view.CNV( 2, 1 ) = up.z; 1118 view.CNV( 0, 2 ) = view_dir.x; 1119 view.CNV( 1, 2 ) = view_dir.y; 1120 view.CNV( 2, 2 ) = view_dir.z; 1121 view.CNV( 3, 0 ) = -fvec3_dot_product( right.v, from ); 1122 view.CNV( 3, 1 ) = -fvec3_dot_product( up.v, from ); 1123 view.CNV( 3, 2 ) = -fvec3_dot_product( view_dir.v, from ); 1124 1125 if ( roll != 0.0f ) 1126 { 1127 // MatrixMult function shown above 1128 view = MatrixMult( RotateZ( -roll ), view ); 1129 } 1130 1131 return view; 1132} 1133 1134//-------------------------------------------------------------------------------------------- 1135static INLINE fmat_4x4_t ProjectionMatrix( const float near_plane, // distance to near clipping plane 1136 const float far_plane, // distance to far clipping plane 1137 const float fov ) // field of view angle, in radians 1138{ 1139 /// @details MN@> Again, there is a gl function for this, glFrustum or gluPerspective... 1140 /// does this account for viewport ratio? 1141 1142 fmat_4x4_t ret = ZERO_MAT_4X4; 1143 1144 float c = COS( fov * 0.5f ); 1145 float s = SIN( fov * 0.5f ); 1146 float Q = s / ( 1.0f - near_plane / far_plane ); 1147 1148 ret.CNV( 0, 0 ) = c; // 0,0 1149 ret.CNV( 1, 1 ) = c; // 1,1 1150 ret.CNV( 2, 2 ) = Q; // 2,2 1151 ret.CNV( 3, 2 ) = -Q * near_plane; // 3,2 1152 ret.CNV( 2, 3 ) = s; // 2,3 1153 1154 return ret; 1155} 1156 1157//---------------------------------------------------- 1158static INLINE void TransformVertices( const fmat_4x4_t *pMatrix, const fvec4_t *pSourceV, fvec4_t *pDestV, const Uint32 NumVertor ) 1159{ 1160 /// @details GS@> This is just a MulVectorMatrix for now. The W division and screen size multiplication 1161 /// must be done afterward. 1162 /// 1163 /// BB@> the matrix transformation for OpenGL vertices. Some minor optimizations. 1164 /// The value pSourceV->w is assumed to be constant for all of the elements of pSourceV 1165 1166 Uint32 cnt; 1167 fvec4_t * SourceIt = ( fvec4_t * )pSourceV; 1168 1169 if ( 1.0f == SourceIt->w ) 1170 { 1171 for ( cnt = 0; cnt < NumVertor; cnt++ ) 1172 { 1173 pDestV->x = SourceIt->x * pMatrix->v[0] + SourceIt->y * pMatrix->v[4] + SourceIt->z * pMatrix->v[8] + pMatrix->v[12]; 1174 pDestV->y = SourceIt->x * pMatrix->v[1] + SourceIt->y * pMatrix->v[5] + SourceIt->z * pMatrix->v[9] + pMatrix->v[13]; 1175 pDestV->z = SourceIt->x * pMatrix->v[2] + SourceIt->y * pMatrix->v[6] + SourceIt->z * pMatrix->v[10] + pMatrix->v[14]; 1176 pDestV->w = SourceIt->x * pMatrix->v[3] + SourceIt->y * pMatrix->v[7] + SourceIt->z * pMatrix->v[11] + pMatrix->v[15]; 1177 1178 pDestV++; 1179 SourceIt++; 1180 } 1181 } 1182 else if ( 0.0f == SourceIt->w ) 1183 { 1184 for ( cnt = 0; cnt < NumVertor; cnt++ ) 1185 { 1186 pDestV->x = SourceIt->x * pMatrix->v[0] + SourceIt->y * pMatrix->v[4] + SourceIt->z * pMatrix->v[8]; 1187 pDestV->y = SourceIt->x * pMatrix->v[1] + SourceIt->y * pMatrix->v[5] + SourceIt->z * pMatrix->v[9]; 1188 pDestV->z = SourceIt->x * pMatrix->v[2] + SourceIt->y * pMatrix->v[6] + SourceIt->z * pMatrix->v[10]; 1189 pDestV->w = SourceIt->x * pMatrix->v[3] + SourceIt->y * pMatrix->v[7] + SourceIt->z * pMatrix->v[11]; 1190 1191 pDestV++; 1192 SourceIt++; 1193 } 1194 } 1195 else 1196 { 1197 for ( cnt = 0; cnt < NumVertor; cnt++ ) 1198 { 1199 pDestV->x = SourceIt->x * pMatrix->v[0] + SourceIt->y * pMatrix->v[4] + SourceIt->z * pMatrix->v[8] + SourceIt->w * pMatrix->v[12]; 1200 pDestV->y = SourceIt->x * pMatrix->v[1] + SourceIt->y * pMatrix->v[5] + SourceIt->z * pMatrix->v[9] + SourceIt->w * pMatrix->v[13]; 1201 pDestV->z = SourceIt->x * pMatrix->v[2] + SourceIt->y * pMatrix->v[6] + SourceIt->z * pMatrix->v[10] + SourceIt->w * pMatrix->v[14]; 1202 pDestV->w = SourceIt->x * pMatrix->v[3] + SourceIt->y * pMatrix->v[7] + SourceIt->z * pMatrix->v[11] + SourceIt->w * pMatrix->v[15]; 1203 1204 pDestV++; 1205 SourceIt++; 1206 } 1207 } 1208} 1209