Extras/PhysicsEffects/include/vecmath/sse/mat_aos.h

   1 /*\r
   2    Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.\r
   3    All rights reserved.\r
   4 \r
   5    Redistribution and use in source and binary forms,\r
   6    with or without modification, are permitted provided that the\r
   7    following conditions are met:\r
   8     * Redistributions of source code must retain the above copyright\r
   9       notice, this list of conditions and the following disclaimer.\r
  10     * Redistributions in binary form must reproduce the above copyright\r
  11       notice, this list of conditions and the following disclaimer in the\r
  12       documentation and/or other materials provided with the distribution.\r
  13     * Neither the name of the Sony Computer Entertainment Inc nor the names\r
  14       of its contributors may be used to endorse or promote products derived\r
  15       from this software without specific prior written permission.\r
  16 \r
  17    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"\r
  18    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\r
  19    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\r
  20    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\r
  21    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\r
  22    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\r
  23    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\r
  24    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\r
  25    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\r
  26    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\r
  27    POSSIBILITY OF SUCH DAMAGE.\r
  28 */\r
  29 \r
  30 \r
  31 #ifndef _VECTORMATH_MAT_AOS_CPP_H\r
  32 #define _VECTORMATH_MAT_AOS_CPP_H\r
  33 \r
  34 namespace Vectormath {\r
  35 namespace Aos {\r
  36 \r
  37 //-----------------------------------------------------------------------------\r
  38 // Constants\r
  39 // for shuffles, words are labeled [x,y,z,w] [a,b,c,d]\r
  40 \r
  41 #define _VECTORMATH_PERM_ZBWX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_B, _VECTORMATH_PERM_W, _VECTORMATH_PERM_X })\r
  42 #define _VECTORMATH_PERM_XCYX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_C, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_X })\r
  43 #define _VECTORMATH_PERM_XYAB ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_A, _VECTORMATH_PERM_B })\r
  44 #define _VECTORMATH_PERM_ZWCD ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_W, _VECTORMATH_PERM_C, _VECTORMATH_PERM_D })\r
  45 #define _VECTORMATH_PERM_XZBX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_Z, _VECTORMATH_PERM_B, _VECTORMATH_PERM_X })     \r
  46 #define _VECTORMATH_PERM_CXXX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_C, _VECTORMATH_PERM_X, _VECTORMATH_PERM_X, _VECTORMATH_PERM_X })\r
  47 #define _VECTORMATH_PERM_YAXX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Y, _VECTORMATH_PERM_A, _VECTORMATH_PERM_X, _VECTORMATH_PERM_X })\r
  48 #define _VECTORMATH_PERM_XAZC ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_A, _VECTORMATH_PERM_Z, _VECTORMATH_PERM_C })\r
  49 #define _VECTORMATH_PERM_YXWZ ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Y, _VECTORMATH_PERM_X, _VECTORMATH_PERM_W, _VECTORMATH_PERM_Z })\r
  50 #define _VECTORMATH_PERM_YBWD ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Y, _VECTORMATH_PERM_B, _VECTORMATH_PERM_W, _VECTORMATH_PERM_D })\r
  51 #define _VECTORMATH_PERM_XYCX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_C, _VECTORMATH_PERM_X })\r
  52 #define _VECTORMATH_PERM_YCXY ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Y, _VECTORMATH_PERM_C, _VECTORMATH_PERM_X, _VECTORMATH_PERM_Y })\r
  53 #define _VECTORMATH_PERM_CXYC ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_C, _VECTORMATH_PERM_X, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_C })\r
  54 #define _VECTORMATH_PERM_ZAYX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_A, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_X })\r
  55 #define _VECTORMATH_PERM_BZXX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_B, _VECTORMATH_PERM_Z, _VECTORMATH_PERM_X, _VECTORMATH_PERM_X })\r
  56 #define _VECTORMATH_PERM_XZYA ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_X, _VECTORMATH_PERM_Z, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_A })\r
  57 #define _VECTORMATH_PERM_ZXXB ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Z, _VECTORMATH_PERM_X, _VECTORMATH_PERM_X, _VECTORMATH_PERM_B })\r
  58 #define _VECTORMATH_PERM_YXXC ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_Y, _VECTORMATH_PERM_X, _VECTORMATH_PERM_X, _VECTORMATH_PERM_C })\r
  59 #define _VECTORMATH_PERM_BBYX ((vec_uchar16)(vec_uint4){ _VECTORMATH_PERM_B, _VECTORMATH_PERM_B, _VECTORMATH_PERM_Y, _VECTORMATH_PERM_X })\r
  60 #define _VECTORMATH_PI_OVER_2 1.570796327f\r
  61 \r
  62 //-----------------------------------------------------------------------------\r
  63 // Definitions\r
  64 \r
  65 VECTORMATH_FORCE_INLINE Matrix3::Matrix3( const Matrix3 & mat )\r
  66 {\r
  67     mCol0 = mat.mCol0;\r
  68     mCol1 = mat.mCol1;\r
  69     mCol2 = mat.mCol2;\r
  70 }\r
  71 \r
  72 VECTORMATH_FORCE_INLINE Matrix3::Matrix3( float scalar )\r
  73 {\r
  74     mCol0 = Vector3( scalar );\r
  75     mCol1 = Vector3( scalar );\r
  76     mCol2 = Vector3( scalar );\r
  77 }\r
  78 \r
  79 VECTORMATH_FORCE_INLINE Matrix3::Matrix3( const floatInVec &scalar )\r
  80 {\r
  81     mCol0 = Vector3( scalar );\r
  82     mCol1 = Vector3( scalar );\r
  83     mCol2 = Vector3( scalar );\r
  84 }\r
  85 \r
  86 VECTORMATH_FORCE_INLINE Matrix3::Matrix3( const Quat &unitQuat )\r
  87 {\r
  88     __m128 xyzw_2, wwww, yzxw, zxyw, yzxw_2, zxyw_2;\r
  89     __m128 tmp0, tmp1, tmp2, tmp3, tmp4, tmp5;\r
  90         VM_ATTRIBUTE_ALIGN16 unsigned int sx[4] = {0xffffffff, 0, 0, 0};\r
  91         VM_ATTRIBUTE_ALIGN16 unsigned int sz[4] = {0, 0, 0xffffffff, 0};\r
  92         __m128 select_x = _mm_load_ps((float *)sx);\r
  93         __m128 select_z = _mm_load_ps((float *)sz);\r
  94 \r
  95     xyzw_2 = _mm_add_ps( unitQuat.get128(), unitQuat.get128() );\r
  96     wwww = _mm_shuffle_ps( unitQuat.get128(), unitQuat.get128(), _MM_SHUFFLE(3,3,3,3) );\r
  97         yzxw = _mm_shuffle_ps( unitQuat.get128(), unitQuat.get128(), _MM_SHUFFLE(3,0,2,1) );\r
  98         zxyw = _mm_shuffle_ps( unitQuat.get128(), unitQuat.get128(), _MM_SHUFFLE(3,1,0,2) );\r
  99     yzxw_2 = _mm_shuffle_ps( xyzw_2, xyzw_2, _MM_SHUFFLE(3,0,2,1) );\r
 100     zxyw_2 = _mm_shuffle_ps( xyzw_2, xyzw_2, _MM_SHUFFLE(3,1,0,2) );\r
 101 \r
 102     tmp0 = _mm_mul_ps( yzxw_2, wwww );                                                                  // tmp0 = 2yw, 2zw, 2xw, 2w2\r
 103         tmp1 = _mm_sub_ps( _mm_set1_ps(1.0f), _mm_mul_ps(yzxw, yzxw_2) );       // tmp1 = 1 - 2y2, 1 - 2z2, 1 - 2x2, 1 - 2w2\r
 104     tmp2 = _mm_mul_ps( yzxw, xyzw_2 );                                                                  // tmp2 = 2xy, 2yz, 2xz, 2w2\r
 105     tmp0 = _mm_add_ps( _mm_mul_ps(zxyw, xyzw_2), tmp0 );                                // tmp0 = 2yw + 2zx, 2zw + 2xy, 2xw + 2yz, 2w2 + 2w2\r
 106     tmp1 = _mm_sub_ps( tmp1, _mm_mul_ps(zxyw, zxyw_2) );                                // tmp1 = 1 - 2y2 - 2z2, 1 - 2z2 - 2x2, 1 - 2x2 - 2y2, 1 - 2w2 - 2w2\r
 107     tmp2 = _mm_sub_ps( tmp2, _mm_mul_ps(zxyw_2, wwww) );                                // tmp2 = 2xy - 2zw, 2yz - 2xw, 2xz - 2yw, 2w2 -2w2\r
 108 \r
 109     tmp3 = vec_sel( tmp0, tmp1, select_x );\r
 110     tmp4 = vec_sel( tmp1, tmp2, select_x );\r
 111     tmp5 = vec_sel( tmp2, tmp0, select_x );\r
 112     mCol0 = Vector3( vec_sel( tmp3, tmp2, select_z ) );\r
 113     mCol1 = Vector3( vec_sel( tmp4, tmp0, select_z ) );\r
 114     mCol2 = Vector3( vec_sel( tmp5, tmp1, select_z ) );\r
 115 }\r
 116 \r
 117 VECTORMATH_FORCE_INLINE Matrix3::Matrix3( const Vector3 &_col0, const Vector3 &_col1, const Vector3 &_col2 )\r
 118 {\r
 119     mCol0 = _col0;\r
 120     mCol1 = _col1;\r
 121     mCol2 = _col2;\r
 122 }\r
 123 \r
 124 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setCol0( const Vector3 &_col0 )\r
 125 {\r
 126     mCol0 = _col0;\r
 127     return *this;\r
 128 }\r
 129 \r
 130 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setCol1( const Vector3 &_col1 )\r
 131 {\r
 132     mCol1 = _col1;\r
 133     return *this;\r
 134 }\r
 135 \r
 136 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setCol2( const Vector3 &_col2 )\r
 137 {\r
 138     mCol2 = _col2;\r
 139     return *this;\r
 140 }\r
 141 \r
 142 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setCol( int col, const Vector3 &vec )\r
 143 {\r
 144     *(&mCol0 + col) = vec;\r
 145     return *this;\r
 146 }\r
 147 \r
 148 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setRow( int row, const Vector3 &vec )\r
 149 {\r
 150     mCol0.setElem( row, vec.getElem( 0 ) );\r
 151     mCol1.setElem( row, vec.getElem( 1 ) );\r
 152     mCol2.setElem( row, vec.getElem( 2 ) );\r
 153     return *this;\r
 154 }\r
 155 \r
 156 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setElem( int col, int row, float val )\r
 157 {\r
 158     (*this)[col].setElem(row, val);\r
 159     return *this;\r
 160 }\r
 161 \r
 162 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::setElem( int col, int row, const floatInVec &val )\r
 163 {\r
 164     Vector3 tmpV3_0;\r
 165     tmpV3_0 = this->getCol( col );\r
 166     tmpV3_0.setElem( row, val );\r
 167     this->setCol( col, tmpV3_0 );\r
 168     return *this;\r
 169 }\r
 170 \r
 171 VECTORMATH_FORCE_INLINE const floatInVec Matrix3::getElem( int col, int row ) const\r
 172 {\r
 173     return this->getCol( col ).getElem( row );\r
 174 }\r
 175 \r
 176 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::getCol0( ) const\r
 177 {\r
 178     return mCol0;\r
 179 }\r
 180 \r
 181 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::getCol1( ) const\r
 182 {\r
 183     return mCol1;\r
 184 }\r
 185 \r
 186 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::getCol2( ) const\r
 187 {\r
 188     return mCol2;\r
 189 }\r
 190 \r
 191 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::getCol( int col ) const\r
 192 {\r
 193     return *(&mCol0 + col);\r
 194 }\r
 195 \r
 196 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::getRow( int row ) const\r
 197 {\r
 198     return Vector3( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ) );\r
 199 }\r
 200 \r
 201 VECTORMATH_FORCE_INLINE Vector3 & Matrix3::operator []( int col )\r
 202 {\r
 203     return *(&mCol0 + col);\r
 204 }\r
 205 \r
 206 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::operator []( int col ) const\r
 207 {\r
 208     return *(&mCol0 + col);\r
 209 }\r
 210 \r
 211 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::operator =( const Matrix3 & mat )\r
 212 {\r
 213     mCol0 = mat.mCol0;\r
 214     mCol1 = mat.mCol1;\r
 215     mCol2 = mat.mCol2;\r
 216     return *this;\r
 217 }\r
 218 \r
 219 VECTORMATH_FORCE_INLINE const Matrix3 transpose( const Matrix3 & mat )\r
 220 {\r
 221     __m128 tmp0, tmp1, res0, res1, res2;\r
 222     tmp0 = vec_mergeh( mat.getCol0().get128(), mat.getCol2().get128() );\r
 223     tmp1 = vec_mergel( mat.getCol0().get128(), mat.getCol2().get128() );\r
 224     res0 = vec_mergeh( tmp0, mat.getCol1().get128() );\r
 225     //res1 = vec_perm( tmp0, mat.getCol1().get128(), _VECTORMATH_PERM_ZBWX );\r
 226         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
 227         res1 = _mm_shuffle_ps( tmp0, tmp0, _MM_SHUFFLE(0,3,2,2));\r
 228         res1 = vec_sel(res1, mat.getCol1().get128(), select_y);\r
 229     //res2 = vec_perm( tmp1, mat.getCol1().get128(), _VECTORMATH_PERM_XCYX );\r
 230         res2 = _mm_shuffle_ps( tmp1, tmp1, _MM_SHUFFLE(0,1,1,0));\r
 231         res2 = vec_sel(res2, vec_splat(mat.getCol1().get128(), 2), select_y);\r
 232     return Matrix3(\r
 233         Vector3( res0 ),\r
 234         Vector3( res1 ),\r
 235         Vector3( res2 )\r
 236     );\r
 237 }\r
 238 \r
 239 VECTORMATH_FORCE_INLINE const Matrix3 inverse( const Matrix3 & mat )\r
 240 {\r
 241     __m128 tmp0, tmp1, tmp2, tmp3, tmp4, dot, invdet, inv0, inv1, inv2;\r
 242     tmp2 = _vmathVfCross( mat.getCol0().get128(), mat.getCol1().get128() );\r
 243     tmp0 = _vmathVfCross( mat.getCol1().get128(), mat.getCol2().get128() );\r
 244     tmp1 = _vmathVfCross( mat.getCol2().get128(), mat.getCol0().get128() );\r
 245     dot = _vmathVfDot3( tmp2, mat.getCol2().get128() );\r
 246     dot = vec_splat( dot, 0 );\r
 247     invdet = recipf4( dot );\r
 248     tmp3 = vec_mergeh( tmp0, tmp2 );\r
 249     tmp4 = vec_mergel( tmp0, tmp2 );\r
 250     inv0 = vec_mergeh( tmp3, tmp1 );\r
 251     //inv1 = vec_perm( tmp3, tmp1, _VECTORMATH_PERM_ZBWX );\r
 252         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
 253         inv1 = _mm_shuffle_ps( tmp3, tmp3, _MM_SHUFFLE(0,3,2,2));\r
 254         inv1 = vec_sel(inv1, tmp1, select_y);\r
 255     //inv2 = vec_perm( tmp4, tmp1, _VECTORMATH_PERM_XCYX );\r
 256         inv2 = _mm_shuffle_ps( tmp4, tmp4, _MM_SHUFFLE(0,1,1,0));\r
 257         inv2 = vec_sel(inv2, vec_splat(tmp1, 2), select_y);\r
 258     inv0 = vec_mul( inv0, invdet );\r
 259     inv1 = vec_mul( inv1, invdet );\r
 260         inv2 = vec_mul( inv2, invdet );\r
 261     return Matrix3(\r
 262         Vector3( inv0 ),\r
 263         Vector3( inv1 ),\r
 264         Vector3( inv2 )\r
 265     );\r
 266 }\r
 267 \r
 268 VECTORMATH_FORCE_INLINE const floatInVec determinant( const Matrix3 & mat )\r
 269 {\r
 270     return dot( mat.getCol2(), cross( mat.getCol0(), mat.getCol1() ) );\r
 271 }\r
 272 \r
 273 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::operator +( const Matrix3 & mat ) const\r
 274 {\r
 275     return Matrix3(\r
 276         ( mCol0 + mat.mCol0 ),\r
 277         ( mCol1 + mat.mCol1 ),\r
 278         ( mCol2 + mat.mCol2 )\r
 279     );\r
 280 }\r
 281 \r
 282 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::operator -( const Matrix3 & mat ) const\r
 283 {\r
 284     return Matrix3(\r
 285         ( mCol0 - mat.mCol0 ),\r
 286         ( mCol1 - mat.mCol1 ),\r
 287         ( mCol2 - mat.mCol2 )\r
 288     );\r
 289 }\r
 290 \r
 291 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::operator +=( const Matrix3 & mat )\r
 292 {\r
 293     *this = *this + mat;\r
 294     return *this;\r
 295 }\r
 296 \r
 297 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::operator -=( const Matrix3 & mat )\r
 298 {\r
 299     *this = *this - mat;\r
 300     return *this;\r
 301 }\r
 302 \r
 303 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::operator -( ) const\r
 304 {\r
 305     return Matrix3(\r
 306         ( -mCol0 ),\r
 307         ( -mCol1 ),\r
 308         ( -mCol2 )\r
 309     );\r
 310 }\r
 311 \r
 312 VECTORMATH_FORCE_INLINE const Matrix3 absPerElem( const Matrix3 & mat )\r
 313 {\r
 314     return Matrix3(\r
 315         absPerElem( mat.getCol0() ),\r
 316         absPerElem( mat.getCol1() ),\r
 317         absPerElem( mat.getCol2() )\r
 318     );\r
 319 }\r
 320 \r
 321 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::operator *( float scalar ) const\r
 322 {\r
 323     return *this * floatInVec(scalar);\r
 324 }\r
 325 \r
 326 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::operator *( const floatInVec &scalar ) const\r
 327 {\r
 328     return Matrix3(\r
 329         ( mCol0 * scalar ),\r
 330         ( mCol1 * scalar ),\r
 331         ( mCol2 * scalar )\r
 332     );\r
 333 }\r
 334 \r
 335 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::operator *=( float scalar )\r
 336 {\r
 337     return *this *= floatInVec(scalar);\r
 338 }\r
 339 \r
 340 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::operator *=( const floatInVec &scalar )\r
 341 {\r
 342     *this = *this * scalar;\r
 343     return *this;\r
 344 }\r
 345 \r
 346 VECTORMATH_FORCE_INLINE const Matrix3 operator *( float scalar, const Matrix3 & mat )\r
 347 {\r
 348     return floatInVec(scalar) * mat;\r
 349 }\r
 350 \r
 351 VECTORMATH_FORCE_INLINE const Matrix3 operator *( const floatInVec &scalar, const Matrix3 & mat )\r
 352 {\r
 353     return mat * scalar;\r
 354 }\r
 355 \r
 356 VECTORMATH_FORCE_INLINE const Vector3 Matrix3::operator *( const Vector3 &vec ) const\r
 357 {\r
 358     __m128 res;\r
 359     __m128 xxxx, yyyy, zzzz;\r
 360     xxxx = vec_splat( vec.get128(), 0 );\r
 361     yyyy = vec_splat( vec.get128(), 1 );\r
 362     zzzz = vec_splat( vec.get128(), 2 );\r
 363     res = vec_mul( mCol0.get128(), xxxx );\r
 364     res = vec_madd( mCol1.get128(), yyyy, res );\r
 365     res = vec_madd( mCol2.get128(), zzzz, res );\r
 366     return Vector3( res );\r
 367 }\r
 368 \r
 369 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::operator *( const Matrix3 & mat ) const\r
 370 {\r
 371     return Matrix3(\r
 372         ( *this * mat.mCol0 ),\r
 373         ( *this * mat.mCol1 ),\r
 374         ( *this * mat.mCol2 )\r
 375     );\r
 376 }\r
 377 \r
 378 VECTORMATH_FORCE_INLINE Matrix3 & Matrix3::operator *=( const Matrix3 & mat )\r
 379 {\r
 380     *this = *this * mat;\r
 381     return *this;\r
 382 }\r
 383 \r
 384 VECTORMATH_FORCE_INLINE const Matrix3 mulPerElem( const Matrix3 & mat0, const Matrix3 & mat1 )\r
 385 {\r
 386     return Matrix3(\r
 387         mulPerElem( mat0.getCol0(), mat1.getCol0() ),\r
 388         mulPerElem( mat0.getCol1(), mat1.getCol1() ),\r
 389         mulPerElem( mat0.getCol2(), mat1.getCol2() )\r
 390     );\r
 391 }\r
 392 \r
 393 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::identity( )\r
 394 {\r
 395     return Matrix3(\r
 396         Vector3::xAxis( ),\r
 397         Vector3::yAxis( ),\r
 398         Vector3::zAxis( )\r
 399     );\r
 400 }\r
 401 \r
 402 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationX( float radians )\r
 403 {\r
 404     return rotationX( floatInVec(radians) );\r
 405 }\r
 406 \r
 407 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationX( const floatInVec &radians )\r
 408 {\r
 409     __m128 s, c, res1, res2;\r
 410     __m128 zero;\r
 411         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
 412         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
 413     zero = _mm_setzero_ps();\r
 414     sincosf4( radians.get128(), &s, &c );\r
 415     res1 = vec_sel( zero, c, select_y );\r
 416     res1 = vec_sel( res1, s, select_z );\r
 417     res2 = vec_sel( zero, negatef4(s), select_y );\r
 418     res2 = vec_sel( res2, c, select_z );\r
 419     return Matrix3(\r
 420         Vector3::xAxis( ),\r
 421         Vector3( res1 ),\r
 422         Vector3( res2 )\r
 423     );\r
 424 }\r
 425 \r
 426 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationY( float radians )\r
 427 {\r
 428     return rotationY( floatInVec(radians) );\r
 429 }\r
 430 \r
 431 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationY( const floatInVec &radians )\r
 432 {\r
 433     __m128 s, c, res0, res2;\r
 434     __m128 zero;\r
 435         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
 436         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
 437     zero = _mm_setzero_ps();\r
 438     sincosf4( radians.get128(), &s, &c );\r
 439     res0 = vec_sel( zero, c, select_x );\r
 440     res0 = vec_sel( res0, negatef4(s), select_z );\r
 441     res2 = vec_sel( zero, s, select_x );\r
 442     res2 = vec_sel( res2, c, select_z );\r
 443     return Matrix3(\r
 444         Vector3( res0 ),\r
 445         Vector3::yAxis( ),\r
 446         Vector3( res2 )\r
 447         );\r
 448 }\r
 449 \r
 450 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationZ( float radians )\r
 451 {\r
 452     return rotationZ( floatInVec(radians) );\r
 453 }\r
 454 \r
 455 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationZ( const floatInVec &radians )\r
 456 {\r
 457     __m128 s, c, res0, res1;\r
 458     __m128 zero;\r
 459         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
 460         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
 461     zero = _mm_setzero_ps();\r
 462     sincosf4( radians.get128(), &s, &c );\r
 463     res0 = vec_sel( zero, c, select_x );\r
 464     res0 = vec_sel( res0, s, select_y );\r
 465     res1 = vec_sel( zero, negatef4(s), select_x );\r
 466     res1 = vec_sel( res1, c, select_y );\r
 467     return Matrix3(\r
 468         Vector3( res0 ),\r
 469         Vector3( res1 ),\r
 470         Vector3::zAxis( )\r
 471         );\r
 472 }\r
 473 \r
 474 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotationZYX( const Vector3 &radiansXYZ )\r
 475 {\r
 476     __m128 angles, s, negS, c, X0, X1, Y0, Y1, Z0, Z1, tmp;\r
 477     angles = Vector4( radiansXYZ, 0.0f ).get128();\r
 478     sincosf4( angles, &s, &c );\r
 479     negS = negatef4( s );\r
 480     Z0 = vec_mergel( c, s );\r
 481     Z1 = vec_mergel( negS, c );\r
 482         VM_ATTRIBUTE_ALIGN16 unsigned int select_xyz[4] = {0xffffffff, 0xffffffff, 0xffffffff, 0};\r
 483     Z1 = vec_and( Z1, _mm_load_ps( (float *)select_xyz ) );\r
 484         Y0 = _mm_shuffle_ps( c, negS, _MM_SHUFFLE(0,1,1,1) );\r
 485         Y1 = _mm_shuffle_ps( s, c, _MM_SHUFFLE(0,1,1,1) );\r
 486     X0 = vec_splat( s, 0 );\r
 487     X1 = vec_splat( c, 0 );\r
 488     tmp = vec_mul( Z0, Y1 );\r
 489     return Matrix3(\r
 490         Vector3( vec_mul( Z0, Y0 ) ),\r
 491         Vector3( vec_madd( Z1, X1, vec_mul( tmp, X0 ) ) ),\r
 492         Vector3( vec_nmsub( Z1, X0, vec_mul( tmp, X1 ) ) )\r
 493     );\r
 494 }\r
 495 \r
 496 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotation( float radians, const Vector3 &unitVec )\r
 497 {\r
 498     return rotation( floatInVec(radians), unitVec );\r
 499 }\r
 500 \r
 501 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotation( const floatInVec &radians, const Vector3 &unitVec )\r
 502 {\r
 503     __m128 axis, s, c, oneMinusC, axisS, negAxisS, xxxx, yyyy, zzzz, tmp0, tmp1, tmp2;\r
 504     axis = unitVec.get128();\r
 505     sincosf4( radians.get128(), &s, &c );\r
 506     xxxx = vec_splat( axis, 0 );\r
 507     yyyy = vec_splat( axis, 1 );\r
 508     zzzz = vec_splat( axis, 2 );\r
 509     oneMinusC = vec_sub( _mm_set1_ps(1.0f), c );\r
 510     axisS = vec_mul( axis, s );\r
 511     negAxisS = negatef4( axisS );\r
 512         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
 513         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
 514         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
 515     //tmp0 = vec_perm( axisS, negAxisS, _VECTORMATH_PERM_XZBX );\r
 516         tmp0 = _mm_shuffle_ps( axisS, axisS, _MM_SHUFFLE(0,0,2,0) );\r
 517         tmp0 = vec_sel(tmp0, vec_splat(negAxisS, 1), select_z);\r
 518     //tmp1 = vec_perm( axisS, negAxisS, _VECTORMATH_PERM_CXXX );\r
 519         tmp1 = vec_sel( vec_splat(axisS, 0), vec_splat(negAxisS, 2), select_x );\r
 520     //tmp2 = vec_perm( axisS, negAxisS, _VECTORMATH_PERM_YAXX );\r
 521         tmp2 = _mm_shuffle_ps( axisS, axisS, _MM_SHUFFLE(0,0,0,1) );\r
 522         tmp2 = vec_sel(tmp2, vec_splat(negAxisS, 0), select_y);\r
 523     tmp0 = vec_sel( tmp0, c, select_x );\r
 524     tmp1 = vec_sel( tmp1, c, select_y );\r
 525     tmp2 = vec_sel( tmp2, c, select_z );\r
 526     return Matrix3(\r
 527         Vector3( vec_madd( vec_mul( axis, xxxx ), oneMinusC, tmp0 ) ),\r
 528         Vector3( vec_madd( vec_mul( axis, yyyy ), oneMinusC, tmp1 ) ),\r
 529         Vector3( vec_madd( vec_mul( axis, zzzz ), oneMinusC, tmp2 ) )\r
 530     );\r
 531 }\r
 532 \r
 533 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::rotation( const Quat &unitQuat )\r
 534 {\r
 535     return Matrix3( unitQuat );\r
 536 }\r
 537 \r
 538 VECTORMATH_FORCE_INLINE const Matrix3 Matrix3::scale( const Vector3 &scaleVec )\r
 539 {\r
 540     __m128 zero = _mm_setzero_ps();\r
 541         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
 542         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
 543         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
 544     return Matrix3(\r
 545         Vector3( vec_sel( zero, scaleVec.get128(), select_x ) ),\r
 546         Vector3( vec_sel( zero, scaleVec.get128(), select_y ) ),\r
 547         Vector3( vec_sel( zero, scaleVec.get128(), select_z ) )\r
 548     );\r
 549 }\r
 550 \r
 551 VECTORMATH_FORCE_INLINE const Matrix3 appendScale( const Matrix3 & mat, const Vector3 &scaleVec )\r
 552 {\r
 553     return Matrix3(\r
 554         ( mat.getCol0() * scaleVec.getX( ) ),\r
 555         ( mat.getCol1() * scaleVec.getY( ) ),\r
 556         ( mat.getCol2() * scaleVec.getZ( ) )\r
 557     );\r
 558 }\r
 559 \r
 560 VECTORMATH_FORCE_INLINE const Matrix3 prependScale( const Vector3 &scaleVec, const Matrix3 & mat )\r
 561 {\r
 562     return Matrix3(\r
 563         mulPerElem( mat.getCol0(), scaleVec ),\r
 564         mulPerElem( mat.getCol1(), scaleVec ),\r
 565         mulPerElem( mat.getCol2(), scaleVec )\r
 566     );\r
 567 }\r
 568 \r
 569 VECTORMATH_FORCE_INLINE const Matrix3 select( const Matrix3 & mat0, const Matrix3 & mat1, bool select1 )\r
 570 {\r
 571     return Matrix3(\r
 572         select( mat0.getCol0(), mat1.getCol0(), select1 ),\r
 573         select( mat0.getCol1(), mat1.getCol1(), select1 ),\r
 574         select( mat0.getCol2(), mat1.getCol2(), select1 )\r
 575     );\r
 576 }\r
 577 \r
 578 VECTORMATH_FORCE_INLINE const Matrix3 select( const Matrix3 & mat0, const Matrix3 & mat1, const boolInVec &select1 )\r
 579 {\r
 580     return Matrix3(\r
 581         select( mat0.getCol0(), mat1.getCol0(), select1 ),\r
 582         select( mat0.getCol1(), mat1.getCol1(), select1 ),\r
 583         select( mat0.getCol2(), mat1.getCol2(), select1 )\r
 584     );\r
 585 }\r
 586 \r
 587 #ifdef _VECTORMATH_DEBUG\r
 588 \r
 589 VECTORMATH_FORCE_INLINE void print( const Matrix3 & mat )\r
 590 {\r
 591     print( mat.getRow( 0 ) );\r
 592     print( mat.getRow( 1 ) );\r
 593     print( mat.getRow( 2 ) );\r
 594 }\r
 595 \r
 596 VECTORMATH_FORCE_INLINE void print( const Matrix3 & mat, const char * name )\r
 597 {\r
 598     printf("%s:\n", name);\r
 599     print( mat );\r
 600 }\r
 601 \r
 602 #endif\r
 603 \r
 604 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( const Matrix4 & mat )\r
 605 {\r
 606     mCol0 = mat.mCol0;\r
 607     mCol1 = mat.mCol1;\r
 608     mCol2 = mat.mCol2;\r
 609     mCol3 = mat.mCol3;\r
 610 }\r
 611 \r
 612 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( float scalar )\r
 613 {\r
 614     mCol0 = Vector4( scalar );\r
 615     mCol1 = Vector4( scalar );\r
 616     mCol2 = Vector4( scalar );\r
 617     mCol3 = Vector4( scalar );\r
 618 }\r
 619 \r
 620 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( const floatInVec &scalar )\r
 621 {\r
 622     mCol0 = Vector4( scalar );\r
 623     mCol1 = Vector4( scalar );\r
 624     mCol2 = Vector4( scalar );\r
 625     mCol3 = Vector4( scalar );\r
 626 }\r
 627 \r
 628 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( const Transform3 & mat )\r
 629 {\r
 630     mCol0 = Vector4( mat.getCol0(), 0.0f );\r
 631     mCol1 = Vector4( mat.getCol1(), 0.0f );\r
 632     mCol2 = Vector4( mat.getCol2(), 0.0f );\r
 633     mCol3 = Vector4( mat.getCol3(), 1.0f );\r
 634 }\r
 635 \r
 636 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( const Vector4 &_col0, const Vector4 &_col1, const Vector4 &_col2, const Vector4 &_col3 )\r
 637 {\r
 638     mCol0 = _col0;\r
 639     mCol1 = _col1;\r
 640     mCol2 = _col2;\r
 641     mCol3 = _col3;\r
 642 }\r
 643 \r
 644 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( const Matrix3 & mat, const Vector3 &translateVec )\r
 645 {\r
 646     mCol0 = Vector4( mat.getCol0(), 0.0f );\r
 647     mCol1 = Vector4( mat.getCol1(), 0.0f );\r
 648     mCol2 = Vector4( mat.getCol2(), 0.0f );\r
 649     mCol3 = Vector4( translateVec, 1.0f );\r
 650 }\r
 651 \r
 652 VECTORMATH_FORCE_INLINE Matrix4::Matrix4( const Quat &unitQuat, const Vector3 &translateVec )\r
 653 {\r
 654     Matrix3 mat;\r
 655     mat = Matrix3( unitQuat );\r
 656     mCol0 = Vector4( mat.getCol0(), 0.0f );\r
 657     mCol1 = Vector4( mat.getCol1(), 0.0f );\r
 658     mCol2 = Vector4( mat.getCol2(), 0.0f );\r
 659     mCol3 = Vector4( translateVec, 1.0f );\r
 660 }\r
 661 \r
 662 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setCol0( const Vector4 &_col0 )\r
 663 {\r
 664     mCol0 = _col0;\r
 665     return *this;\r
 666 }\r
 667 \r
 668 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setCol1( const Vector4 &_col1 )\r
 669 {\r
 670     mCol1 = _col1;\r
 671     return *this;\r
 672 }\r
 673 \r
 674 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setCol2( const Vector4 &_col2 )\r
 675 {\r
 676     mCol2 = _col2;\r
 677     return *this;\r
 678 }\r
 679 \r
 680 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setCol3( const Vector4 &_col3 )\r
 681 {\r
 682     mCol3 = _col3;\r
 683     return *this;\r
 684 }\r
 685 \r
 686 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setCol( int col, const Vector4 &vec )\r
 687 {\r
 688     *(&mCol0 + col) = vec;\r
 689     return *this;\r
 690 }\r
 691 \r
 692 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setRow( int row, const Vector4 &vec )\r
 693 {\r
 694     mCol0.setElem( row, vec.getElem( 0 ) );\r
 695     mCol1.setElem( row, vec.getElem( 1 ) );\r
 696     mCol2.setElem( row, vec.getElem( 2 ) );\r
 697     mCol3.setElem( row, vec.getElem( 3 ) );\r
 698     return *this;\r
 699 }\r
 700 \r
 701 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setElem( int col, int row, float val )\r
 702 {\r
 703     (*this)[col].setElem(row, val);\r
 704     return *this;\r
 705 }\r
 706 \r
 707 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setElem( int col, int row, const floatInVec &val )\r
 708 {\r
 709     Vector4 tmpV3_0;\r
 710     tmpV3_0 = this->getCol( col );\r
 711     tmpV3_0.setElem( row, val );\r
 712     this->setCol( col, tmpV3_0 );\r
 713     return *this;\r
 714 }\r
 715 \r
 716 VECTORMATH_FORCE_INLINE const floatInVec Matrix4::getElem( int col, int row ) const\r
 717 {\r
 718     return this->getCol( col ).getElem( row );\r
 719 }\r
 720 \r
 721 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::getCol0( ) const\r
 722 {\r
 723     return mCol0;\r
 724 }\r
 725 \r
 726 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::getCol1( ) const\r
 727 {\r
 728     return mCol1;\r
 729 }\r
 730 \r
 731 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::getCol2( ) const\r
 732 {\r
 733     return mCol2;\r
 734 }\r
 735 \r
 736 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::getCol3( ) const\r
 737 {\r
 738     return mCol3;\r
 739 }\r
 740 \r
 741 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::getCol( int col ) const\r
 742 {\r
 743     return *(&mCol0 + col);\r
 744 }\r
 745 \r
 746 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::getRow( int row ) const\r
 747 {\r
 748     return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );\r
 749 }\r
 750 \r
 751 VECTORMATH_FORCE_INLINE Vector4 & Matrix4::operator []( int col )\r
 752 {\r
 753     return *(&mCol0 + col);\r
 754 }\r
 755 \r
 756 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::operator []( int col ) const\r
 757 {\r
 758     return *(&mCol0 + col);\r
 759 }\r
 760 \r
 761 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator =( const Matrix4 & mat )\r
 762 {\r
 763     mCol0 = mat.mCol0;\r
 764     mCol1 = mat.mCol1;\r
 765     mCol2 = mat.mCol2;\r
 766     mCol3 = mat.mCol3;\r
 767     return *this;\r
 768 }\r
 769 \r
 770 VECTORMATH_FORCE_INLINE const Matrix4 transpose( const Matrix4 & mat )\r
 771 {\r
 772     __m128 tmp0, tmp1, tmp2, tmp3, res0, res1, res2, res3;\r
 773     tmp0 = vec_mergeh( mat.getCol0().get128(), mat.getCol2().get128() );\r
 774     tmp1 = vec_mergeh( mat.getCol1().get128(), mat.getCol3().get128() );\r
 775     tmp2 = vec_mergel( mat.getCol0().get128(), mat.getCol2().get128() );\r
 776     tmp3 = vec_mergel( mat.getCol1().get128(), mat.getCol3().get128() );\r
 777     res0 = vec_mergeh( tmp0, tmp1 );\r
 778     res1 = vec_mergel( tmp0, tmp1 );\r
 779     res2 = vec_mergeh( tmp2, tmp3 );\r
 780     res3 = vec_mergel( tmp2, tmp3 );\r
 781     return Matrix4(\r
 782         Vector4( res0 ),\r
 783         Vector4( res1 ),\r
 784         Vector4( res2 ),\r
 785         Vector4( res3 )\r
 786     );\r
 787 }\r
 788 \r
 789 // TODO: Tidy\r
 790 static VM_ATTRIBUTE_ALIGN16 const unsigned int _vmathPNPN[4] = {0x00000000, 0x80000000, 0x00000000, 0x80000000};\r
 791 static VM_ATTRIBUTE_ALIGN16 const unsigned int _vmathNPNP[4] = {0x80000000, 0x00000000, 0x80000000, 0x00000000};\r
 792 static VM_ATTRIBUTE_ALIGN16 const float _vmathZERONE[4] = {1.0f, 0.0f, 0.0f, 1.0f};\r
 793 \r
 794 VECTORMATH_FORCE_INLINE const Matrix4 inverse( const Matrix4 & mat )\r
 795 {\r
 796         __m128 Va,Vb,Vc;\r
 797         __m128 r1,r2,r3,tt,tt2;\r
 798         __m128 sum,Det,RDet;\r
 799         __m128 trns0,trns1,trns2,trns3;\r
 800 \r
 801         __m128 _L1 = mat.getCol0().get128();\r
 802         __m128 _L2 = mat.getCol1().get128();\r
 803         __m128 _L3 = mat.getCol2().get128();\r
 804         __m128 _L4 = mat.getCol3().get128();\r
 805         // Calculating the minterms for the first line.\r
 806 \r
 807         // _mm_ror_ps is just a macro using _mm_shuffle_ps().\r
 808         tt = _L4; tt2 = _mm_ror_ps(_L3,1); \r
 809         Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0));                                  // V3'dot V4\r
 810         Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2));                                  // V3'dot V4"\r
 811         Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3));                                  // V3' dot V4^\r
 812 \r
 813         r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2));             // V3" dot V4^ - V3^ dot V4"\r
 814         r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0));             // V3^ dot V4' - V3' dot V4^\r
 815         r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1));             // V3' dot V4" - V3" dot V4'\r
 816 \r
 817         tt = _L2;\r
 818         Va = _mm_ror_ps(tt,1);          sum = _mm_mul_ps(Va,r1);\r
 819         Vb = _mm_ror_ps(tt,2);          sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));\r
 820         Vc = _mm_ror_ps(tt,3);          sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));\r
 821 \r
 822         // Calculating the determinant.\r
 823         Det = _mm_mul_ps(sum,_L1);\r
 824         Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));\r
 825 \r
 826         const __m128 Sign_PNPN = _mm_load_ps((float *)_vmathPNPN);\r
 827         const __m128 Sign_NPNP = _mm_load_ps((float *)_vmathNPNP);\r
 828 \r
 829         __m128 mtL1 = _mm_xor_ps(sum,Sign_PNPN);\r
 830 \r
 831         // Calculating the minterms of the second line (using previous results).\r
 832         tt = _mm_ror_ps(_L1,1);         sum = _mm_mul_ps(tt,r1);\r
 833         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));\r
 834         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));\r
 835         __m128 mtL2 = _mm_xor_ps(sum,Sign_NPNP);\r
 836 \r
 837         // Testing the determinant.\r
 838         Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));\r
 839 \r
 840         // Calculating the minterms of the third line.\r
 841         tt = _mm_ror_ps(_L1,1);\r
 842         Va = _mm_mul_ps(tt,Vb);                                                                 // V1' dot V2"\r
 843         Vb = _mm_mul_ps(tt,Vc);                                                                 // V1' dot V2^\r
 844         Vc = _mm_mul_ps(tt,_L2);                                                                // V1' dot V2\r
 845 \r
 846         r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2));             // V1" dot V2^ - V1^ dot V2"\r
 847         r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0));             // V1^ dot V2' - V1' dot V2^\r
 848         r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1));             // V1' dot V2" - V1" dot V2'\r
 849 \r
 850         tt = _mm_ror_ps(_L4,1);         sum = _mm_mul_ps(tt,r1);\r
 851         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));\r
 852         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));\r
 853         __m128 mtL3 = _mm_xor_ps(sum,Sign_PNPN);\r
 854 \r
 855         // Dividing is FASTER than rcp_nr! (Because rcp_nr causes many register-memory RWs).\r
 856         RDet = _mm_div_ss(_mm_load_ss((float *)&_vmathZERONE), Det); // TODO: just 1.0f?\r
 857         RDet = _mm_shuffle_ps(RDet,RDet,0x00);\r
 858 \r
 859         // Devide the first 12 minterms with the determinant.\r
 860         mtL1 = _mm_mul_ps(mtL1, RDet);\r
 861         mtL2 = _mm_mul_ps(mtL2, RDet);\r
 862         mtL3 = _mm_mul_ps(mtL3, RDet);\r
 863 \r
 864         // Calculate the minterms of the forth line and devide by the determinant.\r
 865         tt = _mm_ror_ps(_L3,1);         sum = _mm_mul_ps(tt,r1);\r
 866         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));\r
 867         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));\r
 868         __m128 mtL4 = _mm_xor_ps(sum,Sign_NPNP);\r
 869         mtL4 = _mm_mul_ps(mtL4, RDet);\r
 870 \r
 871         // Now we just have to transpose the minterms matrix.\r
 872         trns0 = _mm_unpacklo_ps(mtL1,mtL2);\r
 873         trns1 = _mm_unpacklo_ps(mtL3,mtL4);\r
 874         trns2 = _mm_unpackhi_ps(mtL1,mtL2);\r
 875         trns3 = _mm_unpackhi_ps(mtL3,mtL4);\r
 876         _L1 = _mm_movelh_ps(trns0,trns1);\r
 877         _L2 = _mm_movehl_ps(trns1,trns0);\r
 878         _L3 = _mm_movelh_ps(trns2,trns3);\r
 879         _L4 = _mm_movehl_ps(trns3,trns2);\r
 880 \r
 881     return Matrix4(\r
 882         Vector4( _L1 ),\r
 883         Vector4( _L2 ),\r
 884         Vector4( _L3 ),\r
 885         Vector4( _L4 )\r
 886     );\r
 887 }\r
 888 \r
 889 VECTORMATH_FORCE_INLINE const Matrix4 affineInverse( const Matrix4 & mat )\r
 890 {\r
 891     Transform3 affineMat;\r
 892     affineMat.setCol0( mat.getCol0().getXYZ( ) );\r
 893     affineMat.setCol1( mat.getCol1().getXYZ( ) );\r
 894     affineMat.setCol2( mat.getCol2().getXYZ( ) );\r
 895     affineMat.setCol3( mat.getCol3().getXYZ( ) );\r
 896     return Matrix4( inverse( affineMat ) );\r
 897 }\r
 898 \r
 899 VECTORMATH_FORCE_INLINE const Matrix4 orthoInverse( const Matrix4 & mat )\r
 900 {\r
 901     Transform3 affineMat;\r
 902     affineMat.setCol0( mat.getCol0().getXYZ( ) );\r
 903     affineMat.setCol1( mat.getCol1().getXYZ( ) );\r
 904     affineMat.setCol2( mat.getCol2().getXYZ( ) );\r
 905     affineMat.setCol3( mat.getCol3().getXYZ( ) );\r
 906     return Matrix4( orthoInverse( affineMat ) );\r
 907 }\r
 908 \r
 909 VECTORMATH_FORCE_INLINE const floatInVec determinant( const Matrix4 & mat )\r
 910 {\r
 911         __m128 Va,Vb,Vc;\r
 912         __m128 r1,r2,r3,tt,tt2;\r
 913         __m128 sum,Det;\r
 914 \r
 915         __m128 _L1 = mat.getCol0().get128();\r
 916         __m128 _L2 = mat.getCol1().get128();\r
 917         __m128 _L3 = mat.getCol2().get128();\r
 918         __m128 _L4 = mat.getCol3().get128();\r
 919         // Calculating the minterms for the first line.\r
 920 \r
 921         // _mm_ror_ps is just a macro using _mm_shuffle_ps().\r
 922         tt = _L4; tt2 = _mm_ror_ps(_L3,1); \r
 923         Vc = _mm_mul_ps(tt2,_mm_ror_ps(tt,0));                                  // V3' dot V4\r
 924         Va = _mm_mul_ps(tt2,_mm_ror_ps(tt,2));                                  // V3' dot V4"\r
 925         Vb = _mm_mul_ps(tt2,_mm_ror_ps(tt,3));                                  // V3' dot V4^\r
 926 \r
 927         r1 = _mm_sub_ps(_mm_ror_ps(Va,1),_mm_ror_ps(Vc,2));             // V3" dot V4^ - V3^ dot V4"\r
 928         r2 = _mm_sub_ps(_mm_ror_ps(Vb,2),_mm_ror_ps(Vb,0));             // V3^ dot V4' - V3' dot V4^\r
 929         r3 = _mm_sub_ps(_mm_ror_ps(Va,0),_mm_ror_ps(Vc,1));             // V3' dot V4" - V3" dot V4'\r
 930 \r
 931         tt = _L2;\r
 932         Va = _mm_ror_ps(tt,1);          sum = _mm_mul_ps(Va,r1);\r
 933         Vb = _mm_ror_ps(tt,2);          sum = _mm_add_ps(sum,_mm_mul_ps(Vb,r2));\r
 934         Vc = _mm_ror_ps(tt,3);          sum = _mm_add_ps(sum,_mm_mul_ps(Vc,r3));\r
 935 \r
 936         // Calculating the determinant.\r
 937         Det = _mm_mul_ps(sum,_L1);\r
 938         Det = _mm_add_ps(Det,_mm_movehl_ps(Det,Det));\r
 939 \r
 940         // Calculating the minterms of the second line (using previous results).\r
 941         tt = _mm_ror_ps(_L1,1);         sum = _mm_mul_ps(tt,r1);\r
 942         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r2));\r
 943         tt = _mm_ror_ps(tt,1);          sum = _mm_add_ps(sum,_mm_mul_ps(tt,r3));\r
 944 \r
 945         // Testing the determinant.\r
 946         Det = _mm_sub_ss(Det,_mm_shuffle_ps(Det,Det,1));\r
 947         return floatInVec(Det, 0);\r
 948 }\r
 949 \r
 950 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator +( const Matrix4 & mat ) const\r
 951 {\r
 952     return Matrix4(\r
 953         ( mCol0 + mat.mCol0 ),\r
 954         ( mCol1 + mat.mCol1 ),\r
 955         ( mCol2 + mat.mCol2 ),\r
 956         ( mCol3 + mat.mCol3 )\r
 957     );\r
 958 }\r
 959 \r
 960 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator -( const Matrix4 & mat ) const\r
 961 {\r
 962     return Matrix4(\r
 963         ( mCol0 - mat.mCol0 ),\r
 964         ( mCol1 - mat.mCol1 ),\r
 965         ( mCol2 - mat.mCol2 ),\r
 966         ( mCol3 - mat.mCol3 )\r
 967     );\r
 968 }\r
 969 \r
 970 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator +=( const Matrix4 & mat )\r
 971 {\r
 972     *this = *this + mat;\r
 973     return *this;\r
 974 }\r
 975 \r
 976 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator -=( const Matrix4 & mat )\r
 977 {\r
 978     *this = *this - mat;\r
 979     return *this;\r
 980 }\r
 981 \r
 982 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator -( ) const\r
 983 {\r
 984     return Matrix4(\r
 985         ( -mCol0 ),\r
 986         ( -mCol1 ),\r
 987         ( -mCol2 ),\r
 988         ( -mCol3 )\r
 989     );\r
 990 }\r
 991 \r
 992 VECTORMATH_FORCE_INLINE const Matrix4 absPerElem( const Matrix4 & mat )\r
 993 {\r
 994     return Matrix4(\r
 995         absPerElem( mat.getCol0() ),\r
 996         absPerElem( mat.getCol1() ),\r
 997         absPerElem( mat.getCol2() ),\r
 998         absPerElem( mat.getCol3() )\r
 999     );\r
1000 }\r
1001 \r
1002 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator *( float scalar ) const\r
1003 {\r
1004     return *this * floatInVec(scalar);\r
1005 }\r
1006 \r
1007 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator *( const floatInVec &scalar ) const\r
1008 {\r
1009     return Matrix4(\r
1010         ( mCol0 * scalar ),\r
1011         ( mCol1 * scalar ),\r
1012         ( mCol2 * scalar ),\r
1013         ( mCol3 * scalar )\r
1014     );\r
1015 }\r
1016 \r
1017 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator *=( float scalar )\r
1018 {\r
1019     return *this *= floatInVec(scalar);\r
1020 }\r
1021 \r
1022 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator *=( const floatInVec &scalar )\r
1023 {\r
1024     *this = *this * scalar;\r
1025     return *this;\r
1026 }\r
1027 \r
1028 VECTORMATH_FORCE_INLINE const Matrix4 operator *( float scalar, const Matrix4 & mat )\r
1029 {\r
1030     return floatInVec(scalar) * mat;\r
1031 }\r
1032 \r
1033 VECTORMATH_FORCE_INLINE const Matrix4 operator *( const floatInVec &scalar, const Matrix4 & mat )\r
1034 {\r
1035     return mat * scalar;\r
1036 }\r
1037 \r
1038 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::operator *( const Vector4 &vec ) const\r
1039 {\r
1040     return Vector4(\r
1041                 _mm_add_ps(\r
1042                         _mm_add_ps(_mm_mul_ps(mCol0.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(0,0,0,0))), _mm_mul_ps(mCol1.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(1,1,1,1)))),\r
1043                         _mm_add_ps(_mm_mul_ps(mCol2.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(2,2,2,2))), _mm_mul_ps(mCol3.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(3,3,3,3)))))\r
1044                 );\r
1045 }\r
1046 \r
1047 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::operator *( const Vector3 &vec ) const\r
1048 {\r
1049     return Vector4(\r
1050                 _mm_add_ps(\r
1051                         _mm_add_ps(_mm_mul_ps(mCol0.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(0,0,0,0))), _mm_mul_ps(mCol1.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(1,1,1,1)))),\r
1052                         _mm_mul_ps(mCol2.get128(), _mm_shuffle_ps(vec.get128(), vec.get128(), _MM_SHUFFLE(2,2,2,2))))\r
1053                 );\r
1054 }\r
1055 \r
1056 VECTORMATH_FORCE_INLINE const Vector4 Matrix4::operator *( const Point3 &pnt ) const\r
1057 {\r
1058     return Vector4(\r
1059                 _mm_add_ps(\r
1060                         _mm_add_ps(_mm_mul_ps(mCol0.get128(), _mm_shuffle_ps(pnt.get128(), pnt.get128(), _MM_SHUFFLE(0,0,0,0))), _mm_mul_ps(mCol1.get128(), _mm_shuffle_ps(pnt.get128(), pnt.get128(), _MM_SHUFFLE(1,1,1,1)))),\r
1061                         _mm_add_ps(_mm_mul_ps(mCol2.get128(), _mm_shuffle_ps(pnt.get128(), pnt.get128(), _MM_SHUFFLE(2,2,2,2))), mCol3.get128()))\r
1062                 );\r
1063 }\r
1064 \r
1065 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator *( const Matrix4 & mat ) const\r
1066 {\r
1067     return Matrix4(\r
1068         ( *this * mat.mCol0 ),\r
1069         ( *this * mat.mCol1 ),\r
1070         ( *this * mat.mCol2 ),\r
1071         ( *this * mat.mCol3 )\r
1072     );\r
1073 }\r
1074 \r
1075 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator *=( const Matrix4 & mat )\r
1076 {\r
1077     *this = *this * mat;\r
1078     return *this;\r
1079 }\r
1080 \r
1081 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::operator *( const Transform3 & tfrm ) const\r
1082 {\r
1083     return Matrix4(\r
1084         ( *this * tfrm.getCol0() ),\r
1085         ( *this * tfrm.getCol1() ),\r
1086         ( *this * tfrm.getCol2() ),\r
1087         ( *this * Point3( tfrm.getCol3() ) )\r
1088     );\r
1089 }\r
1090 \r
1091 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::operator *=( const Transform3 & tfrm )\r
1092 {\r
1093     *this = *this * tfrm;\r
1094     return *this;\r
1095 }\r
1096 \r
1097 VECTORMATH_FORCE_INLINE const Matrix4 mulPerElem( const Matrix4 & mat0, const Matrix4 & mat1 )\r
1098 {\r
1099     return Matrix4(\r
1100         mulPerElem( mat0.getCol0(), mat1.getCol0() ),\r
1101         mulPerElem( mat0.getCol1(), mat1.getCol1() ),\r
1102         mulPerElem( mat0.getCol2(), mat1.getCol2() ),\r
1103         mulPerElem( mat0.getCol3(), mat1.getCol3() )\r
1104     );\r
1105 }\r
1106 \r
1107 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::identity( )\r
1108 {\r
1109     return Matrix4(\r
1110         Vector4::xAxis( ),\r
1111         Vector4::yAxis( ),\r
1112         Vector4::zAxis( ),\r
1113         Vector4::wAxis( )\r
1114     );\r
1115 }\r
1116 \r
1117 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setUpper3x3( const Matrix3 & mat3 )\r
1118 {\r
1119     mCol0.setXYZ( mat3.getCol0() );\r
1120     mCol1.setXYZ( mat3.getCol1() );\r
1121     mCol2.setXYZ( mat3.getCol2() );\r
1122     return *this;\r
1123 }\r
1124 \r
1125 VECTORMATH_FORCE_INLINE const Matrix3 Matrix4::getUpper3x3( ) const\r
1126 {\r
1127     return Matrix3(\r
1128         mCol0.getXYZ( ),\r
1129         mCol1.getXYZ( ),\r
1130         mCol2.getXYZ( )\r
1131     );\r
1132 }\r
1133 \r
1134 VECTORMATH_FORCE_INLINE Matrix4 & Matrix4::setTranslation( const Vector3 &translateVec )\r
1135 {\r
1136     mCol3.setXYZ( translateVec );\r
1137     return *this;\r
1138 }\r
1139 \r
1140 VECTORMATH_FORCE_INLINE const Vector3 Matrix4::getTranslation( ) const\r
1141 {\r
1142     return mCol3.getXYZ( );\r
1143 }\r
1144 \r
1145 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationX( float radians )\r
1146 {\r
1147     return rotationX( floatInVec(radians) );\r
1148 }\r
1149 \r
1150 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationX( const floatInVec &radians )\r
1151 {\r
1152     __m128 s, c, res1, res2;\r
1153     __m128 zero;\r
1154         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1155         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1156     zero = _mm_setzero_ps();\r
1157     sincosf4( radians.get128(), &s, &c );\r
1158     res1 = vec_sel( zero, c, select_y );\r
1159     res1 = vec_sel( res1, s, select_z );\r
1160     res2 = vec_sel( zero, negatef4(s), select_y );\r
1161     res2 = vec_sel( res2, c, select_z );\r
1162     return Matrix4(\r
1163         Vector4::xAxis( ),\r
1164         Vector4( res1 ),\r
1165         Vector4( res2 ),\r
1166         Vector4::wAxis( )\r
1167     );\r
1168 }\r
1169 \r
1170 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationY( float radians )\r
1171 {\r
1172     return rotationY( floatInVec(radians) );\r
1173 }\r
1174 \r
1175 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationY( const floatInVec &radians )\r
1176 {\r
1177     __m128 s, c, res0, res2;\r
1178     __m128 zero;\r
1179         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1180         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1181     zero = _mm_setzero_ps();\r
1182     sincosf4( radians.get128(), &s, &c );\r
1183     res0 = vec_sel( zero, c, select_x );\r
1184     res0 = vec_sel( res0, negatef4(s), select_z );\r
1185     res2 = vec_sel( zero, s, select_x );\r
1186     res2 = vec_sel( res2, c, select_z );\r
1187     return Matrix4(\r
1188         Vector4( res0 ),\r
1189         Vector4::yAxis( ),\r
1190         Vector4( res2 ),\r
1191         Vector4::wAxis( )\r
1192     );\r
1193 }\r
1194 \r
1195 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationZ( float radians )\r
1196 {\r
1197     return rotationZ( floatInVec(radians) );\r
1198 }\r
1199 \r
1200 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationZ( const floatInVec &radians )\r
1201 {\r
1202     __m128 s, c, res0, res1;\r
1203     __m128 zero;\r
1204         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1205         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1206     zero = _mm_setzero_ps();\r
1207     sincosf4( radians.get128(), &s, &c );\r
1208     res0 = vec_sel( zero, c, select_x );\r
1209     res0 = vec_sel( res0, s, select_y );\r
1210     res1 = vec_sel( zero, negatef4(s), select_x );\r
1211     res1 = vec_sel( res1, c, select_y );\r
1212     return Matrix4(\r
1213         Vector4( res0 ),\r
1214         Vector4( res1 ),\r
1215         Vector4::zAxis( ),\r
1216         Vector4::wAxis( )\r
1217     );\r
1218 }\r
1219 \r
1220 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotationZYX( const Vector3 &radiansXYZ )\r
1221 {\r
1222     __m128 angles, s, negS, c, X0, X1, Y0, Y1, Z0, Z1, tmp;\r
1223     angles = Vector4( radiansXYZ, 0.0f ).get128();\r
1224     sincosf4( angles, &s, &c );\r
1225     negS = negatef4( s );\r
1226     Z0 = vec_mergel( c, s );\r
1227     Z1 = vec_mergel( negS, c );\r
1228         VM_ATTRIBUTE_ALIGN16 unsigned int select_xyz[4] = {0xffffffff, 0xffffffff, 0xffffffff, 0};\r
1229     Z1 = vec_and( Z1, _mm_load_ps( (float *)select_xyz ) );\r
1230         Y0 = _mm_shuffle_ps( c, negS, _MM_SHUFFLE(0,1,1,1) );\r
1231         Y1 = _mm_shuffle_ps( s, c, _MM_SHUFFLE(0,1,1,1) );\r
1232     X0 = vec_splat( s, 0 );\r
1233     X1 = vec_splat( c, 0 );\r
1234     tmp = vec_mul( Z0, Y1 );\r
1235     return Matrix4(\r
1236         Vector4( vec_mul( Z0, Y0 ) ),\r
1237         Vector4( vec_madd( Z1, X1, vec_mul( tmp, X0 ) ) ),\r
1238         Vector4( vec_nmsub( Z1, X0, vec_mul( tmp, X1 ) ) ),\r
1239         Vector4::wAxis( )\r
1240     );\r
1241 }\r
1242 \r
1243 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotation( float radians, const Vector3 &unitVec )\r
1244 {\r
1245     return rotation( floatInVec(radians), unitVec );\r
1246 }\r
1247 \r
1248 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotation( const floatInVec &radians, const Vector3 &unitVec )\r
1249 {\r
1250     __m128 axis, s, c, oneMinusC, axisS, negAxisS, xxxx, yyyy, zzzz, tmp0, tmp1, tmp2;\r
1251     axis = unitVec.get128();\r
1252     sincosf4( radians.get128(), &s, &c );\r
1253     xxxx = vec_splat( axis, 0 );\r
1254     yyyy = vec_splat( axis, 1 );\r
1255     zzzz = vec_splat( axis, 2 );\r
1256     oneMinusC = vec_sub( _mm_set1_ps(1.0f), c );\r
1257     axisS = vec_mul( axis, s );\r
1258     negAxisS = negatef4( axisS );\r
1259         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1260         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1261         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1262     //tmp0 = vec_perm( axisS, negAxisS, _VECTORMATH_PERM_XZBX );\r
1263         tmp0 = _mm_shuffle_ps( axisS, axisS, _MM_SHUFFLE(0,0,2,0) );\r
1264         tmp0 = vec_sel(tmp0, vec_splat(negAxisS, 1), select_z);\r
1265     //tmp1 = vec_perm( axisS, negAxisS, _VECTORMATH_PERM_CXXX );\r
1266         tmp1 = vec_sel( vec_splat(axisS, 0), vec_splat(negAxisS, 2), select_x );\r
1267     //tmp2 = vec_perm( axisS, negAxisS, _VECTORMATH_PERM_YAXX );\r
1268         tmp2 = _mm_shuffle_ps( axisS, axisS, _MM_SHUFFLE(0,0,0,1) );\r
1269         tmp2 = vec_sel(tmp2, vec_splat(negAxisS, 0), select_y);\r
1270     tmp0 = vec_sel( tmp0, c, select_x );\r
1271     tmp1 = vec_sel( tmp1, c, select_y );\r
1272     tmp2 = vec_sel( tmp2, c, select_z );\r
1273         VM_ATTRIBUTE_ALIGN16 unsigned int select_xyz[4] = {0xffffffff, 0xffffffff, 0xffffffff, 0};\r
1274     axis = vec_and( axis, _mm_load_ps( (float *)select_xyz ) );\r
1275     tmp0 = vec_and( tmp0, _mm_load_ps( (float *)select_xyz ) );\r
1276     tmp1 = vec_and( tmp1, _mm_load_ps( (float *)select_xyz ) );\r
1277     tmp2 = vec_and( tmp2, _mm_load_ps( (float *)select_xyz ) );\r
1278     return Matrix4(\r
1279         Vector4( vec_madd( vec_mul( axis, xxxx ), oneMinusC, tmp0 ) ),\r
1280         Vector4( vec_madd( vec_mul( axis, yyyy ), oneMinusC, tmp1 ) ),\r
1281         Vector4( vec_madd( vec_mul( axis, zzzz ), oneMinusC, tmp2 ) ),\r
1282         Vector4::wAxis( )\r
1283     );\r
1284 }\r
1285 \r
1286 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::rotation( const Quat &unitQuat )\r
1287 {\r
1288     return Matrix4( Transform3::rotation( unitQuat ) );\r
1289 }\r
1290 \r
1291 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::scale( const Vector3 &scaleVec )\r
1292 {\r
1293     __m128 zero = _mm_setzero_ps();\r
1294         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1295         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1296         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1297     return Matrix4(\r
1298         Vector4( vec_sel( zero, scaleVec.get128(), select_x ) ),\r
1299         Vector4( vec_sel( zero, scaleVec.get128(), select_y ) ),\r
1300         Vector4( vec_sel( zero, scaleVec.get128(), select_z ) ),\r
1301         Vector4::wAxis( )\r
1302     );\r
1303 }\r
1304 \r
1305 VECTORMATH_FORCE_INLINE const Matrix4 appendScale( const Matrix4 & mat, const Vector3 &scaleVec )\r
1306 {\r
1307     return Matrix4(\r
1308         ( mat.getCol0() * scaleVec.getX( ) ),\r
1309         ( mat.getCol1() * scaleVec.getY( ) ),\r
1310         ( mat.getCol2() * scaleVec.getZ( ) ),\r
1311         mat.getCol3()\r
1312     );\r
1313 }\r
1314 \r
1315 VECTORMATH_FORCE_INLINE const Matrix4 prependScale( const Vector3 &scaleVec, const Matrix4 & mat )\r
1316 {\r
1317     Vector4 scale4;\r
1318     scale4 = Vector4( scaleVec, 1.0f );\r
1319     return Matrix4(\r
1320         mulPerElem( mat.getCol0(), scale4 ),\r
1321         mulPerElem( mat.getCol1(), scale4 ),\r
1322         mulPerElem( mat.getCol2(), scale4 ),\r
1323         mulPerElem( mat.getCol3(), scale4 )\r
1324     );\r
1325 }\r
1326 \r
1327 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::translation( const Vector3 &translateVec )\r
1328 {\r
1329     return Matrix4(\r
1330         Vector4::xAxis( ),\r
1331         Vector4::yAxis( ),\r
1332         Vector4::zAxis( ),\r
1333         Vector4( translateVec, 1.0f )\r
1334     );\r
1335 }\r
1336 \r
1337 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::lookAt( const Point3 &eyePos, const Point3 &lookAtPos, const Vector3 &upVec )\r
1338 {\r
1339     Matrix4 m4EyeFrame;\r
1340     Vector3 v3X, v3Y, v3Z;\r
1341     v3Y = normalize( upVec );\r
1342     v3Z = normalize( ( eyePos - lookAtPos ) );\r
1343     v3X = normalize( cross( v3Y, v3Z ) );\r
1344     v3Y = cross( v3Z, v3X );\r
1345     m4EyeFrame = Matrix4( Vector4( v3X ), Vector4( v3Y ), Vector4( v3Z ), Vector4( eyePos ) );\r
1346     return orthoInverse( m4EyeFrame );\r
1347 }\r
1348 \r
1349 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::perspective( float fovyRadians, float aspect, float zNear, float zFar )\r
1350 {\r
1351     float f, rangeInv;\r
1352     __m128 zero, col0, col1, col2, col3;\r
1353     union { __m128 v; float s[4]; } tmp;\r
1354     f = tanf( _VECTORMATH_PI_OVER_2 - fovyRadians * 0.5f );\r
1355     rangeInv = 1.0f / ( zNear - zFar );\r
1356     zero = _mm_setzero_ps();\r
1357     tmp.v = zero;\r
1358     tmp.s[0] = f / aspect;\r
1359     col0 = tmp.v;\r
1360     tmp.v = zero;\r
1361     tmp.s[1] = f;\r
1362     col1 = tmp.v;\r
1363     tmp.v = zero;\r
1364     tmp.s[2] = ( zNear + zFar ) * rangeInv;\r
1365     tmp.s[3] = -1.0f;\r
1366     col2 = tmp.v;\r
1367     tmp.v = zero;\r
1368     tmp.s[2] = zNear * zFar * rangeInv * 2.0f;\r
1369     col3 = tmp.v;\r
1370     return Matrix4(\r
1371         Vector4( col0 ),\r
1372         Vector4( col1 ),\r
1373         Vector4( col2 ),\r
1374         Vector4( col3 )\r
1375     );\r
1376 }\r
1377 \r
1378 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::frustum( float left, float right, float bottom, float top, float zNear, float zFar )\r
1379 {\r
1380     /* function implementation based on code from STIDC SDK:           */\r
1381     /* --------------------------------------------------------------  */\r
1382     /* PLEASE DO NOT MODIFY THIS SECTION                               */\r
1383     /* This prolog section is automatically generated.                 */\r
1384     /*                                                                 */\r
1385     /* (C)Copyright                                                    */\r
1386     /* Sony Computer Entertainment, Inc.,                              */\r
1387     /* Toshiba Corporation,                                            */\r
1388     /* International Business Machines Corporation,                    */\r
1389     /* 2001,2002.                                                      */\r
1390     /* S/T/I Confidential Information                                  */\r
1391     /* --------------------------------------------------------------  */\r
1392     __m128 lbf, rtn;\r
1393     __m128 diff, sum, inv_diff;\r
1394     __m128 diagonal, column, near2;\r
1395     __m128 zero = _mm_setzero_ps();\r
1396     union { __m128 v; float s[4]; } l, f, r, n, b, t; // TODO: Union?\r
1397     l.s[0] = left;\r
1398     f.s[0] = zFar;\r
1399     r.s[0] = right;\r
1400     n.s[0] = zNear;\r
1401     b.s[0] = bottom;\r
1402     t.s[0] = top;\r
1403     lbf = vec_mergeh( l.v, f.v );\r
1404     rtn = vec_mergeh( r.v, n.v );\r
1405     lbf = vec_mergeh( lbf, b.v );\r
1406     rtn = vec_mergeh( rtn, t.v );\r
1407     diff = vec_sub( rtn, lbf );\r
1408     sum  = vec_add( rtn, lbf );\r
1409     inv_diff = recipf4( diff );\r
1410     near2 = vec_splat( n.v, 0 );\r
1411     near2 = vec_add( near2, near2 );\r
1412     diagonal = vec_mul( near2, inv_diff );\r
1413     column = vec_mul( sum, inv_diff );\r
1414         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1415         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1416         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1417         VM_ATTRIBUTE_ALIGN16 unsigned int select_w[4] = {0, 0, 0, 0xffffffff};\r
1418     return Matrix4(\r
1419         Vector4( vec_sel( zero, diagonal, select_x ) ),\r
1420         Vector4( vec_sel( zero, diagonal, select_y ) ),\r
1421         Vector4( vec_sel( column, _mm_set1_ps(-1.0f), select_w ) ),\r
1422         Vector4( vec_sel( zero, vec_mul( diagonal, vec_splat( f.v, 0 ) ), select_z ) )\r
1423         );\r
1424 }\r
1425 \r
1426 VECTORMATH_FORCE_INLINE const Matrix4 Matrix4::orthographic( float left, float right, float bottom, float top, float zNear, float zFar )\r
1427 {\r
1428     /* function implementation based on code from STIDC SDK:           */\r
1429     /* --------------------------------------------------------------  */\r
1430     /* PLEASE DO NOT MODIFY THIS SECTION                               */\r
1431     /* This prolog section is automatically generated.                 */\r
1432     /*                                                                 */\r
1433     /* (C)Copyright                                                    */\r
1434     /* Sony Computer Entertainment, Inc.,                              */\r
1435     /* Toshiba Corporation,                                            */\r
1436     /* International Business Machines Corporation,                    */\r
1437     /* 2001,2002.                                                      */\r
1438     /* S/T/I Confidential Information                                  */\r
1439     /* --------------------------------------------------------------  */\r
1440     __m128 lbf, rtn;\r
1441     __m128 diff, sum, inv_diff, neg_inv_diff;\r
1442     __m128 diagonal, column;\r
1443     __m128 zero = _mm_setzero_ps();\r
1444     union { __m128 v; float s[4]; } l, f, r, n, b, t;\r
1445     l.s[0] = left;\r
1446     f.s[0] = zFar;\r
1447     r.s[0] = right;\r
1448     n.s[0] = zNear;\r
1449     b.s[0] = bottom;\r
1450     t.s[0] = top;\r
1451     lbf = vec_mergeh( l.v, f.v );\r
1452     rtn = vec_mergeh( r.v, n.v );\r
1453     lbf = vec_mergeh( lbf, b.v );\r
1454     rtn = vec_mergeh( rtn, t.v );\r
1455     diff = vec_sub( rtn, lbf );\r
1456     sum  = vec_add( rtn, lbf );\r
1457     inv_diff = recipf4( diff );\r
1458     neg_inv_diff = negatef4( inv_diff );\r
1459     diagonal = vec_add( inv_diff, inv_diff );\r
1460         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1461         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1462         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1463         VM_ATTRIBUTE_ALIGN16 unsigned int select_w[4] = {0, 0, 0, 0xffffffff};\r
1464     column = vec_mul( sum, vec_sel( neg_inv_diff, inv_diff, select_z ) ); // TODO: no madds with zero\r
1465     return Matrix4(\r
1466         Vector4( vec_sel( zero, diagonal, select_x ) ),\r
1467         Vector4( vec_sel( zero, diagonal, select_y ) ),\r
1468         Vector4( vec_sel( zero, diagonal, select_z ) ),\r
1469         Vector4( vec_sel( column, _mm_set1_ps(1.0f), select_w ) )\r
1470     );\r
1471 }\r
1472 \r
1473 VECTORMATH_FORCE_INLINE const Matrix4 select( const Matrix4 & mat0, const Matrix4 & mat1, bool select1 )\r
1474 {\r
1475     return Matrix4(\r
1476         select( mat0.getCol0(), mat1.getCol0(), select1 ),\r
1477         select( mat0.getCol1(), mat1.getCol1(), select1 ),\r
1478         select( mat0.getCol2(), mat1.getCol2(), select1 ),\r
1479         select( mat0.getCol3(), mat1.getCol3(), select1 )\r
1480     );\r
1481 }\r
1482 \r
1483 VECTORMATH_FORCE_INLINE const Matrix4 select( const Matrix4 & mat0, const Matrix4 & mat1, const boolInVec &select1 )\r
1484 {\r
1485     return Matrix4(\r
1486         select( mat0.getCol0(), mat1.getCol0(), select1 ),\r
1487         select( mat0.getCol1(), mat1.getCol1(), select1 ),\r
1488         select( mat0.getCol2(), mat1.getCol2(), select1 ),\r
1489         select( mat0.getCol3(), mat1.getCol3(), select1 )\r
1490     );\r
1491 }\r
1492 \r
1493 #ifdef _VECTORMATH_DEBUG\r
1494 \r
1495 VECTORMATH_FORCE_INLINE void print( const Matrix4 & mat )\r
1496 {\r
1497     print( mat.getRow( 0 ) );\r
1498     print( mat.getRow( 1 ) );\r
1499     print( mat.getRow( 2 ) );\r
1500     print( mat.getRow( 3 ) );\r
1501 }\r
1502 \r
1503 VECTORMATH_FORCE_INLINE void print( const Matrix4 & mat, const char * name )\r
1504 {\r
1505     printf("%s:\n", name);\r
1506     print( mat );\r
1507 }\r
1508 \r
1509 #endif\r
1510 \r
1511 VECTORMATH_FORCE_INLINE Transform3::Transform3( const Transform3 & tfrm )\r
1512 {\r
1513     mCol0 = tfrm.mCol0;\r
1514     mCol1 = tfrm.mCol1;\r
1515     mCol2 = tfrm.mCol2;\r
1516     mCol3 = tfrm.mCol3;\r
1517 }\r
1518 \r
1519 VECTORMATH_FORCE_INLINE Transform3::Transform3( float scalar )\r
1520 {\r
1521     mCol0 = Vector3( scalar );\r
1522     mCol1 = Vector3( scalar );\r
1523     mCol2 = Vector3( scalar );\r
1524     mCol3 = Vector3( scalar );\r
1525 }\r
1526 \r
1527 VECTORMATH_FORCE_INLINE Transform3::Transform3( const floatInVec &scalar )\r
1528 {\r
1529     mCol0 = Vector3( scalar );\r
1530     mCol1 = Vector3( scalar );\r
1531     mCol2 = Vector3( scalar );\r
1532     mCol3 = Vector3( scalar );\r
1533 }\r
1534 \r
1535 VECTORMATH_FORCE_INLINE Transform3::Transform3( const Vector3 &_col0, const Vector3 &_col1, const Vector3 &_col2, const Vector3 &_col3 )\r
1536 {\r
1537     mCol0 = _col0;\r
1538     mCol1 = _col1;\r
1539     mCol2 = _col2;\r
1540     mCol3 = _col3;\r
1541 }\r
1542 \r
1543 VECTORMATH_FORCE_INLINE Transform3::Transform3( const Matrix3 & tfrm, const Vector3 &translateVec )\r
1544 {\r
1545     this->setUpper3x3( tfrm );\r
1546     this->setTranslation( translateVec );\r
1547 }\r
1548 \r
1549 VECTORMATH_FORCE_INLINE Transform3::Transform3( const Quat &unitQuat, const Vector3 &translateVec )\r
1550 {\r
1551     this->setUpper3x3( Matrix3( unitQuat ) );\r
1552     this->setTranslation( translateVec );\r
1553 }\r
1554 \r
1555 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setCol0( const Vector3 &_col0 )\r
1556 {\r
1557     mCol0 = _col0;\r
1558     return *this;\r
1559 }\r
1560 \r
1561 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setCol1( const Vector3 &_col1 )\r
1562 {\r
1563     mCol1 = _col1;\r
1564     return *this;\r
1565 }\r
1566 \r
1567 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setCol2( const Vector3 &_col2 )\r
1568 {\r
1569     mCol2 = _col2;\r
1570     return *this;\r
1571 }\r
1572 \r
1573 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setCol3( const Vector3 &_col3 )\r
1574 {\r
1575     mCol3 = _col3;\r
1576     return *this;\r
1577 }\r
1578 \r
1579 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setCol( int col, const Vector3 &vec )\r
1580 {\r
1581     *(&mCol0 + col) = vec;\r
1582     return *this;\r
1583 }\r
1584 \r
1585 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setRow( int row, const Vector4 &vec )\r
1586 {\r
1587     mCol0.setElem( row, vec.getElem( 0 ) );\r
1588     mCol1.setElem( row, vec.getElem( 1 ) );\r
1589     mCol2.setElem( row, vec.getElem( 2 ) );\r
1590     mCol3.setElem( row, vec.getElem( 3 ) );\r
1591     return *this;\r
1592 }\r
1593 \r
1594 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setElem( int col, int row, float val )\r
1595 {\r
1596     (*this)[col].setElem(row, val);\r
1597     return *this;\r
1598 }\r
1599 \r
1600 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setElem( int col, int row, const floatInVec &val )\r
1601 {\r
1602     Vector3 tmpV3_0;\r
1603     tmpV3_0 = this->getCol( col );\r
1604     tmpV3_0.setElem( row, val );\r
1605     this->setCol( col, tmpV3_0 );\r
1606     return *this;\r
1607 }\r
1608 \r
1609 VECTORMATH_FORCE_INLINE const floatInVec Transform3::getElem( int col, int row ) const\r
1610 {\r
1611     return this->getCol( col ).getElem( row );\r
1612 }\r
1613 \r
1614 VECTORMATH_FORCE_INLINE const Vector3 Transform3::getCol0( ) const\r
1615 {\r
1616     return mCol0;\r
1617 }\r
1618 \r
1619 VECTORMATH_FORCE_INLINE const Vector3 Transform3::getCol1( ) const\r
1620 {\r
1621     return mCol1;\r
1622 }\r
1623 \r
1624 VECTORMATH_FORCE_INLINE const Vector3 Transform3::getCol2( ) const\r
1625 {\r
1626     return mCol2;\r
1627 }\r
1628 \r
1629 VECTORMATH_FORCE_INLINE const Vector3 Transform3::getCol3( ) const\r
1630 {\r
1631     return mCol3;\r
1632 }\r
1633 \r
1634 VECTORMATH_FORCE_INLINE const Vector3 Transform3::getCol( int col ) const\r
1635 {\r
1636     return *(&mCol0 + col);\r
1637 }\r
1638 \r
1639 VECTORMATH_FORCE_INLINE const Vector4 Transform3::getRow( int row ) const\r
1640 {\r
1641     return Vector4( mCol0.getElem( row ), mCol1.getElem( row ), mCol2.getElem( row ), mCol3.getElem( row ) );\r
1642 }\r
1643 \r
1644 VECTORMATH_FORCE_INLINE Vector3 & Transform3::operator []( int col )\r
1645 {\r
1646     return *(&mCol0 + col);\r
1647 }\r
1648 \r
1649 VECTORMATH_FORCE_INLINE const Vector3 Transform3::operator []( int col ) const\r
1650 {\r
1651     return *(&mCol0 + col);\r
1652 }\r
1653 \r
1654 VECTORMATH_FORCE_INLINE Transform3 & Transform3::operator =( const Transform3 & tfrm )\r
1655 {\r
1656     mCol0 = tfrm.mCol0;\r
1657     mCol1 = tfrm.mCol1;\r
1658     mCol2 = tfrm.mCol2;\r
1659     mCol3 = tfrm.mCol3;\r
1660     return *this;\r
1661 }\r
1662 \r
1663 VECTORMATH_FORCE_INLINE const Transform3 inverse( const Transform3 & tfrm )\r
1664 {\r
1665     __m128 inv0, inv1, inv2, inv3;\r
1666     __m128 tmp0, tmp1, tmp2, tmp3, tmp4, dot, invdet;\r
1667     __m128 xxxx, yyyy, zzzz;\r
1668     tmp2 = _vmathVfCross( tfrm.getCol0().get128(), tfrm.getCol1().get128() );\r
1669     tmp0 = _vmathVfCross( tfrm.getCol1().get128(), tfrm.getCol2().get128() );\r
1670     tmp1 = _vmathVfCross( tfrm.getCol2().get128(), tfrm.getCol0().get128() );\r
1671     inv3 = negatef4( tfrm.getCol3().get128() );\r
1672     dot = _vmathVfDot3( tmp2, tfrm.getCol2().get128() );\r
1673     dot = vec_splat( dot, 0 );\r
1674     invdet = recipf4( dot );\r
1675     tmp3 = vec_mergeh( tmp0, tmp2 );\r
1676     tmp4 = vec_mergel( tmp0, tmp2 );\r
1677     inv0 = vec_mergeh( tmp3, tmp1 );\r
1678     xxxx = vec_splat( inv3, 0 );\r
1679     //inv1 = vec_perm( tmp3, tmp1, _VECTORMATH_PERM_ZBWX );\r
1680         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1681         inv1 = _mm_shuffle_ps( tmp3, tmp3, _MM_SHUFFLE(0,3,2,2));\r
1682         inv1 = vec_sel(inv1, tmp1, select_y);\r
1683     //inv2 = vec_perm( tmp4, tmp1, _VECTORMATH_PERM_XCYX );\r
1684         inv2 = _mm_shuffle_ps( tmp4, tmp4, _MM_SHUFFLE(0,1,1,0));\r
1685         inv2 = vec_sel(inv2, vec_splat(tmp1, 2), select_y);\r
1686     yyyy = vec_splat( inv3, 1 );\r
1687     zzzz = vec_splat( inv3, 2 );\r
1688     inv3 = vec_mul( inv0, xxxx );\r
1689     inv3 = vec_madd( inv1, yyyy, inv3 );\r
1690     inv3 = vec_madd( inv2, zzzz, inv3 );\r
1691     inv0 = vec_mul( inv0, invdet );\r
1692     inv1 = vec_mul( inv1, invdet );\r
1693     inv2 = vec_mul( inv2, invdet );\r
1694     inv3 = vec_mul( inv3, invdet );\r
1695     return Transform3(\r
1696         Vector3( inv0 ),\r
1697         Vector3( inv1 ),\r
1698         Vector3( inv2 ),\r
1699         Vector3( inv3 )\r
1700     );\r
1701 }\r
1702 \r
1703 VECTORMATH_FORCE_INLINE const Transform3 orthoInverse( const Transform3 & tfrm )\r
1704 {\r
1705     __m128 inv0, inv1, inv2, inv3;\r
1706     __m128 tmp0, tmp1;\r
1707     __m128 xxxx, yyyy, zzzz;\r
1708     tmp0 = vec_mergeh( tfrm.getCol0().get128(), tfrm.getCol2().get128() );\r
1709     tmp1 = vec_mergel( tfrm.getCol0().get128(), tfrm.getCol2().get128() );\r
1710     inv3 = negatef4( tfrm.getCol3().get128() );\r
1711     inv0 = vec_mergeh( tmp0, tfrm.getCol1().get128() );\r
1712     xxxx = vec_splat( inv3, 0 );\r
1713     //inv1 = vec_perm( tmp0, tfrm.getCol1().get128(), _VECTORMATH_PERM_ZBWX );\r
1714         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1715         inv1 = _mm_shuffle_ps( tmp0, tmp0, _MM_SHUFFLE(0,3,2,2));\r
1716         inv1 = vec_sel(inv1, tfrm.getCol1().get128(), select_y);\r
1717     //inv2 = vec_perm( tmp1, tfrm.getCol1().get128(), _VECTORMATH_PERM_XCYX );\r
1718         inv2 = _mm_shuffle_ps( tmp1, tmp1, _MM_SHUFFLE(0,1,1,0));\r
1719         inv2 = vec_sel(inv2, vec_splat(tfrm.getCol1().get128(), 2), select_y);\r
1720     yyyy = vec_splat( inv3, 1 );\r
1721     zzzz = vec_splat( inv3, 2 );\r
1722     inv3 = vec_mul( inv0, xxxx );\r
1723     inv3 = vec_madd( inv1, yyyy, inv3 );\r
1724     inv3 = vec_madd( inv2, zzzz, inv3 );\r
1725     return Transform3(\r
1726         Vector3( inv0 ),\r
1727         Vector3( inv1 ),\r
1728         Vector3( inv2 ),\r
1729         Vector3( inv3 )\r
1730     );\r
1731 }\r
1732 \r
1733 VECTORMATH_FORCE_INLINE const Transform3 absPerElem( const Transform3 & tfrm )\r
1734 {\r
1735     return Transform3(\r
1736         absPerElem( tfrm.getCol0() ),\r
1737         absPerElem( tfrm.getCol1() ),\r
1738         absPerElem( tfrm.getCol2() ),\r
1739         absPerElem( tfrm.getCol3() )\r
1740     );\r
1741 }\r
1742 \r
1743 VECTORMATH_FORCE_INLINE const Vector3 Transform3::operator *( const Vector3 &vec ) const\r
1744 {\r
1745     __m128 res;\r
1746     __m128 xxxx, yyyy, zzzz;\r
1747     xxxx = vec_splat( vec.get128(), 0 );\r
1748     yyyy = vec_splat( vec.get128(), 1 );\r
1749     zzzz = vec_splat( vec.get128(), 2 );\r
1750     res = vec_mul( mCol0.get128(), xxxx );\r
1751     res = vec_madd( mCol1.get128(), yyyy, res );\r
1752     res = vec_madd( mCol2.get128(), zzzz, res );\r
1753     return Vector3( res );\r
1754 }\r
1755 \r
1756 VECTORMATH_FORCE_INLINE const Point3 Transform3::operator *( const Point3 &pnt ) const\r
1757 {\r
1758     __m128 tmp0, tmp1, res;\r
1759     __m128 xxxx, yyyy, zzzz;\r
1760     xxxx = vec_splat( pnt.get128(), 0 );\r
1761     yyyy = vec_splat( pnt.get128(), 1 );\r
1762     zzzz = vec_splat( pnt.get128(), 2 );\r
1763     tmp0 = vec_mul( mCol0.get128(), xxxx );\r
1764     tmp1 = vec_mul( mCol1.get128(), yyyy );\r
1765     tmp0 = vec_madd( mCol2.get128(), zzzz, tmp0 );\r
1766     tmp1 = vec_add( mCol3.get128(), tmp1 );\r
1767     res = vec_add( tmp0, tmp1 );\r
1768     return Point3( res );\r
1769 }\r
1770 \r
1771 VECTORMATH_FORCE_INLINE const Transform3 Transform3::operator *( const Transform3 & tfrm ) const\r
1772 {\r
1773     return Transform3(\r
1774         ( *this * tfrm.mCol0 ),\r
1775         ( *this * tfrm.mCol1 ),\r
1776         ( *this * tfrm.mCol2 ),\r
1777         Vector3( ( *this * Point3( tfrm.mCol3 ) ) )\r
1778     );\r
1779 }\r
1780 \r
1781 VECTORMATH_FORCE_INLINE Transform3 & Transform3::operator *=( const Transform3 & tfrm )\r
1782 {\r
1783     *this = *this * tfrm;\r
1784     return *this;\r
1785 }\r
1786 \r
1787 VECTORMATH_FORCE_INLINE const Transform3 mulPerElem( const Transform3 & tfrm0, const Transform3 & tfrm1 )\r
1788 {\r
1789     return Transform3(\r
1790         mulPerElem( tfrm0.getCol0(), tfrm1.getCol0() ),\r
1791         mulPerElem( tfrm0.getCol1(), tfrm1.getCol1() ),\r
1792         mulPerElem( tfrm0.getCol2(), tfrm1.getCol2() ),\r
1793         mulPerElem( tfrm0.getCol3(), tfrm1.getCol3() )\r
1794     );\r
1795 }\r
1796 \r
1797 VECTORMATH_FORCE_INLINE const Transform3 Transform3::identity( )\r
1798 {\r
1799     return Transform3(\r
1800         Vector3::xAxis( ),\r
1801         Vector3::yAxis( ),\r
1802         Vector3::zAxis( ),\r
1803         Vector3( 0.0f )\r
1804     );\r
1805 }\r
1806 \r
1807 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setUpper3x3( const Matrix3 & tfrm )\r
1808 {\r
1809     mCol0 = tfrm.getCol0();\r
1810     mCol1 = tfrm.getCol1();\r
1811     mCol2 = tfrm.getCol2();\r
1812     return *this;\r
1813 }\r
1814 \r
1815 VECTORMATH_FORCE_INLINE const Matrix3 Transform3::getUpper3x3( ) const\r
1816 {\r
1817     return Matrix3( mCol0, mCol1, mCol2 );\r
1818 }\r
1819 \r
1820 VECTORMATH_FORCE_INLINE Transform3 & Transform3::setTranslation( const Vector3 &translateVec )\r
1821 {\r
1822     mCol3 = translateVec;\r
1823     return *this;\r
1824 }\r
1825 \r
1826 VECTORMATH_FORCE_INLINE const Vector3 Transform3::getTranslation( ) const\r
1827 {\r
1828     return mCol3;\r
1829 }\r
1830 \r
1831 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationX( float radians )\r
1832 {\r
1833     return rotationX( floatInVec(radians) );\r
1834 }\r
1835 \r
1836 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationX( const floatInVec &radians )\r
1837 {\r
1838     __m128 s, c, res1, res2;\r
1839     __m128 zero;\r
1840         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1841         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1842     zero = _mm_setzero_ps();\r
1843     sincosf4( radians.get128(), &s, &c );\r
1844     res1 = vec_sel( zero, c, select_y );\r
1845     res1 = vec_sel( res1, s, select_z );\r
1846     res2 = vec_sel( zero, negatef4(s), select_y );\r
1847     res2 = vec_sel( res2, c, select_z );\r
1848     return Transform3(\r
1849         Vector3::xAxis( ),\r
1850         Vector3( res1 ),\r
1851         Vector3( res2 ),\r
1852         Vector3( _mm_setzero_ps() )\r
1853     );\r
1854 }\r
1855 \r
1856 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationY( float radians )\r
1857 {\r
1858     return rotationY( floatInVec(radians) );\r
1859 }\r
1860 \r
1861 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationY( const floatInVec &radians )\r
1862 {\r
1863     __m128 s, c, res0, res2;\r
1864     __m128 zero;\r
1865         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1866         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1867     zero = _mm_setzero_ps();\r
1868     sincosf4( radians.get128(), &s, &c );\r
1869     res0 = vec_sel( zero, c, select_x );\r
1870     res0 = vec_sel( res0, negatef4(s), select_z );\r
1871     res2 = vec_sel( zero, s, select_x );\r
1872     res2 = vec_sel( res2, c, select_z );\r
1873     return Transform3(\r
1874         Vector3( res0 ),\r
1875         Vector3::yAxis( ),\r
1876         Vector3( res2 ),\r
1877         Vector3( 0.0f )\r
1878     );\r
1879 }\r
1880 \r
1881 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationZ( float radians )\r
1882 {\r
1883     return rotationZ( floatInVec(radians) );\r
1884 }\r
1885 \r
1886 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationZ( const floatInVec &radians )\r
1887 {\r
1888     __m128 s, c, res0, res1;\r
1889         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1890         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1891     __m128 zero = _mm_setzero_ps();\r
1892     sincosf4( radians.get128(), &s, &c );\r
1893     res0 = vec_sel( zero, c, select_x );\r
1894     res0 = vec_sel( res0, s, select_y );\r
1895     res1 = vec_sel( zero, negatef4(s), select_x );\r
1896     res1 = vec_sel( res1, c, select_y );\r
1897     return Transform3(\r
1898         Vector3( res0 ),\r
1899         Vector3( res1 ),\r
1900         Vector3::zAxis( ),\r
1901         Vector3( 0.0f )\r
1902     );\r
1903 }\r
1904 \r
1905 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotationZYX( const Vector3 &radiansXYZ )\r
1906 {\r
1907     __m128 angles, s, negS, c, X0, X1, Y0, Y1, Z0, Z1, tmp;\r
1908     angles = Vector4( radiansXYZ, 0.0f ).get128();\r
1909     sincosf4( angles, &s, &c );\r
1910     negS = negatef4( s );\r
1911     Z0 = vec_mergel( c, s );\r
1912     Z1 = vec_mergel( negS, c );\r
1913         VM_ATTRIBUTE_ALIGN16 unsigned int select_xyz[4] = {0xffffffff, 0xffffffff, 0xffffffff, 0};\r
1914     Z1 = vec_and( Z1, _mm_load_ps( (float *)select_xyz ) );\r
1915         Y0 = _mm_shuffle_ps( c, negS, _MM_SHUFFLE(0,1,1,1) );\r
1916         Y1 = _mm_shuffle_ps( s, c, _MM_SHUFFLE(0,1,1,1) );\r
1917     X0 = vec_splat( s, 0 );\r
1918     X1 = vec_splat( c, 0 );\r
1919     tmp = vec_mul( Z0, Y1 );\r
1920     return Transform3(\r
1921         Vector3( vec_mul( Z0, Y0 ) ),\r
1922         Vector3( vec_madd( Z1, X1, vec_mul( tmp, X0 ) ) ),\r
1923         Vector3( vec_nmsub( Z1, X0, vec_mul( tmp, X1 ) ) ),\r
1924         Vector3( 0.0f )\r
1925     );\r
1926 }\r
1927 \r
1928 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotation( float radians, const Vector3 &unitVec )\r
1929 {\r
1930     return rotation( floatInVec(radians), unitVec );\r
1931 }\r
1932 \r
1933 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotation( const floatInVec &radians, const Vector3 &unitVec )\r
1934 {\r
1935     return Transform3( Matrix3::rotation( radians, unitVec ), Vector3( 0.0f ) );\r
1936 }\r
1937 \r
1938 VECTORMATH_FORCE_INLINE const Transform3 Transform3::rotation( const Quat &unitQuat )\r
1939 {\r
1940     return Transform3( Matrix3( unitQuat ), Vector3( 0.0f ) );\r
1941 }\r
1942 \r
1943 VECTORMATH_FORCE_INLINE const Transform3 Transform3::scale( const Vector3 &scaleVec )\r
1944 {\r
1945     __m128 zero = _mm_setzero_ps();\r
1946         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
1947         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
1948         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
1949     return Transform3(\r
1950         Vector3( vec_sel( zero, scaleVec.get128(), select_x ) ),\r
1951         Vector3( vec_sel( zero, scaleVec.get128(), select_y ) ),\r
1952         Vector3( vec_sel( zero, scaleVec.get128(), select_z ) ),\r
1953         Vector3( 0.0f )\r
1954     );\r
1955 }\r
1956 \r
1957 VECTORMATH_FORCE_INLINE const Transform3 appendScale( const Transform3 & tfrm, const Vector3 &scaleVec )\r
1958 {\r
1959     return Transform3(\r
1960         ( tfrm.getCol0() * scaleVec.getX( ) ),\r
1961         ( tfrm.getCol1() * scaleVec.getY( ) ),\r
1962         ( tfrm.getCol2() * scaleVec.getZ( ) ),\r
1963         tfrm.getCol3()\r
1964     );\r
1965 }\r
1966 \r
1967 VECTORMATH_FORCE_INLINE const Transform3 prependScale( const Vector3 &scaleVec, const Transform3 & tfrm )\r
1968 {\r
1969     return Transform3(\r
1970         mulPerElem( tfrm.getCol0(), scaleVec ),\r
1971         mulPerElem( tfrm.getCol1(), scaleVec ),\r
1972         mulPerElem( tfrm.getCol2(), scaleVec ),\r
1973         mulPerElem( tfrm.getCol3(), scaleVec )\r
1974     );\r
1975 }\r
1976 \r
1977 VECTORMATH_FORCE_INLINE const Transform3 Transform3::translation( const Vector3 &translateVec )\r
1978 {\r
1979     return Transform3(\r
1980         Vector3::xAxis( ),\r
1981         Vector3::yAxis( ),\r
1982         Vector3::zAxis( ),\r
1983         translateVec\r
1984     );\r
1985 }\r
1986 \r
1987 VECTORMATH_FORCE_INLINE const Transform3 select( const Transform3 & tfrm0, const Transform3 & tfrm1, bool select1 )\r
1988 {\r
1989     return Transform3(\r
1990         select( tfrm0.getCol0(), tfrm1.getCol0(), select1 ),\r
1991         select( tfrm0.getCol1(), tfrm1.getCol1(), select1 ),\r
1992         select( tfrm0.getCol2(), tfrm1.getCol2(), select1 ),\r
1993         select( tfrm0.getCol3(), tfrm1.getCol3(), select1 )\r
1994     );\r
1995 }\r
1996 \r
1997 VECTORMATH_FORCE_INLINE const Transform3 select( const Transform3 & tfrm0, const Transform3 & tfrm1, const boolInVec &select1 )\r
1998 {\r
1999     return Transform3(\r
2000         select( tfrm0.getCol0(), tfrm1.getCol0(), select1 ),\r
2001         select( tfrm0.getCol1(), tfrm1.getCol1(), select1 ),\r
2002         select( tfrm0.getCol2(), tfrm1.getCol2(), select1 ),\r
2003         select( tfrm0.getCol3(), tfrm1.getCol3(), select1 )\r
2004     );\r
2005 }\r
2006 \r
2007 #ifdef _VECTORMATH_DEBUG\r
2008 \r
2009 VECTORMATH_FORCE_INLINE void print( const Transform3 & tfrm )\r
2010 {\r
2011     print( tfrm.getRow( 0 ) );\r
2012     print( tfrm.getRow( 1 ) );\r
2013     print( tfrm.getRow( 2 ) );\r
2014 }\r
2015 \r
2016 VECTORMATH_FORCE_INLINE void print( const Transform3 & tfrm, const char * name )\r
2017 {\r
2018     printf("%s:\n", name);\r
2019     print( tfrm );\r
2020 }\r
2021 \r
2022 #endif\r
2023 \r
2024 VECTORMATH_FORCE_INLINE Quat::Quat( const Matrix3 & tfrm )\r
2025 {\r
2026     __m128 res;\r
2027     __m128 col0, col1, col2;\r
2028     __m128 xx_yy, xx_yy_zz_xx, yy_zz_xx_yy, zz_xx_yy_zz, diagSum, diagDiff;\r
2029     __m128 zy_xz_yx, yz_zx_xy, sum, diff;\r
2030     __m128 radicand, invSqrt, scale;\r
2031     __m128 res0, res1, res2, res3;\r
2032     __m128 xx, yy, zz;\r
2033         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
2034         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
2035         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
2036         VM_ATTRIBUTE_ALIGN16 unsigned int select_w[4] = {0, 0, 0, 0xffffffff};\r
2037 \r
2038     col0 = tfrm.getCol0().get128();\r
2039     col1 = tfrm.getCol1().get128();\r
2040     col2 = tfrm.getCol2().get128();\r
2041 \r
2042     /* four cases: */\r
2043     /* trace > 0 */\r
2044     /* else */\r
2045     /*    xx largest diagonal element */\r
2046     /*    yy largest diagonal element */\r
2047     /*    zz largest diagonal element */\r
2048 \r
2049     /* compute quaternion for each case */\r
2050 \r
2051     xx_yy = vec_sel( col0, col1, select_y );\r
2052     //xx_yy_zz_xx = vec_perm( xx_yy, col2, _VECTORMATH_PERM_XYCX );\r
2053     //yy_zz_xx_yy = vec_perm( xx_yy, col2, _VECTORMATH_PERM_YCXY );\r
2054     //zz_xx_yy_zz = vec_perm( xx_yy, col2, _VECTORMATH_PERM_CXYC );\r
2055     xx_yy_zz_xx = _mm_shuffle_ps( xx_yy, xx_yy, _MM_SHUFFLE(0,0,1,0) );\r
2056     xx_yy_zz_xx = vec_sel( xx_yy_zz_xx, col2, select_z ); // TODO: Ck\r
2057     yy_zz_xx_yy = _mm_shuffle_ps( xx_yy_zz_xx, xx_yy_zz_xx, _MM_SHUFFLE(1,0,2,1) );\r
2058     zz_xx_yy_zz = _mm_shuffle_ps( xx_yy_zz_xx, xx_yy_zz_xx, _MM_SHUFFLE(2,1,0,2) );\r
2059 \r
2060     diagSum = vec_add( vec_add( xx_yy_zz_xx, yy_zz_xx_yy ), zz_xx_yy_zz );\r
2061     diagDiff = vec_sub( vec_sub( xx_yy_zz_xx, yy_zz_xx_yy ), zz_xx_yy_zz );\r
2062     radicand = vec_add( vec_sel( diagDiff, diagSum, select_w ), _mm_set1_ps(1.0f) );\r
2063  //   invSqrt = rsqrtf4( radicand );\r
2064         invSqrt = newtonrapson_rsqrt4( radicand );\r
2065 \r
2066         \r
2067 \r
2068     zy_xz_yx = vec_sel( col0, col1, select_z );                                                                 // zy_xz_yx = 00 01 12 03\r
2069     //zy_xz_yx = vec_perm( zy_xz_yx, col2, _VECTORMATH_PERM_ZAYX );\r
2070         zy_xz_yx = _mm_shuffle_ps( zy_xz_yx, zy_xz_yx, _MM_SHUFFLE(0,1,2,2) );          // zy_xz_yx = 12 12 01 00\r
2071     zy_xz_yx = vec_sel( zy_xz_yx, vec_splat(col2, 0), select_y );                               // zy_xz_yx = 12 20 01 00\r
2072     yz_zx_xy = vec_sel( col0, col1, select_x );                                                                 // yz_zx_xy = 10 01 02 03\r
2073     //yz_zx_xy = vec_perm( yz_zx_xy, col2, _VECTORMATH_PERM_BZXX );\r
2074         yz_zx_xy = _mm_shuffle_ps( yz_zx_xy, yz_zx_xy, _MM_SHUFFLE(0,0,2,0) );          // yz_zx_xy = 10 02 10 10\r
2075         yz_zx_xy = vec_sel( yz_zx_xy, vec_splat(col2, 1), select_x );                           // yz_zx_xy = 21 02 10 10\r
2076 \r
2077     sum = vec_add( zy_xz_yx, yz_zx_xy );\r
2078     diff = vec_sub( zy_xz_yx, yz_zx_xy );\r
2079 \r
2080     scale = vec_mul( invSqrt, _mm_set1_ps(0.5f) );\r
2081 \r
2082     //res0 = vec_perm( sum, diff, _VECTORMATH_PERM_XZYA );\r
2083         res0 = _mm_shuffle_ps( sum, sum, _MM_SHUFFLE(0,1,2,0) );\r
2084         res0 = vec_sel( res0, vec_splat(diff, 0), select_w );  // TODO: Ck\r
2085     //res1 = vec_perm( sum, diff, _VECTORMATH_PERM_ZXXB );\r
2086         res1 = _mm_shuffle_ps( sum, sum, _MM_SHUFFLE(0,0,0,2) );\r
2087         res1 = vec_sel( res1, vec_splat(diff, 1), select_w );  // TODO: Ck\r
2088     //res2 = vec_perm( sum, diff, _VECTORMATH_PERM_YXXC );\r
2089         res2 = _mm_shuffle_ps( sum, sum, _MM_SHUFFLE(0,0,0,1) );\r
2090         res2 = vec_sel( res2, vec_splat(diff, 2), select_w );  // TODO: Ck\r
2091     res3 = diff;\r
2092     res0 = vec_sel( res0, radicand, select_x );\r
2093     res1 = vec_sel( res1, radicand, select_y );\r
2094     res2 = vec_sel( res2, radicand, select_z );\r
2095     res3 = vec_sel( res3, radicand, select_w );\r
2096     res0 = vec_mul( res0, vec_splat( scale, 0 ) );\r
2097     res1 = vec_mul( res1, vec_splat( scale, 1 ) );\r
2098     res2 = vec_mul( res2, vec_splat( scale, 2 ) );\r
2099     res3 = vec_mul( res3, vec_splat( scale, 3 ) );\r
2100 \r
2101     /* determine case and select answer */\r
2102 \r
2103     xx = vec_splat( col0, 0 );\r
2104     yy = vec_splat( col1, 1 );\r
2105     zz = vec_splat( col2, 2 );\r
2106     res = vec_sel( res0, res1, vec_cmpgt( yy, xx ) );\r
2107     res = vec_sel( res, res2, vec_and( vec_cmpgt( zz, xx ), vec_cmpgt( zz, yy ) ) );\r
2108     res = vec_sel( res, res3, vec_cmpgt( vec_splat( diagSum, 0 ), _mm_setzero_ps() ) );\r
2109     mVec128 = res;\r
2110 }\r
2111 \r
2112 VECTORMATH_FORCE_INLINE const Matrix3 outer( const Vector3 &tfrm0, const Vector3 &tfrm1 )\r
2113 {\r
2114     return Matrix3(\r
2115         ( tfrm0 * tfrm1.getX( ) ),\r
2116         ( tfrm0 * tfrm1.getY( ) ),\r
2117         ( tfrm0 * tfrm1.getZ( ) )\r
2118     );\r
2119 }\r
2120 \r
2121 VECTORMATH_FORCE_INLINE const Matrix4 outer( const Vector4 &tfrm0, const Vector4 &tfrm1 )\r
2122 {\r
2123     return Matrix4(\r
2124         ( tfrm0 * tfrm1.getX( ) ),\r
2125         ( tfrm0 * tfrm1.getY( ) ),\r
2126         ( tfrm0 * tfrm1.getZ( ) ),\r
2127         ( tfrm0 * tfrm1.getW( ) )\r
2128     );\r
2129 }\r
2130 \r
2131 VECTORMATH_FORCE_INLINE const Vector3 rowMul( const Vector3 &vec, const Matrix3 & mat )\r
2132 {\r
2133     __m128 tmp0, tmp1, mcol0, mcol1, mcol2, res;\r
2134     __m128 xxxx, yyyy, zzzz;\r
2135     tmp0 = vec_mergeh( mat.getCol0().get128(), mat.getCol2().get128() );\r
2136     tmp1 = vec_mergel( mat.getCol0().get128(), mat.getCol2().get128() );\r
2137     xxxx = vec_splat( vec.get128(), 0 );\r
2138     mcol0 = vec_mergeh( tmp0, mat.getCol1().get128() );\r
2139     //mcol1 = vec_perm( tmp0, mat.getCol1().get128(), _VECTORMATH_PERM_ZBWX );\r
2140         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
2141         mcol1 = _mm_shuffle_ps( tmp0, tmp0, _MM_SHUFFLE(0,3,2,2));\r
2142         mcol1 = vec_sel(mcol1, mat.getCol1().get128(), select_y);\r
2143     //mcol2 = vec_perm( tmp1, mat.getCol1().get128(), _VECTORMATH_PERM_XCYX );\r
2144         mcol2 = _mm_shuffle_ps( tmp1, tmp1, _MM_SHUFFLE(0,1,1,0));\r
2145         mcol2 = vec_sel(mcol2, vec_splat(mat.getCol1().get128(), 2), select_y);\r
2146     yyyy = vec_splat( vec.get128(), 1 );\r
2147     res = vec_mul( mcol0, xxxx );\r
2148     zzzz = vec_splat( vec.get128(), 2 );\r
2149     res = vec_madd( mcol1, yyyy, res );\r
2150     res = vec_madd( mcol2, zzzz, res );\r
2151     return Vector3( res );\r
2152 }\r
2153 \r
2154 VECTORMATH_FORCE_INLINE const Matrix3 crossMatrix( const Vector3 &vec )\r
2155 {\r
2156     __m128 neg, res0, res1, res2;\r
2157     neg = negatef4( vec.get128() );\r
2158         VM_ATTRIBUTE_ALIGN16 unsigned int select_x[4] = {0xffffffff, 0, 0, 0};\r
2159         VM_ATTRIBUTE_ALIGN16 unsigned int select_y[4] = {0, 0xffffffff, 0, 0};\r
2160         VM_ATTRIBUTE_ALIGN16 unsigned int select_z[4] = {0, 0, 0xffffffff, 0};\r
2161     //res0 = vec_perm( vec.get128(), neg, _VECTORMATH_PERM_XZBX );\r
2162         res0 = _mm_shuffle_ps( vec.get128(), vec.get128(), _MM_SHUFFLE(0,2,2,0) );\r
2163         res0 = vec_sel(res0, vec_splat(neg, 1), select_z);\r
2164     //res1 = vec_perm( vec.get128(), neg, _VECTORMATH_PERM_CXXX );\r
2165         res1 = vec_sel(vec_splat(vec.get128(), 0), vec_splat(neg, 2), select_x);\r
2166     //res2 = vec_perm( vec.get128(), neg, _VECTORMATH_PERM_YAXX );\r
2167         res2 = _mm_shuffle_ps( vec.get128(), vec.get128(), _MM_SHUFFLE(0,0,1,1) );\r
2168         res2 = vec_sel(res2, vec_splat(neg, 0), select_y);\r
2169         VM_ATTRIBUTE_ALIGN16 unsigned int filter_x[4] = {0, 0xffffffff, 0xffffffff, 0xffffffff};\r
2170         VM_ATTRIBUTE_ALIGN16 unsigned int filter_y[4] = {0xffffffff, 0, 0xffffffff, 0xffffffff};\r
2171         VM_ATTRIBUTE_ALIGN16 unsigned int filter_z[4] = {0xffffffff, 0xffffffff, 0, 0xffffffff};\r
2172     res0 = vec_and( res0, _mm_load_ps((float *)filter_x ) );\r
2173     res1 = vec_and( res1, _mm_load_ps((float *)filter_y ) );\r
2174     res2 = vec_and( res2, _mm_load_ps((float *)filter_z ) ); // TODO: Use selects?\r
2175     return Matrix3(\r
2176         Vector3( res0 ),\r
2177         Vector3( res1 ),\r
2178         Vector3( res2 )\r
2179     );\r
2180 }\r
2181 \r
2182 VECTORMATH_FORCE_INLINE const Matrix3 crossMatrixMul( const Vector3 &vec, const Matrix3 & mat )\r
2183 {\r
2184     return Matrix3( cross( vec, mat.getCol0() ), cross( vec, mat.getCol1() ), cross( vec, mat.getCol2() ) );\r
2185 }\r
2186 \r
2187 } // namespace Aos\r
2188 } // namespace Vectormath\r
2189 \r
2190 #endif\r