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

Apple contribution for OSX SSE and iOS NEON optimizations unit tests, thanks to Jordan Hubbard, Ian Ollmann and Hristo Hristov.

Browse Source 
For OSX:
cd build
./premake_osx xcode4
for iOS:
cd build
./ios_build.sh
./ios_run.sh

Also integrated the branches/StackAllocation to make it easier to multi-thread collision detection in the near future. It avoids changing the btCollisionObject while performing collision detection.

As this is a large patch, some stuff might be temporarily broken, I'll keep an eye out on issues.
This commit is contained in:
erwin.coumans
2012-06-07 00:56:30 +00:00
parent 777b92a2ad
commit 73b217fb07
323 changed files with 30730 additions and 13635 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
src/LinearMath/CMakeLists.txt
View File

@@ -10,6 +10,7 @@ SET(LinearMath_SRCS
btGeometryUtil.cpp
btQuickprof.cpp
btSerializer.cpp
btVector3.cpp
)
SET(LinearMath_HDRS

8
src/LinearMath/btAabbUtil2.h
View File

@@ -184,9 +184,7 @@ SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar ma
btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
abs_b[1].dot(halfExtentsWithMargin),
abs_b[2].dot(halfExtentsWithMargin));
btVector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
@@ -203,9 +201,7 @@ SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVec
btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
abs_b[1].dot(localHalfExtents),
abs_b[2].dot(localHalfExtents));
btVector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
aabbMinOut = center-extent;
aabbMaxOut = center+extent;
}

2
src/LinearMath/btAlignedAllocator.cpp
View File

@@ -119,7 +119,7 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen
real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1));
if (real) {
ret = (void*) btAlignPointer((real + 2*sizeof(void *), alignment);
ret = (void*) btAlignPointer(real + 2*sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
*((int*)(ret)-2) = size;

19
src/LinearMath/btAlignedObjectArray.h
View File

@@ -197,8 +197,26 @@ protected:
m_data[m_size].~T();
}
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
{
int curSize = size();
if (newsize < curSize)
{
} else
{
if (newsize > size())
{
reserve(newsize);
}
//leave this uninitialized
}
m_size = newsize;
}
SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T())
{
int curSize = size();
@@ -226,7 +244,6 @@ protected:
m_size = newsize;
}
SIMD_FORCE_INLINE T& expandNonInitializing( )
{
int sz = size();

9
src/LinearMath/btConvexHull.cpp
View File

@@ -22,13 +22,6 @@ subject to the following restrictions:
template <class T>
void Swap(T &a,T &b)
{
T tmp = a;
a=b;
b=tmp;
}
//----------------------------------
@@ -518,7 +511,7 @@ int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectAr
if(p3==p0||p3==p1||p3==p2)
return int4(-1,-1,-1,-1);
btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);}
return int4(p0,p1,p2,p3);
}

5502
src/LinearMath/btConvexHullComputer.cpp
View File

File diff suppressed because it is too large Load Diff

206
src/LinearMath/btConvexHullComputer.h
View File

@@ -1,103 +1,103 @@
/*
Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONVEX_HULL_COMPUTER_H
#define BT_CONVEX_HULL_COMPUTER_H
#include "btVector3.h"
#include "btAlignedObjectArray.h"
/// Convex hull implementation based on Preparata and Hong
/// See http://code.google.com/p/bullet/issues/detail?id=275
/// Ole Kniemeyer, MAXON Computer GmbH
class btConvexHullComputer
{
private:
btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
public:
class Edge
{
private:
int next;
int reverse;
int targetVertex;
friend class btConvexHullComputer;
public:
int getSourceVertex() const
{
return (this + reverse)->targetVertex;
}
int getTargetVertex() const
{
return targetVertex;
}
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}
const Edge* getReverseEdge() const
{
return this + reverse;
}
};
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
// Edges of the output hull
btAlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
btAlignedObjectArray<int> faces;
/*
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
by that amount (each face is moved by "shrink" length units towards the center along its normal).
If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
is the minimum distance of a face to the center of the convex hull.
The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
that the resulting convex hull is empty.
The output convex hull can be found in the member variables "vertices", "edges", "faces".
*/
btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}
};
#endif //BT_CONVEX_HULL_COMPUTER_H
/*
Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONVEX_HULL_COMPUTER_H
#define BT_CONVEX_HULL_COMPUTER_H
#include "btVector3.h"
#include "btAlignedObjectArray.h"
/// Convex hull implementation based on Preparata and Hong
/// See http://code.google.com/p/bullet/issues/detail?id=275
/// Ole Kniemeyer, MAXON Computer GmbH
class btConvexHullComputer
{
private:
btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
public:
class Edge
{
private:
int next;
int reverse;
int targetVertex;
friend class btConvexHullComputer;
public:
int getSourceVertex() const
{
return (this + reverse)->targetVertex;
}
int getTargetVertex() const
{
return targetVertex;
}
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}
const Edge* getReverseEdge() const
{
return this + reverse;
}
};
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
// Edges of the output hull
btAlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
btAlignedObjectArray<int> faces;
/*
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
by that amount (each face is moved by "shrink" length units towards the center along its normal).
If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius"
is the minimum distance of a face to the center of the convex hull.
The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large
that the resulting convex hull is empty.
The output convex hull can be found in the member variables "vertices", "edges", "faces".
*/
btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}
};
#endif //BT_CONVEX_HULL_COMPUTER_H

4
src/LinearMath/btDefaultMotionState.h
View File

