asgardius/virtualx-engine
1
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Update Bullet to the latest commit 126b676

Browse source
This commit is contained in:
Oussama 2019-01-03 14:26:51 +01:00 committed by Rémi Verschelde
parent a6722cf362
commit 22b7c9dfa8
612 changed files with 114715 additions and 103413 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
thirdparty/README.md vendored
View file

@ -19,7 +19,7 @@ comments.
## bullet
- Upstream: https://github.com/bulletphysics/bullet3
- Version: git (12409f1118a7c7a266f9071350c70789dfe73bb9, Commits on Sep 6, 2018 )
- Version: git (126b676, 2018-12-31)
- License: zlib
Files extracted from upstream source:

2
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h vendored
View file

@ -19,14 +19,12 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
struct b3BroadphaseProxy;
struct b3BroadphaseAabbCallback
{
virtual ~b3BroadphaseAabbCallback() {}
virtual bool process(const b3BroadphaseProxy* proxy) = 0;
};
struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
{
///added some cached data to accelerate ray-AABB tests

63
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp vendored
View file

@ -62,7 +62,9 @@ static void b3GetMaxDepth(const b3DbvtNode* node,int depth,int& maxdepth)
{
b3GetMaxDepth(node->childs[0], depth + 1, maxdepth);
b3GetMaxDepth(node->childs[1], depth + 1, maxdepth);
} else maxdepth=b3Max(maxdepth,depth);
}
else
maxdepth = b3Max(maxdepth, depth);
}
//
@ -93,9 +95,14 @@ static B3_DBVT_INLINE b3DbvtNode* b3CreateNode( b3DynamicBvh* pdbvt,
{
b3DbvtNode* node;
if (pdbvt->m_free)
{ node=pdbvt->m_free;pdbvt->m_free=0; }
{
node = pdbvt->m_free;
pdbvt->m_free = 0;
}
else
{ node=new(b3AlignedAlloc(sizeof(b3DbvtNode),16)) b3DbvtNode(); }
{
node = new (b3AlignedAlloc(sizeof(b3DbvtNode), 16)) b3DbvtNode();
}
node->parent = parent;
node->data = data;
node->childs[1] = 0;
@ -139,7 +146,8 @@ static void b3InsertLeaf( b3DynamicBvh* pdbvt,
{
if (!root->isleaf())
{
do {
do
{
root = root->childs[b3Select(leaf->volume,
root->childs[0]->volume,
root->childs[1]->volume)];
@ -150,9 +158,12 @@ static void b3InsertLeaf( b3DynamicBvh* pdbvt,
if (prev)
{
prev->childs[b3IndexOf(root)] = node;
node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node;
do {
node->childs[0] = root;
root->parent = node;
node->childs[1] = leaf;
leaf->parent = node;
do
{
if (!prev->volume.Contain(node->volume))
b3Merge(prev->childs[0]->volume, prev->childs[1]->volume, prev->volume);
else
@ -162,8 +173,10 @@ static void b3InsertLeaf( b3DynamicBvh* pdbvt,
}
else
{
node->childs[0] = root;root->parent=node;
node->childs[1] = leaf;leaf->parent=node;
node->childs[0] = root;
root->parent = node;
node->childs[1] = leaf;
leaf->parent = node;
pdbvt->m_root = node;
}
}
@ -195,7 +208,9 @@ static b3DbvtNode* b3RemoveLeaf( b3DynamicBvh* pdbvt,
if (b3NotEqual(pb, prev->volume))
{
prev = prev->parent;
} else break;
}
else
break;
}
return (prev ? prev : pdbvt->m_root);
}
@ -407,7 +422,10 @@ static B3_DBVT_INLINE b3DbvtNode* b3Sort(b3DbvtNode* n,b3DbvtNode*& r)
b3DbvtNode* s = p->childs[j];
b3DbvtNode* q = p->parent;
b3Assert(n == p->childs[i]);
if(q) q->childs[b3IndexOf(p)]=n; else r=n;
if (q)
q->childs[b3IndexOf(p)] = n;
else
r = n;
s->parent = n;
p->parent = n;
n->parent = q;
@ -461,7 +479,6 @@ void b3DynamicBvh::clear()
m_lkhd = -1;
m_stkStack.clear();
m_opath = 0;
}
//
@ -495,7 +512,8 @@ void b3DynamicBvh::optimizeIncremental(int passes)
if (passes < 0) passes = m_leaves;
if (m_root && (passes > 0))
{
do {
do
{
b3DbvtNode* node = m_root;
unsigned bit = 0;
while (node->isinternal())
@ -530,7 +548,9 @@ void b3DynamicBvh::update(b3DbvtNode* leaf,int lookahead)
{
root = root->parent;
}
} else root=m_root;
}
else
root = m_root;
}
b3InsertLeaf(this, root, leaf);
}
@ -547,7 +567,9 @@ void b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume)
{
root = root->parent;
}
} else root=m_root;
}
else
root = m_root;
}
leaf->volume = volume;
b3InsertLeaf(this, root, leaf);
@ -623,7 +645,8 @@ void b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const
b3AlignedObjectArray<sStkCLN> stack;
stack.reserve(m_leaves);
stack.push_back(sStkCLN(m_root, 0));
do {
do
{
const int i = stack.size() - 1;
const sStkCLN e = stack[i];
b3DbvtNode* n = b3CreateNode(&dest, e.parent, e.node->volume, e.node->data);
@ -682,7 +705,6 @@ void b3DynamicBvh::extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<c
#include <stdio.h>
#include <stdlib.h>
/*
q6600,2.4ghz
@ -729,7 +751,12 @@ struct b3DbvtBenchmark
{
++m_pcount;
if (m_checksort)
{ if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
{
if (depth >= m_depth)
m_depth = depth;
else
printf("wrong depth: %f (should be >= %f)\r\n", depth, m_depth);
}
}
int m_pcount;
b3Scalar m_depth;

193
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h vendored
View file

@ -26,7 +26,6 @@ subject to the following restrictions:
// Compile time configuration
//
// Implementation profiles
#define B3_DBVT_IMPL_GENERIC 0 // Generic implementation
#define B3_DBVT_IMPL_SSE 1 // SSE
@ -82,9 +81,12 @@ subject to the following restrictions:
#define B3_DBVT_VIRTUAL_DTOR(a)
#define B3_DBVT_PREFIX template <typename T>
#define B3_DBVT_IPOLICY T& policy
#define B3_DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker;
#define B3_DBVT_CHECKTYPE \
static const ICollide& typechecker = *(T*)1; \
(void)typechecker;
#else
#define B3_DBVT_VIRTUAL_DTOR(a) virtual ~a() {}
#define B3_DBVT_VIRTUAL_DTOR(a) \
virtual ~a() {}
#define B3_DBVT_VIRTUAL virtual
#define B3_DBVT_PREFIX
#define B3_DBVT_IPOLICY ICollide& policy
@ -166,6 +168,7 @@ struct b3DbvtAabbMm
private:
B3_DBVT_INLINE void AddSpan(const b3Vector3& d, b3Scalar& smi, b3Scalar& smx) const;
private:
b3Vector3 mi, mx;
};
@ -180,8 +183,7 @@ struct b3DbvtNode
b3DbvtNode* parent;
B3_DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
B3_DBVT_INLINE bool isinternal() const { return (!isleaf()); }
union
{
union {
b3DbvtNode* childs[2];
void* data;
int dataAsInt;
@ -249,7 +251,8 @@ struct b3DynamicBvh
};
// Constants
enum {
enum
{
B3_SIMPLE_STACKSIZE = 64,
B3_DOUBLE_STACKSIZE = B3_SIMPLE_STACKSIZE * 2
};
@ -261,11 +264,9 @@ struct b3DynamicBvh
int m_leaves;
unsigned m_opath;
b3AlignedObjectArray<sStkNN> m_stkStack;
mutable b3AlignedObjectArray<const b3DbvtNode*> m_rayTestStack;
// Methods
b3DynamicBvh();
~b3DynamicBvh();
@ -289,7 +290,9 @@ struct b3DynamicBvh
#if B3_DBVT_ENABLE_BENCHMARK
static void benchmark();
#else
static void benchmark(){}
static void benchmark()
{
}
#endif
// B3_DBVT_IPOLICY must support ICollide policy/interface
B3_DBVT_PREFIX
@ -369,7 +372,10 @@ struct b3DynamicBvh
while (l < h)
{
m = (l + h) >> 1;
if(a[i[m]].value>=v) l=m+1; else h=m;
if (a[i[m]].value >= v)
l = m + 1;
else
h = m;
}
return (h);
}
@ -379,9 +385,16 @@ struct b3DynamicBvh
{
int i;
if (ifree.size() > 0)
{ i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
{
i = ifree[ifree.size() - 1];
ifree.pop_back();
stock[i] = value;
}
else
{ i=stock.size();stock.push_back(value); }
{
i = stock.size();
stock.push_back(value);
}
return (i);
}
//
@ -397,7 +410,8 @@ private:
inline b3DbvtAabbMm b3DbvtAabbMm::FromCE(const b3Vector3& c, const b3Vector3& e)
{
b3DbvtAabbMm box;
box.mi=c-e;box.mx=c+e;
box.mi = c - e;
box.mx = c + e;
return (box);
}
@ -411,7 +425,8 @@ inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r)
inline b3DbvtAabbMm b3DbvtAabbMm::FromMM(const b3Vector3& mi, const b3Vector3& mx)
{
b3DbvtAabbMm box;
box.mi=mi;box.mx=mx;
box.mi = mi;
box.mx = mx;
return (box);
}
@ -444,15 +459,25 @@ inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3** ppts,int n)
//
B3_DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e)
{
mi-=e;mx+=e;
mi -= e;
mx += e;
}
//
B3_DBVT_INLINE void b3DbvtAabbMm::SignedExpand(const b3Vector3& e)
{
if(e.x>0) mx.setX(mx.x+e[0]); else mi.setX(mi.x+e[0]);
if(e.y>0) mx.setY(mx.y+e[1]); else mi.setY(mi.y+e[1]);
if(e.z>0) mx.setZ(mx.z+e[2]); else mi.setZ(mi.z+e[2]);
if (e.x > 0)
mx.setX(mx.x + e[0]);
else
mi.setX(mi.x + e[0]);
if (e.y > 0)
mx.setY(mx.y + e[1]);
else
mi.setY(mi.y + e[1]);
if (e.z > 0)
mx.setZ(mx.z + e[2]);
else
mi.setZ(mi.z + e[2]);
}
//
@ -472,22 +497,38 @@ B3_DBVT_INLINE int b3DbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s)
b3Vector3 pi, px;
switch (s)
{
case (0+0+0): px=b3MakeVector3(mi.x,mi.y,mi.z);
pi=b3MakeVector3(mx.x,mx.y,mx.z);break;
case (1+0+0): px=b3MakeVector3(mx.x,mi.y,mi.z);
pi=b3MakeVector3(mi.x,mx.y,mx.z);break;
case (0+2+0): px=b3MakeVector3(mi.x,mx.y,mi.z);
pi=b3MakeVector3(mx.x,mi.y,mx.z);break;
case (1+2+0): px=b3MakeVector3(mx.x,mx.y,mi.z);
pi=b3MakeVector3(mi.x,mi.y,mx.z);break;
case (0+0+4): px=b3MakeVector3(mi.x,mi.y,mx.z);
pi=b3MakeVector3(mx.x,mx.y,mi.z);break;
case (1+0+4): px=b3MakeVector3(mx.x,mi.y,mx.z);
pi=b3MakeVector3(mi.x,mx.y,mi.z);break;
case (0+2+4): px=b3MakeVector3(mi.x,mx.y,mx.z);
pi=b3MakeVector3(mx.x,mi.y,mi.z);break;
case (1+2+4): px=b3MakeVector3(mx.x,mx.y,mx.z);
pi=b3MakeVector3(mi.x,mi.y,mi.z);break;
case (0 + 0 + 0):
px = b3MakeVector3(mi.x, mi.y, mi.z);
pi = b3MakeVector3(mx.x, mx.y, mx.z);
break;
case (1 + 0 + 0):
px = b3MakeVector3(mx.x, mi.y, mi.z);
pi = b3MakeVector3(mi.x, mx.y, mx.z);
break;
case (0 + 2 + 0):
px = b3MakeVector3(mi.x, mx.y, mi.z);
pi = b3MakeVector3(mx.x, mi.y, mx.z);
break;
case (1 + 2 + 0):
px = b3MakeVector3(mx.x, mx.y, mi.z);
pi = b3MakeVector3(mi.x, mi.y, mx.z);
break;
case (0 + 0 + 4):
px = b3MakeVector3(mi.x, mi.y, mx.z);
pi = b3MakeVector3(mx.x, mx.y, mi.z);
break;
case (1 + 0 + 4):
px = b3MakeVector3(mx.x, mi.y, mx.z);
pi = b3MakeVector3(mi.x, mx.y, mi.z);
break;
case (0 + 2 + 4):
px = b3MakeVector3(mi.x, mx.y, mx.z);
pi = b3MakeVector3(mx.x, mi.y, mi.z);
break;
case (1 + 2 + 4):
px = b3MakeVector3(mx.x, mx.y, mx.z);
pi = b3MakeVector3(mi.x, mi.y, mi.z);
break;
}
if ((b3Dot(n, px) + o) < 0) return (-1);
if ((b3Dot(n, pi) + o) >= 0) return (+1);
@ -510,9 +551,15 @@ B3_DBVT_INLINE void b3DbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Sc
for (int i = 0; i < 3; ++i)
{
if (d[i] < 0)
{ smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
{
smi += mx[i] * d[i];
smx += mi[i] * d[i];
}
else
{ smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
{
smi += mi[i] * d[i];
smx += mx[i] * d[i];
}
}
}
@ -539,8 +586,6 @@ B3_DBVT_INLINE bool b3Intersect( const b3DbvtAabbMm& a,
#endif
}
//
B3_DBVT_INLINE bool b3Intersect(const b3DbvtAabbMm& a,
const b3Vector3& b)
@ -553,13 +598,8 @@ B3_DBVT_INLINE bool b3Intersect( const b3DbvtAabbMm& a,
(b.z <= a.mx.z));
}
//////////////////////////////////////
//
B3_DBVT_INLINE b3Scalar b3Proximity(const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b)
@ -568,8 +608,6 @@ B3_DBVT_INLINE b3Scalar b3Proximity( const b3DbvtAabbMm& a,
return (b3Fabs(d.x) + b3Fabs(d.y) + b3Fabs(d.z));
}
//
B3_DBVT_INLINE int b3Select(const b3DbvtAabbMm& o,
const b3DbvtAabbMm& a,
@ -666,8 +704,14 @@ B3_DBVT_INLINE void b3Merge( const b3DbvtAabbMm& a,
#else
for (int i = 0; i < 3; ++i)
{
if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
if (a.mi[i] < b.mi[i])
r.mi[i] = a.mi[i];
else
r.mi[i] = b.mi[i];
if (a.mx[i] > b.mx[i])
r.mx[i] = a.mx[i];
else
r.mx[i] = b.mx[i];
}
#endif
}
@ -733,7 +777,8 @@ inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0,
b3AlignedObjectArray<sStkNN> stkStack;
stkStack.resize(B3_DOUBLE_STACKSIZE);
stkStack[0] = sStkNN(root0, root1);
do {
do
{
sStkNN p = stkStack[--depth];
if (depth > treshold)
{
@ -783,8 +828,6 @@ inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0,
}
}
B3_DBVT_PREFIX
inline void b3DynamicBvh::collideTTpersistentStack(const b3DbvtNode* root0,
const b3DbvtNode* root1,
@ -798,7 +841,8 @@ inline void b3DynamicBvh::collideTTpersistentStack( const b3DbvtNode* root0,
m_stkStack.resize(B3_DOUBLE_STACKSIZE);
m_stkStack[0] = sStkNN(root0, root1);
do {
do
{
sStkNN p = m_stkStack[--depth];
if (depth > treshold)
{
@ -926,12 +970,14 @@ inline void b3DynamicBvh::collideTV( const b3DbvtNode* root,
B3_DBVT_CHECKTYPE
if (root)
{
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) volume(vol);
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
volume(vol);
b3AlignedObjectArray<const b3DbvtNode*> stack;
stack.resize(0);
stack.reserve(B3_SIMPLE_STACKSIZE);
stack.push_back(root);
do {
do
{
const b3DbvtNode* n = stack[stack.size() - 1];
stack.pop_back();
if (b3Intersect(n->volume, volume))
@ -1033,7 +1079,8 @@ inline void b3DynamicBvh::rayTest( const b3DbvtNode* root,
stack.resize(B3_DOUBLE_STACKSIZE);
stack[0] = root;
b3Vector3 bounds[2];
do {
do
{
const b3DbvtNode* node = stack[--depth];
bounds[0] = node->volume.Mins();
@ -1066,7 +1113,6 @@ inline void b3DynamicBvh::rayTest( const b3DbvtNode* root,
}
}
} while (depth);
}
}
@ -1093,7 +1139,8 @@ inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root,
}
stack.reserve(B3_SIMPLE_STACKSIZE);
stack.push_back(sStkNP(root, 0));
do {
do
{
sStkNP se = stack[stack.size() - 1];
bool out = false;
stack.pop_back();
@ -1104,8 +1151,12 @@ inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root,
const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
switch (side)
{
case -1: out=true;break;
case +1: se.mask|=j;break;
case -1:
out = true;
break;
case +1:
se.mask |= j;
break;
}
}
}
@ -1157,10 +1208,12 @@ inline void b3DynamicBvh::collideOCL( const b3DbvtNode* root,
stack.reserve(B3_SIMPLE_STACKSIZE);
ifree.reserve(B3_SIMPLE_STACKSIZE);
stack.push_back(allocate(ifree, stock, sStkNPS(root, 0, root->volume.ProjectMinimum(sortaxis, srtsgns))));
do {
do
{
const int id = stack[stack.size() - 1];
sStkNPS se = stock[id];
stack.pop_back();ifree.push_back(id);
stack.pop_back();
ifree.push_back(id);
if (se.mask != inside)
{
bool out = false;
@ -1171,8 +1224,12 @@ inline void b3DynamicBvh::collideOCL( const b3DbvtNode* root,
const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
switch (side)
{
case -1: out=true;break;
case +1: se.mask|=j;break;
case -1:
out = true;
break;
case +1:
se.mask |= j;
break;
}
}
}
@ -1234,15 +1291,21 @@ inline void b3DynamicBvh::collideTU( const b3DbvtNode* root,
b3AlignedObjectArray<const b3DbvtNode*> stack;
stack.reserve(B3_SIMPLE_STACKSIZE);
stack.push_back(root);
do {
do
{
const b3DbvtNode* n = stack[stack.size() - 1];
stack.pop_back();
if (policy.Descent(n))
{
if (n->isinternal())
{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
{
stack.push_back(n->childs[0]);
stack.push_back(n->childs[1]);
}
else
{ policy.Process(n); }
{
policy.Process(n);
}
}
} while (stack.size() > 0);
}

86
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp vendored
View file

@ -29,8 +29,7 @@ subject to the following restrictions:
#if B3_DBVT_BP_PROFILE
struct b3ProfileScope
{
__forceinline b3ProfileScope(b3Clock& clock,unsigned long& value) :
m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
__forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
{
}
__forceinline ~b3ProfileScope()
@ -64,7 +63,10 @@ static inline void b3ListAppend(T* item,T*& list)
template <typename T>
static inline void b3ListRemove(T* item, T*& list)
{
if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1];
if (item->links[0])
item->links[0]->links[1] = item->links[1];
else
list = item->links[1];
if (item->links[1]) item->links[1]->links[0] = item->links[0];
}
@ -73,7 +75,11 @@ template <typename T>
static inline int b3ListCount(T* root)
{
int n = 0;
while(root) { ++n;root=root->links[1]; }
while (root)
{
++n;
root = root->links[1];
}
return (n);
}
@ -81,7 +87,9 @@ static inline int b3ListCount(T* root)
template <typename T>
static inline void b3Clear(T& value)
{
static const struct ZeroDummy : T {} zerodummy;
static const struct ZeroDummy : T
{
} zerodummy;
value = zerodummy;
}
@ -219,7 +227,6 @@ void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabb
}
*/
struct BroadphaseRayTester : b3DynamicBvh::ICollide
{
b3BroadphaseRayCallback& m_rayCallback;
@ -257,10 +264,8 @@ void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& r
aabbMin,
aabbMax,
callback);
}
struct BroadphaseAabbTester : b3DynamicBvh::ICollide
{
b3BroadphaseAabbCallback& m_aabbCallback;
@ -283,11 +288,8 @@ void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3&
//process all children, that overlap with the given AABB bounds
m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
}
//
void b3DynamicBvhBroadphase::setAabb(int objectId,
const b3Vector3& aabbMin,
@ -296,7 +298,8 @@ void b3DynamicBvhBroadphase::setAabb(int objectId,
{
b3DbvtProxy* proxy = &m_proxies[objectId];
// b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
#if B3_DBVT_BP_PREVENTFALSEUPDATE
if (b3NotEqual(aabb, proxy->leaf->volume))
#endif
@ -356,7 +359,6 @@ void b3DynamicBvhBroadphase::setAabb(int objectId,
}
}
//
void b3DynamicBvhBroadphase::setAabbForceUpdate(b3BroadphaseProxy* absproxy,
const b3Vector3& aabbMin,
@ -364,7 +366,8 @@ void b3DynamicBvhBroadphase::setAabbForceUpdate( b3BroadphaseProxy* abspr
b3Dispatcher* /*dispatcher*/)
{
b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
bool docollide = false;
if (proxy->stage == STAGECOUNT)
{ /* fixed -> dynamic set */
@ -422,15 +425,12 @@ void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispa
#endif
performDeferredRemoval(dispatcher);
}
void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
{
if (m_paircache->hasDeferredRemoval())
{
b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
@ -438,16 +438,12 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
int invalidPair = 0;
int i;
b3BroadphasePair previousPair = b3MakeBroadphasePair(-1, -1);
for (i = 0; i < overlappingPairArray.size(); i++)
{
b3BroadphasePair& pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
@ -466,11 +462,13 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
if (hasOverlap)
{
needsRemoval = false;
} else
}
else
{
needsRemoval = true;
}
} else
}
else
{
//remove duplicate
needsRemoval = true;
@ -485,7 +483,6 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
pair.y = -1;
invalidPair++;
}
}
//perform a sort, to sort 'invalid' pairs to the end
@ -512,8 +509,6 @@ void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
}
*/
b3SPC(m_profiling.m_total);
/* optimize */
m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
@ -529,7 +524,8 @@ void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
if (current)
{
b3DbvtTreeCollider collider(this);
do {
do
{
b3DbvtProxy* next = current->links[1];
b3ListRemove(current, m_stageRoots[current->stage]);
b3ListAppend(current, m_stageRoots[STAGECOUNT]);
@ -540,7 +536,8 @@ void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
b3DynamicBvh::collideTV(m_sets[1].m_root, current->aabb, collider);
#endif
m_sets[0].remove(current->leaf);
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) curAabb=b3DbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
curAabb = b3DbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
current->leaf = m_sets[1].insert(curAabb, current);
current->stage = STAGECOUNT;
current = next;
@ -569,7 +566,6 @@ void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
b3BroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
if (pairs.size() > 0)
{
int ni = b3Min(pairs.size(), b3Max<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
for (int i = 0; i < ni; ++i)
{
@ -583,19 +579,27 @@ void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
b3Swap(pa, pb);
#endif
m_paircache->removeOverlappingPair(pa->getUid(), pb->getUid(), dispatcher);
--ni;--i;
--ni;
--i;
}
}
if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0;
if (pairs.size() > 0)
m_cid = (m_cid + ni) % pairs.size();
else
m_cid = 0;
}
}
++m_pid;
m_newpairs = 1;
m_needcleanup = false;
if (m_updates_call > 0)
{ m_updates_ratio=m_updates_done/(b3Scalar)m_updates_call; }
{
m_updates_ratio = m_updates_done / (b3Scalar)m_updates_call;
}
else
{ m_updates_ratio=0; }
{
m_updates_ratio = 0;
}
m_updates_done /= 2;
m_updates_call /= 2;
}
@ -622,15 +626,17 @@ const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
//
void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const
{
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds;
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
bounds;
if (!m_sets[0].empty())
if(!m_sets[1].empty()) b3Merge( m_sets[0].m_root->volume,
if (!m_sets[1].empty())
b3Merge(m_sets[0].m_root->volume,
m_sets[1].m_root->volume, bounds);
else
bounds = m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else if (!m_sets[1].empty())
bounds = m_sets[1].m_root->volume;
else
bounds = b3DbvtVolume::FromCR(b3MakeVector3(0, 0, 0), 0);
aabbMin = bounds.Mins();
@ -639,7 +645,6 @@ void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector
void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
{
int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
if (!totalObjects)
{
@ -670,7 +675,8 @@ void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
//
void b3DynamicBvhBroadphase::printStats()
{}
{
}
//
#if B3_DBVT_BP_ENABLE_BENCHMARK
@ -790,7 +796,6 @@ void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
objects.resize(0);
b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock);
}
}
#else
/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
@ -801,4 +806,3 @@ void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
#if B3_DBVT_BP_PROFILE
#undef b3SPC
#endif

23
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h vendored
View file

@ -39,12 +39,9 @@ subject to the following restrictions:
#endif
B3_ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy
B3_ATTRIBUTE_ALIGNED16(struct)
b3BroadphaseProxy
{
B3_DECLARE_ALIGNED_ALLOCATOR();
///optional filtering to cull potential collisions
@ -88,10 +85,6 @@ B3_DECLARE_ALIGNED_ALLOCATOR();
}
};
//
// b3DbvtProxy
//
@ -105,8 +98,7 @@ struct b3DbvtProxy : b3BroadphaseProxy
/* ctor */
explicit b3DbvtProxy() {}
b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr, int collisionFilterGroup, int collisionFilterMask) :
b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
{
links[0] = links[1] = 0;
}
@ -120,7 +112,8 @@ typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
struct b3DynamicBvhBroadphase
{
/* Config */
enum {
enum
{
DYNAMIC_SET = 0, /* Dynamic set index */
FIXED_SET = 1, /* Fixed set index */
STAGECOUNT = 2 /* Number of stages */
@ -148,7 +141,8 @@ struct b3DynamicBvhBroadphase
bool m_needcleanup; // Need to run cleanup?
#if B3_DBVT_BP_PROFILE
b3Clock m_clock;
struct {
struct
{
unsigned long m_total;
unsigned long m_ddcollide;
unsigned long m_fdcollide;
@ -177,7 +171,6 @@ struct b3DynamicBvhBroadphase
virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
virtual void printStats();
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(b3Dispatcher* dispatcher);
@ -199,8 +192,6 @@ struct b3DynamicBvhBroadphase
void setAabbForceUpdate(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
//static void benchmark(b3BroadphaseInterface*);
};
#endif

2
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h vendored
View file

@ -52,7 +52,6 @@ inline b3Int4 b3MakeBroadphasePair(int xx,int yy)
class b3BroadphasePairSortPredicate
{
public:
bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
{
const int uidA0 = a.x;
@ -69,4 +68,3 @@ B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePai
}
#endif //B3_OVERLAPPING_PAIR_H

91
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp vendored
View file

@ -13,8 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3OverlappingPairCache.h"
//#include "b3Dispatcher.h"
@ -28,11 +26,7 @@ int b3g_removePairs =0;
int b3g_addedPairs = 0;
int b3g_findPairs = 0;
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
m_overlapFilterCallback(0)
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
//, m_blockedForChanges(false)
{
int initialAllocatedSize = 2;
@ -40,15 +34,10 @@ b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
growTables();
}
b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
{
}
void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
{
/* if (pair.m_algorithm)
@ -60,15 +49,10 @@ void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b
}
}
*/
}
void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
{
class CleanPairCallback : public b3OverlapCallback
{
int m_cleanProxy;
@ -91,21 +75,15 @@ void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* d
}
return false;
}
};
CleanPairCallback cleanPairs(proxy, this, dispatcher);
processAllOverlappingPairs(&cleanPairs, dispatcher);
}
void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
{
class RemovePairCallback : public b3OverlapCallback
{
int m_obsoleteProxy;
@ -120,19 +98,13 @@ void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int pro
return ((pair.x == m_obsoleteProxy) ||
(pair.y == m_obsoleteProxy));
}
};
RemovePairCallback removeCallback(proxy);
processAllOverlappingPairs(&removeCallback, dispatcher);
}
b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
{
b3g_findPairs++;
@ -171,7 +143,6 @@ b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
void b3HashedOverlappingPairCache::growTables()
{
int newCapacity = m_overlappingPairArray.capacity();
if (m_hashTable.size() < newCapacity)
@ -182,7 +153,6 @@ void b3HashedOverlappingPairCache::growTables()
m_hashTable.resize(newCapacity);
m_next.resize(newCapacity);
int i;
for (i = 0; i < newCapacity; ++i)
@ -196,7 +166,6 @@ void b3HashedOverlappingPairCache::growTables()
for (i = 0; i < curHashtableSize; i++)
{
const b3BroadphasePair& pair = m_overlappingPairArray[i];
int proxyId1 = pair.x;
int proxyId2 = pair.y;
@ -206,8 +175,6 @@ void b3HashedOverlappingPairCache::growTables()
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
}
}
}
@ -223,7 +190,6 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1)); // New hash value with new mask
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair != NULL)
{
@ -262,15 +228,12 @@ b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int
//pair->m_algorithm = 0;
//pair->m_internalTmpValue = 0;
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
return pair;
}
void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
{
b3g_removePairs++;
@ -292,8 +255,6 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
cleanOverlappingPair(*pair, dispatcher);
int pairIndex = int(pair - &m_overlappingPairArray[0]);
b3Assert(pairIndex < m_overlappingPairArray.size());
@ -374,30 +335,25 @@ void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
{
int i;
// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
for (i = 0; i < m_overlappingPairArray.size();)
{
b3BroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->x, pair->y, dispatcher);
b3g_overlappingPairs--;
} else
}
else
{
i++;
}
}
}
void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
{
///need to keep hashmap in sync with pair address, so rebuild all
@ -424,18 +380,14 @@ void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher
{
addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
}
}
void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
{
if (!hasDeferredRemoval())
{
b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
if (findIndex < m_overlappingPairArray.size())
{
@ -455,13 +407,6 @@ void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1,
return 0;
}
b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
{
//don't add overlap with own
@ -473,14 +418,12 @@ b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,in
b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
*pair = b3MakeBroadphasePair(proxy0, proxy1);
b3g_overlappingPairs++;
b3g_addedPairs++;
// if (m_ghostPairCallback)
// m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
return pair;
}
///this findPair becomes really slow. Either sort the list to speedup the query, or
@ -504,25 +447,14 @@ b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,in
return 0;
}
//#include <stdio.h>
void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
{
int i;
for (i = 0; i < m_overlappingPairArray.size();)
{
b3BroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
@ -532,18 +464,15 @@ void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback*
m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
m_overlappingPairArray.pop_back();
b3g_overlappingPairs--;
} else
}
else
{
i++;
}
}
}
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache():
m_blockedForChanges(false),
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
m_hasDeferredRemoval(true),
m_overlapFilterCallback(0)
@ -570,10 +499,8 @@ void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b
*/
}
void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
{
class CleanPairCallback : public b3OverlapCallback
{
int m_cleanProxy;
@ -596,19 +523,15 @@ void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* d
}
return false;
}
};
CleanPairCallback cleanPairs(proxy, this, dispatcher);
processAllOverlappingPairs(&cleanPairs, dispatcher);
}
void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
{
class RemovePairCallback : public b3OverlapCallback
{
int m_obsoleteProxy;
@ -623,7 +546,6 @@ void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int pro
return ((pair.x == m_obsoleteProxy) ||
(pair.y == m_obsoleteProxy));
}
};
RemovePairCallback removeCallback(proxy);
@ -635,4 +557,3 @@ void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher
{
//should already be sorted
}

57
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h vendored
View file

