00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef _VECTORMATH_MAT_AOS_CPP_H
00018 #define _VECTORMATH_MAT_AOS_CPP_H
00019
00020 namespace Vectormath {
00021 namespace Aos {
00022
00023
00024
00025
00026 #define _VECTORMATH_PI_OVER_2 1.570796327f
00027
00028
00029
00030
00031 inline Matrix3::Matrix3( const Matrix3 & mat )
00032 {
00033 mCol0 = mat.mCol0;
00034 mCol1 = mat.mCol1;
00035 mCol2 = mat.mCol2;
00036 }
00037
00038 inline Matrix3::Matrix3( float scalar )
00039 {
00040 mCol0 = Vector3( scalar );
00041 mCol1 = Vector3( scalar );
00042 mCol2 = Vector3( scalar );
00043 }
00044
00045 inline Matrix3::Matrix3( const Quat & unitQuat )
00046 {
00047 float qx, qy, qz, qw, qx2, qy2, qz2, qxqx2, qyqy2, qzqz2, qxqy2, qyqz2, qzqw2, qxqz2, qyqw2, qxqw2;
00048 qx = unitQuat.getX();
00049 qy = unitQuat.getY();
00050 qz = unitQuat.getZ();
00051 qw = unitQuat.getW();
00052 qx2 = ( qx + qx );
00053 qy2 = ( qy + qy );
00054 qz2 = ( qz + qz );
00055 qxqx2 = ( qx * qx2 );
00056 qxqy2 = ( qx * qy2 );
00057 qxqz2 = ( qx * qz2 );
00058 qxqw2 = ( qw * qx2 );
00059 qyqy2 = ( qy * qy2 );
00060 qyqz2 = ( qy * qz2 );
00061 qyqw2 = ( qw * qy2 );
00062 qzqz2 = ( qz * qz2 );
00063 qzqw2 = ( qw * qz2 );
00064 mCol0 = Vector3( ( ( 1.0f - qyqy2 ) - qzqz2 ), ( qxqy2 + qzqw2 ), ( qxqz2 - qyqw2 ) );
00065 mCol1 = Vector3( ( qxqy2 - qzqw2 ), ( ( 1.0f - qxqx2 ) - qzqz2 ), ( qyqz2 + qxqw2 ) );
00066 mCol2 = Vector3( ( qxqz2 + qyqw2 ), ( qyqz2 - qxqw2 ), ( ( 1.0f - qxqx2 ) - qyqy2 ) );
00067 }
00068
00069 inline Matrix3::Matrix3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2 )
00070 {
00071 mCol0 = _col0;
00072 mCol1 = _col1;
00073 mCol2 = _col2;
00074 }
00075
00076 inline Matrix3 & Matrix3::setCol0( const Vector3 & _col0 )
00077 {
00078 mCol0 = _col0;
00079 return *this;
00080 }
00081
00082 inline Matrix3 & Matrix3::setCol1( const Vector3 & _col1 )
00083 {
00084 mCol1 = _col1;
00085 return *this;
00086 }
00087
00088 inline Matrix3 & Matrix3::setCol2( const Vector3 & _col2 )
00089 {
00090 mCol2 = _col2;
00091 return *this;
00092 }
00093
00094 inline Matrix3 & Matrix3::setCol( int col, const Vector3 & vec )
00095 {
00096 *(&mCol0 + col) = vec;
00097 return *this;
00098 }
00099
00100 inline Matrix3 & Matrix3::setRow( int row, const Vector3 & vec )
00101 {
00102 mCol0.setElem( row, vec.getElem( 0 ) );
00103 mCol1.setElem( row, vec.getElem( 1 ) );
00104 mCol2.setElem( row, vec.getElem( 2 ) );
00105 return *this;
00106 }
00107
00108 inline Matrix3 & Matrix3::setElem( int col, int row, float val )
00109 {
00110 Vector3 tmpV3_0;
00111 tmpV3_0 = this->getCol( col );
00112 tmpV3_0.setElem( row, val );
00113 this->setCol( col, tmpV3_0 );
00114 return *this;
00115 }
00116
00117 inline float Matrix3::getElem( int col, int row ) const
00118 {
00119 return this->getCol( col ).getElem( row );
00120 }
00121
00122 inline const Vector3 Matrix3::getCol0( ) const
00123 {
00124 return mCol0;
00125 }
00126
00127 inline const Vector3 Matrix3::getCol1( ) const
00128 {
00129 return mCol1;
00130 }
00131
00132 inline const Vector3 Matrix3::getCol2( ) const
00133 {
00134 return mCol2;
00135 }
00136
00137 inline const Vector3 Matrix3::getCol( int col ) const
00138 {
00139 return *(&mCol0 + col);
00140 }
00141
00142 inline const Vector3 Matrix3::getRow( int row ) const
00143 {
00144 return Vector3( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ) );
00145 }
00146
00147 inline Vector3 & Matrix3::operator []( int col )
00148 {
00149 return *(&mCol0 + col);
00150 }
00151
00152 inline const Vector3 Matrix3::operator []( int col ) const
00153 {
00154 return *(&mCol0 + col);
00155 }
00156
00157 inline Matrix3 & Matrix3::operator =( const Matrix3 & mat )
00158 {
00159 mCol0 = mat.mCol0;
00160 mCol1 = mat.mCol1;
00161 mCol2 = mat.mCol2;
00162 return *this;
00163 }
00164
00165 inline const Matrix3 transpose( const Matrix3 & mat )
00166 {
00167 return Matrix3(
00168 Vector3( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX() ),
00169 Vector3( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY() ),
00170 Vector3( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ() )
00171 );
00172 }
00173
00174 inline const Matrix3 inverse( const Matrix3 & mat )
00175 {
00176 Vector3 tmp0, tmp1, tmp2;
00177 float detinv;
00178 tmp0 = cross( mat.getCol1(), mat.getCol2() );
00179 tmp1 = cross( mat.getCol2(), mat.getCol0() );
00180 tmp2 = cross( mat.getCol0(), mat.getCol1() );
00181 detinv = ( 1.0f / dot( mat.getCol2(), tmp2 ) );
00182 return Matrix3(
00183 Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) ),
00184 Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) ),
00185 Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) )
00186 );
00187 }
00188
00189 inline float determinant( const Matrix3 & mat )
00190 {
00191 return dot( mat.getCol2(), cross( mat.getCol0(), mat.getCol1() ) );
00192 }
00193
00194 inline const Matrix3 Matrix3::operator +( const Matrix3 & mat ) const
00195 {
00196 return Matrix3(
00197 ( mCol0 + mat.mCol0 ),
00198 ( mCol1 + mat.mCol1 ),
00199 ( mCol2 + mat.mCol2 )
00200 );
00201 }
00202
00203 inline const Matrix3 Matrix3::operator -( const Matrix3 & mat ) const
00204 {
00205 return Matrix3(
00206 ( mCol0 - mat.mCol0 ),
00207 ( mCol1 - mat.mCol1 ),
00208 ( mCol2 - mat.mCol2 )
00209 );
00210 }
00211
00212 inline Matrix3 & Matrix3::operator +=( const Matrix3 & mat )
00213 {
00214 *this = *this + mat;
00215 return *this;
00216 }
00217
00218 inline Matrix3 & Matrix3::operator -=( const Matrix3 & mat )
00219 {
00220 *this = *this - mat;
00221 return *this;
00222 }
00223
00224 inline const Matrix3 Matrix3::operator -( ) const
00225 {
00226 return Matrix3(
00227 ( -mCol0 ),
00228 ( -mCol1 ),
00229 ( -mCol2 )
00230 );
00231 }
00232
00233 inline const Matrix3 absPerElem( const Matrix3 & mat )
00234 {
00235 return Matrix3(
00236 absPerElem( mat.getCol0() ),
00237 absPerElem( mat.getCol1() ),
00238 absPerElem( mat.getCol2() )
00239 );
00240 }
00241
00242 inline const Matrix3 Matrix3::operator *( float scalar ) const
00243 {
00244 return Matrix3(
00245 ( mCol0 * scalar ),
00246 ( mCol1 * scalar ),
00247 ( mCol2 * scalar )
00248 );
00249 }
00250
00251 inline Matrix3 & Matrix3::operator *=( float scalar )
00252 {
00253 *this = *this * scalar;
00254 return *this;
00255 }
00256
00257 inline const Matrix3 operator *( float scalar, const Matrix3 & mat )
00258 {
00259 return mat * scalar;
00260 }
00261
00262 inline const Vector3 Matrix3::operator *( const Vector3 & vec ) const
00263 {
00264 return Vector3(
00265 ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
00266 ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
00267 ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
00268 );
00269 }
00270
00271 inline const Matrix3 Matrix3::operator *( const Matrix3 & mat ) const
00272 {
00273 return Matrix3(
00274 ( *this * mat.mCol0 ),
00275 ( *this * mat.mCol1 ),
00276 ( *this * mat.mCol2 )
00277 );
00278 }
00279
00280 inline Matrix3 & Matrix3::operator *=( const Matrix3 & mat )
00281 {
00282 *this = *this * mat;
00283 return *this;
00284 }
00285
00286 inline const Matrix3 mulPerElem( const Matrix3 & mat0, const Matrix3 & mat1 )
00287 {
00288 return Matrix3(
00289 mulPerElem( mat0.getCol0(), mat1.getCol0() ),
00290 mulPerElem( mat0.getCol1(), mat1.getCol1() ),
00291 mulPerElem( mat0.getCol2(), mat1.getCol2() )
00292 );
00293 }
00294
00295 inline const Matrix3 Matrix3::identity( )
00296 {
00297 return Matrix3(
00298 Vector3::xAxis( ),
00299 Vector3::yAxis( ),
00300 Vector3::zAxis( )
00301 );
00302 }
00303
00304 inline const Matrix3 Matrix3::rotationX( float radians )
00305 {
00306 float s, c;
00307 s = sinf( radians );
00308 c = cosf( radians );
00309 return Matrix3(
00310 Vector3::xAxis( ),
00311 Vector3( 0.0f, c, s ),
00312 Vector3( 0.0f, -s, c )
00313 );
00314 }
00315
00316 inline const Matrix3 Matrix3::rotationY( float radians )
00317 {
00318 float s, c;
00319 s = sinf( radians );
00320 c = cosf( radians );
00321 return Matrix3(
00322 Vector3( c, 0.0f, -s ),
00323 Vector3::yAxis( ),
00324 Vector3( s, 0.0f, c )
00325 );
00326 }
00327
00328 inline const Matrix3 Matrix3::rotationZ( float radians )
00329 {
00330 float s, c;
00331 s = sinf( radians );
00332 c = cosf( radians );
00333 return Matrix3(
00334 Vector3( c, s, 0.0f ),
00335 Vector3( -s, c, 0.0f ),
00336 Vector3::zAxis( )
00337 );
00338 }
00339
00340 inline const Matrix3 Matrix3::rotationZYX( const Vector3 & radiansXYZ )
00341 {
00342 float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
00343 sX = sinf( radiansXYZ.getX() );
00344 cX = cosf( radiansXYZ.getX() );
00345 sY = sinf( radiansXYZ.getY() );
00346 cY = cosf( radiansXYZ.getY() );
00347 sZ = sinf( radiansXYZ.getZ() );
00348 cZ = cosf( radiansXYZ.getZ() );
00349 tmp0 = ( cZ * sY );
00350 tmp1 = ( sZ * sY );
00351 return Matrix3(
00352 Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
00353 Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
00354 Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) )
00355 );
00356 }
00357
00358 inline const Matrix3 Matrix3::rotation( float radians, const Vector3 & unitVec )
00359 {
00360 float x, y, z, s, c, oneMinusC, xy, yz, zx;
00361 s = sinf( radians );
00362 c = cosf( radians );
00363 x = unitVec.getX();
00364 y = unitVec.getY();
00365 z = unitVec.getZ();
00366 xy = ( x * y );
00367 yz = ( y * z );
00368 zx = ( z * x );
00369 oneMinusC = ( 1.0f - c );
00370 return Matrix3(
00371 Vector3( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ) ),
00372 Vector3( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ) ),
00373 Vector3( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ) )
00374 );
00375 }
00376
00377 inline const Matrix3 Matrix3::rotation( const Quat & unitQuat )
00378 {
00379 return Matrix3( unitQuat );
00380 }
00381
00382 inline const Matrix3 Matrix3::scale( const Vector3 & scaleVec )
00383 {
00384 return Matrix3(
00385 Vector3( scaleVec.getX(), 0.0f, 0.0f ),
00386 Vector3( 0.0f, scaleVec.getY(), 0.0f ),
00387 Vector3( 0.0f, 0.0f, scaleVec.getZ() )
00388 );
00389 }
00390
00391 inline const Matrix3 appendScale( const Matrix3 & mat, const Vector3 & scaleVec )
00392 {
00393 return Matrix3(
00394 ( mat.getCol0() * scaleVec.getX( ) ),
00395 ( mat.getCol1() * scaleVec.getY( ) ),
00396 ( mat.getCol2() * scaleVec.getZ( ) )
00397 );
00398 }
00399
00400 inline const Matrix3 prependScale( const Vector3 & scaleVec, const Matrix3 & mat )
00401 {
00402 return Matrix3(
00403 mulPerElem( mat.getCol0(), scaleVec ),
00404 mulPerElem( mat.getCol1(), scaleVec ),
00405 mulPerElem( mat.getCol2(), scaleVec )
00406 );
00407 }
00408
00409 inline const Matrix3 select( const Matrix3 & mat0, const Matrix3 & mat1, bool select1 )
00410 {
00411 return Matrix3(
00412 select( mat0.getCol0(), mat1.getCol0(), select1 ),
00413 select( mat0.getCol1(), mat1.getCol1(), select1 ),
00414 select( mat0.getCol2(), mat1.getCol2(), select1 )
00415 );
00416 }
00417
00418 #ifdef _VECTORMATH_DEBUG
00419
00420 inline void print( const Matrix3 & mat )
00421 {
00422 print( mat.getRow( 0 ) );
00423 print( mat.getRow( 1 ) );
00424 print( mat.getRow( 2 ) );
00425 }
00426
00427 inline void print( const Matrix3 & mat, const char * name )
00428 {
00429 printf("%s:\n", name);
00430 print( mat );
00431 }
00432
00433 #endif
00434
00435 inline Matrix4::Matrix4( const Matrix4 & mat )
00436 {
00437 mCol0 = mat.mCol0;
00438 mCol1 = mat.mCol1;
00439 mCol2 = mat.mCol2;
00440 mCol3 = mat.mCol3;
00441 }
00442
00443 inline Matrix4::Matrix4( float scalar )
00444 {
00445 mCol0 = Vector4( scalar );
00446 mCol1 = Vector4( scalar );
00447 mCol2 = Vector4( scalar );
00448 mCol3 = Vector4( scalar );
00449 }
00450
00451 inline Matrix4::Matrix4( const Transform3 & mat )
00452 {
00453 mCol0 = Vector4( mat.getCol0(), 0.0f );
00454 mCol1 = Vector4( mat.getCol1(), 0.0f );
00455 mCol2 = Vector4( mat.getCol2(), 0.0f );
00456 mCol3 = Vector4( mat.getCol3(), 1.0f );
00457 }
00458
00459 inline Matrix4::Matrix4( const Vector4 & _col0, const Vector4 & _col1, const Vector4 & _col2, const Vector4 & _col3 )
00460 {
00461 mCol0 = _col0;
00462 mCol1 = _col1;
00463 mCol2 = _col2;
00464 mCol3 = _col3;
00465 }
00466
00467 inline Matrix4::Matrix4( const Matrix3 & mat, const Vector3 & translateVec )
00468 {
00469 mCol0 = Vector4( mat.getCol0(), 0.0f );
00470 mCol1 = Vector4( mat.getCol1(), 0.0f );
00471 mCol2 = Vector4( mat.getCol2(), 0.0f );
00472 mCol3 = Vector4( translateVec, 1.0f );
00473 }
00474
00475 inline Matrix4::Matrix4( const Quat & unitQuat, const Vector3 & translateVec )
00476 {
00477 Matrix3 mat;
00478 mat = Matrix3( unitQuat );
00479 mCol0 = Vector4( mat.getCol0(), 0.0f );
00480 mCol1 = Vector4( mat.getCol1(), 0.0f );
00481 mCol2 = Vector4( mat.getCol2(), 0.0f );
00482 mCol3 = Vector4( translateVec, 1.0f );
00483 }
00484
00485 inline Matrix4 & Matrix4::setCol0( const Vector4 & _col0 )
00486 {
00487 mCol0 = _col0;
00488 return *this;
00489 }
00490
00491 inline Matrix4 & Matrix4::setCol1( const Vector4 & _col1 )
00492 {
00493 mCol1 = _col1;
00494 return *this;
00495 }
00496
00497 inline Matrix4 & Matrix4::setCol2( const Vector4 & _col2 )
00498 {
00499 mCol2 = _col2;
00500 return *this;
00501 }
00502
00503 inline Matrix4 & Matrix4::setCol3( const Vector4 & _col3 )
00504 {
00505 mCol3 = _col3;
00506 return *this;
00507 }
00508
00509 inline Matrix4 & Matrix4::setCol( int col, const Vector4 & vec )
00510 {
00511 *(&mCol0 + col) = vec;
00512 return *this;
00513 }
00514
00515 inline Matrix4 & Matrix4::setRow( int row, const Vector4 & vec )
00516 {
00517 mCol0.setElem( row, vec.getElem( 0 ) );
00518 mCol1.setElem( row, vec.getElem( 1 ) );
00519 mCol2.setElem( row, vec.getElem( 2 ) );
00520 mCol3.setElem( row, vec.getElem( 3 ) );
00521 return *this;
00522 }
00523
00524 inline Matrix4 & Matrix4::setElem( int col, int row, float val )
00525 {
00526 Vector4 tmpV3_0;
00527 tmpV3_0 = this->getCol( col );
00528 tmpV3_0.setElem( row, val );
00529 this->setCol( col, tmpV3_0 );
00530 return *this;
00531 }
00532
00533 inline float Matrix4::getElem( int col, int row ) const
00534 {
00535 return this->getCol( col ).getElem( row );
00536 }
00537
00538 inline const Vector4 Matrix4::getCol0( ) const
00539 {
00540 return mCol0;
00541 }
00542
00543 inline const Vector4 Matrix4::getCol1( ) const
00544 {
00545 return mCol1;
00546 }
00547
00548 inline const Vector4 Matrix4::getCol2( ) const
00549 {
00550 return mCol2;
00551 }
00552
00553 inline const Vector4 Matrix4::getCol3( ) const
00554 {
00555 return mCol3;
00556 }
00557
00558 inline const Vector4 Matrix4::getCol( int col ) const
00559 {
00560 return *(&mCol0 + col);
00561 }
00562
00563 inline const Vector4 Matrix4::getRow( int row ) const
00564 {
00565 return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
00566 }
00567
00568 inline Vector4 & Matrix4::operator []( int col )
00569 {
00570 return *(&mCol0 + col);
00571 }
00572
00573 inline const Vector4 Matrix4::operator []( int col ) const
00574 {
00575 return *(&mCol0 + col);
00576 }
00577
00578 inline Matrix4 & Matrix4::operator =( const Matrix4 & mat )
00579 {
00580 mCol0 = mat.mCol0;
00581 mCol1 = mat.mCol1;
00582 mCol2 = mat.mCol2;
00583 mCol3 = mat.mCol3;
00584 return *this;
00585 }
00586
00587 inline const Matrix4 transpose( const Matrix4 & mat )
00588 {
00589 return Matrix4(
00590 Vector4( mat.getCol0().getX(), mat.getCol1().getX(), mat.getCol2().getX(), mat.getCol3().getX() ),
00591 Vector4( mat.getCol0().getY(), mat.getCol1().getY(), mat.getCol2().getY(), mat.getCol3().getY() ),
00592 Vector4( mat.getCol0().getZ(), mat.getCol1().getZ(), mat.getCol2().getZ(), mat.getCol3().getZ() ),
00593 Vector4( mat.getCol0().getW(), mat.getCol1().getW(), mat.getCol2().getW(), mat.getCol3().getW() )
00594 );
00595 }
00596
00597 inline const Matrix4 inverse( const Matrix4 & mat )
00598 {
00599 Vector4 res0, res1, res2, res3;
00600 float mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, detInv;
00601 mA = mat.getCol0().getX();
00602 mB = mat.getCol0().getY();
00603 mC = mat.getCol0().getZ();
00604 mD = mat.getCol0().getW();
00605 mE = mat.getCol1().getX();
00606 mF = mat.getCol1().getY();
00607 mG = mat.getCol1().getZ();
00608 mH = mat.getCol1().getW();
00609 mI = mat.getCol2().getX();
00610 mJ = mat.getCol2().getY();
00611 mK = mat.getCol2().getZ();
00612 mL = mat.getCol2().getW();
00613 mM = mat.getCol3().getX();
00614 mN = mat.getCol3().getY();
00615 mO = mat.getCol3().getZ();
00616 mP = mat.getCol3().getW();
00617 tmp0 = ( ( mK * mD ) - ( mC * mL ) );
00618 tmp1 = ( ( mO * mH ) - ( mG * mP ) );
00619 tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
00620 tmp3 = ( ( mF * mO ) - ( mN * mG ) );
00621 tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
00622 tmp5 = ( ( mN * mH ) - ( mF * mP ) );
00623 res0.setX( ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) ) );
00624 res0.setY( ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) ) );
00625 res0.setZ( ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) ) );
00626 res0.setW( ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) ) );
00627 detInv = ( 1.0f / ( ( ( ( mA * res0.getX() ) + ( mE * res0.getY() ) ) + ( mI * res0.getZ() ) ) + ( mM * res0.getW() ) ) );
00628 res1.setX( ( mI * tmp1 ) );
00629 res1.setY( ( mM * tmp0 ) );
00630 res1.setZ( ( mA * tmp1 ) );
00631 res1.setW( ( mE * tmp0 ) );
00632 res3.setX( ( mI * tmp3 ) );
00633 res3.setY( ( mM * tmp2 ) );
00634 res3.setZ( ( mA * tmp3 ) );
00635 res3.setW( ( mE * tmp2 ) );
00636 res2.setX( ( mI * tmp5 ) );
00637 res2.setY( ( mM * tmp4 ) );
00638 res2.setZ( ( mA * tmp5 ) );
00639 res2.setW( ( mE * tmp4 ) );
00640 tmp0 = ( ( mI * mB ) - ( mA * mJ ) );
00641 tmp1 = ( ( mM * mF ) - ( mE * mN ) );
00642 tmp2 = ( ( mI * mD ) - ( mA * mL ) );
00643 tmp3 = ( ( mM * mH ) - ( mE * mP ) );
00644 tmp4 = ( ( mI * mC ) - ( mA * mK ) );
00645 tmp5 = ( ( mM * mG ) - ( mE * mO ) );
00646 res2.setX( ( ( ( mL * tmp1 ) - ( mJ * tmp3 ) ) + res2.getX() ) );
00647 res2.setY( ( ( ( mP * tmp0 ) - ( mN * tmp2 ) ) + res2.getY() ) );
00648 res2.setZ( ( ( ( mB * tmp3 ) - ( mD * tmp1 ) ) - res2.getZ() ) );
00649 res2.setW( ( ( ( mF * tmp2 ) - ( mH * tmp0 ) ) - res2.getW() ) );
00650 res3.setX( ( ( ( mJ * tmp5 ) - ( mK * tmp1 ) ) + res3.getX() ) );
00651 res3.setY( ( ( ( mN * tmp4 ) - ( mO * tmp0 ) ) + res3.getY() ) );
00652 res3.setZ( ( ( ( mC * tmp1 ) - ( mB * tmp5 ) ) - res3.getZ() ) );
00653 res3.setW( ( ( ( mG * tmp0 ) - ( mF * tmp4 ) ) - res3.getW() ) );
00654 res1.setX( ( ( ( mK * tmp3 ) - ( mL * tmp5 ) ) - res1.getX() ) );
00655 res1.setY( ( ( ( mO * tmp2 ) - ( mP * tmp4 ) ) - res1.getY() ) );
00656 res1.setZ( ( ( ( mD * tmp5 ) - ( mC * tmp3 ) ) + res1.getZ() ) );
00657 res1.setW( ( ( ( mH * tmp4 ) - ( mG * tmp2 ) ) + res1.getW() ) );
00658 return Matrix4(
00659 ( res0 * detInv ),
00660 ( res1 * detInv ),
00661 ( res2 * detInv ),
00662 ( res3 * detInv )
00663 );
00664 }
00665
00666 inline const Matrix4 affineInverse( const Matrix4 & mat )
00667 {
00668 Transform3 affineMat;
00669 affineMat.setCol0( mat.getCol0().getXYZ( ) );
00670 affineMat.setCol1( mat.getCol1().getXYZ( ) );
00671 affineMat.setCol2( mat.getCol2().getXYZ( ) );
00672 affineMat.setCol3( mat.getCol3().getXYZ( ) );
00673 return Matrix4( inverse( affineMat ) );
00674 }
00675
00676 inline const Matrix4 orthoInverse( const Matrix4 & mat )
00677 {
00678 Transform3 affineMat;
00679 affineMat.setCol0( mat.getCol0().getXYZ( ) );
00680 affineMat.setCol1( mat.getCol1().getXYZ( ) );
00681 affineMat.setCol2( mat.getCol2().getXYZ( ) );
00682 affineMat.setCol3( mat.getCol3().getXYZ( ) );
00683 return Matrix4( orthoInverse( affineMat ) );
00684 }
00685
00686 inline float determinant( const Matrix4 & mat )
00687 {
00688 float dx, dy, dz, dw, mA, mB, mC, mD, mE, mF, mG, mH, mI, mJ, mK, mL, mM, mN, mO, mP, tmp0, tmp1, tmp2, tmp3, tmp4, tmp5;
00689 mA = mat.getCol0().getX();
00690 mB = mat.getCol0().getY();
00691 mC = mat.getCol0().getZ();
00692 mD = mat.getCol0().getW();
00693 mE = mat.getCol1().getX();
00694 mF = mat.getCol1().getY();
00695 mG = mat.getCol1().getZ();
00696 mH = mat.getCol1().getW();
00697 mI = mat.getCol2().getX();
00698 mJ = mat.getCol2().getY();
00699 mK = mat.getCol2().getZ();
00700 mL = mat.getCol2().getW();
00701 mM = mat.getCol3().getX();
00702 mN = mat.getCol3().getY();
00703 mO = mat.getCol3().getZ();
00704 mP = mat.getCol3().getW();
00705 tmp0 = ( ( mK * mD ) - ( mC * mL ) );
00706 tmp1 = ( ( mO * mH ) - ( mG * mP ) );
00707 tmp2 = ( ( mB * mK ) - ( mJ * mC ) );
00708 tmp3 = ( ( mF * mO ) - ( mN * mG ) );
00709 tmp4 = ( ( mJ * mD ) - ( mB * mL ) );
00710 tmp5 = ( ( mN * mH ) - ( mF * mP ) );
00711 dx = ( ( ( mJ * tmp1 ) - ( mL * tmp3 ) ) - ( mK * tmp5 ) );
00712 dy = ( ( ( mN * tmp0 ) - ( mP * tmp2 ) ) - ( mO * tmp4 ) );
00713 dz = ( ( ( mD * tmp3 ) + ( mC * tmp5 ) ) - ( mB * tmp1 ) );
00714 dw = ( ( ( mH * tmp2 ) + ( mG * tmp4 ) ) - ( mF * tmp0 ) );
00715 return ( ( ( ( mA * dx ) + ( mE * dy ) ) + ( mI * dz ) ) + ( mM * dw ) );
00716 }
00717
00718 inline const Matrix4 Matrix4::operator +( const Matrix4 & mat ) const
00719 {
00720 return Matrix4(
00721 ( mCol0 + mat.mCol0 ),
00722 ( mCol1 + mat.mCol1 ),
00723 ( mCol2 + mat.mCol2 ),
00724 ( mCol3 + mat.mCol3 )
00725 );
00726 }
00727
00728 inline const Matrix4 Matrix4::operator -( const Matrix4 & mat ) const
00729 {
00730 return Matrix4(
00731 ( mCol0 - mat.mCol0 ),
00732 ( mCol1 - mat.mCol1 ),
00733 ( mCol2 - mat.mCol2 ),
00734 ( mCol3 - mat.mCol3 )
00735 );
00736 }
00737
00738 inline Matrix4 & Matrix4::operator +=( const Matrix4 & mat )
00739 {
00740 *this = *this + mat;
00741 return *this;
00742 }
00743
00744 inline Matrix4 & Matrix4::operator -=( const Matrix4 & mat )
00745 {
00746 *this = *this - mat;
00747 return *this;
00748 }
00749
00750 inline const Matrix4 Matrix4::operator -( ) const
00751 {
00752 return Matrix4(
00753 ( -mCol0 ),
00754 ( -mCol1 ),
00755 ( -mCol2 ),
00756 ( -mCol3 )
00757 );
00758 }
00759
00760 inline const Matrix4 absPerElem( const Matrix4 & mat )
00761 {
00762 return Matrix4(
00763 absPerElem( mat.getCol0() ),
00764 absPerElem( mat.getCol1() ),
00765 absPerElem( mat.getCol2() ),
00766 absPerElem( mat.getCol3() )
00767 );
00768 }
00769
00770 inline const Matrix4 Matrix4::operator *( float scalar ) const
00771 {
00772 return Matrix4(
00773 ( mCol0 * scalar ),
00774 ( mCol1 * scalar ),
00775 ( mCol2 * scalar ),
00776 ( mCol3 * scalar )
00777 );
00778 }
00779
00780 inline Matrix4 & Matrix4::operator *=( float scalar )
00781 {
00782 *this = *this * scalar;
00783 return *this;
00784 }
00785
00786 inline const Matrix4 operator *( float scalar, const Matrix4 & mat )
00787 {
00788 return mat * scalar;
00789 }
00790
00791 inline const Vector4 Matrix4::operator *( const Vector4 & vec ) const
00792 {
00793 return Vector4(
00794 ( ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ) + ( mCol3.getX() * vec.getW() ) ),
00795 ( ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ) + ( mCol3.getY() * vec.getW() ) ),
00796 ( ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ) + ( mCol3.getZ() * vec.getW() ) ),
00797 ( ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) ) + ( mCol3.getW() * vec.getW() ) )
00798 );
00799 }
00800
00801 inline const Vector4 Matrix4::operator *( const Vector3 & vec ) const
00802 {
00803 return Vector4(
00804 ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
00805 ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
00806 ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) ),
00807 ( ( ( mCol0.getW() * vec.getX() ) + ( mCol1.getW() * vec.getY() ) ) + ( mCol2.getW() * vec.getZ() ) )
00808 );
00809 }
00810
00811 inline const Vector4 Matrix4::operator *( const Point3 & pnt ) const
00812 {
00813 return Vector4(
00814 ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
00815 ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
00816 ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() ),
00817 ( ( ( ( mCol0.getW() * pnt.getX() ) + ( mCol1.getW() * pnt.getY() ) ) + ( mCol2.getW() * pnt.getZ() ) ) + mCol3.getW() )
00818 );
00819 }
00820
00821 inline const Matrix4 Matrix4::operator *( const Matrix4 & mat ) const
00822 {
00823 return Matrix4(
00824 ( *this * mat.mCol0 ),
00825 ( *this * mat.mCol1 ),
00826 ( *this * mat.mCol2 ),
00827 ( *this * mat.mCol3 )
00828 );
00829 }
00830
00831 inline Matrix4 & Matrix4::operator *=( const Matrix4 & mat )
00832 {
00833 *this = *this * mat;
00834 return *this;
00835 }
00836
00837 inline const Matrix4 Matrix4::operator *( const Transform3 & tfrm ) const
00838 {
00839 return Matrix4(
00840 ( *this * tfrm.getCol0() ),
00841 ( *this * tfrm.getCol1() ),
00842 ( *this * tfrm.getCol2() ),
00843 ( *this * Point3( tfrm.getCol3() ) )
00844 );
00845 }
00846
00847 inline Matrix4 & Matrix4::operator *=( const Transform3 & tfrm )
00848 {
00849 *this = *this * tfrm;
00850 return *this;
00851 }
00852
00853 inline const Matrix4 mulPerElem( const Matrix4 & mat0, const Matrix4 & mat1 )
00854 {
00855 return Matrix4(
00856 mulPerElem( mat0.getCol0(), mat1.getCol0() ),
00857 mulPerElem( mat0.getCol1(), mat1.getCol1() ),
00858 mulPerElem( mat0.getCol2(), mat1.getCol2() ),
00859 mulPerElem( mat0.getCol3(), mat1.getCol3() )
00860 );
00861 }
00862
00863 inline const Matrix4 Matrix4::identity( )
00864 {
00865 return Matrix4(
00866 Vector4::xAxis( ),
00867 Vector4::yAxis( ),
00868 Vector4::zAxis( ),
00869 Vector4::wAxis( )
00870 );
00871 }
00872
00873 inline Matrix4 & Matrix4::setUpper3x3( const Matrix3 & mat3 )
00874 {
00875 mCol0.setXYZ( mat3.getCol0() );
00876 mCol1.setXYZ( mat3.getCol1() );
00877 mCol2.setXYZ( mat3.getCol2() );
00878 return *this;
00879 }
00880
00881 inline const Matrix3 Matrix4::getUpper3x3( ) const
00882 {
00883 return Matrix3(
00884 mCol0.getXYZ( ),
00885 mCol1.getXYZ( ),
00886 mCol2.getXYZ( )
00887 );
00888 }
00889
00890 inline Matrix4 & Matrix4::setTranslation( const Vector3 & translateVec )
00891 {
00892 mCol3.setXYZ( translateVec );
00893 return *this;
00894 }
00895
00896 inline const Vector3 Matrix4::getTranslation( ) const
00897 {
00898 return mCol3.getXYZ( );
00899 }
00900
00901 inline const Matrix4 Matrix4::rotationX( float radians )
00902 {
00903 float s, c;
00904 s = sinf( radians );
00905 c = cosf( radians );
00906 return Matrix4(
00907 Vector4::xAxis( ),
00908 Vector4( 0.0f, c, s, 0.0f ),
00909 Vector4( 0.0f, -s, c, 0.0f ),
00910 Vector4::wAxis( )
00911 );
00912 }
00913
00914 inline const Matrix4 Matrix4::rotationY( float radians )
00915 {
00916 float s, c;
00917 s = sinf( radians );
00918 c = cosf( radians );
00919 return Matrix4(
00920 Vector4( c, 0.0f, -s, 0.0f ),
00921 Vector4::yAxis( ),
00922 Vector4( s, 0.0f, c, 0.0f ),
00923 Vector4::wAxis( )
00924 );
00925 }
00926
00927 inline const Matrix4 Matrix4::rotationZ( float radians )
00928 {
00929 float s, c;
00930 s = sinf( radians );
00931 c = cosf( radians );
00932 return Matrix4(
00933 Vector4( c, s, 0.0f, 0.0f ),
00934 Vector4( -s, c, 0.0f, 0.0f ),
00935 Vector4::zAxis( ),
00936 Vector4::wAxis( )
00937 );
00938 }
00939
00940 inline const Matrix4 Matrix4::rotationZYX( const Vector3 & radiansXYZ )
00941 {
00942 float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
00943 sX = sinf( radiansXYZ.getX() );
00944 cX = cosf( radiansXYZ.getX() );
00945 sY = sinf( radiansXYZ.getY() );
00946 cY = cosf( radiansXYZ.getY() );
00947 sZ = sinf( radiansXYZ.getZ() );
00948 cZ = cosf( radiansXYZ.getZ() );
00949 tmp0 = ( cZ * sY );
00950 tmp1 = ( sZ * sY );
00951 return Matrix4(
00952 Vector4( ( cZ * cY ), ( sZ * cY ), -sY, 0.0f ),
00953 Vector4( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ), 0.0f ),
00954 Vector4( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ), 0.0f ),
00955 Vector4::wAxis( )
00956 );
00957 }
00958
00959 inline const Matrix4 Matrix4::rotation( float radians, const Vector3 & unitVec )
00960 {
00961 float x, y, z, s, c, oneMinusC, xy, yz, zx;
00962 s = sinf( radians );
00963 c = cosf( radians );
00964 x = unitVec.getX();
00965 y = unitVec.getY();
00966 z = unitVec.getZ();
00967 xy = ( x * y );
00968 yz = ( y * z );
00969 zx = ( z * x );
00970 oneMinusC = ( 1.0f - c );
00971 return Matrix4(
00972 Vector4( ( ( ( x * x ) * oneMinusC ) + c ), ( ( xy * oneMinusC ) + ( z * s ) ), ( ( zx * oneMinusC ) - ( y * s ) ), 0.0f ),
00973 Vector4( ( ( xy * oneMinusC ) - ( z * s ) ), ( ( ( y * y ) * oneMinusC ) + c ), ( ( yz * oneMinusC ) + ( x * s ) ), 0.0f ),
00974 Vector4( ( ( zx * oneMinusC ) + ( y * s ) ), ( ( yz * oneMinusC ) - ( x * s ) ), ( ( ( z * z ) * oneMinusC ) + c ), 0.0f ),
00975 Vector4::wAxis( )
00976 );
00977 }
00978
00979 inline const Matrix4 Matrix4::rotation( const Quat & unitQuat )
00980 {
00981 return Matrix4( Transform3::rotation( unitQuat ) );
00982 }
00983
00984 inline const Matrix4 Matrix4::scale( const Vector3 & scaleVec )
00985 {
00986 return Matrix4(
00987 Vector4( scaleVec.getX(), 0.0f, 0.0f, 0.0f ),
00988 Vector4( 0.0f, scaleVec.getY(), 0.0f, 0.0f ),
00989 Vector4( 0.0f, 0.0f, scaleVec.getZ(), 0.0f ),
00990 Vector4::wAxis( )
00991 );
00992 }
00993
00994 inline const Matrix4 appendScale( const Matrix4 & mat, const Vector3 & scaleVec )
00995 {
00996 return Matrix4(
00997 ( mat.getCol0() * scaleVec.getX( ) ),
00998 ( mat.getCol1() * scaleVec.getY( ) ),
00999 ( mat.getCol2() * scaleVec.getZ( ) ),
01000 mat.getCol3()
01001 );
01002 }
01003
01004 inline const Matrix4 prependScale( const Vector3 & scaleVec, const Matrix4 & mat )
01005 {
01006 Vector4 scale4;
01007 scale4 = Vector4( scaleVec, 1.0f );
01008 return Matrix4(
01009 mulPerElem( mat.getCol0(), scale4 ),
01010 mulPerElem( mat.getCol1(), scale4 ),
01011 mulPerElem( mat.getCol2(), scale4 ),
01012 mulPerElem( mat.getCol3(), scale4 )
01013 );
01014 }
01015
01016 inline const Matrix4 Matrix4::translation( const Vector3 & translateVec )
01017 {
01018 return Matrix4(
01019 Vector4::xAxis( ),
01020 Vector4::yAxis( ),
01021 Vector4::zAxis( ),
01022 Vector4( translateVec, 1.0f )
01023 );
01024 }
01025
01026 inline const Matrix4 Matrix4::lookAt( const Point3 & eyePos, const Point3 & lookAtPos, const Vector3 & upVec )
01027 {
01028 Matrix4 m4EyeFrame;
01029 Vector3 v3X, v3Y, v3Z;
01030 v3Y = normalize( upVec );
01031 v3Z = normalize( ( eyePos - lookAtPos ) );
01032 v3X = normalize( cross( v3Y, v3Z ) );
01033 v3Y = cross( v3Z, v3X );
01034 m4EyeFrame = Matrix4( Vector4( v3X ), Vector4( v3Y ), Vector4( v3Z ), Vector4( eyePos ) );
01035 return orthoInverse( m4EyeFrame );
01036 }
01037
01038 inline const Matrix4 Matrix4::perspective( float fovyRadians, float aspect, float zNear, float zFar )
01039 {
01040 float f, rangeInv;
01041 f = tanf( ( (float)( _VECTORMATH_PI_OVER_2 ) - ( 0.5f * fovyRadians ) ) );
01042 rangeInv = ( 1.0f / ( zNear - zFar ) );
01043 return Matrix4(
01044 Vector4( ( f / aspect ), 0.0f, 0.0f, 0.0f ),
01045 Vector4( 0.0f, f, 0.0f, 0.0f ),
01046 Vector4( 0.0f, 0.0f, ( ( zNear + zFar ) * rangeInv ), -1.0f ),
01047 Vector4( 0.0f, 0.0f, ( ( ( zNear * zFar ) * rangeInv ) * 2.0f ), 0.0f )
01048 );
01049 }
01050
01051 inline const Matrix4 Matrix4::frustum( float left, float right, float bottom, float top, float zNear, float zFar )
01052 {
01053 float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf, n2;
01054 sum_rl = ( right + left );
01055 sum_tb = ( top + bottom );
01056 sum_nf = ( zNear + zFar );
01057 inv_rl = ( 1.0f / ( right - left ) );
01058 inv_tb = ( 1.0f / ( top - bottom ) );
01059 inv_nf = ( 1.0f / ( zNear - zFar ) );
01060 n2 = ( zNear + zNear );
01061 return Matrix4(
01062 Vector4( ( n2 * inv_rl ), 0.0f, 0.0f, 0.0f ),
01063 Vector4( 0.0f, ( n2 * inv_tb ), 0.0f, 0.0f ),
01064 Vector4( ( sum_rl * inv_rl ), ( sum_tb * inv_tb ), ( sum_nf * inv_nf ), -1.0f ),
01065 Vector4( 0.0f, 0.0f, ( ( n2 * inv_nf ) * zFar ), 0.0f )
01066 );
01067 }
01068
01069 inline const Matrix4 Matrix4::orthographic( float left, float right, float bottom, float top, float zNear, float zFar )
01070 {
01071 float sum_rl, sum_tb, sum_nf, inv_rl, inv_tb, inv_nf;
01072 sum_rl = ( right + left );
01073 sum_tb = ( top + bottom );
01074 sum_nf = ( zNear + zFar );
01075 inv_rl = ( 1.0f / ( right - left ) );
01076 inv_tb = ( 1.0f / ( top - bottom ) );
01077 inv_nf = ( 1.0f / ( zNear - zFar ) );
01078 return Matrix4(
01079 Vector4( ( inv_rl + inv_rl ), 0.0f, 0.0f, 0.0f ),
01080 Vector4( 0.0f, ( inv_tb + inv_tb ), 0.0f, 0.0f ),
01081 Vector4( 0.0f, 0.0f, ( inv_nf + inv_nf ), 0.0f ),
01082 Vector4( ( -sum_rl * inv_rl ), ( -sum_tb * inv_tb ), ( sum_nf * inv_nf ), 1.0f )
01083 );
01084 }
01085
01086 inline const Matrix4 select( const Matrix4 & mat0, const Matrix4 & mat1, bool select1 )
01087 {
01088 return Matrix4(
01089 select( mat0.getCol0(), mat1.getCol0(), select1 ),
01090 select( mat0.getCol1(), mat1.getCol1(), select1 ),
01091 select( mat0.getCol2(), mat1.getCol2(), select1 ),
01092 select( mat0.getCol3(), mat1.getCol3(), select1 )
01093 );
01094 }
01095
01096 #ifdef _VECTORMATH_DEBUG
01097
01098 inline void print( const Matrix4 & mat )
01099 {
01100 print( mat.getRow( 0 ) );
01101 print( mat.getRow( 1 ) );
01102 print( mat.getRow( 2 ) );
01103 print( mat.getRow( 3 ) );
01104 }
01105
01106 inline void print( const Matrix4 & mat, const char * name )
01107 {
01108 printf("%s:\n", name);
01109 print( mat );
01110 }
01111
01112 #endif
01113
01114 inline Transform3::Transform3( const Transform3 & tfrm )
01115 {
01116 mCol0 = tfrm.mCol0;
01117 mCol1 = tfrm.mCol1;
01118 mCol2 = tfrm.mCol2;
01119 mCol3 = tfrm.mCol3;
01120 }
01121
01122 inline Transform3::Transform3( float scalar )
01123 {
01124 mCol0 = Vector3( scalar );
01125 mCol1 = Vector3( scalar );
01126 mCol2 = Vector3( scalar );
01127 mCol3 = Vector3( scalar );
01128 }
01129
01130 inline Transform3::Transform3( const Vector3 & _col0, const Vector3 & _col1, const Vector3 & _col2, const Vector3 & _col3 )
01131 {
01132 mCol0 = _col0;
01133 mCol1 = _col1;
01134 mCol2 = _col2;
01135 mCol3 = _col3;
01136 }
01137
01138 inline Transform3::Transform3( const Matrix3 & tfrm, const Vector3 & translateVec )
01139 {
01140 this->setUpper3x3( tfrm );
01141 this->setTranslation( translateVec );
01142 }
01143
01144 inline Transform3::Transform3( const Quat & unitQuat, const Vector3 & translateVec )
01145 {
01146 this->setUpper3x3( Matrix3( unitQuat ) );
01147 this->setTranslation( translateVec );
01148 }
01149
01150 inline Transform3 & Transform3::setCol0( const Vector3 & _col0 )
01151 {
01152 mCol0 = _col0;
01153 return *this;
01154 }
01155
01156 inline Transform3 & Transform3::setCol1( const Vector3 & _col1 )
01157 {
01158 mCol1 = _col1;
01159 return *this;
01160 }
01161
01162 inline Transform3 & Transform3::setCol2( const Vector3 & _col2 )
01163 {
01164 mCol2 = _col2;
01165 return *this;
01166 }
01167
01168 inline Transform3 & Transform3::setCol3( const Vector3 & _col3 )
01169 {
01170 mCol3 = _col3;
01171 return *this;
01172 }
01173
01174 inline Transform3 & Transform3::setCol( int col, const Vector3 & vec )
01175 {
01176 *(&mCol0 + col) = vec;
01177 return *this;
01178 }
01179
01180 inline Transform3 & Transform3::setRow( int row, const Vector4 & vec )
01181 {
01182 mCol0.setElem( row, vec.getElem( 0 ) );
01183 mCol1.setElem( row, vec.getElem( 1 ) );
01184 mCol2.setElem( row, vec.getElem( 2 ) );
01185 mCol3.setElem( row, vec.getElem( 3 ) );
01186 return *this;
01187 }
01188
01189 inline Transform3 & Transform3::setElem( int col, int row, float val )
01190 {
01191 Vector3 tmpV3_0;
01192 tmpV3_0 = this->getCol( col );
01193 tmpV3_0.setElem( row, val );
01194 this->setCol( col, tmpV3_0 );
01195 return *this;
01196 }
01197
01198 inline float Transform3::getElem( int col, int row ) const
01199 {
01200 return this->getCol( col ).getElem( row );
01201 }
01202
01203 inline const Vector3 Transform3::getCol0( ) const
01204 {
01205 return mCol0;
01206 }
01207
01208 inline const Vector3 Transform3::getCol1( ) const
01209 {
01210 return mCol1;
01211 }
01212
01213 inline const Vector3 Transform3::getCol2( ) const
01214 {
01215 return mCol2;
01216 }
01217
01218 inline const Vector3 Transform3::getCol3( ) const
01219 {
01220 return mCol3;
01221 }
01222
01223 inline const Vector3 Transform3::getCol( int col ) const
01224 {
01225 return *(&mCol0 + col);
01226 }
01227
01228 inline const Vector4 Transform3::getRow( int row ) const
01229 {
01230 return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );
01231 }
01232
01233 inline Vector3 & Transform3::operator []( int col )
01234 {
01235 return *(&mCol0 + col);
01236 }
01237
01238 inline const Vector3 Transform3::operator []( int col ) const
01239 {
01240 return *(&mCol0 + col);
01241 }
01242
01243 inline Transform3 & Transform3::operator =( const Transform3 & tfrm )
01244 {
01245 mCol0 = tfrm.mCol0;
01246 mCol1 = tfrm.mCol1;
01247 mCol2 = tfrm.mCol2;
01248 mCol3 = tfrm.mCol3;
01249 return *this;
01250 }
01251
01252 inline const Transform3 inverse( const Transform3 & tfrm )
01253 {
01254 Vector3 tmp0, tmp1, tmp2, inv0, inv1, inv2;
01255 float detinv;
01256 tmp0 = cross( tfrm.getCol1(), tfrm.getCol2() );
01257 tmp1 = cross( tfrm.getCol2(), tfrm.getCol0() );
01258 tmp2 = cross( tfrm.getCol0(), tfrm.getCol1() );
01259 detinv = ( 1.0f / dot( tfrm.getCol2(), tmp2 ) );
01260 inv0 = Vector3( ( tmp0.getX() * detinv ), ( tmp1.getX() * detinv ), ( tmp2.getX() * detinv ) );
01261 inv1 = Vector3( ( tmp0.getY() * detinv ), ( tmp1.getY() * detinv ), ( tmp2.getY() * detinv ) );
01262 inv2 = Vector3( ( tmp0.getZ() * detinv ), ( tmp1.getZ() * detinv ), ( tmp2.getZ() * detinv ) );
01263 return Transform3(
01264 inv0,
01265 inv1,
01266 inv2,
01267 Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
01268 );
01269 }
01270
01271 inline const Transform3 orthoInverse( const Transform3 & tfrm )
01272 {
01273 Vector3 inv0, inv1, inv2;
01274 inv0 = Vector3( tfrm.getCol0().getX(), tfrm.getCol1().getX(), tfrm.getCol2().getX() );
01275 inv1 = Vector3( tfrm.getCol0().getY(), tfrm.getCol1().getY(), tfrm.getCol2().getY() );
01276 inv2 = Vector3( tfrm.getCol0().getZ(), tfrm.getCol1().getZ(), tfrm.getCol2().getZ() );
01277 return Transform3(
01278 inv0,
01279 inv1,
01280 inv2,
01281 Vector3( ( -( ( inv0 * tfrm.getCol3().getX() ) + ( ( inv1 * tfrm.getCol3().getY() ) + ( inv2 * tfrm.getCol3().getZ() ) ) ) ) )
01282 );
01283 }
01284
01285 inline const Transform3 absPerElem( const Transform3 & tfrm )
01286 {
01287 return Transform3(
01288 absPerElem( tfrm.getCol0() ),
01289 absPerElem( tfrm.getCol1() ),
01290 absPerElem( tfrm.getCol2() ),
01291 absPerElem( tfrm.getCol3() )
01292 );
01293 }
01294
01295 inline const Vector3 Transform3::operator *( const Vector3 & vec ) const
01296 {
01297 return Vector3(
01298 ( ( ( mCol0.getX() * vec.getX() ) + ( mCol1.getX() * vec.getY() ) ) + ( mCol2.getX() * vec.getZ() ) ),
01299 ( ( ( mCol0.getY() * vec.getX() ) + ( mCol1.getY() * vec.getY() ) ) + ( mCol2.getY() * vec.getZ() ) ),
01300 ( ( ( mCol0.getZ() * vec.getX() ) + ( mCol1.getZ() * vec.getY() ) ) + ( mCol2.getZ() * vec.getZ() ) )
01301 );
01302 }
01303
01304 inline const Point3 Transform3::operator *( const Point3 & pnt ) const
01305 {
01306 return Point3(
01307 ( ( ( ( mCol0.getX() * pnt.getX() ) + ( mCol1.getX() * pnt.getY() ) ) + ( mCol2.getX() * pnt.getZ() ) ) + mCol3.getX() ),
01308 ( ( ( ( mCol0.getY() * pnt.getX() ) + ( mCol1.getY() * pnt.getY() ) ) + ( mCol2.getY() * pnt.getZ() ) ) + mCol3.getY() ),
01309 ( ( ( ( mCol0.getZ() * pnt.getX() ) + ( mCol1.getZ() * pnt.getY() ) ) + ( mCol2.getZ() * pnt.getZ() ) ) + mCol3.getZ() )
01310 );
01311 }
01312
01313 inline const Transform3 Transform3::operator *( const Transform3 & tfrm ) const
01314 {
01315 return Transform3(
01316 ( *this * tfrm.mCol0 ),
01317 ( *this * tfrm.mCol1 ),
01318 ( *this * tfrm.mCol2 ),
01319 Vector3( ( *this * Point3( tfrm.mCol3 ) ) )
01320 );
01321 }
01322
01323 inline Transform3 & Transform3::operator *=( const Transform3 & tfrm )
01324 {
01325 *this = *this * tfrm;
01326 return *this;
01327 }
01328
01329 inline const Transform3 mulPerElem( const Transform3 & tfrm0, const Transform3 & tfrm1 )
01330 {
01331 return Transform3(
01332 mulPerElem( tfrm0.getCol0(), tfrm1.getCol0() ),
01333 mulPerElem( tfrm0.getCol1(), tfrm1.getCol1() ),
01334 mulPerElem( tfrm0.getCol2(), tfrm1.getCol2() ),
01335 mulPerElem( tfrm0.getCol3(), tfrm1.getCol3() )
01336 );
01337 }
01338
01339 inline const Transform3 Transform3::identity( )
01340 {
01341 return Transform3(
01342 Vector3::xAxis( ),
01343 Vector3::yAxis( ),
01344 Vector3::zAxis( ),
01345 Vector3( 0.0f )
01346 );
01347 }
01348
01349 inline Transform3 & Transform3::setUpper3x3( const Matrix3 & tfrm )
01350 {
01351 mCol0 = tfrm.getCol0();
01352 mCol1 = tfrm.getCol1();
01353 mCol2 = tfrm.getCol2();
01354 return *this;
01355 }
01356
01357 inline const Matrix3 Transform3::getUpper3x3( ) const
01358 {
01359 return Matrix3( mCol0, mCol1, mCol2 );
01360 }
01361
01362 inline Transform3 & Transform3::setTranslation( const Vector3 & translateVec )
01363 {
01364 mCol3 = translateVec;
01365 return *this;
01366 }
01367
01368 inline const Vector3 Transform3::getTranslation( ) const
01369 {
01370 return mCol3;
01371 }
01372
01373 inline const Transform3 Transform3::rotationX( float radians )
01374 {
01375 float s, c;
01376 s = sinf( radians );
01377 c = cosf( radians );
01378 return Transform3(
01379 Vector3::xAxis( ),
01380 Vector3( 0.0f, c, s ),
01381 Vector3( 0.0f, -s, c ),
01382 Vector3( 0.0f )
01383 );
01384 }
01385
01386 inline const Transform3 Transform3::rotationY( float radians )
01387 {
01388 float s, c;
01389 s = sinf( radians );
01390 c = cosf( radians );
01391 return Transform3(
01392 Vector3( c, 0.0f, -s ),
01393 Vector3::yAxis( ),
01394 Vector3( s, 0.0f, c ),
01395 Vector3( 0.0f )
01396 );
01397 }
01398
01399 inline const Transform3 Transform3::rotationZ( float radians )
01400 {
01401 float s, c;
01402 s = sinf( radians );
01403 c = cosf( radians );
01404 return Transform3(
01405 Vector3( c, s, 0.0f ),
01406 Vector3( -s, c, 0.0f ),
01407 Vector3::zAxis( ),
01408 Vector3( 0.0f )
01409 );
01410 }
01411
01412 inline const Transform3 Transform3::rotationZYX( const Vector3 & radiansXYZ )
01413 {
01414 float sX, cX, sY, cY, sZ, cZ, tmp0, tmp1;
01415 sX = sinf( radiansXYZ.getX() );
01416 cX = cosf( radiansXYZ.getX() );
01417 sY = sinf( radiansXYZ.getY() );
01418 cY = cosf( radiansXYZ.getY() );
01419 sZ = sinf( radiansXYZ.getZ() );
01420 cZ = cosf( radiansXYZ.getZ() );
01421 tmp0 = ( cZ * sY );
01422 tmp1 = ( sZ * sY );
01423 return Transform3(
01424 Vector3( ( cZ * cY ), ( sZ * cY ), -sY ),
01425 Vector3( ( ( tmp0 * sX ) - ( sZ * cX ) ), ( ( tmp1 * sX ) + ( cZ * cX ) ), ( cY * sX ) ),
01426 Vector3( ( ( tmp0 * cX ) + ( sZ * sX ) ), ( ( tmp1 * cX ) - ( cZ * sX ) ), ( cY * cX ) ),
01427 Vector3( 0.0f )
01428 );
01429 }
01430
01431 inline const Transform3 Transform3::rotation( float radians, const Vector3 & unitVec )
01432 {
01433 return Transform3( Matrix3::rotation( radians, unitVec ), Vector3( 0.0f ) );
01434 }
01435
01436 inline const Transform3 Transform3::rotation( const Quat & unitQuat )
01437 {
01438 return Transform3( Matrix3( unitQuat ), Vector3( 0.0f ) );
01439 }
01440
01441 inline const Transform3 Transform3::scale( const Vector3 & scaleVec )
01442 {
01443 return Transform3(
01444 Vector3( scaleVec.getX(), 0.0f, 0.0f ),
01445 Vector3( 0.0f, scaleVec.getY(), 0.0f ),
01446 Vector3( 0.0f, 0.0f, scaleVec.getZ() ),
01447 Vector3( 0.0f )
01448 );
01449 }
01450
01451 inline const Transform3 appendScale( const Transform3 & tfrm, const Vector3 & scaleVec )
01452 {
01453 return Transform3(
01454 ( tfrm.getCol0() * scaleVec.getX( ) ),
01455 ( tfrm.getCol1() * scaleVec.getY( ) ),
01456 ( tfrm.getCol2() * scaleVec.getZ( ) ),
01457 tfrm.getCol3()
01458 );
01459 }
01460
01461 inline const Transform3 prependScale( const Vector3 & scaleVec, const Transform3 & tfrm )
01462 {
01463 return Transform3(
01464 mulPerElem( tfrm.getCol0(), scaleVec ),
01465 mulPerElem( tfrm.getCol1(), scaleVec ),
01466 mulPerElem( tfrm.getCol2(), scaleVec ),
01467 mulPerElem( tfrm.getCol3(), scaleVec )
01468 );
01469 }
01470
01471 inline const Transform3 Transform3::translation( const Vector3 & translateVec )
01472 {
01473 return Transform3(
01474 Vector3::xAxis( ),
01475 Vector3::yAxis( ),
01476 Vector3::zAxis( ),
01477 translateVec
01478 );
01479 }
01480
01481 inline const Transform3 select( const Transform3 & tfrm0, const Transform3 & tfrm1, bool select1 )
01482 {
01483 return Transform3(
01484 select( tfrm0.getCol0(), tfrm1.getCol0(), select1 ),
01485 select( tfrm0.getCol1(), tfrm1.getCol1(), select1 ),
01486 select( tfrm0.getCol2(), tfrm1.getCol2(), select1 ),
01487 select( tfrm0.getCol3(), tfrm1.getCol3(), select1 )
01488 );
01489 }
01490
01491 #ifdef _VECTORMATH_DEBUG
01492
01493 inline void print( const Transform3 & tfrm )
01494 {
01495 print( tfrm.getRow( 0 ) );
01496 print( tfrm.getRow( 1 ) );
01497 print( tfrm.getRow( 2 ) );
01498 }
01499
01500 inline void print( const Transform3 & tfrm, const char * name )
01501 {
01502 printf("%s:\n", name);
01503 print( tfrm );
01504 }
01505
01506 #endif
01507
01508 inline Quat::Quat( const Matrix3 & tfrm )
01509 {
01510 float trace, radicand, scale, xx, yx, zx, xy, yy, zy, xz, yz, zz, tmpx, tmpy, tmpz, tmpw, qx, qy, qz, qw;
01511 int negTrace, ZgtX, ZgtY, YgtX;
01512 int largestXorY, largestYorZ, largestZorX;
01513
01514 xx = tfrm.getCol0().getX();
01515 yx = tfrm.getCol0().getY();
01516 zx = tfrm.getCol0().getZ();
01517 xy = tfrm.getCol1().getX();
01518 yy = tfrm.getCol1().getY();
01519 zy = tfrm.getCol1().getZ();
01520 xz = tfrm.getCol2().getX();
01521 yz = tfrm.getCol2().getY();
01522 zz = tfrm.getCol2().getZ();
01523
01524 trace = ( ( xx + yy ) + zz );
01525
01526 negTrace = ( trace < 0.0f );
01527 ZgtX = zz > xx;
01528 ZgtY = zz > yy;
01529 YgtX = yy > xx;
01530 largestXorY = ( !ZgtX || !ZgtY ) && negTrace;
01531 largestYorZ = ( YgtX || ZgtX ) && negTrace;
01532 largestZorX = ( ZgtY || !YgtX ) && negTrace;
01533
01534 if ( largestXorY )
01535 {
01536 zz = -zz;
01537 xy = -xy;
01538 }
01539 if ( largestYorZ )
01540 {
01541 xx = -xx;
01542 yz = -yz;
01543 }
01544 if ( largestZorX )
01545 {
01546 yy = -yy;
01547 zx = -zx;
01548 }
01549
01550 radicand = ( ( ( xx + yy ) + zz ) + 1.0f );
01551 scale = ( 0.5f * ( 1.0f / sqrtf( radicand ) ) );
01552
01553 tmpx = ( ( zy - yz ) * scale );
01554 tmpy = ( ( xz - zx ) * scale );
01555 tmpz = ( ( yx - xy ) * scale );
01556 tmpw = ( radicand * scale );
01557 qx = tmpx;
01558 qy = tmpy;
01559 qz = tmpz;
01560 qw = tmpw;
01561
01562 if ( largestXorY )
01563 {
01564 qx = tmpw;
01565 qy = tmpz;
01566 qz = tmpy;
01567 qw = tmpx;
01568 }
01569 if ( largestYorZ )
01570 {
01571 tmpx = qx;
01572 tmpz = qz;
01573 qx = qy;
01574 qy = tmpx;
01575 qz = qw;
01576 qw = tmpz;
01577 }
01578
01579 mXYZW[0] = qx;
01580 mXYZW[1] = qy;
01581 mXYZW[2] = qz;
01582 mXYZW[3] = qw;
01583 }
01584
01585 inline const Matrix3 outer( const Vector3 & tfrm0, const Vector3 & tfrm1 )
01586 {
01587 return Matrix3(
01588 ( tfrm0 * tfrm1.getX( ) ),
01589 ( tfrm0 * tfrm1.getY( ) ),
01590 ( tfrm0 * tfrm1.getZ( ) )
01591 );
01592 }
01593
01594 inline const Matrix4 outer( const Vector4 & tfrm0, const Vector4 & tfrm1 )
01595 {
01596 return Matrix4(
01597 ( tfrm0 * tfrm1.getX( ) ),
01598 ( tfrm0 * tfrm1.getY( ) ),
01599 ( tfrm0 * tfrm1.getZ( ) ),
01600 ( tfrm0 * tfrm1.getW( ) )
01601 );
01602 }
01603
01604 inline const Vector3 rowMul( const Vector3 & vec, const Matrix3 & mat )
01605 {
01606 return Vector3(
01607 ( ( ( vec.getX() * mat.getCol0().getX() ) + ( vec.getY() * mat.getCol0().getY() ) ) + ( vec.getZ() * mat.getCol0().getZ() ) ),
01608 ( ( ( vec.getX() * mat.getCol1().getX() ) + ( vec.getY() * mat.getCol1().getY() ) ) + ( vec.getZ() * mat.getCol1().getZ() ) ),
01609 ( ( ( vec.getX() * mat.getCol2().getX() ) + ( vec.getY() * mat.getCol2().getY() ) ) + ( vec.getZ() * mat.getCol2().getZ() ) )
01610 );
01611 }
01612
01613 inline const Matrix3 crossMatrix( const Vector3 & vec )
01614 {
01615 return Matrix3(
01616 Vector3( 0.0f, vec.getZ(), -vec.getY() ),
01617 Vector3( -vec.getZ(), 0.0f, vec.getX() ),
01618 Vector3( vec.getY(), -vec.getX(), 0.0f )
01619 );
01620 }
01621
01622 inline const Matrix3 crossMatrixMul( const Vector3 & vec, const Matrix3 & mat )
01623 {
01624 return Matrix3( cross( vec, mat.getCol0() ), cross( vec, mat.getCol1() ), cross( vec, mat.getCol2() ) );
01625 }
01626
01627 }
01628 }
01629
01630 #endif
01631