@@ -4,13 +4,15 @@
#include "btMotionState.h"
///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
struct btDefaultMotionState : public btMotionState
ATTRIBUTE_ALIGNED16(struct) btDefaultMotionState : public btMotionState
{
btTransform m_graphicsWorldTrans;
btTransform m_centerOfMassOffset;
btTransform m_startWorldTrans;
void* m_userPointer;
BT_DECLARE_ALIGNED_ALLOCATOR();
btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
: m_graphicsWorldTrans(startTrans),
m_centerOfMassOffset(centerOfMassOffset),

3
src/LinearMath/btGrahamScan2dConvexHull.h
View File

@@ -70,7 +70,8 @@ inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& original
{
const btVector3& left = originalPoints[i];
const btVector3& right = originalPoints[0];
if (left.x() < right.x() || !(right.x() < left.x()) && left.y() < right.y())
if (left.x() < right.x() ||
(!(right.x() < left.x()) && left.y() < right.y()))
{
originalPoints.swap(0,i);
}

659
src/LinearMath/btMatrix3x3.h
View File

@@ -18,6 +18,18 @@ subject to the following restrictions:
#include "btVector3.h"
#include "btQuaternion.h"
#include <stdio.h>
#ifdef BT_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};
#endif
#ifdef BT_USE_DOUBLE_PRECISION
#define btMatrix3x3Data btMatrix3x3DoubleData
@@ -28,7 +40,7 @@ subject to the following restrictions:
/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
* Make sure to only include a pure orthogonal matrix without scaling. */
class btMatrix3x3 {
ATTRIBUTE_ALIGNED16(class) btMatrix3x3 {
///Data storage for the matrix, each vector is a row of the matrix
btVector3 m_el[3];
@@ -57,6 +69,42 @@ public:
yx, yy, yz,
zx, zy, zz);
}
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
SIMD_FORCE_INLINE btMatrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 )
{
m_el[0].mVec128 = v0;
m_el[1].mVec128 = v1;
m_el[2].mVec128 = v2;
}
SIMD_FORCE_INLINE btMatrix3x3 (const btVector3& v0, const btVector3& v1, const btVector3& v2 )
{
m_el[0] = v0;
m_el[1] = v1;
m_el[2] = v2;
}
// Copy constructor
SIMD_FORCE_INLINE btMatrix3x3(const btMatrix3x3& rhs)
{
m_el[0].mVec128 = rhs.m_el[0].mVec128;
m_el[1].mVec128 = rhs.m_el[1].mVec128;
m_el[2].mVec128 = rhs.m_el[2].mVec128;
}
// Assignment Operator
SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m)
{
m_el[0].mVec128 = m.m_el[0].mVec128;
m_el[1].mVec128 = m.m_el[1].mVec128;
m_el[2].mVec128 = m.m_el[2].mVec128;
return *this;
}
#else
/** @brief Copy constructor */
SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
{
@@ -64,6 +112,7 @@ public:
m_el[1] = other.m_el[1];
m_el[2] = other.m_el[2];
}
/** @brief Assignment Operator */
SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
{
@@ -73,6 +122,8 @@ public:
return *this;
}
#endif
/** @brief Get a column of the matrix as a vector
* @param i Column number 0 indexed */
SIMD_FORCE_INLINE btVector3 getColumn(int i) const
@@ -155,14 +206,69 @@ public:
btScalar d = q.length2();
btFullAssert(d != btScalar(0.0));
btScalar s = btScalar(2.0) / d;
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs, Q = q.get128();
__m128i Qi = btCastfTo128i(Q);
__m128 Y, Z;
__m128 V1, V2, V3;
__m128 V11, V21, V31;
__m128 NQ = _mm_xor_ps(Q, btvMzeroMask);
__m128i NQi = btCastfTo128i(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 = _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
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
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 = 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_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
vs = _mm_load_ss(&s);
V21 = V21 * Y;
V31 = V31 * Z;
V1 = V1 + V11;
V2 = V2 + V21;
V3 = V3 + V31;
vs = bt_splat3_ps(vs, 0);
// s ready
V1 = V1 * vs;
V2 = V2 * vs;
V3 = V3 * vs;
V1 = V1 + v1000;
V2 = V2 + v0100;
V3 = V3 + v0010;
m_el[0] = V1;
m_el[1] = V2;
m_el[2] = V3;
#else
btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
setValue(
btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
}
#endif
}
/** @brief Set the matrix from euler angles using YPR around YXZ respectively
@@ -205,16 +311,29 @@ 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)
m_el[0] = v1000;
m_el[1] = v0100;
m_el[2] = v0010;
#else
setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
btScalar(0.0), btScalar(1.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(1.0));
#endif
}
static const btMatrix3x3& getIdentity()
{
static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
static const btMatrix3x3
identityMatrix(v1000, v0100, v0010);
#else
static const btMatrix3x3
identityMatrix(
btScalar(1.0), btScalar(0.0), btScalar(0.0),
btScalar(0.0), btScalar(1.0), btScalar(0.0),
btScalar(0.0), btScalar(0.0), btScalar(1.0));
#endif
return identityMatrix;
}
@@ -222,6 +341,40 @@ public:
* @param m The array to be filled */
void getOpenGLSubMatrix(btScalar *m) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_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
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
vm[0] = v0;
vm[1] = v1;
vm[2] = v2;
#elif defined(BT_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;
float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 ); // {x0 x1 z0 z1}, {y0 y1 w0 w1}
float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) ); // {x2 0 }, {y2 0}
float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q ); // z0 z1 z2 0
vm[0] = v0;
vm[1] = v1;
vm[2] = v2;
#else
m[0] = btScalar(m_el[0].x());
m[1] = btScalar(m_el[1].x());
m[2] = btScalar(m_el[2].x());
@@ -234,13 +387,67 @@ public:
m[9] = btScalar(m_el[1].z());
m[10] = btScalar(m_el[2].z());
m[11] = btScalar(0.0);
#endif
}
/**@brief Get the matrix represented as a quaternion
* @param q The quaternion which will be set */
void getRotation(btQuaternion& q) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
btScalar s, x;
union {
btSimdFloat4 vec;
btScalar f[4];
} temp;
if (trace > btScalar(0.0))
{
x = trace + btScalar(1.0);
temp.f[0]=m_el[2].y() - m_el[1].z();
temp.f[1]=m_el[0].z() - m_el[2].x();
temp.f[2]=m_el[1].x() - m_el[0].y();
temp.f[3]=x;
//temp.f[3]= s * btScalar(0.5);
}
else
{
int i, j, k;
if(m_el[0].x() < m_el[1].y())
{
if( m_el[1].y() < m_el[2].z() )
{ i = 2; j = 0; k = 1; }
else
{ i = 1; j = 2; k = 0; }
}
else
{
if( m_el[0].x() < m_el[2].z())
{ i = 2; j = 0; k = 1; }
else
{ i = 0; j = 1; k = 2; }
}
x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0);
temp.f[3] = (m_el[k][j] - m_el[j][k]);
temp.f[j] = (m_el[j][i] + m_el[i][j]);
temp.f[k] = (m_el[k][i] + m_el[i][k]);
temp.f[i] = x;
//temp.f[i] = s * btScalar(0.5);
}
s = btSqrt(x);
q.set128(temp.vec);
s = btScalar(0.5) / s;
q *= s;
#else
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
btScalar temp[4];
if (trace > btScalar(0.0))
@@ -270,6 +477,7 @@ public:
temp[k] = (m_el[k][i] + m_el[i][k]) * s;
}
q.setValue(temp[0],temp[1],temp[2],temp[3]);
#endif
}
/**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
@@ -376,9 +584,14 @@ public:
btMatrix3x3 scaled(const btVector3& s) const
{
return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
#else
return btMatrix3x3(
m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
#endif
}
/**@brief Return the determinant of the matrix */
@@ -527,15 +740,101 @@ public:
SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator*=(const btMatrix3x3& m)
{
setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 rv00, rv01, rv02;
__m128 rv10, rv11, rv12;
__m128 rv20, rv21, rv22;
__m128 mv0, mv1, mv2;
rv02 = m_el[0].mVec128;
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);
// rv0
rv00 = bt_splat_ps(rv02, 0);
rv01 = bt_splat_ps(rv02, 1);
rv02 = bt_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 = _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 = _mm_mul_ps(rv20, mv0);
rv21 = _mm_mul_ps(rv21, mv1);
rv22 = _mm_mul_ps(rv22, mv2);
rv00 = _mm_add_ps(rv00, rv01);
rv10 = _mm_add_ps(rv10, rv11);
rv20 = _mm_add_ps(rv20, rv21);
m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
#elif defined(BT_USE_NEON)
float32x4_t rv0, rv1, rv2;
float32x4_t v0, v1, v2;
float32x4_t mv0, mv1, mv2;
v0 = m_el[0].mVec128;
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);
rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
m_el[0].mVec128 = rv0;
m_el[1].mVec128 = rv1;
m_el[2].mVec128 = rv2;
#else
setValue(
m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
#endif
return *this;
}
SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator+=(const btMatrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_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;
#else
setValue(
m_el[0][0]+m.m_el[0][0],
m_el[0][1]+m.m_el[0][1],
@@ -546,52 +845,89 @@ btMatrix3x3::operator+=(const btMatrix3x3& m)
m_el[2][0]+m.m_el[2][0],
m_el[2][1]+m.m_el[2][1],
m_el[2][2]+m.m_el[2][2]);
#endif
return *this;
}
SIMD_FORCE_INLINE btMatrix3x3
operator*(const btMatrix3x3& m, const btScalar & k)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 vk = bt_splat_ps(_mm_load_ss((float *)&k), 0x80);
return btMatrix3x3(
_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)
return btMatrix3x3(
vmulq_n_f32(m[0].mVec128, k),
vmulq_n_f32(m[1].mVec128, k),
vmulq_n_f32(m[2].mVec128, k));
#else
return btMatrix3x3(
m[0].x()*k,m[0].y()*k,m[0].z()*k,
m[1].x()*k,m[1].y()*k,m[1].z()*k,
m[2].x()*k,m[2].y()*k,m[2].z()*k);
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
SIMD_FORCE_INLINE btMatrix3x3
operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return btMatrix3x3(
m1[0][0]+m2[0][0],
m1[0][1]+m2[0][1],
m1[0][2]+m2[0][2],
m1[1][0]+m2[1][0],
m1[1][1]+m2[1][1],
m1[1][2]+m2[1][2],
m1[2][0]+m2[2][0],
m1[2][1]+m2[2][1],
m1[2][2]+m2[2][2]);
m1[0].mVec128 + m2[0].mVec128,
m1[1].mVec128 + m2[1].mVec128,
m1[2].mVec128 + m2[2].mVec128);
#else
return btMatrix3x3(
m1[0][0]+m2[0][0],
m1[0][1]+m2[0][1],
m1[0][2]+m2[0][2],
m1[1][0]+m2[1][0],
m1[1][1]+m2[1][1],
m1[1][2]+m2[1][2],
m1[2][0]+m2[2][0],
m1[2][1]+m2[2][1],
m1[2][2]+m2[2][2]);
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return btMatrix3x3(
m1[0][0]-m2[0][0],
m1[0][1]-m2[0][1],
m1[0][2]-m2[0][2],
m1[1][0]-m2[1][0],
m1[1][1]-m2[1][1],
m1[1][2]-m2[1][2],
m1[2][0]-m2[2][0],
m1[2][1]-m2[2][1],
m1[2][2]-m2[2][2]);
m1[0].mVec128 - m2[0].mVec128,
m1[1].mVec128 - m2[1].mVec128,
m1[2].mVec128 - m2[2].mVec128);
#else
return btMatrix3x3(
m1[0][0]-m2[0][0],
m1[0][1]-m2[0][1],
m1[0][2]-m2[0][2],
m1[1][0]-m2[1][0],
m1[1][1]-m2[1][1],
m1[1][2]-m2[1][2],
m1[2][0]-m2[2][0],
m1[2][1]-m2[2][1],
m1[2][2]-m2[2][2]);
#endif
}
SIMD_FORCE_INLINE btMatrix3x3&
btMatrix3x3::operator-=(const btMatrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_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;
#else
setValue(
m_el[0][0]-m.m_el[0][0],
m_el[0][1]-m.m_el[0][1],
@@ -602,6 +938,7 @@ btMatrix3x3::operator-=(const btMatrix3x3& m)
m_el[2][0]-m.m_el[2][0],
m_el[2][1]-m.m_el[2][1],
m_el[2][2]-m.m_el[2][2]);
#endif
return *this;
}
@@ -616,18 +953,59 @@ btMatrix3x3::determinant() const
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::absolute() const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
return btMatrix3x3(
_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)
return btMatrix3x3(
(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));
#else
return btMatrix3x3(
btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::transpose() const
{
return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
m_el[0].y(), m_el[1].y(), m_el[2].y(),
m_el[0].z(), m_el[1].z(), m_el[2].z());
#if (defined (BT_USE_SSE_IN_API) && defined (BT_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
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
return btMatrix3x3( v0, v1, v2 );
#elif defined(BT_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}
float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) ); // {x2 0 }, {y2 0}
float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q ); // z0 z1 z2 0
return btMatrix3x3( v0, v1, v2 );
#else
return btMatrix3x3( m_el[0].x(), m_el[1].x(), m_el[2].x(),
m_el[0].y(), m_el[1].y(), m_el[2].y(),
m_el[0].z(), m_el[1].z(), m_el[2].z());
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
@@ -653,7 +1031,47 @@ btMatrix3x3::inverse() const
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
{
return btMatrix3x3(
#if (defined (BT_USE_SSE_IN_API) && defined (BT_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 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));
row = m_el[1].mVec128;
r0 = _mm_add_ps( r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
r1 = _mm_add_ps( r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
r2 = _mm_add_ps( r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
row = m_el[2].mVec128;
r0 = _mm_add_ps( r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
r1 = _mm_add_ps( r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
r2 = _mm_add_ps( r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
return btMatrix3x3( r0, r1, r2 );
#elif defined BT_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 );
float32x4_t m1 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(1).mVec128, xyzMask );
float32x4_t m2 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(2).mVec128, xyzMask );
float32x4_t row = m_el[0].mVec128;
float32x4_t r0 = vmulq_lane_f32( m0, vget_low_f32(row), 0);
float32x4_t r1 = vmulq_lane_f32( m0, vget_low_f32(row), 1);
float32x4_t r2 = vmulq_lane_f32( m0, vget_high_f32(row), 0);
row = m_el[1].mVec128;
r0 = vmlaq_lane_f32( r0, m1, vget_low_f32(row), 0);
r1 = vmlaq_lane_f32( r1, m1, vget_low_f32(row), 1);
r2 = vmlaq_lane_f32( r2, m1, vget_high_f32(row), 0);
row = m_el[2].mVec128;
r0 = vmlaq_lane_f32( r0, m2, vget_low_f32(row), 0);
r1 = vmlaq_lane_f32( r1, m2, vget_low_f32(row), 1);
r2 = vmlaq_lane_f32( r2, m2, vget_high_f32(row), 0);
return btMatrix3x3( r0, r1, r2 );
#else
return btMatrix3x3(
m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
@@ -663,38 +1081,196 @@ btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 a0 = m_el[0].mVec128;
__m128 a1 = m_el[1].mVec128;
__m128 a2 = m_el[2].mVec128;
btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
__m128 mx = mT[0].mVec128;
__m128 my = mT[1].mVec128;
__m128 mz = mT[2].mVec128;
__m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
__m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
__m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
return btMatrix3x3( r0, r1, r2);
#elif defined BT_USE_NEON
float32x4_t a0 = m_el[0].mVec128;
float32x4_t a1 = m_el[1].mVec128;
float32x4_t a2 = m_el[2].mVec128;
btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
float32x4_t mx = mT[0].mVec128;
float32x4_t my = mT[1].mVec128;
float32x4_t mz = mT[2].mVec128;
float32x4_t r0 = vmulq_lane_f32( mx, vget_low_f32(a0), 0);
float32x4_t r1 = vmulq_lane_f32( mx, vget_low_f32(a1), 0);
float32x4_t r2 = vmulq_lane_f32( mx, vget_low_f32(a2), 0);
r0 = vmlaq_lane_f32( r0, my, vget_low_f32(a0), 1);
r1 = vmlaq_lane_f32( r1, my, vget_low_f32(a1), 1);
r2 = vmlaq_lane_f32( r2, my, vget_low_f32(a2), 1);
r0 = vmlaq_lane_f32( r0, mz, vget_high_f32(a0), 0);
r1 = vmlaq_lane_f32( r1, mz, vget_high_f32(a1), 0);
r2 = vmlaq_lane_f32( r2, mz, vget_high_f32(a2), 0);
return btMatrix3x3( r0, r1, r2 );
#else
return btMatrix3x3(
m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
#endif
}
SIMD_FORCE_INLINE btVector3
operator*(const btMatrix3x3& m, const btVector3& v)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
return v.dot3(m[0], m[1], m[2]);
#else
return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
#endif
}
SIMD_FORCE_INLINE btVector3
operator*(const btVector3& v, const btMatrix3x3& m)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_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);
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_add_ps(c0, c1);
c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask) );
return btVector3(_mm_add_ps(c0, c2));
#elif defined(BT_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 = vmulq_lane_f32(c0, vlo, 0);
c1 = vmulq_lane_f32(c1, vlo, 1);
c2 = vmulq_lane_f32(c2, vhi, 0);
c0 = vaddq_f32(c0, c1);
c0 = vaddq_f32(c0, c2);
return btVector3(c0);
#else
return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
#endif
}
SIMD_FORCE_INLINE btMatrix3x3
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
{
#if (defined (BT_USE_SSE_IN_API) && defined (BT_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 c0 = bt_splat_ps( m10, 0);
__m128 c1 = bt_splat_ps( m11, 0);
__m128 c2 = bt_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);
__m128 c0_1 = bt_splat_ps( m10, 1);
__m128 c1_1 = bt_splat_ps( m11, 1);
__m128 c2_1 = bt_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);
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 = _mm_mul_ps(m10, m2v);
m11 = _mm_mul_ps(m11, m2v);
m12 = _mm_mul_ps(m12, m2v);
c0 = _mm_add_ps(c0, m10);
c1 = _mm_add_ps(c1, m11);
c2 = _mm_add_ps(c2, m12);
return btMatrix3x3(c0, c1, c2);
#elif defined(BT_USE_NEON)
float32x4_t rv0, rv1, rv2;
float32x4_t v0, v1, v2;
float32x4_t mv0, mv1, mv2;
v0 = m1[0].mVec128;
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);
rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
return btMatrix3x3(rv0, rv1, rv2);
#else
return btMatrix3x3(
m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
#endif
}
/*
@@ -716,9 +1292,24 @@ 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 btMatrix3x3& m1, const btMatrix3x3& m2)
{
return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 c0, c1, c2;
c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
c0 = _mm_and_ps(c0, c1);
c0 = _mm_and_ps(c0, c2);
return (0x7 == _mm_movemask_ps((__m128)c0));
#else
return
( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
#endif
}
///for serialization

108
src/LinearMath/btQuadWord.h
View File

@@ -20,6 +20,9 @@ subject to the following restrictions:
#include "btMinMax.h"
#if defined (__CELLOS_LV2) && defined (__SPU__)
#include <altivec.h>
#endif
@@ -47,11 +50,53 @@ public:
}
protected:
#else //__CELLOS_LV2__ __SPU__
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
union {
btSimdFloat4 mVec128;
btScalar m_floats[4];
};
public:
SIMD_FORCE_INLINE btSimdFloat4 get128() const
{
return mVec128;
}
SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
{
mVec128 = v128;
}
#else
btScalar m_floats[4];
#endif // BT_USE_SSE
#endif //__CELLOS_LV2__ __SPU__
public:
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec)
{
mVec128 = vec;
}
// Copy constructor
SIMD_FORCE_INLINE btQuadWord(const btQuadWord& rhs)
{
mVec128 = rhs.mVec128;
}
// Assignment Operator
SIMD_FORCE_INLINE btQuadWord&
operator=(const btQuadWord& v)
{
mVec128 = v.mVec128;
return *this;
}
#endif
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
@@ -60,13 +105,13 @@ protected:
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar x) { m_floats[0] = x;};
SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
/**@brief Set the y value */
SIMD_FORCE_INLINE void setY(btScalar y) { m_floats[1] = y;};
SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;};
/**@brief Set the z value */
SIMD_FORCE_INLINE void setZ(btScalar z) { m_floats[2] = z;};
SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar w) { m_floats[3] = w;};
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
@@ -84,7 +129,14 @@ protected:
SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const
{
return ((m_floats[3]==other.m_floats[3]) && (m_floats[2]==other.m_floats[2]) && (m_floats[1]==other.m_floats[1]) && (m_floats[0]==other.m_floats[0]));
#ifdef BT_USE_SSE
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3]==other.m_floats[3]) &&
(m_floats[2]==other.m_floats[2]) &&
(m_floats[1]==other.m_floats[1]) &&
(m_floats[0]==other.m_floats[0]));
#endif
}
SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const
@@ -97,11 +149,11 @@ protected:
* @param y Value of y
* @param z Value of z
*/
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z)
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3] = 0.f;
}
@@ -118,12 +170,12 @@ protected:
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE void setValue(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
{
m_floats[0]=x;
m_floats[1]=y;
m_floats[2]=z;
m_floats[3]=w;
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3]=_w;
}
/**@brief No initialization constructor */
SIMD_FORCE_INLINE btQuadWord()
@@ -136,9 +188,9 @@ protected:
* @param y Value of y
* @param z Value of z
*/
SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z)
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)
{
m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = 0.0f;
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
}
/**@brief Initializing constructor
@@ -147,9 +199,9 @@ protected:
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w)
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
{
m_floats[0] = x, m_floats[1] = y, m_floats[2] = z, m_floats[3] = w;
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
}
/**@brief Set each element to the max of the current values and the values of another btQuadWord
@@ -157,21 +209,33 @@ protected:
*/
SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
{
btSetMax(m_floats[0], other.m_floats[0]);
#ifdef BT_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.m_floats[3]);
}
#endif
}
/**@brief Set each element to the min of the current values and the values of another btQuadWord
* @param other The other btQuadWord to compare with
*/
SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
{
btSetMin(m_floats[0], other.m_floats[0]);
#ifdef BT_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.m_floats[3]);
}
#endif
}

