sse/quat_aos.h

Go to the documentation of this file.
00001 /*
00002    Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
00003    All rights reserved.
00004 
00005    Redistribution and use in source and binary forms,
00006    with or without modification, are permitted provided that the
00007    following conditions are met:
00008     * Redistributions of source code must retain the above copyright
00009       notice, this list of conditions and the following disclaimer.
00010     * Redistributions in binary form must reproduce the above copyright
00011       notice, this list of conditions and the following disclaimer in the
00012       documentation and/or other materials provided with the distribution.
00013     * Neither the name of the Sony Computer Entertainment Inc nor the names
00014       of its contributors may be used to endorse or promote products derived
00015       from this software without specific prior written permission.
00016 
00017    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018    AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019    IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020    ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021    LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022    CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023    SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024    INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025    CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026    ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027    POSSIBILITY OF SUCH DAMAGE.
00028 */
00029 
00030 
00031 #ifndef _VECTORMATH_QUAT_AOS_CPP_H
00032 #define _VECTORMATH_QUAT_AOS_CPP_H
00033 
00034 //-----------------------------------------------------------------------------
00035 // Definitions
00036 
00037 #ifndef _VECTORMATH_INTERNAL_FUNCTIONS
00038 #define _VECTORMATH_INTERNAL_FUNCTIONS
00039 
00040 #endif
00041 
00042 namespace Vectormath {
00043 namespace Aos {
00044 
00045 VECTORMATH_FORCE_INLINE void Quat::set128(vec_float4 vec)
00046 {
00047     mVec128 = vec;
00048 }
00049 
00050 VECTORMATH_FORCE_INLINE Quat::Quat( const floatInVec &_x, const floatInVec &_y, const floatInVec &_z, const floatInVec &_w )
00051 {
00052         mVec128 = _mm_unpacklo_ps(
00053                 _mm_unpacklo_ps( _x.get128(), _z.get128() ),
00054                 _mm_unpacklo_ps( _y.get128(), _w.get128() ) );
00055 }
00056 
00057 VECTORMATH_FORCE_INLINE Quat::Quat( const Vector3 &xyz, float _w )
00058 {
00059     mVec128 = xyz.get128();
00060     _vmathVfSetElement(mVec128, _w, 3);
00061 }
00062 
00063 
00064 
00065 VECTORMATH_FORCE_INLINE  Quat::Quat(const Quat& quat)
00066 {
00067         mVec128 = quat.get128();
00068 }
00069 
00070 VECTORMATH_FORCE_INLINE Quat::Quat( float _x, float _y, float _z, float _w )
00071 {
00072         mVec128 = _mm_setr_ps(_x, _y, _z, _w);
00073 }
00074 
00075 
00076 
00077 
00078 
00079 VECTORMATH_FORCE_INLINE Quat::Quat( const Vector3 &xyz, const floatInVec &_w )
00080 {
00081     mVec128 = xyz.get128();
00082     mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
00083 }
00084 
00085 VECTORMATH_FORCE_INLINE Quat::Quat( const Vector4 &vec )
00086 {
00087     mVec128 = vec.get128();
00088 }
00089 
00090 VECTORMATH_FORCE_INLINE Quat::Quat( float scalar )
00091 {
00092     mVec128 = floatInVec(scalar).get128();
00093 }
00094 
00095 VECTORMATH_FORCE_INLINE Quat::Quat( const floatInVec &scalar )
00096 {
00097     mVec128 = scalar.get128();
00098 }
00099 
00100 VECTORMATH_FORCE_INLINE Quat::Quat( __m128 vf4 )
00101 {
00102     mVec128 = vf4;
00103 }
00104 
00105 VECTORMATH_FORCE_INLINE const Quat Quat::identity( )
00106 {
00107     return Quat( _VECTORMATH_UNIT_0001 );
00108 }
00109 
00110 VECTORMATH_FORCE_INLINE const Quat lerp( float t, const Quat &quat0, const Quat &quat1 )
00111 {
00112     return lerp( floatInVec(t), quat0, quat1 );
00113 }
00114 
00115 VECTORMATH_FORCE_INLINE const Quat lerp( const floatInVec &t, const Quat &quat0, const Quat &quat1 )
00116 {
00117     return ( quat0 + ( ( quat1 - quat0 ) * t ) );
00118 }
00119 
00120 VECTORMATH_FORCE_INLINE const Quat slerp( float t, const Quat &unitQuat0, const Quat &unitQuat1 )
00121 {
00122     return slerp( floatInVec(t), unitQuat0, unitQuat1 );
00123 }
00124 
00125 VECTORMATH_FORCE_INLINE const Quat slerp( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1 )
00126 {
00127     Quat start;
00128     vec_float4 scales, scale0, scale1, cosAngle, angle, tttt, oneMinusT, angles, sines;
00129     __m128 selectMask;
00130     cosAngle = _vmathVfDot4( unitQuat0.get128(), unitQuat1.get128() );
00131     selectMask = (__m128)vec_cmpgt( _mm_setzero_ps(), cosAngle );
00132     cosAngle = vec_sel( cosAngle, negatef4( cosAngle ), selectMask );
00133     start = Quat( vec_sel( unitQuat0.get128(), negatef4( unitQuat0.get128() ), selectMask ) );
00134     selectMask = (__m128)vec_cmpgt( _mm_set1_ps(_VECTORMATH_SLERP_TOL), cosAngle );
00135     angle = acosf4( cosAngle );
00136     tttt = t.get128();
00137     oneMinusT = vec_sub( _mm_set1_ps(1.0f), tttt );
00138     angles = vec_mergeh( _mm_set1_ps(1.0f), tttt );
00139     angles = vec_mergeh( angles, oneMinusT );
00140     angles = vec_madd( angles, angle, _mm_setzero_ps() );
00141     sines = sinf4( angles );
00142     scales = _mm_div_ps( sines, vec_splat( sines, 0 ) );
00143     scale0 = vec_sel( oneMinusT, vec_splat( scales, 1 ), selectMask );
00144     scale1 = vec_sel( tttt, vec_splat( scales, 2 ), selectMask );
00145     return Quat( vec_madd( start.get128(), scale0, vec_mul( unitQuat1.get128(), scale1 ) ) );
00146 }
00147 
00148 VECTORMATH_FORCE_INLINE const Quat squad( float t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )
00149 {
00150     return squad( floatInVec(t), unitQuat0, unitQuat1, unitQuat2, unitQuat3 );
00151 }
00152 
00153 VECTORMATH_FORCE_INLINE const Quat squad( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )
00154 {
00155     return slerp( ( ( floatInVec(2.0f) * t ) * ( floatInVec(1.0f) - t ) ), slerp( t, unitQuat0, unitQuat3 ), slerp( t, unitQuat1, unitQuat2 ) );
00156 }
00157 
00158 VECTORMATH_FORCE_INLINE __m128 Quat::get128( ) const
00159 {
00160     return mVec128;
00161 }
00162 
00163 VECTORMATH_FORCE_INLINE Quat & Quat::operator =( const Quat &quat )
00164 {
00165     mVec128 = quat.mVec128;
00166     return *this;
00167 }
00168 
00169 VECTORMATH_FORCE_INLINE Quat & Quat::setXYZ( const Vector3 &vec )
00170 {
00171         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
00172         mVec128 = vec_sel( vec.get128(), mVec128, sw );
00173     return *this;
00174 }
00175 
00176 VECTORMATH_FORCE_INLINE const Vector3 Quat::getXYZ( ) const
00177 {
00178     return Vector3( mVec128 );
00179 }
00180 
00181 VECTORMATH_FORCE_INLINE Quat & Quat::setX( float _x )
00182 {
00183     _vmathVfSetElement(mVec128, _x, 0);
00184     return *this;
00185 }
00186 
00187 VECTORMATH_FORCE_INLINE Quat & Quat::setX( const floatInVec &_x )
00188 {
00189     mVec128 = _vmathVfInsert(mVec128, _x.get128(), 0);
00190     return *this;
00191 }
00192 
00193 VECTORMATH_FORCE_INLINE const floatInVec Quat::getX( ) const
00194 {
00195     return floatInVec( mVec128, 0 );
00196 }
00197 
00198 VECTORMATH_FORCE_INLINE Quat & Quat::setY( float _y )
00199 {
00200     _vmathVfSetElement(mVec128, _y, 1);
00201     return *this;
00202 }
00203 
00204 VECTORMATH_FORCE_INLINE Quat & Quat::setY( const floatInVec &_y )
00205 {
00206     mVec128 = _vmathVfInsert(mVec128, _y.get128(), 1);
00207     return *this;
00208 }
00209 
00210 VECTORMATH_FORCE_INLINE const floatInVec Quat::getY( ) const
00211 {
00212     return floatInVec( mVec128, 1 );
00213 }
00214 
00215 VECTORMATH_FORCE_INLINE Quat & Quat::setZ( float _z )
00216 {
00217     _vmathVfSetElement(mVec128, _z, 2);
00218     return *this;
00219 }
00220 
00221 VECTORMATH_FORCE_INLINE Quat & Quat::setZ( const floatInVec &_z )
00222 {
00223     mVec128 = _vmathVfInsert(mVec128, _z.get128(), 2);
00224     return *this;
00225 }
00226 
00227 VECTORMATH_FORCE_INLINE const floatInVec Quat::getZ( ) const
00228 {
00229     return floatInVec( mVec128, 2 );
00230 }
00231 
00232 VECTORMATH_FORCE_INLINE Quat & Quat::setW( float _w )
00233 {
00234     _vmathVfSetElement(mVec128, _w, 3);
00235     return *this;
00236 }
00237 
00238 VECTORMATH_FORCE_INLINE Quat & Quat::setW( const floatInVec &_w )
00239 {
00240     mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
00241     return *this;
00242 }
00243 
00244 VECTORMATH_FORCE_INLINE const floatInVec Quat::getW( ) const
00245 {
00246     return floatInVec( mVec128, 3 );
00247 }
00248 
00249 VECTORMATH_FORCE_INLINE Quat & Quat::setElem( int idx, float value )
00250 {
00251     _vmathVfSetElement(mVec128, value, idx);
00252     return *this;
00253 }
00254 
00255 VECTORMATH_FORCE_INLINE Quat & Quat::setElem( int idx, const floatInVec &value )
00256 {
00257     mVec128 = _vmathVfInsert(mVec128, value.get128(), idx);
00258     return *this;
00259 }
00260 
00261 VECTORMATH_FORCE_INLINE const floatInVec Quat::getElem( int idx ) const
00262 {
00263     return floatInVec( mVec128, idx );
00264 }
00265 
00266 VECTORMATH_FORCE_INLINE VecIdx Quat::operator []( int idx )
00267 {
00268     return VecIdx( mVec128, idx );
00269 }
00270 
00271 VECTORMATH_FORCE_INLINE const floatInVec Quat::operator []( int idx ) const
00272 {
00273     return floatInVec( mVec128, idx );
00274 }
00275 
00276 VECTORMATH_FORCE_INLINE const Quat Quat::operator +( const Quat &quat ) const
00277 {
00278     return Quat( _mm_add_ps( mVec128, quat.mVec128 ) );
00279 }
00280 
00281 
00282 VECTORMATH_FORCE_INLINE const Quat Quat::operator -( const Quat &quat ) const
00283 {
00284     return Quat( _mm_sub_ps( mVec128, quat.mVec128 ) );
00285 }
00286 
00287 VECTORMATH_FORCE_INLINE const Quat Quat::operator *( float scalar ) const
00288 {
00289     return *this * floatInVec(scalar);
00290 }
00291 
00292 VECTORMATH_FORCE_INLINE const Quat Quat::operator *( const floatInVec &scalar ) const
00293 {
00294     return Quat( _mm_mul_ps( mVec128, scalar.get128() ) );
00295 }
00296 
00297 VECTORMATH_FORCE_INLINE Quat & Quat::operator +=( const Quat &quat )
00298 {
00299     *this = *this + quat;
00300     return *this;
00301 }
00302 
00303 VECTORMATH_FORCE_INLINE Quat & Quat::operator -=( const Quat &quat )
00304 {
00305     *this = *this - quat;
00306     return *this;
00307 }
00308 
00309 VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( float scalar )
00310 {
00311     *this = *this * scalar;
00312     return *this;
00313 }
00314 
00315 VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( const floatInVec &scalar )
00316 {
00317     *this = *this * scalar;
00318     return *this;
00319 }
00320 
00321 VECTORMATH_FORCE_INLINE const Quat Quat::operator /( float scalar ) const
00322 {
00323     return *this / floatInVec(scalar);
00324 }
00325 
00326 VECTORMATH_FORCE_INLINE const Quat Quat::operator /( const floatInVec &scalar ) const
00327 {
00328     return Quat( _mm_div_ps( mVec128, scalar.get128() ) );
00329 }
00330 
00331 VECTORMATH_FORCE_INLINE Quat & Quat::operator /=( float scalar )
00332 {
00333     *this = *this / scalar;
00334     return *this;
00335 }
00336 
00337 VECTORMATH_FORCE_INLINE Quat & Quat::operator /=( const floatInVec &scalar )
00338 {
00339     *this = *this / scalar;
00340     return *this;
00341 }
00342 
00343 VECTORMATH_FORCE_INLINE const Quat Quat::operator -( ) const
00344 {
00345         return Quat(_mm_sub_ps( _mm_setzero_ps(), mVec128 ) );
00346 }
00347 
00348 VECTORMATH_FORCE_INLINE const Quat operator *( float scalar, const Quat &quat )
00349 {
00350     return floatInVec(scalar) * quat;
00351 }
00352 
00353 VECTORMATH_FORCE_INLINE const Quat operator *( const floatInVec &scalar, const Quat &quat )
00354 {
00355     return quat * scalar;
00356 }
00357 
00358 VECTORMATH_FORCE_INLINE const floatInVec dot( const Quat &quat0, const Quat &quat1 )
00359 {
00360     return floatInVec( _vmathVfDot4( quat0.get128(), quat1.get128() ), 0 );
00361 }
00362 
00363 VECTORMATH_FORCE_INLINE const floatInVec norm( const Quat &quat )
00364 {
00365     return floatInVec(  _vmathVfDot4( quat.get128(), quat.get128() ), 0 );
00366 }
00367 
00368 VECTORMATH_FORCE_INLINE const floatInVec length( const Quat &quat )
00369 {
00370     return floatInVec(  _mm_sqrt_ps(_vmathVfDot4( quat.get128(), quat.get128() )), 0 );
00371 }
00372 
00373 VECTORMATH_FORCE_INLINE const Quat normalize( const Quat &quat )
00374 {
00375         vec_float4 dot =_vmathVfDot4( quat.get128(), quat.get128());
00376     return Quat( _mm_mul_ps( quat.get128(), newtonrapson_rsqrt4( dot ) ) );
00377 }
00378 
00379 
00380 VECTORMATH_FORCE_INLINE const Quat Quat::rotation( const Vector3 &unitVec0, const Vector3 &unitVec1 )
00381 {
00382     Vector3 crossVec;
00383     __m128 cosAngle, cosAngleX2Plus2, recipCosHalfAngleX2, cosHalfAngleX2, res;
00384     cosAngle = _vmathVfDot3( unitVec0.get128(), unitVec1.get128() );
00385     cosAngleX2Plus2 = vec_madd( cosAngle, _mm_set1_ps(2.0f), _mm_set1_ps(2.0f) );
00386     recipCosHalfAngleX2 = _mm_rsqrt_ps( cosAngleX2Plus2 );
00387     cosHalfAngleX2 = vec_mul( recipCosHalfAngleX2, cosAngleX2Plus2 );
00388     crossVec = cross( unitVec0, unitVec1 );
00389     res = vec_mul( crossVec.get128(), recipCosHalfAngleX2 );
00390         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
00391     res = vec_sel( res, vec_mul( cosHalfAngleX2, _mm_set1_ps(0.5f) ), sw );
00392     return Quat( res );
00393 }
00394 
00395 VECTORMATH_FORCE_INLINE const Quat Quat::rotation( float radians, const Vector3 &unitVec )
00396 {
00397     return rotation( floatInVec(radians), unitVec );
00398 }
00399 
00400 VECTORMATH_FORCE_INLINE const Quat Quat::rotation( const floatInVec &radians, const Vector3 &unitVec )
00401 {
00402     __m128 s, c, angle, res;
00403     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
00404     sincosf4( angle, &s, &c );
00405         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
00406     res = vec_sel( vec_mul( unitVec.get128(), s ), c, sw );
00407     return Quat( res );
00408 }
00409 
00410 VECTORMATH_FORCE_INLINE const Quat Quat::rotationX( float radians )
00411 {
00412     return rotationX( floatInVec(radians) );
00413 }
00414 
00415 VECTORMATH_FORCE_INLINE const Quat Quat::rotationX( const floatInVec &radians )
00416 {
00417     __m128 s, c, angle, res;
00418     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
00419     sincosf4( angle, &s, &c );
00420         VM_ATTRIBUTE_ALIGN16 unsigned int xsw[4] = {0xffffffff, 0, 0, 0};
00421         VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
00422     res = vec_sel( _mm_setzero_ps(), s, xsw );
00423     res = vec_sel( res, c, wsw );
00424     return Quat( res );
00425 }
00426 
00427 VECTORMATH_FORCE_INLINE const Quat Quat::rotationY( float radians )
00428 {
00429     return rotationY( floatInVec(radians) );
00430 }
00431 
00432 VECTORMATH_FORCE_INLINE const Quat Quat::rotationY( const floatInVec &radians )
00433 {
00434     __m128 s, c, angle, res;
00435     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
00436     sincosf4( angle, &s, &c );
00437         VM_ATTRIBUTE_ALIGN16 unsigned int ysw[4] = {0, 0xffffffff, 0, 0};
00438         VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
00439     res = vec_sel( _mm_setzero_ps(), s, ysw );
00440     res = vec_sel( res, c, wsw );
00441     return Quat( res );
00442 }
00443 
00444 VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ( float radians )
00445 {
00446     return rotationZ( floatInVec(radians) );
00447 }
00448 
00449 VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ( const floatInVec &radians )
00450 {
00451     __m128 s, c, angle, res;
00452     angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
00453     sincosf4( angle, &s, &c );
00454         VM_ATTRIBUTE_ALIGN16 unsigned int zsw[4] = {0, 0, 0xffffffff, 0};
00455         VM_ATTRIBUTE_ALIGN16 unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
00456     res = vec_sel( _mm_setzero_ps(), s, zsw );
00457     res = vec_sel( res, c, wsw );
00458     return Quat( res );
00459 }
00460 
00461 VECTORMATH_FORCE_INLINE const Quat Quat::operator *( const Quat &quat ) const
00462 {
00463     __m128 ldata, rdata, qv, tmp0, tmp1, tmp2, tmp3;
00464     __m128 product, l_wxyz, r_wxyz, xy, qw;
00465     ldata = mVec128;
00466     rdata = quat.mVec128;
00467     tmp0 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,0,2,1) );
00468     tmp1 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,1,0,2) );
00469     tmp2 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,1,0,2) );
00470     tmp3 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,0,2,1) );
00471     qv = vec_mul( vec_splat( ldata, 3 ), rdata );
00472     qv = vec_madd( vec_splat( rdata, 3 ), ldata, qv );
00473     qv = vec_madd( tmp0, tmp1, qv );
00474     qv = vec_nmsub( tmp2, tmp3, qv );
00475     product = vec_mul( ldata, rdata );
00476     l_wxyz = vec_sld( ldata, ldata, 12 );
00477     r_wxyz = vec_sld( rdata, rdata, 12 );
00478     qw = vec_nmsub( l_wxyz, r_wxyz, product );
00479     xy = vec_madd( l_wxyz, r_wxyz, product );
00480     qw = vec_sub( qw, vec_sld( xy, xy, 8 ) );
00481         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
00482     return Quat( vec_sel( qv, qw, sw ) );
00483 }
00484 
00485 VECTORMATH_FORCE_INLINE Quat & Quat::operator *=( const Quat &quat )
00486 {
00487     *this = *this * quat;
00488     return *this;
00489 }
00490 
00491 VECTORMATH_FORCE_INLINE const Vector3 rotate( const Quat &quat, const Vector3 &vec )
00492 {    __m128 qdata, vdata, product, tmp0, tmp1, tmp2, tmp3, wwww, qv, qw, res;
00493     qdata = quat.get128();
00494     vdata = vec.get128();
00495     tmp0 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,0,2,1) );
00496     tmp1 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,1,0,2) );
00497     tmp2 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,1,0,2) );
00498     tmp3 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,0,2,1) );
00499     wwww = vec_splat( qdata, 3 );
00500     qv = vec_mul( wwww, vdata );
00501     qv = vec_madd( tmp0, tmp1, qv );
00502     qv = vec_nmsub( tmp2, tmp3, qv );
00503     product = vec_mul( qdata, vdata );
00504     qw = vec_madd( vec_sld( qdata, qdata, 4 ), vec_sld( vdata, vdata, 4 ), product );
00505     qw = vec_add( vec_sld( product, product, 8 ), qw );
00506     tmp1 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,1,0,2) );
00507     tmp3 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,0,2,1) );
00508     res = vec_mul( vec_splat( qw, 0 ), qdata );
00509     res = vec_madd( wwww, qv, res );
00510     res = vec_madd( tmp0, tmp1, res );
00511     res = vec_nmsub( tmp2, tmp3, res );
00512     return Vector3( res );
00513 }
00514 
00515 VECTORMATH_FORCE_INLINE const Quat conj( const Quat &quat )
00516 {
00517         VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0x80000000,0x80000000,0x80000000,0};
00518     return Quat( vec_xor( quat.get128(), _mm_load_ps((float *)sw) ) );
00519 }
00520 
00521 VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, bool select1 )
00522 {
00523     return select( quat0, quat1, boolInVec(select1) );
00524 }
00525 
00526 //VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, const boolInVec &select1 )
00527 //{
00528 //    return Quat( vec_sel( quat0.get128(), quat1.get128(), select1.get128() ) );
00529 //}
00530 
00531 VECTORMATH_FORCE_INLINE void loadXYZW(Quat& quat, const float* fptr)
00532 {
00533 #ifdef USE_SSE3_LDDQU
00534         quat = Quat(    SSEFloat(_mm_lddqu_si128((const __m128i*)((float*)(fptr)))).m128                );
00535 #else
00536         SSEFloat fl;
00537         fl.f[0] = fptr[0];
00538         fl.f[1] = fptr[1];
00539         fl.f[2] = fptr[2];
00540         fl.f[3] = fptr[3];
00541     quat = Quat(        fl.m128);
00542 #endif
00543     
00544 
00545 }
00546 
00547 VECTORMATH_FORCE_INLINE void storeXYZW(const Quat& quat, float* fptr)
00548 {
00549         fptr[0] = quat.getX();
00550         fptr[1] = quat.getY();
00551         fptr[2] = quat.getZ();
00552         fptr[3] = quat.getW();
00553 //    _mm_storeu_ps((float*)quat.get128(),fptr);
00554 }
00555 
00556 
00557 
00558 #ifdef _VECTORMATH_DEBUG
00559 
00560 VECTORMATH_FORCE_INLINE void print( const Quat &quat )
00561 {
00562     union { __m128 v; float s[4]; } tmp;
00563     tmp.v = quat.get128();
00564     printf( "( %f %f %f %f )\n", tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );
00565 }
00566 
00567 VECTORMATH_FORCE_INLINE void print( const Quat &quat, const char * name )
00568 {
00569     union { __m128 v; float s[4]; } tmp;
00570     tmp.v = quat.get128();
00571     printf( "%s: ( %f %f %f %f )\n", name, tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );
00572 }
00573 
00574 #endif
00575 
00576 } // namespace Aos
00577 } // namespace Vectormath
00578 
00579 #endif