@ -22,33 +22,26 @@ subject to the following restrictions:
class b3Dispatcher;
#include "b3OverlappingPair.h"
typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
struct b3OverlapCallback
{
virtual ~b3OverlapCallback()
{}
{
}
//return true for deletion of the pair
virtual bool processOverlap(b3BroadphasePair& pair) = 0;
};
struct b3OverlapFilterCallback
{
virtual ~b3OverlapFilterCallback()
{}
{
}
// return true when pairs need collision
virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
};
extern int b3g_removePairs;
extern int b3g_addedPairs;
extern int b3g_findPairs;
@ -89,8 +82,6 @@ public:
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
};
/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
@ -100,12 +91,10 @@ class b3HashedOverlappingPairCache : public b3OverlappingPairCache
b3OverlapFilterCallback* m_overlapFilterCallback;
// bool m_blockedForChanges;
public:
b3HashedOverlappingPairCache();
virtual ~b3HashedOverlappingPairCache();
virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
@ -133,11 +122,8 @@ public:
return internalAddPair(proxy0, proxy1);
}
void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
@ -162,8 +148,6 @@ public:
void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
b3BroadphasePair* findPair(int proxy0, int proxy1);
int GetCount() const { return m_overlappingPairArray.size(); }
@ -183,8 +167,8 @@ public:
{
return m_overlappingPairArray.size();
}
private:
private:
b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
void growTables();
@ -210,8 +194,6 @@ private:
}
*/
B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
{
int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) << 16));
@ -226,10 +208,6 @@ private:
return static_cast<unsigned int>(key);
}
B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
{
int proxyId1 = proxy0;
@ -269,18 +247,12 @@ private:
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
protected:
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
// b3OverlappingPairCallback* m_ghostPairCallback;
};
///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
class b3SortedOverlappingPairCache : public b3OverlappingPairCache
@ -301,7 +273,6 @@ class b3SortedOverlappingPairCache : public b3OverlappingPairCache
// b3OverlappingPairCallback* m_ghostPairCallback;
public:
b3SortedOverlappingPairCache();
virtual ~b3SortedOverlappingPairCache();
@ -315,12 +286,10 @@ class b3SortedOverlappingPairCache : public b3OverlappingPairCache
b3BroadphasePair* findPair(int proxy0, int proxy1);
void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
{
if (m_overlapFilterCallback)
@ -342,9 +311,6 @@ class b3SortedOverlappingPairCache : public b3OverlappingPairCache
return m_overlappingPairArray;
}
b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
@ -381,20 +347,14 @@ class b3SortedOverlappingPairCache : public b3OverlappingPairCache
}
*/
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
};
///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
class b3NullPairCache : public b3OverlappingPairCache
{
b3BroadphasePairArray m_overlappingPairArray;
public:
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
@ -410,7 +370,6 @@ public:
virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
{
}
virtual int getNumOverlappingPairs() const
@ -420,7 +379,6 @@ public:
virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
{
}
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
@ -464,11 +422,6 @@ public:
{
(void)dispatcher;
}
};
#endif //B3_OVERLAPPING_PAIR_CACHE_H

7
thirdparty/bullet/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h vendored
View file

