Extras/PhysicsEffects/include/vecmath/sse/quat_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_QUAT_AOS_CPP_H\r
  32 #define _VECTORMATH_QUAT_AOS_CPP_H\r
  33 \r
  34 //-----------------------------------------------------------------------------\r
  35 // Definitions\r
  36 \r
  37 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS\r
  38 #define _VECTORMATH_INTERNAL_FUNCTIONS\r
  39 \r
  40 #endif\r
  41 \r
  42 namespace Vectormath {\r
  43 namespace Aos {\r
  44 \r
  45 VECTORMATH_FORCE_INLINE void Quat::set128(vec_float4 vec)\r
  46 {\r
  47     mVec128 = vec;\r
  48 }\r
  49 \r
  50 VECTORMATH_FORCE_INLINE Quat::Quat( const floatInVec &_x, const floatInVec &_y, const floatInVec &_z, const floatInVec &_w )\r
  51 {\r
  52         mVec128 = _mm_unpacklo_ps(\r
  53                 _mm_unpacklo_ps( _x.get128(), _z.get128() ),\r
  54                 _mm_unpacklo_ps( _y.get128(), _w.get128() ) );\r
  55 }\r
  56 \r
  57 VECTORMATH_FORCE_INLINE Quat::Quat( const Vector3 &xyz, float _w )\r
  58 {\r
  59     mVec128 = xyz.get128();\r
  60     _vmathVfSetElement(mVec128, _w, 3);\r
  61 }\r
  62 \r
  63 \r
  64 \r
  65 VECTORMATH_FORCE_INLINE  Quat::Quat(const Quat& quat)\r
  66 {\r
  67         mVec128 = quat.get128();\r
  68 }\r
  69 \r
  70 VECTORMATH_FORCE_INLINE Quat::Quat( float _x, float _y, float _z, float _w )\r
  71 {\r
  72         mVec128 = _mm_setr_ps(_x, _y, _z, _w);\r
  73 }\r
  74 \r
  75 \r
  76 \r
  77 \r
  78 \r
  79 VECTORMATH_FORCE_INLINE Quat::Quat( const Vector3 &xyz, const floatInVec &_w )\r
  80 {\r
  81     mVec128 = xyz.get128();\r
  82     mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);\r
  83 }\r
  84 \r
  85 VECTORMATH_FORCE_INLINE Quat::Quat( const Vector4 &vec )\r
  86 {\r
  87     mVec128 = vec.get128();\r
  88 }\r
  89 \r
  90 VECTORMATH_FORCE_INLINE Quat::Quat( float scalar )\r
  91 {\r
  92     mVec128 = floatInVec(scalar).get128();\r
  93 }\r
  94 \r
  95 VECTORMATH_FORCE_INLINE Quat::Quat( const floatInVec &scalar )\r
  96 {\r
  97     mVec128 = scalar.get128();\r
  98 }\r
  99 \r
 100 VECTORMATH_FORCE_INLINE Quat::Quat( __m128 vf4 )\r
 101 {\r
 102     mVec128 = vf4;\r
 103 }\r
 104 \r
 105 VECTORMATH_FORCE_INLINE const Quat Quat::identity( )\r
 106 {\r
 107     return Quat( _VECTORMATH_UNIT_0001 );\r
 108 }\r
 109 \r
 110 VECTORMATH_FORCE_INLINE const Quat lerp( float t, const Quat &quat0, const Quat &quat1 )\r
 111 {\r
 112     return lerp( floatInVec(t), quat0, quat1 );\r
 113 }\r
 114 \r
 115 VECTORMATH_FORCE_INLINE const Quat lerp( const floatInVec &t, const Quat &quat0, const Quat &quat1 )\r
 116 {\r
 117     return ( quat0 + ( ( quat1 - quat0 ) * t ) );\r
 118 }\r
 119 \r
 120 VECTORMATH_FORCE_INLINE const Quat slerp( float t, const Quat &unitQuat0, const Quat &unitQuat1 )\r
 121 {\r
 122     return slerp( floatInVec(t), unitQuat0, unitQuat1 );\r
 123 }\r
 124 \r
 125 VECTORMATH_FORCE_INLINE const Quat slerp( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1 )\r
 126 {\r
 127     Quat start;\r
 128     vec_float4 scales, scale0, scale1, cosAngle, angle, tttt, oneMinusT, angles, sines;\r
 129     __m128 selectMask;\r
 130     cosAngle = _vmathVfDot4( unitQuat0.get128(), unitQuat1.get128() );\r
 131     selectMask = (__m128)vec_cmpgt( _mm_setzero_ps(), cosAngle );\r
 132     cosAngle = vec_sel( cosAngle, negatef4( cosAngle ), selectMask );\r
 133     start = Quat( vec_sel( unitQuat0.get128(), negatef4( unitQuat0.get128() ), selectMask ) );\r
 134     selectMask = (__m128)vec_cmpgt( _mm_set1_ps(_VECTORMATH_SLERP_TOL), cosAngle );\r
 135     angle = acosf4( cosAngle );\r
 136     tttt = t.get128();\r
 137     oneMinusT = vec_sub( _mm_set1_ps(1.0f), tttt );\r
 138     angles = vec_mergeh( _mm_set1_ps(1.0f), tttt );\r
 139     angles = vec_mergeh( angles, oneMinusT );\r
 140     angles = vec_madd( angles, angle, _mm_setzero_ps() );\r
 141     sines = sinf4( angles );\r
 142     scales = _mm_div_ps( sines, vec_splat( sines, 0 ) );\r
 143     scale0 = vec_sel( oneMinusT, vec_splat( scales, 1 ), selectMask );\r
 144     scale1 = vec_sel( tttt, vec_splat( scales, 2 ), selectMask );\r
 145     return Quat( vec_madd( start.get128(), scale0, vec_mul( unitQuat1.get128(), scale1 ) ) );\r
 146 }\r
 147 \r
 148 VECTORMATH_FORCE_INLINE const Quat squad( float t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )\r
 149 {\r
 150     return squad( floatInVec(t), unitQuat0, unitQuat1, unitQuat2, unitQuat3 );\r
 151 }\r
 152 \r
 153 VECTORMATH_FORCE_INLINE const Quat squad( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )\r
 154 {\r
 155     return slerp( ( ( floatInVec(2.0f) * t ) * ( floatInVec(1.0f) - t ) ), slerp( t, unitQuat0, unitQuat3 ), slerp( t, unitQuat1, unitQuat2 ) );\r
 156 }\r
 157 \r
 158 VECTORMATH_FORCE_INLINE __m128 Quat::get128( ) const\r
 159 {\r
 160     return mVec128;\r
 161 }\r
 162 \r
 163 VECTORMATH_FORCE_INLINE Quat & Quat::operator =( const Quat &quat )\r
 164 {\r
 165     mVec128 = quat.mVec128;\r
 166     return *this;\r
 167 }\r
 168 \r
 169 VECTORMATH_FORCE_INLINE Quat & Quat::setXYZ( const Vector3 &vec )\r
 170 {\r
 171         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};\r
 172         mVec128 = vec_sel( vec.get128(), mVec128, sw );\r
 173     return *this;\r
 174 }\r
 175 \r
 176 VECTORMATH_FORCE_INLINE const Vector3 Quat::getXYZ( ) const\r
 177 {\r
 178     return Vector3( mVec128 );\r
 179 }\r
 180 \r
 181 VECTORMATH_FORCE_INLINE Quat & Quat::setX( float _x )\r
 182 {\r
 183     _vmathVfSetElement(mVec128, _x, 0);\r
 184     return *this;\r
 185 }\r
 186 \r
 187 VECTORMATH_FORCE_INLINE Quat & Quat::setX( const floatInVec &_x )\r
 188 {\r
 189     mVec128 = _vmathVfInsert(mVec128, _x.get128(), 0);\r
 190     return *this;\r
 191 }\r
 192 \r
 193 VECTORMATH_FORCE_INLINE const floatInVec Quat::getX( ) const\r
 194 {\r
 195     return floatInVec( mVec128, 0 );\r
 196 }\r
 197 \r
 198 VECTORMATH_FORCE_INLINE Quat & Quat::setY( float _y )\r
 199 {\r
 200     _vmathVfSetElement(mVec128, _y, 1);\r
 201     return *this;\r
 202 }\r
 203 \r
 204 VECTORMATH_FORCE_INLINE Quat & Quat::setY( const floatInVec &_y )\r
 205 {\r
 206     mVec128 = _vmathVfInsert(mVec128, _y.get128(), 1);\r
 207     return *this;\r
 208 }\r
 209 \r
 210 VECTORMATH_FORCE_INLINE const floatInVec Quat::getY( ) const\r
 211 {\r
 212     return floatInVec( mVec128, 1 );\r
 213 }\r
 214 \r
 215 VECTORMATH_FORCE_INLINE Quat & Quat::setZ( float _z )\r
 216 {\r
 217     _vmathVfSetElement(mVec128, _z, 2);\r
 218     return *this;\r
 219 }\r
 220 \r
 221 VECTORMATH_FORCE_INLINE Quat & Quat::setZ( const floatInVec &_z )\r
 222 {\r
 223     mVec128 = _vmathVfInsert(mVec128, _z.get128(), 2);\r
 224     return *this;\r
 225 }\r
 226 \r
 227 VECTORMATH_FORCE_INLINE const floatInVec Quat::getZ( ) const\r
 228 {\r
 229     return floatInVec( mVec128, 2 );\r
 230 }\r
 231 \r
 232 VECTORMATH_FORCE_INLINE Quat & Quat::setW( float _w )\r
 233 {\r
 234     _vmathVfSetElement(mVec128, _w, 3);\r
 235     return *this;\r
 236 }\r
 237 \r
 238 VECTORMATH_FORCE_INLINE Quat & Quat::setW( const floatInVec &_w )\r
 239 {\r
 240     mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);\r
 241     return *this;\r
 242 }\r
 243 \r
 244 VECTORMATH_FORCE_INLINE const floatInVec Quat::getW( ) const\r
 245 {\r
 246     return floatInVec( mVec128, 3 );\r
 247 }\r
 248 \r
 249 VECTORMATH_FORCE_INLINE Quat & Quat::setElem( int idx, float value )\r
 250 {\r
 251     _vmathVfSetElement(mVec128, value, idx);\r
 252     return *this;\r
 253 }\r
 254 \r
 255 VECTORMATH_FORCE_INLINE Quat & Quat::setElem( int idx, const floatInVec &value )\r
 256 {\r
 257     mVec128 = _vmathVfInsert(mVec128, value.get128(), idx);\r
 258     return *this;\r
 259 }\r
 260 \r
 261 VECTORMATH_FORCE_INLINE const floatInVec Quat::getElem( int idx ) const\r
 262 {\r
 263     return floatInVec( mVec128, idx );\r
 264 }\r
 265 \r
 266 VECTORMATH_FORCE_INLINE VecIdx Quat::operator []( int idx )\r
 267 {\r
 268     return VecIdx( mVec128, idx );\r
 269 }\r
 270 \r
 271 VECTORMATH_FORCE_INLINE const floatInVec Quat::operator []( int idx ) const\r
 272 {\r
 273     return floatInVec( mVec128, idx );\r
 274 }\r
 275 \r
 276 VECTORMATH_FORCE_INLINE const Quat Quat::operator +( const Quat &quat ) const\r
 277 {\r
 278     return Quat( _mm_add_ps( mVec128, quat.mVec128 ) );\r
 279 }\r
 280 \r
 281 \r
 282 VECTORMATH_FORCE_INLINE const Quat Quat::operator -( const Quat &quat ) const\r
 283 {\r
 284     return Quat( _mm_sub_ps( mVec128, quat.mVec128 ) );\r
 285 }\r
 286 \r
 287 VECTORMATH_FORCE_INLINE const Quat Quat::operator *( float scalar ) const\r
 288 {\r
 289     return *this * floatInVec(scalar);\r
 290 }\r
 291 \r
 292 VECTORMATH_FORCE_INLINE const Quat Quat::operator *( const floatInVec &scalar ) const\r
 293 {\r
 294     return Quat( _mm_mul_ps( mVec128, scalar.get128() ) );\r
 295 }\r
 296 \r
 297 VECTORMATH_FORCE_INLINE Quat & Quat::operator +=( const Quat &quat )\r
 298 {\r
 299     *this = *this + quat;\r
 300     return *this;\r
 301 }\r
 302 \r
 303 VECTORMATH_FORCE_INLINE Quat & Quat::operator -=( const Quat &quat )\r
 304 {\r
 305     *this = *this - quat;\r
 306     return *this;\r
 307 }\r
 308 \r
 309 VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( float scalar )\r
 310 {\r
 311     *this = *this * scalar;\r
 312     return *this;\r
 313 }\r
 314 \r
 315 VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( const floatInVec &scalar )\r
 316 {\r
 317     *this = *this * scalar;\r
 318     return *this;\r
 319 }\r
 320 \r
 321 VECTORMATH_FORCE_INLINE const Quat Quat::operator /( float scalar ) const\r
 322 {\r
 323     return *this / floatInVec(scalar);\r
 324 }\r
 325 \r
 326 VECTORMATH_FORCE_INLINE const Quat Quat::operator /( const floatInVec &scalar ) const\r
 327 {\r
 328     return Quat( _mm_div_ps( mVec128, scalar.get128() ) );\r
 329 }\r
 330 \r
 331 VECTORMATH_FORCE_INLINE Quat & Quat::operator /=( float scalar )\r
 332 {\r
 333     *this = *this / scalar;\r
 334     return *this;\r
 335 }\r
 336 \r
 337 VECTORMATH_FORCE_INLINE Quat & Quat::operator /=( const floatInVec &scalar )\r
 338 {\r
 339     *this = *this / scalar;\r
 340     return *this;\r
 341 }\r
 342 \r
 343 VECTORMATH_FORCE_INLINE const Quat Quat::operator -( ) const\r
 344 {\r
 345         return Quat(_mm_sub_ps( _mm_setzero_ps(), mVec128 ) );\r
 346 }\r
 347 \r
 348 VECTORMATH_FORCE_INLINE const Quat operator *( float scalar, const Quat &quat )\r
 349 {\r
 350     return floatInVec(scalar) * quat;\r
 351 }\r
 352 \r
 353 VECTORMATH_FORCE_INLINE const Quat operator *( const floatInVec &scalar, const Quat &quat )\r
 354 {\r
 355     return quat * scalar;\r
 356 }\r
 357 \r
 358 VECTORMATH_FORCE_INLINE const floatInVec dot( const Quat &quat0, const Quat &quat1 )\r
 359 {\r
 360     return floatInVec( _vmathVfDot4( quat0.get128(), quat1.get128() ), 0 );\r
 361 }\r
 362 \r
 363 VECTORMATH_FORCE_INLINE const floatInVec norm( const Quat &quat )\r
 364 {\r
 365     return floatInVec(  _vmathVfDot4( quat.get128(), quat.get128() ), 0 );\r
 366 }\r
 367 \r
 368 VECTORMATH_FORCE_INLINE const floatInVec length( const Quat &quat )\r
 369 {\r
 370     return floatInVec(  _mm_sqrt_ps(_vmathVfDot4( quat.get128(), quat.get128() )), 0 );\r
 371 }\r
 372 \r
 373 VECTORMATH_FORCE_INLINE const Quat normalize( const Quat &quat )\r
 374 {\r
 375         vec_float4 dot =_vmathVfDot4( quat.get128(), quat.get128());\r
 376     return Quat( _mm_mul_ps( quat.get128(), newtonrapson_rsqrt4( dot ) ) );\r
 377 }\r
 378 \r
 379 \r
 380 VECTORMATH_FORCE_INLINE const Quat Quat::rotation( const Vector3 &unitVec0, const Vector3 &unitVec1 )\r
 381 {\r
 382     Vector3 crossVec;\r
 383     __m128 cosAngle, cosAngleX2Plus2, recipCosHalfAngleX2, cosHalfAngleX2, res;\r
 384     cosAngle = _vmathVfDot3( unitVec0.get128(), unitVec1.get128() );\r
 385     cosAngleX2Plus2 = vec_madd( cosAngle, _mm_set1_ps(2.0f), _mm_set1_ps(2.0f) );\r
 386     recipCosHalfAngleX2 = _mm_rsqrt_ps( cosAngleX2Plus2 );\r
 387     cosHalfAngleX2 = vec_mul( recipCosHalfAngleX2, cosAngleX2Plus2 );\r
 388     crossVec = cross( unitVec0, unitVec1 );\r
 389     res = vec_mul( crossVec.get128(), recipCosHalfAngleX2 );\r
 390         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};\r
 391     res = vec_sel( res, vec_mul( cosHalfAngleX2, _mm_set1_ps(0.5f) ), sw );\r
 392     return Quat( res );\r
 393 }\r
 394 \r
 395 VECTORMATH_FORCE_INLINE const Quat Quat::rotation( float radians, const Vector3 &unitVec )\r
 396 {\r
 397     return rotation( floatInVec(radians), unitVec );\r
 398 }\r
 399 \r
 400 VECTORMATH_FORCE_INLINE const Quat Quat::rotation( const floatInVec &radians, const Vector3 &unitVec )\r
 401 {\r
 402     __m128 s, c, angle, res;\r
 403     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );\r
 404     sincosf4( angle, &s, &c );\r
 405         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};\r
 406     res = vec_sel( vec_mul( unitVec.get128(), s ), c, sw );\r
 407     return Quat( res );\r
 408 }\r
 409 \r
 410 VECTORMATH_FORCE_INLINE const Quat Quat::rotationX( float radians )\r
 411 {\r
 412     return rotationX( floatInVec(radians) );\r
 413 }\r
 414 \r
 415 VECTORMATH_FORCE_INLINE const Quat Quat::rotationX( const floatInVec &radians )\r
 416 {\r
 417     __m128 s, c, angle, res;\r
 418     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );\r
 419     sincosf4( angle, &s, &c );\r
 420         VM_ATTRIBUTE_ALIGN16 unsigned int xsw[4] = {0xffffffff, 0, 0, 0};\r
 421         VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};\r
 422     res = vec_sel( _mm_setzero_ps(), s, xsw );\r
 423     res = vec_sel( res, c, wsw );\r
 424     return Quat( res );\r
 425 }\r
 426 \r
 427 VECTORMATH_FORCE_INLINE const Quat Quat::rotationY( float radians )\r
 428 {\r
 429     return rotationY( floatInVec(radians) );\r
 430 }\r
 431 \r
 432 VECTORMATH_FORCE_INLINE const Quat Quat::rotationY( const floatInVec &radians )\r
 433 {\r
 434     __m128 s, c, angle, res;\r
 435     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );\r
 436     sincosf4( angle, &s, &c );\r
 437         VM_ATTRIBUTE_ALIGN16 unsigned int ysw[4] = {0, 0xffffffff, 0, 0};\r
 438         VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};\r
 439     res = vec_sel( _mm_setzero_ps(), s, ysw );\r
 440     res = vec_sel( res, c, wsw );\r
 441     return Quat( res );\r
 442 }\r
 443 \r
 444 VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ( float radians )\r
 445 {\r
 446     return rotationZ( floatInVec(radians) );\r
 447 }\r
 448 \r
 449 VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ( const floatInVec &radians )\r
 450 {\r
 451     __m128 s, c, angle, res;\r
 452     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );\r
 453     sincosf4( angle, &s, &c );\r
 454         VM_ATTRIBUTE_ALIGN16 unsigned int zsw[4] = {0, 0, 0xffffffff, 0};\r
 455         VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};\r
 456     res = vec_sel( _mm_setzero_ps(), s, zsw );\r
 457     res = vec_sel( res, c, wsw );\r
 458     return Quat( res );\r
 459 }\r
 460 \r
 461 VECTORMATH_FORCE_INLINE const Quat Quat::operator *( const Quat &quat ) const\r
 462 {\r
 463     __m128 ldata, rdata, qv, tmp0, tmp1, tmp2, tmp3;\r
 464     __m128 product, l_wxyz, r_wxyz, xy, qw;\r
 465     ldata = mVec128;\r
 466     rdata = quat.mVec128;\r
 467     tmp0 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,0,2,1) );\r
 468     tmp1 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,1,0,2) );\r
 469     tmp2 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,1,0,2) );\r
 470     tmp3 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,0,2,1) );\r
 471     qv = vec_mul( vec_splat( ldata, 3 ), rdata );\r
 472     qv = vec_madd( vec_splat( rdata, 3 ), ldata, qv );\r
 473     qv = vec_madd( tmp0, tmp1, qv );\r
 474     qv = vec_nmsub( tmp2, tmp3, qv );\r
 475     product = vec_mul( ldata, rdata );\r
 476     l_wxyz = vec_sld( ldata, ldata, 12 );\r
 477     r_wxyz = vec_sld( rdata, rdata, 12 );\r
 478     qw = vec_nmsub( l_wxyz, r_wxyz, product );\r
 479     xy = vec_madd( l_wxyz, r_wxyz, product );\r
 480     qw = vec_sub( qw, vec_sld( xy, xy, 8 ) );\r
 481         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};\r
 482     return Quat( vec_sel( qv, qw, sw ) );\r
 483 }\r
 484 \r
 485 VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( const Quat &quat )\r
 486 {\r
 487     *this = *this * quat;\r
 488     return *this;\r
 489 }\r
 490 \r
 491 VECTORMATH_FORCE_INLINE const Vector3 rotate( const Quat &quat, const Vector3 &vec )\r
 492 {    __m128 qdata, vdata, product, tmp0, tmp1, tmp2, tmp3, wwww, qv, qw, res;\r
 493     qdata = quat.get128();\r
 494     vdata = vec.get128();\r
 495     tmp0 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,0,2,1) );\r
 496     tmp1 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,1,0,2) );\r
 497     tmp2 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,1,0,2) );\r
 498     tmp3 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,0,2,1) );\r
 499     wwww = vec_splat( qdata, 3 );\r
 500     qv = vec_mul( wwww, vdata );\r
 501     qv = vec_madd( tmp0, tmp1, qv );\r
 502     qv = vec_nmsub( tmp2, tmp3, qv );\r
 503     product = vec_mul( qdata, vdata );\r
 504     qw = vec_madd( vec_sld( qdata, qdata, 4 ), vec_sld( vdata, vdata, 4 ), product );\r
 505     qw = vec_add( vec_sld( product, product, 8 ), qw );\r
 506     tmp1 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,1,0,2) );\r
 507     tmp3 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,0,2,1) );\r
 508     res = vec_mul( vec_splat( qw, 0 ), qdata );\r
 509     res = vec_madd( wwww, qv, res );\r
 510     res = vec_madd( tmp0, tmp1, res );\r
 511     res = vec_nmsub( tmp2, tmp3, res );\r
 512     return Vector3( res );\r
 513 }\r
 514 \r
 515 VECTORMATH_FORCE_INLINE const Quat conj( const Quat &quat )\r
 516 {\r
 517         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0x80000000,0x80000000,0x80000000,0};\r
 518     return Quat( vec_xor( quat.get128(), _mm_load_ps((float *)sw) ) );\r
 519 }\r
 520 \r
 521 VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, bool select1 )\r
 522 {\r
 523     return select( quat0, quat1, boolInVec(select1) );\r
 524 }\r
 525 \r
 526 //VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, const boolInVec &select1 )\r
 527 //{\r
 528 //    return Quat( vec_sel( quat0.get128(), quat1.get128(), select1.get128() ) );\r
 529 //}\r
 530 \r
 531 VECTORMATH_FORCE_INLINE void loadXYZW(Quat& quat, const float* fptr)\r
 532 {\r
 533 #ifdef USE_SSE3_LDDQU\r
 534         quat = Quat(    SSEFloat(_mm_lddqu_si128((const __m128i*)((float*)(fptr)))).m128                );\r
 535 #else\r
 536         SSEFloat fl;\r
 537         fl.f[0] = fptr[0];\r
 538         fl.f[1] = fptr[1];\r
 539         fl.f[2] = fptr[2];\r
 540         fl.f[3] = fptr[3];\r
 541     quat = Quat(        fl.m128);\r
 542 #endif\r
 543     \r
 544 \r
 545 }\r
 546 \r
 547 VECTORMATH_FORCE_INLINE void storeXYZW(const Quat& quat, float* fptr)\r
 548 {\r
 549         fptr[0] = quat.getX();\r
 550         fptr[1] = quat.getY();\r
 551         fptr[2] = quat.getZ();\r
 552         fptr[3] = quat.getW();\r
 553 //    _mm_storeu_ps((float*)quat.get128(),fptr);\r
 554 }\r
 555 \r
 556 \r
 557 \r
 558 #ifdef _VECTORMATH_DEBUG\r
 559 \r
 560 VECTORMATH_FORCE_INLINE void print( const Quat &quat )\r
 561 {\r
 562     union { __m128 v; float s[4]; } tmp;\r
 563     tmp.v = quat.get128();\r
 564     printf( "( %f %f %f %f )\n", tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );\r
 565 }\r
 566 \r
 567 VECTORMATH_FORCE_INLINE void print( const Quat &quat, const char * name )\r
 568 {\r
 569     union { __m128 v; float s[4]; } tmp;\r
 570     tmp.v = quat.get128();\r
 571     printf( "%s: ( %f %f %f %f )\n", name, tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );\r
 572 }\r
 573 \r
 574 #endif\r
 575 \r
 576 } // namespace Aos\r
 577 } // namespace Vectormath\r
 578 \r
 579 #endif\r