Tribes2/T2-CPP
1
0
Fork 0
mirror of https://github.com/Ragora/T2-CPP.git synced 2026-07-15 17:14:34 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions
T2-CPP/Mod Sources/Bullet/include/bullet/vectormath/sse/quat_aos.h

579 lines
16 KiB
C++
Raw Permalink Blame History

/*
Copyright (C) 2006, 2007 Sony Computer Entertainment Inc.
All rights reserved.
Redistribution and use in source and binary forms,
with or without modification, are permitted provided that the
following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Sony Computer Entertainment Inc nor the names
of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef _VECTORMATH_QUAT_AOS_CPP_H
#define _VECTORMATH_QUAT_AOS_CPP_H
//-----------------------------------------------------------------------------
// Definitions
#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
#define _VECTORMATH_INTERNAL_FUNCTIONS
#endif
namespace Vectormath {
namespace Aos {
__forceinline void Quat::set128(vec_float4 vec)
{
mVec128 = vec;
}
__forceinline Quat::Quat( const floatInVec &_x, const floatInVec &_y, const floatInVec &_z, const floatInVec &_w )
{
mVec128 = _mm_unpacklo_ps(
_mm_unpacklo_ps( _x.get128(), _z.get128() ),
_mm_unpacklo_ps( _y.get128(), _w.get128() ) );
}
__forceinline Quat::Quat( const Vector3 &xyz, float _w )
{
mVec128 = xyz.get128();
_vmathVfSetElement(mVec128, _w, 3);
}
__forceinline Quat::Quat(const Quat& quat)
{
mVec128 = quat.get128();
}
__forceinline Quat::Quat( float _x, float _y, float _z, float _w )
{
mVec128 = _mm_setr_ps(_x, _y, _z, _w);
}
__forceinline Quat::Quat( const Vector3 &xyz, const floatInVec &_w )
{
mVec128 = xyz.get128();
mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
}
__forceinline Quat::Quat( const Vector4 &vec )
{
mVec128 = vec.get128();
}
__forceinline Quat::Quat( float scalar )
{
mVec128 = floatInVec(scalar).get128();
}
__forceinline Quat::Quat( const floatInVec &scalar )
{
mVec128 = scalar.get128();
}
__forceinline Quat::Quat( __m128 vf4 )
{
mVec128 = vf4;
}
__forceinline const Quat Quat::identity( )
{
return Quat( _VECTORMATH_UNIT_0001 );
}
__forceinline const Quat lerp( float t, const Quat &quat0, const Quat &quat1 )
{
return lerp( floatInVec(t), quat0, quat1 );
}
__forceinline const Quat lerp( const floatInVec &t, const Quat &quat0, const Quat &quat1 )
{
return ( quat0 + ( ( quat1 - quat0 ) * t ) );
}
__forceinline const Quat slerp( float t, const Quat &unitQuat0, const Quat &unitQuat1 )
{
return slerp( floatInVec(t), unitQuat0, unitQuat1 );
}
__forceinline const Quat slerp( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1 )
{
Quat start;
vec_float4 scales, scale0, scale1, cosAngle, angle, tttt, oneMinusT, angles, sines;
__m128 selectMask;
cosAngle = _vmathVfDot4( unitQuat0.get128(), unitQuat1.get128() );
selectMask = (__m128)vec_cmpgt( _mm_setzero_ps(), cosAngle );
cosAngle = vec_sel( cosAngle, negatef4( cosAngle ), selectMask );
start = Quat( vec_sel( unitQuat0.get128(), negatef4( unitQuat0.get128() ), selectMask ) );
selectMask = (__m128)vec_cmpgt( _mm_set1_ps(_VECTORMATH_SLERP_TOL), cosAngle );
angle = acosf4( cosAngle );
tttt = t.get128();
oneMinusT = vec_sub( _mm_set1_ps(1.0f), tttt );
angles = vec_mergeh( _mm_set1_ps(1.0f), tttt );
angles = vec_mergeh( angles, oneMinusT );
angles = vec_madd( angles, angle, _mm_setzero_ps() );
sines = sinf4( angles );
scales = _mm_div_ps( sines, vec_splat( sines, 0 ) );
scale0 = vec_sel( oneMinusT, vec_splat( scales, 1 ), selectMask );
scale1 = vec_sel( tttt, vec_splat( scales, 2 ), selectMask );
return Quat( vec_madd( start.get128(), scale0, vec_mul( unitQuat1.get128(), scale1 ) ) );
}
__forceinline const Quat squad( float t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )
{
return squad( floatInVec(t), unitQuat0, unitQuat1, unitQuat2, unitQuat3 );
}
__forceinline const Quat squad( const floatInVec &t, const Quat &unitQuat0, const Quat &unitQuat1, const Quat &unitQuat2, const Quat &unitQuat3 )
{
return slerp( ( ( floatInVec(2.0f) * t ) * ( floatInVec(1.0f) - t ) ), slerp( t, unitQuat0, unitQuat3 ), slerp( t, unitQuat1, unitQuat2 ) );
}
__forceinline __m128 Quat::get128( ) const
{
return mVec128;
}
__forceinline Quat & Quat::operator =( const Quat &quat )
{
mVec128 = quat.mVec128;
return *this;
}
__forceinline Quat & Quat::setXYZ( const Vector3 &vec )
{
__declspec(align(16)) unsigned int sw[4] = {0, 0, 0, 0xffffffff};
mVec128 = vec_sel( vec.get128(), mVec128, sw );
return *this;
}
__forceinline const Vector3 Quat::getXYZ( ) const
{
return Vector3( mVec128 );
}
__forceinline Quat & Quat::setX( float _x )
{
_vmathVfSetElement(mVec128, _x, 0);
return *this;
}
__forceinline Quat & Quat::setX( const floatInVec &_x )
{
mVec128 = _vmathVfInsert(mVec128, _x.get128(), 0);
return *this;
}
__forceinline const floatInVec Quat::getX( ) const
{
return floatInVec( mVec128, 0 );
}
__forceinline Quat & Quat::setY( float _y )
{
_vmathVfSetElement(mVec128, _y, 1);
return *this;
}
__forceinline Quat & Quat::setY( const floatInVec &_y )
{
mVec128 = _vmathVfInsert(mVec128, _y.get128(), 1);
return *this;
}
__forceinline const floatInVec Quat::getY( ) const
{
return floatInVec( mVec128, 1 );
}
__forceinline Quat & Quat::setZ( float _z )
{
_vmathVfSetElement(mVec128, _z, 2);
return *this;
}
__forceinline Quat & Quat::setZ( const floatInVec &_z )
{
mVec128 = _vmathVfInsert(mVec128, _z.get128(), 2);
return *this;
}
__forceinline const floatInVec Quat::getZ( ) const
{
return floatInVec( mVec128, 2 );
}
__forceinline Quat & Quat::setW( float _w )
{
_vmathVfSetElement(mVec128, _w, 3);
return *this;
}
__forceinline Quat & Quat::setW( const floatInVec &_w )
{
mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
return *this;
}
__forceinline const floatInVec Quat::getW( ) const
{
return floatInVec( mVec128, 3 );
}
__forceinline Quat & Quat::setElem( int idx, float value )
{
_vmathVfSetElement(mVec128, value, idx);
return *this;
}
__forceinline Quat & Quat::setElem( int idx, const floatInVec &value )
{
mVec128 = _vmathVfInsert(mVec128, value.get128(), idx);
return *this;
}
__forceinline const floatInVec Quat::getElem( int idx ) const
{
return floatInVec( mVec128, idx );
}
__forceinline VecIdx Quat::operator []( int idx )
{
return VecIdx( mVec128, idx );
}
__forceinline const floatInVec Quat::operator []( int idx ) const
{
return floatInVec( mVec128, idx );
}
__forceinline const Quat Quat::operator +( const Quat &quat ) const
{
return Quat( _mm_add_ps( mVec128, quat.mVec128 ) );
}
__forceinline const Quat Quat::operator -( const Quat &quat ) const
{
return Quat( _mm_sub_ps( mVec128, quat.mVec128 ) );
}
__forceinline const Quat Quat::operator *( float scalar ) const
{
return *this * floatInVec(scalar);
}
__forceinline const Quat Quat::operator *( const floatInVec &scalar ) const
{
return Quat( _mm_mul_ps( mVec128, scalar.get128() ) );
}
__forceinline Quat & Quat::operator +=( const Quat &quat )
{
*this = *this + quat;
return *this;
}
__forceinline Quat & Quat::operator -=( const Quat &quat )
{
*this = *this - quat;
return *this;
}
__forceinline Quat & Quat::operator *=( float scalar )
{
*this = *this * scalar;
return *this;
}
__forceinline Quat & Quat::operator *=( const floatInVec &scalar )
{
*this = *this * scalar;
return *this;
}
__forceinline const Quat Quat::operator /( float scalar ) const
{
return *this / floatInVec(scalar);
}
__forceinline const Quat Quat::operator /( const floatInVec &scalar ) const
{
return Quat( _mm_div_ps( mVec128, scalar.get128() ) );
}
__forceinline Quat & Quat::operator /=( float scalar )
{
*this = *this / scalar;
return *this;
}
__forceinline Quat & Quat::operator /=( const floatInVec &scalar )
{
*this = *this / scalar;
return *this;
}
__forceinline const Quat Quat::operator -( ) const
{
return Quat(_mm_sub_ps( _mm_setzero_ps(), mVec128 ) );
}
__forceinline const Quat operator *( float scalar, const Quat &quat )
{
return floatInVec(scalar) * quat;
}
__forceinline const Quat operator *( const floatInVec &scalar, const Quat &quat )
{
return quat * scalar;
}
__forceinline const floatInVec dot( const Quat &quat0, const Quat &quat1 )
{
return floatInVec( _vmathVfDot4( quat0.get128(), quat1.get128() ), 0 );
}
__forceinline const floatInVec norm( const Quat &quat )
{
return floatInVec( _vmathVfDot4( quat.get128(), quat.get128() ), 0 );
}
__forceinline const floatInVec length( const Quat &quat )
{
return floatInVec( _mm_sqrt_ps(_vmathVfDot4( quat.get128(), quat.get128() )), 0 );
}
__forceinline const Quat normalize( const Quat &quat )
{
vec_float4 dot =_vmathVfDot4( quat.get128(), quat.get128());
return Quat( _mm_mul_ps( quat.get128(), newtonrapson_rsqrt4( dot ) ) );
}
__forceinline const Quat Quat::rotation( const Vector3 &unitVec0, const Vector3 &unitVec1 )
{
Vector3 crossVec;
__m128 cosAngle, cosAngleX2Plus2, recipCosHalfAngleX2, cosHalfAngleX2, res;
cosAngle = _vmathVfDot3( unitVec0.get128(), unitVec1.get128() );
cosAngleX2Plus2 = vec_madd( cosAngle, _mm_set1_ps(2.0f), _mm_set1_ps(2.0f) );
recipCosHalfAngleX2 = _mm_rsqrt_ps( cosAngleX2Plus2 );
cosHalfAngleX2 = vec_mul( recipCosHalfAngleX2, cosAngleX2Plus2 );
crossVec = cross( unitVec0, unitVec1 );
res = vec_mul( crossVec.get128(), recipCosHalfAngleX2 );
__declspec(align(16)) unsigned int sw[4] = {0, 0, 0, 0xffffffff};
res = vec_sel( res, vec_mul( cosHalfAngleX2, _mm_set1_ps(0.5f) ), sw );
return Quat( res );
}
__forceinline const Quat Quat::rotation( float radians, const Vector3 &unitVec )
{
return rotation( floatInVec(radians), unitVec );
}
__forceinline const Quat Quat::rotation( const floatInVec &radians, const Vector3 &unitVec )
{
__m128 s, c, angle, res;
angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
sincosf4( angle, &s, &c );
__declspec(align(16)) unsigned int sw[4] = {0, 0, 0, 0xffffffff};
res = vec_sel( vec_mul( unitVec.get128(), s ), c, sw );
return Quat( res );
}
__forceinline const Quat Quat::rotationX( float radians )
{
return rotationX( floatInVec(radians) );
}
__forceinline const Quat Quat::rotationX( const floatInVec &radians )
{
__m128 s, c, angle, res;
angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
sincosf4( angle, &s, &c );
__declspec(align(16)) unsigned int xsw[4] = {0xffffffff, 0, 0, 0};
__declspec(align(16)) unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
res = vec_sel( _mm_setzero_ps(), s, xsw );
res = vec_sel( res, c, wsw );
return Quat( res );
}
__forceinline const Quat Quat::rotationY( float radians )
{
return rotationY( floatInVec(radians) );
}
__forceinline const Quat Quat::rotationY( const floatInVec &radians )
{
__m128 s, c, angle, res;
angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
sincosf4( angle, &s, &c );
__declspec(align(16)) unsigned int ysw[4] = {0, 0xffffffff, 0, 0};
__declspec(align(16)) unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
res = vec_sel( _mm_setzero_ps(), s, ysw );
res = vec_sel( res, c, wsw );
return Quat( res );
}
__forceinline const Quat Quat::rotationZ( float radians )
{
return rotationZ( floatInVec(radians) );
}
__forceinline const Quat Quat::rotationZ( const floatInVec &radians )
{
__m128 s, c, angle, res;
angle = vec_mul( radians.get128(), _mm_set1_ps(0.5f) );
sincosf4( angle, &s, &c );
__declspec(align(16)) unsigned int zsw[4] = {0, 0, 0xffffffff, 0};
__declspec(align(16)) unsigned int wsw[4] = {0, 0, 0, 0xffffffff};
res = vec_sel( _mm_setzero_ps(), s, zsw );
res = vec_sel( res, c, wsw );
return Quat( res );
}
__forceinline const Quat Quat::operator *( const Quat &quat ) const
{
__m128 ldata, rdata, qv, tmp0, tmp1, tmp2, tmp3;
__m128 product, l_wxyz, r_wxyz, xy, qw;
ldata = mVec128;
rdata = quat.mVec128;
tmp0 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,0,2,1) );
tmp1 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,1,0,2) );
tmp2 = _mm_shuffle_ps( ldata, ldata, _MM_SHUFFLE(3,1,0,2) );
tmp3 = _mm_shuffle_ps( rdata, rdata, _MM_SHUFFLE(3,0,2,1) );
qv = vec_mul( vec_splat( ldata, 3 ), rdata );
qv = vec_madd( vec_splat( rdata, 3 ), ldata, qv );
qv = vec_madd( tmp0, tmp1, qv );
qv = vec_nmsub( tmp2, tmp3, qv );
product = vec_mul( ldata, rdata );
l_wxyz = vec_sld( ldata, ldata, 12 );
r_wxyz = vec_sld( rdata, rdata, 12 );
qw = vec_nmsub( l_wxyz, r_wxyz, product );
xy = vec_madd( l_wxyz, r_wxyz, product );
qw = vec_sub( qw, vec_sld( xy, xy, 8 ) );
__declspec(align(16)) unsigned int sw[4] = {0, 0, 0, 0xffffffff};
return Quat( vec_sel( qv, qw, sw ) );
}
__forceinline Quat & Quat::operator *=( const Quat &quat )
{
*this = *this * quat;
return *this;
}
__forceinline const Vector3 rotate( const Quat &quat, const Vector3 &vec )
{ __m128 qdata, vdata, product, tmp0, tmp1, tmp2, tmp3, wwww, qv, qw, res;
qdata = quat.get128();
vdata = vec.get128();
tmp0 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,0,2,1) );
tmp1 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,1,0,2) );
tmp2 = _mm_shuffle_ps( qdata, qdata, _MM_SHUFFLE(3,1,0,2) );
tmp3 = _mm_shuffle_ps( vdata, vdata, _MM_SHUFFLE(3,0,2,1) );
wwww = vec_splat( qdata, 3 );
qv = vec_mul( wwww, vdata );
qv = vec_madd( tmp0, tmp1, qv );
qv = vec_nmsub( tmp2, tmp3, qv );
product = vec_mul( qdata, vdata );
qw = vec_madd( vec_sld( qdata, qdata, 4 ), vec_sld( vdata, vdata, 4 ), product );
qw = vec_add( vec_sld( product, product, 8 ), qw );
tmp1 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,1,0,2) );
tmp3 = _mm_shuffle_ps( qv, qv, _MM_SHUFFLE(3,0,2,1) );
res = vec_mul( vec_splat( qw, 0 ), qdata );
res = vec_madd( wwww, qv, res );
res = vec_madd( tmp0, tmp1, res );
res = vec_nmsub( tmp2, tmp3, res );
return Vector3( res );
}
__forceinline const Quat conj( const Quat &quat )
{
__declspec(align(16)) unsigned int sw[4] = {0x80000000,0x80000000,0x80000000,0};
return Quat( vec_xor( quat.get128(), _mm_load_ps((float *)sw) ) );
}
__forceinline const Quat select( const Quat &quat0, const Quat &quat1, bool select1 )
{
return select( quat0, quat1, boolInVec(select1) );
}
//__forceinline const Quat select( const Quat &quat0, const Quat &quat1, const boolInVec &select1 )
//{
// return Quat( vec_sel( quat0.get128(), quat1.get128(), select1.get128() ) );
//}
__forceinline void loadXYZW(Quat& quat, const float* fptr)
{
#ifdef USE_SSE2_LDDQU
quat = Quat( SSEFloat(_mm_lddqu_si128((const __m128i*)((float*)(fptr)))).m128 );
#else
SSEFloat fl;
fl.f[0] = fptr[0];
fl.f[1] = fptr[1];
fl.f[2] = fptr[2];
fl.f[3] = fptr[3];
quat = Quat( fl.m128);
#endif
}
__forceinline void storeXYZW(const Quat& quat, float* fptr)
{
fptr[0] = quat.getX();
fptr[1] = quat.getY();
fptr[2] = quat.getZ();
fptr[3] = quat.getW();
// _mm_storeu_ps((float*)quat.get128(),fptr);
}
#ifdef _VECTORMATH_DEBUG
__forceinline void print( const Quat &quat )
{
union { __m128 v; float s[4]; } tmp;
tmp.v = quat.get128();
printf( "( %f %f %f %f )\n", tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );
}
__forceinline void print( const Quat &quat, const char * name )
{
union { __m128 v; float s[4]; } tmp;
tmp.v = quat.get128();
printf( "%s: ( %f %f %f %f )\n", name, tmp.s[0], tmp.s[1], tmp.s[2], tmp.s[3] );
}
#endif
} // namespace Aos
} // namespace Vectormath
#endif
Reference in a new issue View git blame Copy permalink