@ -2,7 +2,6 @@
#ifndef B3_AABB_H
#define B3_AABB_H
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Common/shared/b3Mat3x3.h"
@ -10,14 +9,12 @@ typedef struct b3Aabb b3Aabb_t;
struct b3Aabb
{
union
{
union {
float m_min[4];
b3Float4 m_minVec;
int m_minIndices[4];
};
union
{
union {
float m_max[4];
b3Float4 m_maxVec;
int m_signedMaxIndices[4];

2
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Config.h vendored
View file

@ -36,6 +36,4 @@ struct b3Config
}
};
#endif //B3_CONFIG_H

15
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h vendored
View file

@ -19,7 +19,8 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
B3_ATTRIBUTE_ALIGNED16(struct)
b3Contact4 : public b3Contact4Data
{
B3_DECLARE_ALIGNED_ALLOCATOR();
@ -31,9 +32,17 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
int& getBatchIdx() { return m_batchIdx; }
const int& getBatchIdx() const { return m_batchIdx; }
float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
void setRestituitionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_restituitionCoeffCmp = (unsigned short)(c*0xffff); }
void setRestituitionCoeff(float c)
{
b3Assert(c >= 0.f && c <= 1.f);
m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
}
float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
void setFrictionCoeff(float c)
{
b3Assert(c >= 0.f && c <= 1.f);
m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
}
//float& getNPoints() { return m_worldNormal[3]; }
int getNPoints() const { return (int)m_worldNormalOnB.w; }

80
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp vendored
View file

@ -13,26 +13,18 @@ subject to the following restrictions:
*/
//Originally written by Erwin Coumans
#include "b3ConvexUtility.h"
#include "Bullet3Geometry/b3ConvexHullComputer.h"
#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3HashMap.h"
b3ConvexUtility::~b3ConvexUtility()
{
}
bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
{
b3ConvexHullComputer conv;
conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
@ -41,7 +33,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
faceNormals.resize(numFaces);
b3ConvexHullComputer* convexUtil = &conv;
b3AlignedObjectArray<b3MyFace> tmpFaces;
tmpFaces.resize(numFaces);
@ -52,7 +43,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
m_vertices[p] = convexUtil->vertices[p];
}
for (int i = 0; i < numFaces; i++)
{
int face = convexUtil->faces[i];
@ -66,7 +56,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
do
{
int src = edge->getSourceVertex();
tmpFaces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
@ -83,7 +72,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
b3Scalar planeEq = 1e30f;
if (numEdges == 2)
{
faceNormals[i] = edges[0].cross(edges[1]);
@ -92,7 +80,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
tmpFaces[i].m_plane[1] = faceNormals[i].getY();
tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
tmpFaces[i].m_plane[3] = planeEq;
}
else
{
@ -140,7 +127,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
}
}
bool did_merge = false;
if (coplanarFaceGroup.size() > 1)
{
@ -177,8 +163,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
}
}
b3MyFace combinedFace;
for (int i = 0; i < 4; i++)
combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
@ -204,19 +188,20 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
// are there rejected vertices?
bool reject_merge = false;
for(int i = 0; i < orgpoints.size(); i++) {
for (int i = 0; i < orgpoints.size(); i++)
{
if (orgpoints[i].m_orgIndex == -1)
continue; // this is in the hull...
// this vertex is rejected -- is anybody else using this vertex?
for(int j = 0; j < tmpFaces.size(); j++) {
for (int j = 0; j < tmpFaces.size(); j++)
{
b3MyFace& face = tmpFaces[j];
// is this a face of the current coplanar group?
bool is_in_current_group = false;
for(int k = 0; k < coplanarFaceGroup.size(); k++) {
if(coplanarFaceGroup[k] == j) {
for (int k = 0; k < coplanarFaceGroup.size(); k++)
{
if (coplanarFaceGroup[k] == j)
{
is_in_current_group = true;
break;
}
@ -224,8 +209,10 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
if (is_in_current_group) // ignore this face...
continue;
// does this face use this rejected vertex?
for(int v = 0; v < face.m_indices.size(); v++) {
if(face.m_indices[v] == orgpoints[i].m_orgIndex) {
for (int v = 0; v < face.m_indices.size(); v++)
{
if (face.m_indices[v] == orgpoints[i].m_orgIndex)
{
// this rejected vertex is used in another face -- reject merge
reject_merge = true;
break;
@ -252,11 +239,7 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
m_faces.push_back(face);
}
}
}
initialize();
@ -264,11 +247,6 @@ bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices,
return true;
}
inline bool IsAlmostZero(const b3Vector3& v)
{
if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
@ -315,14 +293,22 @@ bool b3ConvexUtility::testContainment() const
for (int p = 0; p < 8; p++)
{
b3Vector3 LocalPt;
if(p==0) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
else if(p==1) LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
else if(p==2) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
else if(p==3) LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if(p==4) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
else if(p==5) LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if(p==6) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if(p==7) LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
if (p == 0)
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
else if (p == 1)
LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
else if (p == 2)
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
else if (p == 3)
LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
else if (p == 4)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
else if (p == 5)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
else if (p == 6)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
else if (p == 7)
LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
for (int i = 0; i < m_faces.size(); i++)
{
@ -338,7 +324,6 @@ bool b3ConvexUtility::testContainment() const
void b3ConvexUtility::initialize()
{
b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
b3Scalar TotalArea = 0.0f;
@ -386,7 +371,8 @@ void b3ConvexUtility::initialize()
// b3Assert(edptr->m_face0>=0);
// b3Assert(edptr->m_face1<0);
edptr->m_face1 = i;
} else
}
else
{
b3InternalEdge ed;
ed.m_face0 = i;
@ -435,9 +421,6 @@ void b3ConvexUtility::initialize()
}
m_localCenter /= TotalArea;
#ifdef TEST_INTERNAL_OBJECTS
if (1)
{
@ -450,7 +433,6 @@ void b3ConvexUtility::initialize()
m_radius = dist;
}
b3Scalar MinX = FLT_MAX;
b3Scalar MinY = FLT_MAX;
b3Scalar MinZ = FLT_MAX;
@ -470,8 +452,6 @@ void b3ConvexUtility::initialize()
mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
// const b3Scalar r = m_radius / sqrtf(2.0f);
const b3Scalar r = m_radius / sqrtf(3.0f);
const int LargestExtent = mE.maxAxis();

11
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h vendored
View file

@ -20,16 +20,14 @@ subject to the following restrictions:
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Common/b3Transform.h"
struct b3MyFace
{
b3AlignedObjectArray<int> m_indices;
b3Scalar m_plane[4];
};
B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
B3_ATTRIBUTE_ALIGNED16(class)
b3ConvexUtility
{
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
@ -44,7 +42,6 @@ B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
b3AlignedObjectArray<b3MyFace> m_faces;
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
b3ConvexUtility()
{
}
@ -54,9 +51,5 @@ B3_ATTRIBUTE_ALIGNED16(class) b3ConvexUtility
void initialize();
bool testContainment() const;
};
#endif

26
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp vendored
View file

@ -5,7 +5,6 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
struct b3CpuNarrowPhaseInternalData
{
b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
@ -13,7 +12,6 @@ struct b3CpuNarrowPhaseInternalData
b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
b3Config m_config;
b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
b3AlignedObjectArray<b3Vector3> m_convexVertices;
@ -25,7 +23,6 @@ struct b3CpuNarrowPhaseInternalData
int m_numAcceleratedShapes;
};
const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
{
return m_data->m_contacts;
@ -41,7 +38,6 @@ const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) cons
return m_data->m_collidablesCPU[collidableIndex];
}
b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
{
m_data = new b3CpuNarrowPhaseInternalData;
@ -81,7 +77,6 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
// computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
//printf("convex-sphere\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@ -90,7 +85,6 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
// computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
@ -99,7 +93,6 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
// computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("plane-convex\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
@ -109,17 +102,14 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
// nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
// printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
{
// computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("convex-plane\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
@ -128,7 +118,6 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
// computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
// &m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
// printf("plane-convex\n");
}
if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
@ -140,16 +129,12 @@ void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3Al
int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
if (contactIndex >= 0)
{
pairs[i].z = contactIndex;
}
// printf("plane-convex\n");
}
}
m_data->m_contacts.resize(numContacts);
@ -161,12 +146,10 @@ int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
if (collidableIndex < 0)
return collidableIndex;
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
col.m_shapeType = SHAPE_CONVEX_HULL;
col.m_shapeIndex = -1;
{
b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
@ -200,7 +183,6 @@ int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
aabb.m_signedMaxIndices[3] = 0;
m_data->m_localShapeAABBCPU.push_back(aabb);
}
return collidableIndex;
@ -219,7 +201,6 @@ int b3CpuNarrowPhase::allocateCollidable()
b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
}
return -1;
}
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
@ -246,14 +227,11 @@ int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideI
return collidableIndex;
}
int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
{
m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
convex.mC = convexPtr->mC;
convex.mE = convexPtr->mE;
@ -280,7 +258,6 @@ int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr
m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
for (i = 0; i < convexPtr->m_faces.size(); i++)
{
m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
@ -288,7 +265,6 @@ int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr
convexPtr->m_faces[i].m_plane[2],
convexPtr->m_faces[i].m_plane[3]);
int indexOffset = m_data->m_convexIndices.size();
int numIndices = convexPtr->m_faces[i].m_indices.size();
m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
@ -312,8 +288,6 @@ int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr
(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
return m_data->m_numAcceleratedShapes++;
}

13
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h vendored
View file

@ -12,7 +12,6 @@
class b3CpuNarrowPhase
{
protected:
struct b3CpuNarrowPhaseInternalData* m_data;
int m_acceleratedCompanionShapeIndex;
int m_planeBodyIndex;
@ -22,10 +21,6 @@ protected:
int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
public:
b3CpuNarrowPhase(const struct b3Config& config);
virtual ~b3CpuNarrowPhase(void);
@ -54,25 +49,19 @@ public:
void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
const struct b3RigidBodyData* getBodiesCpu() const;
//struct b3RigidBodyData* getBodiesCpu();
int getNumBodiesGpu() const;
int getNumBodyInertiasGpu() const;
const struct b3Collidable* getCollidablesCpu() const;
int getNumCollidablesGpu() const;
/*const struct b3Contact4* getContactsCPU() const;
@ -81,7 +70,6 @@ public:
const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
int getNumRigidBodies() const;
int allocateCollidable();
@ -102,4 +90,3 @@ public:
};
#endif //B3_CPU_NARROWPHASE_H

7
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h vendored
View file

@ -4,13 +4,15 @@
#include "Bullet3Common/b3Vector3.h"
B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
B3_ATTRIBUTE_ALIGNED16(struct)
b3RayInfo
{
b3Vector3 m_from;
b3Vector3 m_to;
};
B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
B3_ATTRIBUTE_ALIGNED16(struct)
b3RayHit
{
b3Scalar m_hitFraction;
int m_hitBody;
@ -21,4 +23,3 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
};
#endif //B3_RAYCAST_INFO_H

2
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h vendored
View file

@ -20,11 +20,9 @@ subject to the following restrictions:
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
inline float b3GetInvMass(const b3RigidBodyData& body)
{
return body.m_invMass;
}
#endif //B3_RIGID_BODY_CL

1
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h vendored
View file

@ -17,4 +17,3 @@ struct b3BvhSubtreeInfoData
};
#endif //B3_BVH_SUBTREE_INFO_DATA_H

17
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h vendored
View file

@ -7,8 +7,6 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
// work-in-progress
void b3BvhTraversal(__global const b3Int4* pairs,
__global const b3RigidBodyData* rigidBodies,
@ -23,7 +21,6 @@ void b3BvhTraversal( __global const b3Int4* pairs,
int maxNumConcavePairsCapacity,
int id)
{
int bodyIndexA = pairs[id].x;
int bodyIndexB = pairs[id].y;
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
@ -42,8 +39,7 @@ void b3BvhTraversal( __global const b3Int4* pairs,
if (shapeTypeB != SHAPE_CONVEX_HULL &&
shapeTypeB != SHAPE_SPHERE &&
shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
)
shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
return;
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
@ -55,7 +51,6 @@ void b3BvhTraversal( __global const b3Int4* pairs,
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
unsigned short int quantizedQueryAabbMin[3];
unsigned short int quantizedQueryAabbMax[3];
b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
@ -97,7 +92,8 @@ void b3BvhTraversal( __global const b3Int4* pairs,
concavePairsOut[pairIdx + b] = newPair;
}
}
} else
}
else
{
int pairIdx = b3AtomicInc(numConcavePairsOut);
if (pairIdx < maxNumConcavePairsCapacity)
@ -108,12 +104,14 @@ void b3BvhTraversal( __global const b3Int4* pairs,
}
}
curIndex++;
} else
}
else
{
if (isLeafNode)
{
curIndex++;
} else
}
else
{
escapeIndex = b3GetEscapeIndex(&rootNode);
curIndex += escapeIndex;
@ -122,5 +120,4 @@ void b3BvhTraversal( __global const b3Int4* pairs,
}
}
}
}

19
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h vendored
View file

@ -1,7 +1,6 @@
#ifndef B3_CLIP_FACES_H
#define B3_CLIP_FACES_H
#include "Bullet3Common/shared/b3Int4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
@ -10,7 +9,6 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
{
return b3MakeFloat4(a.x + (b.x - a.x) * t,
@ -22,7 +20,6 @@ inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t)
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
{
int ve;
float ds, de;
int numVertsOut = 0;
@ -67,7 +64,6 @@ int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4Cons
return numVertsOut;
}
__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
__global const int* hasSeparatingAxis,
__global b3Int4* clippingFacesOut,
@ -76,8 +72,7 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
__global b3Float4* worldVertsB1,
__global b3Float4* worldVertsB2,
int vertexFaceCapacity,
int pairIndex
)
int pairIndex)
{
// int i = get_global_id(0);
//int pairIndex = i;
@ -88,10 +83,8 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
// if (i<numPairs)
{
if (hasSeparatingAxis[i])
{
// int bodyIndexA = pairs[i].x;
// int bodyIndexB = pairs[i].y;
@ -102,11 +95,9 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
{
__global b3Int4* clippingFaces = clippingFacesOut;
int closestFaceA = clippingFaces[pairIndex].x;
// int closestFaceB = clippingFaces[pairIndex].y;
int numVertsInA = clippingFaces[pairIndex].z;
@ -116,9 +107,6 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
if (closestFaceA >= 0)
{
// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
for (int e0 = 0; e0 < numVertsInA; e0++)
@ -169,11 +157,8 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
}
}
}
clippingFaces[pairIndex].w = numLocalContactsOut;
}
for (int i = 0; i < numLocalContactsOut; i++)
@ -181,8 +166,6 @@ __kernel void clipFacesAndFindContactsKernel( __global const b3Float4* sepa
} // if (hasSeparatingAxis[i])
} // if (i<numPairs)
}
#endif //B3_CLIP_FACES_H

17
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h vendored
View file

@ -2,7 +2,6 @@
#ifndef B3_COLLIDABLE_H
#define B3_COLLIDABLE_H
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Common/shared/b3Quat.h"
@ -20,22 +19,19 @@ enum b3ShapeTypes
typedef struct b3Collidable b3Collidable_t;
struct b3Collidable
{
union {
int m_numChildShapes;
int m_bvhIndex;
};
union
{
union {
float m_radius;
int m_compoundBvhIndex;
};
int m_shapeType;
union
{
union {
int m_shapeIndex;
float m_height;
};
@ -46,18 +42,15 @@ struct b3GpuChildShape
{
b3Float4 m_childPosition;
b3Quat m_childOrientation;
union
{
union {
int m_shapeIndex; //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
int m_capsuleAxis;
};
union
{
union {
float m_radius; //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
int m_numChildShapes; //used for compound shape
};
union
{
union {
float m_height; //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
int m_collidableShapeIndex;
};

4
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h vendored
View file

@ -21,8 +21,6 @@ struct b3Contact4Data
int m_childIndexB;
int m_unused1;
int m_unused2;
};
inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
@ -35,6 +33,4 @@ inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numP
contact->m_worldNormalOnB.w = (float)numPoints;
};
#endif //B3_CONTACT4DATA_H

46
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h vendored
View file

@ -2,15 +2,12 @@
#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
#define B3_CONTACT_CONVEX_CONVEX_SAT_H
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
#define B3_MAX_VERTS 1024
inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
{
return b3MakeVector3(a.x + (b.x - a.x) * t,
@ -19,11 +16,9 @@ inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t)
0.f);
}
// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
{
int ve;
float ds, de;
int numVertsOut = 0;
@ -69,7 +64,6 @@ inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNor
return numVertsOut;
}
inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
b3Float4* worldVertsB2, int capacityWorldVertsB2,
@ -142,7 +136,6 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
numVertsOut = 0;
}
// only keep points that are behind the witness face
{
b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
@ -165,7 +158,8 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
//printf("depth=%f\n",depth);
}
} else
}
else
{
b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
}
@ -175,8 +169,6 @@ inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3Conve
return numContactsOut;
}
inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
@ -220,7 +212,7 @@ inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
}
}
#endif //BT_DEBUG_SAT_FACE
#endif //BT_DEBUG_SAT_FACE \
//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
{
const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
@ -241,7 +233,6 @@ inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
once = false;
}
b3Assert(closestFaceB >= 0);
{
//B3_PROFILE("worldVertsB1");
@ -267,9 +258,6 @@ inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
return numContactsOut;
}
inline int b3ClipHullHullSingle(
int bodyIndexA, int bodyIndexB,
const b3Float4& posA,
@ -308,12 +296,10 @@ inline int b3ClipHullHullSingle(
hullA = hostConvexDataA[colA.m_shapeIndex];
//printf("numvertsA = %d\n",hullA.m_numVertices);
b3Collidable colB = hostCollidablesB[collidableIndexB];
hullB = hostConvexDataB[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
b3Float4 contactsOut[B3_MAX_VERTS];
int localContactCapacity = B3_MAX_VERTS;
@ -322,9 +308,7 @@ inline int b3ClipHullHullSingle(
b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
#endif
{
b3Float4 worldVertsB1[B3_MAX_VERTS];
b3Float4 worldVertsB2[B3_MAX_VERTS];
int capacityWorldVerts = B3_MAX_VERTS;
@ -336,8 +320,6 @@ inline int b3ClipHullHullSingle(
b3Scalar minDist = -1;
b3Scalar maxDist = 0.;
b3Transform trA, trB;
{
//B3_PROFILE("b3TransformPoint computation");
@ -408,7 +390,8 @@ inline int b3ClipHullHullSingle(
//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
nContacts++;
} else
}
else
{
b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
}
@ -417,10 +400,6 @@ inline int b3ClipHullHullSingle(
return contactIndex;
}
inline int b3ContactConvexConvexSAT(
int pairIndex,
int bodyIndexA, int bodyIndexB,
@ -438,31 +417,23 @@ inline int b3ContactConvexConvexSAT(
{
int contactIndex = -1;
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
b3ConvexPolyhedronData hullA, hullB;
b3Float4 sepNormalWorldSpace;
b3Collidable colA = collidables[collidableIndexA];
hullA = convexShapes[colA.m_shapeIndex];
//printf("numvertsA = %d\n",hullA.m_numVertices);
b3Collidable colB = collidables[collidableIndexB];
hullB = convexShapes[colB.m_shapeIndex];
//printf("numvertsB = %d\n",hullB.m_numVertices);
#ifdef _WIN32
b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
@ -477,14 +448,10 @@ inline int b3ContactConvexConvexSAT(
convexVertices, uniqueEdges, faces, convexIndices,
convexVertices, uniqueEdges, faces, convexIndices,
sepNormalWorldSpace
);
sepNormalWorldSpace);
if (foundSepAxis)
{
contactIndex = b3ClipHullHullSingle(
bodyIndexA, bodyIndexB,
posA, ornA,
@ -511,7 +478,6 @@ inline int b3ContactConvexConvexSAT(
collidables,
sepNormalWorldSpace,
maxContactCapacity);
}
return contactIndex;

17
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h vendored
View file

@ -2,10 +2,6 @@
#ifndef B3_CONTACT_SPHERE_SPHERE_H
#define B3_CONTACT_SPHERE_SPHERE_H
void computeContactSphereConvex(int pairIndex,
int bodyIndexA, int bodyIndexB,
int collidableIndexA, int collidableIndexB,
@ -19,16 +15,12 @@ void computeContactSphereConvex(int pairIndex,
int& nGlobalContactsOut,
int maxContactCapacity)
{
float radius = collidables[collidableIndexA].m_radius;
float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
float4 pos = rigidBodies[bodyIndexB].m_pos;
b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
b3Transform tr;
@ -85,7 +77,8 @@ void computeContactSphereConvex(int pairIndex,
localHitNormal = planeEqn;
region = 1;
}
} else
}
else
{
b3Vector3 tmp = spherePos - out;
b3Scalar l2 = tmp.length2();
@ -99,8 +92,8 @@ void computeContactSphereConvex(int pairIndex,
localHitNormal = tmp / dist;
region = 2;
}
} else
}
else
{
bCollide = false;
break;
@ -123,7 +116,6 @@ void computeContactSphereConvex(int pairIndex,
if (bCollide && minDist > -10000)
{
float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal; //-hitNormalWorld;
float4 pOnB1 = tr(closestPnt);
//printf("dist ,%f,",minDist);
@ -157,6 +149,5 @@ void computeContactSphereConvex(int pairIndex,
} //if (dstIdx < numPairs)
}
} //if (hasCollision)
}
#endif //B3_CONTACT_SPHERE_SPHERE_H

2
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h vendored
View file

@ -2,8 +2,6 @@
#ifndef B3_CONVEX_POLYHEDRON_DATA_H
#define B3_CONVEX_POLYHEDRON_DATA_H
#include "Bullet3Common/shared/b3Float4.h"
#include "Bullet3Common/shared/b3Quat.h"

59
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h vendored
View file

@ -3,7 +3,6 @@
#define B3_TRIANGLE_NUM_CONVEX_FACES 5
#include "Bullet3Common/shared/b3Int4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
@ -12,7 +11,6 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
{
@ -40,7 +38,6 @@ const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
max[0] += offset;
}
inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
b3Float4ConstArg posA, b3QuatConstArg ornA,
b3Float4ConstArg posB, b3QuatConstArg ornB,
@ -60,7 +57,6 @@ inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3
return true;
}
bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
b3Float4ConstArg posA1,
b3QuatConstArg ornA,
@ -80,8 +76,6 @@ bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b
b3Float4* sep,
float* dmin)
{
b3Float4 posA = posA1;
posA.w = 0.f;
b3Float4 posB = posB1;
@ -130,7 +124,6 @@ bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b
return true;
}
b3Vector3 unitSphere162[] =
{
b3MakeVector3(0.000000, -1.000000, 0.000000),
@ -294,9 +287,7 @@ b3MakeVector3(-0.052790,0.723612,-0.688185),
b3MakeVector3(-0.361804, 0.723612, -0.587778),
b3MakeVector3(0.361800, 0.894429, -0.262863),
b3MakeVector3(0.638194, 0.723610, -0.262864),
b3MakeVector3(0.447209,0.723612,-0.525728)
};
b3MakeVector3(0.447209, 0.723612, -0.525728)};
bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
b3Float4ConstArg posA1,
@ -316,8 +307,6 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
float* dmin,
bool searchAllEdgeEdge)
{
b3Float4 posA = posA1;
posA.w = 0.f;
b3Float4 posB = posB1;
@ -335,10 +324,8 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
printf("maxEdgeTests = %d\n", maxEdgeTests);
printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
}
if (searchAllEdgeEdge)
{
for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
@ -351,7 +338,6 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
b3Float4 edge1World = b3QuatRotate(ornB, edge1);
b3Float4 crossje = b3Cross(edge0World, edge1World);
curEdgeEdge++;
@ -376,10 +362,8 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
float d1 = Max1 - Min0;
dist = d0 < d1 ? d0 : d1;
result = true;
}
if (dist < *dmin)
{
*dmin = dist;
@ -387,14 +371,13 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
}
}
}
}
} else
}
else
{
int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
//printf("numDirections =%d\n",numDirections );
for (int i = 0; i < numDirections; i++)
{
b3Float4 crossje = unitSphere162[i];
@ -416,10 +399,8 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
float d1 = Max1 - Min0;
dist = d0 < d1 ? d0 : d1;
result = true;
}
if (dist < *dmin)
{
*dmin = dist;
@ -428,10 +409,8 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
}
}
}
}
if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
{
*sep = -(*sep);
@ -439,8 +418,6 @@ bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global
return true;
}
inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
__global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
@ -461,7 +438,6 @@ inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
int numContactsOut = 0;
int numWorldVertsB1 = 0;
int closestFaceB = -1;
float dmax = -FLT_MAX;
@ -524,13 +500,9 @@ inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
clippingFaces[pairIndex].z = numVerticesA;
clippingFaces[pairIndex].w = numWorldVertsB1;
return numContactsOut;
}
__kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
__global const b3RigidBodyData* rigidBodies,
__global const b3Collidable* collidables,
@ -549,8 +521,7 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
__global int* hasSeparatingNormals,
int vertexFaceCapacity,
int numConcavePairs,
int pairIdx
)
int pairIdx)
{
int i = pairIdx;
/* int i = get_global_id(0);
@ -606,7 +577,6 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
}
overlap = true;
@ -632,7 +602,6 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
convexPolyhedronA.m_faceOffset = 0;
b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
@ -703,7 +672,6 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
posA.w = 0.f;
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
@ -712,9 +680,6 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
///////////////////
///compound shape support
@ -739,7 +704,6 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
const b3Float4 DeltaC2 = c0 - c1;
bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
posA, ornA,
posB, ornB,
@ -751,7 +715,8 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
if (!sepA)
{
hasSeparatingAxis = 0;
} else
}
else
{
bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
posB, ornB,
@ -764,7 +729,8 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
if (!sepB)
{
hasSeparatingAxis = 0;
} else
}
else
{
bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
posA, ornA,
@ -777,7 +743,8 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
if (!sepEE)
{
hasSeparatingAxis = 0;
} else
}
else
{
hasSeparatingAxis = 1;
}
@ -813,8 +780,8 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
faces,
indices,
clippingFacesOut, pairIdx);
} else
}
else
{
//mark this pair as in-active
concavePairs[pairIdx].w = -1;
@ -827,6 +794,4 @@ __kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
}
}
#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H

9
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h vendored
View file

@ -1,7 +1,6 @@
#ifndef B3_FIND_SEPARATING_AXIS_H
#define B3_FIND_SEPARATING_AXIS_H
inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
{
min = FLT_MAX;
@ -32,7 +31,6 @@ inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& p
max += offset;
}
inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA, const b3Quaternion& ornA,
const b3Float4& posB, const b3Quaternion& ornB,
@ -54,7 +52,6 @@ inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPol
return true;
}
inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
const b3Float4& posA1,
const b3Quaternion& ornA,
@ -107,7 +104,6 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
gActualNbTests++;
#endif
b3Scalar d;
if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
return false;
@ -163,7 +159,6 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
b3Float4 crossje = b3Cross3(edge0World, edge1World);
curEdgeEdge++;
@ -173,7 +168,6 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
if (b3Dot3F4(deltaC2, crossje) < 0)
crossje *= -1.f;
#ifdef TEST_INTERNAL_OBJECTS
gExpectedNbTests++;
if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
@ -192,10 +186,8 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
}
}
}
}
if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
sep = -sep;
@ -203,4 +195,3 @@ inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3C
}
#endif //B3_FIND_SEPARATING_AXIS_H

192
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h vendored
View file

@ -15,9 +15,6 @@
* See the License for more information.
*/
#ifndef B3_MPR_PENETRATION_H
#define B3_MPR_PENETRATION_H
@ -27,9 +24,6 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
#ifdef __cplusplus
#define B3_MPR_SQRT sqrtf
#else
@ -66,13 +60,11 @@ inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
s->last = size - 1;
}
inline int b3MprSimplexSize(const b3MprSimplex_t *s)
{
return s->last + 1;
}
inline const b3MprSupport_t *b3MprSimplexPoint(const b3MprSimplex_t *s, int idx)
{
// here is no check on boundaries
@ -89,7 +81,6 @@ inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t
b3MprSupportCopy(s->ps + pos, a);
}
inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
{
b3MprSupport_t supp;
@ -99,14 +90,11 @@ inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
b3MprSupportCopy(&s->ps[pos2], &supp);
}
inline int b3MprIsZero(float val)
{
return B3_MPR_FABS(val) < FLT_EPSILON;
}
inline int b3MprEq(float _a, float _b)
{
float ab;
@ -118,23 +106,21 @@ inline int b3MprEq(float _a, float _b)
a = B3_MPR_FABS(_a);
b = B3_MPR_FABS(_b);
if (b > a){
if (b > a)
{
return ab < FLT_EPSILON * b;
}else{
}
else
{
return ab < FLT_EPSILON * a;
}
}
inline int b3MprVec3Eq(const b3Float4 *a, const b3Float4 *b)
{
return b3MprEq((*a).x, (*b).x)
&& b3MprEq((*a).y, (*b).y)
&& b3MprEq((*a).z, (*b).z);
return b3MprEq((*a).x, (*b).x) && b3MprEq((*a).y, (*b).y) && b3MprEq((*a).z, (*b).z);
}
inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec, __global const b3ConvexPolyhedronData_t *hull, b3ConstArray(b3Float4) verticesA)
{
b3Float4 supVec = b3MakeFloat4(0, 0, 0, 0);
@ -148,10 +134,8 @@ inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global con
}
return supVec;
}
B3_STATIC void b3MprConvexSupport(int pairIndex, int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
@ -168,7 +152,6 @@ B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3R
const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), dir);
//find local support vertex
int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
@ -178,8 +161,6 @@ B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3R
b3Float4 pInA;
if (logme)
{
// b3Float4 supVec = b3MakeFloat4(0,0,0,0);
float maxDot = -B3_LARGE_FLOAT;
@ -188,18 +169,15 @@ B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3R
const b3Float4 scaled = localDir;
int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
pInA = cpuVertices[hull->m_vertexOffset + index];
}
} else
}
else
{
pInA = b3LocalGetSupportVertex(localDir, hull, cpuVertices);
}
//move vertex to world space
*outp = b3TransformPoint(pInA, pos, orn);
}
inline void b3MprSupport(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
@ -217,17 +195,8 @@ inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3Const
supp->v = supp->v1 - supp->v2;
}
inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
{
center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
center->v = center->v1 - center->v2;
@ -266,7 +235,6 @@ inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
return dot;
}
inline float b3MprVec3Len2(const b3Float4 *v)
{
return b3MprVec3Dot(v, v);
@ -281,10 +249,8 @@ inline void b3MprVec3Normalize(b3Float4 *d)
inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
{
*d = b3Cross3(*a, *b);
}
inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
{
*d = *v - *w;
@ -302,7 +268,6 @@ inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
b3MprVec3Normalize(dir);
}
inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
const b3Float4 *dir)
{
@ -352,25 +317,32 @@ inline void b3ExpandPortal(b3MprSimplex_t *portal,
b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
if (dot > 0.f){
if (dot > 0.f)
{
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
if (dot > 0.f){
if (dot > 0.f)
{
b3MprSimplexSet(portal, 1, v4);
}else{
}
else
{
b3MprSimplexSet(portal, 3, v4);
}
}else{
}
else
{
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
if (dot > 0.f){
if (dot > 0.f)
{
b3MprSimplexSet(portal, 2, v4);
}else{
}
else
{
b3MprSimplexSet(portal, 1, v4);
}
}
}
B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
@ -383,19 +355,16 @@ B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b
float dot;
int cont;
// vertex 0 is center of portal
b3FindOrigin(bodyIndexA, bodyIndexB, cpuBodyBuf, b3MprSimplexPointW(portal, 0));
// vertex 0 is center of portal
b3MprSimplexSetSize(portal, 1);
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
b3Float4 *b3mpr_vec3_origin = &zero;
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin))
{
// Portal's center lies on origin (0,0,0) => we know that objects
// intersect but we would need to know penetration info.
// So move center little bit...
@ -403,13 +372,11 @@ B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b
b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
}
// vertex 1 = support in direction of origin
b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
b3MprVec3Scale(&dir, -1.f);
b3MprVec3Normalize(&dir);
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 1));
b3MprSimplexSetSize(portal, 2);
@ -417,19 +384,21 @@ B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b
// test if origin isn't outside of v1
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
if (b3MprIsZero(dot) || dot < 0.f)
return -1;
// vertex 2
b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
&b3MprSimplexPoint(portal, 1)->v);
if (b3MprIsZero(b3MprVec3Len2(&dir))){
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){
if (b3MprIsZero(b3MprVec3Len2(&dir)))
{
if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin))
{
// origin lies on v1
return 1;
}else{
}
else
{
// origin lies on v0-v1 segment
return 2;
}
@ -454,12 +423,14 @@ B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b
// it is better to form portal faces to be oriented "outside" origin
dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
if (dot > 0.f){
if (dot > 0.f)
{
b3MprSimplexSwap(portal, 1, 2);
b3MprVec3Scale(&dir, -1.f);
}
while (b3MprSimplexSize(portal) < 4){
while (b3MprSimplexSize(portal) < 4)
{
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 3));
dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
@ -473,31 +444,37 @@ B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b
b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
&b3MprSimplexPoint(portal, 3)->v);
dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
if (dot < 0.f && !b3MprIsZero(dot)){
if (dot < 0.f && !b3MprIsZero(dot))
{
b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
cont = 1;
}
if (!cont){
if (!cont)
{
// test if origin is outside (v3, v0, v2) - set v1 as v3 and
// continue
b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
&b3MprSimplexPoint(portal, 2)->v);
dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
if (dot < 0.f && !b3MprIsZero(dot)){
if (dot < 0.f && !b3MprIsZero(dot))
{
b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
cont = 1;
}
}
if (cont){
if (cont)
{
b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
&b3MprSimplexPoint(portal, 0)->v);
b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
&b3MprSimplexPoint(portal, 0)->v);
b3MprVec3Cross(&dir, &va, &vb);
b3MprVec3Normalize(&dir);
}else{
}
else
{
b3MprSimplexSetSize(portal, 4);
}
}
@ -505,7 +482,6 @@ B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b
return 0;
}
B3_STATIC int b3RefinePortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
@ -531,11 +507,9 @@ B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3Co
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
// test if v4 can expand portal to contain origin and if portal
// expanding doesn't reach given tolerance
if (!portalCanEncapsuleOrigin(portal, &v4, &dir)
|| portalReachTolerance(portal, &v4, &dir))
if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
{
return -1;
}
@ -550,7 +524,6 @@ B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3Co
B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
{
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
b3Float4 *b3mpr_vec3_origin = &zero;
@ -580,7 +553,8 @@ B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
sum = b[0] + b[1] + b[2] + b[3];
if (b3MprIsZero(sum) || sum < 0.f){
if (b3MprIsZero(sum) || sum < 0.f)
{
b[0] = 0.f;
b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
@ -600,7 +574,8 @@ B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
b3MprVec3Copy(&p1, b3mpr_vec3_origin);
b3MprVec3Copy(&p2, b3mpr_vec3_origin);
for (i = 0; i < 4; i++){
for (i = 0; i < 4; i++)
{
b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
b3MprVec3Scale(&vec, b[i]);
b3MprVec3Add(&p1, &vec);
@ -656,21 +631,29 @@ inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
t = -1.f * b3MprVec3Dot(&a, &d);
t /= b3MprVec3Len2(&d);
if (t < 0.f || b3MprIsZero(t)){
if (t < 0.f || b3MprIsZero(t))
{
dist = b3MprVec3Dist2(x0, P);
if (witness)
b3MprVec3Copy(witness, x0);
}else if (t > 1.f || b3MprEq(t, 1.f)){
}
else if (t > 1.f || b3MprEq(t, 1.f))
{
dist = b3MprVec3Dist2(b, P);
if (witness)
b3MprVec3Copy(witness, b);
}else{
if (witness){
}
else
{
if (witness)
{
b3MprVec3Copy(witness, &d);
b3MprVec3Scale(witness, t);
b3MprVec3Add(witness, x0);
dist = b3MprVec3Dist2(witness, P);
}else{
}
else
{
// recycling variables
b3MprVec3Scale(&d, t);
b3MprVec3Add(&d, &a);
@ -681,7 +664,6 @@ inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
return dist;
}
inline float b3MprVec3PointTriDist2(const b3Float4 *P,
const b3Float4 *x0, const b3Float4 *B,
const b3Float4 *C,
@ -715,13 +697,10 @@ inline float b3MprVec3PointTriDist2(const b3Float4 *P,
s = (q * r - w * p) / (w * v - r * r);
t = (-s * r - q) / w;
if ((b3MprIsZero(s) || s > 0.f)
&& (b3MprEq(s, 1.f) || s < 1.f)
&& (b3MprIsZero(t) || t > 0.f)
&& (b3MprEq(t, 1.f) || t < 1.f)
&& (b3MprEq(t + s, 1.f) || t + s < 1.f)){
if (witness){
if ((b3MprIsZero(s) || s > 0.f) && (b3MprEq(s, 1.f) || s < 1.f) && (b3MprIsZero(t) || t > 0.f) && (b3MprEq(t, 1.f) || t < 1.f) && (b3MprEq(t + s, 1.f) || t + s < 1.f))
{
if (witness)
{
b3MprVec3Scale(&d1, s);
b3MprVec3Scale(&d2, t);
b3MprVec3Copy(witness, x0);
@ -729,7 +708,9 @@ inline float b3MprVec3PointTriDist2(const b3Float4 *P,
b3MprVec3Add(witness, &d2);
dist = b3MprVec3Dist2(witness, P);
}else{
}
else
{
dist = s * s * v;
dist += t * t * w;
dist += 2.f * s * t * r;
@ -737,18 +718,22 @@ inline float b3MprVec3PointTriDist2(const b3Float4 *P,
dist += 2.f * t * q;
dist += u;
}
}else{
}
else
{
dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
if (dist2 < dist){
if (dist2 < dist)
{
dist = dist2;
if (witness)
b3MprVec3Copy(witness, &witness2);
}
dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
if (dist2 < dist){
if (dist2 < dist)
{
dist = dist2;
if (witness)
b3MprVec3Copy(witness, &witness2);
@ -758,7 +743,6 @@ inline float b3MprVec3PointTriDist2(const b3Float4 *P,
return dist;
}
B3_STATIC void b3FindPenetr(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
b3ConstArray(b3Collidable_t) cpuCollidables,
@ -774,7 +758,6 @@ B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3Con
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
b3Float4 *b3mpr_vec3_origin = &zero;
iterations = 1UL;
for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
//while (1)
@ -784,17 +767,14 @@ B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3Con
b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
// reached tolerance -> find penetration info
if (portalReachTolerance(portal, &v4, &dir)
|| iterations ==B3_MPR_MAX_ITERATIONS)
if (portalReachTolerance(portal, &v4, &dir) || iterations == B3_MPR_MAX_ITERATIONS)
{
*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin, &b3MprSimplexPoint(portal, 1)->v, &b3MprSimplexPoint(portal, 2)->v, &b3MprSimplexPoint(portal, 3)->v, pdir);
*depth = B3_MPR_SQRT(*depth);
if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
{
*pdir = dir;
}
b3MprVec3Normalize(pdir);
@ -802,7 +782,6 @@ B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3Con
// barycentric coordinates:
b3FindPos(portal, pos);
return;
}
@ -820,7 +799,6 @@ B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *
b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
b3Float4 *b3mpr_vec3_origin = &zero;
b3MprVec3Copy(dir, b3mpr_vec3_origin);
b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
@ -831,7 +809,6 @@ B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *
B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
float *depth, b3Float4 *dir, b3Float4 *pos)
{
// Origin lies on v0-v1 segment.
// Depth is distance to v1, direction also and position must be
// computed
@ -840,14 +817,11 @@ B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
b3MprVec3Scale(pos, 0.5f);
b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
b3MprVec3Normalize(dir);
}
inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
@ -857,10 +831,8 @@ inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
__global int *hasSepAxis,
float *depthOut, b3Float4 *dirOut, b3Float4 *posOut)
{
b3MprSimplex_t portal;
// if (!hasSepAxis[pairIndex])
// return -1;
@ -870,7 +842,6 @@ inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
// Phase 1: Portal discovery
res = b3DiscoverPortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, hasSepAxis, &portal);
//sepAxis[pairIndex] = *pdir;//or -dir?
switch (res)
@ -897,7 +868,6 @@ inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
}
case 2:
{
b3FindPenetrSegment(&portal, depthOut, dirOut, posOut);
break;
}
@ -915,6 +885,4 @@ inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
return 0;
};
#endif //B3_MPR_PENETRATION_H

27
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h vendored
View file

@ -8,7 +8,6 @@
#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
{
if (nPoints == 0)
@ -17,20 +16,16 @@ int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b
if (nPoints <= 4)
return nPoints;
if (nPoints > 64)
nPoints = 64;
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
{
for (int i = 0; i < nPoints; i++)
center += p[i];
center /= (float)nPoints;
}
// sample 4 directions
b3Float4 aVector = p[0] - center;
@ -39,7 +34,6 @@ int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b
u = b3Normalized(u);
v = b3Normalized(v);
//keep point with deepest penetration
float minW = FLT_MAX;
@ -75,7 +69,6 @@ int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b
contactIdx[0].y = ie;
}
f = b3Dot(v, r);
if (f < maxDots.z)
{
@ -89,7 +82,6 @@ int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b
maxDots.w = f;
contactIdx[0].w = ie;
}
}
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
@ -99,7 +91,6 @@ int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b
}
return 4;
}
__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
@ -113,8 +104,7 @@ __kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
int vertexFaceCapacity,
int contactCapacity,
int numPairs,
int pairIndex
)
int pairIndex)
{
// int i = get_global_id(0);
//int pairIndex = i;
@ -125,18 +115,12 @@ __kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
if (i < numPairs)
{
if (hasSeparatingAxis[i])
{
int nPoints = clippingFaces[pairIndex].w;
if (nPoints > 0)
{
__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
b3Float4 normal = -separatingNormals[i];
@ -149,10 +133,10 @@ __kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
b3Assert(dstIdx < contactCapacity);
if (dstIdx < contactCapacity)
{
__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
c->m_worldNormalOnB = -normal;
c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
c->m_restituitionCoeffCmp = (0.f * 0xffff);
c->m_frictionCoeffCmp = (0.7f * 0xffff);
c->m_batchIdx = pairIndex;
int bodyA = pairs[pairIndex].x;
int bodyB = pairs[pairIndex].y;
@ -180,17 +164,12 @@ __kernel void b3NewContactReductionKernel( __global b3Int4* pairs,
};
GET_NPOINTS(*c) = nReducedContacts;
}
//#endif
} // if (numContactsOut>0)
} // if (hasSeparatingAxis[i])
} // if (i<numPairs)
}
#endif

6
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h vendored
View file

@ -50,16 +50,15 @@ inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int
out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
} else
}
else
{
out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
}
}
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
const unsigned short int* aabbMin1,
const unsigned short int* aabbMax1,
@ -86,5 +85,4 @@ inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
//return overlap;
}
#endif //B3_QUANTIZED_BVH_NODE_H

8
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h vendored
View file

@ -9,20 +9,16 @@ inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& near
if (nPoints <= 4)
return nPoints;
if (nPoints > 64)
nPoints = 64;
b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
{
for (int i = 0; i < nPoints; i++)
center += p[i];
center /= (float)nPoints;
}
// sample 4 directions
b3Float4 aVector = p[0] - center;
@ -31,7 +27,6 @@ inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& near
u = b3FastNormalized3(u);
v = b3FastNormalized3(v);
//keep point with deepest penetration
float minW = FLT_MAX;
@ -67,7 +62,6 @@ inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& near
contactIdx[0].y = ie;
}
f = b3Dot3F4(v, r);
if (f < maxDots.z)
{
@ -81,7 +75,6 @@ inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& near
maxDots.w = f;
contactIdx[0].w = ie;
}
}
if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
@ -91,7 +84,6 @@ inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& near
}
return 4;
}
#endif //B3_REDUCE_CONTACTS_H

3
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h vendored
View file

@ -7,7 +7,6 @@
typedef struct b3RigidBodyData b3RigidBodyData_t;
struct b3RigidBodyData
{
b3Float4 m_pos;
@ -29,6 +28,4 @@ struct b3InertiaData
b3Mat3x3 m_initInvInertia;
};
#endif //B3_RIGIDBODY_DATA_H

5
thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h vendored
View file

@ -1,14 +1,10 @@
#ifndef B3_UPDATE_AABBS_H
#define B3_UPDATE_AABBS_H
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
{
__global const b3RigidBodyData_t* body = &bodies[bodyId];
@ -21,7 +17,6 @@ void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies,
if (shapeIndex >= 0)
{
b3Aabb_t localAabb = localShapeAABB[collidableIndex];
b3Aabb_t worldAabb;

51
thirdparty/bullet/Bullet3Common/b3AlignedAllocator.cpp vendored
View file

@ -15,9 +15,11 @@ subject to the following restrictions:
#include "b3AlignedAllocator.h"
#ifdef B3_ALLOCATOR_STATISTICS
int b3g_numAlignedAllocs = 0;
int b3g_numAlignedFree = 0;
int b3g_totalBytesAlignedAllocs = 0; //detect memory leaks
#endif
static void *b3AllocDefault(size_t size)
{
@ -32,8 +34,6 @@ static void b3FreeDefault(void *ptr)
static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
#if defined(B3_HAS_ALIGNED_ALLOCATOR)
#include <malloc.h>
static void *b3AlignedAllocDefault(size_t size, int alignment)
@ -59,19 +59,18 @@ static inline void b3AlignedFreeDefault(void *ptr)
}
#else
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
{
void *ret;
char *real;
real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
if (real) {
if (real)
{
ret = b3AlignPointer(real + sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
} else {
}
else
{
ret = (void *)(real);
}
return (ret);
@ -81,14 +80,14 @@ static inline void b3AlignedFreeDefault(void *ptr)
{
void *real;
if (ptr) {
if (ptr)
{
real = *((void **)(ptr)-1);
b3s_freeFunc(real);
}
}
#endif
static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
@ -112,18 +111,19 @@ void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filen
{
void *ret;
char *real;
#ifdef B3_ALLOCATOR_STATISTICS
b3g_totalBytesAlignedAllocs += size;
b3g_numAlignedAllocs++;
#endif
real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
if (real) {
if (real)
{
ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
*((int *)(ret)-2) = size;
} else {
}
else
{
ret = (void *)(real); //??
}
@ -136,19 +136,22 @@ void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filen
void b3AlignedFreeInternal(void *ptr, int line, char *filename)
{
void *real;
#ifdef B3_ALLOCATOR_STATISTICS
b3g_numAlignedFree++;
if (ptr) {
#endif
if (ptr)
{
real = *((void **)(ptr)-1);
int size = *((int *)(ptr)-2);
#ifdef B3_ALLOCATOR_STATISTICS
b3g_totalBytesAlignedAllocs -= size;
#endif
b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
b3s_freeFunc(real);
} else
}
else
{
b3Printf("NULL ptr\n");
}
@ -158,7 +161,9 @@ void b3AlignedFreeInternal (void* ptr,int line,char* filename)
void *b3AlignedAllocInternal(size_t size, int alignment)
{
#ifdef B3_ALLOCATOR_STATISTICS
b3g_numAlignedAllocs++;
#endif
void *ptr;
ptr = b3s_alignedAllocFunc(size, alignment);
// b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
@ -171,11 +176,11 @@ void b3AlignedFreeInternal (void* ptr)
{
return;
}
#ifdef B3_ALLOCATOR_STATISTICS
b3g_numAlignedFree++;
#endif
// b3Printf("b3AlignedFreeInternal %x\n",ptr);
b3s_alignedFreeFunc(ptr);
}
#endif //B3_DEBUG_MEMORY_ALLOCATIONS

29
thirdparty/bullet/Bullet3Common/b3AlignedAllocator.h vendored
View file

@ -54,16 +54,14 @@ void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc);
///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
///The b3AlignedAllocator is a portable class for aligned memory allocations.
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
template <typename T, unsigned Alignment>
class b3AlignedAllocator {
class b3AlignedAllocator
{
typedef b3AlignedAllocator<T, Alignment> self_type;
public:
//just going down a list:
b3AlignedAllocator() {}
/*
@ -71,7 +69,9 @@ public:
*/
template <typename Other>
b3AlignedAllocator( const b3AlignedAllocator< Other , Alignment > & ) {}
b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
{
}
typedef const T* const_pointer;
typedef const T& const_reference;
@ -81,27 +81,30 @@ public:
pointer address(reference ref) const { return &ref; }
const_pointer address(const_reference ref) const { return &ref; }
pointer allocate ( btSizeType n , const_pointer * hint = 0 ) {
pointer allocate(btSizeType n, const_pointer* hint = 0)
{
(void)hint;
return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
}
void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
void deallocate( pointer ptr ) {
void deallocate(pointer ptr)
{
b3AlignedFree(reinterpret_cast<void*>(ptr));
}
void destroy(pointer ptr) { ptr->~value_type(); }
template < typename O > struct rebind {
template <typename O>
struct rebind
{
typedef b3AlignedAllocator<O, Alignment> other;
};
template <typename O>
self_type & operator=( const b3AlignedAllocator< O , Alignment > & ) { return *this; }
self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
{
return *this;
}
friend bool operator==(const self_type&, const self_type&) { return true; }
};
#endif //B3_ALIGNED_ALLOCATOR

39
thirdparty/bullet/Bullet3Common/b3AlignedObjectArray.h vendored
View file

@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_OBJECT_ARRAY__
#define B3_OBJECT_ARRAY__
@ -39,7 +38,6 @@ subject to the following restrictions:
#include <new> //for placement new
#endif //B3_USE_PLACEMENT_NEW
///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
template <typename T>
@ -108,7 +106,8 @@ protected:
B3_FORCE_INLINE void deallocate()
{
if(m_data) {
if (m_data)
{
//PCK: enclosed the deallocation in this block
if (m_ownsMemory)
{
@ -118,11 +117,7 @@ protected:
}
}
public:
b3AlignedObjectArray()
{
init();
@ -143,8 +138,6 @@ protected:
otherArray.copy(0, otherSize, m_data);
}
/// return the number of elements in the array
B3_FORCE_INLINE int size() const
{
@ -179,7 +172,6 @@ protected:
return m_data[n];
}
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
B3_FORCE_INLINE void clear()
{
@ -197,7 +189,6 @@ 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.
B3_FORCE_INLINE void resizeNoInitialize(int newsize)
@ -206,7 +197,8 @@ protected:
if (newsize < curSize)
{
} else
}
else
{
if (newsize > size())
{
@ -227,7 +219,8 @@ protected:
{
m_data[i].~T();
}
} else
}
else
{
if (newsize > size())
{
@ -239,7 +232,6 @@ protected:
new (&m_data[i]) T(fillData);
}
#endif //B3_USE_PLACEMENT_NEW
}
m_size = newsize;
@ -256,7 +248,6 @@ protected:
return m_data[sz];
}
B3_FORCE_INLINE T& expand(const T& fillValue = T())
{
int sz = size();
@ -272,7 +263,6 @@ protected:
return m_data[sz];
}
B3_FORCE_INLINE void push_back(const T& _Val)
{
int sz = size();
@ -290,7 +280,6 @@ protected:
m_size++;
}
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
B3_FORCE_INLINE int capacity() const
{
@ -321,22 +310,18 @@ protected:
m_data = s;
m_capacity = _Count;
}
}
class less
{
public:
bool operator()(const T& a, const T& b)
{
return (a < b);
}
};
template <typename L>
void quickSortInternal(const L& CompareFunc, int lo, int hi)
{
@ -355,7 +340,8 @@ protected:
if (i <= j)
{
swap(i, j);
i++; j--;
i++;
j--;
}
} while (i <= j);
@ -366,7 +352,6 @@ protected:
quickSortInternal(CompareFunc, i, hi);
}
template <typename L>
void quickSort(const L& CompareFunc)
{
@ -377,7 +362,6 @@ protected:
}
}
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename L>
void downHeap(T* pArr, int k, int n, const L& CompareFunc)
@ -422,7 +406,6 @@ protected:
m_data[index0] = m_data[index1];
m_data[index1] = temp;
#endif //B3_USE_PLACEMENT_NEW
}
template <typename L>
@ -441,7 +424,6 @@ protected:
{
swap(0, n - 1); /* largest of a[0..n-1] */
n = n - 1;
/* restore a[1..i-1] heap */
downHeap(m_data, 1, n, CompareFunc);
@ -455,7 +437,8 @@ protected:
int last = size() - 1;
//assume sorted array
while (first <= last) {
while (first <= last)
{
int mid = (first + last) / 2; // compute mid point.
if (key > m_data[mid])
first = mid + 1; // repeat search in top half.
@ -467,7 +450,6 @@ protected:
return size(); // failed to find key
}
int findLinearSearch(const T& key) const
{
int index = size();
@ -502,7 +484,6 @@ protected:
void remove(const T& key)
{
int findIndex = findLinearSearch(key);
if (findIndex < size())
{

27
thirdparty/bullet/Bullet3Common/b3CommandLineArgs.h vendored
View file

@ -12,11 +12,9 @@
class b3CommandLineArgs
{
protected:
std::map<std::string, std::string> pairs;
public:
// Constructor
b3CommandLineArgs(int argc, char **argv)
{
@ -29,16 +27,20 @@ public:
{
std::string arg = argv[i];
if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-')) {
if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
{
continue;
}
std::string::size_type pos;
std::string key, val;
if ((pos = arg.find( '=')) == std::string::npos) {
if ((pos = arg.find('=')) == std::string::npos)
{
key = std::string(arg, 2, arg.length() - 2);
val = "";
} else {
}
else
{
key = std::string(arg, 2, pos - 2);
val = std::string(arg, pos + 1, arg.length() - 1);
}
@ -54,7 +56,8 @@ public:
bool CheckCmdLineFlag(const char *arg_name)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
if ((itr = pairs.find(arg_name)) != pairs.end())
{
return true;
}
return false;
@ -73,7 +76,8 @@ template <typename T>
inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
if ((itr = pairs.find(arg_name)) != pairs.end())
{
std::istringstream strstream(itr->second);
strstream >> val;
return true;
@ -85,17 +89,18 @@ template <>
inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
{
std::map<std::string, std::string>::iterator itr;
if ((itr = pairs.find(arg_name)) != pairs.end()) {
if ((itr = pairs.find(arg_name)) != pairs.end())
{
std::string s = itr->second;
val = (char *)malloc(sizeof(char) * (s.length() + 1));
std::strcpy(val, s.c_str());
return true;
} else {
}
else
{
val = NULL;
}
return false;
}
#endif //COMMAND_LINE_ARGS_H

9
thirdparty/bullet/Bullet3Common/b3FileUtils.h vendored
View file

@ -70,8 +70,6 @@ struct b3FileUtils
return oriptr;
}
static int extractPath(const char* fileName, char* path, int maxPathLength)
{
const char* stripped = strip2(fileName, "/");
@ -82,13 +80,13 @@ struct b3FileUtils
if (len && ((len + 1) < maxPathLength))
{
for (int i = 0; i < len; i++)
{
path[i] = fileName[i];
}
path[len] = 0;
} else
}
else
{
len = 0;
b3Assert(maxPathLength > 0);
@ -108,7 +106,6 @@ struct b3FileUtils
return t;
}
static void toLower(char* str)
{
int len = strlen(str);
@ -118,7 +115,6 @@ struct b3FileUtils
}
}
/*static const char* strip2(const char* name, const char* pattern)
{
size_t const patlen = strlen(pattern);
@ -133,6 +129,5 @@ struct b3FileUtils
return oriptr;
}
*/
};
#endif //B3_FILE_UTILS_H

72
thirdparty/bullet/Bullet3Common/b3HashMap.h vendored
View file

@ -13,13 +13,11 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_HASH_MAP_H
#define B3_HASH_MAP_H
#include "b3AlignedObjectArray.h"
#include <string>
///very basic hashable string implementation, compatible with b3HashMap
@ -33,11 +31,9 @@ struct b3HashString
return m_hash;
}
b3HashString(const char* name)
: m_string(name)
{
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
static const unsigned int InitialFNV = 2166136261u;
static const unsigned int FNVMultiple = 16777619u;
@ -72,16 +68,14 @@ struct b3HashString
{
return (m_string == other.m_string);
}
};
const int B3_HASH_NULL = 0xffffffff;
class b3HashInt
{
int m_uid;
public:
b3HashInt(int uid) : m_uid(uid)
{
@ -106,24 +100,24 @@ public:
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
class b3HashPtr
{
union
{
union {
const void* m_pointer;
int m_hashValues[2];
};
public:
b3HashPtr(const void* ptr)
: m_pointer(ptr)
{
@ -147,20 +141,22 @@ public:
int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
template <class Value>
class b3HashKeyPtr
{
int m_uid;
public:
public:
b3HashKeyPtr(int uid) : m_uid(uid)
{
}
@ -180,20 +176,22 @@ public:
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
template <class Value>
class b3HashKey
{
int m_uid;
public:
public:
b3HashKey(int uid) : m_uid(uid)
{
}
@ -212,18 +210,21 @@ public:
{
int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
///The b3HashMap template class implements a generic and lightweight hashmap.
///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
template <class Key, class Value>
class b3HashMap
{
protected:
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
@ -263,14 +264,12 @@ protected:
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
}
}
}
public:
void insert(const Key& key, const Value& value) {
void insert(const Key& key, const Value& value)
{
int hash = key.getHash() & (m_valueArray.capacity() - 1);
//replace value if the key is already there
@ -297,8 +296,8 @@ protected:
m_hashTable[hash] = count;
}
void remove(const Key& key) {
void remove(const Key& key)
{
int hash = key.getHash() & (m_valueArray.capacity() - 1);
int pairIndex = findIndex(key);
@ -376,10 +375,8 @@ protected:
m_valueArray.pop_back();
m_keyArray.pop_back();
}
int size() const
{
return m_valueArray.size();
@ -411,7 +408,8 @@ protected:
return m_keyArray[index];
}
Value* operator[](const Key& key) {
Value* operator[](const Key& key)
{
return find(key);
}
@ -435,7 +433,6 @@ protected:
return &m_valueArray[index];
}
int findIndex(const Key& key) const
{
unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
@ -460,7 +457,6 @@ protected:
m_valueArray.clear();
m_keyArray.clear();
}
};
#endif //B3_HASH_MAP_H

15
thirdparty/bullet/Bullet3Common/b3Logging.cpp vendored
View file

@ -22,7 +22,6 @@ subject to the following restrictions:
#include <windows.h>
#endif //_WIN32
void b3PrintfFuncDefault(const char* msg)
{
#ifdef _WIN32
@ -41,10 +40,8 @@ void b3WarningMessageFuncDefault(const char* msg)
printf("%s", msg);
//is this portable?
fflush(stdout);
}
void b3ErrorMessageFuncDefault(const char* msg)
{
#ifdef _WIN32
@ -54,16 +51,12 @@ void b3ErrorMessageFuncDefault(const char* msg)
//is this portable?
fflush(stdout);
}
static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
///The developer can route b3Printf output using their own implementation
void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
{
@ -81,7 +74,6 @@ void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
//#define B3_MAX_DEBUG_STRING_LENGTH 2048
#define B3_MAX_DEBUG_STRING_LENGTH 32768
void b3OutputPrintfVarArgsInternal(const char* str, ...)
{
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
@ -110,7 +102,6 @@ void b3OutputWarningMessageVarArgsInternal(const char *str, ...)
}
void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
{
char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
va_list argList;
va_start(argList, str);
@ -121,10 +112,8 @@ void b3OutputErrorMessageVarArgsInternal(const char *str, ...)
#endif
(b3s_errorMessageFunc)(strDebug);
va_end(argList);
}
void b3EnterProfileZoneDefault(const char* name)
{
}
@ -151,10 +140,6 @@ void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
b3s_leaveFunc = leaveFunc;
}
#ifndef _MSC_VER
#undef vsprintf_s
#endif

19
thirdparty/bullet/Bullet3Common/b3Logging.h vendored
View file

@ -3,7 +3,8 @@
#define B3_LOGGING_H
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
@ -14,9 +15,18 @@ extern "C" {
//#define b3Printf(...) printf(__VA_ARGS__)
//#define b3Printf(...)
#define b3Warning(...) do {b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n",__FILE__,__LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); }while(0)
#define b3Error(...) do {b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n",__FILE__,__LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); } while(0)
#define b3Warning(...) \
do \
{ \
b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__); \
b3OutputWarningMessageVarArgsInternal(__VA_ARGS__); \
} while (0)
#define b3Error(...) \
do \
{ \
b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__); \
b3OutputErrorMessageVarArgsInternal(__VA_ARGS__); \
} while (0)
#ifndef B3_NO_PROFILE
@ -49,7 +59,6 @@ public:
#endif //#ifndef B3_NO_PROFILE
typedef void(b3PrintfFunc)(const char* msg);
typedef void(b3WarningMessageFunc)(const char* msg);
typedef void(b3ErrorMessageFunc)(const char* msg);

62
thirdparty/bullet/Bullet3Common/b3Matrix3x3.h vendored
View file

@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_MATRIX3x3_H
#define B3_MATRIX3x3_H
@ -37,11 +36,11 @@ const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
#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.
* Make sure to only include a pure orthogonal matrix without scaling. */
B3_ATTRIBUTE_ALIGNED16(class) b3Matrix3x3 {
B3_ATTRIBUTE_ALIGNED16(class)
b3Matrix3x3
{
///Data storage for the matrix, each vector is a row of the matrix
b3Vector3 m_el[3];
@ -131,7 +130,6 @@ public:
return b3MakeVector3(m_el[0][i], m_el[1][i], m_el[2][i]);
}
/** @brief Get a row of the matrix as a vector
* @param i Row number 0 indexed */
B3_FORCE_INLINE const b3Vector3& getRow(int i) const
@ -178,7 +176,6 @@ public:
m_el[0].setValue(m[0], m[4], m[8]);
m_el[1].setValue(m[1], m[5], m[9]);
m_el[2].setValue(m[2], m[6], m[10]);
}
/** @brief Set the values of the matrix explicitly (row major)
* @param xx Top left
@ -270,7 +267,6 @@ public:
#endif
}
/** @brief Set the matrix from euler angles using YPR around YXZ respectively
* @param yaw Yaw about Y axis
* @param pitch Pitch about X axis
@ -290,7 +286,8 @@ public:
* angles are applied in ZYX order. I.e a vector is first rotated
* about X then Y and then Z
**/
void setEulerZYX(b3Scalar eulerX,b3Scalar eulerY,b3Scalar eulerZ) {
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(b3Cos(eulerX));
b3Scalar cj(b3Cos(eulerY));
@ -419,16 +416,32 @@ public:
if (m_el[0].getX() < m_el[1].getY())
{
if (m_el[1].getY() < m_el[2].getZ())
{ i = 2; j = 0; k = 1; }
{
i = 2;
j = 0;
k = 1;
}
else
{ i = 1; j = 2; k = 0; }
{
i = 1;
j = 2;
k = 0;
}
}
else
{
if (m_el[0].getX() < m_el[2].getZ())
{ i = 2; j = 0; k = 1; }
{
i = 2;
j = 0;
k = 1;
}
else
{ i = 0; j = 1; k = 2; }
{
i = 0;
j = 1;
k = 2;
}
}
x = m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0);
@ -462,9 +475,7 @@ public:
}
else
{
int i = m_el[0].getX() < m_el[1].getY() ?
(m_el[1].getY() < m_el[2].getZ() ? 2 : 1) :
(m_el[0].getX() < m_el[2].getZ() ? 2 : 0);
int i = m_el[0].getX() < m_el[1].getY() ? (m_el[1].getY() < m_el[2].getZ() ? 2 : 1) : (m_el[0].getX() < m_el[2].getZ() ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
@ -486,7 +497,6 @@ public:
* @param roll around Z axis */
void getEulerYPR(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll) const
{
// first use the normal calculus
yaw = b3Scalar(b3Atan2(m_el[1].getX(), m_el[0].getX()));
pitch = b3Scalar(b3Asin(-m_el[2].getX()));
@ -507,7 +517,6 @@ public:
}
};
/**@brief Get the matrix represented as euler angles around ZYX
* @param yaw Yaw around X axis
* @param pitch Pitch around Y axis
@ -621,7 +630,6 @@ public:
return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ();
}
/**@brief diagonalizes this matrix by the Jacobi method.
* @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
* coordinate system, i.e., old_this = rot * new_this * rot^T.
@ -709,9 +717,6 @@ public:
}
}
/**@brief Calculate the matrix cofactor
* @param r1 The first row to use for calculating the cofactor
* @param c1 The first column to use for calculating the cofactor
@ -733,10 +738,8 @@ public:
void deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn);
void deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn);
};
B3_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator*=(const b3Matrix3x3& m)
{
@ -919,7 +922,6 @@ operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
#endif
}
B3_FORCE_INLINE b3Matrix3x3&
b3Matrix3x3::operator-=(const b3Matrix3x3& m)
{
@ -942,14 +944,12 @@ b3Matrix3x3::operator-=(const b3Matrix3x3& m)
return *this;
}
B3_FORCE_INLINE b3Scalar
b3Matrix3x3::determinant() const
{
return b3Triple((*this)[0], (*this)[1], (*this)[2]);
}
B3_FORCE_INLINE b3Matrix3x3
b3Matrix3x3::absolute() const
{
@ -989,7 +989,6 @@ b3Matrix3x3::transpose() const
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(B3_USE_NEON)
// note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
@ -1147,7 +1146,6 @@ operator*(const b3Matrix3x3& m, const b3Vector3& v)
#endif
}
B3_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Matrix3x3& m)
{
@ -1305,8 +1303,7 @@ B3_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
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] &&
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
@ -1324,9 +1321,6 @@ struct b3Matrix3x3DoubleData
b3Vector3DoubleData m_el[3];
};
B3_FORCE_INLINE void b3Matrix3x3::serialize(struct b3Matrix3x3Data& dataOut) const
{
for (int i = 0; i < 3; i++)
@ -1339,7 +1333,6 @@ B3_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct b3Matrix3x3FloatData& da
m_el[i].serializeFloat(dataOut.m_el[i]);
}
B3_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct b3Matrix3x3Data& dataIn)
{
for (int i = 0; i < 3; i++)
@ -1359,4 +1352,3 @@ B3_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct b3Matrix3x3Doub
}
#endif //B3_MATRIX3x3_H

2
thirdparty/bullet/Bullet3Common/b3MinMax.h vendored
View file

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GEN_MINMAX_H
#define B3_GEN_MINMAX_H

12
thirdparty/bullet/Bullet3Common/b3PoolAllocator.h vendored
View file

@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _BT_POOL_ALLOCATOR_H
#define _BT_POOL_ALLOCATOR_H
@ -29,7 +28,6 @@ class b3PoolAllocator
unsigned char* m_pool;
public:
b3PoolAllocator(int elemSize, int maxElements)
: m_elemSize(elemSize),
m_maxElements(maxElements)
@ -40,7 +38,8 @@ public:
m_firstFree = p;
m_freeCount = m_maxElements;
int count = m_maxElements;
while (--count) {
while (--count)
{
*(void**)p = (p + m_elemSize);
p += m_elemSize;
}
@ -81,7 +80,8 @@ public:
bool validPtr(void* ptr)
{
if (ptr) {
if (ptr)
{
if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
{
return true;
@ -92,7 +92,8 @@ public:
void freeMemory(void* ptr)
{
if (ptr) {
if (ptr)
{
b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
*(void**)ptr = m_firstFree;
@ -115,7 +116,6 @@ public:
{
return m_pool;
}
};
#endif //_BT_POOL_ALLOCATOR_H

29
thirdparty/bullet/Bullet3Common/b3QuadWord.h vendored
View file

@ -12,17 +12,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SIMD_QUADWORD_H
#define B3_SIMD_QUADWORD_H
#include "b3Scalar.h"
#include "b3MinMax.h"
#if defined(__CELLOS_LV2) && defined(__SPU__)
#include <altivec.h>
#endif
@ -31,18 +26,19 @@ subject to the following restrictions:
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/
#ifndef USE_LIBSPE2
B3_ATTRIBUTE_ALIGNED16(class) b3QuadWord
B3_ATTRIBUTE_ALIGNED16(class)
b3QuadWord
#else
class b3QuadWord
#endif
{
protected:
#if defined(__SPU__) && defined(__CELLOS_LV2__)
union {
vec_float4 mVec128;
b3Scalar m_floats[4];
};
public:
vec_float4 get128() const
{
@ -56,8 +52,12 @@ public:
union {
b3SimdFloat4 mVec128;
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
struct
{
b3Scalar x, y, z, w;
};
};
public:
B3_FORCE_INLINE b3SimdFloat4 get128() const
{
@ -69,17 +69,18 @@ public:
}
#else
public:
union
{
union {
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
struct
{
b3Scalar x, y, z, w;
};
};
#endif // B3_USE_SSE
#endif //__CELLOS_LV2__ __SPU__
public:
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
// Set Vector
@ -121,7 +122,6 @@ public:
B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
/**@brief Return the x value */
//B3_FORCE_INLINE b3Scalar& operator[](int i) { return (&m_floats[0])[i]; }
//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
@ -237,9 +237,6 @@ public:
b3SetMin(m_floats[3], other.m_floats[3]);
#endif
}
};
#endif //B3_SIMD_QUADWORD_H

31
thirdparty/bullet/Bullet3Common/b3Quaternion.h vendored
View file

@ -12,19 +12,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SIMD__QUATERNION_H_
#define B3_SIMD__QUATERNION_H_
#include "b3Vector3.h"
#include "b3QuadWord.h"
#ifdef B3_USE_SSE
const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
@ -39,7 +32,8 @@ const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f
#endif
/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
class b3Quaternion : public b3QuadWord {
class b3Quaternion : public b3QuadWord
{
public:
/**@brief No initialization constructor */
b3Quaternion() {}
@ -102,10 +96,17 @@ public:
{
b3Scalar d = axis.length();
b3Assert(d != b3Scalar(0.0));
if (d < B3_EPSILON)
{
setValue(0, 0, 0, 1);
}
else
{
b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
b3Cos(_angle * b3Scalar(0.5)));
}
}
/**@brief Set the quaternion using Euler angles
* @param yaw Angle around Y
* @param pitch Angle around X
@ -513,7 +514,6 @@ public:
return (-qd);
}
/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
* @param q The other quaternion to interpolate with
* @param t The ratio between this and q to interpolate. If t = 0 the result is this, if t=1 the result is q.
@ -553,14 +553,8 @@ public:
}
B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
};
/**@brief Return the product of two quaternions */
B3_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q1, const b3Quaternion& q2)
@ -823,7 +817,6 @@ b3Dot(const b3Quaternion& q1, const b3Quaternion& q2)
return q1.dot(q2);
}
/**@brief Return the length of a quaternion */
B3_FORCE_INLINE b3Scalar
b3Length(const b3Quaternion& q)
@ -868,8 +861,6 @@ b3QuatNormalized(const b3Quaternion& orn)
return orn.normalized();
}
B3_FORCE_INLINE b3Vector3
b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
{
@ -901,7 +892,6 @@ b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming
b3Scalar rs = 1.0f / s;
return b3Quaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f);
}
B3_FORCE_INLINE b3Quaternion
@ -913,6 +903,3 @@ b3ShortestArcQuatNormalize2(b3Vector3& v0,b3Vector3& v1)
}
#endif //B3_SIMD__QUATERNION_H_

4
thirdparty/bullet/Bullet3Common/b3Random.h vendored
View file

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GEN_RANDOM_H
#define B3_GEN_RANDOM_H
@ -45,6 +43,4 @@ inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
}
#endif //B3_GEN_RANDOM_H

11
thirdparty/bullet/Bullet3Common/b3ResizablePool.h vendored
View file

@ -29,7 +29,6 @@ struct b3PoolBodyHandle : public U
template <typename T>
class b3ResizablePool
{
protected:
b3AlignedObjectArray<T> m_bodyHandles;
int m_numUsedHandles; // number of active handles
@ -38,7 +37,6 @@ protected:
T* getHandleInternal(int handle)
{
return &m_bodyHandles[handle];
}
const T* getHandleInternal(int handle) const
{
@ -46,7 +44,6 @@ protected:
}
public:
b3ResizablePool()
{
initHandles();
@ -65,7 +62,6 @@ public:
void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
{
for (int i = 0; i < m_bodyHandles.size(); i++)
{
if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
@ -75,8 +71,6 @@ public:
}
}
T* getHandle(int handle)
{
b3Assert(handle >= 0);
@ -91,7 +85,6 @@ public:
return &m_bodyHandles[handle];
}
return 0;
}
const T* getHandle(int handle) const
{
@ -120,7 +113,6 @@ public:
for (int i = curCapacity; i < newCapacity; i++)
m_bodyHandles[i].setNextFree(i + 1);
m_bodyHandles[newCapacity - 1].setNextFree(-1);
}
m_firstFreeHandle = curCapacity;
@ -154,7 +146,6 @@ public:
int additionalCapacity = m_bodyHandles.size();
increaseHandleCapacity(additionalCapacity);
getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
}
getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
@ -162,7 +153,6 @@ public:
return handle;
}
void freeHandle(int handle)
{
b3Assert(handle >= 0);
@ -179,4 +169,3 @@ public:
///end handle management
#endif //B3_RESIZABLE_POOL_H

114
thirdparty/bullet/Bullet3Common/b3Scalar.h vendored
View file

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_SCALAR_H
#define B3_SCALAR_H
@ -22,8 +20,6 @@ subject to the following restrictions:
#pragma unmanaged
#endif
#include <math.h>
#include <stdlib.h> //size_t for MSVC 6.0
#include <float.h>
@ -42,7 +38,6 @@ inline int b3GetVersion()
#include "b3Logging.h" //for b3Error
#ifdef _WIN32
#if defined(__MINGW32__) || defined(__CYGWIN__) || (defined(_MSC_VER) && _MSC_VER < 1300)
@ -93,7 +88,17 @@ inline int b3GetVersion()
#ifdef B3_DEBUG
#ifdef _MSC_VER
#include <stdio.h>
#define b3Assert(x) { if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }}
#define b3Assert(x) \
{ \
if (!(x)) \
{ \
b3Error( \
"Assert "__FILE__ \
":%u (" #x ")\n", \
__LINE__); \
__debugbreak(); \
} \
}
#else //_MSC_VER
#include <assert.h>
#define b3Assert assert
@ -121,7 +126,17 @@ inline int b3GetVersion()
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#define b3Assert(x) {if(!(x)){b3Error("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
#define b3Assert(x) \
{ \
if (!(x)) \
{ \
b3Error( \
"Assert "__FILE__ \
":%u (" #x ")\n", \
__LINE__); \
spu_hcmpeq(0, 0); \
} \
}
#else
#define b3Assert assert
#endif
@ -154,11 +169,9 @@ inline int b3GetVersion()
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) __builtin_expect((_c), 1)
#define b3Unlikely(_c) __builtin_expect((_c), 0)
#else
//non-windows systems
@ -253,7 +266,6 @@ inline int b3GetVersion()
#endif //__CELLOS_LV2__
#endif
///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
#if defined(B3_USE_DOUBLE_PRECISION)
typedef double b3Scalar;
@ -311,7 +323,8 @@ inline __m128 operator * (const __m128 A, const __m128 B)
#define b3CastiTo128f(a) ((__m128)(a))
#define b3CastdTo128f(a) ((__m128)(a))
#define b3CastdTo128i(a) ((__m128i)(a))
#define b3Assign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
#define b3Assign128(r0, r1, r2, r3) \
(__m128) { r0, r1, r2, r3 }
#endif //_WIN32
#endif //B3_USE_SSE_IN_API
@ -321,13 +334,10 @@ inline __m128 operator * (const __m128 A, const __m128 B)
typedef float32x4_t b3SimdFloat4;
#define B3_INFINITY INFINITY
#define B3_NAN NAN
#define b3Assign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
#define b3Assign128(r0, r1, r2, r3) \
(float32x4_t) { r0, r1, r2, r3 }
#endif
#define B3_DECLARE_ALIGNED_ALLOCATOR() \
B3_FORCE_INLINE void *operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
B3_FORCE_INLINE void operator delete(void *ptr) { b3AlignedFree(ptr); } \
@ -336,19 +346,30 @@ typedef float32x4_t b3SimdFloat4;
B3_FORCE_INLINE void *operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes, 16); } \
B3_FORCE_INLINE void operator delete[](void *ptr) { b3AlignedFree(ptr); } \
B3_FORCE_INLINE void *operator new[](size_t, void *ptr) { return ptr; } \
B3_FORCE_INLINE void operator delete[](void*, void*) { } \
B3_FORCE_INLINE void operator delete[](void *, void *) {}
#if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x) { return sqrt(x); }
B3_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x)
{
return sqrt(x);
}
B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return acos(x); }
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return asin(x); }
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
{
if (x < b3Scalar(-1)) x = b3Scalar(-1);
if (x > b3Scalar(1)) x = b3Scalar(1);
return acos(x);
}
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
{
if (x < b3Scalar(-1)) x = b3Scalar(-1);
if (x > b3Scalar(1)) x = b3Scalar(1);
return asin(x);
}
B3_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
B3_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
B3_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
@ -382,14 +403,16 @@ B3_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
B3_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
B3_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
B3_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) {
B3_FORCE_INLINE b3Scalar b3Acos(b3Scalar x)
{
if (x < b3Scalar(-1))
x = b3Scalar(-1);
if (x > b3Scalar(1))
x = b3Scalar(1);
return acosf(x);
}
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) {
B3_FORCE_INLINE b3Scalar b3Asin(b3Scalar x)
{
if (x < b3Scalar(-1))
x = b3Scalar(-1);
if (x > b3Scalar(1))
@ -414,7 +437,6 @@ B3_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0) / b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
#ifdef B3_USE_DOUBLE_PRECISION
#define B3_EPSILON DBL_EPSILON
#define B3_INFINITY DBL_MAX
@ -429,10 +451,13 @@ B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
b3Scalar coeff_2 = 3.0f * coeff_1;
b3Scalar abs_y = b3Fabs(y);
b3Scalar angle;
if (x >= 0.0f) {
if (x >= 0.0f)
{
b3Scalar r = (x - abs_y) / (x + abs_y);
angle = coeff_1 - coeff_1 * r;
} else {
}
else
{
b3Scalar r = (x + abs_y) / (abs_y - x);
angle = coeff_2 - coeff_1 * r;
}
@ -441,22 +466,28 @@ B3_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
B3_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < B3_EPSILON; }
B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps) {
B3_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps)
{
return (((a) <= eps) && !((a) < -eps));
}
B3_FORCE_INLINE bool b3GreaterEqual (b3Scalar a, b3Scalar eps) {
B3_FORCE_INLINE bool b3GreaterEqual(b3Scalar a, b3Scalar eps)
{
return (!((a) <= eps));
}
B3_FORCE_INLINE int b3IsNegative(b3Scalar x) {
B3_FORCE_INLINE int b3IsNegative(b3Scalar x)
{
return x < b3Scalar(0.0) ? 1 : 0;
}
B3_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * B3_RADS_PER_DEG; }
B3_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * B3_DEGS_PER_RAD; }
#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#define B3_DECLARE_HANDLE(name) \
typedef struct name##__ \
{ \
int unused; \
} * name
#ifndef b3Fsel
B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
@ -466,7 +497,6 @@ B3_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
#endif
#define b3Fsels(a, b, c) (b3Scalar) b3Fsel(a, b, c)
B3_FORCE_INLINE bool b3MachineIsLittleEndian()
{
long int i = 1;
@ -477,8 +507,6 @@ B3_FORCE_INLINE bool b3MachineIsLittleEndian()
return false;
}
///b3Select avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html
B3_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
@ -506,14 +534,14 @@ B3_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero
#endif
}
template<typename T> B3_FORCE_INLINE void b3Swap(T& a, T& b)
template <typename T>
B3_FORCE_INLINE void b3Swap(T &a, T &b)
{
T tmp = a;
a = b;
b = tmp;
}
//PCK: endian swapping functions
B3_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
{
@ -569,7 +597,6 @@ B3_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
return d;
}
// swap using char pointers
B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char *dst)
{
@ -583,7 +610,6 @@ B3_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char* dst)
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
}
// unswap using char pointers
@ -636,23 +662,19 @@ struct b3TypedObject
}
};
///align a pointer to the provided alignment, upwards
template <typename T>T* b3AlignPointer(T* unalignedPtr, size_t alignment)
template <typename T>
T *b3AlignPointer(T *unalignedPtr, size_t alignment)
{
struct b3ConvertPointerSizeT
{
union
{
union {
T *ptr;
size_t integer;
};
};
b3ConvertPointerSizeT converter;
const size_t bit_mask = ~(alignment - 1);
converter.ptr = unalignedPtr;
converter.integer += alignment - 1;

8
thirdparty/bullet/Bullet3Common/b3StackAlloc.h vendored
View file

@ -34,8 +34,11 @@ struct b3Block
class b3StackAlloc
{
public:
b3StackAlloc(unsigned int size) { ctor();create(size); }
b3StackAlloc(unsigned int size)
{
ctor();
create(size);
}
~b3StackAlloc() { destroy(); }
inline void create(unsigned int size)
@ -57,7 +60,6 @@ public:
data = 0;
usedsize = 0;
}
}
int getAvailableMemory() const

40
thirdparty/bullet/Bullet3Common/b3Transform.h vendored
View file

@ -12,11 +12,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_TRANSFORM_H
#define B3_TRANSFORM_H
#include "b3Matrix3x3.h"
#ifdef B3_USE_DOUBLE_PRECISION
@ -25,20 +23,17 @@ subject to the following restrictions:
#define b3TransformData b3TransformFloatData
#endif
/**@brief The b3Transform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with b3Vector3, b3Quaternion and b3Matrix3x3 linear algebra classes. */
B3_ATTRIBUTE_ALIGNED16(class) b3Transform {
B3_ATTRIBUTE_ALIGNED16(class)
b3Transform
{
///Storage for the rotation
b3Matrix3x3 m_basis;
///Storage for the translation
b3Vector3 m_origin;
public:
/**@brief No initialization constructor */
b3Transform() {}
/**@brief Constructor from b3Quaternion (optional b3Vector3 )
@ -48,7 +43,8 @@ public:
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(q),
m_origin(c)
{}
{
}
/**@brief Constructor from b3Matrix3x3 (optional b3Vector3)
* @param b Rotation from Matrix
@ -57,7 +53,8 @@ public:
const b3Vector3& c = b3MakeVector3(b3Scalar(0), b3Scalar(0), b3Scalar(0)))
: m_basis(b),
m_origin(c)
{}
{
}
/**@brief Copy constructor */
B3_FORCE_INLINE b3Transform(const b3Transform& other)
: m_basis(other.m_basis),
@ -72,12 +69,12 @@ public:
return *this;
}
/**@brief Set the current transform as the value of the product of two transforms
* @param t1 Transform 1
* @param t2 Transform 2
* This = Transform1 * Transform2 */
B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2) {
B3_FORCE_INLINE void mult(const b3Transform& t1, const b3Transform& t2)
{
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
@ -118,13 +115,13 @@ public:
B3_FORCE_INLINE const b3Vector3& getOrigin() const { return m_origin; }
/**@brief Return a quaternion representing the rotation */
b3Quaternion getRotation() const {
b3Quaternion getRotation() const
{
b3Quaternion q;
m_basis.getRotation(q);
return q;
}
/**@brief Set from an array
* @param m A pointer to a 15 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void setFromOpenGLMatrix(const b3Scalar* m)
@ -153,7 +150,6 @@ public:
B3_FORCE_INLINE b3Vector3 invXform(const b3Vector3& inVec) const;
/**@brief Set the rotational element by b3Matrix3x3 */
B3_FORCE_INLINE void setBasis(const b3Matrix3x3& basis)
{
@ -166,7 +162,6 @@ public:
m_basis.setRotation(q);
}
/**@brief Set this transformation to the identity */
void setIdentity()
{
@ -214,10 +209,8 @@ public:
void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
void deSerializeFloat(const struct b3TransformFloatData& dataIn);
};
B3_FORCE_INLINE b3Vector3
b3Transform::invXform(const b3Vector3& inVec) const
{
@ -247,7 +240,6 @@ B3_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
t1.getOrigin() == t2.getOrigin());
}
///for serialization
struct b3TransformFloatData
{
@ -261,8 +253,6 @@ struct b3TransformDoubleData
b3Vector3DoubleData m_origin;
};
B3_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
@ -275,7 +265,6 @@ B3_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut)
m_origin.serializeFloat(dataOut.m_origin);
}
B3_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
@ -294,11 +283,4 @@ B3_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData&
m_origin.deSerializeDouble(dataIn.m_origin);
}
#endif //B3_TRANSFORM_H

22
thirdparty/bullet/Bullet3Common/b3TransformUtil.h vendored
View file

@ -12,16 +12,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_TRANSFORM_UTIL_H
#define B3_TRANSFORM_UTIL_H
#include "b3Transform.h"
#define B3_ANGULAR_MOTION_THRESHOLD b3Scalar(0.5) * B3_HALF_PI
B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents, const b3Vector3& supportDir)
{
return b3MakeVector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
@ -29,17 +25,10 @@ B3_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents,const b3Vec
supportDir.getZ() < b3Scalar(0.0) ? -halfExtents.getZ() : halfExtents.getZ());
}
/// Utils related to temporal transforms
class b3TransformUtil
{
public:
static void integrateTransform(const b3Transform& curTrans, const b3Vector3& linvel, const b3Vector3& angvel, b3Scalar timeStep, b3Transform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
@ -88,7 +77,8 @@ public:
{
calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
angVel = axis * angle / timeStep;
} else
}
else
{
angVel.setValue(0, 0, 0);
}
@ -137,10 +127,8 @@ public:
else
axis /= b3Sqrt(len);
}
};
///The b3ConvexSeparatingDistanceUtil can help speed up convex collision detection
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
class b3ConvexSeparatingDistanceUtil
@ -157,7 +145,6 @@ class b3ConvexSeparatingDistanceUtil
b3Scalar m_separatingDistance;
public:
b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA, b3Scalar boundingRadiusB)
: m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
@ -179,8 +166,6 @@ public:
if (m_separatingDistance > 0.f)
{
b3Vector3 linVelA, angVelA, linVelB, angVelB;
b3TransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, b3Scalar(1.), linVelA, angVelA);
b3TransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, b3Scalar(1.), linVelB, angVelB);
@ -220,9 +205,6 @@ public:
m_ornB = toOrnB;
}
}
};
#endif //B3_TRANSFORM_UTIL_H