492
src/LinearMath/btQuaternion.h
View File

@@ -21,24 +21,65 @@ subject to the following restrictions:
#include "btVector3.h"
#include "btQuadWord.h"
#ifdef BT_USE_SSE
const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
#endif
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
#endif
/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
class btQuaternion : public btQuadWord {
public:
/**@brief No initialization constructor */
btQuaternion() {}
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE btQuaternion(const btSimdFloat4 vec)
{
mVec128 = vec;
}
// Copy constructor
SIMD_FORCE_INLINE btQuaternion(const btQuaternion& rhs)
{
mVec128 = rhs.mVec128;
}
// Assignment Operator
SIMD_FORCE_INLINE btQuaternion&
operator=(const btQuaternion& v)
{
mVec128 = v.mVec128;
return *this;
}
#endif
// template <typename btScalar>
// explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
/**@brief Constructor from scalars */
btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
: btQuadWord(x, y, z, w)
btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
: btQuadWord(_x, _y, _z, _w)
{}
/**@brief Axis angle Constructor
* @param axis The axis which the rotation is around
* @param angle The magnitude of the rotation around the angle (Radians) */
btQuaternion(const btVector3& axis, const btScalar& angle)
btQuaternion(const btVector3& _axis, const btScalar& _angle)
{
setRotation(axis, angle);
setRotation(_axis, _angle);
}
/**@brief Constructor from Euler angles
* @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
@@ -55,13 +96,13 @@ public:
/**@brief Set the rotation using axis angle notation
* @param axis The axis around which to rotate
* @param angle The magnitude of the rotation in Radians */
void setRotation(const btVector3& axis, const btScalar& angle)
void setRotation(const btVector3& axis, const btScalar& _angle)
{
btScalar d = axis.length();
btAssert(d != btScalar(0.0));
btScalar s = btSin(angle * btScalar(0.5)) / d;
btScalar s = btSin(_angle * btScalar(0.5)) / d;
setValue(axis.x() * s, axis.y() * s, axis.z() * s,
btCos(angle * btScalar(0.5)));
btCos(_angle * btScalar(0.5)));
}
/**@brief Set the quaternion using Euler angles
* @param yaw Angle around Y
@@ -107,7 +148,16 @@ public:
* @param q The quaternion to add to this one */
SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)
{
m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_add_ps(mVec128, q.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vaddq_f32(mVec128, q.mVec128);
#else
m_floats[0] += q.x();
m_floats[1] += q.y();
m_floats[2] += q.z();
m_floats[3] += q.m_floats[3];
#endif
return *this;
}
@@ -115,15 +165,35 @@ public:
* @param q The quaternion to subtract from this one */
btQuaternion& operator-=(const btQuaternion& q)
{
m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
return *this;
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, q.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vsubq_f32(mVec128, q.mVec128);
#else
m_floats[0] -= q.x();
m_floats[1] -= q.y();
m_floats[2] -= q.z();
m_floats[3] -= q.m_floats[3];
#endif
return *this;
}
/**@brief Scale this quaternion
* @param s The scalar to scale by */
btQuaternion& operator*=(const btScalar& s)
{
m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0); // (S S S S)
mVec128 = _mm_mul_ps(mVec128, vs);
#elif defined(BT_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s);
#else
m_floats[0] *= s;
m_floats[1] *= s;
m_floats[2] *= s;
m_floats[3] *= s;
#endif
return *this;
}
@@ -132,17 +202,111 @@ public:
* Equivilant to this = this * q */
btQuaternion& operator*=(const btQuaternion& q)
{
setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ2 = q.get128();
__m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0));
__m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));
A1 = A1 * B1;
__m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1));
__m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
A2 = A2 * B2;
B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2));
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
B1 = B1 * B2; // A3 *= B3
mVec128 = bt_splat_ps(mVec128, 3); // A0
mVec128 = mVec128 * vQ2; // A0 * B0
A1 = A1 + A2; // AB12
mVec128 = mVec128 - B1; // AB03 = AB0 - AB3
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
mVec128 = mVec128+ A1; // AB03 + AB12
#elif defined(BT_USE_NEON)
float32x4_t vQ1 = mVec128;
float32x4_t vQ2 = q.get128();
float32x4_t A0, A1, B1, A2, B2, A3, B3;
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
{
float32x2x2_t tmp;
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
vQ1zx = tmp.val[0];
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
vQ2zx = tmp.val[0];
}
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
mVec128 = A0;
#else
setValue(
m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
#endif
return *this;
}
/**@brief Return the dot product between this quaternion and another
* @param q The other quaternion */
btScalar dot(const btQuaternion& q) const
{
return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vd;
vd = _mm_mul_ps(mVec128, q.mVec128);
__m128 t = _mm_movehl_ps(vd, vd);
vd = _mm_add_ps(vd, t);
t = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, t);
return _mm_cvtss_f32(vd);
#elif defined(BT_USE_NEON)
float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
x = vpadd_f32(x, x);
return vget_lane_f32(x, 0);
#else
return m_floats[0] * q.x() +
m_floats[1] * q.y() +
m_floats[2] * q.z() +
m_floats[3] * q.m_floats[3];
#endif
}
/**@brief Return the length squared of the quaternion */
@@ -161,7 +325,25 @@ public:
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
btQuaternion& normalize()
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vd;
vd = _mm_mul_ps(mVec128, mVec128);
__m128 t = _mm_movehl_ps(vd, vd);
vd = _mm_add_ps(vd, t);
t = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, t);
vd = _mm_sqrt_ss(vd);
vd = _mm_div_ss(vOnes, vd);
vd = bt_pshufd_ps(vd, 0); // splat
mVec128 = _mm_mul_ps(mVec128, vd);
return *this;
#else
return *this /= length();
#endif
}
/**@brief Return a scaled version of this quaternion
@@ -169,10 +351,18 @@ public:
SIMD_FORCE_INLINE btQuaternion
operator*(const btScalar& s) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
return btQuaternion(_mm_mul_ps(mVec128, vs));
#elif defined(BT_USE_NEON)
return btQuaternion(vmulq_n_f32(mVec128, s));
#else
return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
#endif
}
/**@brief Return an inversely scaled versionof this quaternion
* @param s The inverse scale factor */
btQuaternion operator/(const btScalar& s) const
@@ -223,7 +413,13 @@ public:
/**@brief Return the inverse of this quaternion */
btQuaternion inverse() const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_xor_ps(mVec128, vQInv));
#elif defined(BT_USE_NEON)
return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
#else
return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
#endif
}
/**@brief Return the sum of this quaternion and the other
@@ -231,8 +427,14 @@ public:
SIMD_FORCE_INLINE btQuaternion
operator+(const btQuaternion& q2) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_add_ps(mVec128, q2.mVec128));
#elif defined(BT_USE_NEON)
return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
#else
const btQuaternion& q1 = *this;
return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
#endif
}
/**@brief Return the difference between this quaternion and the other
@@ -240,16 +442,28 @@ public:
SIMD_FORCE_INLINE btQuaternion
operator-(const btQuaternion& q2) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128));
#elif defined(BT_USE_NEON)
return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
#else
const btQuaternion& q1 = *this;
return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
#endif
}
/**@brief Return the negative of this quaternion
* This simply negates each element */
SIMD_FORCE_INLINE btQuaternion operator-() const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask));
#elif defined(BT_USE_NEON)
return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
#else
const btQuaternion& q2 = *this;
return btQuaternion( - q2.x(), - q2.y(), - q2.z(), - q2.m_floats[3]);
#endif
}
/**@todo document this and it's use */
SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
@@ -323,29 +537,257 @@ public:
/**@brief Return the product of two quaternions */
SIMD_FORCE_INLINE btQuaternion
operator*(const btQuaternion& q1, const btQuaternion& q2) {
return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
operator*(const btQuaternion& q1, const btQuaternion& q2)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ1 = q1.get128();
__m128 vQ2 = q2.get128();
__m128 A0, A1, B1, A2, B2;
A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x // vtrn
B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X // vdup vext
A1 = A1 * B1;
A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z X Y // vext
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup
A2 = A2 * B2;
B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn
B1 = B1 * B2; // A3 *= B3
A0 = bt_splat_ps(vQ1, 3); // A0
A0 = A0 * vQ2; // A0 * B0
A1 = A1 + A2; // AB12
A0 = A0 - B1; // AB03 = AB0 - AB3
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
A0 = A0 + A1; // AB03 + AB12
return btQuaternion(A0);
#elif defined(BT_USE_NEON)
float32x4_t vQ1 = q1.get128();
float32x4_t vQ2 = q2.get128();
float32x4_t A0, A1, B1, A2, B2, A3, B3;
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
{
float32x2x2_t tmp;
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
vQ1zx = tmp.val[0];
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
vQ2zx = tmp.val[0];
}
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); // A0 * B0
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
return btQuaternion(A0);
#else
return btQuaternion(
q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
#endif
}
SIMD_FORCE_INLINE btQuaternion
operator*(const btQuaternion& q, const btVector3& w)
{
return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ1 = q.get128();
__m128 vQ2 = w.get128();
__m128 A1, B1, A2, B2, A3, B3;
A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0));
B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0));
A1 = A1 * B1;
A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
A2 = A2 * B2;
A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
A3 = A3 * B3; // A3 *= B3
A1 = A1 + A2; // AB12
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
A1 = A1 - A3; // AB123 = AB12 - AB3
return btQuaternion(A1);
#elif defined(BT_USE_NEON)
float32x4_t vQ1 = q.get128();
float32x4_t vQ2 = w.get128();
float32x4_t A1, B1, A2, B2, A3, B3;
float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
{
float32x2x2_t tmp;
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
vQ2zx = tmp.val[0];
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
vQ1zx = tmp.val[0];
}
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W W X
B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx); // X Y z x
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
return btQuaternion(A1);
#else
return btQuaternion(
q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
-q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
#endif
}
SIMD_FORCE_INLINE btQuaternion
operator*(const btVector3& w, const btQuaternion& q)
{
return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
__m128 vQ1 = w.get128();
__m128 vQ2 = q.get128();
__m128 A1, B1, A2, B2, A3, B3;
A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x
B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X
A1 = A1 * B1;
A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
A2 = A2 *B2;
A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
A3 = A3 * B3; // A3 *= B3
A1 = A1 + A2; // AB12
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
A1 = A1 - A3; // AB123 = AB12 - AB3
return btQuaternion(A1);
#elif defined(BT_USE_NEON)
float32x4_t vQ1 = w.get128();
float32x4_t vQ2 = q.get128();
float32x4_t A1, B1, A2, B2, A3, B3;
float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
{
float32x2x2_t tmp;
tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) ); // {z x}, {w y}
vQ1zx = tmp.val[0];
tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) ); // {z x}, {w y}
vQ2zx = tmp.val[0];
}
vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx); // X Y z x
B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W W X
A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
A3 = vcombine_f32(vQ1zx, vQ1yz); // Z X Y Z
B3 = vcombine_f32(vQ2yz, vQ2xz); // Y Z x z
A1 = vmulq_f32(A1, B1);
A2 = vmulq_f32(A2, B2);
A3 = vmulq_f32(A3, B3); // A3 *= B3
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
return btQuaternion(A1);
#else
return btQuaternion(
+w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
+w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
+w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
-w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
#endif
}
/**@brief Calculate the dot product between two quaternions */
@@ -393,7 +835,13 @@ quatRotate(const btQuaternion& rotation, const btVector3& v)
{
btQuaternion q = rotation * v;
q *= rotation.inverse();
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
#elif defined(BT_USE_NEON)
return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
#else
return btVector3(q.getX(),q.getY(),q.getZ());
#endif
}
SIMD_FORCE_INLINE btQuaternion

