ab8f16961e
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
575 lines
16 KiB
C++
575 lines
16 KiB
C++
/*
|
|
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
|
|
{
|
|
VECTORMATH_FORCE_INLINE void Quat::set128(vec_float4 vec)
|
|
{
|
|
mVec128 = vec;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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()));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(const Vector3 &xyz, float _w)
|
|
{
|
|
mVec128 = xyz.get128();
|
|
_vmathVfSetElement(mVec128, _w, 3);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(const Quat &quat)
|
|
{
|
|
mVec128 = quat.get128();
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(float _x, float _y, float _z, float _w)
|
|
{
|
|
mVec128 = _mm_setr_ps(_x, _y, _z, _w);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(const Vector3 &xyz, const floatInVec &_w)
|
|
{
|
|
mVec128 = xyz.get128();
|
|
mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(const Vector4 &vec)
|
|
{
|
|
mVec128 = vec.get128();
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(float scalar)
|
|
{
|
|
mVec128 = floatInVec(scalar).get128();
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(const floatInVec &scalar)
|
|
{
|
|
mVec128 = scalar.get128();
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat::Quat(__m128 vf4)
|
|
{
|
|
mVec128 = vf4;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::identity()
|
|
{
|
|
return Quat(_VECTORMATH_UNIT_0001);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat lerp(float t, const Quat &quat0, const Quat &quat1)
|
|
{
|
|
return lerp(floatInVec(t), quat0, quat1);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat lerp(const floatInVec &t, const Quat &quat0, const Quat &quat1)
|
|
{
|
|
return (quat0 + ((quat1 - quat0) * t));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat slerp(float t, const Quat &unitQuat0, const Quat &unitQuat1)
|
|
{
|
|
return slerp(floatInVec(t), unitQuat0, unitQuat1);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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)));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE __m128 Quat::get128() const
|
|
{
|
|
return mVec128;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator=(const Quat &quat)
|
|
{
|
|
mVec128 = quat.mVec128;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setXYZ(const Vector3 &vec)
|
|
{
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
|
|
mVec128 = vec_sel(vec.get128(), mVec128, sw);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Vector3 Quat::getXYZ() const
|
|
{
|
|
return Vector3(mVec128);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setX(float _x)
|
|
{
|
|
_vmathVfSetElement(mVec128, _x, 0);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setX(const floatInVec &_x)
|
|
{
|
|
mVec128 = _vmathVfInsert(mVec128, _x.get128(), 0);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec Quat::getX() const
|
|
{
|
|
return floatInVec(mVec128, 0);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setY(float _y)
|
|
{
|
|
_vmathVfSetElement(mVec128, _y, 1);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setY(const floatInVec &_y)
|
|
{
|
|
mVec128 = _vmathVfInsert(mVec128, _y.get128(), 1);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec Quat::getY() const
|
|
{
|
|
return floatInVec(mVec128, 1);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setZ(float _z)
|
|
{
|
|
_vmathVfSetElement(mVec128, _z, 2);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setZ(const floatInVec &_z)
|
|
{
|
|
mVec128 = _vmathVfInsert(mVec128, _z.get128(), 2);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec Quat::getZ() const
|
|
{
|
|
return floatInVec(mVec128, 2);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setW(float _w)
|
|
{
|
|
_vmathVfSetElement(mVec128, _w, 3);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setW(const floatInVec &_w)
|
|
{
|
|
mVec128 = _vmathVfInsert(mVec128, _w.get128(), 3);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec Quat::getW() const
|
|
{
|
|
return floatInVec(mVec128, 3);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setElem(int idx, float value)
|
|
{
|
|
_vmathVfSetElement(mVec128, value, idx);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::setElem(int idx, const floatInVec &value)
|
|
{
|
|
mVec128 = _vmathVfInsert(mVec128, value.get128(), idx);
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec Quat::getElem(int idx) const
|
|
{
|
|
return floatInVec(mVec128, idx);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE VecIdx Quat::operator[](int idx)
|
|
{
|
|
return VecIdx(mVec128, idx);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec Quat::operator[](int idx) const
|
|
{
|
|
return floatInVec(mVec128, idx);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator+(const Quat &quat) const
|
|
{
|
|
return Quat(_mm_add_ps(mVec128, quat.mVec128));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator-(const Quat &quat) const
|
|
{
|
|
return Quat(_mm_sub_ps(mVec128, quat.mVec128));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator*(float scalar) const
|
|
{
|
|
return *this * floatInVec(scalar);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator*(const floatInVec &scalar) const
|
|
{
|
|
return Quat(_mm_mul_ps(mVec128, scalar.get128()));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator+=(const Quat &quat)
|
|
{
|
|
*this = *this + quat;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator-=(const Quat &quat)
|
|
{
|
|
*this = *this - quat;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator*=(float scalar)
|
|
{
|
|
*this = *this * scalar;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator*=(const floatInVec &scalar)
|
|
{
|
|
*this = *this * scalar;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator/(float scalar) const
|
|
{
|
|
return *this / floatInVec(scalar);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator/(const floatInVec &scalar) const
|
|
{
|
|
return Quat(_mm_div_ps(mVec128, scalar.get128()));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator/=(float scalar)
|
|
{
|
|
*this = *this / scalar;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator/=(const floatInVec &scalar)
|
|
{
|
|
*this = *this / scalar;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::operator-() const
|
|
{
|
|
return Quat(_mm_sub_ps(_mm_setzero_ps(), mVec128));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat operator*(float scalar, const Quat &quat)
|
|
{
|
|
return floatInVec(scalar) * quat;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat operator*(const floatInVec &scalar, const Quat &quat)
|
|
{
|
|
return quat * scalar;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec dot(const Quat &quat0, const Quat &quat1)
|
|
{
|
|
return floatInVec(_vmathVfDot4(quat0.get128(), quat1.get128()), 0);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec norm(const Quat &quat)
|
|
{
|
|
return floatInVec(_vmathVfDot4(quat.get128(), quat.get128()), 0);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const floatInVec length(const Quat &quat)
|
|
{
|
|
return floatInVec(_mm_sqrt_ps(_vmathVfDot4(quat.get128(), quat.get128())), 0);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat normalize(const Quat &quat)
|
|
{
|
|
vec_float4 dot = _vmathVfDot4(quat.get128(), quat.get128());
|
|
return Quat(_mm_mul_ps(quat.get128(), newtonrapson_rsqrt4(dot)));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
|
|
res = vec_sel(res, vec_mul(cosHalfAngleX2, _mm_set1_ps(0.5f)), sw);
|
|
return Quat(res);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::rotation(float radians, const Vector3 &unitVec)
|
|
{
|
|
return rotation(floatInVec(radians), unitVec);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
|
|
res = vec_sel(vec_mul(unitVec.get128(), s), c, sw);
|
|
return Quat(res);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::rotationX(float radians)
|
|
{
|
|
return rotationX(floatInVec(radians));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int xsw[4] = {0xffffffff, 0, 0, 0};
|
|
VM_ATTRIBUTE_ALIGN16 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);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::rotationY(float radians)
|
|
{
|
|
return rotationY(floatInVec(radians));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int ysw[4] = {0, 0xffffffff, 0, 0};
|
|
VM_ATTRIBUTE_ALIGN16 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);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat Quat::rotationZ(float radians)
|
|
{
|
|
return rotationZ(floatInVec(radians));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int zsw[4] = {0, 0, 0xffffffff, 0};
|
|
VM_ATTRIBUTE_ALIGN16 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);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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));
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0, 0, 0, 0xffffffff};
|
|
return Quat(vec_sel(qv, qw, sw));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE Quat &Quat::operator*=(const Quat &quat)
|
|
{
|
|
*this = *this * quat;
|
|
return *this;
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat conj(const Quat &quat)
|
|
{
|
|
VM_ATTRIBUTE_ALIGN16 unsigned int sw[4] = {0x80000000, 0x80000000, 0x80000000, 0};
|
|
return Quat(vec_xor(quat.get128(), _mm_load_ps((float *)sw)));
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE const Quat select(const Quat &quat0, const Quat &quat1, bool select1)
|
|
{
|
|
return select(quat0, quat1, boolInVec(select1));
|
|
}
|
|
|
|
//VECTORMATH_FORCE_INLINE const Quat select( const Quat &quat0, const Quat &quat1, const boolInVec &select1 )
|
|
//{
|
|
// return Quat( vec_sel( quat0.get128(), quat1.get128(), select1.get128() ) );
|
|
//}
|
|
|
|
VECTORMATH_FORCE_INLINE void loadXYZW(Quat &quat, const float *fptr)
|
|
{
|
|
#ifdef USE_SSE3_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
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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
|
|
|
|
VECTORMATH_FORCE_INLINE 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]);
|
|
}
|
|
|
|
VECTORMATH_FORCE_INLINE 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
|