102
thirdparty/bullet/Bullet3Common/b3Vector3.cpp vendored
View file

@ -30,7 +30,6 @@ typedef float float4 __attribute__ ((vector_size(16)));
#endif
//typedef uint32_t uint4 __attribute__ ((vector_size(16)));
#if defined B3_USE_SSE || defined _WIN32
#define LOG2_ARRAY_SIZE 6
@ -69,7 +68,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
float4 v0 = vertices[0];
float4 v1 = vertices[1];
float4 v2 = vertices[2];
float4 v3 = vertices[3]; vertices += 4;
float4 v3 = vertices[3];
vertices += 4;
float4 lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
float4 hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -90,7 +90,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -111,7 +112,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -132,7 +134,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -165,7 +168,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
// find first occurrence of that max
size_t test;
for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], max))); index++) // local_count must be a multiple of 4
{}
{
}
// record where it is.
maxIndex = 4 * index + segment + indexTable[test];
}
@ -178,7 +182,6 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
max = dotMax;
index = 0;
if (b3Unlikely(count > 16))
{
for (; index + 4 <= count / 4; index += 4)
@ -186,7 +189,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
float4 v0 = vertices[0];
float4 v1 = vertices[1];
float4 v2 = vertices[2];
float4 v3 = vertices[3]; vertices += 4;
float4 v3 = vertices[3];
vertices += 4;
float4 lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
float4 hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -207,7 +211,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -228,7 +233,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -249,7 +255,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -280,8 +287,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
vertices += localCount; // counter the offset
size_t byteIndex = -(localCount) * sizeof(float);
//AT&T Code style assembly
asm volatile
( ".align 4 \n\
asm volatile(
".align 4 \n\
0: movaps %[max], %[t2] // move max out of the way to avoid propagating NaNs in max \n\
movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\
movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\
@ -309,8 +316,7 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
"
: [max] "+x"(max), [t0] "=&x"(t0), [t1] "=&x"(t1), [t2] "=&x"(t2), [t3] "=&x"(t3), [t4] "=&x"(t4), [byteIndex] "+r"(byteIndex)
: [vLo] "x"(vLo), [vHi] "x"(vHi), [vertices] "r"(vertices), [sap] "r"(sap)
: "memory", "cc"
);
: "memory", "cc");
index += localCount / 4;
#else
{
@ -414,7 +420,8 @@ long b3_maxdot_large( const float *vv, const float *vec, unsigned long count, fl
// scan for the first occurence of max in the array
size_t test;
for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], max))); index++) // local_count must be a multiple of 4
{}
{
}
maxIndex = 4 * index + segment + indexTable[test];
}
@ -455,7 +462,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
float4 v0 = vertices[0];
float4 v1 = vertices[1];
float4 v2 = vertices[2];
float4 v3 = vertices[3]; vertices += 4;
float4 v3 = vertices[3];
vertices += 4;
float4 lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
float4 hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -476,7 +484,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -497,7 +506,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -518,7 +528,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -551,7 +562,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
// find first occurrence of that min
size_t test;
for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++) // local_count must be a multiple of 4
{}
{
}
// record where it is.
minIndex = 4 * index + segment + indexTable[test];
}
@ -564,7 +576,6 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
min = dotmin;
index = 0;
if (b3Unlikely(count > 16))
{
for (; index + 4 <= count / 4; index += 4)
@ -572,7 +583,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
float4 v0 = vertices[0];
float4 v1 = vertices[1];
float4 v2 = vertices[2];
float4 v3 = vertices[3]; vertices += 4;
float4 v3 = vertices[3];
vertices += 4;
float4 lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
float4 hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -593,7 +605,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -614,7 +627,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -635,7 +649,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
v0 = vertices[0];
v1 = vertices[1];
v2 = vertices[2];
v3 = vertices[3]; vertices += 4;
v3 = vertices[3];
vertices += 4;
lo0 = _mm_movelh_ps(v0, v1); // x0y0x1y1
hi0 = _mm_movehl_ps(v1, v0); // z0?0z1?1
@ -660,16 +675,14 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
size_t localCount = (count & -4L) - 4 * index;
if (localCount)
{
#ifdef __APPLE__
vertices += localCount; // counter the offset
float4 t0, t1, t2, t3, t4;
size_t byteIndex = -(localCount) * sizeof(float);
float4 *sap = &stack_array[index + localCount / 4];
asm volatile
( ".align 4 \n\
asm volatile(
".align 4 \n\
0: movaps %[min], %[t2] // move min out of the way to avoid propagating NaNs in min \n\
movaps (%[vertices], %[byteIndex], 4), %[t0] // vertices[0] \n\
movaps 16(%[vertices], %[byteIndex], 4), %[t1] // vertices[1] \n\
@ -697,8 +710,7 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
"
: [min] "+x"(min), [t0] "=&x"(t0), [t1] "=&x"(t1), [t2] "=&x"(t2), [t3] "=&x"(t3), [t4] "=&x"(t4), [byteIndex] "+r"(byteIndex)
: [vLo] "x"(vLo), [vHi] "x"(vHi), [vertices] "r"(vertices), [sap] "r"(sap)
: "memory", "cc"
);
: "memory", "cc");
index += localCount / 4;
#else
{
@ -803,7 +815,8 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
// scan for the first occurence of min in the array
size_t test;
for (index = 0; 0 == (test = _mm_movemask_ps(_mm_cmpeq_ps(stack_array[index], min))); index++) // local_count must be a multiple of 4
{}
{
}
minIndex = 4 * index + segment + indexTable[test];
}
@ -811,12 +824,10 @@ long b3_mindot_large( const float *vv, const float *vec, unsigned long count, fl
return minIndex;
}
#elif defined B3_USE_NEON
#define ARM_NEON_GCC_COMPATIBILITY 1
#include <arm_neon.h>
static long b3_maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult);
static long b3_maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult);
static long b3_maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult);
@ -827,7 +838,10 @@ static long b3_mindot_large_sel( const float *vv, const float *vec, unsigned lon
long (*b3_maxdot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = b3_maxdot_large_sel;
long (*b3_mindot_large)(const float *vv, const float *vec, unsigned long count, float *dotResult) = b3_mindot_large_sel;
extern "C" {int _get_cpu_capabilities( void );}
extern "C"
{
int _get_cpu_capabilities(void);
}
static long b3_maxdot_large_sel(const float *vv, const float *vec, unsigned long count, float *dotResult)
{
@ -849,11 +863,8 @@ static long b3_mindot_large_sel( const float *vv, const float *vec, unsigned lon
return b3_mindot_large(vv, vec, count, dotResult);
}
#define vld1q_f32_aligned_postincrement(_ptr) ({ float32x4_t _r; asm( "vld1.f32 {%0}, [%1, :128]!\n" : "=w" (_r), "+r" (_ptr) ); /*return*/ _r; })
long b3_maxdot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult)
{
unsigned long i = 0;
@ -1043,7 +1054,6 @@ long b3_maxdot_large_v0( const float *vv, const float *vec, unsigned long count,
return vget_lane_u32(iLo, 0);
}
long b3_maxdot_large_v1(const float *vv, const float *vec, unsigned long count, float *dotResult)
{
float32x4_t vvec = vld1q_f32_aligned_postincrement(vec);
@ -1139,7 +1149,8 @@ long b3_maxdot_large_v1( const float *vv, const float *vec, unsigned long count,
local_index = vaddq_u32(local_index, four);
}
switch (count & 3) {
switch (count & 3)
{
case 3:
{
float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
@ -1221,7 +1232,6 @@ long b3_maxdot_large_v1( const float *vv, const float *vec, unsigned long count,
break;
}
// select best answer between hi and lo results
uint32x2_t mask = vcgt_f32(vget_high_f32(maxDot), vget_low_f32(maxDot));
float32x2_t maxDot2 = vbsl_f32(mask, vget_high_f32(maxDot), vget_low_f32(maxDot));
@ -1236,7 +1246,6 @@ long b3_maxdot_large_v1( const float *vv, const float *vec, unsigned long count,
*dotResult = vget_lane_f32(maxDot2, 0);
return vget_lane_u32(index2, 0);
}
long b3_mindot_large_v0(const float *vv, const float *vec, unsigned long count, float *dotResult)
@ -1522,7 +1531,8 @@ long b3_mindot_large_v1( const float *vv, const float *vec, unsigned long count,
local_index = vaddq_u32(local_index, four);
}
switch (count & 3) {
switch (count & 3)
{
case 3:
{
float32x4_t v0 = vld1q_f32_aligned_postincrement(vv);
@ -1604,7 +1614,6 @@ long b3_mindot_large_v1( const float *vv, const float *vec, unsigned long count,
break;
}
// select best answer between hi and lo results
uint32x2_t mask = vclt_f32(vget_high_f32(minDot), vget_low_f32(minDot));
float32x2_t minDot2 = vbsl_f32(mask, vget_high_f32(minDot), vget_low_f32(minDot));
@ -1619,7 +1628,6 @@ long b3_mindot_large_v1( const float *vv, const float *vec, unsigned long count,
*dotResult = vget_lane_f32(minDot2, 0);
return vget_lane_u32(index2, 0);
}
#else
@ -1627,5 +1635,3 @@ long b3_mindot_large_v1( const float *vv, const float *vec, unsigned long count,
#endif
#endif /* __APPLE__ */

75
thirdparty/bullet/Bullet3Common/b3Vector3.h vendored
View file

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_VECTOR3_H
#define B3_VECTOR3_H
@ -38,7 +36,6 @@ subject to the following restrictions:
#pragma warning(disable : 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
#endif
#define B3_SHUFFLE(x, y, z, w) ((w) << 6 | (z) << 4 | (y) << 2 | (x))
//#define b3_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
#define b3_pshufd_ps(_a, _mask) _mm_shuffle_ps((_a), (_a), (_mask))
@ -53,8 +50,6 @@ subject to the following restrictions:
#define b3vxyzMaskf b3vFFF0fMask
#define b3vAbsfMask b3CastiTo128f(b3vAbsMask)
const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
const __m128 B3_ATTRIBUTE_ALIGNED16(b3v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
const __m128 B3_ATTRIBUTE_ALIGNED16(b3vHalf) = {0.5f, 0.5f, 0.5f, 0.5f};
@ -84,32 +79,34 @@ inline b3Vector3 b3MakeVector3(b3Scalar x,b3Scalar y,b3Scalar z);
inline b3Vector3 b3MakeVector3(b3Scalar x, b3Scalar y, b3Scalar z, b3Scalar w);
inline b3Vector4 b3MakeVector4(b3Scalar x, b3Scalar y, b3Scalar z, b3Scalar w);
/**@brief b3Vector3 can be used to represent 3D points and vectors.
* It has an un-used w component to suit 16-byte alignment when b3Vector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
* Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
*/
B3_ATTRIBUTE_ALIGNED16(class) b3Vector3
B3_ATTRIBUTE_ALIGNED16(class)
b3Vector3
{
public:
#if defined(B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM
union {
b3SimdFloat4 mVec128;
float m_floats[4];
struct {float x,y,z,w;};
struct
{
float x, y, z, w;
};
};
#else
union
{
union {
float m_floats[4];
struct {float x,y,z,w;};
struct
{
float x, y, z, w;
};
};
#endif
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
#if defined(B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM
@ -130,9 +127,6 @@ public:
#endif
public:
/**@brief Add a vector to this one
* @param The vector to add to this one */
B3_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
@ -149,7 +143,6 @@ public:
return *this;
}
/**@brief Subtract a vector from this one
* @param The vector to subtract */
B3_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
@ -299,7 +292,6 @@ public:
#endif
return *this;
#else
return *this /= length();
@ -422,8 +414,7 @@ public:
x = vadd_f32(x, vget_high_f32(V));
return vget_lane_f32(x, 0);
#else
return
m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
return m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) +
m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) +
m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
#endif
@ -453,7 +444,6 @@ public:
return absolute().maxAxis();
}
B3_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
@ -500,8 +490,7 @@ public:
return b3Vector3(vl);
#else
return
b3MakeVector3( m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
return b3MakeVector3(m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
#endif
@ -851,7 +840,6 @@ b3Dot(const b3Vector3& v1, const b3Vector3& v2)
return v1.dot(v2);
}
/**@brief Return the distance squared between two vectors */
B3_FORCE_INLINE b3Scalar
b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
@ -859,7 +847,6 @@ b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
return v1.distance2(v2);
}
/**@brief Return the distance between two vectors */
B3_FORCE_INLINE b3Scalar
b3Distance(const b3Vector3& v1, const b3Vector3& v2)
@ -897,8 +884,6 @@ b3Lerp(const b3Vector3& v1, const b3Vector3& v2, const b3Scalar& t)
return v1.lerp(v2, t);
}
B3_FORCE_INLINE b3Scalar b3Vector3::distance2(const b3Vector3& v) const
{
return (v - *this).length2();
@ -1044,16 +1029,9 @@ B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_
#endif
}
class b3Vector4 : public b3Vector3
{
public:
B3_FORCE_INLINE b3Vector4 absolute4() const
{
#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
@ -1069,10 +1047,8 @@ public:
#endif
}
b3Scalar getW() const { return m_floats[3]; }
B3_FORCE_INLINE int maxAxis4() const
{
int maxIndex = -1;
@ -1100,7 +1076,6 @@ public:
return maxIndex;
}
B3_FORCE_INLINE int minAxis4() const
{
int minIndex = -1;
@ -1129,22 +1104,17 @@ public:
return minIndex;
}
B3_FORCE_INLINE int closestAxis4() const
{
return absolute4().maxAxis4();
}
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
/* void getValue(b3Scalar *m) const
{
m[0] = m_floats[0];
@ -1165,11 +1135,8 @@ public:
m_floats[2] = _z;
m_floats[3] = _w;
}
};
///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
B3_FORCE_INLINE void b3SwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal)
{
@ -1200,13 +1167,11 @@ B3_FORCE_INLINE void b3SwapVector3Endian(const b3Vector3& sourceVec, b3Vector3&
{
b3SwapScalarEndian(sourceVec[i], destVec[i]);
}
}
///b3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
B3_FORCE_INLINE void b3UnSwapVector3Endian(b3Vector3& vector)
{
b3Vector3 swappedVec;
for (int i = 0; i < 4; i++)
{
@ -1218,7 +1183,8 @@ B3_FORCE_INLINE void b3UnSwapVector3Endian(b3Vector3& vector)
template <class T>
B3_FORCE_INLINE void b3PlaneSpace1(const T& n, T& p, T& q)
{
if (b3Fabs(n[2]) > B3_SQRT12) {
if (b3Fabs(n[2]) > B3_SQRT12)
{
// choose p in y-z plane
b3Scalar a = n[1] * n[1] + n[2] * n[2];
b3Scalar k = b3RecipSqrt(a);
@ -1230,7 +1196,8 @@ B3_FORCE_INLINE void b3PlaneSpace1 (const T& n, T& p, T& q)
q[1] = -n[0] * p[2];
q[2] = n[0] * p[1];
}
else {
else
{
// choose p in x-y plane
b3Scalar a = n[0] * n[0] + n[1] * n[1];
b3Scalar k = b3RecipSqrt(a);
@ -1244,7 +1211,6 @@ B3_FORCE_INLINE void b3PlaneSpace1 (const T& n, T& p, T& q)
}
}
struct b3Vector3FloatData
{
float m_floats[4];
@ -1253,7 +1219,6 @@ struct b3Vector3FloatData
struct b3Vector3DoubleData
{
double m_floats[4];
};
B3_FORCE_INLINE void b3Vector3::serializeFloat(struct b3Vector3FloatData& dataOut) const
@ -1269,7 +1234,6 @@ B3_FORCE_INLINE void b3Vector3::deSerializeFloat(const struct b3Vector3FloatData
m_floats[i] = b3Scalar(dataIn.m_floats[i]);
}
B3_FORCE_INLINE void b3Vector3::serializeDouble(struct b3Vector3DoubleData& dataOut) const
{
///could also do a memcpy, check if it is worth it
@ -1283,7 +1247,6 @@ B3_FORCE_INLINE void b3Vector3::deSerializeDouble(const struct b3Vector3DoubleDa
m_floats[i] = b3Scalar(dataIn.m_floats[i]);
}
B3_FORCE_INLINE void b3Vector3::serialize(struct b3Vector3Data& dataOut) const
{
///could also do a memcpy, check if it is worth it
@ -1297,9 +1260,6 @@ B3_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn)
m_floats[i] = dataIn.m_floats[i];
}
inline b3Vector3 b3MakeVector3(b3Scalar x, b3Scalar y, b3Scalar z)
{
b3Vector3 tmp;
@ -1340,5 +1300,4 @@ inline b3Vector4 b3MakeVector4(b3SimdFloat4 vec)
#endif
#endif //B3_VECTOR3_H

7
thirdparty/bullet/Bullet3Common/shared/b3Float4.h vendored
View file

@ -33,8 +33,6 @@
return tmp;
}
#else
typedef float4 b3Float4;
#define b3Float4ConstArg const b3Float4
@ -58,8 +56,6 @@
#endif
inline bool b3IsAlmostZero(b3Float4ConstArg v)
{
if (b3Fabs(v.x) > 1e-6 || b3Fabs(v.y) > 1e-6 || b3Fabs(v.z) > 1e-6)
@ -67,7 +63,6 @@ inline bool b3IsAlmostZero(b3Float4ConstArg v)
return true;
}
inline int b3MaxDot(b3Float4ConstArg vec, __global const b3Float4* vecArray, int vecLen, float* dotOut)
{
float maxDot = -B3_INFINITY;
@ -92,6 +87,4 @@ inline int b3MaxDot( b3Float4ConstArg vec, __global const b3Float4* vecArray,
return ptIndex;
}
#endif //B3_FLOAT4_H

9
thirdparty/bullet/Bullet3Common/shared/b3Int2.h vendored
View file

@ -20,8 +20,7 @@ subject to the following restrictions:
struct b3UnsignedInt2
{
union
{
union {
struct
{
unsigned int x, y;
@ -35,8 +34,7 @@ struct b3UnsignedInt2
struct b3Int2
{
union
{
union {
struct
{
int x, y;
@ -51,7 +49,8 @@ struct b3Int2
inline b3Int2 b3MakeInt2(int x, int y)
{
b3Int2 v;
v.s[0] = x; v.s[1] = y;
v.s[0] = x;
v.s[1] = y;
return v;
}
#else

25
thirdparty/bullet/Bullet3Common/shared/b3Int4.h vendored
View file

@ -5,13 +5,12 @@
#include "Bullet3Common/b3Scalar.h"
B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
B3_ATTRIBUTE_ALIGNED16(struct)
b3UnsignedInt4
{
B3_DECLARE_ALIGNED_ALLOCATOR();
union
{
union {
struct
{
unsigned int x, y, z, w;
@ -23,12 +22,12 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
};
};
B3_ATTRIBUTE_ALIGNED16(struct) b3Int4
B3_ATTRIBUTE_ALIGNED16(struct)
b3Int4
{
B3_DECLARE_ALIGNED_ALLOCATOR();
union
{
union {
struct
{
int x, y, z, w;
@ -43,26 +42,30 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3Int4
B3_FORCE_INLINE b3Int4 b3MakeInt4(int x, int y, int z, int w = 0)
{
b3Int4 v;
v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w;
v.s[0] = x;
v.s[1] = y;
v.s[2] = z;
v.s[3] = w;
return v;
}
B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y, unsigned int z, unsigned int w = 0)
{
b3UnsignedInt4 v;
v.s[0] = x; v.s[1] = y; v.s[2] = z; v.s[3] = w;
v.s[0] = x;
v.s[1] = y;
v.s[2] = z;
v.s[3] = w;
return v;
}
#else
#define b3UnsignedInt4 uint4
#define b3Int4 int4
#define b3MakeInt4 (int4)
#define b3MakeUnsignedInt4 (uint4)
#endif //__cplusplus
#endif //B3_INT4_H

48
thirdparty/bullet/Bullet3Common/shared/b3Mat3x3.h vendored
View file

@ -4,7 +4,6 @@
#include "Bullet3Common/shared/b3Quat.h"
#ifdef __cplusplus
#include "Bullet3Common/b3Matrix3x3.h"
@ -24,13 +23,11 @@ inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg mat)
#define b3GetRow(m, row) m.getRow(row)
__inline
b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
__inline b3Float4 mtMul3(b3Float4ConstArg a, b3Mat3x3ConstArg b)
{
return b * a;
}
#else
typedef struct
@ -73,27 +70,19 @@ inline b3Mat3x3 b3AbsoluteMat3x3(b3Mat3x3ConstArg matIn)
return out;
}
__inline b3Mat3x3 mtZero();
__inline
b3Mat3x3 mtZero();
__inline b3Mat3x3 mtIdentity();
__inline
b3Mat3x3 mtIdentity();
__inline b3Mat3x3 mtTranspose(b3Mat3x3 m);
__inline
b3Mat3x3 mtTranspose(b3Mat3x3 m);
__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
__inline
b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b);
__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
__inline
b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b);
__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
__inline
b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b);
__inline
b3Mat3x3 mtZero()
__inline b3Mat3x3 mtZero()
{
b3Mat3x3 m;
m.m_row[0] = (b3Float4)(0.f);
@ -102,8 +91,7 @@ b3Mat3x3 mtZero()
return m;
}
__inline
b3Mat3x3 mtIdentity()
__inline b3Mat3x3 mtIdentity()
{
b3Mat3x3 m;
m.m_row[0] = (b3Float4)(1, 0, 0, 0);
@ -112,8 +100,7 @@ b3Mat3x3 mtIdentity()
return m;
}
__inline
b3Mat3x3 mtTranspose(b3Mat3x3 m)
__inline b3Mat3x3 mtTranspose(b3Mat3x3 m)
{
b3Mat3x3 out;
out.m_row[0] = (b3Float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
@ -122,8 +109,7 @@ b3Mat3x3 mtTranspose(b3Mat3x3 m)
return out;
}
__inline
b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
__inline b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
{
b3Mat3x3 transB;
transB = mtTranspose(b);
@ -143,8 +129,7 @@ b3Mat3x3 mtMul(b3Mat3x3 a, b3Mat3x3 b)
return ans;
}
__inline
b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
__inline b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
{
b3Float4 ans;
ans.x = b3Dot3F4(a.m_row[0], b);
@ -154,8 +139,7 @@ b3Float4 mtMul1(b3Mat3x3 a, b3Float4 b)
return ans;
}
__inline
b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
__inline b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
{
b3Float4 colx = b3MakeFloat4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
b3Float4 coly = b3MakeFloat4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
@ -168,12 +152,6 @@ b3Float4 mtMul3(b3Float4 a, b3Mat3x3 b)
return ans;
}
#endif
#endif //B3_MAT3x3_H

3
thirdparty/bullet/Bullet3Common/shared/b3Quat.h vendored
View file

@ -27,7 +27,6 @@
typedef float4 b3Quat;
#define b3QuatConstArg const b3Quat
inline float4 b3FastNormalize4(float4 v)
{
v = (float4)(v.xyz, 0.f);
@ -76,8 +75,6 @@ inline float4 b3QuatRotate(b3QuatConstArg q, b3QuatConstArg vec)
return out;
}
inline b3Quat b3QuatInverse(b3QuatConstArg q)
{
return (b3Quat)(-q.xyz, q.w);

10
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h vendored
View file

@ -34,8 +34,6 @@ enum b3SolverMode
struct b3ContactSolverInfoData
{
b3Scalar m_tau;
b3Scalar m_damping; //global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
b3Scalar m_friction;
@ -58,15 +56,10 @@ struct b3ContactSolverInfoData
int m_minimumSolverBatchSize;
b3Scalar m_maxGyroscopicForce;
b3Scalar m_singleAxisRollingFrictionThreshold;
};
struct b3ContactSolverInfo : public b3ContactSolverInfoData
{
inline b3ContactSolverInfo()
{
m_tau = b3Scalar(0.6);
@ -120,7 +113,6 @@ struct b3ContactSolverInfoDoubleData
int m_minimumSolverBatchSize;
int m_splitImpulse;
char m_padding[4];
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3ContactSolverInfoFloatData
@ -154,6 +146,4 @@ struct b3ContactSolverInfoFloatData
char m_padding[4];
};
#endif //B3_CONTACT_SOLVER_INFO

5
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.cpp vendored
View file

@ -4,21 +4,18 @@
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
b3FixedConstraint::b3FixedConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB)
: b3TypedConstraint(B3_FIXED_CONSTRAINT_TYPE, rbA, rbB)
{
m_pivotInA = frameInA.getOrigin();
m_pivotInB = frameInB.getOrigin();
m_relTargetAB = frameInA.getRotation() * frameInB.getRotation().inverse();
}
b3FixedConstraint::~b3FixedConstraint()
{
}
void b3FixedConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
info->m_numConstraintRows = 6;
@ -91,7 +88,6 @@ void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData
info->m_J2angularAxis[start_index + s * 2 + 2] = -1;
}
// set right hand side for the angular dofs
b3Vector3 diff;
@ -104,5 +100,4 @@ void b3FixedConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyData
{
info->m_constraintError[(3 + j) * info->rowskip] = k * diff[j];
}
}

5
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h vendored
View file

@ -4,7 +4,8 @@
#include "b3TypedConstraint.h"
B3_ATTRIBUTE_ALIGNED16(class) b3FixedConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3FixedConstraint : public b3TypedConstraint
{
b3Vector3 m_pivotInA;
b3Vector3 m_pivotInB;
@ -15,7 +16,6 @@ public:
virtual ~b3FixedConstraint();
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
virtual void getInfo2(b3ConstraintInfo2 * info, const b3RigidBodyData* bodies);
@ -29,7 +29,6 @@ public:
b3Assert(0);
return 0.f;
}
};
#endif //B3_FIXED_CONSTRAINT_H

80
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.cpp vendored
View file

@ -26,36 +26,17 @@ http://gimpact.sf.net
#include "Bullet3Common/b3TransformUtil.h"
#include <new>
#define D6_USE_OBSOLETE_METHOD false
#define D6_USE_FRAME_OFFSET true
b3Generic6DofConstraint::b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB)
, m_frameInA(frameInA)
, m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA),
m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
m_flags(0)
: b3TypedConstraint(B3_D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0)
{
calculateTransforms(bodies);
}
#define GENERIC_D6_DISABLE_WARMSTARTING 1
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index);
b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
{
@ -64,8 +45,6 @@ b3Scalar btGetMatrixElem(const b3Matrix3x3& mat, int index)
return mat[i][j];
}
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz);
bool matrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
@ -136,20 +115,12 @@ int b3RotationalLimitMotor::testLimitValue(b3Scalar test_value)
m_currentLimit = 0; //Free from violation
return 0;
}
//////////////////////////// End b3RotationalLimitMotor ////////////////////////////////////
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_value)
{
b3Scalar loLimit = m_lowerLimit[limitIndex];
@ -179,8 +150,6 @@ int b3TranslationalLimitMotor::testLimitValue(int limitIndex, b3Scalar test_valu
return 0;
}
//////////////////////////// b3TranslationalLimitMotor ////////////////////////////////////
void b3Generic6DofConstraint::calculateAngleInfo()
@ -211,7 +180,6 @@ void b3Generic6DofConstraint::calculateAngleInfo()
m_calculatedAxis[0].normalize();
m_calculatedAxis[1].normalize();
m_calculatedAxis[2].normalize();
}
static b3Transform getCenterOfMassTransform(const b3RigidBodyData& body)
@ -253,12 +221,6 @@ void b3Generic6DofConstraint::calculateTransforms(const b3Transform& transA,cons
}
}
bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
{
b3Scalar angle = m_calculatedAxisAngleDiff[axis_index];
@ -269,9 +231,6 @@ bool b3Generic6DofConstraint::testAngularLimitMotor(int axis_index)
return m_angularLimits[axis_index].needApplyTorques();
}
void b3Generic6DofConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
//prepare constraint
@ -307,10 +266,8 @@ void b3Generic6DofConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const
info->nub = 0;
}
void b3Generic6DofConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
b3Transform transA = getCenterOfMassTransform(bodies[m_rbA]);
b3Transform transB = getCenterOfMassTransform(bodies[m_rbB]);
const b3Vector3& linVelA = bodies[m_rbA].m_linVel;
@ -328,13 +285,10 @@ void b3Generic6DofConstraint::getInfo2 (b3ConstraintInfo2* info,const b3RigidBod
int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
}
}
void b3Generic6DofConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies)
{
//prepare constraint
calculateTransforms(transA, transB, bodies);
@ -356,8 +310,6 @@ void b3Generic6DofConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const
}
}
int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
{
// int row = 0;
@ -404,8 +356,6 @@ int b3Generic6DofConstraint::setLinearLimits(b3ConstraintInfo2* info, int row, c
return row;
}
int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2* info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB)
{
b3Generic6DofConstraint* d6constraint = this;
@ -437,16 +387,11 @@ int b3Generic6DofConstraint::setAngularLimits(b3ConstraintInfo2 *info, int row_o
return row;
}
void b3Generic6DofConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
}
void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Transform& frameB, const b3RigidBodyData* bodies)
{
m_frameInA = frameA;
@ -455,27 +400,21 @@ void b3Generic6DofConstraint::setFrames(const b3Transform& frameA, const b3Trans
calculateTransforms(bodies);
}
b3Vector3 b3Generic6DofConstraint::getAxis(int axis_index) const
{
return m_calculatedAxis[axis_index];
}
b3Scalar b3Generic6DofConstraint::getRelativePivotPosition(int axisIndex) const
{
return m_calculatedLinearDiff[axisIndex];
}
b3Scalar b3Generic6DofConstraint::getAngle(int axisIndex) const
{
return m_calculatedAxisAngleDiff[axisIndex];
}
void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
{
b3Scalar imA = bodies[m_rbA].m_invMass;
@ -495,8 +434,6 @@ void b3Generic6DofConstraint::calcAnchorPos(const b3RigidBodyData* bodies)
return;
}
void b3Generic6DofConstraint::calculateLinearInfo()
{
m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
@ -508,8 +445,6 @@ void b3Generic6DofConstraint::calculateLinearInfo()
}
}
int b3Generic6DofConstraint::get_limit_motor_info2(
b3RotationalLimitMotor* limot,
const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB,
@ -566,7 +501,8 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
int i;
for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
} else
}
else
{
b3Vector3 ltd; // Linear Torque Decoupling vector
b3Vector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
@ -676,14 +612,10 @@ int b3Generic6DofConstraint::get_limit_motor_info2(
}
return 1;
}
else return 0;
else
return 0;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
///If no axis is provided, it uses the default axis for this constraint.
void b3Generic6DofConstraint::setParam(int num, b3Scalar value, int axis)
@ -785,8 +717,6 @@ b3Scalar b3Generic6DofConstraint::getParam(int num, int axis) const
return retVal;
}
void b3Generic6DofConstraint::setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies)
{
b3Vector3 zAxis = axis1.normalized();

37
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h vendored
View file

@ -23,7 +23,6 @@ email: projectileman@yahoo.com
http://gimpact.sf.net
*/
#ifndef B3_GENERIC_6DOF_CONSTRAINT_H
#define B3_GENERIC_6DOF_CONSTRAINT_H
@ -33,9 +32,6 @@ http://gimpact.sf.net
struct b3RigidBodyData;
//! Rotation Limit structure for generic joints
class b3RotationalLimitMotor
{
@ -100,8 +96,6 @@ public:
m_enableMotor = limot.m_enableMotor;
}
//! Is limited
bool isLimited()
{
@ -124,11 +118,8 @@ public:
//! apply the correction impulses for two bodies
b3Scalar solveAngularLimits(b3Scalar timeStep, b3Vector3& axis, b3Scalar jacDiagABInv, b3RigidBodyData* body0, b3RigidBodyData* body1);
};
class b3TranslationalLimitMotor
{
public:
@ -210,7 +201,6 @@ public:
}
int testLimitValue(int limitIndex, b3Scalar test_value);
b3Scalar solveLinearAxis(
b3Scalar timeStep,
b3Scalar jacDiagABInv,
@ -219,8 +209,6 @@ public:
int limit_index,
const b3Vector3& axis_normal_on_a,
const b3Vector3& anchorPos);
};
enum b36DofFlags
@ -231,7 +219,6 @@ enum b36DofFlags
};
#define B3_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
/// b3Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
/*!
b3Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
@ -268,10 +255,10 @@ This brings support for limit parameters and motors. </li>
</ul>
*/
B3_ATTRIBUTE_ALIGNED16(class) b3Generic6DofConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3Generic6DofConstraint : public b3TypedConstraint
{
protected:
//! relative_frames
//!@{
b3Transform m_frameInA; //!< the constraint space w.r.t body A
@ -289,13 +276,11 @@ protected:
b3TranslationalLimitMotor m_linearLimits;
//!@}
//! hinge_parameters
//!@{
b3RotationalLimitMotor m_angularLimits[3];
//!@}
protected:
//! temporal variables
//!@{
@ -325,22 +310,17 @@ protected:
return *this;
}
int setAngularLimits(b3ConstraintInfo2 * info, int row_offset, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
int setLinearLimits(b3ConstraintInfo2 * info, int row, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB);
// tests linear limits
void calculateLinearInfo();
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Generic6DofConstraint(int rbA, int rbB, const b3Transform& frameInA, const b3Transform& frameInB, bool useLinearReferenceFrameA, const b3RigidBodyData* bodies);
@ -382,7 +362,6 @@ public:
return m_frameInB;
}
b3Transform& getFrameOffsetA()
{
return m_frameInA;
@ -393,8 +372,6 @@ public:
return m_frameInB;
}
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
@ -403,7 +380,6 @@ public:
void getInfo2NonVirtual(b3ConstraintInfo2 * info, const b3Transform& transA, const b3Transform& transB, const b3Vector3& linVelA, const b3Vector3& linVelB, const b3Vector3& angVelA, const b3Vector3& angVelB, const b3RigidBodyData* bodies);
void updateRHS(b3Scalar timeStep);
//! Get the rotation axis in global coordinates
@ -515,7 +491,6 @@ public:
if (limitIndex < 3)
{
return m_linearLimits.isLimited(limitIndex);
}
return m_angularLimits[limitIndex - 3].isLimited();
}
@ -537,14 +512,6 @@ public:
virtual b3Scalar getParam(int num, int axis = -1) const;
void setAxis(const b3Vector3& axis1, const b3Vector3& axis2, const b3RigidBodyData* bodies);
};
#endif //B3_GENERIC_6DOF_CONSTRAINT_H

11
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3JacobianEntry.h vendored
View file

@ -18,7 +18,6 @@ subject to the following restrictions:
#include "Bullet3Common/b3Matrix3x3.h"
//notes:
// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
// which makes the b3JacobianEntry memory layout 16 bytes
@ -27,7 +26,8 @@ subject to the following restrictions:
/// Jacobian entry is an abstraction that allows to describe constraints
/// it can be used in combination with a constraint solver
/// Can be used to relate the effect of an impulse to the constraint error
B3_ATTRIBUTE_ALIGNED16(class) b3JacobianEntry
B3_ATTRIBUTE_ALIGNED16(class)
b3JacobianEntry
{
public:
b3JacobianEntry(){};
@ -74,9 +74,7 @@ public:
const b3Vector3& axisInB,
const b3Vector3& inertiaInvA,
const b3Vector3& inertiaInvB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)))
, m_aJ(axisInA)
, m_bJ(-axisInB)
: m_linearJointAxis(b3MakeVector3(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.))), m_aJ(axisInA), m_bJ(-axisInB)
{
m_0MinvJt = inertiaInvA * m_aJ;
m_1MinvJt = inertiaInvB * m_bJ;
@ -114,8 +112,6 @@ public:
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
b3Scalar getNonDiagonal(const b3JacobianEntry& jacB, const b3Scalar massInvA, const b3Scalar massInvB) const
{
@ -149,7 +145,6 @@ public:
b3Vector3 m_1MinvJt;
//Optimization: can be stored in the w/last component of one of the vectors
b3Scalar m_Adiag;
};
#endif //B3_JACOBIAN_ENTRY_H

235
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp vendored
View file

@ -33,7 +33,6 @@ subject to the following restrictions:
//#include "../../dynamics/basic_demo/Stubs/AdlContact4.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
static b3Transform getWorldTransform(b3RigidBodyData* rb)
@ -49,8 +48,6 @@ static const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaData* inertia)
return inertia->m_invInertiaWorld;
}
static const b3Vector3& getLinearVelocity(b3RigidBodyData* rb)
{
return rb->m_linVel;
@ -65,7 +62,6 @@ static b3Vector3 getVelocityInLocalPoint(b3RigidBodyData* rb, const b3Vector3& r
{
//we also calculate lin/ang velocity for kinematic objects
return getLinearVelocity(rb) + getAngularVelocity(rb).cross(rel_pos);
}
struct b3ContactPoint
@ -130,7 +126,6 @@ void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& poin
pointOut.m_lateralFrictionDir2 = l2;
pointOut.m_lateralFrictionInitialized = true;
b3Vector3 worldPosB = contact->m_worldPosB[contactIndex];
pointOut.m_positionWorldOnB = worldPosB;
pointOut.m_positionWorldOnA = worldPosB + normalOnB * pointOut.m_distance;
@ -146,7 +141,6 @@ b3PgsJacobiSolver::b3PgsJacobiSolver(bool usePgs)
m_numSplitImpulseRecoveries(0),
m_btSeed2(0)
{
}
b3PgsJacobiSolver::~b3PgsJacobiSolver()
@ -166,16 +160,12 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyData* bodies, b3
//if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
//if ((infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & B3_SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
solveGroup(bodies, inertias, numBodies, contacts, numContacts, constraints, numConstraints, infoGlobal);
if (!numContacts)
return;
}
/// b3PgsJacobiSolver Sequentially applies impulses
b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies,
b3InertiaData* inertias,
@ -186,7 +176,6 @@ b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies,
int numConstraints,
const b3ContactSolverInfo& infoGlobal)
{
B3_PROFILE("solveGroup");
//you need to provide at least some bodies
@ -199,14 +188,6 @@ b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyData* bodies,
return 0.f;
}
#ifdef USE_SIMD
#include <emmintrin.h>
#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e, e, e, e))
@ -335,7 +316,6 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body
body2.internalApplyImpulse(-c.m_contactNormal * body2.internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
}
void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
b3SolverBody& body1,
b3SolverBody& body2,
@ -400,16 +380,12 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
#endif
}
unsigned long b3PgsJacobiSolver::b3Rand2()
{
m_btSeed2 = (1664525L * m_btSeed2 + 1013904223L) & 0xffffffff;
return m_btSeed2;
}
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
int b3PgsJacobiSolver::b3RandInt2(int n)
{
@ -419,15 +395,20 @@ int b3PgsJacobiSolver::b3RandInt2 (int n)
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
if (un <= 0x00010000UL) {
if (un <= 0x00010000UL)
{
r ^= (r >> 16);
if (un <= 0x00000100UL) {
if (un <= 0x00000100UL)
{
r ^= (r >> 8);
if (un <= 0x00000010UL) {
if (un <= 0x00000010UL)
{
r ^= (r >> 4);
if (un <= 0x00000004UL) {
if (un <= 0x00000004UL)
{
r ^= (r >> 2);
if (un <= 0x00000002UL) {
if (un <= 0x00000002UL)
{
r ^= (r >> 1);
}
}
@ -438,11 +419,8 @@ int b3PgsJacobiSolver::b3RandInt2 (int n)
return (int)(r % un);
}
void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyData* rb)
{
solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f);
solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f);
solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
@ -457,7 +435,8 @@ void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody,
solverBody->m_linearFactor = b3MakeVector3(1, 1, 1);
solverBody->m_linearVelocity = getLinearVelocity(rb);
solverBody->m_angularVelocity = getAngularVelocity(rb);
} else
}
else
{
solverBody->m_worldTransform.setIdentity();
solverBody->internalSetInvMass(b3MakeVector3(0, 0, 0));
@ -467,30 +446,16 @@ void b3PgsJacobiSolver::initSolverBody(int bodyIndex, b3SolverBody* solverBody,
solverBody->m_linearVelocity.setValue(0, 0, 0);
solverBody->m_angularVelocity.setValue(0, 0, 0);
}
}
b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitution)
{
b3Scalar rest = restitution * -rel_vel;
return rest;
}
void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
solverConstraint.m_contactNormal = normalAxis;
b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
@ -498,7 +463,6 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
b3RigidBodyData* body0 = &bodies[solverBodyA.m_originalBodyIndex];
b3RigidBodyData* body1 = &bodies[solverBodyB.m_originalBodyIndex];
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
@ -540,7 +504,8 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
if (m_usePgs)
{
scaledDenom = denom = relaxation / (denom0 + denom1);
} else
}
else
{
denom = relaxation / (denom0 + denom1);
b3Scalar countA = body0->m_invMass ? b3Scalar(m_bodyCount[solverBodyA.m_originalBodyIndex]) : 1.f;
@ -553,13 +518,9 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
}
{
b3Scalar rel_vel;
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0));
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0));
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0));
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0));
rel_vel = vel1Dotn + vel2Dotn;
@ -571,7 +532,6 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyData* bodies,b3Inerti
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
@ -584,7 +544,6 @@ b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyData* bo
return solverConstraint;
}
void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2,
b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation,
@ -593,7 +552,6 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b
{
b3Vector3 normalAxis = b3MakeVector3(0, 0, 0);
solverConstraint.m_contactNormal = normalAxis;
b3SolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
@ -621,7 +579,6 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b
solverConstraint.m_angularComponentB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * ftorqueAxis1 : b3MakeVector3(0, 0, 0);
}
{
b3Vector3 iMJaA = body0 ? getInvInertiaTensorWorld(&inertias[solverBodyA.m_originalBodyIndex]) * solverConstraint.m_relpos1CrossNormal : b3MakeVector3(0, 0, 0);
b3Vector3 iMJaB = body1 ? getInvInertiaTensorWorld(&inertias[solverBodyB.m_originalBodyIndex]) * solverConstraint.m_relpos2CrossNormal : b3MakeVector3(0, 0, 0);
@ -632,13 +589,9 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b
}
{
b3Scalar rel_vel;
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:b3MakeVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:b3MakeVector3(0,0,0));
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:b3MakeVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:b3MakeVector3(0,0,0));
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0 ? solverBodyA.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(body0 ? solverBodyA.m_angularVelocity : b3MakeVector3(0, 0, 0));
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1 ? solverBodyB.m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(body1 ? solverBodyB.m_angularVelocity : b3MakeVector3(0, 0, 0));
rel_vel = vel1Dotn + vel2Dotn;
@ -650,17 +603,9 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyData* bodies,b
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
@ -670,7 +615,6 @@ b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyD
return solverConstraint;
}
int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodies, b3InertiaData* inertias)
{
//b3Assert(bodyIndex< m_tmpSolverBodyPool.size());
@ -686,11 +630,13 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodie
initSolverBody(bodyIndex, &solverBody, &body);
solverBody.m_originalBodyIndex = bodyIndex;
m_bodyCount[bodyIndex] = curIndex;
} else
}
else
{
curIndex = m_bodyCount[bodyIndex];
}
} else
}
else
{
b3Assert(m_bodyCount[bodyIndex] > 0);
m_bodyCountCheck[bodyIndex]++;
@ -702,18 +648,15 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyData* bodie
b3Assert(curIndex >= 0);
return curIndex;
}
#include <stdio.h>
void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3Vector3& rel_pos1, b3Vector3& rel_pos2)
{
const b3Vector3& pos1 = cp.getPositionWorldOnA();
const b3Vector3& pos2 = cp.getPositionWorldOnB();
@ -756,12 +699,12 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
}
#endif //COMPUTE_IMPULSE_DENOM
b3Scalar denom;
if (m_usePgs)
{
scaledDenom = denom = relaxation / (denom0 + denom1);
} else
}
else
{
denom = relaxation / (denom0 + denom1);
@ -789,11 +732,8 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
vel = vel1 - vel2;
rel_vel = cp.m_normalWorldOnB.dot(vel);
solverConstraint.m_friction = cp.m_combinedFriction;
restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
if (restitution <= b3Scalar(0.))
{
@ -801,7 +741,6 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
};
}
///warm starting (or zero if disabled)
if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
{
@ -810,7 +749,8 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
bodyA->internalApplyImpulse(solverConstraint.m_contactNormal * bodyA->internalGetInvMass(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
if (rb1)
bodyB->internalApplyImpulse(solverConstraint.m_contactNormal * bodyB->internalGetInvMass(), -solverConstraint.m_angularComponentB, -(b3Scalar)solverConstraint.m_appliedImpulse);
} else
}
else
{
solverConstraint.m_appliedImpulse = 0.f;
}
@ -818,16 +758,13 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
solverConstraint.m_appliedPushImpulse = 0.f;
{
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:b3MakeVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:b3MakeVector3(0,0,0));
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:b3MakeVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:b3MakeVector3(0,0,0));
b3Scalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0 ? bodyA->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? bodyA->m_angularVelocity : b3MakeVector3(0, 0, 0));
b3Scalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1 ? bodyB->m_linearVelocity : b3MakeVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? bodyB->m_angularVelocity : b3MakeVector3(0, 0, 0));
b3Scalar rel_vel = vel1Dotn + vel2Dotn;
b3Scalar positionalError = 0.f;
b3Scalar velocityError = restitution - rel_vel; // * damping;
b3Scalar erp = infoGlobal.m_erp2;
if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
{
@ -839,7 +776,8 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
} else
}
else
{
positionalError = -penetration * erp / infoGlobal.m_timeStep;
}
@ -852,8 +790,8 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
//combine position and velocity into rhs
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
} else
}
else
{
//split position and velocity into rhs and m_rhsPenetration
solverConstraint.m_rhs = velocityImpulse;
@ -863,23 +801,15 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyData* bodies, b3Inerti
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
void b3PgsJacobiSolver::setFrictionConstraintImpulse(b3RigidBodyData* bodies, b3InertiaData* inertias, b3SolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
{
b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
{
b3SolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
if (infoGlobal.m_solverMode & B3_SOLVER_USE_WARMSTARTING)
@ -889,7 +819,8 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b
bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint1.m_angularComponentA, frictionConstraint1.m_appliedImpulse);
if (bodies[bodyB->m_originalBodyIndex].m_invMass)
bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint1.m_angularComponentB, -(b3Scalar)frictionConstraint1.m_appliedImpulse);
} else
}
else
{
frictionConstraint1.m_appliedImpulse = 0.f;
}
@ -905,21 +836,18 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyData* bodies, b
bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyA->m_originalBodyIndex].m_invMass, frictionConstraint2.m_angularComponentA, frictionConstraint2.m_appliedImpulse);
if (bodies[bodyB->m_originalBodyIndex].m_invMass)
bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal * bodies[bodyB->m_originalBodyIndex].m_invMass, -frictionConstraint2.m_angularComponentB, -(b3Scalar)frictionConstraint2.m_appliedImpulse);
} else
}
else
{
frictionConstraint2.m_appliedImpulse = 0.f;
}
}
}
void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal)
{
b3RigidBodyData *colObj0 = 0, *colObj1 = 0;
int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(), bodies, inertias);
int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(), bodies, inertias);
@ -929,8 +857,6 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
///avoid collision response between two static objects
if (solverBodyA->m_invMass.isZero() && solverBodyB->m_invMass.isZero())
return;
@ -939,7 +865,6 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
int numContacts = getNumContacts(manifold);
for (int j = 0; j < numContacts; j++)
{
b3ContactPoint cp;
getContactPoint(manifold, j, cp);
@ -983,8 +908,8 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
relAngVel.normalize();
if (relAngVel.length() > 0.001)
addRollingFrictionConstraint(bodies, inertias, relAngVel, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
} else
}
else
{
addRollingFrictionConstraint(bodies, inertias, cp.m_normalWorldOnB, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
b3Vector3 axis0, axis1;
@ -993,7 +918,6 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
addRollingFrictionConstraint(bodies, inertias, axis0, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
if (axis1.length() > 0.001)
addRollingFrictionConstraint(bodies, inertias, axis1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
}
}
@ -1024,12 +948,11 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize(); //??
addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
}
addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
} else
}
else
{
b3PlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
@ -1045,8 +968,8 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
cp.m_lateralFrictionInitialized = true;
}
}
} else
}
else
{
addFrictionConstraint(bodies, inertias, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, cp.m_contactMotion1, cp.m_contactCFM1);
@ -1055,10 +978,6 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyData* bodies, b3InertiaData* i
setFrictionConstraintImpulse(bodies, inertias, solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
}
}
}
}
@ -1067,14 +986,10 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
{
B3_PROFILE("solveGroupCacheFriendlySetup");
m_maxOverrideNumSolverIterations = 0;
m_tmpSolverBodyPool.resize(0);
m_bodyCount.resize(0);
m_bodyCount.resize(numBodies, 0);
m_bodyCountCheck.resize(0);
@ -1095,12 +1010,12 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
{
m_bodyCount[bodyIndexA] = -1;
m_bodyCount[bodyIndexB] = -1;
} else
}
else
{
//didn't implement joints with Jacobi version yet
b3Assert(0);
}
}
for (int i = 0; i < numManifolds; i++)
{
@ -1110,7 +1025,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
{
m_bodyCount[bodyIndexA] = -1;
m_bodyCount[bodyIndexB] = -1;
} else
}
else
{
if (bodies[bodyIndexA].m_invMass)
{
@ -1126,11 +1042,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
else
m_bodyCount[bodyIndexB] = -1;
}
}
if (1)
{
int j;
@ -1146,7 +1059,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
//if (1)
{
{
int totalNumRows = 0;
int i;
@ -1170,7 +1082,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
if (constraints[i]->isEnabled())
{
constraints[i]->getInfo1(&info1, bodies);
} else
}
else
{
info1.m_numConstraintRows = 0;
info1.nub = 0;
@ -1179,7 +1092,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
}
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
#ifndef DISABLE_JOINTS
///setup the b3SolverConstraints
int currentRow = 0;
@ -1206,14 +1118,10 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
b3SolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
if (overrideNumSolverIterations > m_maxOverrideNumSolverIterations)
m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
int j;
for (j = 0; j < info1.m_numConstraintRows; j++)
{
@ -1236,7 +1144,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
bodyBPtr->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
bodyBPtr->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
b3TypedConstraint::b3ConstraintInfo2 info2;
info2.fps = 1.f / infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
@ -1275,7 +1182,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
b3Matrix3x3& invInertiaWorldA = inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
{
//b3Vector3 angularFactorA(1,1,1);
const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = invInertiaWorldA * ftorqueAxis1; //*angularFactorA;
@ -1283,7 +1189,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
b3Matrix3x3& invInertiaWorldB = inertias[constraint->getRigidBodyB()].m_invInertiaWorld;
{
const b3Vector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
solverConstraint.m_angularComponentB = invInertiaWorldB * ftorqueAxis2; //*constraint->getRigidBodyB().getAngularFactor();
}
@ -1305,7 +1210,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
solverConstraint.m_jacDiagABInv = fsum > B3_EPSILON ? b3Scalar(1.) / sum : 0.f;
}
///fix rhs
///todo: add force/torque accelerators
{
@ -1322,7 +1226,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
b3Scalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_appliedImpulse = 0.f;
}
}
}
@ -1331,7 +1234,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
#endif //DISABLE_JOINTS
}
{
int i;
@ -1345,7 +1247,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
// b3ContactSolverInfo info = infoGlobal;
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
@ -1375,13 +1276,10 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyData* bodies
}
return 0.f;
}
b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
@ -1390,8 +1288,8 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
{
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for (int j=0; j<numNonContactPool; ++j) {
for (int j = 0; j < numNonContactPool; ++j)
{
int tmp = m_orderNonContactConstraintPool[j];
int swapi = b3RandInt2(j + 1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
@ -1401,14 +1299,16 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
//contact/friction constraints are not solved more than
if (iteration < infoGlobal.m_numIterations)
{
for (int j=0; j<numConstraintPool; ++j) {
for (int j = 0; j < numConstraintPool; ++j)
{
int tmp = m_orderTmpConstraintPool[j];
int swapi = b3RandInt2(j + 1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j=0; j<numFrictionPool; ++j) {
for (int j = 0; j < numFrictionPool; ++j)
{
int tmp = m_orderFrictionConstraintPool[j];
int swapi = b3RandInt2(j + 1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
@ -1430,7 +1330,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
if (iteration < infoGlobal.m_numIterations)
{
///solve all contact constraints using SIMD, if available
if (infoGlobal.m_solverMode & B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
@ -1450,7 +1349,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
if (applyFriction)
{
{
b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier]];
if (totalImpulse > b3Scalar(0))
@ -1464,7 +1362,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
if (infoGlobal.m_solverMode & B3_SOLVER_USE_2_FRICTION_DIRECTIONS)
{
b3SolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c * multiplier + 1]];
if (totalImpulse > b3Scalar(0))
@ -1477,7 +1374,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
}
}
}
}
else //B3_SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
@ -1489,13 +1385,11 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
{
const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
}
if (!m_usePgs)
averageVelocities();
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
@ -1513,11 +1407,9 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j = 0; j < numRollingFrictionPoolConstraints; j++)
{
b3SolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
b3Scalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse > b3Scalar(0))
@ -1532,11 +1424,10 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
}
}
}
}
} else
}
else
{
//non-SIMD version
///solve all joint constraints
@ -1549,7 +1440,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
if (iteration < infoGlobal.m_numIterations)
{
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (int j = 0; j < numPoolConstraints; j++)
@ -1595,7 +1485,6 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
return 0.f;
}
void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal)
{
int iteration;
@ -1649,16 +1538,13 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint*
for (int iteration = 0; iteration < maxIterations; iteration++)
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{
solveSingleIteration(iteration, constraints, numConstraints, infoGlobal);
if (!m_usePgs)
{
averageVelocities();
}
}
}
return 0.f;
}
@ -1690,12 +1576,10 @@ void b3PgsJacobiSolver::averageVelocities()
if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
{
b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
b3Scalar factor = 1.f / b3Scalar(m_bodyCount[orgBodyIndex]);
m_tmpSolverBodyPool[i].m_deltaLinearVelocity = m_deltaLinearVelocities[orgBodyIndex] * factor;
m_tmpSolverBodyPool[i].m_deltaAngularVelocity = m_deltaAngularVelocities[orgBodyIndex] * factor;
}
@ -1743,7 +1627,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
fb->m_appliedForceBodyB += -solverConstr.m_contactNormal * solverConstr.m_appliedImpulse * bodyB->m_linearFactor / infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal * bodyA->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal * bodyB->m_angularFactor * solverConstr.m_appliedImpulse / infoGlobal.m_timeStep;
}
constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
@ -1772,7 +1655,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
{
body->m_linVel = m_tmpSolverBodyPool[i].m_linearVelocity;
body->m_angVel = m_tmpSolverBodyPool[i].m_angularVelocity;
} else
}
else
{
b3Scalar factor = 1.f / b3Scalar(m_bodyCount[bodyIndex]);
@ -1797,7 +1681,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
}
}
m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
@ -1807,8 +1690,6 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyData* bodie
return 0.f;
}
void b3PgsJacobiSolver::reset()
{
m_btSeed2 = 0;

16
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h vendored
View file

@ -1,11 +1,9 @@
#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER
struct b3Contact4;
struct b3ContactPoint;
class b3Dispatcher;
#include "b3TypedConstraint.h"
@ -18,7 +16,6 @@ struct b3InertiaData;
class b3PgsJacobiSolver
{
protected:
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
@ -61,7 +58,6 @@ protected:
b3SolverConstraint& addFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0., b3Scalar cfmSlip = 0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias, const b3Vector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, b3ContactPoint& cp, const b3Vector3& rel_pos1, const b3Vector3& rel_pos2, b3RigidBodyData* colObj0, b3RigidBodyData* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity = 0, b3Scalar cfmSlip = 0.f);
void setupContactConstraint(b3RigidBodyData* bodies, b3InertiaData* inertias,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, b3ContactPoint& cp,
const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
@ -73,12 +69,10 @@ protected:
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
void convertContact(b3RigidBodyData* bodies, b3InertiaData* inertias, b3Contact4* manifold, const b3ContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
b3SolverBody& bodyA, b3SolverBody& bodyB,
const b3SolverConstraint& contactConstraint);
@ -100,20 +94,15 @@ protected:
void resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA, b3SolverBody& bodyB, const b3SolverConstraint& contactConstraint);
protected:
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints, int numConstraints, const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyData* bodies, b3InertiaData* inertias, int numBodies, const b3ContactSolverInfo& infoGlobal);
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3PgsJacobiSolver(bool usePgs);
@ -139,11 +128,6 @@ public:
{
return m_btSeed2;
}
};
#endif //B3_PGS_JACOBI_SOLVER

