Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

bt -> b3 and BT -> B3 rename for content and filenames

Browse Source
This commit is contained in:
erwin coumans
2013-04-28 23:11:10 -07:00
parent 6bcb5b9d5f
commit 7366e262fd
178 changed files with 5218 additions and 5218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

338
src/Bullet3Common/b3Matrix3x3.h
View File

@@ -13,29 +13,29 @@ subject to the following restrictions:
*/
#ifndef BT_MATRIX3x3_H
#define BT_MATRIX3x3_H
#ifndef B3_MATRIX3x3_H
#define B3_MATRIX3x3_H
#include "b3Vector3.h"
#include "b3Quaternion.h"
#include <stdio.h>
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
//const __m128 ATTRIBUTE_ALIGNED16(v2220) = {2.0f, 2.0f, 2.0f, 0.0f};
const __m128 ATTRIBUTE_ALIGNED16(vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f};
#endif
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
const b3SimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
const b3SimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
const b3SimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
#endif
#ifdef BT_USE_DOUBLE_PRECISION
#define btMatrix3x3Data btMatrix3x3DoubleData
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Matrix3x3Data b3Matrix3x3DoubleData
#else
#define btMatrix3x3Data btMatrix3x3FloatData
#endif //BT_USE_DOUBLE_PRECISION
#define b3Matrix3x3Data b3Matrix3x3FloatData
#endif //B3_USE_DOUBLE_PRECISION
/**@brief The b3Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with b3Quaternion, b3Transform and b3Vector3.
@@ -70,8 +70,8 @@ public:
zx, zy, zz);
}
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
SIMD_FORCE_INLINE b3Matrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 )
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
SIMD_FORCE_INLINE b3Matrix3x3 (const b3SimdFloat4 v0, const b3SimdFloat4 v1, const b3SimdFloat4 v2 )
{
m_el[0].mVec128 = v0;
m_el[1].mVec128 = v1;
@@ -136,7 +136,7 @@ public:
* @param i Row number 0 indexed */
SIMD_FORCE_INLINE const b3Vector3& getRow(int i) const
{
btFullAssert(0 <= i && i < 3);
b3FullAssert(0 <= i && i < 3);
return m_el[i];
}
@@ -144,7 +144,7 @@ public:
* @param i Row number 0 indexed */
SIMD_FORCE_INLINE b3Vector3& operator[](int i)
{
btFullAssert(0 <= i && i < 3);
b3FullAssert(0 <= i && i < 3);
return m_el[i];
}
@@ -152,7 +152,7 @@ public:
* @param i Row number 0 indexed */
SIMD_FORCE_INLINE const b3Vector3& operator[](int i) const
{
btFullAssert(0 <= i && i < 3);
b3FullAssert(0 <= i && i < 3);
return m_el[i];
}
@@ -204,38 +204,38 @@ public:
void setRotation(const b3Quaternion& q)
{
b3Scalar d = q.length2();
btFullAssert(d != b3Scalar(0.0));
b3FullAssert(d != b3Scalar(0.0));
b3Scalar s = b3Scalar(2.0) / d;
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vs, Q = q.get128();
__m128i Qi = btCastfTo128i(Q);
__m128i Qi = b3CastfTo128i(Q);
__m128 Y, Z;
__m128 V1, V2, V3;
__m128 V11, V21, V31;
__m128 NQ = _mm_xor_ps(Q, btvMzeroMask);
__m128i NQi = btCastfTo128i(NQ);
__m128 NQ = _mm_xor_ps(Q, b3vMzeroMask);
__m128i NQi = b3CastfTo128i(NQ);
V1 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,2,3))); // Y X Z W
V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0,0,1,3)); // -X -X Y W
V3 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(2,1,0,3))); // Z Y X W
V1 = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(1,0,2,3))); // Y X Z W
V2 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(0,0,1,3)); // -X -X Y W
V3 = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(2,1,0,3))); // Z Y X W
V1 = _mm_xor_ps(V1, vMPPP); // change the sign of the first element
V11 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,1,0,3))); // Y Y X W
V11 = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(1,1,0,3))); // Y Y X W
V21 = _mm_unpackhi_ps(Q, Q); // Z Z W W
V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(0,2,0,3)); // X Z -X -W
V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(0,2,0,3)); // X Z -X -W
V2 = V2 * V1; //
V1 = V1 * V11; //
V3 = V3 * V31; //
V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2,3,1,3)); // -Z -W Y W
V11 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(2,3,1,3)); // -Z -W Y W
V11 = V11 * V21; //
V21 = _mm_xor_ps(V21, vMPPP); // change the sign of the first element
V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3,3,1,3)); // W W -Y -W
V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(3,3,1,3)); // W W -Y -W
V31 = _mm_xor_ps(V31, vMPPP); // change the sign of the first element
Y = btCastiTo128f(_mm_shuffle_epi32 (NQi, BT_SHUFFLE(3,2,0,3))); // -W -Z -X -W
Z = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,1,3))); // Y X Y W
Y = b3CastiTo128f(_mm_shuffle_epi32 (NQi, B3_SHUFFLE(3,2,0,3))); // -W -Z -X -W
Z = b3CastiTo128f(_mm_shuffle_epi32 (Qi, B3_SHUFFLE(1,0,1,3))); // Y X Y W
vs = _mm_load_ss(&s);
V21 = V21 * Y;
@@ -245,7 +245,7 @@ public:
V2 = V2 + V21;
V3 = V3 + V31;
vs = bt_splat3_ps(vs, 0);
vs = b3_splat3_ps(vs, 0);
// s ready
V1 = V1 * vs;
V2 = V2 * vs;
@@ -292,12 +292,12 @@ public:
**/
void setEulerZYX(b3Scalar eulerX,b3Scalar eulerY,b3Scalar eulerZ) {
///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
b3Scalar ci ( btCos(eulerX));
b3Scalar cj ( btCos(eulerY));
b3Scalar ch ( btCos(eulerZ));
b3Scalar si ( btSin(eulerX));
b3Scalar sj ( btSin(eulerY));
b3Scalar sh ( btSin(eulerZ));
b3Scalar ci ( b3Cos(eulerX));
b3Scalar cj ( b3Cos(eulerY));
b3Scalar ch ( b3Cos(eulerZ));
b3Scalar si ( b3Sin(eulerX));
b3Scalar sj ( b3Sin(eulerY));
b3Scalar sh ( b3Sin(eulerZ));
b3Scalar cc = ci * ch;
b3Scalar cs = ci * sh;
b3Scalar sc = si * ch;
@@ -311,7 +311,7 @@ public:
/**@brief Set the matrix to the identity */
void setIdentity()
{
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) || defined(B3_USE_NEON)
m_el[0] = v1000;
m_el[1] = v0100;
m_el[2] = v0010;
@@ -324,7 +324,7 @@ public:
static const b3Matrix3x3& getIdentity()
{
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) || defined(B3_USE_NEON)
static const b3Matrix3x3
identityMatrix(v1000, v0100, v0010);
#else
@@ -341,26 +341,26 @@ public:
* @param m The array to be filled */
void getOpenGLSubMatrix(b3Scalar *m) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 v0 = m_el[0].mVec128;
__m128 v1 = m_el[1].mVec128;
__m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2
__m128 *vm = (__m128 *)m;
__m128 vT;
v2 = _mm_and_ps(v2, btvFFF0fMask); // x2 y2 z2 0
v2 = _mm_and_ps(v2, b3vFFF0fMask); // x2 y2 z2 0
vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * *
v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1
v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0
v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0
v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0
v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0
v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0
v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT))); // z0 z1 z2 0
vm[0] = v0;
vm[1] = v1;
vm[2] = v2;
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
float32x4_t *vm = (float32x4_t *)m;
@@ -394,12 +394,12 @@ public:
* @param q The quaternion which will be set */
void getRotation(b3Quaternion& q) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
b3Scalar s, x;
union {
btSimdFloat4 vec;
b3SimdFloat4 vec;
b3Scalar f[4];
} temp;
@@ -440,7 +440,7 @@ public:
//temp.f[i] = s * b3Scalar(0.5);
}
s = btSqrt(x);
s = b3Sqrt(x);
q.set128(temp.vec);
s = b3Scalar(0.5) / s;
@@ -452,7 +452,7 @@ public:
if (trace > b3Scalar(0.0))
{
b3Scalar s = btSqrt(trace + b3Scalar(1.0));
b3Scalar s = b3Sqrt(trace + b3Scalar(1.0));
temp[3]=(s * b3Scalar(0.5));
s = b3Scalar(0.5) / s;
@@ -468,7 +468,7 @@ public:
int j = (i + 1) % 3;
int k = (i + 2) % 3;
b3Scalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0));
b3Scalar s = b3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0));
temp[i] = s * b3Scalar(0.5);
s = b3Scalar(0.5) / s;
@@ -488,12 +488,12 @@ public:
{
// first use the normal calculus
yaw = b3Scalar(btAtan2(m_el[1].getX(), m_el[0].getX()));
pitch = b3Scalar(btAsin(-m_el[2].getX()));
roll = b3Scalar(btAtan2(m_el[2].getY(), m_el[2].getZ()));
yaw = b3Scalar(b3Atan2(m_el[1].getX(), m_el[0].getX()));
pitch = b3Scalar(b3Asin(-m_el[2].getX()));
roll = b3Scalar(b3Atan2(m_el[2].getY(), m_el[2].getZ()));
// on pitch = +/-HalfPI
if (btFabs(pitch)==SIMD_HALF_PI)
if (b3Fabs(pitch)==SIMD_HALF_PI)
{
if (yaw>0)
yaw-=SIMD_PI;
@@ -527,13 +527,13 @@ public:
//get the pointer to the raw data
// Check that pitch is not at a singularity
if (btFabs(m_el[2].getX()) >= 1)
if (b3Fabs(m_el[2].getX()) >= 1)
{
euler_out.yaw = 0;
euler_out2.yaw = 0;
// From difference of angles formula
b3Scalar delta = btAtan2(m_el[0].getX(),m_el[0].getZ());
b3Scalar delta = b3Atan2(m_el[0].getX(),m_el[0].getZ());
if (m_el[2].getX() > 0) //gimbal locked up
{
euler_out.pitch = SIMD_PI / b3Scalar(2.0);
@@ -551,18 +551,18 @@ public:
}
else
{
euler_out.pitch = - btAsin(m_el[2].getX());
euler_out.pitch = - b3Asin(m_el[2].getX());
euler_out2.pitch = SIMD_PI - euler_out.pitch;
euler_out.roll = btAtan2(m_el[2].getY()/btCos(euler_out.pitch),
m_el[2].getZ()/btCos(euler_out.pitch));
euler_out2.roll = btAtan2(m_el[2].getY()/btCos(euler_out2.pitch),
m_el[2].getZ()/btCos(euler_out2.pitch));
euler_out.roll = b3Atan2(m_el[2].getY()/b3Cos(euler_out.pitch),
m_el[2].getZ()/b3Cos(euler_out.pitch));
euler_out2.roll = b3Atan2(m_el[2].getY()/b3Cos(euler_out2.pitch),
m_el[2].getZ()/b3Cos(euler_out2.pitch));
euler_out.yaw = btAtan2(m_el[1].getX()/btCos(euler_out.pitch),
m_el[0].getX()/btCos(euler_out.pitch));
euler_out2.yaw = btAtan2(m_el[1].getX()/btCos(euler_out2.pitch),
m_el[0].getX()/btCos(euler_out2.pitch));
euler_out.yaw = b3Atan2(m_el[1].getX()/b3Cos(euler_out.pitch),
m_el[0].getX()/b3Cos(euler_out.pitch));
euler_out2.yaw = b3Atan2(m_el[1].getX()/b3Cos(euler_out2.pitch),
m_el[0].getX()/b3Cos(euler_out2.pitch));
}
if (solution_number == 1)
@@ -584,7 +584,7 @@ public:
b3Matrix3x3 scaled(const b3Vector3& s) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
return b3Matrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
#else
return b3Matrix3x3(
@@ -640,15 +640,15 @@ public:
int p = 0;
int q = 1;
int r = 2;
b3Scalar max = btFabs(m_el[0][1]);
b3Scalar v = btFabs(m_el[0][2]);
b3Scalar max = b3Fabs(m_el[0][1]);
b3Scalar v = b3Fabs(m_el[0][2]);
if (v > max)
{
q = 2;
r = 1;
max = v;
}
v = btFabs(m_el[1][2]);
v = b3Fabs(m_el[1][2]);
if (v > max)
{
p = 1;
@@ -657,7 +657,7 @@ public:
max = v;
}
b3Scalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
b3Scalar t = threshold * (b3Fabs(m_el[0][0]) + b3Fabs(m_el[1][1]) + b3Fabs(m_el[2][2]));
if (max <= t)
{
if (max <= SIMD_EPSILON * t)
@@ -675,9 +675,9 @@ public:
b3Scalar sin;
if (theta2 * theta2 < b3Scalar(10 / SIMD_EPSILON))
{
t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
: 1 / (theta - btSqrt(1 + theta2));
cos = 1 / btSqrt(1 + t * t);
t = (theta >= 0) ? 1 / (theta + b3Sqrt(1 + theta2))
: 1 / (theta - b3Sqrt(1 + theta2));
cos = 1 / b3Sqrt(1 + t * t);
sin = cos * t;
}
else
@@ -724,15 +724,15 @@ public:
return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
}
void serialize(struct btMatrix3x3Data& dataOut) const;
void serialize(struct b3Matrix3x3Data& dataOut) const;
void serializeFloat(struct btMatrix3x3FloatData& dataOut) const;
void serializeFloat(struct b3Matrix3x3FloatData& dataOut) const;
void deSerialize(const struct btMatrix3x3Data& dataIn);
void deSerialize(const struct b3Matrix3x3Data& dataIn);
void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn);
void deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn);
void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn);
void deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn);
};
@@ -740,7 +740,7 @@ public:
SIMD_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator*=(const b3Matrix3x3& m)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 rv00, rv01, rv02;
__m128 rv10, rv11, rv12;
__m128 rv20, rv21, rv22;
@@ -750,32 +750,32 @@ b3Matrix3x3::operator*=(const b3Matrix3x3& m)
rv12 = m_el[1].mVec128;
rv22 = m_el[2].mVec128;
mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask);
mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask);
mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask);
mv0 = _mm_and_ps(m[0].mVec128, b3vFFF0fMask);
mv1 = _mm_and_ps(m[1].mVec128, b3vFFF0fMask);
mv2 = _mm_and_ps(m[2].mVec128, b3vFFF0fMask);
// rv0
rv00 = bt_splat_ps(rv02, 0);
rv01 = bt_splat_ps(rv02, 1);
rv02 = bt_splat_ps(rv02, 2);
rv00 = b3_splat_ps(rv02, 0);
rv01 = b3_splat_ps(rv02, 1);
rv02 = b3_splat_ps(rv02, 2);
rv00 = _mm_mul_ps(rv00, mv0);
rv01 = _mm_mul_ps(rv01, mv1);
rv02 = _mm_mul_ps(rv02, mv2);
// rv1
rv10 = bt_splat_ps(rv12, 0);
rv11 = bt_splat_ps(rv12, 1);
rv12 = bt_splat_ps(rv12, 2);
rv10 = b3_splat_ps(rv12, 0);
rv11 = b3_splat_ps(rv12, 1);
rv12 = b3_splat_ps(rv12, 2);
rv10 = _mm_mul_ps(rv10, mv0);
rv11 = _mm_mul_ps(rv11, mv1);
rv12 = _mm_mul_ps(rv12, mv2);
// rv2
rv20 = bt_splat_ps(rv22, 0);
rv21 = bt_splat_ps(rv22, 1);
rv22 = bt_splat_ps(rv22, 2);
rv20 = b3_splat_ps(rv22, 0);
rv21 = b3_splat_ps(rv22, 1);
rv22 = b3_splat_ps(rv22, 2);
rv20 = _mm_mul_ps(rv20, mv0);
rv21 = _mm_mul_ps(rv21, mv1);
@@ -789,7 +789,7 @@ b3Matrix3x3::operator*=(const b3Matrix3x3& m)
m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t rv0, rv1, rv2;
float32x4_t v0, v1, v2;
@@ -799,9 +799,9 @@ b3Matrix3x3::operator*=(const b3Matrix3x3& m)
v1 = m_el[1].mVec128;
v2 = m_el[2].mVec128;
mv0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
mv1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
mv2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
mv0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
mv1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
mv2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
@@ -830,7 +830,7 @@ b3Matrix3x3::operator*=(const b3Matrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator+=(const b3Matrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
@@ -852,13 +852,13 @@ b3Matrix3x3::operator+=(const b3Matrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3
operator*(const b3Matrix3x3& m, const b3Scalar & k)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 vk = bt_splat_ps(_mm_load_ss((float *)&k), 0x80);
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 vk = b3_splat_ps(_mm_load_ss((float *)&k), 0x80);
return b3Matrix3x3(
_mm_mul_ps(m[0].mVec128, vk),
_mm_mul_ps(m[1].mVec128, vk),
_mm_mul_ps(m[2].mVec128, vk));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
return b3Matrix3x3(
vmulq_n_f32(m[0].mVec128, k),
vmulq_n_f32(m[1].mVec128, k),
@@ -874,7 +874,7 @@ operator*(const b3Matrix3x3& m, const b3Scalar & k)
SIMD_FORCE_INLINE b3Matrix3x3
operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
return b3Matrix3x3(
m1[0].mVec128 + m2[0].mVec128,
m1[1].mVec128 + m2[1].mVec128,
@@ -898,7 +898,7 @@ operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
SIMD_FORCE_INLINE b3Matrix3x3
operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
return b3Matrix3x3(
m1[0].mVec128 - m2[0].mVec128,
m1[1].mVec128 - m2[1].mVec128,
@@ -923,7 +923,7 @@ operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
SIMD_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator-=(const b3Matrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
@@ -946,52 +946,52 @@ b3Matrix3x3::operator-=(const b3Matrix3x3& m)
SIMD_FORCE_INLINE b3Scalar
b3Matrix3x3::determinant() const
{
return btTriple((*this)[0], (*this)[1], (*this)[2]);
return b3Triple((*this)[0], (*this)[1], (*this)[2]);
}
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::absolute() const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
return b3Matrix3x3(
_mm_and_ps(m_el[0].mVec128, btvAbsfMask),
_mm_and_ps(m_el[1].mVec128, btvAbsfMask),
_mm_and_ps(m_el[2].mVec128, btvAbsfMask));
#elif defined(BT_USE_NEON)
_mm_and_ps(m_el[0].mVec128, b3vAbsfMask),
_mm_and_ps(m_el[1].mVec128, b3vAbsfMask),
_mm_and_ps(m_el[2].mVec128, b3vAbsfMask));
#elif defined(B3_USE_NEON)
return b3Matrix3x3(
(float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask),
(float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask),
(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
(float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, b3v3AbsMask),
(float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, b3v3AbsMask),
(float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, b3v3AbsMask));
#else
return b3Matrix3x3(
btFabs(m_el[0].getX()), btFabs(m_el[0].getY()), btFabs(m_el[0].getZ()),
btFabs(m_el[1].getX()), btFabs(m_el[1].getY()), btFabs(m_el[1].getZ()),
btFabs(m_el[2].getX()), btFabs(m_el[2].getY()), btFabs(m_el[2].getZ()));
b3Fabs(m_el[0].getX()), b3Fabs(m_el[0].getY()), b3Fabs(m_el[0].getZ()),
b3Fabs(m_el[1].getX()), b3Fabs(m_el[1].getY()), b3Fabs(m_el[1].getZ()),
b3Fabs(m_el[2].getX()), b3Fabs(m_el[2].getY()), b3Fabs(m_el[2].getZ()));
#endif
}
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::transpose() const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 v0 = m_el[0].mVec128;
__m128 v1 = m_el[1].mVec128;
__m128 v2 = m_el[2].mVec128; // x2 y2 z2 w2
__m128 vT;
v2 = _mm_and_ps(v2, btvFFF0fMask); // x2 y2 z2 0
v2 = _mm_and_ps(v2, b3vFFF0fMask); // x2 y2 z2 0
vT = _mm_unpackhi_ps(v0, v1); // z0 z1 * *
v0 = _mm_unpacklo_ps(v0, v1); // x0 x1 y0 y1
v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0
v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0
v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT))); // z0 z1 z2 0
v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3) ); // y0 y1 y2 0
v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3) ); // x0 x1 x2 0
v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT))); // z0 z1 z2 0
return b3Matrix3x3( v0, v1, v2 );
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 ); // {x0 x1 z0 z1}, {y0 y1 w0 w1}
@@ -1021,7 +1021,7 @@ b3Matrix3x3::inverse() const
{
b3Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
b3Scalar det = (*this)[0].dot(co);
btFullAssert(det != b3Scalar(0.0));
b3FullAssert(det != b3Scalar(0.0));
b3Scalar s = b3Scalar(1.0) / det;
return b3Matrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
@@ -1031,13 +1031,13 @@ b3Matrix3x3::inverse() const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
// zeros w
// static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
__m128 row = m_el[0].mVec128;
__m128 m0 = _mm_and_ps( m.getRow(0).mVec128, btvFFF0fMask );
__m128 m1 = _mm_and_ps( m.getRow(1).mVec128, btvFFF0fMask);
__m128 m2 = _mm_and_ps( m.getRow(2).mVec128, btvFFF0fMask );
__m128 m0 = _mm_and_ps( m.getRow(0).mVec128, b3vFFF0fMask );
__m128 m1 = _mm_and_ps( m.getRow(1).mVec128, b3vFFF0fMask);
__m128 m2 = _mm_and_ps( m.getRow(2).mVec128, b3vFFF0fMask );
__m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
__m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
__m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
@@ -1051,7 +1051,7 @@ b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
r2 = _mm_add_ps( r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
return b3Matrix3x3( r0, r1, r2 );
#elif defined BT_USE_NEON
#elif defined B3_USE_NEON
// zeros w
static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
float32x4_t m0 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(0).mVec128, xyzMask );
@@ -1087,7 +1087,7 @@ b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
SIMD_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 a0 = m_el[0].mVec128;
__m128 a1 = m_el[1].mVec128;
__m128 a2 = m_el[2].mVec128;
@@ -1108,7 +1108,7 @@ b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
return b3Matrix3x3( r0, r1, r2);
#elif defined BT_USE_NEON
#elif defined B3_USE_NEON
float32x4_t a0 = m_el[0].mVec128;
float32x4_t a1 = m_el[1].mVec128;
float32x4_t a2 = m_el[2].mVec128;
@@ -1140,7 +1140,7 @@ b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Matrix3x3& m, const b3Vector3& v)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))|| defined (B3_USE_NEON)
return v.dot3(m[0], m[1], m[2]);
#else
return b3Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
@@ -1151,30 +1151,30 @@ operator*(const b3Matrix3x3& m, const b3Vector3& v)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Matrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
const __m128 vv = v.mVec128;
__m128 c0 = bt_splat_ps( vv, 0);
__m128 c1 = bt_splat_ps( vv, 1);
__m128 c2 = bt_splat_ps( vv, 2);
__m128 c0 = b3_splat_ps( vv, 0);
__m128 c1 = b3_splat_ps( vv, 1);
__m128 c2 = b3_splat_ps( vv, 2);
c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask) );
c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask) );
c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, b3vFFF0fMask) );
c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, b3vFFF0fMask) );
c0 = _mm_add_ps(c0, c1);
c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask) );
c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, b3vFFF0fMask) );
return b3Vector3(_mm_add_ps(c0, c2));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
const float32x4_t vv = v.mVec128;
const float32x2_t vlo = vget_low_f32(vv);
const float32x2_t vhi = vget_high_f32(vv);
float32x4_t c0, c1, c2;
c0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
c1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
c2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
c0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
c1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
c2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
c0 = vmulq_lane_f32(c0, vlo, 0);
c1 = vmulq_lane_f32(c1, vlo, 1);
@@ -1191,41 +1191,41 @@ operator*(const b3Vector3& v, const b3Matrix3x3& m)
SIMD_FORCE_INLINE b3Matrix3x3
operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 m10 = m1[0].mVec128;
__m128 m11 = m1[1].mVec128;
__m128 m12 = m1[2].mVec128;
__m128 m2v = _mm_and_ps(m2[0].mVec128, btvFFF0fMask);
__m128 m2v = _mm_and_ps(m2[0].mVec128, b3vFFF0fMask);
__m128 c0 = bt_splat_ps( m10, 0);
__m128 c1 = bt_splat_ps( m11, 0);
__m128 c2 = bt_splat_ps( m12, 0);
__m128 c0 = b3_splat_ps( m10, 0);
__m128 c1 = b3_splat_ps( m11, 0);
__m128 c2 = b3_splat_ps( m12, 0);
c0 = _mm_mul_ps(c0, m2v);
c1 = _mm_mul_ps(c1, m2v);
c2 = _mm_mul_ps(c2, m2v);
m2v = _mm_and_ps(m2[1].mVec128, btvFFF0fMask);
m2v = _mm_and_ps(m2[1].mVec128, b3vFFF0fMask);
__m128 c0_1 = bt_splat_ps( m10, 1);
__m128 c1_1 = bt_splat_ps( m11, 1);
__m128 c2_1 = bt_splat_ps( m12, 1);
__m128 c0_1 = b3_splat_ps( m10, 1);
__m128 c1_1 = b3_splat_ps( m11, 1);
__m128 c2_1 = b3_splat_ps( m12, 1);
c0_1 = _mm_mul_ps(c0_1, m2v);
c1_1 = _mm_mul_ps(c1_1, m2v);
c2_1 = _mm_mul_ps(c2_1, m2v);
m2v = _mm_and_ps(m2[2].mVec128, btvFFF0fMask);
m2v = _mm_and_ps(m2[2].mVec128, b3vFFF0fMask);
c0 = _mm_add_ps(c0, c0_1);
c1 = _mm_add_ps(c1, c1_1);
c2 = _mm_add_ps(c2, c2_1);
m10 = bt_splat_ps( m10, 2);
m11 = bt_splat_ps( m11, 2);
m12 = bt_splat_ps( m12, 2);
m10 = b3_splat_ps( m10, 2);
m11 = b3_splat_ps( m11, 2);
m12 = b3_splat_ps( m12, 2);
m10 = _mm_mul_ps(m10, m2v);
m11 = _mm_mul_ps(m11, m2v);
@@ -1237,7 +1237,7 @@ operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
return b3Matrix3x3(c0, c1, c2);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t rv0, rv1, rv2;
float32x4_t v0, v1, v2;
@@ -1247,9 +1247,9 @@ operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
v1 = m1[1].mVec128;
v2 = m1[2].mVec128;
mv0 = (float32x4_t) vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask);
mv1 = (float32x4_t) vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask);
mv2 = (float32x4_t) vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask);
mv0 = (float32x4_t) vandq_s32((int32x4_t)m2[0].mVec128, b3vFFF0Mask);
mv1 = (float32x4_t) vandq_s32((int32x4_t)m2[1].mVec128, b3vFFF0Mask);
mv2 = (float32x4_t) vandq_s32((int32x4_t)m2[2].mVec128, b3vFFF0Mask);
rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
@@ -1274,7 +1274,7 @@ operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
}
/*
SIMD_FORCE_INLINE b3Matrix3x3 btMultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) {
SIMD_FORCE_INLINE b3Matrix3x3 b3MultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) {
return b3Matrix3x3(
m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
@@ -1292,7 +1292,7 @@ m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
* It will test all elements are equal. */
SIMD_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 c0, c1, c2;
@@ -1313,50 +1313,50 @@ SIMD_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
}
///for serialization
struct btMatrix3x3FloatData
struct b3Matrix3x3FloatData
{
btVector3FloatData m_el[3];
b3Vector3FloatData m_el[3];
};
///for serialization
struct btMatrix3x3DoubleData
struct b3Matrix3x3DoubleData
{
btVector3DoubleData m_el[3];
b3Vector3DoubleData m_el[3];
};
SIMD_FORCE_INLINE void b3Matrix3x3::serialize(struct btMatrix3x3Data& dataOut) const
SIMD_FORCE_INLINE void b3Matrix3x3::serialize(struct b3Matrix3x3Data& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serialize(dataOut.m_el[i]);
}
SIMD_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const
SIMD_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct b3Matrix3x3FloatData& dataOut) const
{
for (int i=0;i<3;i++)
m_el[i].serializeFloat(dataOut.m_el[i]);
}
SIMD_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn)
SIMD_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct b3Matrix3x3Data& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerialize(dataIn.m_el[i]);
}
SIMD_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn)
SIMD_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeFloat(dataIn.m_el[i]);
}
SIMD_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn)
SIMD_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn)
{
for (int i=0;i<3;i++)
m_el[i].deSerializeDouble(dataIn.m_el[i]);
}
#endif //BT_MATRIX3x3_H
#endif //B3_MATRIX3x3_H