108
src/LinearMath/btScalar.h
View File

@@ -69,6 +69,15 @@ inline int btGetVersion()
#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
#define BT_USE_SSE
#ifdef BT_USE_SSE
//BT_USE_SSE_IN_API is disabled under Windows by default, because
//it makes it harder to integrate Bullet into your application under Windows
//(structured embedding Bullet structs/classes need to be 16-byte aligned)
//with relatively little performance gain
//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
//#define BT_USE_SSE_IN_API
#endif //BT_USE_SSE
#include <emmintrin.h>
#endif
@@ -143,11 +152,39 @@ inline int btGetVersion()
#else
//non-windows systems
#if (defined (__APPLE__) && defined (__i386__) && (!defined (BT_USE_DOUBLE_PRECISION)))
#define BT_USE_SSE
#include <emmintrin.h>
#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
#if defined (__i386__) || defined (__x86_64__)
#define BT_USE_SSE
//BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
//if apps run into issues, we will disable the next line
#define BT_USE_SSE_IN_API
#ifdef BT_USE_SSE
// include appropriate SSE level
#if defined (__SSE4_1__)
#include <smmintrin.h>
#elif defined (__SSSE3__)
#include <tmmintrin.h>
#elif defined (__SSE3__)
#include <pmmintrin.h>
#else
#include <emmintrin.h>
#endif
#endif //BT_USE_SSE
#elif defined( __arm__ )
#ifdef __clang__
#define BT_USE_NEON 1
#if defined BT_USE_NEON && defined (__clang__)
#if! defined( ARM_NEON_GCC_COMPATIBILITY )
// -DARM_NEON_GCC_COMPATIBILITY=1 changes neon vector types to raw vectors, syntactically similar to SSE and AltiVec
// instead of vectors wrapped up in structs. This code base assumes GCC style raw vectors are used.
#error The C preprocessor macro ARM_NEON_GCC_COMPATIBILITY must be defined. Pass -DARM_NEON_GCC_COMPATIBILITY=1 to the compiler.
#endif//!ARM_NEON_GCC_COMPATIBILITY
#include <arm_neon.h>
#endif//BT_USE_NEON
#endif //__clang__
#endif//__arm__
#define SIMD_FORCE_INLINE inline
#define SIMD_FORCE_INLINE inline __attribute__ ((always_inline))
///@todo: check out alignment methods for other platforms/compilers
#define ATTRIBUTE_ALIGNED16(a) a __attribute__ ((aligned (16)))
#define ATTRIBUTE_ALIGNED64(a) a __attribute__ ((aligned (64)))
@@ -210,6 +247,69 @@ typedef float btScalar;
#define BT_LARGE_FLOAT 1e18f
#endif
#ifdef BT_USE_SSE
typedef __m128 btSimdFloat4;
#endif//BT_USE_SSE
#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
#ifdef _WIN32
#ifndef BT_NAN
static int btNanMask = 0x7F800001;
#define BT_NAN (*(float*)&btNanMask)
#endif
#ifndef BT_INFINITY
static int btInfinityMask = 0x7F800000;
#define BT_INFINITY (*(float*)&btInfinityMask)
#endif
inline __m128 operator + (const __m128 A, const __m128 B)
{
return _mm_add_ps(A, B);
}
inline __m128 operator - (const __m128 A, const __m128 B)
{
return _mm_sub_ps(A, B);
}
inline __m128 operator * (const __m128 A, const __m128 B)
{
return _mm_mul_ps(A, B);
}
#define btCastfTo128i(a) (_mm_castps_si128(a))
#define btCastfTo128d(a) (_mm_castps_pd(a))
#define btCastiTo128f(a) (_mm_castsi128_ps(a))
#define btCastdTo128f(a) (_mm_castpd_ps(a))
#define btCastdTo128i(a) (_mm_castpd_si128(a))
#define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
#else//_WIN32
#define btCastfTo128i(a) ((__m128i)(a))
#define btCastfTo128d(a) ((__m128d)(a))
#define btCastiTo128f(a) ((__m128) (a))
#define btCastdTo128f(a) ((__m128) (a))
#define btCastdTo128i(a) ((__m128i)(a))
#define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
#define BT_INFINITY INFINITY
#define BT_NAN NAN
#endif//_WIN32
#endif //BT_USE_SSE_IN_API
#ifdef BT_USE_NEON
#include <arm_neon.h>
typedef float32x4_t btSimdFloat4;
#define BT_INFINITY INFINITY
#define BT_NAN NAN
#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
#endif
#define BT_DECLARE_ALIGNED_ALLOCATOR() \

6
src/LinearMath/btTransform.h
View File

@@ -31,7 +31,7 @@ subject to the following restrictions:
/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */
class btTransform {
ATTRIBUTE_ALIGNED16(class) btTransform {
///Storage for the rotation
btMatrix3x3 m_basis;
@@ -93,9 +93,7 @@ public:
/**@brief Return the transform of the vector */
SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
{
return btVector3(m_basis[0].dot(x) + m_origin.x(),
m_basis[1].dot(x) + m_origin.y(),
m_basis[2].dot(x) + m_origin.z());
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
}
/**@brief Return the transform of the vector */

1631
src/LinearMath/btVector3.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

793
src/LinearMath/btVector3.h
View File

File diff suppressed because it is too large Load Diff