21
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp vendored
View file

@ -13,21 +13,14 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3Point2PointConstraint.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
#include <new>
b3Point2PointConstraint::b3Point2PointConstraint(int rbA, int rbB, const b3Vector3& pivotInA, const b3Vector3& pivotInB)
:b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
m_flags(0)
: b3TypedConstraint(B3_POINT2POINT_CONSTRAINT_TYPE, rbA, rbB), m_pivotInA(pivotInA), m_pivotInB(pivotInB), m_flags(0)
{
}
/*
@ -40,7 +33,6 @@ m_useSolveConstraintObsolete(false)
}
*/
void b3Point2PointConstraint::getInfo1(b3ConstraintInfo1* info, const b3RigidBodyData* bodies)
{
getInfo1NonVirtual(info, bodies);
@ -52,9 +44,6 @@ void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info,const
info->nub = 3;
}
void b3Point2PointConstraint::getInfo2(b3ConstraintInfo2* info, const b3RigidBodyData* bodies)
{
b3Transform trA;
@ -72,7 +61,6 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
void b3Point2PointConstraint::getInfo2NonVirtual(b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
{
//retrieve matrices
// anchor points in global coordinates with respect to body PORs.
@ -110,8 +98,6 @@ void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const
a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
}
// set right hand side
b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar k = info->fps * currERP;
@ -139,15 +125,11 @@ void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const
}
}
info->m_damping = m_setting.m_damping;
}
void b3Point2PointConstraint::updateRHS(b3Scalar timeStep)
{
(void)timeStep;
}
///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
@ -206,4 +188,3 @@ b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
}
return retVal;
}

12
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h vendored
View file

@ -22,7 +22,6 @@ subject to the following restrictions:
class b3RigidBody;
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
@ -33,8 +32,7 @@ class b3RigidBody;
struct b3ConstraintSetting
{
b3ConstraintSetting() :
m_tau(b3Scalar(0.3)),
b3ConstraintSetting() : m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
{
@ -51,7 +49,8 @@ enum b3Point2PointFlags
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
B3_ATTRIBUTE_ALIGNED16(class) b3Point2PointConstraint : public b3TypedConstraint
B3_ATTRIBUTE_ALIGNED16(class)
b3Point2PointConstraint : public b3TypedConstraint
{
#ifdef IN_PARALLELL_SOLVER
public:
@ -65,7 +64,6 @@ public:
b3Scalar m_cfm;
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
b3ConstraintSetting m_setting;
@ -74,8 +72,6 @@ public:
//b3Point2PointConstraint(int rbA,const b3Vector3& pivotInA);
virtual void getInfo1(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
void getInfo1NonVirtual(b3ConstraintInfo1 * info, const b3RigidBodyData* bodies);
@ -116,8 +112,6 @@ public:
///fills the dataBuffer and returns the struct name (and 0 on failure)
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64

29
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h vendored
View file

@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_SOLVER_BODY_H
#define B3_SOLVER_BODY_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
@ -28,14 +27,12 @@ subject to the following restrictions:
#define USE_SIMD 1
#endif //
#ifdef USE_SIMD
struct b3SimdScalar
{
B3_FORCE_INLINE b3SimdScalar()
{
}
B3_FORCE_INLINE b3SimdScalar(float fl)
@ -47,8 +44,7 @@ struct b3SimdScalar
: m_vec128(v128)
{
}
union
{
union {
__m128 m_vec128;
float m_floats[4];
float x, y, z, w;
@ -83,7 +79,6 @@ struct b3SimdScalar
{
return m_floats[0];
}
};
///@brief Return the elementwise product of two b3SimdScalar
@ -100,13 +95,13 @@ operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
return b3SimdScalar(_mm_add_ps(v1.get128(), v2.get128()));
}
#else
#define b3SimdScalar b3Scalar
#endif
///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
B3_ATTRIBUTE_ALIGNED16(struct)
b3SolverBody
{
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Transform m_worldTransform;
@ -120,15 +115,13 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
b3Vector3 m_linearVelocity;
b3Vector3 m_angularVelocity;
union
{
union {
void* m_originalBody;
int m_originalBodyIndex;
};
int padding[3];
void setWorldTransform(const b3Transform& worldTransform)
{
m_worldTransform = worldTransform;
@ -155,7 +148,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
angVel.setValue(0, 0, 0);
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
{
@ -175,8 +167,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
}
}
const b3Vector3& getDeltaLinearVelocity() const
{
return m_deltaLinearVelocity;
@ -197,7 +187,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
return m_turnVelocity;
}
////////////////////////////////////////////////
///some internal methods, don't use them
@ -246,7 +235,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
angVel = m_angularVelocity + m_deltaAngularVelocity;
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent, const b3Scalar impulseMagnitude)
{
@ -257,9 +245,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
}
}
void writebackVelocity()
{
//if (m_originalBody>=0)
@ -271,7 +256,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
}
}
void writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
{
(void)timeStep;
@ -292,11 +276,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverBody
//m_originalBody->setCompanionId(-1);
}
}
};
#endif //B3_SOLVER_BODY_H

13
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h vendored
View file

@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_SOLVER_CONSTRAINT_H
#define B3_SOLVER_CONSTRAINT_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
//#include "b3JacobianEntry.h"
@ -25,9 +24,9 @@ subject to the following restrictions:
//#define NO_FRICTION_TANGENTIALS 1
#include "b3SolverBody.h"
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
B3_ATTRIBUTE_ALIGNED16(struct)
b3SolverConstraint
{
B3_DECLARE_ALIGNED_ALLOCATOR();
@ -52,8 +51,7 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
b3Scalar m_lowerLimit;
b3Scalar m_upperLimit;
b3Scalar m_rhsPenetration;
union
{
union {
void* m_originalContactPoint;
b3Scalar m_unusedPadding4;
};
@ -63,7 +61,6 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
int m_solverBodyIdA;
int m_solverBodyIdB;
enum b3SolverConstraintType
{
B3_SOLVER_CONTACT_1D = 0,
@ -73,8 +70,4 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //B3_SOLVER_CONSTRAINT_H

10
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp vendored
View file

@ -13,15 +13,11 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3TypedConstraint.h"
//#include "Bullet3Common/b3Serializer.h"
#define B3_DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA, int rbB)
: b3TypedObject(type),
m_userConstraintType(-1),
@ -38,9 +34,6 @@ m_jointFeedback(0)
{
}
b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
{
if (lowLim > uppLim)
@ -90,8 +83,6 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
return lim_fact;
}
void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
@ -125,7 +116,6 @@ void b3AngularLimit::test(const b3Scalar angle)
}
}
b3Scalar b3AngularLimit::getError() const
{
return m_correction * m_sign;

42
thirdparty/bullet/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h vendored
View file

@ -16,7 +16,6 @@ subject to the following restrictions:
#ifndef B3_TYPED_CONSTRAINT_H
#define B3_TYPED_CONSTRAINT_H
#include "Bullet3Common/b3Scalar.h"
#include "b3SolverConstraint.h"
@ -37,7 +36,6 @@ enum b3TypedConstraintType
B3_MAX_CONSTRAINT_TYPE
};
enum b3ConstraintParams
{
B3_CONSTRAINT_ERP = 1,
@ -52,8 +50,8 @@ enum b3ConstraintParams
#define b3AssertConstrParams(_par)
#endif
B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
B3_ATTRIBUTE_ALIGNED16(struct)
b3JointFeedback
{
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
@ -61,17 +59,15 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
b3Vector3 m_appliedTorqueBodyB;
};
struct b3RigidBodyData;
///TypedConstraint is the baseclass for Bullet constraints and vehicles
B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
B3_ATTRIBUTE_ALIGNED16(class)
b3TypedConstraint : public b3TypedObject
{
int m_userConstraintType;
union
{
union {
int m_userConstraintId;
void* m_userConstraintPtr;
};
@ -81,7 +77,6 @@ B3_ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
bool m_needsFeedback;
int m_overrideNumSolverIterations;
b3TypedConstraint& operator=(b3TypedConstraint& other)
{
b3Assert(0);
@ -99,21 +94,19 @@ protected:
///internal method used by the constraint solver, don't use them directly
b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
public:
B3_DECLARE_ALIGNED_ALLOCATOR();
virtual ~b3TypedConstraint(){};
b3TypedConstraint(b3TypedConstraintType type, int bodyA, int bodyB);
struct b3ConstraintInfo1 {
struct b3ConstraintInfo1
{
int m_numConstraintRows, nub;
};
struct b3ConstraintInfo2 {
struct b3ConstraintInfo2
{
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
b3Scalar fps, erp;
@ -159,7 +152,6 @@ public:
m_overrideNumSolverIterations = overideNumIterations;
}
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(b3ConstraintArray & ca, int solverBodyA, int solverBodyB, b3Scalar timeStep)
{
@ -186,7 +178,6 @@ public:
return m_appliedImpulse;
}
b3Scalar getBreakingImpulseThreshold() const
{
return m_breakingImpulseThreshold;
@ -207,11 +198,9 @@ public:
m_isEnabled = enabled;
}
///internal method used by the constraint solver, don't use them directly
virtual void solveConstraintObsolete(b3SolverBody& /*bodyA*/, b3SolverBody& /*bodyB*/, b3Scalar /*timeStep*/){};
int getRigidBodyA() const
{
return m_rbA;
@ -221,7 +210,6 @@ public:
return m_rbB;
}
int getRigidBodyA()
{
return m_rbA;
@ -276,7 +264,6 @@ public:
return m_jointFeedback;
}
int getUid() const
{
return m_userConstraintId;
@ -327,7 +314,6 @@ public:
///fills the dataBuffer and returns the struct name (and 0 on failure)
//virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
// returns angle in range [-B3_2_PI, B3_2_PI], closest to one of the limits
@ -356,6 +342,7 @@ B3_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar
}
}
// clang-format off
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct b3TypedConstraintData
{
@ -379,13 +366,14 @@ struct b3TypedConstraintData
};
// clang-format on
/*B3_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(b3TypedConstraintData);
}
*/
class b3AngularLimit
{
private:
@ -412,7 +400,8 @@ public:
m_correction(0.0f),
m_sign(0.0f),
m_solveLimit(false)
{}
{
}
/// Sets all limit's parameters.
/// When low > high limit becomes inactive.
@ -475,9 +464,6 @@ public:
b3Scalar getLow() const;
b3Scalar getHigh() const;
};
#endif //B3_TYPED_CONSTRAINT_H

73
thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp vendored
View file

@ -11,7 +11,6 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Dynamics/shared/b3Inertia.h"
struct b3CpuRigidBodyPipelineInternalData
{
b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
@ -23,7 +22,6 @@ struct b3CpuRigidBodyPipelineInternalData
b3Config m_config;
};
b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
{
m_data = new b3CpuRigidBodyPipelineInternalData;
@ -39,7 +37,6 @@ b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
{
for (int i = 0; i < this->getNumBodies(); i++)
{
b3RigidBodyData* body = &m_data->m_rigidBodies[i];
@ -52,8 +49,6 @@ void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
if (shapeIndex >= 0)
{
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(shapeIndex);
b3Aabb& worldAabb = m_data->m_aabbWorldSpace[i];
float margin = 0.f;
@ -73,15 +68,12 @@ void b3CpuRigidBodyPipeline::computeOverlappingPairs()
void b3CpuRigidBodyPipeline::computeContactPoints()
{
b3AlignedObjectArray<b3Int4>& pairs = m_data->m_bp->getOverlappingPairCache()->getOverlappingPairArray();
m_data->m_np->computeContacts(pairs, m_data->m_aabbWorldSpace, m_data->m_rigidBodies);
}
void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
{
//update world space aabb's
updateAabbWorldSpace();
@ -95,18 +87,14 @@ void b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
//update transforms
integrate(deltaTime);
}
static inline float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1,
const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
{
return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
}
static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
{
@ -115,18 +103,19 @@ static inline void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0
angular1 = b3Cross(r1, n);
}
static inline void b3SolveContact(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
b3Vector3 dLinVelA; dLinVelA.setZero();
b3Vector3 dAngVelA; dAngVelA.setZero();
b3Vector3 dLinVelB; dLinVelB.setZero();
b3Vector3 dAngVelB; dAngVelB.setZero();
b3Vector3 dLinVelA;
dLinVelA.setZero();
b3Vector3 dAngVelA;
dAngVelA.setZero();
b3Vector3 dLinVelB;
dLinVelB.setZero();
b3Vector3 dAngVelB;
dAngVelB.setZero();
for (int ic = 0; ic < 4; ic++)
{
@ -140,7 +129,8 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
b3SetLinearAndAngular((const b3Vector3&)-cs.m_linear, (const b3Vector3&)r0, (const b3Vector3&)r1, linear, angular0, angular1);
float rambdaDt = b3CalcRelVel((const b3Vector3&)cs.m_linear, (const b3Vector3&)-cs.m_linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
linVelA, angVelA, linVelB, angVelB) +
cs.m_b[ic];
rambdaDt *= cs.m_jacCoeffInv[ic];
{
@ -169,20 +159,13 @@ static inline void b3SolveContact(b3ContactConstraint4& cs,
}
}
}
}
static inline void b3SolveFriction(b3ContactConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if (cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0) return;
const b3Vector3& center = (const b3Vector3&)cs.m_center;
@ -238,13 +221,8 @@ static inline void b3SolveFriction(b3ContactConstraint4& cs,
angVelB -= (angNB * 0.1f) * n;
}
}
}
struct b3SolveTask // : public ThreadPool::Task
{
b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,
@ -252,15 +230,10 @@ struct b3SolveTask// : public ThreadPool::Task
b3AlignedObjectArray<b3ContactConstraint4>& constraints,
int start, int nConstraints,
int maxNumBatches,
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
)
: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ),
m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx),
m_start( start ),
m_nConstraints( nConstraints ),
m_solveFriction( true ),
m_maxNumBatches(maxNumBatches)
{}
b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx)
: m_bodies(bodies), m_shapes(shapes), m_constraints(constraints), m_wgUsedBodies(wgUsedBodies), m_curWgidx(curWgidx), m_start(start), m_nConstraints(nConstraints), m_solveFriction(true), m_maxNumBatches(maxNumBatches)
{
}
unsigned short int getType() { return 0; }
@ -269,14 +242,12 @@ m_start( start ),
b3AlignedObjectArray<int> usedBodies;
//printf("run..............\n");
for (int bb = 0; bb < m_maxNumBatches; bb++)
{
usedBodies.resize(0);
for (int ic = m_nConstraints - 1; ic >= 0; ic--)
//for(int ic=0; ic<m_nConstraints; ic++)
{
int i = m_start + ic;
if (m_constraints[i].m_batchIdx != bb)
continue;
@ -324,7 +295,6 @@ m_start( start ),
usedBodies[bIdx]++;
}
if (!m_solveFriction)
{
float maxRambdaDt[4] = {FLT_MAX, FLT_MAX, FLT_MAX, FLT_MAX};
@ -333,7 +303,6 @@ m_start( start ),
b3SolveContact(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt);
}
else
{
@ -355,7 +324,6 @@ m_start( start ),
b3SolveFriction(m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3&)m_shapes[aIdx].m_invInertiaWorld,
(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3&)m_shapes[bIdx].m_invInertiaWorld,
maxRambdaDt, minRambdaDt);
}
}
@ -374,11 +342,7 @@ m_start( start ),
}
}
}
}
}
b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
@ -401,8 +365,6 @@ void b3CpuRigidBodyPipeline::solveContactConstraints()
int n = contactConstraints.size();
//convert contacts...
int maxNumBatches = 250;
for (int iter = 0; iter < m_nIterations; iter++)
@ -430,7 +392,6 @@ void b3CpuRigidBodyPipeline::integrate(float deltaTime)
{
b3IntegrateTransform(&m_data->m_rigidBodies[i], deltaTime, angDamping, gravityAcceleration);
}
}
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
@ -448,7 +409,6 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
m_data->m_rigidBodies.push_back(body);
if (collidableIndex >= 0)
{
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
@ -467,8 +427,8 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
m_data->m_bp->createProxy(worldAabb.m_minVec, worldAabb.m_maxVec, bodyIndex, 0, 1, 1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
} else
}
else
{
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
}
@ -476,7 +436,6 @@ int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
return bodyIndex;
}
const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
{
return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;

5
thirdparty/bullet/Bullet3Dynamics/b3CpuRigidBodyPipeline.h vendored
View file

@ -16,8 +16,6 @@ subject to the following restrictions:
#ifndef B3_CPU_RIGIDBODY_PIPELINE_H
#define B3_CPU_RIGIDBODY_PIPELINE_H
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h"
@ -29,8 +27,6 @@ protected:
int allocateCollidable();
public:
b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
virtual ~b3CpuRigidBodyPipeline();
@ -61,7 +57,6 @@ public:
const struct b3RigidBodyData* getBodyBuffer() const;
int getNumBodies() const;
};
#endif //B3_CPU_RIGIDBODY_PIPELINE_H

3
thirdparty/bullet/Bullet3Dynamics/shared/b3ContactConstraint4.h vendored
View file

@ -5,10 +5,8 @@
typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
struct b3ContactConstraint4
{
b3Float4 m_linear; //normal?
b3Float4 m_worldPos[4];
b3Float4 m_center; // friction
@ -22,7 +20,6 @@ struct b3ContactConstraint4
unsigned int m_bodyB;
int m_batchIdx;
unsigned int m_paddings;
};
//inline void setFrictionCoeff(float value) { m_linear[3] = value; }

13
thirdparty/bullet/Bullet3Dynamics/shared/b3ConvertConstraint4.h vendored
View file

@ -4,11 +4,11 @@
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q);
void b3PlaneSpace1(b3Float4ConstArg n, b3Float4* p, b3Float4* q)
{
if (b3Fabs(n.z) > 0.70710678f) {
if (b3Fabs(n.z) > 0.70710678f)
{
// choose p in y-z plane
float a = n.y * n.y + n.z * n.z;
float k = 1.f / sqrt(a);
@ -20,7 +20,8 @@ void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
q[0].y = -n.x * p[0].z;
q[0].z = n.x * p[0].y;
}
else {
else
{
// choose p in x-y plane
float a = n.x * n.x + n.y * n.y;
float k = 1.f / sqrt(a);
@ -34,8 +35,6 @@ void b3PlaneSpace1 (b3Float4ConstArg n, b3Float4* p, b3Float4* q);
}
}
void setLinearAndAngular(b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4ConstArg r1, b3Float4* linear, b3Float4* angular0, b3Float4* angular1)
{
*linear = b3MakeFloat4(n.x, n.y, n.z, 0.f);
@ -43,14 +42,12 @@ void setLinearAndAngular( b3Float4ConstArg n, b3Float4ConstArg r0, b3Float4Const
*angular1 = -b3Cross3(r1, n);
}
float calcRelVel(b3Float4ConstArg l0, b3Float4ConstArg l1, b3Float4ConstArg a0, b3Float4ConstArg a1, b3Float4ConstArg linVel0,
b3Float4ConstArg angVel0, b3Float4ConstArg linVel1, b3Float4ConstArg angVel1)
{
return b3Dot3F4(l0, linVel0) + b3Dot3F4(a0, angVel0) + b3Dot3F4(l1, linVel1) + b3Dot3F4(a1, angVel1);
}
float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4ConstArg angular0, b3Float4ConstArg angular1,
float invMass0, const b3Mat3x3* invInertia0, float invMass1, const b3Mat3x3* invInertia1)
{
@ -62,7 +59,6 @@ float calcJacCoeff(b3Float4ConstArg linear0, b3Float4ConstArg linear1, b3Float4C
return -1.f / (jmj0 + jmj1 + jmj2 + jmj3);
}
void setConstraint4(b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4ConstArg angVelA, float invMassA, b3Mat3x3ConstArg invInertiaA,
b3Float4ConstArg posB, b3Float4ConstArg linVelB, b3Float4ConstArg angVelB, float invMassB, b3Mat3x3ConstArg invInertiaB,
__global struct b3Contact4Data* src, float dt, float positionDrift, float positionConstraintCoeff,
@ -78,7 +74,6 @@ void setConstraint4( b3Float4ConstArg posA, b3Float4ConstArg linVelA, b3Float4Co
}
dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
dstC->m_linear = src->m_worldNormalOnB;
dstC->m_linear.w = 0.7f; //src->getFrictionCoeff() );
for (int ic = 0; ic < 4; ic++)

1
thirdparty/bullet/Bullet3Dynamics/shared/b3Inertia.h vendored
View file

@ -11,5 +11,4 @@ struct b3Inertia
b3Mat3x3 m_initInvInertia;
};
#endif //B3_INERTIA_H

7
thirdparty/bullet/Bullet3Dynamics/shared/b3IntegrateTransforms.h vendored
View file

@ -2,11 +2,8 @@
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
inline void integrateSingleTransform(__global b3RigidBodyData_t* bodies, int nodeID, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
{
if (bodies[nodeID].m_invMass != 0.f)
{
float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
@ -54,9 +51,7 @@ inline void integrateSingleTransform( __global b3RigidBodyData_t* bodies,int nod
//apply gravity
bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
}
}
inline void b3IntegrateTransform(__global b3RigidBodyData_t* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
@ -107,7 +102,5 @@ inline void b3IntegrateTransform( __global b3RigidBodyData_t* body, float timeSt
//linear velocity
body->m_pos += body->m_linVel * timeStep;
}
}

17
thirdparty/bullet/Bullet3Geometry/b3AabbUtil.h vendored
View file

@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_AABB_UTIL2
#define B3_AABB_UTIL2
@ -21,8 +19,6 @@ subject to the following restrictions:
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3MinMax.h"
B3_FORCE_INLINE void b3AabbExpand(b3Vector3& aabbMin,
b3Vector3& aabbMax,
const b3Vector3& expansionMin,
@ -43,7 +39,6 @@ B3_FORCE_INLINE bool b3TestPointAgainstAabb2(const b3Vector3 &aabbMin1, const b3
return overlap;
}
/// conservative test for overlap between two aabbs
B3_FORCE_INLINE bool b3TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
const b3Vector3& aabbMin2, const b3Vector3& aabbMax2)
@ -74,7 +69,6 @@ B3_FORCE_INLINE bool b3TestTriangleAgainstAabb2(const b3Vector3 *vertices,
return true;
}
B3_FORCE_INLINE int b3Outcode(const b3Vector3& p, const b3Vector3& halfExtent)
{
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
@ -85,8 +79,6 @@ B3_FORCE_INLINE int b3Outcode(const b3Vector3& p,const b3Vector3& halfExtent)
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
}
B3_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
const b3Vector3& rayInvDirection,
const unsigned int raySign[3],
@ -177,8 +169,6 @@ B3_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
return false;
}
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin, const b3Transform& t, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
{
b3Vector3 halfExtentsWithMargin = halfExtents + b3MakeVector3(margin, margin, margin);
@ -189,7 +179,6 @@ B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar marg
aabbMaxOut = center + extent;
}
B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin, const b3Vector3& localAabbMax, b3Scalar margin, const b3Transform& trans, b3Vector3& aabbMinOut, b3Vector3& aabbMaxOut)
{
//b3Assert(localAabbMin.getX() <= localAabbMax.getX());
@ -211,9 +200,7 @@ B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vecto
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
B3_FORCE_INLINE unsigned b3TestQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
{
return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
return static_cast<unsigned int>(b3Select((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0));
}
#else
@ -228,5 +215,3 @@ B3_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vecto
#endif //B3_USE_BANCHLESS
#endif //B3_AABB_UTIL2

56
thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.cpp vendored
View file

@ -37,13 +37,11 @@ subject to the following restrictions:
typedef unsigned long long int btUint64_t;
#endif
//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines
//#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly
// #define USE_X86_64_ASM
//#endif
//#define DEBUG_CONVEX_HULL
//#define SHOW_ITERATIONS
@ -56,7 +54,6 @@ subject to the following restrictions:
class b3ConvexHullInternal
{
public:
class Point64
{
public:
@ -176,7 +173,8 @@ class b3ConvexHullInternal
{
#ifdef USE_X86_64_ASM
Int128 result;
__asm__ ("addq %[bl], %[rl]\n\t"
__asm__(
"addq %[bl], %[rl]\n\t"
"adcq %[bh], %[rh]\n\t"
: [rl] "=r"(result.low), [rh] "=r"(result.high)
: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
@ -192,7 +190,8 @@ class b3ConvexHullInternal
{
#ifdef USE_X86_64_ASM
Int128 result;
__asm__ ("subq %[bl], %[rl]\n\t"
__asm__(
"subq %[bl], %[rl]\n\t"
"sbbq %[bh], %[rh]\n\t"
: [rl] "=r"(result.low), [rh] "=r"(result.high)
: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
@ -206,7 +205,8 @@ class b3ConvexHullInternal
Int128& operator+=(const Int128& b)
{
#ifdef USE_X86_64_ASM
__asm__ ("addq %[bl], %[rl]\n\t"
__asm__(
"addq %[bl], %[rl]\n\t"
"adcq %[bh], %[rh]\n\t"
: [rl] "=r"(low), [rh] "=r"(high)
: "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high)
@ -272,7 +272,6 @@ class b3ConvexHullInternal
}
};
class Rational64
{
private:
@ -331,7 +330,6 @@ class b3ConvexHullInternal
}
};
class Rational128
{
private:
@ -428,7 +426,6 @@ class b3ConvexHullInternal
}
};
class Edge;
class Face;
@ -507,7 +504,6 @@ class b3ConvexHullInternal
}
};
class Edge
{
public:
@ -580,7 +576,8 @@ class b3ConvexHullInternal
}
};
template<typename UWord, typename UHWord> class DMul
template <typename UWord, typename UHWord>
class DMul
{
private:
static btUint32_t high(btUint64_t value)
@ -625,7 +622,6 @@ class b3ConvexHullInternal
}
public:
static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh)
{
UWord p00 = mul(low(a), low(b));
@ -648,7 +644,6 @@ class b3ConvexHullInternal
};
private:
class IntermediateHull
{
public:
@ -664,9 +659,15 @@ class b3ConvexHullInternal
void print();
};
enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE};
enum Orientation
{
NONE,
CLOCKWISE,
COUNTER_CLOCKWISE
};
template <typename T> class PoolArray
template <typename T>
class PoolArray
{
private:
T* array;
@ -696,7 +697,8 @@ class b3ConvexHullInternal
}
};
template <typename T> class Pool
template <typename T>
class Pool
{
private:
PoolArray<T>* arrays;
@ -838,7 +840,6 @@ class b3ConvexHullInternal
b3Scalar shrink(b3Scalar amount, b3Scalar clampAmount);
};
b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::operator*(btInt64_t b) const
{
bool negative = (btInt64_t)high < 0;
@ -915,7 +916,8 @@ int b3ConvexHullInternal::Rational64::compare(const Rational64& b) const
int result;
btInt64_t tmp;
btInt64_t dummy;
__asm__ ("mulq %[bn]\n\t"
__asm__(
"mulq %[bn]\n\t"
"movq %%rax, %[tmp]\n\t"
"movq %%rdx, %%rbx\n\t"
"movq %[tn], %%rax\n\t"
@ -998,7 +1000,6 @@ int b3ConvexHullInternal::Rational128::compare(btInt64_t b) const
return numerator.ucmp(denominator * b) * sign;
}
b3ConvexHullInternal::Edge* b3ConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
{
b3Assert(from && to);
@ -1647,7 +1648,6 @@ void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
#endif
}
void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
{
if (!h1.maxXy)
@ -1935,7 +1935,6 @@ void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
}
}
static bool b3PointCmp(const b3ConvexHullInternal::Point32& p, const b3ConvexHullInternal::Point32& q)
{
return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z))));
@ -2478,12 +2477,9 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
Vertex* v = vertexPool.newObject();
v->point.index = -1;
v->copy = -1;
v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01)
+ Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01)
+ Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01)
+ Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x,
Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y,
Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z,
det);
v->point.x = (btInt32_t)v->point128.xvalue();
v->point.y = (btInt32_t)v->point128.yvalue();
@ -2626,7 +2622,6 @@ bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
return true;
}
static int getVertexCopy(b3ConvexHullInternal::Vertex* vertex, b3AlignedObjectArray<b3ConvexHullInternal::Vertex*>& vertices)
{
int index = vertex->copy;
@ -2748,8 +2743,3 @@ b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, in
return shift;
}

4
thirdparty/bullet/Bullet3Geometry/b3ConvexHullComputer.h vendored
View file

@ -27,7 +27,6 @@ class b3ConvexHullComputer
b3Scalar compute(const void* coords, bool doubleCoords, int stride, int count, b3Scalar shrink, b3Scalar shrinkClamp);
public:
class Edge
{
private:
@ -64,7 +63,6 @@ class b3ConvexHullComputer
}
};
// Vertices of the output hull
b3AlignedObjectArray<b3Vector3> vertices;
@ -98,6 +96,4 @@ class b3ConvexHullComputer
}
};
#endif //B3_CONVEX_HULL_COMPUTER_H

23
thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.cpp vendored
View file

@ -12,11 +12,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "b3GeometryUtil.h"
/*
Make sure this dummy function never changes so that it
can be used by probes that are checking whether the
@ -29,7 +26,6 @@ extern "C"
void b3BulletMathProbe() {}
}
bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin)
{
int numbrushes = planeEquations.size();
@ -43,10 +39,8 @@ bool b3GeometryUtil::isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>&
}
}
return true;
}
bool b3GeometryUtil::areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin)
{
int numvertices = vertices.size();
@ -86,14 +80,12 @@ void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector
{
const b3Vector3& N1 = vertices[i];
for (int j = i + 1; j < numvertices; j++)
{
const b3Vector3& N2 = vertices[j];
for (int k = j + 1; k < numvertices; k++)
{
const b3Vector3& N3 = vertices[k];
b3Vector3 planeEquation, edge0, edge1;
@ -119,11 +111,9 @@ void b3GeometryUtil::getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector
}
normalSign = b3Scalar(-1.);
}
}
}
}
}
void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut)
@ -134,19 +124,20 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
{
const b3Vector3& N1 = planeEquations[i];
for (int j = i + 1; j < numbrushes; j++)
{
const b3Vector3& N2 = planeEquations[j];
for (int k = j + 1; k < numbrushes; k++)
{
const b3Vector3& N3 = planeEquations[k];
b3Vector3 n2n3; n2n3 = N2.cross(N3);
b3Vector3 n3n1; n3n1 = N3.cross(N1);
b3Vector3 n1n2; n1n2 = N1.cross(N2);
b3Vector3 n2n3;
n2n3 = N2.cross(N3);
b3Vector3 n3n1;
n3n1 = N3.cross(N1);
b3Vector3 n1n2;
n1n2 = N1.cross(N2);
if ((n2n3.length2() > b3Scalar(0.0001)) &&
(n3n1.length2() > b3Scalar(0.0001)) &&
@ -158,7 +149,6 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
b3Scalar quotient = (N1.dot(n2n3));
if (b3Fabs(quotient) > b3Scalar(0.000001))
{
@ -182,4 +172,3 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
}
}
}

6
thirdparty/bullet/Bullet3Geometry/b3GeometryUtil.h vendored
View file

@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GEOMETRY_UTIL_H
#define B3_GEOMETRY_UTIL_H
@ -23,8 +22,6 @@ subject to the following restrictions:
class b3GeometryUtil
{
public:
static void getPlaneEquationsFromVertices(b3AlignedObjectArray<b3Vector3>& vertices, b3AlignedObjectArray<b3Vector3>& planeEquationsOut);
static void getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3Vector3>& planeEquations, b3AlignedObjectArray<b3Vector3>& verticesOut);
@ -34,9 +31,6 @@ class b3GeometryUtil
static bool isPointInsidePlanes(const b3AlignedObjectArray<b3Vector3>& planeEquations, const b3Vector3& point, b3Scalar margin);
static bool areVerticesBehindPlane(const b3Vector3& planeNormal, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar margin);
};
#endif //B3_GEOMETRY_UTIL_H

13
thirdparty/bullet/Bullet3Geometry/b3GrahamScan2dConvexHull.h vendored
View file

@ -13,11 +13,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
#define B3_GRAHAM_SCAN_2D_CONVEX_HULL_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
@ -32,14 +30,15 @@ struct b3GrahamVector3 : public b3Vector3
int m_orgIndex;
};
struct b3AngleCompareFunc {
struct b3AngleCompareFunc
{
b3Vector3 m_anchor;
b3AngleCompareFunc(const b3Vector3& anchor)
: m_anchor(anchor)
{
}
bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const {
bool operator()(const b3GrahamVector3& a, const b3GrahamVector3& b) const
{
if (a.m_angle != b.m_angle)
return a.m_angle < b.m_angle;
else
@ -61,7 +60,6 @@ inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray<b3GrahamVector3>& orig
b3Vector3 axis0, axis1;
b3PlaneSpace1(normalAxis, axis0, axis1);
if (originalPoints.size() <= 1)
{
for (int i = 0; i < originalPoints.size(); i++)
@ -102,7 +100,8 @@ inline void b3GrahamScanConvexHull2D(b3AlignedObjectArray<b3GrahamVector3>& orig
for (; i != originalPoints.size(); i++)
{
bool isConvex = false;
while (!isConvex&& hull.size()>1) {
while (!isConvex && hull.size() > 1)
{
b3Vector3& a = hull[hull.size() - 2];
b3Vector3& b = hull[hull.size() - 1];
isConvex = b3Cross(a - b, a - originalPoints[i]).dot(normalAxis) > 0;

2
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h vendored
View file

@ -12,7 +12,6 @@
class b3GpuBroadphaseInterface
{
public:
typedef class b3GpuBroadphaseInterface*(CreateFunc)(cl_context ctx, cl_device_id device, cl_command_queue q);
virtual ~b3GpuBroadphaseInterface()
@ -38,7 +37,6 @@ public:
virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU() = 0;
virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU() = 0;
virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU() = 0;
};
#endif //B3_GPU_BROADPHASE_INTERFACE_H

51
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp vendored
View file

@ -5,12 +5,9 @@
#include "kernels/sapKernels.h"
//#include "kernels/gridBroadphase.cl"
#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
#define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl"
#define B3_GRID_BROADPHASE_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl"
@ -21,10 +18,6 @@ cl_kernel kFindOverlappingPairs;
cl_kernel m_copyAabbsKernel;
cl_kernel m_sap2Kernel;
//int maxPairsPerBody = 64;
int maxBodiesPerCell = 256; //??
@ -42,8 +35,6 @@ m_hashGpu(ctx,q),
m_cellStartGpu(ctx, q),
m_paramsGPU(ctx, q)
{
b3Vector3 gridSize = b3MakeVector3(3, 3, 3);
b3Vector3 invGridSize = b3MakeVector3(1.f / gridSize[0], 1.f / gridSize[1], 1.f / gridSize[2]);
@ -70,7 +61,6 @@ m_paramsGPU(ctx,q)
}
{
cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, gridBroadphaseCL, &errNum, "", B3_GRID_BROADPHASE_PATH);
b3Assert(errNum == CL_SUCCESS);
@ -83,17 +73,11 @@ m_paramsGPU(ctx,q)
kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindCellStart", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindOverlappingPairs", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
}
m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
}
b3GpuGridBroadphase::~b3GpuGridBroadphase()
{
@ -104,13 +88,9 @@ b3GpuGridBroadphase::~b3GpuGridBroadphase()
clReleaseKernel(m_sap2Kernel);
clReleaseKernel(m_copyAabbsKernel);
delete m_sorter;
}
void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
{
b3SapAabb aabb;
@ -121,7 +101,6 @@ void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3
m_smallAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
m_allAabbsCPU1.push_back(aabb);
}
void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
{
@ -139,7 +118,6 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
{
B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
if (0)
{
calculateOverlappingPairsHost(maxPairs);
@ -155,10 +133,6 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
return;
}
int numSmallAabbs = m_smallAabbsMappingGPU.size();
b3OpenCLArray<int> pairCount(m_context, m_queue);
@ -195,9 +169,6 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
}
}
if (numSmallAabbs)
{
B3_PROFILE("gridKernel");
@ -219,7 +190,6 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
m_cellStartGpu.resize(numCells);
//b3AlignedObjectArray<int > cellStartCpu;
{
B3_PROFILE("kClearCellStart");
b3LauncherCL launch(m_queue, kClearCellStart, "kClearCellStart");
@ -228,10 +198,8 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
launch.launch1D(numCells);
//m_cellStartGpu.copyToHost(cellStartCpu);
//printf("??\n");
}
{
B3_PROFILE("kFindCellStart");
b3LauncherCL launch(m_queue, kFindCellStart, "kFindCellStart");
@ -241,13 +209,11 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
launch.launch1D(numSmallAabbs);
//m_cellStartGpu.copyToHost(cellStartCpu);
//printf("??\n");
}
{
B3_PROFILE("kFindOverlappingPairs");
b3LauncherCL launch(m_queue, kFindOverlappingPairs, "kFindOverlappingPairs");
launch.setConst(numSmallAabbs);
launch.setBuffer(m_allAabbsGPU1.getBufferCL());
@ -263,7 +229,6 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
launch.setConst(maxPairs);
launch.launch1D(numSmallAabbs);
int numPairs = pairCount.at(0);
if (numPairs > maxPairs)
{
@ -287,21 +252,13 @@ void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
printf("?!?\n");
}
}
}
//calculateOverlappingPairsHost(maxPairs);
}
void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
m_hostPairs.resize(0);
m_allAabbsGPU1.copyToHost(m_allAabbsCPU1);
for (int i = 0; i < m_allAabbsCPU1.size(); i++)
@ -318,7 +275,8 @@ void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
pair.x = a;
pair.y = b; //store the original index in the unsorted aabb array
} else
}
else
{
pair.x = b;
pair.y = a; //store the original index in the unsorted aabb array
@ -332,10 +290,7 @@ void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
}
}
m_gpuPairs.copyFromHost(m_hostPairs);
}
//call writeAabbsToGpu after done making all changes (createProxy etc)
@ -344,7 +299,6 @@ void b3GpuGridBroadphase::writeAabbsToGpu()
m_allAabbsGPU1.copyFromHost(m_allAabbsCPU1);
m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU);
m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
}
cl_mem b3GpuGridBroadphase::getAabbBufferWS()
@ -382,4 +336,3 @@ b3OpenCLArray<int>& b3GpuGridBroadphase::getLargeAabbIndicesGPU()
{
return m_largeAabbsMappingGPU;
}

8
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h vendored
View file

@ -6,7 +6,6 @@
struct b3ParamsGridBroadphaseCL
{
float m_invCellSize[4];
int m_gridSize[4];
@ -21,7 +20,6 @@ struct b3ParamsGridBroadphaseCL
}
};
class b3GpuGridBroadphase : public b3GpuBroadphaseInterface
{
protected:
@ -44,14 +42,12 @@ protected:
b3OpenCLArray<b3SortData> m_hashGpu;
b3OpenCLArray<int> m_cellStartGpu;
b3ParamsGridBroadphaseCL m_paramsCPU;
b3OpenCLArray<b3ParamsGridBroadphaseCL> m_paramsGPU;
class b3RadixSort32CL* m_sorter;
public:
b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q);
virtual ~b3GpuGridBroadphase();
@ -60,9 +56,6 @@ public:
return new b3GpuGridBroadphase(ctx, device, q);
}
virtual void createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
virtual void createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask);
@ -82,7 +75,6 @@ public:
virtual b3OpenCLArray<b3Int4>& getOverlappingPairsGPU();
virtual b3OpenCLArray<int>& getSmallAabbIndicesGPU();
virtual b3OpenCLArray<int>& getLargeAabbIndicesGPU();
};
#endif //B3_GPU_GRID_BROADPHASE_H

48
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvh.cpp vendored
View file

@ -16,8 +16,7 @@ subject to the following restrictions:
#include "b3GpuParallelLinearBvh.h"
b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) :
m_queue(queue),
b3GpuParallelLinearBvh::b3GpuParallelLinearBvh(cl_context context, cl_device_id device, cl_command_queue queue) : m_queue(queue),
m_radixSorter(context, device, queue),
m_rootNodeIndex(context, queue),
@ -130,8 +129,7 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
b3BufferInfoCL(worldSpaceAabbs.getBufferCL()),
b3BufferInfoCL(largeAabbIndices.getBufferCL()),
b3BufferInfoCL( m_largeAabbs.getBufferCL() )
};
b3BufferInfoCL(m_largeAabbs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -147,8 +145,7 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
b3BufferInfoCL(worldSpaceAabbs.getBufferCL()),
b3BufferInfoCL(smallAabbIndices.getBufferCL()),
b3BufferInfoCL( m_leafNodeAabbs.getBufferCL() )
};
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_separateAabbsKernel, "m_separateAabbsKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -240,8 +237,7 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
{
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL(m_mergedAabb.getBufferCL()),
b3BufferInfoCL( m_mortonCodesAndAabbIndicies.getBufferCL() )
};
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL())};
b3LauncherCL launcher(m_queue, m_assignMortonCodesAndAabbIndiciesKernel, "m_assignMortonCodesAndAabbIndiciesKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -262,7 +258,6 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
//
constructBinaryRadixTree();
//Since it is a sorted binary radix tree, each internal node contains a contiguous subset of leaf node indices.
//The root node contains leaf node indices in the range [0, numLeafNodes - 1].
//The child nodes of each node split their parent's index range into 2 contiguous halves.
@ -277,8 +272,7 @@ void b3GpuParallelLinearBvh::build(const b3OpenCLArray<b3SapAabb>& worldSpaceAab
b3BufferInfoCL bufferInfo[] =
{
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL( m_internalNodeLeafIndexRanges.getBufferCL() )
};
b3BufferInfoCL(m_internalNodeLeafIndexRanges.getBufferCL())};
b3LauncherCL launcher(m_queue, m_findLeafIndexRangesKernel, "m_findLeafIndexRangesKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -315,8 +309,7 @@ void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray<b3Int4>& ou
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(numPairsGpu.getBufferCL()),
b3BufferInfoCL( out_overlappingPairs.getBufferCL() )
};
b3BufferInfoCL(out_overlappingPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhCalculateOverlappingPairsKernel, "m_plbvhCalculateOverlappingPairsKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -340,8 +333,7 @@ void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray<b3Int4>& ou
b3BufferInfoCL(m_largeAabbs.getBufferCL()),
b3BufferInfoCL(numPairsGpu.getBufferCL()),
b3BufferInfoCL( out_overlappingPairs.getBufferCL() )
};
b3BufferInfoCL(out_overlappingPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhLargeAabbAabbTestKernel, "m_plbvhLargeAabbAabbTestKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -353,7 +345,6 @@ void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray<b3Int4>& ou
clFinish(m_queue);
}
//
int numPairs = -1;
numPairsGpu.copyToHostPointer(&numPairs, 1);
@ -367,7 +358,6 @@ void b3GpuParallelLinearBvh::calculateOverlappingPairs(b3OpenCLArray<b3Int4>& ou
out_overlappingPairs.resize(numPairs);
}
void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayInfo>& rays,
b3OpenCLArray<int>& out_numRayRigidPairs, b3OpenCLArray<b3Int2>& out_rayRigidPairs)
{
@ -397,8 +387,7 @@ void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayIn
b3BufferInfoCL(rays.getBufferCL()),
b3BufferInfoCL(out_numRayRigidPairs.getBufferCL()),
b3BufferInfoCL( out_rayRigidPairs.getBufferCL() )
};
b3BufferInfoCL(out_rayRigidPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhRayTraverseKernel, "m_plbvhRayTraverseKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -420,8 +409,7 @@ void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayIn
b3BufferInfoCL(rays.getBufferCL()),
b3BufferInfoCL(out_numRayRigidPairs.getBufferCL()),
b3BufferInfoCL( out_rayRigidPairs.getBufferCL() )
};
b3BufferInfoCL(out_rayRigidPairs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_plbvhLargeAabbRayTestKernel, "m_plbvhLargeAabbRayTestKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -439,7 +427,6 @@ void b3GpuParallelLinearBvh::testRaysAgainstBvhAabbs(const b3OpenCLArray<b3RayIn
if (numRayRigidPairs > maxRayRigidPairs)
b3Error("Error running out of rayRigid pairs: numRayRigidPairs = %d, maxRayRigidPairs = %d.\n", numRayRigidPairs, maxRayRigidPairs);
}
void b3GpuParallelLinearBvh::constructBinaryRadixTree()
@ -459,8 +446,7 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
{
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(m_commonPrefixes.getBufferCL()),
b3BufferInfoCL( m_commonPrefixLengths.getBufferCL() )
};
b3BufferInfoCL(m_commonPrefixLengths.getBufferCL())};
b3LauncherCL launcher(m_queue, m_computeAdjacentPairCommonPrefixKernel, "m_computeAdjacentPairCommonPrefixKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -479,8 +465,7 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
{
b3BufferInfoCL(m_commonPrefixLengths.getBufferCL()),
b3BufferInfoCL(m_leafNodeParentNodes.getBufferCL()),
b3BufferInfoCL( m_internalNodeChildNodes.getBufferCL() )
};
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL())};
b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeLeafNodesKernel, "m_buildBinaryRadixTreeLeafNodesKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -501,8 +486,7 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
b3BufferInfoCL(m_commonPrefixLengths.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_internalNodeParentNodes.getBufferCL()),
b3BufferInfoCL( m_rootNodeIndex.getBufferCL() )
};
b3BufferInfoCL(m_rootNodeIndex.getBufferCL())};
b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeInternalNodesKernel, "m_buildBinaryRadixTreeInternalNodesKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -523,8 +507,7 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
b3BufferInfoCL(m_rootNodeIndex.getBufferCL()),
b3BufferInfoCL(m_internalNodeParentNodes.getBufferCL()),
b3BufferInfoCL(m_maxDistanceFromRoot.getBufferCL()),
b3BufferInfoCL( m_distanceFromRoot.getBufferCL() )
};
b3BufferInfoCL(m_distanceFromRoot.getBufferCL())};
b3LauncherCL launcher(m_queue, m_findDistanceFromRootKernel, "m_findDistanceFromRootKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -555,8 +538,7 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
b3BufferInfoCL(m_mortonCodesAndAabbIndicies.getBufferCL()),
b3BufferInfoCL(m_internalNodeChildNodes.getBufferCL()),
b3BufferInfoCL(m_leafNodeAabbs.getBufferCL()),
b3BufferInfoCL( m_internalNodeAabbs.getBufferCL() )
};
b3BufferInfoCL(m_internalNodeAabbs.getBufferCL())};
b3LauncherCL launcher(m_queue, m_buildBinaryRadixTreeAabbsRecursiveKernel, "m_buildBinaryRadixTreeAabbsRecursiveKernel");
launcher.setBuffers(bufferInfo, sizeof(bufferInfo) / sizeof(b3BufferInfoCL));
@ -573,5 +555,3 @@ void b3GpuParallelLinearBvh::constructBinaryRadixTree()
clFinish(m_queue);
}
}

6
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuParallelLinearBvhBroadphase.cpp vendored
View file

@ -13,8 +13,7 @@ subject to the following restrictions:
#include "b3GpuParallelLinearBvhBroadphase.h"
b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) :
m_plbvh(context, device, queue),
b3GpuParallelLinearBvhBroadphase::b3GpuParallelLinearBvhBroadphase(cl_context context, cl_device_id device, cl_command_queue queue) : m_plbvh(context, device, queue),
m_overlappingPairsGpu(context, queue),
@ -75,6 +74,3 @@ void b3GpuParallelLinearBvhBroadphase::writeAabbsToGpu()
m_smallAabbsMappingGpu.copyFromHost(m_smallAabbsMappingCpu);
m_largeAabbsMappingGpu.copyFromHost(m_largeAabbsMappingCpu);
}

114
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp vendored
View file

@ -6,7 +6,6 @@ bool searchIncremental3dSapOnGpu = true;
#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.h"
#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
#include "kernels/sapKernels.h"
@ -91,7 +90,6 @@ m_gpuSmallSortedAabbs(ctx,q)
{
const char* sapSrc = sapCL;
cl_int errNum = 0;
b3Assert(m_context);
@ -99,7 +97,6 @@ m_gpuSmallSortedAabbs(ctx,q)
cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
b3Assert(errNum == CL_SUCCESS);
b3Assert(errNum == CL_SUCCESS);
#ifndef __APPLE__
m_prefixScanFloat4 = new b3PrefixScanFloat4CL(m_context, m_device, m_queue);
@ -144,15 +141,12 @@ m_gpuSmallSortedAabbs(ctx,q)
}
};
m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
b3Assert(errNum == CL_SUCCESS);
m_prepareSumVarianceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "prepareSumVarianceKernel", &errNum, sapProg);
b3Assert(errNum == CL_SUCCESS);
m_flipFloatKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "flipFloatKernel", &errNum, sapProg);
m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
@ -173,8 +167,6 @@ b3GpuSapBroadphase::~b3GpuSapBroadphase()
clReleaseKernel(m_sapKernel);
clReleaseKernel(m_sap2Kernel);
clReleaseKernel(m_prepareSumVarianceKernel);
}
/// conservative test for overlap between two aabbs
@ -188,8 +180,6 @@ static bool TestAabbAgainstAabb2(const b3Vector3 &aabbMin1, const b3Vector3 &aab
return overlap;
}
//http://stereopsis.com/radix.html
static unsigned int FloatFlip(float fl)
{
@ -243,23 +233,21 @@ void b3GpuSapBroadphase::init3dSap()
if (destIndex & 1)
{
m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].y = i;
} else
}
else
{
m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].x = i;
}
}
}
}
}
static bool b3PairCmp(const b3Int4& p, const b3Int4& q)
{
return ((p.x < q.x) || ((p.x == q.x) && (p.y < q.y)));
}
static bool operator==(const b3Int4& a, const b3Int4& b)
{
return a.x == b.x && a.y == b.y;
@ -285,7 +273,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
//static int framepje = 0;
//printf("framepje=%d\n",framepje++);
B3_PROFILE("calculateOverlappingPairsHostIncremental3Sap");
addedHostPairs.resize(0);
@ -301,7 +288,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
}
}
if (m_currentBuffer < 0)
return;
{
@ -328,7 +314,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
m_allAabbsCPU[53].m_max[0], m_allAabbsCPU[53].m_max[1], m_allAabbsCPU[53].m_max[2]);
}
{
b3Int4 newPair;
newPair.x = 40;
@ -345,22 +330,19 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
{
int prevOverlap = TestAabbAgainstAabb2((const b3Vector3&)preAabbs[40].m_min, (const b3Vector3&)preAabbs[40].m_max, (const b3Vector3&)preAabbs[53].m_min, (const b3Vector3&)preAabbs[53].m_max);
printf("prevoverlap=%d\n", prevOverlap);
} else
}
else
{
printf("unknown prevoverlap\n");
}
}
}
if (0)
{
for (int i = 0; i < m_allAabbsCPU.size(); i++)
{
//printf("aabb[%d] min=%f,%f,%f max=%f,%f,%f\n",i,m_allAabbsCPU[i].m_min[0],m_allAabbsCPU[i].m_min[1],m_allAabbsCPU[i].m_min[2], m_allAabbsCPU[i].m_max[0],m_allAabbsCPU[i].m_max[1],m_allAabbsCPU[i].m_max[2]);
}
for (int axis = 0; axis < 3; axis++)
@ -372,20 +354,14 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
}
}
m_currentBuffer = 1 - m_currentBuffer;
int totalNumAabbs = m_allAabbsCPU.size();
{
B3_PROFILE("assign m_sortedAxisCPU(FloatFlip)");
for (int i = 0; i < totalNumAabbs; i++)
{
unsigned int keyMin[3];
unsigned int keyMax[3];
for (int axis = 0; axis < 3; axis++)
@ -401,12 +377,9 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
m_sortedAxisCPU[axis][m_currentBuffer][i * 2 + 1].m_value = i * 2 + 1;
}
//printf("aabb[%d] min=%u,%u,%u max %u,%u,%u\n", i,keyMin[0],keyMin[1],keyMin[2],keyMax[0],keyMax[1],keyMax[2]);
}
}
{
B3_PROFILE("sort m_sortedAxisCPU");
for (int axis = 0; axis < 3; axis++)
@ -444,7 +417,8 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
if (destIndex & 1)
{
m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].y = i;
} else
}
else
{
m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].x = i;
}
@ -485,7 +459,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
}
#endif
int a = m_objectMinMaxIndexCPU[0][m_currentBuffer].size();
int b = m_objectMinMaxIndexCPU[1][m_currentBuffer].size();
int c = m_objectMinMaxIndexCPU[2][m_currentBuffer].size();
@ -593,8 +566,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
unsigned int prevMaxIndex = m_objectMinMaxIndexCPU[axis][1 - m_currentBuffer][i].y;
int dmax = curMaxIndex - prevMaxIndex;
if (dmin != 0)
{
@ -642,11 +613,8 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
prevOverlap = false;
}
//b3Assert(overlap==overlap2);
if (dmin < 0)
{
if (overlap && !prevOverlap)
@ -657,7 +625,8 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
{
newPair.x = i;
newPair.y = otherIndex;
} else
}
else
{
newPair.x = otherIndex;
newPair.y = i;
@ -669,14 +638,14 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
{
if (!overlap && prevOverlap)
{
//remove a pair
b3Int4 removedPair;
if (i <= otherIndex)
{
removedPair.x = i;
removedPair.y = otherIndex;
} else
}
else
{
removedPair.x = otherIndex;
removedPair.y = i;
@ -723,7 +692,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
prevOverlap = false;
}
if (dmax > 0)
{
if (overlap && !prevOverlap)
@ -734,13 +702,13 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
{
newPair.x = i;
newPair.y = otherIndex;
} else
}
else
{
newPair.x = otherIndex;
newPair.y = i;
}
addedHostPairs.push_back(newPair);
}
}
else
@ -754,13 +722,13 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
{
removedPair.x = i;
removedPair.y = otherIndex;
} else
}
else
{
removedPair.x = otherIndex;
removedPair.y = i;
}
removedHostPairs.push_back(removedPair);
}
}
@ -775,8 +743,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
//remove duplicates and add/remove then to existing m_overlappingPairs
{
{
B3_PROFILE("sort allPairs");
@ -807,13 +773,10 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
b3Int4 removedPair = removedHostPairs[i];
if ((removedPair.x != prevPair.x) || (removedPair.y != prevPair.y))
{
int index1 = allPairs.findBinarySearch(removedPair);
//#ifdef _DEBUG
int index2 = allPairs.findLinearSearch(removedPair);
b3Assert(index1 == index2);
@ -862,11 +825,9 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
//#ifdef _DEBUG
int index1 = allPairs.findBinarySearch(newPair);
int index2 = allPairs.findLinearSearch(newPair);
b3Assert(index1 == index2);
b3Assert(index1 == allPairs.size());
if (index1 != allPairs.size())
{
@ -892,18 +853,12 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
//if (uniqueAddedPairs)
// printf("uniqueAddedPairs=%d\n", uniqueAddedPairs);
{
B3_PROFILE("m_overlappingPairs.copyFromHost");
m_overlappingPairs.copyFromHost(allPairs);
}
}
void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
//test
@ -913,8 +868,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
b3Assert(m_allAabbsCPU.size() == m_allAabbsGPU.size());
m_allAabbsGPU.copyToHost(m_allAabbsCPU);
int axis = 0;
{
B3_PROFILE("CPU compute best variance axis");
@ -940,9 +893,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
axis = 2;
}
b3AlignedObjectArray<b3Int4> hostPairs;
{
@ -954,7 +904,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
for (int j = i + 1; j < numSmallAabbs; j++)
{
b3SapAabb smallAabbj = m_allAabbsCPU[m_smallAabbsMappingCPU[j]];
if (TestAabbAgainstAabb2((b3Vector3&)smallAabbi.m_min, (b3Vector3&)smallAabbi.m_max,
@ -967,7 +916,8 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
pair.x = a; //store the original index in the unsorted aabb array
pair.y = b;
} else
}
else
{
pair.x = b; //store the original index in the unsorted aabb array
pair.y = a;
@ -978,7 +928,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
}
}
{
int numSmallAabbs = m_smallAabbsMappingCPU.size();
for (int i = 0; i < numSmallAabbs; i++)
@ -1001,7 +950,8 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
{
pair.x = a;
pair.y = b; //store the original index in the unsorted aabb array
} else
}
else
{
pair.x = b;
pair.y = a; //store the original index in the unsorted aabb array
@ -1021,13 +971,13 @@ void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
if (hostPairs.size())
{
m_overlappingPairs.copyFromHost(hostPairs);
} else
}
else
{
m_overlappingPairs.resize(0);
}
//init3dSap();
}
void b3GpuSapBroadphase::reset()
@ -1035,7 +985,6 @@ void b3GpuSapBroadphase::reset()
m_allAabbsGPU.resize(0);
m_allAabbsCPU.resize(0);
m_smallAabbsMappingGPU.resize(0);
m_smallAabbsMappingCPU.resize(0);
@ -1043,10 +992,8 @@ void b3GpuSapBroadphase::reset()
m_largeAabbsMappingGPU.resize(0);
m_largeAabbsMappingCPU.resize(0);
}
void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
{
if (m_sapKernel == 0)
@ -1065,7 +1012,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
int axis = 0;
{
//bool syncOnHost = false;
int numSmallAabbs = m_smallAabbsMappingCPU.size();
@ -1092,7 +1038,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
int num = numSmallAabbs;
launcher.launch1D(num);
b3Vector3 s;
b3Vector3 s2;
m_prefixScanFloat4->execute(m_sum, m_dst, numSmallAabbs + 1, &s);
@ -1106,15 +1051,11 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
axis = 2;
}
m_gpuSmallSortData.resize(numSmallAabbs);
#if 1
if (m_smallAabbsMappingGPU.size())
{
B3_PROFILE("flipFloatKernel");
b3BufferInfoCL bInfo[] = {
b3BufferInfoCL(m_allAabbsGPU.getBufferCL(), true),
@ -1153,10 +1094,8 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
int num = numSmallAabbs;
launcher.launch1D(num);
clFinish(m_queue);
}
m_overlappingPairs.resize(maxPairs);
m_pairCount.resize(0);
@ -1202,7 +1141,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
launcher.setConst(axis);
launcher.setConst(maxPairs);
int num = numSmallAabbs;
#if 0
int buffSize = launcher.getSerializationBufferSize();
@ -1243,7 +1181,6 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
#else
int numPairs = 0;
b3LauncherCL launcher(m_queue, m_sapKernel);
const char* fileName = "m_sapKernelArgs.bin";
@ -1277,17 +1214,16 @@ void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
//printf("hello %d\n", m_overlappingPairs.size());
free(buf);
fclose(f);
} else {
}
else
{
printf("error: cannot find file %s\n", fileName);
}
clFinish(m_queue);
#endif
m_overlappingPairs.resize(numPairs);
} //B3_PROFILE("GPU_RADIX SORT");
@ -1300,9 +1236,6 @@ void b3GpuSapBroadphase::writeAabbsToGpu()
m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
m_allAabbsGPU.copyFromHost(m_allAabbsCPU); //might not be necessary, the 'setupGpuAabbsFull' already takes care of this
}
void b3GpuSapBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
@ -1334,7 +1267,6 @@ void b3GpuSapBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3&
aabb.m_signedMaxIndices[3] = m_allAabbsCPU.size();
m_smallAabbsMappingCPU.push_back(m_allAabbsCPU.size());
m_allAabbsCPU.push_back(aabb);
}

8
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h vendored
View file

@ -11,10 +11,8 @@ class b3Vector3;
#include "b3GpuBroadphaseInterface.h"
class b3GpuSapBroadphase : public b3GpuBroadphaseInterface
{
cl_context m_context;
cl_device_id m_device;
cl_command_queue m_queue;
@ -25,7 +23,6 @@ class b3GpuSapBroadphase : public b3GpuBroadphaseInterface
cl_kernel m_sap2Kernel;
cl_kernel m_prepareSumVarianceKernel;
class b3RadixSort32CL* m_sorter;
///test for 3d SAP
@ -45,7 +42,6 @@ class b3GpuSapBroadphase : public b3GpuBroadphaseInterface
b3OpenCLArray<b3SortData> m_sortedAxisGPU1prev;
b3OpenCLArray<b3SortData> m_sortedAxisGPU2prev;
b3OpenCLArray<b3Int4> m_addedHostPairsGPU;
b3OpenCLArray<b3Int4> m_removedHostPairsGPU;
b3OpenCLArray<int> m_addedCountGPU;
@ -54,10 +50,8 @@ class b3GpuSapBroadphase : public b3GpuBroadphaseInterface
int m_currentBuffer;
public:
b3OpenCLArray<int> m_pairCount;
b3OpenCLArray<b3SapAabb> m_allAabbsGPU;
b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU;
@ -80,7 +74,6 @@ public:
b3OpenCLArray<int> m_largeAabbsMappingGPU;
b3AlignedObjectArray<int> m_largeAabbsMappingCPU;
b3OpenCLArray<b3Int4> m_overlappingPairs;
//temporary gpu work memory
@ -124,7 +117,6 @@ public:
return new b3GpuSapBroadphase(ctx, device, q, B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY);
}
virtual void calculateOverlappingPairs(int maxPairs);
virtual void calculateOverlappingPairsHost(int maxPairs);

5
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h vendored
View file

@ -5,10 +5,9 @@
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
///just make sure that the b3Aabb is 16-byte aligned
B3_ATTRIBUTE_ALIGNED16(struct) b3SapAabb : public b3Aabb
{
B3_ATTRIBUTE_ALIGNED16(struct)
b3SapAabb : public b3Aabb{
};
#endif //B3_SAP_AABB_H

5
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphaseKernels.h vendored
View file

@ -1,5 +1,5 @@
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
static const char* gridBroadphaseCL= \
static const char* gridBroadphaseCL =
"int getPosHash(int4 gridPos, __global float4* pParams)\n"
"{\n"
" int4 gridDim = *((__global int4*)(pParams + 1));\n"
@ -195,5 +195,4 @@ static const char* gridBroadphaseCL= \
" }\n"
" }\n"
" }\n"
"}\n"
;
"}\n";

5
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/parallelLinearBvhKernels.h vendored
View file

@ -1,5 +1,5 @@
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
static const char* parallelLinearBvhCL= \
static const char* parallelLinearBvhCL =
"/*\n"
"This software is provided 'as-is', without any express or implied warranty.\n"
"In no event will the authors be held liable for any damages arising from the use of this software.\n"
@ -725,5 +725,4 @@ static const char* parallelLinearBvhCL= \
" \n"
" //\n"
" out_leafIndexRanges[internalNodeIndex] = leafIndexRange;\n"
"}\n"
;
"}\n";

5
thirdparty/bullet/Bullet3OpenCL/BroadphaseCollision/kernels/sapKernels.h vendored
View file

@ -1,5 +1,5 @@
//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
static const char* sapCL= \
static const char* sapCL =
"/*\n"
"Copyright (c) 2012 Advanced Micro Devices, Inc. \n"
"This software is provided 'as-is', without any express or implied warranty.\n"
@ -338,5 +338,4 @@ static const char* sapCL= \
" s = (smallAabb.m_max+smallAabb.m_min)*0.5f;\n"
" sum[i]=s;\n"
" sum2[i]=s*s; \n"
"}\n"
;
"}\n";

9
thirdparty/bullet/Bullet3OpenCL/Initialize/b3OpenCLInclude.h vendored
View file

@ -41,8 +41,11 @@ subject to the following restrictions:
#include <assert.h>
#include <stdio.h>
#define oclCHECKERROR(a, b) if((a)!=(b)) { printf("OCL Error : %d\n", (a)); assert((a) == (b)); }
#define oclCHECKERROR(a, b) \
if ((a) != (b)) \
{ \
printf("OCL Error : %d\n", (a)); \
assert((a) == (b)); \
}
#endif //B3_OPENCL_INCLUDE_H

96
thirdparty/bullet/Bullet3OpenCL/Initialize/b3OpenCLUtils.cpp vendored
View file

@ -16,7 +16,6 @@ subject to the following restrictions:
//Original author: Roman Ponomarev
//Mostly Reimplemented by Erwin Coumans
bool gDebugForceLoadingFromSource = false;
bool gDebugSkipLoadingBinary = false;
@ -48,7 +47,6 @@ bool gDebugSkipLoadingBinary = false;
static const char* sCachedBinaryPath = "cache";
//Set the preferred platform vendor using the OpenCL SDK
static const char* spPlatformVendor =
#if defined(CL_PLATFORM_MINI_CL)
@ -73,26 +71,23 @@ static const char* spPlatformVendor =
#endif //_WIN32
#endif
void MyFatalBreakAPPLE(const char* errstr,
const void* private_info,
size_t cb,
void* user_data)
{
const char* patloc = strstr(errstr, "Warning");
//find out if it is a warning or error, exit if error
if (patloc)
{
b3Warning("Warning: %s\n", errstr);
} else
}
else
{
b3Error("Error: %s\n", errstr);
b3Assert(0);
}
}
#ifdef B3_USE_CLEW
@ -105,14 +100,15 @@ int b3OpenCLUtils_clewInit()
const char* cl = "OpenCL.dll";
#elif defined __APPLE__
const char* cl = "/System/Library/Frameworks/OpenCL.framework/Versions/Current/OpenCL";
#else//presumable Linux?
#else //presumable Linux? \
//linux (tested on Ubuntu 12.10 with Catalyst 13.4 beta drivers, not that there is no symbolic link from libOpenCL.so
const char* cl = "libOpenCL.so.1";
result = clewInit(cl);
if (result != CLEW_SUCCESS)
{
cl = "libOpenCL.so";
} else
}
else
{
clewExit();
}
@ -148,7 +144,6 @@ int b3OpenCLUtils_getNumPlatforms(cl_int* pErrNum)
*pErrNum = ciErrNum;
}
return numPlatforms;
}
const char* b3OpenCLUtils_getSdkVendorName()
@ -213,8 +208,6 @@ void b3OpenCLUtils_printPlatformInfo( cl_platform_id platform)
b3Printf(" CL_PLATFORM_VERSION: \t\t\t%s\n", platformInfo.m_platformVersion);
}
cl_context b3OpenCLUtils_createContextFromPlatform(cl_platform_id platform, cl_device_type deviceType, cl_int* pErrNum, void* pGLContext, void* pGLDC, int preferredDeviceIndex, int preferredPlatformIndex)
{
cl_context retContext = 0;
@ -244,7 +237,6 @@ cl_context b3OpenCLUtils_createContextFromPlatform(cl_platform_id platform, cl_d
#endif //_WIN32
num_entries = B3_MAX_CL_DEVICES;
num_devices = -1;
ciErrNum = clGetDeviceIDs(
@ -281,7 +273,8 @@ cl_context b3OpenCLUtils_createContextFromPlatform(cl_platform_id platform, cl_d
{
//create a context of the preferred device index
retContext = clCreateContext(cprops, 1, &devices[preferredDeviceIndex], NULL, NULL, &ciErrNum);
} else
}
else
{
//create a context of all devices
#if defined(__APPLE__)
@ -307,7 +300,6 @@ cl_context b3OpenCLUtils_createContextFromType(cl_device_type deviceType, cl_int
b3OpenCLUtils_clewInit();
#endif
cl_uint numPlatforms;
cl_context retContext = 0;
unsigned int i;
@ -330,8 +322,6 @@ cl_context b3OpenCLUtils_createContextFromType(cl_device_type deviceType, cl_int
return NULL;
}
for (i = 0; i < numPlatforms; ++i)
{
char pbuf[128];
@ -352,7 +342,8 @@ cl_context b3OpenCLUtils_createContextFromType(cl_device_type deviceType, cl_int
platforms[0] = platforms[i];
platforms[i] = tmpPlatform;
break;
} else
}
else
{
if (!strcmp(pbuf, spPlatformVendor))
{
@ -389,7 +380,6 @@ cl_context b3OpenCLUtils_createContextFromType(cl_device_type deviceType, cl_int
return retContext;
}
//////////////////////////////////////////////////////////////////////////////
//! Gets the id of the nth device from the context
//!
@ -408,7 +398,8 @@ cl_device_id b3OpenCLUtils_getDevice(cl_context cxMainContext, int deviceIndex)
// get the list of devices associated with context
clGetContextInfo(cxMainContext, CL_CONTEXT_DEVICES, 0, NULL, &szParmDataBytes);
if( szParmDataBytes / sizeof(cl_device_id) < (unsigned int)deviceIndex ) {
if (szParmDataBytes / sizeof(cl_device_id) < (unsigned int)deviceIndex)
{
return (cl_device_id)-1;
}
@ -431,8 +422,6 @@ int b3OpenCLUtils_getNumDevices(cl_context cxMainContext)
return device_count;
}
void b3OpenCLUtils::getDeviceInfo(cl_device_id device, b3OpenCLDeviceInfo* info)
{
// CL_DEVICE_NAME
@ -514,7 +503,6 @@ void b3OpenCLUtils::getDeviceInfo(cl_device_id device, b3OpenCLDeviceInfo* info)
clGetDeviceInfo(device, CL_DEVICE_PREFERRED_VECTOR_WIDTH_DOUBLE, sizeof(cl_uint), &info->m_vecWidthDouble, NULL);
}
void b3OpenCLUtils_printDeviceInfo(cl_device_id device)
{
b3OpenCLDeviceInfo info;
@ -571,11 +559,8 @@ void b3OpenCLUtils_printDeviceInfo(cl_device_id device)
b3Printf(" CL_DEVICE_PREFERRED_VECTOR_WIDTH_<t>\t");
b3Printf("CHAR %u, SHORT %u, INT %u,LONG %u, FLOAT %u, DOUBLE %u\n\n\n",
info.m_vecWidthChar, info.m_vecWidthShort, info.m_vecWidthInt, info.m_vecWidthLong, info.m_vecWidthFloat, info.m_vecWidthDouble);
}
static const char* strip2(const char* name, const char* pattern)
{
size_t const patlen = strlen(pattern);
@ -627,19 +612,14 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
}
if (clFileNameForCaching && !(disableBinaryCaching || gDebugSkipLoadingBinary || gDebugForceLoadingFromSource))
{
#ifdef _WIN32
char* bla = 0;
//printf("searching for %s\n", binaryFileName);
FILETIME modtimeBinary;
CreateDirectoryA(sCachedBinaryPath, 0);
{
HANDLE binaryFileHandle = CreateFileA(binaryFileName, GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
if (binaryFileHandle == INVALID_HANDLE_VALUE)
{
@ -662,14 +642,16 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
b3Warning("\nFailed reading cached file with errorCode = %d\n", errorCode);
}
}
} else
}
else
{
if (GetFileTime(binaryFileHandle, NULL, NULL, &modtimeBinary) == 0)
{
DWORD errorCode;
errorCode = GetLastError();
b3Warning("\nGetFileTime errorCode = %d\n", errorCode);
} else
}
else
{
binaryFileValid = 1;
}
@ -689,10 +671,8 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
sprintf(relativeFileName, "%s%s", prefix[i], clFileNameForCaching);
srcFileHandle = CreateFileA(relativeFileName, GENERIC_READ, 0, 0, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, 0);
}
}
if (srcFileHandle != INVALID_HANDLE_VALUE)
{
FILETIME modtimeSrc;
@ -702,11 +682,11 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
errorCode = GetLastError();
b3Warning("\nGetFileTime errorCode = %d\n", errorCode);
}
if ( ( modtimeSrc.dwHighDateTime < modtimeBinary.dwHighDateTime)
||(( modtimeSrc.dwHighDateTime == modtimeBinary.dwHighDateTime)&&(modtimeSrc.dwLowDateTime <= modtimeBinary.dwLowDateTime)))
if ((modtimeSrc.dwHighDateTime < modtimeBinary.dwHighDateTime) || ((modtimeSrc.dwHighDateTime == modtimeBinary.dwHighDateTime) && (modtimeSrc.dwLowDateTime <= modtimeBinary.dwLowDateTime)))
{
fileUpToDate = 1;
} else
}
else
{
b3Warning("\nCached binary file out-of-date (%s)\n", binaryFileName);
}
@ -745,12 +725,8 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
#endif
}
}
}
#else
fileUpToDate = true;
if (mkdir(sCachedBinaryPath, 0777) == -1)
@ -763,7 +739,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
#endif //_WIN32
}
if (fileUpToDate)
{
#ifdef _MSC_VER
@ -806,43 +781,31 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
m_cpProgram = 0;
b3Warning("clBuildProgram reported failure on cached binary: %s\n", binaryFileName);
} else
}
else
{
b3Printf("clBuildProgram successfully compiled cached binary: %s\n", binaryFileName);
}
free(binary);
} else
}
else
{
b3Warning("Cannot open cached binary: %s\n", binaryFileName);
}
}
if (!m_cpProgram)
{
cl_int localErrNum;
char* compileFlags;
int flagsize;
const char* kernelSource = kernelSourceOrg;
if (!kernelSourceOrg || gDebugForceLoadingFromSource)
{
if (clFileNameForCaching)
{
FILE* file = fopen(clFileNameForCaching, "rb");
//in many cases the relative path is a few levels up the directory hierarchy, so try it
if (!file)
@ -879,7 +842,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
const char* flags = "";
#endif
m_cpProgram = clCreateProgramWithSource(clContext, 1, (const char**)&kernelSource, &program_length, &localErrNum);
if (localErrNum != CL_SUCCESS)
{
@ -890,8 +852,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
// Build the program with 'mad' Optimization option
flagsize = sizeof(char) * (strlen(additionalMacros) + strlen(flags) + 5);
compileFlags = (char*)malloc(flagsize);
#ifdef _MSC_VER
@ -912,7 +872,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
// there's no information in the reference whether the string is 0 terminated or not
build_log[ret_val_size] = '\0';
b3Error("Error in clBuildProgram, Line %u in file %s, Log: \n%s\n !!!\n\n", __LINE__, __FILE__, build_log);
free(build_log);
if (pErrNum)
@ -920,7 +879,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
return 0;
}
if (!disableBinaryCaching && clFileNameForCaching)
{ // write to binary
@ -929,7 +887,6 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
b3Assert(status == CL_SUCCESS);
if (numAssociatedDevices == 1)
{
size_t binarySize;
char* binary;
@ -953,7 +910,8 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
{
fwrite(binary, sizeof(char), binarySize, file);
fclose(file);
} else
}
else
{
b3Warning("cannot write file %s\n", binaryFileName);
}
@ -964,15 +922,12 @@ cl_program b3OpenCLUtils_compileCLProgramFromString(cl_context clContext, cl_dev
}
free(compileFlags);
}
return m_cpProgram;
}
cl_kernel b3OpenCLUtils_compileCLKernelFromString(cl_context clContext, cl_device_id device, const char* kernelSource, const char* kernelName, cl_int* pErrNum, cl_program prog, const char* additionalMacros)
{
cl_kernel kernel;
cl_int localErrNum;
@ -985,7 +940,6 @@ cl_kernel b3OpenCLUtils_compileCLKernelFromString(cl_context clContext, cl_devic
m_cpProgram = b3OpenCLUtils_compileCLProgramFromString(clContext, device, kernelSource, pErrNum, additionalMacros, 0, false);
}
// Create the kernel
kernel = clCreateKernel(m_cpProgram, kernelName, &localErrNum);
if (localErrNum != CL_SUCCESS)
@ -1003,9 +957,7 @@ cl_kernel b3OpenCLUtils_compileCLKernelFromString(cl_context clContext, cl_devic
}
b3Printf("ready. \n");
if (pErrNum)
*pErrNum = CL_SUCCESS;
return kernel;
}

8
thirdparty/bullet/Bullet3OpenCL/Initialize/b3OpenCLUtils.h vendored
View file

@ -22,10 +22,10 @@ subject to the following restrictions:
#include "b3OpenCLInclude.h"
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
///C API for OpenCL utilities: convenience functions, see below for C++ API
/// CL Context optionally takes a GL context. This is a generic type because we don't really want this code
@ -49,7 +49,6 @@ int b3OpenCLUtils_getNumPlatforms(cl_int* pErrNum);
///get the nr'th platform, where nr is in the range [0..getNumPlatforms)
cl_platform_id b3OpenCLUtils_getPlatform(int nr, cl_int* pErrNum);
void b3OpenCLUtils_printPlatformInfo(cl_platform_id platform);
const char* b3OpenCLUtils_getSdkVendorName();
@ -90,8 +89,6 @@ typedef struct
cl_uint m_maxReadImageArgs;
cl_uint m_maxWriteImageArgs;
cl_uint m_addressBits;
cl_ulong m_maxMemAllocSize;
cl_command_queue_properties m_queueProperties;
@ -119,7 +116,6 @@ struct b3OpenCLPlatformInfo
}
};
///C++ API for OpenCL utilities: convenience functions
struct b3OpenCLUtils
{

Some files were not shown because too many files have changed in this diff Show more