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

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

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

12
src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h
View File

@@ -3,24 +3,24 @@
#define B3_BROADPHASE_CALLBACK_H
#include "Bullet3Common/b3Vector3.h"
struct btBroadphaseProxy;
struct b3BroadphaseProxy;
struct btBroadphaseAabbCallback
struct b3BroadphaseAabbCallback
{
virtual ~btBroadphaseAabbCallback() {}
virtual bool process(const btBroadphaseProxy* proxy) = 0;
virtual ~b3BroadphaseAabbCallback() {}
virtual bool process(const b3BroadphaseProxy* proxy) = 0;
};
struct btBroadphaseRayCallback : public btBroadphaseAabbCallback
struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
{
///added some cached data to accelerate ray-AABB tests
b3Vector3 m_rayDirectionInverse;
unsigned int m_signs[3];
b3Scalar m_lambda_max;
virtual ~btBroadphaseRayCallback() {}
virtual ~b3BroadphaseRayCallback() {}
};
#endif //B3_BROADPHASE_CALLBACK_H

316
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp
View File

@@ -17,38 +17,38 @@ subject to the following restrictions:
#include "b3DynamicBvh.h"
//
typedef b3AlignedObjectArray<btDbvtNode*> tNodeArray;
typedef b3AlignedObjectArray<const btDbvtNode*> tConstNodeArray;
typedef b3AlignedObjectArray<b3DbvtNode*> tNodeArray;
typedef b3AlignedObjectArray<const b3DbvtNode*> tConstNodeArray;
//
struct btDbvtNodeEnumerator : b3DynamicBvh::ICollide
struct b3DbvtNodeEnumerator : b3DynamicBvh::ICollide
{
tConstNodeArray nodes;
void Process(const btDbvtNode* n) { nodes.push_back(n); }
void Process(const b3DbvtNode* n) { nodes.push_back(n); }
};
//
static DBVT_INLINE int indexof(const btDbvtNode* node)
static DBVT_INLINE int indexof(const b3DbvtNode* node)
{
return(node->parent->childs[1]==node);
}
//
static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a,
const btDbvtVolume& b)
static DBVT_INLINE b3DbvtVolume merge( const b3DbvtVolume& a,
const b3DbvtVolume& b)
{
#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
btDbvtVolume& res=*(btDbvtVolume*)locals;
ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtAabbMm)]);
b3DbvtVolume& res=*(b3DbvtVolume*)locals;
#else
btDbvtVolume res;
b3DbvtVolume res;
#endif
Merge(a,b,res);
return(res);
}
// volume+edge lengths
static DBVT_INLINE b3Scalar size(const btDbvtVolume& a)
static DBVT_INLINE b3Scalar size(const b3DbvtVolume& a)
{
const b3Vector3 edges=a.Lengths();
return( edges.x*edges.y*edges.z+
@@ -56,26 +56,26 @@ static DBVT_INLINE b3Scalar size(const btDbvtVolume& a)
}
//
static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
static void getmaxdepth(const b3DbvtNode* node,int depth,int& maxdepth)
{
if(node->isinternal())
{
getmaxdepth(node->childs[0],depth+1,maxdepth);
getmaxdepth(node->childs[1],depth+1,maxdepth);
} else maxdepth=btMax(maxdepth,depth);
} else maxdepth=b3Max(maxdepth,depth);
}
//
static DBVT_INLINE void deletenode( b3DynamicBvh* pdbvt,
btDbvtNode* node)
b3DbvtNode* node)
{
btAlignedFree(pdbvt->m_free);
b3AlignedFree(pdbvt->m_free);
pdbvt->m_free=node;
}
//
static void recursedeletenode( b3DynamicBvh* pdbvt,
btDbvtNode* node)
b3DbvtNode* node)
{
if(!node->isleaf())
{
@@ -87,15 +87,15 @@ static void recursedeletenode( b3DynamicBvh* pdbvt,
}
//
static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt,
btDbvtNode* parent,
static DBVT_INLINE b3DbvtNode* createnode( b3DynamicBvh* pdbvt,
b3DbvtNode* parent,
void* data)
{
btDbvtNode* node;
b3DbvtNode* node;
if(pdbvt->m_free)
{ node=pdbvt->m_free;pdbvt->m_free=0; }
else
{ node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
{ node=new(b3AlignedAlloc(sizeof(b3DbvtNode),16)) b3DbvtNode(); }
node->parent = parent;
node->data = data;
node->childs[1] = 0;
@@ -103,32 +103,32 @@ static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt,
}
//
static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt,
btDbvtNode* parent,
const btDbvtVolume& volume,
static DBVT_INLINE b3DbvtNode* createnode( b3DynamicBvh* pdbvt,
b3DbvtNode* parent,
const b3DbvtVolume& volume,
void* data)
{
btDbvtNode* node=createnode(pdbvt,parent,data);
b3DbvtNode* node=createnode(pdbvt,parent,data);
node->volume=volume;
return(node);
}
//
static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt,
btDbvtNode* parent,
const btDbvtVolume& volume0,
const btDbvtVolume& volume1,
static DBVT_INLINE b3DbvtNode* createnode( b3DynamicBvh* pdbvt,
b3DbvtNode* parent,
const b3DbvtVolume& volume0,
const b3DbvtVolume& volume1,
void* data)
{
btDbvtNode* node=createnode(pdbvt,parent,data);
b3DbvtNode* node=createnode(pdbvt,parent,data);
Merge(volume0,volume1,node->volume);
return(node);
}
//
static void insertleaf( b3DynamicBvh* pdbvt,
btDbvtNode* root,
btDbvtNode* leaf)
b3DbvtNode* root,
b3DbvtNode* leaf)
{
if(!pdbvt->m_root)
{
@@ -145,8 +145,8 @@ static void insertleaf( b3DynamicBvh* pdbvt,
root->childs[1]->volume)];
} while(!root->isleaf());
}
btDbvtNode* prev=root->parent;
btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
b3DbvtNode* prev=root->parent;
b3DbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
if(prev)
{
prev->childs[indexof(root)] = node;
@@ -170,8 +170,8 @@ static void insertleaf( b3DynamicBvh* pdbvt,
}
//
static btDbvtNode* removeleaf( b3DynamicBvh* pdbvt,
btDbvtNode* leaf)
static b3DbvtNode* removeleaf( b3DynamicBvh* pdbvt,
b3DbvtNode* leaf)
{
if(leaf==pdbvt->m_root)
{
@@ -180,9 +180,9 @@ static btDbvtNode* removeleaf( b3DynamicBvh* pdbvt,
}
else
{
btDbvtNode* parent=leaf->parent;
btDbvtNode* prev=parent->parent;
btDbvtNode* sibling=parent->childs[1-indexof(leaf)];
b3DbvtNode* parent=leaf->parent;
b3DbvtNode* prev=parent->parent;
b3DbvtNode* sibling=parent->childs[1-indexof(leaf)];
if(prev)
{
prev->childs[indexof(parent)]=sibling;
@@ -190,7 +190,7 @@ static btDbvtNode* removeleaf( b3DynamicBvh* pdbvt,
deletenode(pdbvt,parent);
while(prev)
{
const btDbvtVolume pb=prev->volume;
const b3DbvtVolume pb=prev->volume;
Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
if(NotEqual(pb,prev->volume))
{
@@ -211,7 +211,7 @@ static btDbvtNode* removeleaf( b3DynamicBvh* pdbvt,
//
static void fetchleaves(b3DynamicBvh* pdbvt,
btDbvtNode* root,
b3DbvtNode* root,
tNodeArray& leaves,
int depth=-1)
{
@@ -238,7 +238,7 @@ static void split( const tNodeArray& leaves,
right.resize(0);
for(int i=0,ni=leaves.size();i<ni;++i)
{
if(btDot(axis,leaves[i]->volume.Center()-org)<0)
if(b3Dot(axis,leaves[i]->volume.Center()-org)<0)
left.push_back(leaves[i]);
else
right.push_back(leaves[i]);
@@ -246,14 +246,14 @@ static void split( const tNodeArray& leaves,
}
//
static btDbvtVolume bounds( const tNodeArray& leaves)
static b3DbvtVolume bounds( const tNodeArray& leaves)
{
#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]);
btDbvtVolume& volume=*(btDbvtVolume*)locals;
ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]);
b3DbvtVolume& volume=*(b3DbvtVolume*)locals;
volume=leaves[0]->volume;
#else
btDbvtVolume volume=leaves[0]->volume;
b3DbvtVolume volume=leaves[0]->volume;
#endif
for(int i=1,ni=leaves.size();i<ni;++i)
{
@@ -283,8 +283,8 @@ static void bottomup( b3DynamicBvh* pdbvt,
}
}
}
btDbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]};
btDbvtNode* p = createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
b3DbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]};
b3DbvtNode* p = createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
p->childs[0] = n[0];
p->childs[1] = n[1];
n[0]->parent = p;
@@ -296,7 +296,7 @@ static void bottomup( b3DynamicBvh* pdbvt,
}
//
static btDbvtNode* topdown(b3DynamicBvh* pdbvt,
static b3DbvtNode* topdown(b3DynamicBvh* pdbvt,
tNodeArray& leaves,
int bu_treshold)
{
@@ -307,7 +307,7 @@ static btDbvtNode* topdown(b3DynamicBvh* pdbvt,
{
if(leaves.size()>bu_treshold)
{
const btDbvtVolume vol=bounds(leaves);
const b3DbvtVolume vol=bounds(leaves);
const b3Vector3 org=vol.Center();
tNodeArray sets[2];
int bestaxis=-1;
@@ -319,14 +319,14 @@ static btDbvtNode* topdown(b3DynamicBvh* pdbvt,
const b3Vector3 x=leaves[i]->volume.Center()-org;
for(int j=0;j<3;++j)
{
++splitcount[j][btDot(x,axis[j])>0?1:0];
++splitcount[j][b3Dot(x,axis[j])>0?1:0];
}
}
for( i=0;i<3;++i)
{
if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
{
const int midp=(int)btFabs(b3Scalar(splitcount[i][0]-splitcount[i][1]));
const int midp=(int)b3Fabs(b3Scalar(splitcount[i][0]-splitcount[i][1]));
if(midp<bestmidp)
{
bestaxis=i;
@@ -349,7 +349,7 @@ static btDbvtNode* topdown(b3DynamicBvh* pdbvt,
sets[i&1].push_back(leaves[i]);
}
}
btDbvtNode* node=createnode(pdbvt,0,vol,0);
b3DbvtNode* node=createnode(pdbvt,0,vol,0);
node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
node->childs[0]->parent=node;
@@ -366,17 +366,17 @@ static btDbvtNode* topdown(b3DynamicBvh* pdbvt,
}
//
static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r)
static DBVT_INLINE b3DbvtNode* sort(b3DbvtNode* n,b3DbvtNode*& r)
{
btDbvtNode* p=n->parent;
btAssert(n->isinternal());
b3DbvtNode* p=n->parent;
b3Assert(n->isinternal());
if(p>n)
{
const int i=indexof(n);
const int j=1-i;
btDbvtNode* s=p->childs[j];
btDbvtNode* q=p->parent;
btAssert(n==p->childs[i]);
b3DbvtNode* s=p->childs[j];
b3DbvtNode* q=p->parent;
b3Assert(n==p->childs[i]);
if(q) q->childs[indexof(p)]=n; else r=n;
s->parent=n;
p->parent=n;
@@ -387,14 +387,14 @@ static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r)
n->childs[1]->parent=p;
n->childs[i]=p;
n->childs[j]=s;
btSwap(p->volume,n->volume);
b3Swap(p->volume,n->volume);
return(p);
}
return(n);
}
#if 0
static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count)
static DBVT_INLINE b3DbvtNode* walkup(b3DbvtNode* n,int count)
{
while(n&&(count--)) n=n->parent;
return(n);
@@ -426,7 +426,7 @@ void b3DynamicBvh::clear()
{
if(m_root)
recursedeletenode(this,m_root);
btAlignedFree(m_free);
b3AlignedFree(m_free);
m_free=0;
m_lkhd = -1;
m_stkStack.clear();
@@ -466,7 +466,7 @@ void b3DynamicBvh::optimizeIncremental(int passes)
if(m_root&&(passes>0))
{
do {
btDbvtNode* node=m_root;
b3DbvtNode* node=m_root;
unsigned bit=0;
while(node->isinternal())
{
@@ -480,18 +480,18 @@ void b3DynamicBvh::optimizeIncremental(int passes)
}
//
btDbvtNode* b3DynamicBvh::insert(const btDbvtVolume& volume,void* data)
b3DbvtNode* b3DynamicBvh::insert(const b3DbvtVolume& volume,void* data)
{
btDbvtNode* leaf=createnode(this,0,volume,data);
b3DbvtNode* leaf=createnode(this,0,volume,data);
insertleaf(this,m_root,leaf);
++m_leaves;
return(leaf);
}
//
void b3DynamicBvh::update(btDbvtNode* leaf,int lookahead)
void b3DynamicBvh::update(b3DbvtNode* leaf,int lookahead)
{
btDbvtNode* root=removeleaf(this,leaf);
b3DbvtNode* root=removeleaf(this,leaf);
if(root)
{
if(lookahead>=0)
@@ -506,9 +506,9 @@ void b3DynamicBvh::update(btDbvtNode* leaf,int lookahead)
}
//
void b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume)
void b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume)
{
btDbvtNode* root=removeleaf(this,leaf);
b3DbvtNode* root=removeleaf(this,leaf);
if(root)
{
if(m_lkhd>=0)
@@ -524,7 +524,7 @@ void b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume)
}
//
bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin)
bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin)
{
if(leaf->volume.Contain(volume)) return(false);
volume.Expand(b3Vector3(margin,margin,margin));
@@ -534,7 +534,7 @@ bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector
}
//
bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity)
bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity)
{
if(leaf->volume.Contain(volume)) return(false);
volume.SignedExpand(velocity);
@@ -543,7 +543,7 @@ bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector
}
//
bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,b3Scalar margin)
bool b3DynamicBvh::update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin)
{
if(leaf->volume.Contain(volume)) return(false);
volume.Expand(b3Vector3(margin,margin,margin));
@@ -552,7 +552,7 @@ bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,b3Scalar margi
}
//
void b3DynamicBvh::remove(btDbvtNode* leaf)
void b3DynamicBvh::remove(b3DbvtNode* leaf)
{
removeleaf(this,leaf);
deletenode(this,leaf);
@@ -562,13 +562,13 @@ void b3DynamicBvh::remove(btDbvtNode* leaf)
//
void b3DynamicBvh::write(IWriter* iwriter) const
{
btDbvtNodeEnumerator nodes;
b3DbvtNodeEnumerator nodes;
nodes.nodes.reserve(m_leaves*2);
enumNodes(m_root,nodes);
iwriter->Prepare(m_root,nodes.nodes.size());
for(int i=0;i<nodes.nodes.size();++i)
{
const btDbvtNode* n=nodes.nodes[i];
const b3DbvtNode* n=nodes.nodes[i];
int p=-1;
if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
if(n->isinternal())
@@ -596,7 +596,7 @@ void b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const
do {
const int i=stack.size()-1;
const sStkCLN e=stack[i];
btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data);
b3DbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data);
stack.pop_back();
if(e.parent!=0)
e.parent->childs[i&1]=n;
@@ -616,7 +616,7 @@ void b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const
}
//
int b3DynamicBvh::maxdepth(const btDbvtNode* node)
int b3DynamicBvh::maxdepth(const b3DbvtNode* node)
{
int depth=0;
if(node) getmaxdepth(node,1,depth);
@@ -624,7 +624,7 @@ int b3DynamicBvh::maxdepth(const btDbvtNode* node)
}
//
int b3DynamicBvh::countLeaves(const btDbvtNode* node)
int b3DynamicBvh::countLeaves(const b3DbvtNode* node)
{
if(node->isinternal())
return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
@@ -633,7 +633,7 @@ int b3DynamicBvh::countLeaves(const btDbvtNode* node)
}
//
void b3DynamicBvh::extractLeaves(const btDbvtNode* node,b3AlignedObjectArray<const btDbvtNode*>& leaves)
void b3DynamicBvh::extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<const b3DbvtNode*>& leaves)
{
if(node->isinternal())
{
@@ -651,7 +651,7 @@ void b3DynamicBvh::extractLeaves(const btDbvtNode* node,b3AlignedObjectArray<c
#include <stdio.h>
#include <stdlib.h>
#include "LinearMath/btQuickProf.h"
#include "LinearMath/b3QuickProf.h"
/*
q6600,2.4ghz
@@ -667,10 +667,10 @@ World scale: 100.000000
Extents base: 1.000000
Extents range: 4.000000
Leaves: 8192
sizeof(btDbvtVolume): 32 bytes
sizeof(btDbvtNode): 44 bytes
[1] btDbvtVolume intersections: 3499 ms (-1%)
[2] btDbvtVolume merges: 1934 ms (0%)
sizeof(b3DbvtVolume): 32 bytes
sizeof(b3DbvtNode): 44 bytes
[1] b3DbvtVolume intersections: 3499 ms (-1%)
[2] b3DbvtVolume merges: 1934 ms (0%)
[3] b3DynamicBvh::collideTT: 5485 ms (-21%)
[4] b3DynamicBvh::collideTT self: 2814 ms (-20%)
[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%)
@@ -680,22 +680,22 @@ sizeof(btDbvtNode): 44 bytes
[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
[10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
[12] btDbvtVolume notequal: 3659 ms (0%)
[12] b3DbvtVolume notequal: 3659 ms (0%)
[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
[17] btDbvtVolume select: 3419 ms (0%)
[17] b3DbvtVolume select: 3419 ms (0%)
*/
struct btDbvtBenchmark
struct b3DbvtBenchmark
{
struct NilPolicy : b3DynamicBvh::ICollide
{
NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {}
void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; }
void Process(const btDbvtNode*) { ++m_pcount; }
void Process(const btDbvtNode*,b3Scalar depth)
void Process(const b3DbvtNode*,const b3DbvtNode*) { ++m_pcount; }
void Process(const b3DbvtNode*) { ++m_pcount; }
void Process(const b3DbvtNode*,b3Scalar depth)
{
++m_pcount;
if(m_checksort)
@@ -709,10 +709,10 @@ struct btDbvtBenchmark
{
struct Node
{
const btDbvtNode* leaf;
const b3DbvtNode* leaf;
b3Scalar depth;
};
void Process(const btDbvtNode* leaf,b3Scalar depth)
void Process(const b3DbvtNode* leaf,b3Scalar depth)
{
Node n;
n.leaf = leaf;
@@ -730,10 +730,10 @@ struct btDbvtBenchmark
{
struct Node
{
const btDbvtNode* leaf;
const b3DbvtNode* leaf;
b3Scalar depth;
};
void Process(const btDbvtNode* leaf)
void Process(const b3DbvtNode* leaf)
{
Node n;
n.leaf = leaf;
@@ -760,15 +760,15 @@ struct btDbvtBenchmark
{
return(RandVector3()*cs-b3Vector3(cs,cs,cs)/2);
}
static btDbvtVolume RandVolume(b3Scalar cs,b3Scalar eb,b3Scalar es)
static b3DbvtVolume RandVolume(b3Scalar cs,b3Scalar eb,b3Scalar es)
{
return(btDbvtVolume::FromCE(RandVector3(cs),b3Vector3(eb,eb,eb)+RandVector3()*es));
return(b3DbvtVolume::FromCE(RandVector3(cs),b3Vector3(eb,eb,eb)+RandVector3()*es));
}
static b3Transform RandTransform(b3Scalar cs)
{
b3Transform t;
t.setOrigin(RandVector3(cs));
t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
t.setRotation(b3Quaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
return(t);
}
static void RandTree(b3Scalar cs,b3Scalar eb,b3Scalar es,int leaves,b3DynamicBvh& dbvt)
@@ -789,11 +789,11 @@ void b3DynamicBvh::benchmark()
static const int cfgLeaves = 8192;
static const bool cfgEnable = true;
//[1] btDbvtVolume intersections
//[1] b3DbvtVolume intersections
bool cfgBenchmark1_Enable = cfgEnable;
static const int cfgBenchmark1_Iterations = 8;
static const int cfgBenchmark1_Reference = 3499;
//[2] btDbvtVolume merges
//[2] b3DbvtVolume merges
bool cfgBenchmark2_Enable = cfgEnable;
static const int cfgBenchmark2_Iterations = 4;
static const int cfgBenchmark2_Reference = 1945;
@@ -841,7 +841,7 @@ void b3DynamicBvh::benchmark()
static const int cfgBenchmark11_Passes = 64;
static const int cfgBenchmark11_Iterations = 65536;
static const int cfgBenchmark11_Reference = 2510;
//[12] btDbvtVolume notequal
//[12] b3DbvtVolume notequal
bool cfgBenchmark12_Enable = cfgEnable;
static const int cfgBenchmark12_Iterations = 32;
static const int cfgBenchmark12_Reference = 3677;
@@ -867,26 +867,26 @@ void b3DynamicBvh::benchmark()
static const int cfgBenchmark17_Iterations = 4;
static const int cfgBenchmark17_Reference = 3390;
btClock wallclock;
b3Clock wallclock;
printf("Benchmarking dbvt...\r\n");
printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
printf("\tLeaves: %u\r\n",cfgLeaves);
printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
printf("\tsizeof(btDbvtNode): %u bytes\r\n",sizeof(btDbvtNode));
printf("\tsizeof(b3DbvtVolume): %u bytes\r\n",sizeof(b3DbvtVolume));
printf("\tsizeof(b3DbvtNode): %u bytes\r\n",sizeof(b3DbvtNode));
if(cfgBenchmark1_Enable)
{// Benchmark 1
srand(380843);
b3AlignedObjectArray<btDbvtVolume> volumes;
b3AlignedObjectArray<b3DbvtVolume> volumes;
b3AlignedObjectArray<bool> results;
volumes.resize(cfgLeaves);
results.resize(cfgLeaves);
for(int i=0;i<cfgLeaves;++i)
{
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
printf("[1] btDbvtVolume intersections: ");
printf("[1] b3DbvtVolume intersections: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark1_Iterations;++i)
{
@@ -904,15 +904,15 @@ void b3DynamicBvh::benchmark()
if(cfgBenchmark2_Enable)
{// Benchmark 2
srand(380843);
b3AlignedObjectArray<btDbvtVolume> volumes;
b3AlignedObjectArray<btDbvtVolume> results;
b3AlignedObjectArray<b3DbvtVolume> volumes;
b3AlignedObjectArray<b3DbvtVolume> results;
volumes.resize(cfgLeaves);
results.resize(cfgLeaves);
for(int i=0;i<cfgLeaves;++i)
{
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
printf("[2] btDbvtVolume merges: ");
printf("[2] b3DbvtVolume merges: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark2_Iterations;++i)
{
@@ -931,9 +931,9 @@ void b3DynamicBvh::benchmark()
{// Benchmark 3
srand(380843);
b3DynamicBvh dbvt[2];
btDbvtBenchmark::NilPolicy policy;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
b3DbvtBenchmark::NilPolicy policy;
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
dbvt[0].optimizeTopDown();
dbvt[1].optimizeTopDown();
printf("[3] b3DynamicBvh::collideTT: ");
@@ -949,8 +949,8 @@ void b3DynamicBvh::benchmark()
{// Benchmark 4
srand(380843);
b3DynamicBvh dbvt;
btDbvtBenchmark::NilPolicy policy;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::NilPolicy policy;
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[4] b3DynamicBvh::collideTT self: ");
wallclock.reset();
@@ -966,14 +966,14 @@ void b3DynamicBvh::benchmark()
srand(380843);
b3DynamicBvh dbvt[2];
b3AlignedObjectArray<b3Transform> transforms;
btDbvtBenchmark::NilPolicy policy;
b3DbvtBenchmark::NilPolicy policy;
transforms.resize(cfgBenchmark5_Iterations);
for(int i=0;i<transforms.size();++i)
{
transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
transforms[i]=b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
dbvt[0].optimizeTopDown();
dbvt[1].optimizeTopDown();
printf("[5] b3DynamicBvh::collideTT xform: ");
@@ -990,13 +990,13 @@ void b3DynamicBvh::benchmark()
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<b3Transform> transforms;
btDbvtBenchmark::NilPolicy policy;
b3DbvtBenchmark::NilPolicy policy;
transforms.resize(cfgBenchmark6_Iterations);
for(int i=0;i<transforms.size();++i)
{
transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
transforms[i]=b3DbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[6] b3DynamicBvh::collideTT xform,self: ");
wallclock.reset();
@@ -1013,15 +1013,15 @@ void b3DynamicBvh::benchmark()
b3DynamicBvh dbvt;
b3AlignedObjectArray<b3Vector3> rayorg;
b3AlignedObjectArray<b3Vector3> raydir;
btDbvtBenchmark::NilPolicy policy;
b3DbvtBenchmark::NilPolicy policy;
rayorg.resize(cfgBenchmark7_Iterations);
raydir.resize(cfgBenchmark7_Iterations);
for(int i=0;i<rayorg.size();++i)
{
rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
rayorg[i]=b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
raydir[i]=b3DbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[7] b3DynamicBvh::rayTest: ");
wallclock.reset();
@@ -1040,7 +1040,7 @@ void b3DynamicBvh::benchmark()
{// Benchmark 8
srand(380843);
b3DynamicBvh dbvt;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[8] insert/remove: ");
wallclock.reset();
@@ -1048,7 +1048,7 @@ void b3DynamicBvh::benchmark()
{
for(int j=0;j<cfgBenchmark8_Iterations;++j)
{
dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
dbvt.remove(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
}
}
const int time=(int)wallclock.getTimeMilliseconds();
@@ -1059,8 +1059,8 @@ void b3DynamicBvh::benchmark()
{// Benchmark 9
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<const btDbvtNode*> leaves;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3AlignedObjectArray<const b3DbvtNode*> leaves;
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
dbvt.extractLeaves(dbvt.m_root,leaves);
printf("[9] updates (teleport): ");
@@ -1069,8 +1069,8 @@ void b3DynamicBvh::benchmark()
{
for(int j=0;j<cfgBenchmark9_Iterations;++j)
{
dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
dbvt.update(const_cast<b3DbvtNode*>(leaves[rand()%cfgLeaves]),
b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
}
}
const int time=(int)wallclock.getTimeMilliseconds();
@@ -1081,14 +1081,14 @@ void b3DynamicBvh::benchmark()
{// Benchmark 10
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<const btDbvtNode*> leaves;
b3AlignedObjectArray<const b3DbvtNode*> leaves;
b3AlignedObjectArray<b3Vector3> vectors;
vectors.resize(cfgBenchmark10_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1))*cfgBenchmark10_Scale;
vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1))*cfgBenchmark10_Scale;
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
dbvt.extractLeaves(dbvt.m_root,leaves);
printf("[10] updates (jitter): ");
@@ -1099,8 +1099,8 @@ void b3DynamicBvh::benchmark()
for(int j=0;j<cfgBenchmark10_Iterations;++j)
{
const b3Vector3& d=vectors[j];
btDbvtNode* l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
b3DbvtNode* l=const_cast<b3DbvtNode*>(leaves[rand()%cfgLeaves]);
b3DbvtVolume v=b3DbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
dbvt.update(l,v);
}
}
@@ -1112,7 +1112,7 @@ void b3DynamicBvh::benchmark()
{// Benchmark 11
srand(380843);
b3DynamicBvh dbvt;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[11] optimize (incremental): ");
wallclock.reset();
@@ -1127,15 +1127,15 @@ void b3DynamicBvh::benchmark()
if(cfgBenchmark12_Enable)
{// Benchmark 12
srand(380843);
b3AlignedObjectArray<btDbvtVolume> volumes;
b3AlignedObjectArray<b3DbvtVolume> volumes;
b3AlignedObjectArray<bool> results;
volumes.resize(cfgLeaves);
results.resize(cfgLeaves);
for(int i=0;i<cfgLeaves;++i)
{
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
printf("[12] btDbvtVolume notequal: ");
printf("[12] b3DbvtVolume notequal: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark12_Iterations;++i)
{
@@ -1155,13 +1155,13 @@ void b3DynamicBvh::benchmark()
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<b3Vector3> vectors;
btDbvtBenchmark::NilPolicy policy;
b3DbvtBenchmark::NilPolicy policy;
vectors.resize(cfgBenchmark13_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
printf("[13] culling(OCL+fullsort): ");
wallclock.reset();
@@ -1180,13 +1180,13 @@ void b3DynamicBvh::benchmark()
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<b3Vector3> vectors;
btDbvtBenchmark::P14 policy;
b3DbvtBenchmark::P14 policy;
vectors.resize(cfgBenchmark14_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
policy.m_nodes.reserve(cfgLeaves);
printf("[14] culling(OCL+qsort): ");
@@ -1196,7 +1196,7 @@ void b3DynamicBvh::benchmark()
static const b3Scalar offset=0;
policy.m_nodes.resize(0);
dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
policy.m_nodes.quickSort(b3DbvtBenchmark::P14::sortfnc);
}
const int time=(int)wallclock.getTimeMilliseconds();
const int t=cfgBenchmark14_Iterations;
@@ -1207,13 +1207,13 @@ void b3DynamicBvh::benchmark()
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<b3Vector3> vectors;
btDbvtBenchmark::P15 policy;
b3DbvtBenchmark::P15 policy;
vectors.resize(cfgBenchmark15_Iterations);
for(int i=0;i<vectors.size();++i)
{
vectors[i]=(btDbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
vectors[i]=(b3DbvtBenchmark::RandVector3()*2-b3Vector3(1,1,1)).normalized();
}
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
policy.m_nodes.reserve(cfgLeaves);
printf("[15] culling(KDOP+qsort): ");
@@ -1224,7 +1224,7 @@ void b3DynamicBvh::benchmark()
policy.m_nodes.resize(0);
policy.m_axis=vectors[i];
dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
policy.m_nodes.quickSort(b3DbvtBenchmark::P15::sortfnc);
}
const int time=(int)wallclock.getTimeMilliseconds();
const int t=cfgBenchmark15_Iterations;
@@ -1234,8 +1234,8 @@ void b3DynamicBvh::benchmark()
{// Benchmark 16
srand(380843);
b3DynamicBvh dbvt;
b3AlignedObjectArray<btDbvtNode*> batch;
btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
b3AlignedObjectArray<b3DbvtNode*> batch;
b3DbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
dbvt.optimizeTopDown();
batch.reserve(cfgBenchmark16_BatchCount);
printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
@@ -1244,7 +1244,7 @@ void b3DynamicBvh::benchmark()
{
for(int j=0;j<cfgBenchmark16_BatchCount;++j)
{
batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
batch.push_back(dbvt.insert(b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
}
for(int j=0;j<cfgBenchmark16_BatchCount;++j)
{
@@ -1259,7 +1259,7 @@ void b3DynamicBvh::benchmark()
if(cfgBenchmark17_Enable)
{// Benchmark 17
srand(380843);
b3AlignedObjectArray<btDbvtVolume> volumes;
b3AlignedObjectArray<b3DbvtVolume> volumes;
b3AlignedObjectArray<int> results;
b3AlignedObjectArray<int> indices;
volumes.resize(cfgLeaves);
@@ -1268,13 +1268,13 @@ void b3DynamicBvh::benchmark()
for(int i=0;i<cfgLeaves;++i)
{
indices[i]=i;
volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
volumes[i]=b3DbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
}
for(int i=0;i<cfgLeaves;++i)
{
btSwap(indices[i],indices[rand()%cfgLeaves]);
b3Swap(indices[i],indices[rand()%cfgLeaves]);
}
printf("[17] btDbvtVolume select: ");
printf("[17] b3DbvtVolume select: ");
wallclock.reset();
for(int i=0;i<cfgBenchmark17_Iterations;++i)
{

304
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
*/
///b3DynamicBvh implementation by Nathanael Presson
#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
#ifndef B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
#define B3_DYNAMIC_BOUNDING_VOLUME_TREE_H
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Common/b3Vector3.h"
@@ -57,7 +57,7 @@ subject to the following restrictions:
// Specific methods implementation
//SSE gives errors on a MSVC 7.1
#if defined (BT_USE_SSE) //&& defined (_WIN32)
#if defined (B3_USE_SSE) //&& defined (_WIN32)
#define DBVT_SELECT_IMPL DBVT_IMPL_SSE
#define DBVT_MERGE_IMPL DBVT_IMPL_SSE
#define DBVT_INT0_IMPL DBVT_IMPL_SSE
@@ -126,40 +126,40 @@ subject to the following restrictions:
// Defaults volumes
//
/* btDbvtAabbMm */
struct btDbvtAabbMm
/* b3DbvtAabbMm */
struct b3DbvtAabbMm
{
DBVT_INLINE b3Vector3 Center() const { return((mi+mx)/2); }
DBVT_INLINE b3Vector3 Lengths() const { return(mx-mi); }
DBVT_INLINE b3Vector3 Extents() const { return((mx-mi)/2); }
DBVT_INLINE const b3Vector3& Mins() const { return(mi); }
DBVT_INLINE const b3Vector3& Maxs() const { return(mx); }
static inline btDbvtAabbMm FromCE(const b3Vector3& c,const b3Vector3& e);
static inline btDbvtAabbMm FromCR(const b3Vector3& c,b3Scalar r);
static inline btDbvtAabbMm FromMM(const b3Vector3& mi,const b3Vector3& mx);
static inline btDbvtAabbMm FromPoints(const b3Vector3* pts,int n);
static inline btDbvtAabbMm FromPoints(const b3Vector3** ppts,int n);
static inline b3DbvtAabbMm FromCE(const b3Vector3& c,const b3Vector3& e);
static inline b3DbvtAabbMm FromCR(const b3Vector3& c,b3Scalar r);
static inline b3DbvtAabbMm FromMM(const b3Vector3& mi,const b3Vector3& mx);
static inline b3DbvtAabbMm FromPoints(const b3Vector3* pts,int n);
static inline b3DbvtAabbMm FromPoints(const b3Vector3** ppts,int n);
DBVT_INLINE void Expand(const b3Vector3& e);
DBVT_INLINE void SignedExpand(const b3Vector3& e);
DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const;
DBVT_INLINE bool Contain(const b3DbvtAabbMm& a) const;
DBVT_INLINE int Classify(const b3Vector3& n,b3Scalar o,int s) const;
DBVT_INLINE b3Scalar ProjectMinimum(const b3Vector3& v,unsigned signs) const;
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
DBVT_INLINE friend bool Intersect( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b);
DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
DBVT_INLINE friend bool Intersect( const b3DbvtAabbMm& a,
const b3Vector3& b);
DBVT_INLINE friend b3Scalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
DBVT_INLINE friend int Select( const btDbvtAabbMm& o,
const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
DBVT_INLINE friend void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b,
btDbvtAabbMm& r);
DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b);
DBVT_INLINE friend b3Scalar Proximity( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b);
DBVT_INLINE friend int Select( const b3DbvtAabbMm& o,
const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b);
DBVT_INLINE friend void Merge( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b,
b3DbvtAabbMm& r);
DBVT_INLINE friend bool NotEqual( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b);
DBVT_INLINE b3Vector3& tMins() { return(mi); }
DBVT_INLINE b3Vector3& tMaxs() { return(mx); }
@@ -171,18 +171,18 @@ private:
};
// Types
typedef btDbvtAabbMm btDbvtVolume;
typedef b3DbvtAabbMm b3DbvtVolume;
/* btDbvtNode */
struct btDbvtNode
/* b3DbvtNode */
struct b3DbvtNode
{
btDbvtVolume volume;
btDbvtNode* parent;
b3DbvtVolume volume;
b3DbvtNode* parent;
DBVT_INLINE bool isleaf() const { return(childs[1]==0); }
DBVT_INLINE bool isinternal() const { return(!isleaf()); }
union
{
btDbvtNode* childs[2];
b3DbvtNode* childs[2];
void* data;
int dataAsInt;
};
@@ -190,36 +190,36 @@ struct btDbvtNode
///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes.
///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
///Unlike the b3QuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
struct b3DynamicBvh
{
/* Stack element */
struct sStkNN
{
const btDbvtNode* a;
const btDbvtNode* b;
const b3DbvtNode* a;
const b3DbvtNode* b;
sStkNN() {}
sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
sStkNN(const b3DbvtNode* na,const b3DbvtNode* nb) : a(na),b(nb) {}
};
struct sStkNP
{
const btDbvtNode* node;
const b3DbvtNode* node;
int mask;
sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
sStkNP(const b3DbvtNode* n,unsigned m) : node(n),mask(m) {}
};
struct sStkNPS
{
const btDbvtNode* node;
const b3DbvtNode* node;
int mask;
b3Scalar value;
sStkNPS() {}
sStkNPS(const btDbvtNode* n,unsigned m,b3Scalar v) : node(n),mask(m),value(v) {}
sStkNPS(const b3DbvtNode* n,unsigned m,b3Scalar v) : node(n),mask(m),value(v) {}
};
struct sStkCLN
{
const btDbvtNode* node;
btDbvtNode* parent;
sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
const b3DbvtNode* node;
b3DbvtNode* parent;
sStkCLN(const b3DbvtNode* n,b3DbvtNode* p) : node(n),parent(p) {}
};
// Policies/Interfaces
@@ -227,25 +227,25 @@ struct b3DynamicBvh
struct ICollide
{
DBVT_VIRTUAL_DTOR(ICollide)
DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {}
DBVT_VIRTUAL void Process(const btDbvtNode*) {}
DBVT_VIRTUAL void Process(const btDbvtNode* n,b3Scalar) { Process(n); }
DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); }
DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); }
DBVT_VIRTUAL void Process(const b3DbvtNode*,const b3DbvtNode*) {}
DBVT_VIRTUAL void Process(const b3DbvtNode*) {}
DBVT_VIRTUAL void Process(const b3DbvtNode* n,b3Scalar) { Process(n); }
DBVT_VIRTUAL bool Descent(const b3DbvtNode*) { return(true); }
DBVT_VIRTUAL bool AllLeaves(const b3DbvtNode*) { return(true); }
};
/* IWriter */
struct IWriter
{
virtual ~IWriter() {}
virtual void Prepare(const btDbvtNode* root,int numnodes)=0;
virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0;
virtual void Prepare(const b3DbvtNode* root,int numnodes)=0;
virtual void WriteNode(const b3DbvtNode*,int index,int parent,int child0,int child1)=0;
virtual void WriteLeaf(const b3DbvtNode*,int index,int parent)=0;
};
/* IClone */
struct IClone
{
virtual ~IClone() {}
virtual void CloneLeaf(btDbvtNode*) {}
virtual void CloneLeaf(b3DbvtNode*) {}
};
// Constants
@@ -255,15 +255,15 @@ struct b3DynamicBvh
};
// Fields
btDbvtNode* m_root;
btDbvtNode* m_free;
b3DbvtNode* m_root;
b3DbvtNode* m_free;
int m_lkhd;
int m_leaves;
unsigned m_opath;
b3AlignedObjectArray<sStkNN> m_stkStack;
mutable b3AlignedObjectArray<const btDbvtNode*> m_rayTestStack;
mutable b3AlignedObjectArray<const b3DbvtNode*> m_rayTestStack;
// Methods
@@ -274,18 +274,18 @@ struct b3DynamicBvh
void optimizeBottomUp();
void optimizeTopDown(int bu_treshold=128);
void optimizeIncremental(int passes);
btDbvtNode* insert(const btDbvtVolume& box,void* data);
void update(btDbvtNode* leaf,int lookahead=-1);
void update(btDbvtNode* leaf,btDbvtVolume& volume);
bool update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin);
bool update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity);
bool update(btDbvtNode* leaf,btDbvtVolume& volume,b3Scalar margin);
void remove(btDbvtNode* leaf);
b3DbvtNode* insert(const b3DbvtVolume& box,void* data);
void update(b3DbvtNode* leaf,int lookahead=-1);
void update(b3DbvtNode* leaf,b3DbvtVolume& volume);
bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin);
bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,const b3Vector3& velocity);
bool update(b3DbvtNode* leaf,b3DbvtVolume& volume,b3Scalar margin);
void remove(b3DbvtNode* leaf);
void write(IWriter* iwriter) const;
void clone(b3DynamicBvh& dest,IClone* iclone=0) const;
static int maxdepth(const btDbvtNode* node);
static int countLeaves(const btDbvtNode* node);
static void extractLeaves(const btDbvtNode* node,b3AlignedObjectArray<const btDbvtNode*>& leaves);
static int maxdepth(const b3DbvtNode* node);
static int countLeaves(const b3DbvtNode* node);
static void extractLeaves(const b3DbvtNode* node,b3AlignedObjectArray<const b3DbvtNode*>& leaves);
#if DBVT_ENABLE_BENCHMARK
static void benchmark();
#else
@@ -293,49 +293,49 @@ struct b3DynamicBvh
#endif
// DBVT_IPOLICY must support ICollide policy/interface
DBVT_PREFIX
static void enumNodes( const btDbvtNode* root,
static void enumNodes( const b3DbvtNode* root,
DBVT_IPOLICY);
DBVT_PREFIX
static void enumLeaves( const btDbvtNode* root,
static void enumLeaves( const b3DbvtNode* root,
DBVT_IPOLICY);
DBVT_PREFIX
void collideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
void collideTT( const b3DbvtNode* root0,
const b3DbvtNode* root1,
DBVT_IPOLICY);
DBVT_PREFIX
void collideTTpersistentStack( const btDbvtNode* root0,
const btDbvtNode* root1,
void collideTTpersistentStack( const b3DbvtNode* root0,
const b3DbvtNode* root1,
DBVT_IPOLICY);
#if 0
DBVT_PREFIX
void collideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
void collideTT( const b3DbvtNode* root0,
const b3DbvtNode* root1,
const b3Transform& xform,
DBVT_IPOLICY);
DBVT_PREFIX
void collideTT( const btDbvtNode* root0,
void collideTT( const b3DbvtNode* root0,
const b3Transform& xform0,
const btDbvtNode* root1,
const b3DbvtNode* root1,
const b3Transform& xform1,
DBVT_IPOLICY);
#endif
DBVT_PREFIX
void collideTV( const btDbvtNode* root,
const btDbvtVolume& volume,
void collideTV( const b3DbvtNode* root,
const b3DbvtVolume& volume,
DBVT_IPOLICY) const;
///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
///rayTest is a re-entrant ray test, and can be called in parallel as long as the b3AlignedAlloc is thread-safe (uses locking etc)
///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
DBVT_PREFIX
static void rayTest( const btDbvtNode* root,
static void rayTest( const b3DbvtNode* root,
const b3Vector3& rayFrom,
const b3Vector3& rayTo,
DBVT_IPOLICY);
///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
///rayTestInternal is used by b3DynamicBvhBroadphase to accelerate world ray casts
DBVT_PREFIX
void rayTestInternal( const btDbvtNode* root,
void rayTestInternal( const b3DbvtNode* root,
const b3Vector3& rayFrom,
const b3Vector3& rayTo,
const b3Vector3& rayDirectionInverse,
@@ -346,13 +346,13 @@ struct b3DynamicBvh
DBVT_IPOLICY) const;
DBVT_PREFIX
static void collideKDOP(const btDbvtNode* root,
static void collideKDOP(const b3DbvtNode* root,
const b3Vector3* normals,
const b3Scalar* offsets,
int count,
DBVT_IPOLICY);
DBVT_PREFIX
static void collideOCL( const btDbvtNode* root,
static void collideOCL( const b3DbvtNode* root,
const b3Vector3* normals,
const b3Scalar* offsets,
const b3Vector3& sortaxis,
@@ -360,7 +360,7 @@ struct b3DynamicBvh
DBVT_IPOLICY,
bool fullsort=true);
DBVT_PREFIX
static void collideTU( const btDbvtNode* root,
static void collideTU( const b3DbvtNode* root,
DBVT_IPOLICY);
// Helpers
static DBVT_INLINE int nearest(const int* i,const b3DynamicBvh::sStkNPS* a,b3Scalar v,int l,int h)
@@ -394,31 +394,31 @@ private:
//
//
inline btDbvtAabbMm btDbvtAabbMm::FromCE(const b3Vector3& c,const b3Vector3& e)
inline b3DbvtAabbMm b3DbvtAabbMm::FromCE(const b3Vector3& c,const b3Vector3& e)
{
btDbvtAabbMm box;
b3DbvtAabbMm box;
box.mi=c-e;box.mx=c+e;
return(box);
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r)
inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r)
{
return(FromCE(c,b3Vector3(r,r,r)));
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromMM(const b3Vector3& mi,const b3Vector3& mx)
inline b3DbvtAabbMm b3DbvtAabbMm::FromMM(const b3Vector3& mi,const b3Vector3& mx)
{
btDbvtAabbMm box;
b3DbvtAabbMm box;
box.mi=mi;box.mx=mx;
return(box);
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const b3Vector3* pts,int n)
inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3* pts,int n)
{
btDbvtAabbMm box;
b3DbvtAabbMm box;
box.mi=box.mx=pts[0];
for(int i=1;i<n;++i)
{
@@ -429,9 +429,9 @@ inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const b3Vector3* pts,int n)
}
//
inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const b3Vector3** ppts,int n)
inline b3DbvtAabbMm b3DbvtAabbMm::FromPoints(const b3Vector3** ppts,int n)
{
btDbvtAabbMm box;
b3DbvtAabbMm box;
box.mi=box.mx=*ppts[0];
for(int i=1;i<n;++i)
{
@@ -442,13 +442,13 @@ inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const b3Vector3** ppts,int n)
}
//
DBVT_INLINE void btDbvtAabbMm::Expand(const b3Vector3& e)
DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e)
{
mi-=e;mx+=e;
}
//
DBVT_INLINE void btDbvtAabbMm::SignedExpand(const b3Vector3& e)
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]);
@@ -456,7 +456,7 @@ DBVT_INLINE void btDbvtAabbMm::SignedExpand(const b3Vector3& e)
}
//
DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
DBVT_INLINE bool b3DbvtAabbMm::Contain(const b3DbvtAabbMm& a) const
{
return( (mi.x<=a.mi.x)&&
(mi.y<=a.mi.y)&&
@@ -467,7 +467,7 @@ DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
}
//
DBVT_INLINE int btDbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) const
DBVT_INLINE int b3DbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) const
{
b3Vector3 pi,px;
switch(s)
@@ -489,23 +489,23 @@ DBVT_INLINE int btDbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) con
case (1+2+4): px=b3Vector3(mx.x,mx.y,mx.z);
pi=b3Vector3(mi.x,mi.y,mi.z);break;
}
if((btDot(n,px)+o)<0) return(-1);
if((btDot(n,pi)+o)>=0) return(+1);
if((b3Dot(n,px)+o)<0) return(-1);
if((b3Dot(n,pi)+o)>=0) return(+1);
return(0);
}
//
DBVT_INLINE b3Scalar btDbvtAabbMm::ProjectMinimum(const b3Vector3& v,unsigned signs) const
DBVT_INLINE b3Scalar b3DbvtAabbMm::ProjectMinimum(const b3Vector3& v,unsigned signs) const
{
const b3Vector3* b[]={&mx,&mi};
const b3Vector3 p( b[(signs>>0)&1]->x,
b[(signs>>1)&1]->y,
b[(signs>>2)&1]->z);
return(btDot(p,v));
return(b3Dot(p,v));
}
//
DBVT_INLINE void btDbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const
DBVT_INLINE void b3DbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const
{
for(int i=0;i<3;++i)
{
@@ -517,8 +517,8 @@ DBVT_INLINE void btDbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scala
}
//
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
DBVT_INLINE bool Intersect( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b)
{
#if DBVT_INT0_IMPL == DBVT_IMPL_SSE
const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
@@ -542,7 +542,7 @@ DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
//
DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
DBVT_INLINE bool Intersect( const b3DbvtAabbMm& a,
const b3Vector3& b)
{
return( (b.x>=a.mi.x)&&
@@ -561,19 +561,19 @@ DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
//
DBVT_INLINE b3Scalar Proximity( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
DBVT_INLINE b3Scalar Proximity( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b)
{
const b3Vector3 d=(a.mi+a.mx)-(b.mi+b.mx);
return(btFabs(d.x)+btFabs(d.y)+btFabs(d.z));
return(b3Fabs(d.x)+b3Fabs(d.y)+b3Fabs(d.z));
}
//
DBVT_INLINE int Select( const btDbvtAabbMm& o,
const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
DBVT_INLINE int Select( const b3DbvtAabbMm& o,
const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b)
{
#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
@@ -585,7 +585,7 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o,
///@todo: the intrinsic version is 11% slower
#if DBVT_USE_INTRINSIC_SSE
union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
union b3SSEUnion ///NOTE: if we use more intrinsics, move b3SSEUnion into the LinearMath directory
{
__m128 ssereg;
float floats[4];
@@ -609,7 +609,7 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o,
bmi=_mm_add_ps(bmi,t1);
bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
btSSEUnion tmp;
b3SSEUnion tmp;
tmp.ssereg = _mm_cmple_ss(bmi,ami);
return tmp.ints[0]&1;
@@ -650,9 +650,9 @@ DBVT_INLINE int Select( const btDbvtAabbMm& o,
}
//
DBVT_INLINE void Merge( const btDbvtAabbMm& a,
const btDbvtAabbMm& b,
btDbvtAabbMm& r)
DBVT_INLINE void Merge( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b,
b3DbvtAabbMm& r)
{
#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
__m128 ami(_mm_load_ps(a.mi));
@@ -673,8 +673,8 @@ DBVT_INLINE void Merge( const btDbvtAabbMm& a,
}
//
DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
const btDbvtAabbMm& b)
DBVT_INLINE bool NotEqual( const b3DbvtAabbMm& a,
const b3DbvtAabbMm& b)
{
return( (a.mi.x!=b.mi.x)||
(a.mi.y!=b.mi.y)||
@@ -690,7 +690,7 @@ DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
//
DBVT_PREFIX
inline void b3DynamicBvh::enumNodes( const btDbvtNode* root,
inline void b3DynamicBvh::enumNodes( const b3DbvtNode* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
@@ -704,7 +704,7 @@ inline void b3DynamicBvh::enumNodes( const btDbvtNode* root,
//
DBVT_PREFIX
inline void b3DynamicBvh::enumLeaves( const btDbvtNode* root,
inline void b3DynamicBvh::enumLeaves( const b3DbvtNode* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
@@ -721,8 +721,8 @@ inline void b3DynamicBvh::enumLeaves( const btDbvtNode* root,
//
DBVT_PREFIX
inline void b3DynamicBvh::collideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0,
const b3DbvtNode* root1,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
@@ -786,8 +786,8 @@ inline void b3DynamicBvh::collideTT( const btDbvtNode* root0,
DBVT_PREFIX
inline void b3DynamicBvh::collideTTpersistentStack( const btDbvtNode* root0,
const btDbvtNode* root1,
inline void b3DynamicBvh::collideTTpersistentStack( const b3DbvtNode* root0,
const b3DbvtNode* root1,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
@@ -851,8 +851,8 @@ inline void b3DynamicBvh::collideTTpersistentStack( const btDbvtNode* root0,
#if 0
//
DBVT_PREFIX
inline void b3DynamicBvh::collideTT( const btDbvtNode* root0,
const btDbvtNode* root1,
inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0,
const b3DbvtNode* root1,
const b3Transform& xform,
DBVT_IPOLICY)
{
@@ -906,9 +906,9 @@ inline void b3DynamicBvh::collideTT( const btDbvtNode* root0,
}
//
DBVT_PREFIX
inline void b3DynamicBvh::collideTT( const btDbvtNode* root0,
inline void b3DynamicBvh::collideTT( const b3DbvtNode* root0,
const b3Transform& xform0,
const btDbvtNode* root1,
const b3DbvtNode* root1,
const b3Transform& xform1,
DBVT_IPOLICY)
{
@@ -919,20 +919,20 @@ inline void b3DynamicBvh::collideTT( const btDbvtNode* root0,
//
DBVT_PREFIX
inline void b3DynamicBvh::collideTV( const btDbvtNode* root,
const btDbvtVolume& vol,
inline void b3DynamicBvh::collideTV( const b3DbvtNode* root,
const b3DbvtVolume& vol,
DBVT_IPOLICY) const
{
DBVT_CHECKTYPE
if(root)
{
ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
b3AlignedObjectArray<const btDbvtNode*> stack;
ATTRIBUTE_ALIGNED16(b3DbvtVolume) volume(vol);
b3AlignedObjectArray<const b3DbvtNode*> stack;
stack.resize(0);
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(root);
do {
const btDbvtNode* n=stack[stack.size()-1];
const b3DbvtNode* n=stack[stack.size()-1];
stack.pop_back();
if(Intersect(n->volume,volume))
{
@@ -951,7 +951,7 @@ inline void b3DynamicBvh::collideTV( const btDbvtNode* root,
}
DBVT_PREFIX
inline void b3DynamicBvh::rayTestInternal( const btDbvtNode* root,
inline void b3DynamicBvh::rayTestInternal( const b3DbvtNode* root,
const b3Vector3& rayFrom,
const b3Vector3& rayTo,
const b3Vector3& rayDirectionInverse,
@@ -969,18 +969,18 @@ inline void b3DynamicBvh::rayTestInternal( const btDbvtNode* root,
int depth=1;
int treshold=DOUBLE_STACKSIZE-2;
b3AlignedObjectArray<const btDbvtNode*>& stack = m_rayTestStack;
b3AlignedObjectArray<const b3DbvtNode*>& stack = m_rayTestStack;
stack.resize(DOUBLE_STACKSIZE);
stack[0]=root;
b3Vector3 bounds[2];
do
{
const btDbvtNode* node=stack[--depth];
const b3DbvtNode* node=stack[--depth];
bounds[0] = node->volume.Mins()-aabbMax;
bounds[1] = node->volume.Maxs()-aabbMin;
b3Scalar tmin=1.f,lambda_min=0.f;
unsigned int result1=false;
result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
if(result1)
{
if(node->isinternal())
@@ -1004,7 +1004,7 @@ inline void b3DynamicBvh::rayTestInternal( const btDbvtNode* root,
//
DBVT_PREFIX
inline void b3DynamicBvh::rayTest( const btDbvtNode* root,
inline void b3DynamicBvh::rayTest( const b3DbvtNode* root,
const b3Vector3& rayFrom,
const b3Vector3& rayTo,
DBVT_IPOLICY)
@@ -1015,18 +1015,18 @@ inline void b3DynamicBvh::rayTest( const btDbvtNode* root,
b3Vector3 rayDir = (rayTo-rayFrom);
rayDir.normalize ();
///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
///what about division by zero? --> just set rayDirection[i] to INF/B3_LARGE_FLOAT
b3Vector3 rayDirectionInverse;
rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(BT_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0];
rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(BT_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1];
rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(BT_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2];
rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0];
rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1];
rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(B3_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2];
unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
b3Scalar lambda_max = rayDir.dot(rayTo-rayFrom);
b3Vector3 resultNormal;
b3AlignedObjectArray<const btDbvtNode*> stack;
b3AlignedObjectArray<const b3DbvtNode*> stack;
int depth=1;
int treshold=DOUBLE_STACKSIZE-2;
@@ -1035,18 +1035,18 @@ inline void b3DynamicBvh::rayTest( const btDbvtNode* root,
stack[0]=root;
b3Vector3 bounds[2];
do {
const btDbvtNode* node=stack[--depth];
const b3DbvtNode* node=stack[--depth];
bounds[0] = node->volume.Mins();
bounds[1] = node->volume.Maxs();
b3Scalar tmin=1.f,lambda_min=0.f;
unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
unsigned int result1 = b3RayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
#ifdef COMPARE_BTRAY_AABB2
b3Scalar param=1.f;
bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
btAssert(result1 == result2);
bool result2 = b3RayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
b3Assert(result1 == result2);
#endif //TEST_BTRAY_AABB2
if(result1)
@@ -1073,7 +1073,7 @@ inline void b3DynamicBvh::rayTest( const btDbvtNode* root,
//
DBVT_PREFIX
inline void b3DynamicBvh::collideKDOP(const btDbvtNode* root,
inline void b3DynamicBvh::collideKDOP(const b3DbvtNode* root,
const b3Vector3* normals,
const b3Scalar* offsets,
int count,
@@ -1085,7 +1085,7 @@ inline void b3DynamicBvh::collideKDOP(const btDbvtNode* root,
const int inside=(1<<count)-1;
b3AlignedObjectArray<sStkNP> stack;
int signs[sizeof(unsigned)*8];
btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
b3Assert(count<int (sizeof(signs)/sizeof(signs[0])));
for(int i=0;i<count;++i)
{
signs[i]= ((normals[i].x>=0)?1:0)+
@@ -1128,7 +1128,7 @@ inline void b3DynamicBvh::collideKDOP(const btDbvtNode* root,
//
DBVT_PREFIX
inline void b3DynamicBvh::collideOCL( const btDbvtNode* root,
inline void b3DynamicBvh::collideOCL( const b3DbvtNode* root,
const b3Vector3* normals,
const b3Scalar* offsets,
const b3Vector3& sortaxis,
@@ -1147,7 +1147,7 @@ inline void b3DynamicBvh::collideOCL( const btDbvtNode* root,
b3AlignedObjectArray<int> ifree;
b3AlignedObjectArray<int> stack;
int signs[sizeof(unsigned)*8];
btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
b3Assert(count<int (sizeof(signs)/sizeof(signs[0])));
for(int i=0;i<count;++i)
{
signs[i]= ((normals[i].x>=0)?1:0)+
@@ -1183,7 +1183,7 @@ inline void b3DynamicBvh::collideOCL( const btDbvtNode* root,
{
if(se.node->isinternal())
{
const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
const b3DbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
const int q=nes[0].value<nes[1].value?1:0;
@@ -1226,17 +1226,17 @@ inline void b3DynamicBvh::collideOCL( const btDbvtNode* root,
//
DBVT_PREFIX
inline void b3DynamicBvh::collideTU( const btDbvtNode* root,
inline void b3DynamicBvh::collideTU( const b3DbvtNode* root,
DBVT_IPOLICY)
{
DBVT_CHECKTYPE
if(root)
{
b3AlignedObjectArray<const btDbvtNode*> stack;
b3AlignedObjectArray<const b3DbvtNode*> stack;
stack.reserve(SIMPLE_STACKSIZE);
stack.push_back(root);
do {
const btDbvtNode* n=stack[stack.size()-1];
const b3DbvtNode* n=stack[stack.size()-1];
stack.pop_back();
if(policy.Descent(n))
{

192
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
View File

@@ -29,7 +29,7 @@ subject to the following restrictions:
#if DBVT_BP_PROFILE
struct ProfileScope
{
__forceinline ProfileScope(btClock& clock,unsigned long& value) :
__forceinline ProfileScope(b3Clock& clock,unsigned long& value) :
m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds())
{
}
@@ -37,7 +37,7 @@ struct ProfileScope
{
(*m_value)+=m_clock->getTimeMicroseconds()-m_base;
}
btClock* m_clock;
b3Clock* m_clock;
unsigned long* m_value;
unsigned long m_base;
};
@@ -90,26 +90,26 @@ static inline void clear(T& value)
//
/* Tree collider */
struct btDbvtTreeCollider : b3DynamicBvh::ICollide
struct b3DbvtTreeCollider : b3DynamicBvh::ICollide
{
b3DynamicBvhBroadphase* pbp;
btDbvtProxy* proxy;
btDbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
void Process(const btDbvtNode* na,const btDbvtNode* nb)
b3DbvtProxy* proxy;
b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
void Process(const b3DbvtNode* na,const b3DbvtNode* nb)
{
if(na!=nb)
{
btDbvtProxy* pa=(btDbvtProxy*)na->data;
btDbvtProxy* pb=(btDbvtProxy*)nb->data;
b3DbvtProxy* pa=(b3DbvtProxy*)na->data;
b3DbvtProxy* pb=(b3DbvtProxy*)nb->data;
#if DBVT_BP_SORTPAIRS
if(pa->m_uniqueId>pb->m_uniqueId)
btSwap(pa,pb);
b3Swap(pa,pb);
#endif
pbp->m_paircache->addOverlappingPair(pa->getUid(),pb->getUid());
++pbp->m_newpairs;
}
}
void Process(const btDbvtNode* n)
void Process(const b3DbvtNode* n)
{
Process(n,proxy->leaf);
}
@@ -135,7 +135,7 @@ b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingP
m_updates_call = 0;
m_updates_done = 0;
m_updates_ratio = 0;
m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
m_paircache = paircache? paircache : new(b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache),16)) b3HashedOverlappingPairCache();
m_pid = 0;
m_cid = 0;
@@ -155,33 +155,33 @@ b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
if(m_releasepaircache)
{
m_paircache->~b3OverlappingPairCache();
btAlignedFree(m_paircache);
b3AlignedFree(m_paircache);
}
}
//
btBroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin,
b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
int objectId,
void* userPtr,
short int collisionFilterGroup,
short int collisionFilterMask)
{
btDbvtProxy* mem = &m_proxies[objectId];
btDbvtProxy* proxy=new(mem) btDbvtProxy( aabbMin,aabbMax,userPtr,
b3DbvtProxy* mem = &m_proxies[objectId];
b3DbvtProxy* proxy=new(mem) b3DbvtProxy( aabbMin,aabbMax,userPtr,
collisionFilterGroup,
collisionFilterMask);
btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax);
//bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax);
//bproxy->aabb = b3DbvtVolume::FromMM(aabbMin,aabbMax);
proxy->stage = m_stageCurrent;
proxy->m_uniqueId = objectId;
proxy->leaf = m_sets[0].insert(aabb,proxy);
listappend(proxy,m_stageRoots[m_stageCurrent]);
if(!m_deferedcollide)
{
btDbvtTreeCollider collider(this);
b3DbvtTreeCollider collider(this);
collider.proxy=proxy;
m_sets[0].collideTV(m_sets[0].m_root,aabb,collider);
m_sets[1].collideTV(m_sets[1].m_root,aabb,collider);
@@ -190,10 +190,10 @@ btBroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabb
}
//
void b3DynamicBvhBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
btDispatcher* dispatcher)
void b3DynamicBvhBroadphase::destroyProxy( b3BroadphaseProxy* absproxy,
b3Dispatcher* dispatcher)
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
if(proxy->stage==STAGECOUNT)
m_sets[1].remove(proxy->leaf);
else
@@ -204,28 +204,28 @@ void b3DynamicBvhBroadphase::destroyProxy( btBroadphaseProxy* absproxy,
m_needcleanup=true;
}
void b3DynamicBvhBroadphase::getAabb(btBroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
void b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
aabbMin = proxy->m_aabbMin;
aabbMax = proxy->m_aabbMax;
}
struct BroadphaseRayTester : b3DynamicBvh::ICollide
{
btBroadphaseRayCallback& m_rayCallback;
BroadphaseRayTester(btBroadphaseRayCallback& orgCallback)
b3BroadphaseRayCallback& m_rayCallback;
BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
:m_rayCallback(orgCallback)
{
}
void Process(const btDbvtNode* leaf)
void Process(const b3DbvtNode* leaf)
{
btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data;
m_rayCallback.process(proxy);
}
};
void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, btBroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax)
void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax)
{
BroadphaseRayTester callback(rayCallback);
@@ -254,23 +254,23 @@ void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& r
struct BroadphaseAabbTester : b3DynamicBvh::ICollide
{
btBroadphaseAabbCallback& m_aabbCallback;
BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback)
b3BroadphaseAabbCallback& m_aabbCallback;
BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
:m_aabbCallback(orgCallback)
{
}
void Process(const btDbvtNode* leaf)
void Process(const b3DbvtNode* leaf)
{
btDbvtProxy* proxy=(btDbvtProxy*)leaf->data;
b3DbvtProxy* proxy=(b3DbvtProxy*)leaf->data;
m_aabbCallback.process(proxy);
}
};
void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,btBroadphaseAabbCallback& aabbCallback)
void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3BroadphaseAabbCallback& aabbCallback)
{
BroadphaseAabbTester callback(aabbCallback);
const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax);
const ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds=b3DbvtVolume::FromMM(aabbMin,aabbMax);
//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);
@@ -280,13 +280,13 @@ void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3&
//
void b3DynamicBvhBroadphase::setAabb( btBroadphaseProxy* absproxy,
void b3DynamicBvhBroadphase::setAabb( b3BroadphaseProxy* absproxy,
const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
btDispatcher* /*dispatcher*/)
b3Dispatcher* /*dispatcher*/)
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
#if DBVT_BP_PREVENTFALSEUPDATE
if(NotEqual(aabb,proxy->leaf->volume))
#endif
@@ -338,7 +338,7 @@ void b3DynamicBvhBroadphase::setAabb( btBroadphaseProxy* absproxy,
m_needcleanup=true;
if(!m_deferedcollide)
{
btDbvtTreeCollider collider(this);
b3DbvtTreeCollider collider(this);
m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
}
@@ -348,13 +348,13 @@ void b3DynamicBvhBroadphase::setAabb( btBroadphaseProxy* absproxy,
//
void b3DynamicBvhBroadphase::setAabbForceUpdate( btBroadphaseProxy* absproxy,
void b3DynamicBvhBroadphase::setAabbForceUpdate( b3BroadphaseProxy* absproxy,
const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
btDispatcher* /*dispatcher*/)
b3Dispatcher* /*dispatcher*/)
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
b3DbvtProxy* proxy=(b3DbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(b3DbvtVolume) aabb=b3DbvtVolume::FromMM(aabbMin,aabbMax);
bool docollide=false;
if(proxy->stage==STAGECOUNT)
{/* fixed -> dynamic set */
@@ -380,7 +380,7 @@ void b3DynamicBvhBroadphase::setAabbForceUpdate( btBroadphaseProxy* abspr
m_needcleanup=true;
if(!m_deferedcollide)
{
btDbvtTreeCollider collider(this);
b3DbvtTreeCollider collider(this);
m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider);
m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider);
}
@@ -388,7 +388,7 @@ void b3DynamicBvhBroadphase::setAabbForceUpdate( btBroadphaseProxy* abspr
}
//
void b3DynamicBvhBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
{
collide(dispatcher);
#if DBVT_BP_PROFILE
@@ -415,30 +415,30 @@ void b3DynamicBvhBroadphase::calculateOverlappingPairs(btDispatcher* dispa
}
void b3DynamicBvhBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
{
if (m_paircache->hasDeferredRemoval())
{
btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
int invalidPair = 0;
int i;
btBroadphasePair previousPair(-1,-1);
b3BroadphasePair previousPair(-1,-1);
for (i=0;i<overlappingPairArray.size();i++)
{
btBroadphasePair& pair = overlappingPairArray[i];
b3BroadphasePair& pair = overlappingPairArray[i];
bool isDuplicate = (pair == previousPair);
@@ -449,8 +449,8 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
if (!isDuplicate)
{
//important to perform AABB check that is consistent with the broadphase
btDbvtProxy* pa=&m_proxies[pair.x];
btDbvtProxy* pb=&m_proxies[pair.y];
b3DbvtProxy* pa=&m_proxies[pair.x];
b3DbvtProxy* pb=&m_proxies[pair.y];
bool hasOverlap = Intersect(pa->leaf->volume,pb->leaf->volume);
if (hasOverlap)
@@ -479,13 +479,13 @@ void b3DynamicBvhBroadphase::performDeferredRemoval(btDispatcher* dispatcher)
}
//perform a sort, to sort 'invalid' pairs to the end
overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
}
}
//
void b3DynamicBvhBroadphase::collide(btDispatcher* dispatcher)
void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
{
/*printf("---------------------------------------------------------\n");
printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
@@ -511,16 +511,16 @@ void b3DynamicBvhBroadphase::collide(btDispatcher* dispatcher)
{
const int count=1+(m_sets[1].m_leaves*m_fupdates)/100;
m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100);
m_fixedleft=btMax<int>(0,m_fixedleft-count);
m_fixedleft=b3Max<int>(0,m_fixedleft-count);
}
/* dynamic -> fixed set */
m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
btDbvtProxy* current=m_stageRoots[m_stageCurrent];
b3DbvtProxy* current=m_stageRoots[m_stageCurrent];
if(current)
{
btDbvtTreeCollider collider(this);
b3DbvtTreeCollider collider(this);
do {
btDbvtProxy* next=current->links[1];
b3DbvtProxy* next=current->links[1];
listremove(current,m_stageRoots[current->stage]);
listappend(current,m_stageRoots[STAGECOUNT]);
#if DBVT_BP_ACCURATESLEEPING
@@ -530,7 +530,7 @@ void b3DynamicBvhBroadphase::collide(btDispatcher* dispatcher)
b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider);
#endif
m_sets[0].remove(current->leaf);
ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax);
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;
@@ -540,7 +540,7 @@ void b3DynamicBvhBroadphase::collide(btDispatcher* dispatcher)
}
/* collide dynamics */
{
btDbvtTreeCollider collider(this);
b3DbvtTreeCollider collider(this);
if(m_deferedcollide)
{
SPC(m_profiling.m_fdcollide);
@@ -556,21 +556,21 @@ void b3DynamicBvhBroadphase::collide(btDispatcher* dispatcher)
if(m_needcleanup)
{
SPC(m_profiling.m_cleanup);
btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
b3BroadphasePairArray& pairs=m_paircache->getOverlappingPairArray();
if(pairs.size()>0)
{
int ni=btMin(pairs.size(),btMax<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
int ni=b3Min(pairs.size(),b3Max<int>(m_newpairs,(pairs.size()*m_cupdates)/100));
for(int i=0;i<ni;++i)
{
btBroadphasePair& p=pairs[(m_cid+i)%pairs.size()];
btDbvtProxy* pa=&m_proxies[p.x];
btDbvtProxy* pb=&m_proxies[p.y];
b3BroadphasePair& p=pairs[(m_cid+i)%pairs.size()];
b3DbvtProxy* pa=&m_proxies[p.x];
b3DbvtProxy* pb=&m_proxies[p.y];
if(!Intersect(pa->leaf->volume,pb->leaf->volume))
{
#if DBVT_BP_SORTPAIRS
if(pa->m_uniqueId>pb->m_uniqueId)
btSwap(pa,pb);
b3Swap(pa,pb);
#endif
m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),dispatcher);
--ni;--i;
@@ -613,7 +613,7 @@ const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const
{
ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds;
ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds;
if(!m_sets[0].empty())
if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume,
@@ -622,12 +622,12 @@ void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector
bounds=m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else
bounds=btDbvtVolume::FromCR(b3Vector3(0,0,0),0);
bounds=b3DbvtVolume::FromCR(b3Vector3(0,0,0),0);
aabbMin=bounds.Mins();
aabbMax=bounds.Maxs();
}
void b3DynamicBvhBroadphase::resetPool(btDispatcher* dispatcher)
void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
{
int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
@@ -665,7 +665,7 @@ void b3DynamicBvhBroadphase::printStats()
//
#if DBVT_BP_ENABLE_BENCHMARK
struct btBroadphaseBenchmark
struct b3BroadphaseBenchmark
{
struct Experiment
{
@@ -681,22 +681,22 @@ struct btBroadphaseBenchmark
{
b3Vector3 center;
b3Vector3 extents;
btBroadphaseProxy* proxy;
b3BroadphaseProxy* proxy;
b3Scalar time;
void update(b3Scalar speed,b3Scalar amplitude,btBroadphaseInterface* pbi)
void update(b3Scalar speed,b3Scalar amplitude,b3BroadphaseInterface* pbi)
{
time += speed;
center[0] = btCos(time*(b3Scalar)2.17)*amplitude+
btSin(time)*amplitude/2;
center[1] = btCos(time*(b3Scalar)1.38)*amplitude+
btSin(time)*amplitude;
center[2] = btSin(time*(b3Scalar)0.777)*amplitude;
center[0] = b3Cos(time*(b3Scalar)2.17)*amplitude+
b3Sin(time)*amplitude/2;
center[1] = b3Cos(time*(b3Scalar)1.38)*amplitude+
b3Sin(time)*amplitude;
center[2] = b3Sin(time*(b3Scalar)0.777)*amplitude;
pbi->setAabb(proxy,center-extents,center+extents,0);
}
};
static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); }
static b3Scalar UnitRand() { return(UnsignedRand(16384)/(b3Scalar)16384); }
static void OutputTime(const char* name,btClock& c,unsigned count=0)
static void OutputTime(const char* name,b3Clock& c,unsigned count=0)
{
const unsigned long us=c.getTimeMicroseconds();
const unsigned long ms=(us+500)/1000;
@@ -708,21 +708,21 @@ struct btBroadphaseBenchmark
}
};
void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface* pbi)
void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
{
static const btBroadphaseBenchmark::Experiment experiments[]=
static const b3BroadphaseBenchmark::Experiment experiments[]=
{
{"1024o.10%",1024,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
};
static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]);
b3AlignedObjectArray<btBroadphaseBenchmark::Object*> objects;
btClock wallclock;
b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
b3Clock wallclock;
/* Begin */
for(int iexp=0;iexp<nexperiments;++iexp)
{
const btBroadphaseBenchmark::Experiment& experiment=experiments[iexp];
const b3BroadphaseBenchmark::Experiment& experiment=experiments[iexp];
const int object_count=experiment.object_count;
const int update_count=(object_count*experiment.update_count)/100;
const int spawn_count=(object_count*experiment.spawn_count)/100;
@@ -740,25 +740,25 @@ void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface* pbi)
objects.reserve(object_count);
for(int i=0;i<object_count;++i)
{
btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object();
po->center[0]=btBroadphaseBenchmark::UnitRand()*50;
po->center[1]=btBroadphaseBenchmark::UnitRand()*50;
po->center[2]=btBroadphaseBenchmark::UnitRand()*50;
po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2;
po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2;
po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2;
po->time=btBroadphaseBenchmark::UnitRand()*2000;
b3BroadphaseBenchmark::Object* po=new b3BroadphaseBenchmark::Object();
po->center[0]=b3BroadphaseBenchmark::UnitRand()*50;
po->center[1]=b3BroadphaseBenchmark::UnitRand()*50;
po->center[2]=b3BroadphaseBenchmark::UnitRand()*50;
po->extents[0]=b3BroadphaseBenchmark::UnitRand()*2+2;
po->extents[1]=b3BroadphaseBenchmark::UnitRand()*2+2;
po->extents[2]=b3BroadphaseBenchmark::UnitRand()*2+2;
po->time=b3BroadphaseBenchmark::UnitRand()*2000;
po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0);
objects.push_back(po);
}
btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
b3BroadphaseBenchmark::OutputTime("\tInitialization",wallclock);
/* First update */
wallclock.reset();
for(int i=0;i<objects.size();++i)
{
objects[i]->update(speed,amplitude,pbi);
}
btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
b3BroadphaseBenchmark::OutputTime("\tFirst update",wallclock);
/* Updates */
wallclock.reset();
for(int i=0;i<experiment.iterations;++i)
@@ -769,7 +769,7 @@ void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface* pbi)
}
pbi->calculateOverlappingPairs(0);
}
btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
b3BroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations);
/* Clean up */
wallclock.reset();
for(int i=0;i<objects.size();++i)
@@ -778,12 +778,12 @@ void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface* pbi)
delete objects[i];
}
objects.resize(0);
btBroadphaseBenchmark::OutputTime("\tRelease",wallclock);
b3BroadphaseBenchmark::OutputTime("\tRelease",wallclock);
}
}
#else
/*void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface*)
/*void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
{}
*/
#endif

70
src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
*/
///b3DynamicBvhBroadphase implementation by Nathanael Presson
#ifndef BT_DBVT_BROADPHASE_H
#define BT_DBVT_BROADPHASE_H
#ifndef B3_DBVT_BROADPHASE_H
#define B3_DBVT_BROADPHASE_H
#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
@@ -36,16 +36,16 @@ subject to the following restrictions:
#if DBVT_BP_PROFILE
#define DBVT_BP_PROFILING_RATE 256
#include "LinearMath/btQuickprof.h"
#include "LinearMath/b3Quickprof.h"
#endif
ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy
ATTRIBUTE_ALIGNED16(struct) b3BroadphaseProxy
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
///optional filtering to cull potential collisions
enum CollisionFilterGroups
@@ -59,7 +59,7 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
};
//Usually the client btCollisionObject or Rigidbody class
//Usually the client b3CollisionObject or Rigidbody class
void* m_clientObject;
short int m_collisionFilterGroup;
short int m_collisionFilterMask;
@@ -75,11 +75,11 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
}
//used for memory pools
btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
b3BroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0)
{
}
btBroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
b3BroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0)
:m_clientObject(userPtr),
m_collisionFilterGroup(collisionFilterGroup),
m_collisionFilterMask(collisionFilterMask),
@@ -95,30 +95,30 @@ BT_DECLARE_ALIGNED_ALLOCATOR();
//
// btDbvtProxy
// b3DbvtProxy
//
struct btDbvtProxy : btBroadphaseProxy
struct b3DbvtProxy : b3BroadphaseProxy
{
/* Fields */
//btDbvtAabbMm aabb;
btDbvtNode* leaf;
btDbvtProxy* links[2];
//b3DbvtAabbMm aabb;
b3DbvtNode* leaf;
b3DbvtProxy* links[2];
int stage;
/* ctor */
explicit btDbvtProxy() {}
btDbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
explicit b3DbvtProxy() {}
b3DbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) :
b3BroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask)
{
links[0]=links[1]=0;
}
};
typedef b3AlignedObjectArray<btDbvtProxy*> btDbvtProxyArray;
typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3.
///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
struct b3DynamicBvhBroadphase
{
/* Config */
@@ -129,9 +129,9 @@ struct b3DynamicBvhBroadphase
};
/* Fields */
b3DynamicBvh m_sets[2]; // Dbvt sets
btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
b3DbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list
b3AlignedObjectArray<btDbvtProxy> m_proxies;
b3AlignedObjectArray<b3DbvtProxy> m_proxies;
b3OverlappingPairCache* m_paircache; // Pair cache
b3Scalar m_prediction; // Velocity prediction
int m_stageCurrent; // Current stage
@@ -149,7 +149,7 @@ struct b3DynamicBvhBroadphase
bool m_deferedcollide; // Defere dynamic/static collision to collide call
bool m_needcleanup; // Need to run cleanup?
#if DBVT_BP_PROFILE
btClock m_clock;
b3Clock m_clock;
struct {
unsigned long m_total;
unsigned long m_ddcollide;
@@ -161,18 +161,18 @@ struct b3DynamicBvhBroadphase
/* Methods */
b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0);
~b3DynamicBvhBroadphase();
void collide(btDispatcher* dispatcher);
void collide(b3Dispatcher* dispatcher);
void optimize();
/* btBroadphaseInterface Implementation */
btBroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher);
virtual void setAabb(btBroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,btDispatcher* dispatcher);
virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, btBroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3Vector3(0,0,0), const b3Vector3& aabbMax = b3Vector3(0,0,0));
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, btBroadphaseAabbCallback& callback);
/* b3BroadphaseInterface Implementation */
b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
virtual void destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
virtual void setAabb(b3BroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3Vector3(0,0,0), const b3Vector3& aabbMax = b3Vector3(0,0,0));
virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
virtual void getAabb(btBroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
virtual void calculateOverlappingPairs(btDispatcher* dispatcher=0);
virtual void getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
virtual b3OverlappingPairCache* getOverlappingPairCache();
virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
virtual void getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const;
@@ -180,9 +180,9 @@ struct b3DynamicBvhBroadphase
///reset broadphase internal structures, to ensure determinism/reproducability
virtual void resetPool(btDispatcher* dispatcher);
virtual void resetPool(b3Dispatcher* dispatcher);
void performDeferredRemoval(btDispatcher* dispatcher);
void performDeferredRemoval(b3Dispatcher* dispatcher);
void setVelocityPrediction(b3Scalar prediction)
{
@@ -194,12 +194,12 @@ struct b3DynamicBvhBroadphase
}
///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
///it is not part of the btBroadphaseInterface but specific to b3DynamicBvhBroadphase.
///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
///http://code.google.com/p/bullet/issues/detail?id=223
void setAabbForceUpdate( btBroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,btDispatcher* /*dispatcher*/);
void setAabbForceUpdate( b3BroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* /*dispatcher*/);
//static void benchmark(btBroadphaseInterface*);
//static void benchmark(b3BroadphaseInterface*);
};

16
src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h
View File

@@ -1,13 +1,13 @@
#ifndef B3_OVERLAPPING_PAIR_H
#define B3_OVERLAPPING_PAIR_H
#include "Bullet3Common/btInt2.h"
#include "Bullet3Common/b3Int2.h"
//typedef btInt2 btBroadphasePair;
struct btBroadphasePair : public btInt2
//typedef b3Int2 b3BroadphasePair;
struct b3BroadphasePair : public b3Int2
{
explicit btBroadphasePair(){}
btBroadphasePair(int xx,int yy)
explicit b3BroadphasePair(){}
b3BroadphasePair(int xx,int yy)
{
if (xx < yy)
{
@@ -22,11 +22,11 @@ struct btBroadphasePair : public btInt2
}
};
class btBroadphasePairSortPredicate
class b3BroadphasePairSortPredicate
{
public:
bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const
bool operator() ( const b3BroadphasePair& a, const b3BroadphasePair& b ) const
{
const int uidA0 = a.x;
const int uidB0 = b.x;
@@ -36,7 +36,7 @@ class btBroadphasePairSortPredicate
}
};
SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b)
SIMD_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
{
return (a.x == b.x ) && (a.y == b.y );
}

150
src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp
View File

@@ -17,8 +17,8 @@ subject to the following restrictions:
#include "b3OverlappingPairCache.h"
//#include "btDispatcher.h"
//#include "btCollisionAlgorithm.h"
//#include "b3Dispatcher.h"
//#include "b3CollisionAlgorithm.h"
#include "Bullet3Geometry/b3AabbUtil.h"
#include <stdio.h>
@@ -32,7 +32,7 @@ int gFindPairs =0;
btHashedOverlappingPairCache::btHashedOverlappingPairCache():
b3HashedOverlappingPairCache::b3HashedOverlappingPairCache():
m_overlapFilterCallback(0),
m_blockedForChanges(false)
{
@@ -44,18 +44,18 @@ btHashedOverlappingPairCache::btHashedOverlappingPairCache():
btHashedOverlappingPairCache::~btHashedOverlappingPairCache()
b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
{
}
void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
{
/* if (pair.m_algorithm)
{
{
pair.m_algorithm->~btCollisionAlgorithm();
pair.m_algorithm->~b3CollisionAlgorithm();
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm=0;
}
@@ -67,23 +67,23 @@ void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,b
void btHashedOverlappingPairCache::cleanProxyFromPairs(int proxy,btDispatcher* dispatcher)
void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
{
class CleanPairCallback : public btOverlapCallback
class CleanPairCallback : public b3OverlapCallback
{
int m_cleanProxy;
b3OverlappingPairCache* m_pairCache;
btDispatcher* m_dispatcher;
b3Dispatcher* m_dispatcher;
public:
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,btDispatcher* dispatcher)
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
:m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
if ((pair.x == m_cleanProxy) ||
(pair.y == m_cleanProxy))
@@ -104,10 +104,10 @@ void btHashedOverlappingPairCache::cleanProxyFromPairs(int proxy,btDispatcher* d
void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher)
void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
{
class RemovePairCallback : public btOverlapCallback
class RemovePairCallback : public b3OverlapCallback
{
int m_obsoleteProxy;
@@ -116,7 +116,7 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int pro
:m_obsoleteProxy(obsoleteProxy)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
return ((pair.x == m_obsoleteProxy) ||
(pair.y == m_obsoleteProxy));
@@ -134,16 +134,16 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int pro
btBroadphasePair* btHashedOverlappingPairCache::findPair(int proxy0, int proxy1)
b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
{
gFindPairs++;
if(proxy0 >proxy1)
btSwap(proxy0,proxy1);
b3Swap(proxy0,proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
b3Swap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
@@ -153,24 +153,24 @@ btBroadphasePair* btHashedOverlappingPairCache::findPair(int proxy0, int proxy1)
}
int index = m_hashTable[hash];
while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
{
index = m_next[index];
}
if (index == BT_NULL_PAIR)
if (index == B3_NULL_PAIR)
{
return NULL;
}
btAssert(index < m_overlappingPairArray.size());
b3Assert(index < m_overlappingPairArray.size());
return &m_overlappingPairArray[index];
}
//#include <stdio.h>
void btHashedOverlappingPairCache::growTables()
void b3HashedOverlappingPairCache::growTables()
{
int newCapacity = m_overlappingPairArray.capacity();
@@ -188,21 +188,21 @@ void btHashedOverlappingPairCache::growTables()
for (i= 0; i < newCapacity; ++i)
{
m_hashTable[i] = BT_NULL_PAIR;
m_hashTable[i] = B3_NULL_PAIR;
}
for (i = 0; i < newCapacity; ++i)
{
m_next[i] = BT_NULL_PAIR;
m_next[i] = B3_NULL_PAIR;
}
for(i=0;i<curHashtableSize;i++)
{
const btBroadphasePair& pair = m_overlappingPairArray[i];
const b3BroadphasePair& pair = m_overlappingPairArray[i];
int proxyId1 = pair.x;
int proxyId2 = pair.y;
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
b3Swap(proxyId1, proxyId2);*/
int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
@@ -212,20 +212,20 @@ void btHashedOverlappingPairCache::growTables()
}
}
btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
{
if(proxy0>proxy1)
btSwap(proxy0,proxy1);
b3Swap(proxy0,proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
b3Swap(proxyId1, proxyId2);*/
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
btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair != NULL)
{
return pair;
@@ -256,7 +256,7 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(int proxy0, int
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
}
pair = new (mem) btBroadphasePair(proxy0,proxy1);
pair = new (mem) b3BroadphasePair(proxy0,proxy1);
// pair->m_pProxy0 = proxy0;
// pair->m_pProxy1 = proxy1;
@@ -272,20 +272,20 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(int proxy0, int
void* btHashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,btDispatcher* dispatcher)
void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,b3Dispatcher* dispatcher)
{
gRemovePairs++;
if(proxy0>proxy1)
btSwap(proxy0,proxy1);
b3Swap(proxy0,proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
b3Swap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair == NULL)
{
return 0;
@@ -296,22 +296,22 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
int pairIndex = int(pair - &m_overlappingPairArray[0]);
btAssert(pairIndex < m_overlappingPairArray.size());
b3Assert(pairIndex < m_overlappingPairArray.size());
// Remove the pair from the hash table.
int index = m_hashTable[hash];
btAssert(index != BT_NULL_PAIR);
b3Assert(index != B3_NULL_PAIR);
int previous = BT_NULL_PAIR;
int previous = B3_NULL_PAIR;
while (index != pairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_NULL_PAIR)
if (previous != B3_NULL_PAIR)
{
btAssert(m_next[previous] == pairIndex);
b3Assert(m_next[previous] == pairIndex);
m_next[previous] = m_next[pairIndex];
}
else
@@ -336,23 +336,23 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
}
// Remove the last pair from the hash table.
const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity()-1));
index = m_hashTable[lastHash];
btAssert(index != BT_NULL_PAIR);
b3Assert(index != B3_NULL_PAIR);
previous = BT_NULL_PAIR;
previous = B3_NULL_PAIR;
while (index != lastPairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_NULL_PAIR)
if (previous != B3_NULL_PAIR)
{
btAssert(m_next[previous] == lastPairIndex);
b3Assert(m_next[previous] == lastPairIndex);
m_next[previous] = m_next[lastPairIndex];
}
else
@@ -373,7 +373,7 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1
}
//#include <stdio.h>
void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
{
int i;
@@ -382,7 +382,7 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
for (i=0;i<m_overlappingPairArray.size();)
{
btBroadphasePair* pair = &m_overlappingPairArray[i];
b3BroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
removeOverlappingPair(pair->x,pair->y,dispatcher);
@@ -399,10 +399,10 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
{
///need to keep hashmap in sync with pair address, so rebuild all
btBroadphasePairArray tmpPairs;
b3BroadphasePairArray tmpPairs;
int i;
for (i=0;i<m_overlappingPairArray.size();i++)
{
@@ -416,10 +416,10 @@ void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher
for (i = 0; i < m_next.size(); i++)
{
m_next[i] = BT_NULL_PAIR;
m_next[i] = B3_NULL_PAIR;
}
tmpPairs.quickSort(btBroadphasePairSortPredicate());
tmpPairs.quickSort(b3BroadphasePairSortPredicate());
for (i=0;i<tmpPairs.size();i++)
{
@@ -430,18 +430,18 @@ void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher
}
void* btSortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, btDispatcher* dispatcher )
void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1, b3Dispatcher* dispatcher )
{
if (!hasDeferredRemoval())
{
btBroadphasePair findPair(proxy0,proxy1);
b3BroadphasePair findPair(proxy0,proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
if (findIndex < m_overlappingPairArray.size())
{
gOverlappingPairs--;
btBroadphasePair& pair = m_overlappingPairArray[findIndex];
b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
cleanOverlappingPair(pair,dispatcher);
//if (m_ghostPairCallback)
@@ -463,16 +463,16 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(int proxy0,int proxy1,
btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1)
b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1)
{
//don't add overlap with own
btAssert(proxy0 != proxy1);
b3Assert(proxy0 != proxy1);
if (!needsBroadphaseCollision(proxy0,proxy1))
return 0;
void* mem = &m_overlappingPairArray.expandNonInitializing();
btBroadphasePair* pair = new (mem) btBroadphasePair(proxy0,proxy1);
b3BroadphasePair* pair = new (mem) b3BroadphasePair(proxy0,proxy1);
gOverlappingPairs++;
@@ -488,18 +488,18 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(int proxy0,in
///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
btBroadphasePair* btSortedOverlappingPairCache::findPair(int proxy0,int proxy1)
b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0,int proxy1)
{
if (!needsBroadphaseCollision(proxy0,proxy1))
return 0;
btBroadphasePair tmpPair(proxy0,proxy1);
b3BroadphasePair tmpPair(proxy0,proxy1);
int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
if (findIndex < m_overlappingPairArray.size())
{
//btAssert(it != m_overlappingPairSet.end());
btBroadphasePair* pair = &m_overlappingPairArray[findIndex];
//b3Assert(it != m_overlappingPairSet.end());
b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
return pair;
}
return 0;
@@ -516,7 +516,7 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(int proxy0,in
//#include <stdio.h>
void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback,b3Dispatcher* dispatcher)
{
int i;
@@ -524,7 +524,7 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
for (i=0;i<m_overlappingPairArray.size();)
{
btBroadphasePair* pair = &m_overlappingPairArray[i];
b3BroadphasePair* pair = &m_overlappingPairArray[i];
if (callback->processOverlap(*pair))
{
cleanOverlappingPair(*pair,dispatcher);
@@ -543,7 +543,7 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback*
btSortedOverlappingPairCache::btSortedOverlappingPairCache():
b3SortedOverlappingPairCache::b3SortedOverlappingPairCache():
m_blockedForChanges(false),
m_hasDeferredRemoval(true),
m_overlapFilterCallback(0)
@@ -553,16 +553,16 @@ btSortedOverlappingPairCache::btSortedOverlappingPairCache():
m_overlappingPairArray.reserve(initialAllocatedSize);
}
btSortedOverlappingPairCache::~btSortedOverlappingPairCache()
b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
{
}
void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher)
void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher)
{
/* if (pair.m_algorithm)
{
{
pair.m_algorithm->~btCollisionAlgorithm();
pair.m_algorithm->~b3CollisionAlgorithm();
dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
pair.m_algorithm=0;
gRemovePairs--;
@@ -572,23 +572,23 @@ void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,b
}
void btSortedOverlappingPairCache::cleanProxyFromPairs(int proxy,btDispatcher* dispatcher)
void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher)
{
class CleanPairCallback : public btOverlapCallback
class CleanPairCallback : public b3OverlapCallback
{
int m_cleanProxy;
b3OverlappingPairCache* m_pairCache;
btDispatcher* m_dispatcher;
b3Dispatcher* m_dispatcher;
public:
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,btDispatcher* dispatcher)
CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,b3Dispatcher* dispatcher)
:m_cleanProxy(cleanProxy),
m_pairCache(pairCache),
m_dispatcher(dispatcher)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
if ((pair.x == m_cleanProxy) ||
(pair.y == m_cleanProxy))
@@ -607,10 +607,10 @@ void btSortedOverlappingPairCache::cleanProxyFromPairs(int proxy,btDispatcher* d
}
void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher)
void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher)
{
class RemovePairCallback : public btOverlapCallback
class RemovePairCallback : public b3OverlapCallback
{
int m_obsoleteProxy;
@@ -619,7 +619,7 @@ void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int pro
:m_obsoleteProxy(obsoleteProxy)
{
}
virtual bool processOverlap(btBroadphasePair& pair)
virtual bool processOverlap(b3BroadphasePair& pair)
{
return ((pair.x == m_obsoleteProxy) ||
(pair.y == m_obsoleteProxy));
@@ -632,7 +632,7 @@ void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int pro
processAllOverlappingPairs(&removeCallback,dispatcher);
}
void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher)
void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
{
//should already be sorted
}

190
src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h
View File

@@ -13,31 +13,31 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_OVERLAPPING_PAIR_CACHE_H
#define BT_OVERLAPPING_PAIR_CACHE_H
#ifndef B3_OVERLAPPING_PAIR_CACHE_H
#define B3_OVERLAPPING_PAIR_CACHE_H
#include "Bullet3Common/btInt2.h"
#include "Bullet3Common/b3Int2.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
class btDispatcher;
class b3Dispatcher;
#include "b3OverlappingPair.h"
typedef b3AlignedObjectArray<btBroadphasePair> btBroadphasePairArray;
typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
struct btOverlapCallback
struct b3OverlapCallback
{
virtual ~btOverlapCallback()
virtual ~b3OverlapCallback()
{}
//return true for deletion of the pair
virtual bool processOverlap(btBroadphasePair& pair) = 0;
virtual bool processOverlap(b3BroadphasePair& pair) = 0;
};
struct btOverlapFilterCallback
struct b3OverlapFilterCallback
{
virtual ~btOverlapFilterCallback()
virtual ~b3OverlapFilterCallback()
{}
// return true when pairs need collision
virtual bool needBroadphaseCollision(int proxy0,int proxy1) const = 0;
@@ -53,62 +53,62 @@ extern int gRemovePairs;
extern int gAddedPairs;
extern int gFindPairs;
const int BT_NULL_PAIR=0xffffffff;
const int B3_NULL_PAIR=0xffffffff;
///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases.
///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations.
///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
class b3OverlappingPairCache
{
public:
virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor
virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
virtual btBroadphasePairArray& getOverlappingPairArray() = 0;
virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0;
virtual void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher) = 0;
virtual int getNumOverlappingPairs() const = 0;
virtual void cleanProxyFromPairs(int proxy,btDispatcher* dispatcher) = 0;
virtual void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher) = 0;
virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0;
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0;
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher) = 0;
virtual btBroadphasePair* findPair(int proxy0, int proxy1) = 0;
virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
virtual bool hasDeferredRemoval() = 0;
//virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0;
//virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
virtual btBroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0;
virtual void* removeOverlappingPair(int proxy0,int proxy1,btDispatcher* dispatcher)=0;
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,btDispatcher* /*dispatcher*/)=0;
virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0;
virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher)=0;
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)=0;
virtual void sortOverlappingPairs(btDispatcher* 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
class btHashedOverlappingPairCache : public b3OverlappingPairCache
class b3HashedOverlappingPairCache : public b3OverlappingPairCache
{
btBroadphasePairArray m_overlappingPairArray;
btOverlapFilterCallback* m_overlapFilterCallback;
b3BroadphasePairArray m_overlappingPairArray;
b3OverlapFilterCallback* m_overlapFilterCallback;
bool m_blockedForChanges;
public:
btHashedOverlappingPairCache();
virtual ~btHashedOverlappingPairCache();
b3HashedOverlappingPairCache();
virtual ~b3HashedOverlappingPairCache();
virtual void removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher);
virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
virtual void* removeOverlappingPair(int proxy0,int proxy1,btDispatcher* dispatcher);
virtual void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
SIMD_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const
{
@@ -123,7 +123,7 @@ public:
// Add a pair and return the new pair. If the pair already exists,
// no new pair is created and the old one is returned.
virtual btBroadphasePair* addOverlappingPair(int proxy0,int proxy1)
virtual b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1)
{
gAddedPairs++;
@@ -135,46 +135,46 @@ public:
void cleanProxyFromPairs(int proxy,btDispatcher* dispatcher);
void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
virtual btBroadphasePair* getOverlappingPairArrayPtr()
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
btBroadphasePairArray& getOverlappingPairArray()
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const btBroadphasePairArray& getOverlappingPairArray() const
const b3BroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
btBroadphasePair* findPair(int proxy0, int proxy1);
b3BroadphasePair* findPair(int proxy0, int proxy1);
int GetCount() const { return m_overlappingPairArray.size(); }
// btBroadphasePair* GetPairs() { return m_pairs; }
// b3BroadphasePair* GetPairs() { return m_pairs; }
btOverlapFilterCallback* getOverlapFilterCallback()
b3OverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
void setOverlapFilterCallback(btOverlapFilterCallback* callback)
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
{
m_overlapFilterCallback = callback;
}
@@ -185,11 +185,11 @@ public:
}
private:
btBroadphasePair* internalAddPair(int proxy0,int proxy1);
b3BroadphasePair* internalAddPair(int proxy0,int proxy1);
void growTables();
SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2)
SIMD_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
{
return pair.x == proxyId1 && pair.y == proxyId2;
}
@@ -230,28 +230,28 @@ private:
SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
SIMD_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
{
int proxyId1 = proxy0;
int proxyId2 = proxy1;
#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);
b3Swap(proxyId1, proxyId2);
#endif
int index = m_hashTable[hash];
while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
while( index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
{
index = m_next[index];
}
if ( index == BT_NULL_PAIR )
if ( index == B3_NULL_PAIR )
{
return NULL;
}
btAssert(index < m_overlappingPairArray.size());
b3Assert(index < m_overlappingPairArray.size());
return &m_overlappingPairArray[index];
}
@@ -261,33 +261,33 @@ private:
return false;
}
/* virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
*/
virtual void sortOverlappingPairs(btDispatcher* dispatcher);
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
protected:
b3AlignedObjectArray<int> m_hashTable;
b3AlignedObjectArray<int> m_next;
// btOverlappingPairCallback* m_ghostPairCallback;
// b3OverlappingPairCallback* m_ghostPairCallback;
};
///btSortedOverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase
class btSortedOverlappingPairCache : public b3OverlappingPairCache
///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
class b3SortedOverlappingPairCache : public b3OverlappingPairCache
{
protected:
//avoid brute-force finding all the time
btBroadphasePairArray m_overlappingPairArray;
b3BroadphasePairArray m_overlappingPairArray;
//during the dispatch, check that user doesn't destroy/create proxy
bool m_blockedForChanges;
@@ -296,29 +296,29 @@ class btSortedOverlappingPairCache : public b3OverlappingPairCache
bool m_hasDeferredRemoval;
//if set, use the callback instead of the built in filter in needBroadphaseCollision
btOverlapFilterCallback* m_overlapFilterCallback;
b3OverlapFilterCallback* m_overlapFilterCallback;
// btOverlappingPairCallback* m_ghostPairCallback;
// b3OverlappingPairCallback* m_ghostPairCallback;
public:
btSortedOverlappingPairCache();
virtual ~btSortedOverlappingPairCache();
b3SortedOverlappingPairCache();
virtual ~b3SortedOverlappingPairCache();
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher);
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* dispatcher);
void* removeOverlappingPair(int proxy0,int proxy1,btDispatcher* dispatcher);
void* removeOverlappingPair(int proxy0,int proxy1,b3Dispatcher* dispatcher);
void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher);
void cleanOverlappingPair(b3BroadphasePair& pair,b3Dispatcher* dispatcher);
btBroadphasePair* addOverlappingPair(int proxy0,int proxy1);
b3BroadphasePair* addOverlappingPair(int proxy0,int proxy1);
btBroadphasePair* findPair(int proxy0,int proxy1);
b3BroadphasePair* findPair(int proxy0,int proxy1);
void cleanProxyFromPairs(int proxy,btDispatcher* dispatcher);
void cleanProxyFromPairs(int proxy,b3Dispatcher* dispatcher);
virtual void removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher);
virtual void removeOverlappingPairsContainingProxy(int proxy,b3Dispatcher* dispatcher);
inline bool needsBroadphaseCollision(int proxy0,int proxy1) const
@@ -332,12 +332,12 @@ class btSortedOverlappingPairCache : public b3OverlappingPairCache
return collides;
}
btBroadphasePairArray& getOverlappingPairArray()
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
const btBroadphasePairArray& getOverlappingPairArray() const
const b3BroadphasePairArray& getOverlappingPairArray() const
{
return m_overlappingPairArray;
}
@@ -345,12 +345,12 @@ class btSortedOverlappingPairCache : public b3OverlappingPairCache
btBroadphasePair* getOverlappingPairArrayPtr()
b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
@@ -360,12 +360,12 @@ class btSortedOverlappingPairCache : public b3OverlappingPairCache
return m_overlappingPairArray.size();
}
btOverlapFilterCallback* getOverlapFilterCallback()
b3OverlapFilterCallback* getOverlapFilterCallback()
{
return m_overlapFilterCallback;
}
void setOverlapFilterCallback(btOverlapFilterCallback* callback)
void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
{
m_overlapFilterCallback = callback;
}
@@ -375,40 +375,40 @@ class btSortedOverlappingPairCache : public b3OverlappingPairCache
return m_hasDeferredRemoval;
}
/* virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)
/* virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
{
m_ghostPairCallback = ghostPairCallback;
}
*/
virtual void sortOverlappingPairs(btDispatcher* dispatcher);
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
};
///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
class btNullPairCache : public b3OverlappingPairCache
///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
class b3NullPairCache : public b3OverlappingPairCache
{
btBroadphasePairArray m_overlappingPairArray;
b3BroadphasePairArray m_overlappingPairArray;
public:
virtual btBroadphasePair* getOverlappingPairArrayPtr()
virtual b3BroadphasePair* getOverlappingPairArrayPtr()
{
return &m_overlappingPairArray[0];
}
const btBroadphasePair* getOverlappingPairArrayPtr() const
const b3BroadphasePair* getOverlappingPairArrayPtr() const
{
return &m_overlappingPairArray[0];
}
btBroadphasePairArray& getOverlappingPairArray()
b3BroadphasePairArray& getOverlappingPairArray()
{
return m_overlappingPairArray;
}
virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/)
virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/,b3Dispatcher* /*dispatcher*/)
{
}
@@ -418,20 +418,20 @@ public:
return 0;
}
virtual void cleanProxyFromPairs(int /*proxy*/,btDispatcher* /*dispatcher*/)
virtual void cleanProxyFromPairs(int /*proxy*/,b3Dispatcher* /*dispatcher*/)
{
}
virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/)
virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
{
}
virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/)
virtual void processAllOverlappingPairs(b3OverlapCallback*,b3Dispatcher* /*dispatcher*/)
{
}
virtual btBroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
{
return 0;
}
@@ -441,26 +441,26 @@ public:
return true;
}
// virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */)
// virtual void setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
// {
//
// }
virtual btBroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/)
virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/)
{
return 0;
}
virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,btDispatcher* /*dispatcher*/)
virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,b3Dispatcher* /*dispatcher*/)
{
return 0;
}
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,btDispatcher* /*dispatcher*/)
virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,b3Dispatcher* /*dispatcher*/)
{
}
virtual void sortOverlappingPairs(btDispatcher* dispatcher)
virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
{
(void) dispatcher;
}
@@ -469,6 +469,6 @@ public:
};
#endif //BT_OVERLAPPING_PAIR_CACHE_H
#endif //B3_OVERLAPPING_PAIR_CACHE_H

12
src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h
View File

@@ -1,12 +1,12 @@
#ifndef BT_CONTACT4_H
#define BT_CONTACT4_H
#ifndef B3_CONTACT4_H
#define B3_CONTACT4_H
#include "Bullet3Common/b3Vector3.h"
ATTRIBUTE_ALIGNED16(struct) b3Contact4
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_worldPos[4];
b3Vector3 m_worldNormal;
@@ -27,9 +27,9 @@ ATTRIBUTE_ALIGNED16(struct) b3Contact4
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 ) { btAssert( 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 ) { btAssert( 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]; }
float getNPoints() const { return m_worldNormal[3]; }
@@ -39,4 +39,4 @@ ATTRIBUTE_ALIGNED16(struct) b3Contact4
bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
};
#endif //BT_CONTACT4_H
#endif //B3_CONTACT4_H

10
src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h
View File

@@ -1,12 +1,12 @@
#ifndef BT_RIGID_BODY_CL
#define BT_RIGID_BODY_CL
#ifndef B3_RIGID_BODY_CL
#define B3_RIGID_BODY_CL
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Matrix3x3.h"
ATTRIBUTE_ALIGNED16(struct) b3RigidBodyCL
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_pos;
b3Quaternion m_quat;
@@ -25,11 +25,11 @@ ATTRIBUTE_ALIGNED16(struct) b3RigidBodyCL
};
struct btInertiaCL
struct b3InertiaCL
{
b3Matrix3x3 m_invInertiaWorld;
b3Matrix3x3 m_initInvInertia;
};
#endif//BT_RIGID_BODY_CL
#endif//B3_RIGID_BODY_CL

60
src/Bullet3Common/b3AlignedAllocator.cpp
View File

@@ -19,41 +19,41 @@ int gNumAlignedAllocs = 0;
int gNumAlignedFree = 0;
int gTotalBytesAlignedAllocs = 0;//detect memory leaks
static void *btAllocDefault(size_t size)
static void *b3AllocDefault(size_t size)
{
return malloc(size);
}
static void btFreeDefault(void *ptr)
static void b3FreeDefault(void *ptr)
{
free(ptr);
}
static btAllocFunc *sAllocFunc = btAllocDefault;
static btFreeFunc *sFreeFunc = btFreeDefault;
static b3AllocFunc *sAllocFunc = b3AllocDefault;
static b3FreeFunc *sFreeFunc = b3FreeDefault;
#if defined (BT_HAS_ALIGNED_ALLOCATOR)
#if defined (B3_HAS_ALIGNED_ALLOCATOR)
#include <malloc.h>
static void *btAlignedAllocDefault(size_t size, int alignment)
static void *b3AlignedAllocDefault(size_t size, int alignment)
{
return _aligned_malloc(size, (size_t)alignment);
}
static void btAlignedFreeDefault(void *ptr)
static void b3AlignedFreeDefault(void *ptr)
{
_aligned_free(ptr);
}
#elif defined(__CELLOS_LV2__)
#include <stdlib.h>
static inline void *btAlignedAllocDefault(size_t size, int alignment)
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
{
return memalign(alignment, size);
}
static inline void btAlignedFreeDefault(void *ptr)
static inline void b3AlignedFreeDefault(void *ptr)
{
free(ptr);
}
@@ -63,13 +63,13 @@ static inline void btAlignedFreeDefault(void *ptr)
static inline void *btAlignedAllocDefault(size_t size, int alignment)
static inline void *b3AlignedAllocDefault(size_t size, int alignment)
{
void *ret;
char *real;
real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
if (real) {
ret = btAlignPointer(real + sizeof(void *),alignment);
ret = b3AlignPointer(real + sizeof(void *),alignment);
*((void **)(ret)-1) = (void *)(real);
} else {
ret = (void *)(real);
@@ -77,7 +77,7 @@ static inline void *btAlignedAllocDefault(size_t size, int alignment)
return (ret);
}
static inline void btAlignedFreeDefault(void *ptr)
static inline void b3AlignedFreeDefault(void *ptr)
{
void* real;
@@ -89,26 +89,26 @@ static inline void btAlignedFreeDefault(void *ptr)
#endif
static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
static b3AlignedAllocFunc *sAlignedAllocFunc = b3AlignedAllocDefault;
static b3AlignedFreeFunc *sAlignedFreeFunc = b3AlignedFreeDefault;
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
{
sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
sAlignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
sAlignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
}
void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc)
void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
{
sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
sAllocFunc = allocFunc ? allocFunc : b3AllocDefault;
sFreeFunc = freeFunc ? freeFunc : b3FreeDefault;
}
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
//this generic allocator provides the total allocated number of bytes
#include <stdio.h>
void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename)
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename)
{
void *ret;
char *real;
@@ -119,7 +119,7 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen
real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1));
if (real) {
ret = (void*) btAlignPointer(real + 2*sizeof(void *), alignment);
ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
*((int*)(ret)-2) = size;
@@ -134,7 +134,7 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen
return (ret);
}
void btAlignedFreeInternal (void* ptr,int line,char* filename)
void b3AlignedFreeInternal (void* ptr,int line,char* filename)
{
void* real;
@@ -154,18 +154,18 @@ void btAlignedFreeInternal (void* ptr,int line,char* filename)
}
}
#else //BT_DEBUG_MEMORY_ALLOCATIONS
#else //B3_DEBUG_MEMORY_ALLOCATIONS
void* btAlignedAllocInternal (size_t size, int alignment)
void* b3AlignedAllocInternal (size_t size, int alignment)
{
gNumAlignedAllocs++;
void* ptr;
ptr = sAlignedAllocFunc(size, alignment);
// printf("btAlignedAllocInternal %d, %x\n",size,ptr);
// printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
return ptr;
}
void btAlignedFreeInternal (void* ptr)
void b3AlignedFreeInternal (void* ptr)
{
if (!ptr)
{
@@ -173,9 +173,9 @@ void btAlignedFreeInternal (void* ptr)
}
gNumAlignedFree++;
// printf("btAlignedFreeInternal %x\n",ptr);
// printf("b3AlignedFreeInternal %x\n",ptr);
sAlignedFreeFunc(ptr);
}
#endif //BT_DEBUG_MEMORY_ALLOCATIONS
#endif //B3_DEBUG_MEMORY_ALLOCATIONS

52
src/Bullet3Common/b3AlignedAllocator.h
View File

@@ -13,50 +13,50 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_ALIGNED_ALLOCATOR
#define BT_ALIGNED_ALLOCATOR
#ifndef B3_ALIGNED_ALLOCATOR
#define B3_ALIGNED_ALLOCATOR
///we probably replace this with our own aligned memory allocator
///so we replace _aligned_malloc and _aligned_free with our own
///that is better portable and more predictable
#include "b3Scalar.h"
//#define BT_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
#define btAlignedAlloc(a,b) \
btAlignedAllocInternal(a,b,__LINE__,__FILE__)
#define b3AlignedAlloc(a,b) \
b3AlignedAllocInternal(a,b,__LINE__,__FILE__)
#define btAlignedFree(ptr) \
btAlignedFreeInternal(ptr,__LINE__,__FILE__)
#define b3AlignedFree(ptr) \
b3AlignedFreeInternal(ptr,__LINE__,__FILE__)
void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename);
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename);
void btAlignedFreeInternal (void* ptr,int line,char* filename);
void b3AlignedFreeInternal (void* ptr,int line,char* filename);
#else
void* btAlignedAllocInternal (size_t size, int alignment);
void btAlignedFreeInternal (void* ptr);
void* b3AlignedAllocInternal (size_t size, int alignment);
void b3AlignedFreeInternal (void* ptr);
#define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
#define b3AlignedAlloc(size,alignment) b3AlignedAllocInternal(size,alignment)
#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
#endif
typedef int size_type;
typedef void *(btAlignedAllocFunc)(size_t size, int alignment);
typedef void (btAlignedFreeFunc)(void *memblock);
typedef void *(btAllocFunc)(size_t size);
typedef void (btFreeFunc)(void *memblock);
typedef void *(b3AlignedAllocFunc)(size_t size, int alignment);
typedef void (b3AlignedFreeFunc)(void *memblock);
typedef void *(b3AllocFunc)(size_t size);
typedef void (b3FreeFunc)(void *memblock);
///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
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 btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
///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 {
@@ -83,11 +83,11 @@ public:
const_pointer address ( const_reference ref ) const { return &ref; }
pointer allocate ( size_type n , const_pointer * hint = 0 ) {
(void)hint;
return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
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 ) {
btAlignedFree( reinterpret_cast< void * >( ptr ) );
b3AlignedFree( reinterpret_cast< void * >( ptr ) );
}
void destroy ( pointer ptr ) { ptr->~value_type(); }
@@ -103,5 +103,5 @@ public:
#endif //BT_ALIGNED_ALLOCATOR
#endif //B3_ALIGNED_ALLOCATOR

68
src/Bullet3Common/b3AlignedObjectArray.h
View File

@@ -14,30 +14,30 @@ subject to the following restrictions:
*/
#ifndef BT_OBJECT_ARRAY__
#define BT_OBJECT_ARRAY__
#ifndef B3_OBJECT_ARRAY__
#define B3_OBJECT_ARRAY__
#include "b3Scalar.h" // has definitions like SIMD_FORCE_INLINE
#include "b3AlignedAllocator.h"
///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
///You can enable BT_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
#define BT_USE_PLACEMENT_NEW 1
//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
#define B3_USE_PLACEMENT_NEW 1
//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
#define B3_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
#ifdef BT_USE_MEMCPY
#ifdef B3_USE_MEMCPY
#include <memory.h>
#include <string.h>
#endif //BT_USE_MEMCPY
#endif //B3_USE_MEMCPY
#ifdef BT_USE_PLACEMENT_NEW
#ifdef B3_USE_PLACEMENT_NEW
#include <new> //for placement new
#endif //BT_USE_PLACEMENT_NEW
#endif //B3_USE_PLACEMENT_NEW
///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
@@ -54,17 +54,17 @@ class b3AlignedObjectArray
//PCK: added this line
bool m_ownsMemory;
#ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
public:
SIMD_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other)
{
copyFromArray(other);
return *this;
}
#else//BT_ALLOW_ARRAY_COPY_OPERATOR
#else//B3_ALLOW_ARRAY_COPY_OPERATOR
private:
SIMD_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T> &other);
#endif//BT_ALLOW_ARRAY_COPY_OPERATOR
#endif//B3_ALLOW_ARRAY_COPY_OPERATOR
protected:
SIMD_FORCE_INLINE int allocSize(int size)
@@ -75,11 +75,11 @@ protected:
{
int i;
for (i=start;i<end;++i)
#ifdef BT_USE_PLACEMENT_NEW
#ifdef B3_USE_PLACEMENT_NEW
new (&dest[i]) T(m_data[i]);
#else
dest[i] = m_data[i];
#endif //BT_USE_PLACEMENT_NEW
#endif //B3_USE_PLACEMENT_NEW
}
SIMD_FORCE_INLINE void init()
@@ -153,29 +153,29 @@ protected:
SIMD_FORCE_INLINE const T& at(int n) const
{
btAssert(n>=0);
btAssert(n<size());
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE T& at(int n)
{
btAssert(n>=0);
btAssert(n<size());
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE const T& operator[](int n) const
{
btAssert(n>=0);
btAssert(n<size());
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE T& operator[](int n)
{
btAssert(n>=0);
btAssert(n<size());
b3Assert(n>=0);
b3Assert(n<size());
return m_data[n];
}
@@ -192,7 +192,7 @@ protected:
SIMD_FORCE_INLINE void pop_back()
{
btAssert(m_size>0);
b3Assert(m_size>0);
m_size--;
m_data[m_size].~T();
}
@@ -233,12 +233,12 @@ protected:
{
reserve(newsize);
}
#ifdef BT_USE_PLACEMENT_NEW
#ifdef B3_USE_PLACEMENT_NEW
for (int i=curSize;i<newsize;i++)
{
new ( &m_data[i]) T(fillData);
}
#endif //BT_USE_PLACEMENT_NEW
#endif //B3_USE_PLACEMENT_NEW
}
@@ -265,7 +265,7 @@ protected:
reserve( allocSize(size()) );
}
m_size++;
#ifdef BT_USE_PLACEMENT_NEW
#ifdef B3_USE_PLACEMENT_NEW
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
#endif
@@ -281,11 +281,11 @@ protected:
reserve( allocSize(size()) );
}
#ifdef BT_USE_PLACEMENT_NEW
#ifdef B3_USE_PLACEMENT_NEW
new ( &m_data[m_size] ) T(_Val);
#else
m_data[size()] = _Val;
#endif //BT_USE_PLACEMENT_NEW
#endif //B3_USE_PLACEMENT_NEW
m_size++;
}
@@ -302,7 +302,7 @@ protected:
if (capacity() < _Count)
{ // not enough room, reallocate
T* s = (T*)allocate(_Count);
btAssert(s);
b3Assert(s);
copy(0, size(), s);
@@ -407,7 +407,7 @@ protected:
void swap(int index0,int index1)
{
#ifdef BT_USE_MEMCPY
#ifdef B3_USE_MEMCPY
char temp[sizeof(T)];
memcpy(temp,&m_data[index0],sizeof(T));
memcpy(&m_data[index0],&m_data[index1],sizeof(T));
@@ -416,7 +416,7 @@ protected:
T temp = m_data[index0];
m_data[index0] = m_data[index1];
m_data[index1] = temp;
#endif //BT_USE_PLACEMENT_NEW
#endif //B3_USE_PLACEMENT_NEW
}
@@ -509,4 +509,4 @@ protected:
};
#endif //BT_OBJECT_ARRAY__
#endif //B3_OBJECT_ARRAY__

74
src/Bullet3Common/b3HashMap.h
View File

@@ -14,13 +14,13 @@ subject to the following restrictions:
*/
#ifndef BT_HASH_MAP_H
#define BT_HASH_MAP_H
#ifndef B3_HASH_MAP_H
#define B3_HASH_MAP_H
#include "b3AlignedObjectArray.h"
///very basic hashable string implementation, compatible with b3HashMap
struct btHashString
struct b3HashString
{
const char* m_string;
unsigned int m_hash;
@@ -30,7 +30,7 @@ struct btHashString
return m_hash;
}
btHashString(const char* name)
b3HashString(const char* name)
:m_string(name)
{
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
@@ -63,7 +63,7 @@ struct btHashString
return( ret );
}
bool equals(const btHashString& other) const
bool equals(const b3HashString& other) const
{
return (m_string == other.m_string) ||
(0==portableStringCompare(m_string,other.m_string));
@@ -72,14 +72,14 @@ struct btHashString
};
const int BT_HASH_NULL=0xffffffff;
const int B3_HASH_NULL=0xffffffff;
class btHashInt
class b3HashInt
{
int m_uid;
public:
btHashInt(int uid) :m_uid(uid)
b3HashInt(int uid) :m_uid(uid)
{
}
@@ -93,7 +93,7 @@ public:
m_uid = uid;
}
bool equals(const btHashInt& other) const
bool equals(const b3HashInt& other) const
{
return getUid1() == other.getUid1();
}
@@ -109,7 +109,7 @@ public:
class btHashPtr
class b3HashPtr
{
union
@@ -120,7 +120,7 @@ class btHashPtr
public:
btHashPtr(const void* ptr)
b3HashPtr(const void* ptr)
:m_pointer(ptr)
{
}
@@ -130,7 +130,7 @@ public:
return m_pointer;
}
bool equals(const btHashPtr& other) const
bool equals(const b3HashPtr& other) const
{
return getPointer() == other.getPointer();
}
@@ -152,12 +152,12 @@ public:
template <class Value>
class btHashKeyPtr
class b3HashKeyPtr
{
int m_uid;
public:
btHashKeyPtr(int uid) :m_uid(uid)
b3HashKeyPtr(int uid) :m_uid(uid)
{
}
@@ -166,7 +166,7 @@ public:
return m_uid;
}
bool equals(const btHashKeyPtr<Value>& other) const
bool equals(const b3HashKeyPtr<Value>& other) const
{
return getUid1() == other.getUid1();
}
@@ -185,12 +185,12 @@ public:
template <class Value>
class btHashKey
class b3HashKey
{
int m_uid;
public:
btHashKey(int uid) :m_uid(uid)
b3HashKey(int uid) :m_uid(uid)
{
}
@@ -199,7 +199,7 @@ public:
return m_uid;
}
bool equals(const btHashKey<Value>& other) const
bool equals(const b3HashKey<Value>& other) const
{
return getUid1() == other.getUid1();
}
@@ -243,11 +243,11 @@ protected:
for (i= 0; i < newCapacity; ++i)
{
m_hashTable[i] = BT_HASH_NULL;
m_hashTable[i] = B3_HASH_NULL;
}
for (i = 0; i < newCapacity; ++i)
{
m_next[i] = BT_HASH_NULL;
m_next[i] = B3_HASH_NULL;
}
for(i=0;i<curHashtableSize;i++)
@@ -271,7 +271,7 @@ protected:
//replace value if the key is already there
int index = findIndex(key);
if (index != BT_HASH_NULL)
if (index != B3_HASH_NULL)
{
m_valueArray[index]=value;
return;
@@ -299,25 +299,25 @@ protected:
int pairIndex = findIndex(key);
if (pairIndex ==BT_HASH_NULL)
if (pairIndex ==B3_HASH_NULL)
{
return;
}
// Remove the pair from the hash table.
int index = m_hashTable[hash];
btAssert(index != BT_HASH_NULL);
b3Assert(index != B3_HASH_NULL);
int previous = BT_HASH_NULL;
int previous = B3_HASH_NULL;
while (index != pairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_HASH_NULL)
if (previous != B3_HASH_NULL)
{
btAssert(m_next[previous] == pairIndex);
b3Assert(m_next[previous] == pairIndex);
m_next[previous] = m_next[pairIndex];
}
else
@@ -343,18 +343,18 @@ protected:
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
index = m_hashTable[lastHash];
btAssert(index != BT_HASH_NULL);
b3Assert(index != B3_HASH_NULL);
previous = BT_HASH_NULL;
previous = B3_HASH_NULL;
while (index != lastPairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_HASH_NULL)
if (previous != B3_HASH_NULL)
{
btAssert(m_next[previous] == lastPairIndex);
b3Assert(m_next[previous] == lastPairIndex);
m_next[previous] = m_next[lastPairIndex];
}
else
@@ -383,14 +383,14 @@ protected:
const Value* getAtIndex(int index) const
{
btAssert(index < m_valueArray.size());
b3Assert(index < m_valueArray.size());
return &m_valueArray[index];
}
Value* getAtIndex(int index)
{
btAssert(index < m_valueArray.size());
b3Assert(index < m_valueArray.size());
return &m_valueArray[index];
}
@@ -402,7 +402,7 @@ protected:
const Value* find(const Key& key) const
{
int index = findIndex(key);
if (index == BT_HASH_NULL)
if (index == B3_HASH_NULL)
{
return NULL;
}
@@ -412,7 +412,7 @@ protected:
Value* find(const Key& key)
{
int index = findIndex(key);
if (index == BT_HASH_NULL)
if (index == B3_HASH_NULL)
{
return NULL;
}
@@ -426,11 +426,11 @@ protected:
if (hash >= (unsigned int)m_hashTable.size())
{
return BT_HASH_NULL;
return B3_HASH_NULL;
}
int index = m_hashTable[hash];
while ((index != BT_HASH_NULL) && key.equals(m_keyArray[index]) == false)
while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
{
index = m_next[index];
}
@@ -447,4 +447,4 @@ protected:
};
#endif //BT_HASH_MAP_H
#endif //B3_HASH_MAP_H

8
src/Bullet3Common/btInt2.h → src/Bullet3Common/b3Int2.h
View File

@@ -1,7 +1,7 @@
#ifndef BT_INT2_H
#define BT_INT2_H
#ifndef B3_INT2_H
#define B3_INT2_H
struct btUnsignedInt2
struct b3UnsignedInt2
{
union
{
@@ -16,7 +16,7 @@ struct btUnsignedInt2
};
};
struct btInt2
struct b3Int2
{
union
{

338
src/Bullet3Common/b3Matrix3x3.h
View File

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

18
src/Bullet3Common/b3MinMax.h
View File

@@ -14,31 +14,31 @@ subject to the following restrictions:
#ifndef BT_GEN_MINMAX_H
#define BT_GEN_MINMAX_H
#ifndef B3_GEN_MINMAX_H
#define B3_GEN_MINMAX_H
#include "b3Scalar.h"
template <class T>
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
SIMD_FORCE_INLINE const T& b3Min(const T& a, const T& b)
{
return a < b ? a : b ;
}
template <class T>
SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
SIMD_FORCE_INLINE const T& b3Max(const T& a, const T& b)
{
return a > b ? a : b;
}
template <class T>
SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
SIMD_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
{
return a < lb ? lb : (ub < a ? ub : a);
}
template <class T>
SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
SIMD_FORCE_INLINE void b3SetMin(T& a, const T& b)
{
if (b < a)
{
@@ -47,7 +47,7 @@ SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
}
template <class T>
SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
SIMD_FORCE_INLINE void b3SetMax(T& a, const T& b)
{
if (a < b)
{
@@ -56,7 +56,7 @@ SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
}
template <class T>
SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
SIMD_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
{
if (a < lb)
{
@@ -68,4 +68,4 @@ SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
}
}
#endif //BT_GEN_MINMAX_H
#endif //B3_GEN_MINMAX_H

10
src/Bullet3Common/b3PoolAllocator.h
View File

@@ -34,7 +34,7 @@ public:
:m_elemSize(elemSize),
m_maxElements(maxElements)
{
m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
m_pool = (unsigned char*) b3AlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
unsigned char* p = m_pool;
m_firstFree = p;
@@ -49,7 +49,7 @@ public:
~b3PoolAllocator()
{
btAlignedFree( m_pool);
b3AlignedFree( m_pool);
}
int getFreeCount() const
@@ -71,8 +71,8 @@ public:
{
// release mode fix
(void)size;
btAssert(!size || size<=m_elemSize);
btAssert(m_freeCount>0);
b3Assert(!size || size<=m_elemSize);
b3Assert(m_freeCount>0);
void* result = m_firstFree;
m_firstFree = *(void**)m_firstFree;
--m_freeCount;
@@ -93,7 +93,7 @@ public:
void freeMemory(void* ptr)
{
if (ptr) {
btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
*(void**)ptr = m_firstFree;
m_firstFree = ptr;

46
src/Bullet3Common/b3QuadWord.h
View File

@@ -13,8 +13,8 @@ subject to the following restrictions:
*/
#ifndef BT_SIMD_QUADWORD_H
#define BT_SIMD_QUADWORD_H
#ifndef B3_SIMD_QUADWORD_H
#define B3_SIMD_QUADWORD_H
#include "b3Scalar.h"
#include "b3MinMax.h"
@@ -51,33 +51,33 @@ public:
#else //__CELLOS_LV2__ __SPU__
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
union {
btSimdFloat4 mVec128;
b3SimdFloat4 mVec128;
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
};
public:
SIMD_FORCE_INLINE btSimdFloat4 get128() const
SIMD_FORCE_INLINE b3SimdFloat4 get128() const
{
return mVec128;
}
SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
SIMD_FORCE_INLINE void set128(b3SimdFloat4 v128)
{
mVec128 = v128;
}
#else
b3Scalar m_floats[4];
#endif // BT_USE_SSE
#endif // B3_USE_SSE
#endif //__CELLOS_LV2__ __SPU__
public:
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE b3QuadWord(const btSimdFloat4 vec)
SIMD_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
{
mVec128 = vec;
}
@@ -124,7 +124,7 @@ public:
SIMD_FORCE_INLINE bool operator==(const b3QuadWord& other) const
{
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3]==other.m_floats[3]) &&
@@ -204,15 +204,15 @@ public:
*/
SIMD_FORCE_INLINE void setMax(const b3QuadWord& other)
{
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.m_floats[3]);
b3SetMax(m_floats[0], other.m_floats[0]);
b3SetMax(m_floats[1], other.m_floats[1]);
b3SetMax(m_floats[2], other.m_floats[2]);
b3SetMax(m_floats[3], other.m_floats[3]);
#endif
}
/**@brief Set each element to the min of the current values and the values of another b3QuadWord
@@ -220,15 +220,15 @@ public:
*/
SIMD_FORCE_INLINE void setMin(const b3QuadWord& other)
{
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.m_floats[3]);
b3SetMin(m_floats[0], other.m_floats[0]);
b3SetMin(m_floats[1], other.m_floats[1]);
b3SetMin(m_floats[2], other.m_floats[2]);
b3SetMin(m_floats[3], other.m_floats[3]);
#endif
}
@@ -236,4 +236,4 @@ public:
};
#endif //BT_SIMD_QUADWORD_H
#endif //B3_SIMD_QUADWORD_H

228
src/Bullet3Common/b3Quaternion.h
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
#ifndef BT_SIMD__QUATERNION_H_
#define BT_SIMD__QUATERNION_H_
#ifndef B3_SIMD__QUATERNION_H_
#define B3_SIMD__QUATERNION_H_
#include "b3Vector3.h"
@@ -25,16 +25,16 @@ subject to the following restrictions:
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
#endif
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
const b3SimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
const b3SimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
#endif
@@ -44,9 +44,9 @@ public:
/**@brief No initialization constructor */
b3Quaternion() {}
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON)
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))|| defined(B3_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE b3Quaternion(const btSimdFloat4 vec)
SIMD_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec)
{
mVec128 = vec;
}
@@ -74,7 +74,7 @@ public:
b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
: b3QuadWord(_x, _y, _z, _w)
{
btAssert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
}
/**@brief Axis angle Constructor
* @param axis The axis which the rotation is around
@@ -84,12 +84,12 @@ public:
setRotation(_axis, _angle);
}
/**@brief Constructor from Euler angles
* @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
* @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
* @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
* @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z
* @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y
* @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */
b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
{
#ifndef BT_EULER_DEFAULT_ZYX
#ifndef B3_EULER_DEFAULT_ZYX
setEuler(yaw, pitch, roll);
#else
setEulerZYX(yaw, pitch, roll);
@@ -101,10 +101,10 @@ public:
void setRotation(const b3Vector3& axis, const b3Scalar& _angle)
{
b3Scalar d = axis.length();
btAssert(d != b3Scalar(0.0));
b3Scalar s = btSin(_angle * b3Scalar(0.5)) / d;
b3Assert(d != b3Scalar(0.0));
b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
btCos(_angle * b3Scalar(0.5)));
b3Cos(_angle * b3Scalar(0.5)));
}
/**@brief Set the quaternion using Euler angles
* @param yaw Angle around Y
@@ -115,12 +115,12 @@ public:
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
b3Scalar cosYaw = btCos(halfYaw);
b3Scalar sinYaw = btSin(halfYaw);
b3Scalar cosPitch = btCos(halfPitch);
b3Scalar sinPitch = btSin(halfPitch);
b3Scalar cosRoll = btCos(halfRoll);
b3Scalar sinRoll = btSin(halfRoll);
b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch);
b3Scalar sinPitch = b3Sin(halfPitch);
b3Scalar cosRoll = b3Cos(halfRoll);
b3Scalar sinRoll = b3Sin(halfRoll);
setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
@@ -135,12 +135,12 @@ public:
b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
b3Scalar cosYaw = btCos(halfYaw);
b3Scalar sinYaw = btSin(halfYaw);
b3Scalar cosPitch = btCos(halfPitch);
b3Scalar sinPitch = btSin(halfPitch);
b3Scalar cosRoll = btCos(halfRoll);
b3Scalar sinRoll = btSin(halfRoll);
b3Scalar cosYaw = b3Cos(halfYaw);
b3Scalar sinYaw = b3Sin(halfYaw);
b3Scalar cosPitch = b3Cos(halfPitch);
b3Scalar sinPitch = b3Sin(halfPitch);
b3Scalar cosRoll = b3Cos(halfRoll);
b3Scalar sinRoll = b3Sin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
@@ -150,9 +150,9 @@ public:
* @param q The quaternion to add to this one */
SIMD_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_add_ps(mVec128, q.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vaddq_f32(mVec128, q.mVec128);
#else
m_floats[0] += q.getX();
@@ -167,9 +167,9 @@ public:
* @param q The quaternion to subtract from this one */
b3Quaternion& operator-=(const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, q.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vsubq_f32(mVec128, q.mVec128);
#else
m_floats[0] -= q.getX();
@@ -184,11 +184,11 @@ public:
* @param s The scalar to scale by */
b3Quaternion& operator*=(const b3Scalar& s)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0); // (S S S S)
vs = b3_pshufd_ps(vs, 0); // (S S S S)
mVec128 = _mm_mul_ps(mVec128, vs);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s);
#else
m_floats[0] *= s;
@@ -204,25 +204,25 @@ public:
* Equivilant to this = this * q */
b3Quaternion& operator*=(const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vQ2 = q.get128();
__m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0));
__m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));
__m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0,1,2,0));
__m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0));
A1 = A1 * B1;
__m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1));
__m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
__m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1,2,0,1));
__m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1));
A2 = A2 * B2;
B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2));
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2,0,1,2));
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2));
B1 = B1 * B2; // A3 *= B3
mVec128 = bt_splat_ps(mVec128, 3); // A0
mVec128 = b3_splat_ps(mVec128, 3); // A0
mVec128 = mVec128 * vQ2; // A0 * B0
A1 = A1 + A2; // AB12
@@ -230,7 +230,7 @@ public:
A1 = _mm_xor_ps(A1, vPPPM); // change sign of the last element
mVec128 = mVec128+ A1; // AB03 + AB12
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = mVec128;
float32x4_t vQ2 = q.get128();
@@ -270,7 +270,7 @@ public:
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
mVec128 = A0;
@@ -287,7 +287,7 @@ public:
* @param q The other quaternion */
b3Scalar dot(const b3Quaternion& q) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vd;
vd = _mm_mul_ps(mVec128, q.mVec128);
@@ -298,7 +298,7 @@ public:
vd = _mm_add_ss(vd, t);
return _mm_cvtss_f32(vd);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
x = vpadd_f32(x, x);
@@ -320,14 +320,14 @@ public:
/**@brief Return the length of the quaternion */
b3Scalar length() const
{
return btSqrt(length2());
return b3Sqrt(length2());
}
/**@brief Normalize the quaternion
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
b3Quaternion& normalize()
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vd;
vd = _mm_mul_ps(mVec128, mVec128);
@@ -339,7 +339,7 @@ public:
vd = _mm_sqrt_ss(vd);
vd = _mm_div_ss(vOnes, vd);
vd = bt_pshufd_ps(vd, 0); // splat
vd = b3_pshufd_ps(vd, 0); // splat
mVec128 = _mm_mul_ps(mVec128, vd);
return *this;
@@ -353,12 +353,12 @@ public:
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Scalar& s) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
return b3Quaternion(_mm_mul_ps(mVec128, vs));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
return b3Quaternion(vmulq_n_f32(mVec128, s));
#else
return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
@@ -369,7 +369,7 @@ public:
* @param s The inverse scale factor */
b3Quaternion operator/(const b3Scalar& s) const
{
btAssert(s != b3Scalar(0.0));
b3Assert(s != b3Scalar(0.0));
return *this * (b3Scalar(1.0) / s);
}
@@ -377,7 +377,7 @@ public:
* @param s The scale factor */
b3Quaternion& operator/=(const b3Scalar& s)
{
btAssert(s != b3Scalar(0.0));
b3Assert(s != b3Scalar(0.0));
return *this *= b3Scalar(1.0) / s;
}
@@ -390,14 +390,14 @@ public:
* @param q The other quaternion */
b3Scalar angle(const b3Quaternion& q) const
{
b3Scalar s = btSqrt(length2() * q.length2());
btAssert(s != b3Scalar(0.0));
return btAcos(dot(q) / s);
b3Scalar s = b3Sqrt(length2() * q.length2());
b3Assert(s != b3Scalar(0.0));
return b3Acos(dot(q) / s);
}
/**@brief Return the angle of rotation represented by this quaternion */
b3Scalar getAngle() const
{
b3Scalar s = b3Scalar(2.) * btAcos(m_floats[3]);
b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]);
return s;
}
@@ -408,17 +408,17 @@ public:
if (s_squared < b3Scalar(10.) * SIMD_EPSILON) //Check for divide by zero
return b3Vector3(1.0, 0.0, 0.0); // Arbitrary
b3Scalar s = 1.f/btSqrt(s_squared);
b3Scalar s = 1.f/b3Sqrt(s_squared);
return b3Vector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
}
/**@brief Return the inverse of this quaternion */
b3Quaternion inverse() const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Quaternion(_mm_xor_ps(mVec128, vQInv));
#elif defined(BT_USE_NEON)
return b3Quaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
#elif defined(B3_USE_NEON)
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
#else
return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
#endif
@@ -429,9 +429,9 @@ public:
SIMD_FORCE_INLINE b3Quaternion
operator+(const b3Quaternion& q2) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
#else
const b3Quaternion& q1 = *this;
@@ -444,9 +444,9 @@ public:
SIMD_FORCE_INLINE b3Quaternion
operator-(const b3Quaternion& q2) const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
#else
const b3Quaternion& q1 = *this;
@@ -458,10 +458,10 @@ public:
* This simply negates each element */
SIMD_FORCE_INLINE b3Quaternion operator-() const
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return b3Quaternion(_mm_xor_ps(mVec128, btvMzeroMask));
#elif defined(BT_USE_NEON)
return b3Quaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask));
#elif defined(B3_USE_NEON)
return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask) );
#else
const b3Quaternion& q2 = *this;
return b3Quaternion( - q2.getX(), - q2.getY(), - q2.getZ(), - q2.m_floats[3]);
@@ -496,19 +496,19 @@ public:
* Slerp interpolates assuming constant velocity. */
b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
{
b3Scalar magnitude = btSqrt(length2() * q.length2());
btAssert(magnitude > b3Scalar(0));
b3Scalar magnitude = b3Sqrt(length2() * q.length2());
b3Assert(magnitude > b3Scalar(0));
b3Scalar product = dot(q) / magnitude;
if (btFabs(product) < b3Scalar(1))
if (b3Fabs(product) < b3Scalar(1))
{
// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
const b3Scalar theta = btAcos(sign * product);
const b3Scalar s1 = btSin(sign * t * theta);
const b3Scalar d = b3Scalar(1.0) / btSin(theta);
const b3Scalar s0 = btSin((b3Scalar(1.0) - t) * theta);
const b3Scalar theta = b3Acos(sign * product);
const b3Scalar s1 = b3Sin(sign * t * theta);
const b3Scalar d = b3Scalar(1.0) / b3Sin(theta);
const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta);
return b3Quaternion(
(m_floats[0] * s0 + q.getX() * s1) * d,
@@ -541,27 +541,27 @@ public:
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q1, const b3Quaternion& q2)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vQ1 = q1.get128();
__m128 vQ2 = q2.get128();
__m128 A0, A1, B1, A2, B2;
A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x // vtrn
B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X // vdup vext
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0,1,2,0)); // X Y z x // vtrn
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); // W W W X // vdup vext
A1 = A1 * B1;
A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z X Y // vext
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1)); // Y Z X Y // vext
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1)); // z x Y Y // vtrn vdup
A2 = A2 * B2;
B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn
B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2)); // z x Y Z // vtrn vext
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2)); // Y Z x z // vext vtrn
B1 = B1 * B2; // A3 *= B3
A0 = bt_splat_ps(vQ1, 3); // A0
A0 = b3_splat_ps(vQ1, 3); // A0
A0 = A0 * vQ2; // A0 * B0
A1 = A1 + A2; // AB12
@@ -572,7 +572,7 @@ operator*(const b3Quaternion& q1, const b3Quaternion& q2)
return b3Quaternion(A0);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = q1.get128();
float32x4_t vQ2 = q2.get128();
@@ -612,7 +612,7 @@ operator*(const b3Quaternion& q1, const b3Quaternion& q2)
A0 = vsubq_f32(A0, A3); // AB03 = AB0 - AB3
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A0 = vaddq_f32(A0, A1); // AB03 + AB12
return b3Quaternion(A0);
@@ -629,23 +629,23 @@ operator*(const b3Quaternion& q1, const b3Quaternion& q2)
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Quaternion& q, const b3Vector3& w)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vQ1 = q.get128();
__m128 vQ2 = w.get128();
__m128 A1, B1, A2, B2, A3, B3;
A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0));
B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0));
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3,3,3,0));
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0,1,2,0));
A1 = A1 * B1;
A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1));
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1));
A2 = A2 * B2;
A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2));
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2));
A3 = A3 * B3; // A3 *= B3
@@ -655,7 +655,7 @@ operator*(const b3Quaternion& q, const b3Vector3& w)
return b3Quaternion(A1);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = q.get128();
float32x4_t vQ2 = w.get128();
@@ -694,7 +694,7 @@ operator*(const b3Quaternion& q, const b3Vector3& w)
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
@@ -712,23 +712,23 @@ operator*(const b3Quaternion& q, const b3Vector3& w)
SIMD_FORCE_INLINE b3Quaternion
operator*(const b3Vector3& w, const b3Quaternion& q)
{
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vQ1 = w.get128();
__m128 vQ2 = q.get128();
__m128 A1, B1, A2, B2, A3, B3;
A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y z x
B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W W X
A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0,1,2,0)); // X Y z x
B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3,3,3,0)); // W W W X
A1 = A1 * B1;
A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1,2,0,1));
B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2,0,1,1));
A2 = A2 *B2;
A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2,0,1,2));
B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1,2,0,2));
A3 = A3 * B3; // A3 *= B3
@@ -738,7 +738,7 @@ operator*(const b3Vector3& w, const b3Quaternion& q)
return b3Quaternion(A1);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vQ1 = w.get128();
float32x4_t vQ2 = q.get128();
@@ -777,7 +777,7 @@ operator*(const b3Vector3& w, const b3Quaternion& q)
A1 = vaddq_f32(A1, A2); // AB12 = AB1 + AB2
// change the sign of the last element
A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);
A1 = vsubq_f32(A1, A3); // AB123 = AB12 - AB3
@@ -809,7 +809,7 @@ length(const b3Quaternion& q)
/**@brief Return the angle between two quaternions*/
SIMD_FORCE_INLINE b3Scalar
btAngle(const b3Quaternion& q1, const b3Quaternion& q2)
b3Angle(const b3Quaternion& q1, const b3Quaternion& q2)
{
return q1.angle(q2);
}
@@ -837,10 +837,10 @@ quatRotate(const b3Quaternion& rotation, const b3Vector3& v)
{
b3Quaternion q = rotation * v;
q *= rotation.inverse();
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return b3Vector3(_mm_and_ps(q.get128(), btvFFF0fMask));
#elif defined(BT_USE_NEON)
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
#if defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Vector3(_mm_and_ps(q.get128(), b3vFFF0fMask));
#elif defined(B3_USE_NEON)
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask));
#else
return b3Vector3(q.getX(),q.getY(),q.getZ());
#endif
@@ -855,11 +855,11 @@ shortestArcQuat(const b3Vector3& v0, const b3Vector3& v1) // Game Programming Ge
if (d < -1.0 + SIMD_EPSILON)
{
b3Vector3 n,unused;
btPlaneSpace1(v0,n,unused);
b3PlaneSpace1(v0,n,unused);
return b3Quaternion(n.getX(),n.getY(),n.getZ(),0.0f); // just pick any vector that is orthogonal to v0
}
b3Scalar s = btSqrt((1.0f + d) * 2.0f);
b3Scalar s = b3Sqrt((1.0f + d) * 2.0f);
b3Scalar rs = 1.0f / s;
return b3Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
@@ -873,7 +873,7 @@ shortestArcQuatNormalize2(b3Vector3& v0,b3Vector3& v1)
return shortestArcQuat(v0,v1);
}
#endif //BT_SIMD__QUATERNION_H_
#endif //B3_SIMD__QUATERNION_H_

42
src/Bullet3Common/b3Quickprof.cpp
View File

@@ -15,10 +15,10 @@
#include "b3Quickprof.h"
#ifndef BT_NO_PROFILE
#ifndef B3_NO_PROFILE
static btClock gProfileClock;
static b3Clock gProfileClock;
#ifdef __CELLOS_LV2__
@@ -33,7 +33,7 @@ static btClock gProfileClock;
#if defined(WIN32) || defined(_WIN32)
#define BT_USE_WINDOWS_TIMERS
#define B3_USE_WINDOWS_TIMERS
#define WIN32_LEAN_AND_MEAN
#define NOWINRES
#define NOMCX
@@ -54,10 +54,10 @@ static btClock gProfileClock;
#define mymin(a,b) (a > b ? a : b)
struct btClockData
struct b3ClockData
{
#ifdef BT_USE_WINDOWS_TIMERS
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER mClockFrequency;
DWORD mStartTick;
LONGLONG mPrevElapsedTime;
@@ -72,28 +72,28 @@ struct btClockData
};
///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
btClock::btClock()
///The b3Clock is a portable basic clock that measures accurate time in seconds, use for profiling.
b3Clock::b3Clock()
{
m_data = new btClockData;
#ifdef BT_USE_WINDOWS_TIMERS
m_data = new b3ClockData;
#ifdef B3_USE_WINDOWS_TIMERS
QueryPerformanceFrequency(&m_data->mClockFrequency);
#endif
reset();
}
btClock::~btClock()
b3Clock::~b3Clock()
{
delete m_data;
}
btClock::btClock(const btClock& other)
b3Clock::b3Clock(const b3Clock& other)
{
m_data = new btClockData;
m_data = new b3ClockData;
*m_data = *other.m_data;
}
btClock& btClock::operator=(const btClock& other)
b3Clock& b3Clock::operator=(const b3Clock& other)
{
*m_data = *other.m_data;
return *this;
@@ -101,9 +101,9 @@ btClock& btClock::operator=(const btClock& other)
/// Resets the initial reference time.
void btClock::reset()
void b3Clock::reset()
{
#ifdef BT_USE_WINDOWS_TIMERS
#ifdef B3_USE_WINDOWS_TIMERS
QueryPerformanceCounter(&m_data->mStartTime);
m_data->mStartTick = GetTickCount();
m_data->mPrevElapsedTime = 0;
@@ -122,10 +122,10 @@ void btClock::reset()
}
/// Returns the time in ms since the last call to reset or since
/// the btClock was created.
unsigned long int btClock::getTimeMilliseconds()
/// the b3Clock was created.
unsigned long int b3Clock::getTimeMilliseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER currentTime;
QueryPerformanceCounter(&currentTime);
LONGLONG elapsedTime = currentTime.QuadPart -
@@ -179,9 +179,9 @@ unsigned long int btClock::getTimeMilliseconds()
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
unsigned long int btClock::getTimeMicroseconds()
unsigned long int b3Clock::getTimeMicroseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
#ifdef B3_USE_WINDOWS_TIMERS
LARGE_INTEGER currentTime;
QueryPerformanceCounter(&currentTime);
LONGLONG elapsedTime = currentTime.QuadPart -
@@ -563,4 +563,4 @@ void CProfileManager::dumpAll()
#endif //BT_NO_PROFILE
#endif //B3_NO_PROFILE

34
src/Bullet3Common/b3Quickprof.h
View File

@@ -12,12 +12,12 @@
#ifndef BT_QUICK_PROF_H
#define BT_QUICK_PROF_H
#ifndef B3_QUICK_PROF_H
#define B3_QUICK_PROF_H
//To disable built-in profiling, please comment out next line
//#define BT_NO_PROFILE 1
#ifndef BT_NO_PROFILE
//#define B3_NO_PROFILE 1
#ifndef B3_NO_PROFILE
#include <stdio.h>//@todo remove this, backwards compatibility
#include "b3Scalar.h"
#include "b3AlignedAllocator.h"
@@ -31,29 +31,29 @@
#ifdef USE_BT_CLOCK
///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
class btClock
///The b3Clock is a portable basic clock that measures accurate time in seconds, use for profiling.
class b3Clock
{
public:
btClock();
b3Clock();
btClock(const btClock& other);
btClock& operator=(const btClock& other);
b3Clock(const b3Clock& other);
b3Clock& operator=(const b3Clock& other);
~btClock();
~b3Clock();
/// Resets the initial reference time.
void reset();
/// Returns the time in ms since the last call to reset or since
/// the btClock was created.
/// the b3Clock was created.
unsigned long int getTimeMilliseconds();
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
unsigned long int getTimeMicroseconds();
private:
struct btClockData* m_data;
struct b3ClockData* m_data;
};
#endif //USE_BT_CLOCK
@@ -173,7 +173,7 @@ private:
///ProfileSampleClass is a simple way to profile a function's scope
///Use the BT_PROFILE macro at the start of scope to time
///Use the B3_PROFILE macro at the start of scope to time
class CProfileSample {
public:
CProfileSample( const char * name )
@@ -188,16 +188,16 @@ public:
};
#define BT_PROFILE( name ) CProfileSample __profile( name )
#define B3_PROFILE( name ) CProfileSample __profile( name )
#else
#define BT_PROFILE( name )
#define B3_PROFILE( name )
#endif //#ifndef BT_NO_PROFILE
#endif //#ifndef B3_NO_PROFILE
#endif //BT_QUICK_PROF_H
#endif //B3_QUICK_PROF_H

6
src/Bullet3Common/b3Random.h
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
#ifndef BT_GEN_RANDOM_H
#define BT_GEN_RANDOM_H
#ifndef B3_GEN_RANDOM_H
#define B3_GEN_RANDOM_H
#ifdef MT19937
@@ -38,5 +38,5 @@ SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); }
#endif
#endif //BT_GEN_RANDOM_H
#endif //B3_GEN_RANDOM_H

340
src/Bullet3Common/b3Scalar.h
View File

@@ -14,10 +14,10 @@ subject to the following restrictions:
#ifndef BT_SCALAR_H
#define BT_SCALAR_H
#ifndef B3_SCALAR_H
#define B3_SCALAR_H
#ifdef BT_MANAGED_CODE
#ifdef B3_MANAGED_CODE
//Aligned data types not supported in managed code
#pragma unmanaged
#endif
@@ -28,15 +28,15 @@ subject to the following restrictions:
#include <float.h>
/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/
#define BT_BULLET_VERSION 281
#define B3_BULLET_VERSION 281
inline int btGetVersion()
inline int b3GetVersion()
{
return BT_BULLET_VERSION;
return B3_BULLET_VERSION;
}
#if defined(DEBUG) || defined (_DEBUG)
#define BT_DEBUG
#define B3_DEBUG
#endif
@@ -49,7 +49,7 @@ inline int btGetVersion()
#define ATTRIBUTE_ALIGNED64(a) a
#define ATTRIBUTE_ALIGNED128(a) a
#else
//#define BT_HAS_ALIGNED_ALLOCATOR
//#define B3_HAS_ALIGNED_ALLOCATOR
#pragma warning(disable : 4324) // disable padding warning
// #pragma warning(disable:4530) // Disable the exception disable but used in MSCV Stl warning.
// #pragma warning(disable:4996) //Turn off warnings about deprecated C routines
@@ -60,24 +60,24 @@ inline int btGetVersion()
#define ATTRIBUTE_ALIGNED64(a) __declspec(align(64)) a
#define ATTRIBUTE_ALIGNED128(a) __declspec (align(128)) a
#ifdef _XBOX
#define BT_USE_VMX128
#define B3_USE_VMX128
#include <ppcintrinsics.h>
#define BT_HAVE_NATIVE_FSEL
#define btFsel(a,b,c) __fsel((a),(b),(c))
#define B3_HAVE_NATIVE_FSEL
#define b3Fsel(a,b,c) __fsel((a),(b),(c))
#else
#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
#define BT_USE_SSE
#ifdef BT_USE_SSE
//BT_USE_SSE_IN_API is disabled under Windows by default, because
#if (defined (_WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (B3_USE_DOUBLE_PRECISION))
#define B3_USE_SSE
#ifdef B3_USE_SSE
//B3_USE_SSE_IN_API is disabled under Windows by default, because
//it makes it harder to integrate Bullet into your application under Windows
//(structured embedding Bullet structs/classes need to be 16-byte aligned)
//with relatively little performance gain
//If you are not embedded Bullet data in your classes, or make sure that you align those classes on 16-byte boundaries
//you can manually enable this line or set it in the build system for a bit of performance gain (a few percent, dependent on usage)
//#define BT_USE_SSE_IN_API
#endif //BT_USE_SSE
//#define B3_USE_SSE_IN_API
#endif //B3_USE_SSE
#include <emmintrin.h>
#endif
@@ -85,22 +85,22 @@ inline int btGetVersion()
#endif //__MINGW32__
#ifdef BT_DEBUG
#ifdef B3_DEBUG
#ifdef _MSC_VER
#include <stdio.h>
#define btAssert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }}
#define b3Assert(x) { if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);__debugbreak(); }}
#else//_MSC_VER
#include <assert.h>
#define btAssert assert
#define b3Assert assert
#endif//_MSC_VER
#else
#define btAssert(x)
#define b3Assert(x)
#endif
//btFullAssert is optional, slows down a lot
#define btFullAssert(x)
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define btLikely(_c) _c
#define btUnlikely(_c) _c
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
@@ -112,23 +112,23 @@ inline int btGetVersion()
#ifndef assert
#include <assert.h>
#endif
#ifdef BT_DEBUG
#ifdef B3_DEBUG
#ifdef __SPU__
#include <spu_printf.h>
#define printf spu_printf
#define btAssert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
#define b3Assert(x) {if(!(x)){printf("Assert "__FILE__ ":%u ("#x")\n", __LINE__);spu_hcmpeq(0,0);}}
#else
#define btAssert assert
#define b3Assert assert
#endif
#else
#define btAssert(x)
#define b3Assert(x)
#endif
//btFullAssert is optional, slows down a lot
#define btFullAssert(x)
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define btLikely(_c) _c
#define btUnlikely(_c) _c
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
@@ -141,29 +141,29 @@ inline int btGetVersion()
#ifndef assert
#include <assert.h>
#endif
#ifdef BT_DEBUG
#define btAssert assert
#ifdef B3_DEBUG
#define b3Assert assert
#else
#define btAssert(x)
#define b3Assert(x)
#endif
//btFullAssert is optional, slows down a lot
#define btFullAssert(x)
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define btLikely(_c) __builtin_expect((_c), 1)
#define btUnlikely(_c) __builtin_expect((_c), 0)
#define b3Likely(_c) __builtin_expect((_c), 1)
#define b3Unlikely(_c) __builtin_expect((_c), 0)
#else
//non-windows systems
#if (defined (__APPLE__) && (!defined (BT_USE_DOUBLE_PRECISION)))
#if (defined (__APPLE__) && (!defined (B3_USE_DOUBLE_PRECISION)))
#if defined (__i386__) || defined (__x86_64__)
#define BT_USE_SSE
//BT_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
#define B3_USE_SSE
//B3_USE_SSE_IN_API is enabled on Mac OSX by default, because memory is automatically aligned on 16-byte boundaries
//if apps run into issues, we will disable the next line
#define BT_USE_SSE_IN_API
#ifdef BT_USE_SSE
#define B3_USE_SSE_IN_API
#ifdef B3_USE_SSE
// include appropriate SSE level
#if defined (__SSE4_1__)
#include <smmintrin.h>
@@ -174,14 +174,14 @@ inline int btGetVersion()
#else
#include <emmintrin.h>
#endif
#endif //BT_USE_SSE
#endif //B3_USE_SSE
#elif defined( __armv7__ )
#ifdef __clang__
#define BT_USE_NEON 1
#define B3_USE_NEON 1
#if defined BT_USE_NEON && defined (__clang__)
#if defined B3_USE_NEON && defined (__clang__)
#include <arm_neon.h>
#endif//BT_USE_NEON
#endif//B3_USE_NEON
#endif //__clang__
#endif//__arm__
@@ -197,7 +197,7 @@ inline int btGetVersion()
#if defined(DEBUG) || defined (_DEBUG)
#if defined (__i386__) || defined (__x86_64__)
#include <stdio.h>
#define btAssert(x)\
#define b3Assert(x)\
{\
if(!(x))\
{\
@@ -206,16 +206,16 @@ inline int btGetVersion()
}\
}
#else//defined (__i386__) || defined (__x86_64__)
#define btAssert assert
#define b3Assert assert
#endif//defined (__i386__) || defined (__x86_64__)
#else//defined(DEBUG) || defined (_DEBUG)
#define btAssert(x)
#define b3Assert(x)
#endif//defined(DEBUG) || defined (_DEBUG)
//btFullAssert is optional, slows down a lot
#define btFullAssert(x)
#define btLikely(_c) _c
#define btUnlikely(_c) _c
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#else
@@ -232,15 +232,15 @@ inline int btGetVersion()
#endif
#if defined(DEBUG) || defined (_DEBUG)
#define btAssert assert
#define b3Assert assert
#else
#define btAssert(x)
#define b3Assert(x)
#endif
//btFullAssert is optional, slows down a lot
#define btFullAssert(x)
#define btLikely(_c) _c
#define btUnlikely(_c) _c
//b3FullAssert is optional, slows down a lot
#define b3FullAssert(x)
#define b3Likely(_c) _c
#define b3Unlikely(_c) _c
#endif //__APPLE__
#endif // LIBSPE2
@@ -250,31 +250,31 @@ inline int btGetVersion()
///The b3Scalar type abstracts floating point numbers, to easily switch between double and single floating point precision.
#if defined(BT_USE_DOUBLE_PRECISION)
#if defined(B3_USE_DOUBLE_PRECISION)
typedef double b3Scalar;
//this number could be bigger in double precision
#define BT_LARGE_FLOAT 1e30
#define B3_LARGE_FLOAT 1e30
#else
typedef float b3Scalar;
//keep BT_LARGE_FLOAT*BT_LARGE_FLOAT < FLT_MAX
#define BT_LARGE_FLOAT 1e18f
//keep B3_LARGE_FLOAT*B3_LARGE_FLOAT < FLT_MAX
#define B3_LARGE_FLOAT 1e18f
#endif
#ifdef BT_USE_SSE
typedef __m128 btSimdFloat4;
#endif//BT_USE_SSE
#ifdef B3_USE_SSE
typedef __m128 b3SimdFloat4;
#endif//B3_USE_SSE
#if defined BT_USE_SSE_IN_API && defined (BT_USE_SSE)
#if defined B3_USE_SSE_IN_API && defined (B3_USE_SSE)
#ifdef _WIN32
#ifndef BT_NAN
static int btNanMask = 0x7F800001;
#define BT_NAN (*(float*)&btNanMask)
#ifndef B3_NAN
static int b3NanMask = 0x7F800001;
#define B3_NAN (*(float*)&b3NanMask)
#endif
#ifndef BT_INFINITY
static int btInfinityMask = 0x7F800000;
#define BT_INFINITY (*(float*)&btInfinityMask)
#ifndef B3_INFINITY
static int b3InfinityMask = 0x7F800000;
#define B3_INFINITY (*(float*)&b3InfinityMask)
#endif
inline __m128 operator + (const __m128 A, const __m128 B)
@@ -292,70 +292,70 @@ inline __m128 operator * (const __m128 A, const __m128 B)
return _mm_mul_ps(A, B);
}
#define btCastfTo128i(a) (_mm_castps_si128(a))
#define btCastfTo128d(a) (_mm_castps_pd(a))
#define btCastiTo128f(a) (_mm_castsi128_ps(a))
#define btCastdTo128f(a) (_mm_castpd_ps(a))
#define btCastdTo128i(a) (_mm_castpd_si128(a))
#define btAssign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
#define b3CastfTo128i(a) (_mm_castps_si128(a))
#define b3CastfTo128d(a) (_mm_castps_pd(a))
#define b3CastiTo128f(a) (_mm_castsi128_ps(a))
#define b3CastdTo128f(a) (_mm_castpd_ps(a))
#define b3CastdTo128i(a) (_mm_castpd_si128(a))
#define b3Assign128(r0,r1,r2,r3) _mm_setr_ps(r0,r1,r2,r3)
#else//_WIN32
#define btCastfTo128i(a) ((__m128i)(a))
#define btCastfTo128d(a) ((__m128d)(a))
#define btCastiTo128f(a) ((__m128) (a))
#define btCastdTo128f(a) ((__m128) (a))
#define btCastdTo128i(a) ((__m128i)(a))
#define btAssign128(r0,r1,r2,r3) (__m128){r0,r1,r2,r3}
#define BT_INFINITY INFINITY
#define BT_NAN NAN
#define b3CastfTo128i(a) ((__m128i)(a))
#define b3CastfTo128d(a) ((__m128d)(a))
#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 B3_INFINITY INFINITY
#define B3_NAN NAN
#endif//_WIN32
#endif //BT_USE_SSE_IN_API
#endif //B3_USE_SSE_IN_API
#ifdef BT_USE_NEON
#ifdef B3_USE_NEON
#include <arm_neon.h>
typedef float32x4_t btSimdFloat4;
#define BT_INFINITY INFINITY
#define BT_NAN NAN
#define btAssign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
typedef float32x4_t b3SimdFloat4;
#define B3_INFINITY INFINITY
#define B3_NAN NAN
#define b3Assign128(r0,r1,r2,r3) (float32x4_t){r0,r1,r2,r3}
#endif
#define BT_DECLARE_ALIGNED_ALLOCATOR() \
SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
SIMD_FORCE_INLINE void operator delete(void* ptr) { btAlignedFree(ptr); } \
#define B3_DECLARE_ALIGNED_ALLOCATOR() \
SIMD_FORCE_INLINE void* operator new(size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes,16); } \
SIMD_FORCE_INLINE void operator delete(void* ptr) { b3AlignedFree(ptr); } \
SIMD_FORCE_INLINE void* operator new(size_t, void* ptr) { return ptr; } \
SIMD_FORCE_INLINE void operator delete(void*, void*) { } \
SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return btAlignedAlloc(sizeInBytes,16); } \
SIMD_FORCE_INLINE void operator delete[](void* ptr) { btAlignedFree(ptr); } \
SIMD_FORCE_INLINE void* operator new[](size_t sizeInBytes) { return b3AlignedAlloc(sizeInBytes,16); } \
SIMD_FORCE_INLINE void operator delete[](void* ptr) { b3AlignedFree(ptr); } \
SIMD_FORCE_INLINE void* operator new[](size_t, void* ptr) { return ptr; } \
SIMD_FORCE_INLINE void operator delete[](void*, void*) { } \
#if defined(BT_USE_DOUBLE_PRECISION) || defined(BT_FORCE_DOUBLE_FUNCTIONS)
#if defined(B3_USE_DOUBLE_PRECISION) || defined(B3_FORCE_DOUBLE_FUNCTIONS)
SIMD_FORCE_INLINE b3Scalar btSqrt(b3Scalar x) { return sqrt(x); }
SIMD_FORCE_INLINE b3Scalar btFabs(b3Scalar x) { return fabs(x); }
SIMD_FORCE_INLINE b3Scalar btCos(b3Scalar x) { return cos(x); }
SIMD_FORCE_INLINE b3Scalar btSin(b3Scalar x) { return sin(x); }
SIMD_FORCE_INLINE b3Scalar btTan(b3Scalar x) { return tan(x); }
SIMD_FORCE_INLINE b3Scalar btAcos(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return acos(x); }
SIMD_FORCE_INLINE b3Scalar btAsin(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return asin(x); }
SIMD_FORCE_INLINE b3Scalar btAtan(b3Scalar x) { return atan(x); }
SIMD_FORCE_INLINE b3Scalar btAtan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
SIMD_FORCE_INLINE b3Scalar btExp(b3Scalar x) { return exp(x); }
SIMD_FORCE_INLINE b3Scalar btLog(b3Scalar x) { return log(x); }
SIMD_FORCE_INLINE b3Scalar btPow(b3Scalar x,b3Scalar y) { return pow(x,y); }
SIMD_FORCE_INLINE b3Scalar btFmod(b3Scalar x,b3Scalar y) { return fmod(x,y); }
SIMD_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar x) { return sqrt(x); }
SIMD_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabs(x); }
SIMD_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cos(x); }
SIMD_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sin(x); }
SIMD_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tan(x); }
SIMD_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return acos(x); }
SIMD_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) { if (x<b3Scalar(-1)) x=b3Scalar(-1); if (x>b3Scalar(1)) x=b3Scalar(1); return asin(x); }
SIMD_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atan(x); }
SIMD_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2(x, y); }
SIMD_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return exp(x); }
SIMD_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return log(x); }
SIMD_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return pow(x,y); }
SIMD_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmod(x,y); }
#else
SIMD_FORCE_INLINE b3Scalar btSqrt(b3Scalar y)
SIMD_FORCE_INLINE b3Scalar b3Sqrt(b3Scalar y)
{
#ifdef USE_APPROXIMATION
double x, z, tempf;
@@ -375,30 +375,30 @@ SIMD_FORCE_INLINE b3Scalar btSqrt(b3Scalar y)
return sqrtf(y);
#endif
}
SIMD_FORCE_INLINE b3Scalar btFabs(b3Scalar x) { return fabsf(x); }
SIMD_FORCE_INLINE b3Scalar btCos(b3Scalar x) { return cosf(x); }
SIMD_FORCE_INLINE b3Scalar btSin(b3Scalar x) { return sinf(x); }
SIMD_FORCE_INLINE b3Scalar btTan(b3Scalar x) { return tanf(x); }
SIMD_FORCE_INLINE b3Scalar btAcos(b3Scalar x) {
SIMD_FORCE_INLINE b3Scalar b3Fabs(b3Scalar x) { return fabsf(x); }
SIMD_FORCE_INLINE b3Scalar b3Cos(b3Scalar x) { return cosf(x); }
SIMD_FORCE_INLINE b3Scalar b3Sin(b3Scalar x) { return sinf(x); }
SIMD_FORCE_INLINE b3Scalar b3Tan(b3Scalar x) { return tanf(x); }
SIMD_FORCE_INLINE b3Scalar b3Acos(b3Scalar x) {
if (x<b3Scalar(-1))
x=b3Scalar(-1);
if (x>b3Scalar(1))
x=b3Scalar(1);
return acosf(x);
}
SIMD_FORCE_INLINE b3Scalar btAsin(b3Scalar x) {
SIMD_FORCE_INLINE b3Scalar b3Asin(b3Scalar x) {
if (x<b3Scalar(-1))
x=b3Scalar(-1);
if (x>b3Scalar(1))
x=b3Scalar(1);
return asinf(x);
}
SIMD_FORCE_INLINE b3Scalar btAtan(b3Scalar x) { return atanf(x); }
SIMD_FORCE_INLINE b3Scalar btAtan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
SIMD_FORCE_INLINE b3Scalar btExp(b3Scalar x) { return expf(x); }
SIMD_FORCE_INLINE b3Scalar btLog(b3Scalar x) { return logf(x); }
SIMD_FORCE_INLINE b3Scalar btPow(b3Scalar x,b3Scalar y) { return powf(x,y); }
SIMD_FORCE_INLINE b3Scalar btFmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
SIMD_FORCE_INLINE b3Scalar b3Atan(b3Scalar x) { return atanf(x); }
SIMD_FORCE_INLINE b3Scalar b3Atan2(b3Scalar x, b3Scalar y) { return atan2f(x, y); }
SIMD_FORCE_INLINE b3Scalar b3Exp(b3Scalar x) { return expf(x); }
SIMD_FORCE_INLINE b3Scalar b3Log(b3Scalar x) { return logf(x); }
SIMD_FORCE_INLINE b3Scalar b3Pow(b3Scalar x,b3Scalar y) { return powf(x,y); }
SIMD_FORCE_INLINE b3Scalar b3Fmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
#endif
@@ -409,10 +409,10 @@ SIMD_FORCE_INLINE b3Scalar btFmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
#define SIMD_DEGS_PER_RAD (b3Scalar(360.0) / SIMD_2_PI)
#define SIMDSQRT12 b3Scalar(0.7071067811865475244008443621048490)
#define btRecipSqrt(x) ((b3Scalar)(b3Scalar(1.0)/btSqrt(b3Scalar(x)))) /* reciprocal square root */
#define b3RecipSqrt(x) ((b3Scalar)(b3Scalar(1.0)/b3Sqrt(b3Scalar(x)))) /* reciprocal square root */
#ifdef BT_USE_DOUBLE_PRECISION
#ifdef B3_USE_DOUBLE_PRECISION
#define SIMD_EPSILON DBL_EPSILON
#define SIMD_INFINITY DBL_MAX
#else
@@ -420,11 +420,11 @@ SIMD_FORCE_INLINE b3Scalar btFmod(b3Scalar x,b3Scalar y) { return fmodf(x,y); }
#define SIMD_INFINITY FLT_MAX
#endif
SIMD_FORCE_INLINE b3Scalar btAtan2Fast(b3Scalar y, b3Scalar x)
SIMD_FORCE_INLINE b3Scalar b3Atan2Fast(b3Scalar y, b3Scalar x)
{
b3Scalar coeff_1 = SIMD_PI / 4.0f;
b3Scalar coeff_2 = 3.0f * coeff_1;
b3Scalar abs_y = btFabs(y);
b3Scalar abs_y = b3Fabs(y);
b3Scalar angle;
if (x >= 0.0f) {
b3Scalar r = (x - abs_y) / (x + abs_y);
@@ -436,35 +436,35 @@ SIMD_FORCE_INLINE b3Scalar btAtan2Fast(b3Scalar y, b3Scalar x)
return (y < 0.0f) ? -angle : angle;
}
SIMD_FORCE_INLINE bool btFuzzyZero(b3Scalar x) { return btFabs(x) < SIMD_EPSILON; }
SIMD_FORCE_INLINE bool b3FuzzyZero(b3Scalar x) { return b3Fabs(x) < SIMD_EPSILON; }
SIMD_FORCE_INLINE bool btEqual(b3Scalar a, b3Scalar eps) {
SIMD_FORCE_INLINE bool b3Equal(b3Scalar a, b3Scalar eps) {
return (((a) <= eps) && !((a) < -eps));
}
SIMD_FORCE_INLINE bool btGreaterEqual (b3Scalar a, b3Scalar eps) {
SIMD_FORCE_INLINE bool b3GreaterEqual (b3Scalar a, b3Scalar eps) {
return (!((a) <= eps));
}
SIMD_FORCE_INLINE int btIsNegative(b3Scalar x) {
SIMD_FORCE_INLINE int b3IsNegative(b3Scalar x) {
return x < b3Scalar(0.0) ? 1 : 0;
}
SIMD_FORCE_INLINE b3Scalar btRadians(b3Scalar x) { return x * SIMD_RADS_PER_DEG; }
SIMD_FORCE_INLINE b3Scalar btDegrees(b3Scalar x) { return x * SIMD_DEGS_PER_RAD; }
SIMD_FORCE_INLINE b3Scalar b3Radians(b3Scalar x) { return x * SIMD_RADS_PER_DEG; }
SIMD_FORCE_INLINE b3Scalar b3Degrees(b3Scalar x) { return x * SIMD_DEGS_PER_RAD; }
#define BT_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
#ifndef btFsel
SIMD_FORCE_INLINE b3Scalar btFsel(b3Scalar a, b3Scalar b, b3Scalar c)
#ifndef b3Fsel
SIMD_FORCE_INLINE b3Scalar b3Fsel(b3Scalar a, b3Scalar b, b3Scalar c)
{
return a >= 0 ? b : c;
}
#endif
#define btFsels(a,b,c) (b3Scalar)btFsel(a,b,c)
#define b3Fsels(a,b,c) (b3Scalar)b3Fsel(a,b,c)
SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
SIMD_FORCE_INLINE bool b3MachineIsLittleEndian()
{
long int i = 1;
const char *p = (const char *) &i;
@@ -476,9 +476,9 @@ SIMD_FORCE_INLINE bool btMachineIsLittleEndian()
///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360
///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
SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
SIMD_FORCE_INLINE unsigned b3Select(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero)
{
// Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero
// Rely on positive value or'ed with its negative having sign bit on
@@ -488,22 +488,22 @@ SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditio
unsigned testEqz = ~testNz;
return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
SIMD_FORCE_INLINE int b3Select(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero)
{
unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31);
unsigned testEqz = ~testNz;
return static_cast<int>((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz));
}
SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
SIMD_FORCE_INLINE float b3Select(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero)
{
#ifdef BT_HAVE_NATIVE_FSEL
return (float)btFsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#ifdef B3_HAVE_NATIVE_FSEL
return (float)b3Fsel((b3Scalar)condition - b3Scalar(1.0f), valueIfConditionNonZero, valueIfConditionZero);
#else
return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero;
#endif
}
template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
template<typename T> SIMD_FORCE_INLINE void b3Swap(T& a, T& b)
{
T tmp = a;
a = b;
@@ -512,33 +512,33 @@ template<typename T> SIMD_FORCE_INLINE void btSwap(T& a, T& b)
//PCK: endian swapping functions
SIMD_FORCE_INLINE unsigned btSwapEndian(unsigned val)
SIMD_FORCE_INLINE unsigned b3SwapEndian(unsigned val)
{
return (((val & 0xff000000) >> 24) | ((val & 0x00ff0000) >> 8) | ((val & 0x0000ff00) << 8) | ((val & 0x000000ff) << 24));
}
SIMD_FORCE_INLINE unsigned short btSwapEndian(unsigned short val)
SIMD_FORCE_INLINE unsigned short b3SwapEndian(unsigned short val)
{
return static_cast<unsigned short>(((val & 0xff00) >> 8) | ((val & 0x00ff) << 8));
}
SIMD_FORCE_INLINE unsigned btSwapEndian(int val)
SIMD_FORCE_INLINE unsigned b3SwapEndian(int val)
{
return btSwapEndian((unsigned)val);
return b3SwapEndian((unsigned)val);
}
SIMD_FORCE_INLINE unsigned short btSwapEndian(short val)
SIMD_FORCE_INLINE unsigned short b3SwapEndian(short val)
{
return btSwapEndian((unsigned short) val);
return b3SwapEndian((unsigned short) val);
}
///btSwapFloat uses using char pointers to swap the endianness
////btSwapFloat/btSwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
///b3SwapFloat uses using char pointers to swap the endianness
////b3SwapFloat/b3SwapDouble will NOT return a float, because the machine might 'correct' invalid floating point values
///Not all values of sign/exponent/mantissa are valid floating point numbers according to IEEE 754.
///When a floating point unit is faced with an invalid value, it may actually change the value, or worse, throw an exception.
///In most systems, running user mode code, you wouldn't get an exception, but instead the hardware/os/runtime will 'fix' the number for you.
///so instead of returning a float/double, we return integer/long long integer
SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
SIMD_FORCE_INLINE unsigned int b3SwapEndianFloat(float d)
{
unsigned int a = 0;
unsigned char *dst = (unsigned char *)&a;
@@ -552,7 +552,7 @@ SIMD_FORCE_INLINE unsigned int btSwapEndianFloat(float d)
}
// unswap using char pointers
SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
SIMD_FORCE_INLINE float b3UnswapEndianFloat(unsigned int a)
{
float d = 0.0f;
unsigned char *src = (unsigned char *)&a;
@@ -568,7 +568,7 @@ SIMD_FORCE_INLINE float btUnswapEndianFloat(unsigned int a)
// swap using char pointers
SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
SIMD_FORCE_INLINE void b3SwapEndianDouble(double d, unsigned char* dst)
{
unsigned char *src = (unsigned char *)&d;
@@ -584,7 +584,7 @@ SIMD_FORCE_INLINE void btSwapEndianDouble(double d, unsigned char* dst)
}
// unswap using char pointers
SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
SIMD_FORCE_INLINE double b3UnswapEndianDouble(const unsigned char *src)
{
double d = 0.0;
unsigned char *dst = (unsigned char *)&d;
@@ -602,9 +602,9 @@ SIMD_FORCE_INLINE double btUnswapEndianDouble(const unsigned char *src)
}
// returns normalized value in range [-SIMD_PI, SIMD_PI]
SIMD_FORCE_INLINE b3Scalar btNormalizeAngle(b3Scalar angleInRadians)
SIMD_FORCE_INLINE b3Scalar b3NormalizeAngle(b3Scalar angleInRadians)
{
angleInRadians = btFmod(angleInRadians, SIMD_2_PI);
angleInRadians = b3Fmod(angleInRadians, SIMD_2_PI);
if(angleInRadians < -SIMD_PI)
{
return angleInRadians + SIMD_2_PI;
@@ -620,9 +620,9 @@ SIMD_FORCE_INLINE b3Scalar btNormalizeAngle(b3Scalar angleInRadians)
}
///rudimentary class to provide type info
struct btTypedObject
struct b3TypedObject
{
btTypedObject(int objectType)
b3TypedObject(int objectType)
:m_objectType(objectType)
{
}
@@ -636,10 +636,10 @@ struct btTypedObject
///align a pointer to the provided alignment, upwards
template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
template <typename T>T* b3AlignPointer(T* unalignedPtr, size_t alignment)
{
struct btConvertPointerSizeT
struct b3ConvertPointerSizeT
{
union
{
@@ -647,7 +647,7 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
size_t integer;
};
};
btConvertPointerSizeT converter;
b3ConvertPointerSizeT converter;
const size_t bit_mask = ~(alignment - 1);
@@ -657,4 +657,4 @@ template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment)
return converter.ptr;
}
#endif //BT_SCALAR_H
#endif //B3_SCALAR_H

34
src/Bullet3Common/b3StackAlloc.h
View File

@@ -17,16 +17,16 @@ StackAlloc extracted from GJK-EPA collision solver by Nathanael Presson
Nov.2006
*/
#ifndef BT_STACK_ALLOC
#define BT_STACK_ALLOC
#ifndef B3_STACK_ALLOC
#define B3_STACK_ALLOC
#include "b3Scalar.h" //for btAssert
#include "b3Scalar.h" //for b3Assert
#include "b3AlignedAllocator.h"
///The btBlock class is an internal structure for the b3StackAlloc memory allocator.
struct btBlock
///The b3Block class is an internal structure for the b3StackAlloc memory allocator.
struct b3Block
{
btBlock* previous;
b3Block* previous;
unsigned char* address;
};
@@ -41,18 +41,18 @@ public:
inline void create(unsigned int size)
{
destroy();
data = (unsigned char*) btAlignedAlloc(size,16);
data = (unsigned char*) b3AlignedAlloc(size,16);
totalsize = size;
}
inline void destroy()
{
btAssert(usedsize==0);
b3Assert(usedsize==0);
//Raise(L"StackAlloc is still in use");
if(usedsize==0)
{
if(!ischild && data)
btAlignedFree(data);
b3AlignedFree(data);
data = 0;
usedsize = 0;
@@ -73,27 +73,27 @@ public:
usedsize=nus;
return(data+(usedsize-size));
}
btAssert(0);
b3Assert(0);
//&& (L"Not enough memory"));
return(0);
}
SIMD_FORCE_INLINE btBlock* beginBlock()
SIMD_FORCE_INLINE b3Block* beginBlock()
{
btBlock* pb = (btBlock*)allocate(sizeof(btBlock));
b3Block* pb = (b3Block*)allocate(sizeof(b3Block));
pb->previous = current;
pb->address = data+usedsize;
current = pb;
return(pb);
}
SIMD_FORCE_INLINE void endBlock(btBlock* block)
SIMD_FORCE_INLINE void endBlock(b3Block* block)
{
btAssert(block==current);
b3Assert(block==current);
//Raise(L"Unmatched blocks");
if(block==current)
{
current = block->previous;
usedsize = (unsigned int)((block->address-data)-sizeof(btBlock));
usedsize = (unsigned int)((block->address-data)-sizeof(b3Block));
}
}
@@ -109,8 +109,8 @@ private:
unsigned char* data;
unsigned int totalsize;
unsigned int usedsize;
btBlock* current;
b3Block* current;
bool ischild;
};
#endif //BT_STACK_ALLOC
#endif //B3_STACK_ALLOC

46
src/Bullet3Common/b3Transform.h
View File

@@ -14,16 +14,16 @@ subject to the following restrictions:
#ifndef BT_TRANSFORM_H
#define BT_TRANSFORM_H
#ifndef B3_TRANSFORM_H
#define B3_TRANSFORM_H
#include "b3Matrix3x3.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btTransformData btTransformDoubleData
#ifdef B3_USE_DOUBLE_PRECISION
#define b3TransformData b3TransformDoubleData
#else
#define btTransformData btTransformFloatData
#define b3TransformData b3TransformFloatData
#endif
@@ -85,7 +85,7 @@ public:
/* void multInverseLeft(const b3Transform& t1, const b3Transform& t2) {
b3Vector3 v = t2.m_origin - t1.m_origin;
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
m_basis = b3MultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis;
}
*/
@@ -206,15 +206,15 @@ public:
return identityTransform;
}
void serialize(struct btTransformData& dataOut) const;
void serialize(struct b3TransformData& dataOut) const;
void serializeFloat(struct btTransformFloatData& dataOut) const;
void serializeFloat(struct b3TransformFloatData& dataOut) const;
void deSerialize(const struct btTransformData& dataIn);
void deSerialize(const struct b3TransformData& dataIn);
void deSerializeDouble(const struct btTransformDoubleData& dataIn);
void deSerializeDouble(const struct b3TransformDoubleData& dataIn);
void deSerializeFloat(const struct btTransformFloatData& dataIn);
void deSerializeFloat(const struct b3TransformFloatData& dataIn);
};
@@ -250,53 +250,53 @@ SIMD_FORCE_INLINE bool operator==(const b3Transform& t1, const b3Transform& t2)
///for serialization
struct btTransformFloatData
struct b3TransformFloatData
{
btMatrix3x3FloatData m_basis;
btVector3FloatData m_origin;
b3Matrix3x3FloatData m_basis;
b3Vector3FloatData m_origin;
};
struct btTransformDoubleData
struct b3TransformDoubleData
{
btMatrix3x3DoubleData m_basis;
btVector3DoubleData m_origin;
b3Matrix3x3DoubleData m_basis;
b3Vector3DoubleData m_origin;
};
SIMD_FORCE_INLINE void b3Transform::serialize(btTransformData& dataOut) const
SIMD_FORCE_INLINE void b3Transform::serialize(b3TransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
m_origin.serialize(dataOut.m_origin);
}
SIMD_FORCE_INLINE void b3Transform::serializeFloat(btTransformFloatData& dataOut) const
SIMD_FORCE_INLINE void b3Transform::serializeFloat(b3TransformFloatData& dataOut) const
{
m_basis.serializeFloat(dataOut.m_basis);
m_origin.serializeFloat(dataOut.m_origin);
}
SIMD_FORCE_INLINE void b3Transform::deSerialize(const btTransformData& dataIn)
SIMD_FORCE_INLINE void b3Transform::deSerialize(const b3TransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
m_origin.deSerialize(dataIn.m_origin);
}
SIMD_FORCE_INLINE void b3Transform::deSerializeFloat(const btTransformFloatData& dataIn)
SIMD_FORCE_INLINE void b3Transform::deSerializeFloat(const b3TransformFloatData& dataIn)
{
m_basis.deSerializeFloat(dataIn.m_basis);
m_origin.deSerializeFloat(dataIn.m_origin);
}
SIMD_FORCE_INLINE void b3Transform::deSerializeDouble(const btTransformDoubleData& dataIn)
SIMD_FORCE_INLINE void b3Transform::deSerializeDouble(const b3TransformDoubleData& dataIn)
{
m_basis.deSerializeDouble(dataIn.m_basis);
m_origin.deSerializeDouble(dataIn.m_origin);
}
#endif //BT_TRANSFORM_H
#endif //B3_TRANSFORM_H

22
src/Bullet3Common/b3TransformUtil.h
View File

@@ -13,8 +13,8 @@ subject to the following restrictions:
*/
#ifndef BT_TRANSFORM_UTIL_H
#define BT_TRANSFORM_UTIL_H
#ifndef B3_TRANSFORM_UTIL_H
#define B3_TRANSFORM_UTIL_H
#include "b3Transform.h"
#define ANGULAR_MOTION_THRESHOLD b3Scalar(0.5)*SIMD_HALF_PI
@@ -22,7 +22,7 @@ subject to the following restrictions:
SIMD_FORCE_INLINE b3Vector3 btAabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir)
SIMD_FORCE_INLINE b3Vector3 b3AabbSupport(const b3Vector3& halfExtents,const b3Vector3& supportDir)
{
return b3Vector3(supportDir.getX() < b3Scalar(0.0) ? -halfExtents.getX() : halfExtents.getX(),
supportDir.getY() < b3Scalar(0.0) ? -halfExtents.getY() : halfExtents.getY(),
@@ -68,9 +68,9 @@ public:
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( btSin(b3Scalar(0.5)*fAngle*timeStep)/fAngle );
axis = angvel*( b3Sin(b3Scalar(0.5)*fAngle*timeStep)/fAngle );
}
b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),btCos( fAngle*timeStep*b3Scalar(0.5) ));
b3Quaternion dorn (axis.getX(),axis.getY(),axis.getZ(),b3Cos( fAngle*timeStep*b3Scalar(0.5) ));
b3Quaternion orn0 = curTrans.getRotation();
b3Quaternion predictedOrn = dorn * orn0;
@@ -106,7 +106,7 @@ public:
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
else
axis /= btSqrt(len);
axis /= b3Sqrt(len);
}
static void calculateVelocity(const b3Transform& transform0,const b3Transform& transform1,b3Scalar timeStep,b3Vector3& linVel,b3Vector3& angVel)
@@ -135,15 +135,15 @@ public:
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = b3Vector3(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.));
else
axis /= btSqrt(len);
axis /= b3Sqrt(len);
}
};
///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
///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 btConvexSeparatingDistanceUtil
class b3ConvexSeparatingDistanceUtil
{
b3Quaternion m_ornA;
b3Quaternion m_ornB;
@@ -158,7 +158,7 @@ class btConvexSeparatingDistanceUtil
public:
btConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA,b3Scalar boundingRadiusB)
b3ConvexSeparatingDistanceUtil(b3Scalar boundingRadiusA,b3Scalar boundingRadiusB)
:m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
@@ -224,5 +224,5 @@ public:
};
#endif //BT_TRANSFORM_UTIL_H
#endif //B3_TRANSFORM_UTIL_H

32
src/Bullet3Common/b3Vector3.cpp
View File

@@ -16,12 +16,12 @@
*/
#if defined (_WIN32) || defined (__i386__)
#define BT_USE_SSE_IN_API
#define B3_USE_SSE_IN_API
#endif
#include "b3Vector3.h"
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
#ifdef __APPLE__
#include <stdint.h>
@@ -32,7 +32,7 @@ typedef float float4 __attribute__ ((vector_size(16)));
//typedef uint32_t uint4 __attribute__ ((vector_size(16)));
#if defined BT_USE_SSE || defined _WIN32
#if defined B3_USE_SSE || defined _WIN32
#define LOG2_ARRAY_SIZE 6
#define STACK_ARRAY_COUNT (1UL << LOG2_ARRAY_SIZE)
@@ -44,9 +44,9 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa
{
const float4 *vertices = (const float4*) vv;
static const unsigned char indexTable[16] = {-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
float4 dotMax = btAssign128( -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY );
float4 dotMax = b3Assign128( -B3_INFINITY, -B3_INFINITY, -B3_INFINITY, -B3_INFINITY );
float4 vvec = _mm_loadu_ps( vec );
float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa )); /// zzzz
float4 vHi = b3CastiTo128f(_mm_shuffle_epi32( b3CastfTo128i( vvec), 0xaa )); /// zzzz
float4 vLo = _mm_movelh_ps( vvec, vvec ); /// xyxy
long maxIndex = -1L;
@@ -180,7 +180,7 @@ long _maxdot_large( const float *vv, const float *vec, unsigned long count, floa
index = 0;
if( btUnlikely( count > 16) )
if( b3Unlikely( count > 16) )
{
for( ; index + 4 <= count / 4; index+=4 )
{ // do four dot products at a time. Carefully avoid touching the w element.
@@ -429,9 +429,9 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
{
const float4 *vertices = (const float4*) vv;
static const unsigned char indexTable[16] = {-1, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0 };
float4 dotmin = btAssign128( BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY );
float4 dotmin = b3Assign128( B3_INFINITY, B3_INFINITY, B3_INFINITY, B3_INFINITY );
float4 vvec = _mm_loadu_ps( vec );
float4 vHi = btCastiTo128f(_mm_shuffle_epi32( btCastfTo128i( vvec), 0xaa )); /// zzzz
float4 vHi = b3CastiTo128f(_mm_shuffle_epi32( b3CastfTo128i( vvec), 0xaa )); /// zzzz
float4 vLo = _mm_movelh_ps( vvec, vvec ); /// xyxy
long minIndex = -1L;
@@ -565,7 +565,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
index = 0;
if(btUnlikely( count > 16) )
if(b3Unlikely( count > 16) )
{
for( ; index + 4 <= count / 4; index+=4 )
{ // do four dot products at a time. Carefully avoid touching the w element.
@@ -812,7 +812,7 @@ long _mindot_large( const float *vv, const float *vec, unsigned long count, floa
}
#elif defined BT_USE_NEON
#elif defined B3_USE_NEON
#define ARM_NEON_GCC_COMPATIBILITY 1
#include <arm_neon.h>
@@ -860,8 +860,8 @@ long _maxdot_large_v0( const float *vv, const float *vec, unsigned long count, f
float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
float32x2_t vLo = vget_low_f32(vvec);
float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
float32x2_t dotMaxLo = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
float32x2_t dotMaxHi = (float32x2_t) { -BT_INFINITY, -BT_INFINITY };
float32x2_t dotMaxLo = (float32x2_t) { -B3_INFINITY, -B3_INFINITY };
float32x2_t dotMaxHi = (float32x2_t) { -B3_INFINITY, -B3_INFINITY };
uint32x2_t indexLo = (uint32x2_t) {0, 1};
uint32x2_t indexHi = (uint32x2_t) {2, 3};
uint32x2_t iLo = (uint32x2_t) {-1, -1};
@@ -1052,7 +1052,7 @@ long _maxdot_large_v1( const float *vv, const float *vec, unsigned long count, f
const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 };
float32x4_t maxDot = (float32x4_t) { -BT_INFINITY, -BT_INFINITY, -BT_INFINITY, -BT_INFINITY };
float32x4_t maxDot = (float32x4_t) { -B3_INFINITY, -B3_INFINITY, -B3_INFINITY, -B3_INFINITY };
unsigned long i = 0;
for( ; i + 8 <= count; i += 8 )
@@ -1245,8 +1245,8 @@ long _mindot_large_v0( const float *vv, const float *vec, unsigned long count, f
float32x4_t vvec = vld1q_f32_aligned_postincrement( vec );
float32x2_t vLo = vget_low_f32(vvec);
float32x2_t vHi = vdup_lane_f32(vget_high_f32(vvec), 0);
float32x2_t dotMinLo = (float32x2_t) { BT_INFINITY, BT_INFINITY };
float32x2_t dotMinHi = (float32x2_t) { BT_INFINITY, BT_INFINITY };
float32x2_t dotMinLo = (float32x2_t) { B3_INFINITY, B3_INFINITY };
float32x2_t dotMinHi = (float32x2_t) { B3_INFINITY, B3_INFINITY };
uint32x2_t indexLo = (uint32x2_t) {0, 1};
uint32x2_t indexHi = (uint32x2_t) {2, 3};
uint32x2_t iLo = (uint32x2_t) {-1, -1};
@@ -1435,7 +1435,7 @@ long _mindot_large_v1( const float *vv, const float *vec, unsigned long count, f
const uint32x4_t four = (uint32x4_t){ 4, 4, 4, 4 };
uint32x4_t local_index = (uint32x4_t) {0, 1, 2, 3};
uint32x4_t index = (uint32x4_t) { -1, -1, -1, -1 };
float32x4_t minDot = (float32x4_t) { BT_INFINITY, BT_INFINITY, BT_INFINITY, BT_INFINITY };
float32x4_t minDot = (float32x4_t) { B3_INFINITY, B3_INFINITY, B3_INFINITY, B3_INFINITY };
unsigned long i = 0;
for( ; i + 8 <= count; i += 8 )

402
src/Bullet3Common/b3Vector3.h
View File

@@ -14,23 +14,23 @@ subject to the following restrictions:
#ifndef BT_VECTOR3_H
#define BT_VECTOR3_H
#ifndef B3_VECTOR3_H
#define B3_VECTOR3_H
//#include <stdint.h>
#include "b3Scalar.h"
#include "b3MinMax.h"
#include "b3AlignedAllocator.h"
#ifdef BT_USE_DOUBLE_PRECISION
#define btVector3Data btVector3DoubleData
#define btVector3DataName "btVector3DoubleData"
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Vector3Data b3Vector3DoubleData
#define b3Vector3DataName "b3Vector3DoubleData"
#else
#define btVector3Data btVector3FloatData
#define btVector3DataName "btVector3FloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define b3Vector3Data b3Vector3FloatData
#define b3Vector3DataName "b3Vector3FloatData"
#endif //B3_USE_DOUBLE_PRECISION
#if defined BT_USE_SSE
#if defined B3_USE_SSE
//typedef uint32_t __m128i __attribute__ ((vector_size(16)));
@@ -39,35 +39,35 @@ subject to the following restrictions:
#endif
#define BT_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x))
//#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
#define bt_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) )
#define bt_splat3_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i, 3) )
#define bt_splat_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i,_i) )
#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) )
#define b3_splat3_ps( _a, _i ) b3_pshufd_ps((_a), B3_SHUFFLE(_i,_i,_i, 3) )
#define b3_splat_ps( _a, _i ) b3_pshufd_ps((_a), B3_SHUFFLE(_i,_i,_i,_i) )
#define btv3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
#define btvAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
#define btvFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
#define btv3AbsfMask btCastiTo128f(btv3AbsiMask)
#define btvFFF0fMask btCastiTo128f(btvFFF0Mask)
#define btvxyzMaskf btvFFF0fMask
#define btvAbsfMask btCastiTo128f(btvAbsMask)
#define b3v3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
#define b3vAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
#define b3vFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
#define b3v3AbsfMask b3CastiTo128f(b3v3AbsiMask)
#define b3vFFF0fMask b3CastiTo128f(b3vFFF0Mask)
#define b3vxyzMaskf b3vFFF0fMask
#define b3vAbsfMask b3CastiTo128f(b3vAbsMask)
const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
const __m128 ATTRIBUTE_ALIGNED16(b3vMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
const __m128 ATTRIBUTE_ALIGNED16(vHalf) = {0.5f, 0.5f, 0.5f, 0.5f};
const __m128 ATTRIBUTE_ALIGNED16(v1_5) = {1.5f, 1.5f, 1.5f, 1.5f};
#endif
#ifdef BT_USE_NEON
#ifdef B3_USE_NEON
const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
const float32x4_t ATTRIBUTE_ALIGNED16(b3vMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
const int32x4_t ATTRIBUTE_ALIGNED16(b3vFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
const int32x4_t ATTRIBUTE_ALIGNED16(b3vAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
const int32x4_t ATTRIBUTE_ALIGNED16(b3v3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
#endif
@@ -79,7 +79,7 @@ ATTRIBUTE_ALIGNED16(class) b3Vector3
{
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
#if defined (__SPU__) && defined (__CELLOS_LV2__)
b3Scalar m_floats[4];
@@ -90,18 +90,18 @@ public:
}
public:
#else //__CELLOS_LV2__ __SPU__
#if defined (BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM
#if defined (B3_USE_SSE) || defined(B3_USE_NEON) // _WIN32 || ARM
union {
btSimdFloat4 mVec128;
b3SimdFloat4 mVec128;
b3Scalar m_floats[4];
struct {b3Scalar x,y,z,w;};
};
SIMD_FORCE_INLINE btSimdFloat4 get128() const
SIMD_FORCE_INLINE b3SimdFloat4 get128() const
{
return mVec128;
}
SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
SIMD_FORCE_INLINE void set128(b3SimdFloat4 v128)
{
mVec128 = v128;
}
@@ -133,9 +133,9 @@ public:
m_floats[3] = b3Scalar(0.f);
}
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) )|| defined (BT_USE_NEON)
#if (defined (B3_USE_SSE_IN_API) && defined (B3_USE_SSE) )|| defined (B3_USE_NEON)
// Set Vector
SIMD_FORCE_INLINE b3Vector3( btSimdFloat4 v)
SIMD_FORCE_INLINE b3Vector3( b3SimdFloat4 v)
{
mVec128 = v;
}
@@ -154,15 +154,15 @@ public:
return *this;
}
#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
#endif // #if defined (B3_USE_SSE_IN_API) || defined (B3_USE_NEON)
/**@brief Add a vector to this one
* @param The vector to add to this one */
SIMD_FORCE_INLINE b3Vector3& operator+=(const b3Vector3& v)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_add_ps(mVec128, v.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vaddq_f32(mVec128, v.mVec128);
#else
m_floats[0] += v.m_floats[0];
@@ -177,9 +177,9 @@ public:
* @param The vector to subtract */
SIMD_FORCE_INLINE b3Vector3& operator-=(const b3Vector3& v)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_sub_ps(mVec128, v.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vsubq_f32(mVec128, v.mVec128);
#else
m_floats[0] -= v.m_floats[0];
@@ -193,11 +193,11 @@ public:
* @param s Scale factor */
SIMD_FORCE_INLINE b3Vector3& operator*=(const b3Scalar& s)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
vs = b3_pshufd_ps(vs, 0x80); // (S S S 0.0)
mVec128 = _mm_mul_ps(mVec128, vs);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vmulq_n_f32(mVec128, s);
#else
m_floats[0] *= s;
@@ -211,13 +211,13 @@ public:
* @param s Scale factor to divide by */
SIMD_FORCE_INLINE b3Vector3& operator/=(const b3Scalar& s)
{
btFullAssert(s != b3Scalar(0.0));
b3FullAssert(s != b3Scalar(0.0));
#if 0 //defined(BT_USE_SSE_IN_API)
#if 0 //defined(B3_USE_SSE_IN_API)
// this code is not faster !
__m128 vs = _mm_load_ss(&s);
vs = _mm_div_ss(v1110, vs);
vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
mVec128 = _mm_mul_ps(mVec128, vs);
@@ -231,14 +231,14 @@ public:
* @param v The other vector in the dot product */
SIMD_FORCE_INLINE b3Scalar dot(const b3Vector3& v) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vd = _mm_mul_ps(mVec128, v.mVec128);
__m128 z = _mm_movehl_ps(vd, vd);
__m128 y = _mm_shuffle_ps(vd, vd, 0x55);
vd = _mm_add_ss(vd, y);
vd = _mm_add_ss(vd, z);
return _mm_cvtss_f32(vd);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));
x = vadd_f32(x, vget_high_f32(vd));
@@ -259,7 +259,7 @@ public:
/**@brief Return the length of the vector */
SIMD_FORCE_INLINE b3Scalar length() const
{
return btSqrt(length2());
return b3Sqrt(length2());
}
/**@brief Return the distance squared between the ends of this and another vector
@@ -287,7 +287,7 @@ public:
* x^2 + y^2 + z^2 = 1 */
SIMD_FORCE_INLINE b3Vector3& normalize()
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
// dot product first
__m128 vd = _mm_mul_ps(mVec128, mVec128);
__m128 z = _mm_movehl_ps(vd, vd);
@@ -298,7 +298,7 @@ public:
#if 0
vd = _mm_sqrt_ss(vd);
vd = _mm_div_ss(v1110, vd);
vd = bt_splat_ps(vd, 0x80);
vd = b3_splat_ps(vd, 0x80);
mVec128 = _mm_mul_ps(mVec128, vd);
#else
@@ -315,7 +315,7 @@ public:
y = _mm_mul_ss(y, z); // y0 * (1.5 - vd * 0.5 * y0 * y0)
y = bt_splat_ps(y, 0x80);
y = b3_splat_ps(y, 0x80);
mVec128 = _mm_mul_ps(mVec128, y);
#endif
@@ -339,23 +339,23 @@ public:
* @param v The other vector */
SIMD_FORCE_INLINE b3Scalar angle(const b3Vector3& v) const
{
b3Scalar s = btSqrt(length2() * v.length2());
btFullAssert(s != b3Scalar(0.0));
return btAcos(dot(v) / s);
b3Scalar s = b3Sqrt(length2() * v.length2());
b3FullAssert(s != b3Scalar(0.0));
return b3Acos(dot(v) / s);
}
/**@brief Return a vector will the absolute values of each element */
SIMD_FORCE_INLINE b3Vector3 absolute() const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return b3Vector3(_mm_and_ps(mVec128, btv3AbsfMask));
#elif defined(BT_USE_NEON)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Vector3(_mm_and_ps(mVec128, b3v3AbsfMask));
#elif defined(B3_USE_NEON)
return b3Vector3(vabsq_f32(mVec128));
#else
return b3Vector3(
btFabs(m_floats[0]),
btFabs(m_floats[1]),
btFabs(m_floats[2]));
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]));
#endif
}
@@ -363,19 +363,19 @@ public:
* @param v The other vector */
SIMD_FORCE_INLINE b3Vector3 cross(const b3Vector3& v) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 T, V;
T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
T = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = b3_pshufd_ps(v.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = _mm_mul_ps(V, mVec128);
T = _mm_mul_ps(T, v.mVec128);
V = _mm_sub_ps(V, T);
V = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3));
V = b3_pshufd_ps(V, B3_SHUFFLE(1, 2, 0, 3));
return b3Vector3(V);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t T, V;
// form (Y, Z, X, _) of mVec128 and v.mVec128
float32x2_t Tlow = vget_low_f32(mVec128);
@@ -389,7 +389,7 @@ public:
Vlow = vget_low_f32(V);
// form (Y, Z, X, _);
V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask);
V = (float32x4_t)vandq_s32((int32x4_t)V, b3vFFF0Mask);
return b3Vector3(V);
#else
@@ -402,16 +402,16 @@ public:
SIMD_FORCE_INLINE b3Scalar triple(const b3Vector3& v1, const b3Vector3& v2) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
// cross:
__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
__m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
__m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, B3_SHUFFLE(1, 2, 0, 3)); // (Y Z X 0)
V = _mm_mul_ps(V, v1.mVec128);
T = _mm_mul_ps(T, v2.mVec128);
V = _mm_sub_ps(V, T);
V = _mm_shuffle_ps(V, V, BT_SHUFFLE(1, 2, 0, 3));
V = _mm_shuffle_ps(V, V, B3_SHUFFLE(1, 2, 0, 3));
// dot:
V = _mm_mul_ps(V, mVec128);
@@ -421,7 +421,7 @@ public:
V = _mm_add_ss(V, z);
return _mm_cvtss_f32(V);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
// cross:
float32x4_t T, V;
// form (Y, Z, X, _) of mVec128 and v.mVec128
@@ -477,17 +477,17 @@ public:
SIMD_FORCE_INLINE void setInterpolate3(const b3Vector3& v0, const b3Vector3& v1, b3Scalar rt)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vrt = _mm_load_ss(&rt); // (rt 0 0 0)
b3Scalar s = b3Scalar(1.0) - rt;
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
vs = b3_pshufd_ps(vs, 0x80); // (S S S 0.0)
__m128 r0 = _mm_mul_ps(v0.mVec128, vs);
vrt = bt_pshufd_ps(vrt, 0x80); // (rt rt rt 0.0)
vrt = b3_pshufd_ps(vrt, 0x80); // (rt rt rt 0.0)
__m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
__m128 tmp3 = _mm_add_ps(r0,r1);
mVec128 = tmp3;
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vsubq_f32(v1.mVec128, v0.mVec128);
mVec128 = vmulq_n_f32(mVec128, rt);
mVec128 = vaddq_f32(mVec128, v0.mVec128);
@@ -506,15 +506,15 @@ public:
* @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
SIMD_FORCE_INLINE b3Vector3 lerp(const b3Vector3& v, const b3Scalar& t) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vt = _mm_load_ss(&t); // (t 0 0 0)
vt = bt_pshufd_ps(vt, 0x80); // (rt rt rt 0.0)
vt = b3_pshufd_ps(vt, 0x80); // (rt rt rt 0.0)
__m128 vl = _mm_sub_ps(v.mVec128, mVec128);
vl = _mm_mul_ps(vl, vt);
vl = _mm_add_ps(vl, mVec128);
return b3Vector3(vl);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
vl = vmulq_n_f32(vl, t);
vl = vaddq_f32(vl, mVec128);
@@ -532,9 +532,9 @@ public:
* @param v The other vector */
SIMD_FORCE_INLINE b3Vector3& operator*=(const b3Vector3& v)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_mul_ps(mVec128, v.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vmulq_f32(mVec128, v.mVec128);
#else
m_floats[0] *= v.m_floats[0];
@@ -570,7 +570,7 @@ public:
SIMD_FORCE_INLINE bool operator==(const b3Vector3& other) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3]==other.m_floats[3]) &&
@@ -590,15 +590,15 @@ public:
*/
SIMD_FORCE_INLINE void setMax(const b3Vector3& other)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.m_floats[3]);
b3SetMax(m_floats[0], other.m_floats[0]);
b3SetMax(m_floats[1], other.m_floats[1]);
b3SetMax(m_floats[2], other.m_floats[2]);
b3SetMax(m_floats[3], other.m_floats[3]);
#endif
}
@@ -607,15 +607,15 @@ public:
*/
SIMD_FORCE_INLINE void setMin(const b3Vector3& other)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.m_floats[3]);
b3SetMin(m_floats[0], other.m_floats[0]);
b3SetMin(m_floats[1], other.m_floats[1]);
b3SetMin(m_floats[2], other.m_floats[2]);
b3SetMin(m_floats[3], other.m_floats[3]);
#endif
}
@@ -629,10 +629,10 @@ public:
void getSkewSymmetricMatrix(b3Vector3* v0,b3Vector3* v1,b3Vector3* v2) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 V = _mm_and_ps(mVec128, btvFFF0fMask);
__m128 V0 = _mm_xor_ps(btvMzeroMask, V);
__m128 V = _mm_and_ps(mVec128, b3vFFF0fMask);
__m128 V0 = _mm_xor_ps(b3vMzeroMask, V);
__m128 V2 = _mm_movelh_ps(V0, V);
__m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
@@ -652,9 +652,9 @@ public:
void setZero()
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
int32x4_t vi = vdupq_n_s32(0);
mVec128 = vreinterpretq_f32_s32(vi);
#else
@@ -672,17 +672,17 @@ public:
return length2() < SIMD_EPSILON;
}
SIMD_FORCE_INLINE void serialize(struct btVector3Data& dataOut) const;
SIMD_FORCE_INLINE void serialize(struct b3Vector3Data& dataOut) const;
SIMD_FORCE_INLINE void deSerialize(const struct btVector3Data& dataIn);
SIMD_FORCE_INLINE void deSerialize(const struct b3Vector3Data& dataIn);
SIMD_FORCE_INLINE void serializeFloat(struct btVector3FloatData& dataOut) const;
SIMD_FORCE_INLINE void serializeFloat(struct b3Vector3FloatData& dataOut) const;
SIMD_FORCE_INLINE void deSerializeFloat(const struct btVector3FloatData& dataIn);
SIMD_FORCE_INLINE void deSerializeFloat(const struct b3Vector3FloatData& dataIn);
SIMD_FORCE_INLINE void serializeDouble(struct btVector3DoubleData& dataOut) const;
SIMD_FORCE_INLINE void serializeDouble(struct b3Vector3DoubleData& dataOut) const;
SIMD_FORCE_INLINE void deSerializeDouble(const struct btVector3DoubleData& dataIn);
SIMD_FORCE_INLINE void deSerializeDouble(const struct b3Vector3DoubleData& dataIn);
/**@brief returns index of maximum dot product between this and vectors in array[]
* @param array The other vectors
@@ -699,7 +699,7 @@ public:
/* create a vector as b3Vector3( this->dot( b3Vector3 v0 ), this->dot( b3Vector3 v1), this->dot( b3Vector3 v2 )) */
SIMD_FORCE_INLINE b3Vector3 dot3( const b3Vector3 &v0, const b3Vector3 &v1, const b3Vector3 &v2 ) const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
__m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
@@ -709,11 +709,11 @@ public:
__m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
__m128 r = _mm_movelh_ps( b0, b2 );
r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
a2 = _mm_and_ps( a2, btvxyzMaskf);
r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
a2 = _mm_and_ps( a2, b3vxyzMaskf);
r = _mm_add_ps( r, b3CastdTo128f (_mm_move_sd( b3CastfTo128d(a2), b3CastfTo128d(b1) )));
return b3Vector3(r);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
@@ -733,9 +733,9 @@ public:
SIMD_FORCE_INLINE b3Vector3
operator+(const b3Vector3& v1, const b3Vector3& v2)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Vector3(_mm_add_ps(v1.mVec128, v2.mVec128));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
return b3Vector3(vaddq_f32(v1.mVec128, v2.mVec128));
#else
return b3Vector3(
@@ -749,9 +749,9 @@ operator+(const b3Vector3& v1, const b3Vector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v1, const b3Vector3& v2)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Vector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
return b3Vector3(vmulq_f32(v1.mVec128, v2.mVec128));
#else
return b3Vector3(
@@ -765,14 +765,14 @@ operator*(const b3Vector3& v1, const b3Vector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v1, const b3Vector3& v2)
{
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))
#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
// without _mm_and_ps this code causes slowdown in Concave moving
__m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
return b3Vector3(_mm_and_ps(r, btvFFF0fMask));
#elif defined(BT_USE_NEON)
return b3Vector3(_mm_and_ps(r, b3vFFF0fMask));
#elif defined(B3_USE_NEON)
float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
#else
return b3Vector3(
v1.m_floats[0] - v2.m_floats[0],
@@ -785,11 +785,11 @@ operator-(const b3Vector3& v1, const b3Vector3& v2)
SIMD_FORCE_INLINE b3Vector3
operator-(const b3Vector3& v)
{
#if (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
__m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
return b3Vector3(_mm_and_ps(r, btvFFF0fMask));
#elif defined(BT_USE_NEON)
return b3Vector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
#if (defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE))
__m128 r = _mm_xor_ps(v.mVec128, b3vMzeroMask);
return b3Vector3(_mm_and_ps(r, b3vFFF0fMask));
#elif defined(B3_USE_NEON)
return b3Vector3((b3SimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)b3vMzeroMask));
#else
return b3Vector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
#endif
@@ -799,13 +799,13 @@ operator-(const b3Vector3& v)
SIMD_FORCE_INLINE b3Vector3
operator*(const b3Vector3& v, const b3Scalar& s)
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 vs = _mm_load_ss(&s); // (S 0 0 0)
vs = bt_pshufd_ps(vs, 0x80); // (S S S 0.0)
vs = b3_pshufd_ps(vs, 0x80); // (S S S 0.0)
return b3Vector3(_mm_mul_ps(v.mVec128, vs));
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t r = vmulq_n_f32(v.mVec128, s);
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
return b3Vector3((float32x4_t)vandq_s32((int32x4_t)r, b3vFFF0Mask));
#else
return b3Vector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
#endif
@@ -822,12 +822,12 @@ operator*(const b3Scalar& s, const b3Vector3& v)
SIMD_FORCE_INLINE b3Vector3
operator/(const b3Vector3& v, const b3Scalar& s)
{
btFullAssert(s != b3Scalar(0.0));
#if 0 //defined(BT_USE_SSE_IN_API)
b3FullAssert(s != b3Scalar(0.0));
#if 0 //defined(B3_USE_SSE_IN_API)
// this code is not faster !
__m128 vs = _mm_load_ss(&s);
vs = _mm_div_ss(v1110, vs);
vs = bt_pshufd_ps(vs, 0x00); // (S S S S)
vs = b3_pshufd_ps(vs, 0x00); // (S S S S)
return b3Vector3(_mm_mul_ps(v.mVec128, vs));
#else
@@ -839,11 +839,11 @@ operator/(const b3Vector3& v, const b3Scalar& s)
SIMD_FORCE_INLINE b3Vector3
operator/(const b3Vector3& v1, const b3Vector3& v2)
{
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
#if (defined(B3_USE_SSE_IN_API)&& defined (B3_USE_SSE))
__m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
vec = _mm_and_ps(vec, btvFFF0fMask);
vec = _mm_and_ps(vec, b3vFFF0fMask);
return b3Vector3(vec);
#elif defined(BT_USE_NEON)
#elif defined(B3_USE_NEON)
float32x4_t x, y, v, m;
x = v1.mVec128;
@@ -867,7 +867,7 @@ operator/(const b3Vector3& v1, const b3Vector3& v2)
/**@brief Return the dot product between two vectors */
SIMD_FORCE_INLINE b3Scalar
btDot(const b3Vector3& v1, const b3Vector3& v2)
b3Dot(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.dot(v2);
}
@@ -875,7 +875,7 @@ btDot(const b3Vector3& v1, const b3Vector3& v2)
/**@brief Return the distance squared between two vectors */
SIMD_FORCE_INLINE b3Scalar
btDistance2(const b3Vector3& v1, const b3Vector3& v2)
b3Distance2(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance2(v2);
}
@@ -883,27 +883,27 @@ btDistance2(const b3Vector3& v1, const b3Vector3& v2)
/**@brief Return the distance between two vectors */
SIMD_FORCE_INLINE b3Scalar
btDistance(const b3Vector3& v1, const b3Vector3& v2)
b3Distance(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.distance(v2);
}
/**@brief Return the angle between two vectors */
SIMD_FORCE_INLINE b3Scalar
btAngle(const b3Vector3& v1, const b3Vector3& v2)
b3Angle(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.angle(v2);
}
/**@brief Return the cross product of two vectors */
SIMD_FORCE_INLINE b3Vector3
btCross(const b3Vector3& v1, const b3Vector3& v2)
b3Cross(const b3Vector3& v1, const b3Vector3& v2)
{
return v1.cross(v2);
}
SIMD_FORCE_INLINE b3Scalar
btTriple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
b3Triple(const b3Vector3& v1, const b3Vector3& v2, const b3Vector3& v3)
{
return v1.triple(v2, v3);
}
@@ -932,7 +932,7 @@ SIMD_FORCE_INLINE b3Scalar b3Vector3::distance(const b3Vector3& v) const
SIMD_FORCE_INLINE b3Vector3 b3Vector3::normalized() const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
b3Vector3 norm = *this;
return norm.normalize();
@@ -945,23 +945,23 @@ SIMD_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3S
{
// wAxis must be a unit lenght vector
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
__m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
b3Scalar ssin = btSin( _angle );
b3Scalar ssin = b3Sin( _angle );
__m128 C = wAxis.cross( mVec128 ).mVec128;
O = _mm_and_ps(O, btvFFF0fMask);
b3Scalar scos = btCos( _angle );
O = _mm_and_ps(O, b3vFFF0fMask);
b3Scalar scos = b3Cos( _angle );
__m128 vsin = _mm_load_ss(&ssin); // (S 0 0 0)
__m128 vcos = _mm_load_ss(&scos); // (S 0 0 0)
__m128 Y = bt_pshufd_ps(O, 0xC9); // (Y Z X 0)
__m128 Z = bt_pshufd_ps(O, 0xD2); // (Z X Y 0)
__m128 Y = b3_pshufd_ps(O, 0xC9); // (Y Z X 0)
__m128 Z = b3_pshufd_ps(O, 0xD2); // (Z X Y 0)
O = _mm_add_ps(O, Y);
vsin = bt_pshufd_ps(vsin, 0x80); // (S S S 0)
vsin = b3_pshufd_ps(vsin, 0x80); // (S S S 0)
O = _mm_add_ps(O, Z);
vcos = bt_pshufd_ps(vcos, 0x80); // (S S S 0)
vcos = b3_pshufd_ps(vcos, 0x80); // (S S S 0)
vsin = vsin * C;
O = O * wAxis.mVec128;
@@ -979,24 +979,24 @@ SIMD_FORCE_INLINE b3Vector3 b3Vector3::rotate( const b3Vector3& wAxis, const b3S
_y = wAxis.cross( *this );
return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) );
return ( o + _x * b3Cos( _angle ) + _y * b3Sin( _angle ) );
#endif
}
SIMD_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const
{
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined _WIN32 || defined (BT_USE_SSE)
#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
#if defined _WIN32 || defined (B3_USE_SSE)
const long scalar_cutoff = 10;
long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
#elif defined BT_USE_NEON
#elif defined B3_USE_NEON
const long scalar_cutoff = 4;
extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
#endif
if( array_count < scalar_cutoff )
#else
#endif//BT_USE_SSE || BT_USE_NEON
#endif//B3_USE_SSE || B3_USE_NEON
{
b3Scalar maxDot = -SIMD_INFINITY;
int i = 0;
@@ -1015,18 +1015,18 @@ SIMD_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long arra
dotOut = maxDot;
return ptIndex;
}
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
#endif
}
SIMD_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_count, b3Scalar &dotOut ) const
{
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined BT_USE_SSE
#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
#if defined B3_USE_SSE
const long scalar_cutoff = 10;
long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
#elif defined BT_USE_NEON
#elif defined B3_USE_NEON
const long scalar_cutoff = 4;
extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
#else
@@ -1034,7 +1034,7 @@ SIMD_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long arra
#endif
if( array_count < scalar_cutoff )
#endif//BT_USE_SSE || BT_USE_NEON
#endif//B3_USE_SSE || B3_USE_NEON
{
b3Scalar minDot = SIMD_INFINITY;
int i = 0;
@@ -1055,56 +1055,56 @@ SIMD_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long arra
return ptIndex;
}
#if defined (BT_USE_SSE) || defined (BT_USE_NEON)
#if defined (B3_USE_SSE) || defined (B3_USE_NEON)
return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
#endif
}
class btVector4 : public b3Vector3
class b3Vector4 : public b3Vector3
{
public:
SIMD_FORCE_INLINE btVector4() {}
SIMD_FORCE_INLINE b3Vector4() {}
SIMD_FORCE_INLINE btVector4(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
SIMD_FORCE_INLINE b3Vector4(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z,const b3Scalar& _w)
: b3Vector3(_x,_y,_z)
{
m_floats[3] = _w;
}
#if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON)
SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec)
#if (defined (B3_USE_SSE_IN_API)&& defined (B3_USE_SSE)) || defined (B3_USE_NEON)
SIMD_FORCE_INLINE b3Vector4(const b3SimdFloat4 vec)
{
mVec128 = vec;
}
SIMD_FORCE_INLINE btVector4(const b3Vector3& rhs)
SIMD_FORCE_INLINE b3Vector4(const b3Vector3& rhs)
{
mVec128 = rhs.mVec128;
}
SIMD_FORCE_INLINE btVector4&
operator=(const btVector4& v)
SIMD_FORCE_INLINE b3Vector4&
operator=(const b3Vector4& v)
{
mVec128 = v.mVec128;
return *this;
}
#endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
#endif // #if defined (B3_USE_SSE_IN_API) || defined (B3_USE_NEON)
SIMD_FORCE_INLINE btVector4 absolute4() const
SIMD_FORCE_INLINE b3Vector4 absolute4() const
{
#if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
return btVector4(_mm_and_ps(mVec128, btvAbsfMask));
#elif defined(BT_USE_NEON)
return btVector4(vabsq_f32(mVec128));
#if defined(B3_USE_SSE_IN_API) && defined (B3_USE_SSE)
return b3Vector4(_mm_and_ps(mVec128, b3vAbsfMask));
#elif defined(B3_USE_NEON)
return b3Vector4(vabsq_f32(mVec128));
#else
return btVector4(
btFabs(m_floats[0]),
btFabs(m_floats[1]),
btFabs(m_floats[2]),
btFabs(m_floats[3]));
return b3Vector4(
b3Fabs(m_floats[0]),
b3Fabs(m_floats[1]),
b3Fabs(m_floats[2]),
b3Fabs(m_floats[3]));
#endif
}
@@ -1115,7 +1115,7 @@ public:
SIMD_FORCE_INLINE int maxAxis4() const
{
int maxIndex = -1;
b3Scalar maxVal = b3Scalar(-BT_LARGE_FLOAT);
b3Scalar maxVal = b3Scalar(-B3_LARGE_FLOAT);
if (m_floats[0] > maxVal)
{
maxIndex = 0;
@@ -1144,7 +1144,7 @@ public:
SIMD_FORCE_INLINE int minAxis4() const
{
int minIndex = -1;
b3Scalar minVal = b3Scalar(BT_LARGE_FLOAT);
b3Scalar minVal = b3Scalar(B3_LARGE_FLOAT);
if (m_floats[0] < minVal)
{
minIndex = 0;
@@ -1210,10 +1210,10 @@ public:
};
///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void btSwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal)
///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void b3SwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& destVal)
{
#ifdef BT_USE_DOUBLE_PRECISION
#ifdef B3_USE_DOUBLE_PRECISION
unsigned char* dest = (unsigned char*) &destVal;
unsigned char* src = (unsigned char*) &sourceVal;
dest[0] = src[7];
@@ -1231,37 +1231,37 @@ SIMD_FORCE_INLINE void btSwapScalarEndian(const b3Scalar& sourceVal, b3Scalar& d
dest[1] = src[2];
dest[2] = src[1];
dest[3] = src[0];
#endif //BT_USE_DOUBLE_PRECISION
#endif //B3_USE_DOUBLE_PRECISION
}
///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void btSwapVector3Endian(const b3Vector3& sourceVec, b3Vector3& destVec)
///b3SwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void b3SwapVector3Endian(const b3Vector3& sourceVec, b3Vector3& destVec)
{
for (int i=0;i<4;i++)
{
btSwapScalarEndian(sourceVec[i],destVec[i]);
b3SwapScalarEndian(sourceVec[i],destVec[i]);
}
}
///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void btUnSwapVector3Endian(b3Vector3& vector)
///b3UnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
SIMD_FORCE_INLINE void b3UnSwapVector3Endian(b3Vector3& vector)
{
b3Vector3 swappedVec;
for (int i=0;i<4;i++)
{
btSwapScalarEndian(vector[i],swappedVec[i]);
b3SwapScalarEndian(vector[i],swappedVec[i]);
}
vector = swappedVec;
}
template <class T>
SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
SIMD_FORCE_INLINE void b3PlaneSpace1 (const T& n, T& p, T& q)
{
if (btFabs(n[2]) > SIMDSQRT12) {
if (b3Fabs(n[2]) > SIMDSQRT12) {
// choose p in y-z plane
b3Scalar a = n[1]*n[1] + n[2]*n[2];
b3Scalar k = btRecipSqrt (a);
b3Scalar k = b3RecipSqrt (a);
p[0] = 0;
p[1] = -n[2]*k;
p[2] = n[1]*k;
@@ -1273,7 +1273,7 @@ SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
else {
// choose p in x-y plane
b3Scalar a = n[0]*n[0] + n[1]*n[1];
b3Scalar k = btRecipSqrt (a);
b3Scalar k = b3RecipSqrt (a);
p[0] = -n[1]*k;
p[1] = n[0]*k;
p[2] = 0;
@@ -1285,56 +1285,56 @@ SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
}
struct btVector3FloatData
struct b3Vector3FloatData
{
float m_floats[4];
};
struct btVector3DoubleData
struct b3Vector3DoubleData
{
double m_floats[4];
};
SIMD_FORCE_INLINE void b3Vector3::serializeFloat(struct btVector3FloatData& dataOut) const
SIMD_FORCE_INLINE void b3Vector3::serializeFloat(struct b3Vector3FloatData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = float(m_floats[i]);
}
SIMD_FORCE_INLINE void b3Vector3::deSerializeFloat(const struct btVector3FloatData& dataIn)
SIMD_FORCE_INLINE void b3Vector3::deSerializeFloat(const struct b3Vector3FloatData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = b3Scalar(dataIn.m_floats[i]);
}
SIMD_FORCE_INLINE void b3Vector3::serializeDouble(struct btVector3DoubleData& dataOut) const
SIMD_FORCE_INLINE void b3Vector3::serializeDouble(struct b3Vector3DoubleData& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = double(m_floats[i]);
}
SIMD_FORCE_INLINE void b3Vector3::deSerializeDouble(const struct btVector3DoubleData& dataIn)
SIMD_FORCE_INLINE void b3Vector3::deSerializeDouble(const struct b3Vector3DoubleData& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = b3Scalar(dataIn.m_floats[i]);
}
SIMD_FORCE_INLINE void b3Vector3::serialize(struct btVector3Data& dataOut) const
SIMD_FORCE_INLINE void b3Vector3::serialize(struct b3Vector3Data& dataOut) const
{
///could also do a memcpy, check if it is worth it
for (int i=0;i<4;i++)
dataOut.m_floats[i] = m_floats[i];
}
SIMD_FORCE_INLINE void b3Vector3::deSerialize(const struct btVector3Data& dataIn)
SIMD_FORCE_INLINE void b3Vector3::deSerialize(const struct b3Vector3Data& dataIn)
{
for (int i=0;i<4;i++)
m_floats[i] = dataIn.m_floats[i];
}
#endif //BT_VECTOR3_H
#endif //B3_VECTOR3_H

18
src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h
View File

@@ -13,12 +13,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONTACT_SOLVER_INFO
#define BT_CONTACT_SOLVER_INFO
#ifndef B3_CONTACT_SOLVER_INFO
#define B3_CONTACT_SOLVER_INFO
#include "Bullet3Common/b3Scalar.h"
enum btSolverMode
enum b3SolverMode
{
SOLVER_RANDMIZE_ORDER = 1,
SOLVER_FRICTION_SEPARATE = 2,
@@ -32,7 +32,7 @@ enum btSolverMode
SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
};
struct btContactSolverInfoData
struct b3ContactSolverInfoData
{
@@ -62,7 +62,7 @@ struct btContactSolverInfoData
};
struct b3ContactSolverInfo : public btContactSolverInfoData
struct b3ContactSolverInfo : public b3ContactSolverInfoData
{
@@ -89,13 +89,13 @@ struct b3ContactSolverInfo : public btContactSolverInfoData
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_restingContactRestitutionThreshold = 2;//unused as of 2.81
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their B3_ENABLE_GYROPSCOPIC_FORCE flag set (using b3RigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoDoubleData
struct b3ContactSolverInfoDoubleData
{
double m_tau;
double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
@@ -123,7 +123,7 @@ struct btContactSolverInfoDoubleData
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btContactSolverInfoFloatData
struct b3ContactSolverInfoFloatData
{
float m_tau;
float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
@@ -156,4 +156,4 @@ struct btContactSolverInfoFloatData
#endif //BT_CONTACT_SOLVER_INFO
#endif //B3_CONTACT_SOLVER_INFO

164
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
View File

@@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
//enable BT_SOLVER_DEBUG if you experience solver crashes
//#define BT_SOLVER_DEBUG
//enable B3_SOLVER_DEBUG if you experience solver crashes
//#define B3_SOLVER_DEBUG
//#define COMPUTE_IMPULSE_DENOM 1
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
@@ -46,7 +46,7 @@ b3Transform getWorldTransform(b3RigidBodyCL* rb)
return newTrans;
}
const b3Matrix3x3& getInvInertiaTensorWorld(btInertiaCL* inertia)
const b3Matrix3x3& getInvInertiaTensorWorld(b3InertiaCL* inertia)
{
return inertia->m_invInertiaWorld;
}
@@ -70,7 +70,7 @@ b3Vector3 getVelocityInLocalPoint(b3RigidBodyCL* rb, const b3Vector3& rel_pos)
}
struct btContactPoint
struct b3ContactPoint
{
b3Vector3 m_positionWorldOnA;
b3Vector3 m_positionWorldOnB;
@@ -107,7 +107,7 @@ struct btContactPoint
}
};
void getContactPoint(b3Contact4* contact, int contactIndex, btContactPoint& pointOut)
void getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& pointOut)
{
pointOut.m_appliedImpulse = 0.f;
pointOut.m_appliedImpulseLateral1 = 0.f;
@@ -125,7 +125,7 @@ void getContactPoint(b3Contact4* contact, int contactIndex, btContactPoint& poin
normalOnB.normalize();
b3Vector3 l1,l2;
btPlaneSpace1(normalOnB,l1,l2);
b3PlaneSpace1(normalOnB,l1,l2);
pointOut.m_normalWorldOnB = normalOnB;
//printf("normalOnB = %f,%f,%f\n",normalOnB.getX(),normalOnB.getY(),normalOnB.getZ());
@@ -154,7 +154,7 @@ b3PgsJacobiSolver::~b3PgsJacobiSolver()
{
}
void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints)
{
b3ContactSolverInfo infoGlobal;
infoGlobal.m_splitImpulse = false;
@@ -179,7 +179,7 @@ void b3PgsJacobiSolver::solveContacts(int numBodies, b3RigidBodyCL* bodies, btIn
/// b3PgsJacobiSolver Sequentially applies impulses
b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies,
btInertiaCL* inertias,
b3InertiaCL* inertias,
int numBodies,
b3Contact4* manifoldPtr,
int numManifolds,
@@ -188,7 +188,7 @@ b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies,
const b3ContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveGroup");
B3_PROFILE("solveGroup");
//you need to provide at least some bodies
solveGroupCacheFriendlySetup( bodies, inertias,numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal);
@@ -210,11 +210,11 @@ b3Scalar b3PgsJacobiSolver::solveGroup(b3RigidBodyCL* bodies,
#ifdef USE_SIMD
#include <emmintrin.h>
#define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
#define b3VecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
static inline __m128 b3SimdDot3( __m128 vec0, __m128 vec1 )
{
__m128 result = _mm_mul_ps( vec0, vec1);
return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
return _mm_add_ps( b3VecSplat( result, 0 ), _mm_add_ps( b3VecSplat( result, 1 ), b3VecSplat( result, 2 ) ) );
}
#endif//USE_SIMD
@@ -226,12 +226,12 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
b3SimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
@@ -290,12 +290,12 @@ void b3PgsJacobiSolver::resolveSingleConstraintRowGenericSIMD(b3SolverBody& body
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
b3SimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
@@ -378,12 +378,12 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
__m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
__m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
__m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
__m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
__m128 deltaVel1Dotn = _mm_add_ps(b3SimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), b3SimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
__m128 deltaVel2Dotn = _mm_sub_ps(b3SimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),b3SimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
btSimdScalar resultLowerLess,resultUpperLess;
b3SimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
b3SimdScalar resultLowerLess,resultUpperLess;
resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
__m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
@@ -403,7 +403,7 @@ void b3PgsJacobiSolver::resolveSplitPenetrationImpulseCacheFriendly(
unsigned long b3PgsJacobiSolver::btRand2()
unsigned long b3PgsJacobiSolver::b3Rand2()
{
m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
return m_btSeed2;
@@ -412,11 +412,11 @@ unsigned long b3PgsJacobiSolver::btRand2()
//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
int b3PgsJacobiSolver::btRandInt2 (int n)
int b3PgsJacobiSolver::b3RandInt2 (int n)
{
// seems good; xor-fold and modulus
const unsigned long un = static_cast<unsigned long>(n);
unsigned long r = btRand2();
unsigned long r = b3Rand2();
// note: probably more aggressive than it needs to be -- might be
// able to get away without one or two of the innermost branches.
@@ -488,7 +488,7 @@ b3Scalar b3PgsJacobiSolver::restitutionCurve(b3Scalar rel_vel, b3Scalar restitut
void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
@@ -566,8 +566,8 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,btInertiaC
// b3Scalar positionalError = 0.f;
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv);
b3SimdScalar velocityError = desiredVelocity - rel_vel;
b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(scaledDenom);//solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
@@ -576,7 +576,7 @@ void b3PgsJacobiSolver::setupFrictionConstraint(b3RigidBodyCL* bodies,btInertiaC
}
}
b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
b3SolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
@@ -586,8 +586,8 @@ b3SolverConstraint& b3PgsJacobiSolver::addFrictionConstraint(b3RigidBodyCL* bodi
}
void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity, b3Scalar cfmSlip)
@@ -645,8 +645,8 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btI
// b3Scalar positionalError = 0.f;
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
b3SimdScalar velocityError = desiredVelocity - rel_vel;
b3SimdScalar velocityImpulse = velocityError * b3SimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
@@ -662,7 +662,7 @@ void b3PgsJacobiSolver::setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btI
b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity, b3Scalar cfmSlip)
{
b3SolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
@@ -672,9 +672,9 @@ b3SolverConstraint& b3PgsJacobiSolver::addRollingFrictionConstraint(b3RigidBodyC
}
int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,btInertiaCL* inertias)
int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias)
{
//btAssert(bodyIndex< m_tmpSolverBodyPool.size());
//b3Assert(bodyIndex< m_tmpSolverBodyPool.size());
b3RigidBodyCL& body = bodies[bodyIndex];
int curIndex = -1;
@@ -693,7 +693,7 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,
}
} else
{
btAssert(m_bodyCount[bodyIndex]>0);
b3Assert(m_bodyCount[bodyIndex]>0);
m_bodyCountCheck[bodyIndex]++;
curIndex = m_tmpSolverBodyPool.size();
b3SolverBody& solverBody = m_tmpSolverBodyPool.expand();
@@ -701,16 +701,16 @@ int b3PgsJacobiSolver::getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,
solverBody.m_originalBodyIndex = bodyIndex;
}
btAssert(curIndex>=0);
b3Assert(curIndex>=0);
return curIndex;
}
#include <stdio.h>
void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint,
void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal,
b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation,
b3Vector3& rel_pos1, b3Vector3& rel_pos2)
{
@@ -872,9 +872,9 @@ void b3PgsJacobiSolver::setupContactConstraint(b3RigidBodyCL* bodies, btInertiaC
void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint,
void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal)
{
b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
@@ -916,7 +916,7 @@ void b3PgsJacobiSolver::setFrictionConstraintImpulse( b3RigidBodyCL* bodies, btI
void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal)
void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal)
{
b3RigidBodyCL* colObj0=0,*colObj1=0;
@@ -924,8 +924,8 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
int solverBodyIdA = getOrInitSolverBody(manifold->getBodyA(),bodies,inertias);
int solverBodyIdB = getOrInitSolverBody(manifold->getBodyB(),bodies,inertias);
// btRigidBody* bodyA = btRigidBody::upcast(colObj0);
// btRigidBody* bodyB = btRigidBody::upcast(colObj1);
// b3RigidBody* bodyA = b3RigidBody::upcast(colObj0);
// b3RigidBody* bodyB = b3RigidBody::upcast(colObj1);
b3SolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
b3SolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
@@ -941,7 +941,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
for (int j=0;j<numContacts;j++)
{
btContactPoint cp;
b3ContactPoint cp;
getContactPoint(manifold,j,cp);
if (cp.getDistance() <= getContactProcessingThreshold(manifold))
@@ -954,8 +954,8 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
int frictionIndex = m_tmpSolverContactConstraintPool.size();
b3SolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
// b3RigidBody* rb0 = b3RigidBody::upcast(colObj0);
// b3RigidBody* rb1 = b3RigidBody::upcast(colObj1);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
@@ -989,7 +989,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
{
addRollingFrictionConstraint(bodies,inertias,cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
b3Vector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
b3PlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
if (axis0.length()>0.001)
addRollingFrictionConstraint(bodies,inertias,axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
@@ -1019,7 +1019,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
b3Scalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
cp.m_lateralFrictionDir1 *= 1.f/b3Sqrt(lat_rel_vel);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
@@ -1032,7 +1032,7 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
} else
{
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
b3PlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
@@ -1064,9 +1064,9 @@ void b3PgsJacobiSolver::convertContact(b3RigidBodyCL* bodies, btInertiaCL* inert
}
}
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, btInertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numBodies, b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveGroupCacheFriendlySetup");
B3_PROFILE("solveGroupCacheFriendlySetup");
m_maxOverrideNumSolverIterations = 0;
@@ -1099,7 +1099,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
} else
{
//didn't implement joints with Jacobi version yet
btAssert(0);
b3Assert(0);
}
}
@@ -1143,7 +1143,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
}
}
//btRigidBody* rb0=0,*rb1=0;
//b3RigidBody* rb0=0,*rb1=0;
//if (1)
{
{
@@ -1155,8 +1155,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
//calculate the total number of contraint rows
for (i=0;i<numConstraints;i++)
{
b3TypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
btJointFeedback* fb = constraints[i]->getJointFeedback();
b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
b3JointFeedback* fb = constraints[i]->getJointFeedback();
if (fb)
{
fb->m_appliedForceBodyA.setZero();
@@ -1182,23 +1182,23 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
#ifndef DISABLE_JOINTS
///setup the btSolverConstraints
///setup the b3SolverConstraints
int currentRow = 0;
for (i=0;i<numConstraints;i++)
{
const b3TypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
const b3TypedConstraint::b3ConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
if (info1.m_numConstraintRows)
{
btAssert(currentRow<totalNumRows);
b3Assert(currentRow<totalNumRows);
b3SolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
b3TypedConstraint* constraint = constraints[i];
b3RigidBodyCL& rbA = bodies[ constraint->getRigidBodyA()];
//btRigidBody& rbA = constraint->getRigidBodyA();
// btRigidBody& rbB = constraint->getRigidBodyB();
//b3RigidBody& rbA = constraint->getRigidBodyA();
// b3RigidBody& rbB = constraint->getRigidBodyB();
b3RigidBodyCL& rbB = bodies[ constraint->getRigidBodyB()];
int solverBodyIdA = getOrInitSolverBody(constraint->getRigidBodyA(),bodies,inertias);
@@ -1238,7 +1238,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
b3TypedConstraint::btConstraintInfo2 info2;
b3TypedConstraint::b3ConstraintInfo2 info2;
info2.fps = 1.f/infoGlobal.m_timeStep;
info2.erp = infoGlobal.m_erp;
info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
@@ -1247,7 +1247,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
info2.rowskip = sizeof(b3SolverConstraint)/sizeof(b3Scalar);//check this
///the size of b3SolverConstraint needs be a multiple of b3Scalar
btAssert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint));
b3Assert(info2.rowskip*sizeof(b3Scalar)== sizeof(b3SolverConstraint));
info2.m_constraintError = &currentConstraintRow->m_rhs;
currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
info2.m_damping = infoGlobal.m_damping;
@@ -1277,7 +1277,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
b3Matrix3x3& invInertiaWorldA= inertias[constraint->getRigidBodyA()].m_invInertiaWorld;
{
//btVector3 angularFactorA(1,1,1);
//b3Vector3 angularFactorA(1,1,1);
const b3Vector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
solverConstraint.m_angularComponentA = invInertiaWorldA*ftorqueAxis1;//*angularFactorA;
}
@@ -1299,8 +1299,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJlB.dot(solverConstraint.m_contactNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
b3Scalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
b3Scalar fsum = b3Fabs(sum);
b3Assert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?b3Scalar(1.)/sum : 0.f;
}
@@ -1392,7 +1392,7 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
for (int j=0; j<numNonContactPool; ++j) {
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = b3RandInt2(j+1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
@@ -1402,14 +1402,14 @@ b3Scalar b3PgsJacobiSolver::solveSingleIteration(int iteration,b3TypedConstraint
{
for (int j=0; j<numConstraintPool; ++j) {
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = b3RandInt2(j+1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j=0; j<numFrictionPool; ++j) {
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2(j+1);
int swapi = b3RandInt2(j+1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
@@ -1637,7 +1637,7 @@ void b3PgsJacobiSolver::solveGroupCacheFriendlySplitImpulseIterations(b3TypedCon
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal)
{
BT_PROFILE("solveGroupCacheFriendlyIterations");
B3_PROFILE("solveGroupCacheFriendlyIterations");
{
///this is a special step to resolve penetrations (just for contacts)
@@ -1664,7 +1664,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyIterations(b3TypedConstraint*
void b3PgsJacobiSolver::averageVelocities()
{
BT_PROFILE("averaging");
B3_PROFILE("averaging");
//average the velocities
int numBodies = m_bodyCount.size();
@@ -1690,7 +1690,7 @@ void b3PgsJacobiSolver::averageVelocities()
if (!m_tmpSolverBodyPool[i].m_invMass.isZero())
{
btAssert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
b3Assert(m_bodyCount[orgBodyIndex] == m_bodyCountCheck[orgBodyIndex]);
b3Scalar factor = 1.f/b3Scalar(m_bodyCount[orgBodyIndex]);
@@ -1701,7 +1701,7 @@ void b3PgsJacobiSolver::averageVelocities()
}
}
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int i,j;
@@ -1711,8 +1711,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
for (j=0;j<numPoolConstraints;j++)
{
const b3SolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
btContactPoint* pt = (btContactPoint*) solveManifold.m_originalContactPoint;
btAssert(pt);
b3ContactPoint* pt = (b3ContactPoint*) solveManifold.m_originalContactPoint;
b3Assert(pt);
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
// float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
// printf("pt->m_appliedImpulseLateral1 = %f\n", f);
@@ -1731,7 +1731,7 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
{
const b3SolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
b3TypedConstraint* constr = (b3TypedConstraint*)solverConstr.m_originalContactPoint;
btJointFeedback* fb = constr->getJointFeedback();
b3JointFeedback* fb = constr->getJointFeedback();
if (fb)
{
b3SolverBody* bodyA = &m_tmpSolverBodyPool[solverConstr.m_solverBodyIdA];
@@ -1745,18 +1745,18 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies,
}
constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
if (b3Fabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
{
constr->setEnabled(false);
}
}
{
BT_PROFILE("write back velocities and transforms");
B3_PROFILE("write back velocities and transforms");
for ( i=0;i<m_tmpSolverBodyPool.size();i++)
{
int bodyIndex = m_tmpSolverBodyPool[i].m_originalBodyIndex;
//btAssert(i==bodyIndex);
//b3Assert(i==bodyIndex);
b3RigidBodyCL* body = &bodies[bodyIndex];
if (body->getInvMass())

62
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h
View File

@@ -1,12 +1,12 @@
#ifndef BT_PGS_JACOBI_SOLVER
#define BT_PGS_JACOBI_SOLVER
#ifndef B3_PGS_JACOBI_SOLVER
#define B3_PGS_JACOBI_SOLVER
struct b3Contact4;
struct btContactPoint;
struct b3ContactPoint;
class btDispatcher;
class b3Dispatcher;
#include "b3TypedConstraint.h"
#include "b3ContactSolverInfo.h"
@@ -14,22 +14,22 @@ class btDispatcher;
#include "b3SolverConstraint.h"
struct b3RigidBodyCL;
struct btInertiaCL;
struct b3InertiaCL;
class b3PgsJacobiSolver
{
protected:
b3AlignedObjectArray<b3SolverBody> m_tmpSolverBodyPool;
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactConstraintPool;
b3ConstraintArray m_tmpSolverNonContactConstraintPool;
b3ConstraintArray m_tmpSolverContactFrictionConstraintPool;
b3ConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
b3AlignedObjectArray<int> m_orderTmpConstraintPool;
b3AlignedObjectArray<int> m_orderNonContactConstraintPool;
b3AlignedObjectArray<int> m_orderFrictionConstraintPool;
b3AlignedObjectArray<b3TypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
b3AlignedObjectArray<b3TypedConstraint::b3ConstraintInfo1> m_tmpConstraintSizesPool;
b3AlignedObjectArray<int> m_bodyCount;
b3AlignedObjectArray<int> m_bodyCountCheck;
@@ -45,27 +45,27 @@ protected:
{
return 0.02f;
}
void setupFrictionConstraint( b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
void setupFrictionConstraint( b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
void setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
void setupRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation,
b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
b3SolverConstraint& addFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
b3SolverConstraint& addRollingFrictionConstraint(b3RigidBodyCL* bodies,b3InertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,b3ContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
void setupContactConstraint(b3RigidBodyCL* bodies, btInertiaCL* inertias,
b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btContactPoint& cp,
void setupContactConstraint(b3RigidBodyCL* bodies, b3InertiaCL* 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);
void setFrictionConstraintImpulse( b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
btContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
void setFrictionConstraintImpulse( b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB,
b3ContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
@@ -73,7 +73,7 @@ protected:
b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
void convertContact(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
void convertContact(b3RigidBodyCL* bodies, b3InertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
@@ -85,7 +85,7 @@ protected:
const b3SolverConstraint& contactConstraint);
//internal method
int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,btInertiaCL* inertias);
int getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,b3InertiaCL* inertias);
void initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyCL* collisionObject);
void resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
@@ -98,7 +98,7 @@ protected:
protected:
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, b3InertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
@@ -106,27 +106,27 @@ protected:
b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, b3InertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3PgsJacobiSolver();
virtual ~b3PgsJacobiSolver();
// void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
// void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts);
void solveContacts(int numBodies, b3RigidBodyCL* bodies, b3InertiaCL* inertias, int numContacts, b3Contact4* contacts, int numConstraints, b3TypedConstraint** constraints);
b3Scalar solveGroup(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
b3Scalar solveGroup(b3RigidBodyCL* bodies,b3InertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
///clear internal cached data and reset random seed
virtual void reset();
unsigned long btRand2();
unsigned long b3Rand2();
int btRandInt2 (int n);
int b3RandInt2 (int n);
void setRandSeed(unsigned long seed)
{
@@ -142,5 +142,5 @@ public:
};
#endif //BT_PGS_JACOBI_SOLVER
#endif //B3_PGS_JACOBI_SOLVER

46
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
View File

@@ -51,12 +51,12 @@ void b3Point2PointConstraint::buildJacobian()
}
void b3Point2PointConstraint::getInfo1 (btConstraintInfo1* info)
void b3Point2PointConstraint::getInfo1 (b3ConstraintInfo1* info)
{
getInfo1NonVirtual(info);
}
void b3Point2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
void b3Point2PointConstraint::getInfo1NonVirtual (b3ConstraintInfo1* info)
{
if (m_useSolveConstraintObsolete)
{
@@ -72,7 +72,7 @@ void b3Point2PointConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
void b3Point2PointConstraint::getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)
void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)
{
b3Transform trA;
trA.setIdentity();
@@ -87,9 +87,9 @@ void b3Point2PointConstraint::getInfo2 (btConstraintInfo2* info, const b3RigidBo
getInfo2NonVirtual(info, trA,trB);
}
void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans)
{
btAssert(!m_useSolveConstraintObsolete);
b3Assert(!m_useSolveConstraintObsolete);
//retrieve matrices
@@ -129,7 +129,7 @@ void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
// set right hand side
b3Scalar currERP = (m_flags & BT_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp;
b3Scalar k = info->fps * currERP;
int j;
for (j=0; j<3; j++)
@@ -137,7 +137,7 @@ void b3Point2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const
info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if(m_flags & BT_P2P_FLAGS_CFM)
if(m_flags & B3_P2P_FLAGS_CFM)
{
for (j=0; j<3; j++)
{
@@ -172,24 +172,24 @@ void b3Point2PointConstraint::setParam(int num, b3Scalar value, int axis)
{
if(axis != -1)
{
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
m_erp = value;
m_flags |= BT_P2P_FLAGS_ERP;
m_flags |= B3_P2P_FLAGS_ERP;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
m_cfm = value;
m_flags |= BT_P2P_FLAGS_CFM;
m_flags |= B3_P2P_FLAGS_CFM;
break;
default:
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
}
}
@@ -200,24 +200,24 @@ b3Scalar b3Point2PointConstraint::getParam(int num, int axis) const
b3Scalar retVal(SIMD_INFINITY);
if(axis != -1)
{
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
else
{
switch(num)
{
case BT_CONSTRAINT_ERP :
case BT_CONSTRAINT_STOP_ERP :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_ERP);
case B3_CONSTRAINT_ERP :
case B3_CONSTRAINT_STOP_ERP :
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_ERP);
retVal = m_erp;
break;
case BT_CONSTRAINT_CFM :
case BT_CONSTRAINT_STOP_CFM :
btAssertConstrParams(m_flags & BT_P2P_FLAGS_CFM);
case B3_CONSTRAINT_CFM :
case B3_CONSTRAINT_STOP_CFM :
b3AssertConstrParams(m_flags & B3_P2P_FLAGS_CFM);
retVal = m_cfm;
break;
default:
btAssertConstrParams(0);
b3AssertConstrParams(0);
}
}
return retVal;

68
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h
View File

@@ -13,27 +13,27 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_POINT2POINTCONSTRAINT_H
#define BT_POINT2POINTCONSTRAINT_H
#ifndef B3_POINT2POINTCONSTRAINT_H
#define B3_POINT2POINTCONSTRAINT_H
#include "Bullet3Common/b3Vector3.h"
//#include "b3JacobianEntry.h"
#include "b3TypedConstraint.h"
class btRigidBody;
class b3RigidBody;
#ifdef BT_USE_DOUBLE_PRECISION
#define btPoint2PointConstraintData btPoint2PointConstraintDoubleData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintDoubleData"
#ifdef B3_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintDoubleData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintDoubleData"
#else
#define btPoint2PointConstraintData btPoint2PointConstraintFloatData
#define btPoint2PointConstraintDataName "btPoint2PointConstraintFloatData"
#endif //BT_USE_DOUBLE_PRECISION
#define b3Point2PointConstraintData b3Point2PointConstraintFloatData
#define b3Point2PointConstraintDataName "b3Point2PointConstraintFloatData"
#endif //B3_USE_DOUBLE_PRECISION
struct btConstraintSetting
struct b3ConstraintSetting
{
btConstraintSetting() :
b3ConstraintSetting() :
m_tau(b3Scalar(0.3)),
m_damping(b3Scalar(1.)),
m_impulseClamp(b3Scalar(0.))
@@ -44,10 +44,10 @@ struct btConstraintSetting
b3Scalar m_impulseClamp;
};
enum btPoint2PointFlags
enum b3Point2PointFlags
{
BT_P2P_FLAGS_ERP = 1,
BT_P2P_FLAGS_CFM = 2
B3_P2P_FLAGS_ERP = 1,
B3_P2P_FLAGS_CFM = 2
};
/// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space
@@ -66,12 +66,12 @@ public:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
///for backwards compatibility during the transition to 'getInfo/getInfo2'
bool m_useSolveConstraintObsolete;
btConstraintSetting m_setting;
b3ConstraintSetting m_setting;
b3Point2PointConstraint(int rbA,int rbB, const b3Vector3& pivotInA,const b3Vector3& pivotInB);
@@ -80,13 +80,13 @@ public:
virtual void buildJacobian();
virtual void getInfo1 (btConstraintInfo1* info);
virtual void getInfo1 (b3ConstraintInfo1* info);
void getInfo1NonVirtual (btConstraintInfo1* info);
void getInfo1NonVirtual (b3ConstraintInfo1* info);
virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies);
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies);
void getInfo2NonVirtual (btConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
void getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans);
void updateRHS(b3Scalar timeStep);
@@ -119,45 +119,45 @@ public:
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
// virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
// virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintFloatData
struct b3Point2PointConstraintFloatData
{
btTypedConstraintData m_typeConstraintData;
btVector3FloatData m_pivotInA;
btVector3FloatData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
};
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btPoint2PointConstraintDoubleData
struct b3Point2PointConstraintDoubleData
{
btTypedConstraintData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
};
/*
SIMD_FORCE_INLINE int b3Point2PointConstraint::calculateSerializeBufferSize() const
{
return sizeof(btPoint2PointConstraintData);
return sizeof(b3Point2PointConstraintData);
}
///fills the dataBuffer and returns the struct name (and 0 on failure)
SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
SIMD_FORCE_INLINE const char* b3Point2PointConstraint::serialize(void* dataBuffer, b3Serializer* serializer) const
{
btPoint2PointConstraintData* p2pData = (btPoint2PointConstraintData*)dataBuffer;
b3Point2PointConstraintData* p2pData = (b3Point2PointConstraintData*)dataBuffer;
b3TypedConstraint::serialize(&p2pData->m_typeConstraintData,serializer);
m_pivotInA.serialize(p2pData->m_pivotInA);
m_pivotInB.serialize(p2pData->m_pivotInB);
return btPoint2PointConstraintDataName;
return b3Point2PointConstraintDataName;
}
*/
#endif //BT_POINT2POINTCONSTRAINT_H
#endif //B3_POINT2POINTCONSTRAINT_H

40
src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
View File

@@ -13,10 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SOLVER_BODY_H
#define BT_SOLVER_BODY_H
#ifndef B3_SOLVER_BODY_H
#define B3_SOLVER_BODY_H
class btRigidBody;
class b3RigidBody;
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
@@ -24,26 +24,26 @@ class btRigidBody;
#include "Bullet3Common/b3TransformUtil.h"
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
#ifdef BT_USE_SSE
#ifdef B3_USE_SSE
#define USE_SIMD 1
#endif //
#ifdef USE_SIMD
struct btSimdScalar
struct b3SimdScalar
{
SIMD_FORCE_INLINE btSimdScalar()
SIMD_FORCE_INLINE b3SimdScalar()
{
}
SIMD_FORCE_INLINE btSimdScalar(float fl)
SIMD_FORCE_INLINE b3SimdScalar(float fl)
:m_vec128 (_mm_set1_ps(fl))
{
}
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
SIMD_FORCE_INLINE b3SimdScalar(__m128 v128)
:m_vec128(v128)
{
}
@@ -85,29 +85,29 @@ struct btSimdScalar
};
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator*(const btSimdScalar& v1, const btSimdScalar& v2)
///@brief Return the elementwise product of two b3SimdScalar
SIMD_FORCE_INLINE b3SimdScalar
operator*(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
}
///@brief Return the elementwise product of two btSimdScalar
SIMD_FORCE_INLINE btSimdScalar
operator+(const btSimdScalar& v1, const btSimdScalar& v2)
///@brief Return the elementwise product of two b3SimdScalar
SIMD_FORCE_INLINE b3SimdScalar
operator+(const b3SimdScalar& v1, const b3SimdScalar& v2)
{
return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
return b3SimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
}
#else
#define btSimdScalar b3Scalar
#define b3SimdScalar b3Scalar
#endif
///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Transform m_worldTransform;
b3Vector3 m_deltaLinearVelocity;
b3Vector3 m_deltaAngularVelocity;
@@ -281,7 +281,7 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
b3Transform newTransform;
if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
{
// btQuaternion orn = m_worldTransform.getRotation();
// b3Quaternion orn = m_worldTransform.getRotation();
b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
m_worldTransform = newTransform;
}
@@ -294,6 +294,6 @@ ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
};
#endif //BT_SOLVER_BODY_H
#endif //B3_SOLVER_BODY_H

24
src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h
View File

@@ -13,13 +13,13 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SOLVER_CONSTRAINT_H
#define BT_SOLVER_CONSTRAINT_H
#ifndef B3_SOLVER_CONSTRAINT_H
#define B3_SOLVER_CONSTRAINT_H
class btRigidBody;
class b3RigidBody;
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
//#include "btJacobianEntry.h"
//#include "b3JacobianEntry.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
//#define NO_FRICTION_TANGENTIALS 1
@@ -29,7 +29,7 @@ class btRigidBody;
///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
{
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
b3Vector3 m_relpos1CrossNormal;
b3Vector3 m_contactNormal;
@@ -40,8 +40,8 @@ ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
b3Vector3 m_angularComponentA;
b3Vector3 m_angularComponentB;
mutable btSimdScalar m_appliedPushImpulse;
mutable btSimdScalar m_appliedImpulse;
mutable b3SimdScalar m_appliedPushImpulse;
mutable b3SimdScalar m_appliedImpulse;
b3Scalar m_friction;
b3Scalar m_jacDiagABInv;
@@ -63,17 +63,17 @@ ATTRIBUTE_ALIGNED16 (struct) b3SolverConstraint
int m_solverBodyIdB;
enum btSolverConstraintType
enum b3SolverConstraintType
{
BT_SOLVER_CONTACT_1D = 0,
BT_SOLVER_FRICTION_1D
B3_SOLVER_CONTACT_1D = 0,
B3_SOLVER_FRICTION_1D
};
};
typedef b3AlignedObjectArray<b3SolverConstraint> btConstraintArray;
typedef b3AlignedObjectArray<b3SolverConstraint> b3ConstraintArray;
#endif //BT_SOLVER_CONSTRAINT_H
#endif //B3_SOLVER_CONSTRAINT_H

30
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp
View File

@@ -15,15 +15,15 @@ subject to the following restrictions:
#include "b3TypedConstraint.h"
//#include "Bullet3Common/btSerializer.h"
//#include "Bullet3Common/b3Serializer.h"
#define DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
b3TypedConstraint::b3TypedConstraint(btTypedConstraintType type, int rbA,int rbB)
:btTypedObject(type),
b3TypedConstraint::b3TypedConstraint(b3TypedConstraintType type, int rbA,int rbB)
:b3TypedObject(type),
m_userConstraintType(-1),
m_userConstraintId(-1),
m_breakingImpulseThreshold(SIMD_INFINITY),
@@ -92,16 +92,16 @@ b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scal
void btAngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
void b3AngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
{
m_halfRange = (high - low) / 2.0f;
m_center = btNormalizeAngle(low + m_halfRange);
m_center = b3NormalizeAngle(low + m_halfRange);
m_softness = _softness;
m_biasFactor = _biasFactor;
m_relaxationFactor = _relaxationFactor;
}
void btAngularLimit::test(const b3Scalar angle)
void b3AngularLimit::test(const b3Scalar angle)
{
m_correction = 0.0f;
m_sign = 0.0f;
@@ -109,7 +109,7 @@ void btAngularLimit::test(const b3Scalar angle)
if (m_halfRange >= 0.0f)
{
b3Scalar deviation = btNormalizeAngle(angle - m_center);
b3Scalar deviation = b3NormalizeAngle(angle - m_center);
if (deviation < -m_halfRange)
{
m_solveLimit = true;
@@ -126,17 +126,17 @@ void btAngularLimit::test(const b3Scalar angle)
}
b3Scalar btAngularLimit::getError() const
b3Scalar b3AngularLimit::getError() const
{
return m_correction * m_sign;
}
void btAngularLimit::fit(b3Scalar& angle) const
void b3AngularLimit::fit(b3Scalar& angle) const
{
if (m_halfRange > 0.0f)
{
b3Scalar relativeAngle = btNormalizeAngle(angle - m_center);
if (!btEqual(relativeAngle, m_halfRange))
b3Scalar relativeAngle = b3NormalizeAngle(angle - m_center);
if (!b3Equal(relativeAngle, m_halfRange))
{
if (relativeAngle > 0.0f)
{
@@ -150,12 +150,12 @@ void btAngularLimit::fit(b3Scalar& angle) const
}
}
b3Scalar btAngularLimit::getLow() const
b3Scalar b3AngularLimit::getLow() const
{
return btNormalizeAngle(m_center - m_halfRange);
return b3NormalizeAngle(m_center - m_halfRange);
}
b3Scalar btAngularLimit::getHigh() const
b3Scalar b3AngularLimit::getHigh() const
{
return btNormalizeAngle(m_center + m_halfRange);
return b3NormalizeAngle(m_center + m_halfRange);
}

80
src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h
View File

@@ -13,17 +13,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TYPED_CONSTRAINT_H
#define BT_TYPED_CONSTRAINT_H
#ifndef B3_TYPED_CONSTRAINT_H
#define B3_TYPED_CONSTRAINT_H
#include "Bullet3Common/b3Scalar.h"
#include "b3SolverConstraint.h"
class btSerializer;
class b3Serializer;
//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
enum btTypedConstraintType
enum b3TypedConstraintType
{
POINT2POINT_CONSTRAINT_TYPE=3,
HINGE_CONSTRAINT_TYPE,
@@ -37,22 +37,22 @@ enum btTypedConstraintType
};
enum btConstraintParams
enum b3ConstraintParams
{
BT_CONSTRAINT_ERP=1,
BT_CONSTRAINT_STOP_ERP,
BT_CONSTRAINT_CFM,
BT_CONSTRAINT_STOP_CFM
B3_CONSTRAINT_ERP=1,
B3_CONSTRAINT_STOP_ERP,
B3_CONSTRAINT_CFM,
B3_CONSTRAINT_STOP_CFM
};
#if 1
#define btAssertConstrParams(_par) btAssert(_par)
#define b3AssertConstrParams(_par) b3Assert(_par)
#else
#define btAssertConstrParams(_par)
#define b3AssertConstrParams(_par)
#endif
ATTRIBUTE_ALIGNED16(struct) btJointFeedback
ATTRIBUTE_ALIGNED16(struct) b3JointFeedback
{
b3Vector3 m_appliedForceBodyA;
b3Vector3 m_appliedTorqueBodyA;
@@ -64,7 +64,7 @@ struct b3RigidBodyCL;
///TypedConstraint is the baseclass for Bullet constraints and vehicles
ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject
ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public b3TypedObject
{
int m_userConstraintType;
@@ -82,7 +82,7 @@ ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject
b3TypedConstraint& operator=(b3TypedConstraint& other)
{
btAssert(0);
b3Assert(0);
(void) other;
return *this;
}
@@ -92,7 +92,7 @@ protected:
int m_rbB;
b3Scalar m_appliedImpulse;
b3Scalar m_dbgDrawSize;
btJointFeedback* m_jointFeedback;
b3JointFeedback* m_jointFeedback;
///internal method used by the constraint solver, don't use them directly
b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
@@ -100,18 +100,18 @@ protected:
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
B3_DECLARE_ALIGNED_ALLOCATOR();
virtual ~b3TypedConstraint() {};
b3TypedConstraint(btTypedConstraintType type, int bodyA,int bodyB);
b3TypedConstraint(b3TypedConstraintType type, int bodyA,int bodyB);
struct btConstraintInfo1 {
struct b3ConstraintInfo1 {
int m_numConstraintRows,nub;
};
static btRigidBody& getFixedBody();
static b3RigidBody& getFixedBody();
struct btConstraintInfo2 {
struct b3ConstraintInfo2 {
// integrator parameters: frames per second (1/stepsize), default error
// reduction parameter (0..1).
b3Scalar fps,erp;
@@ -161,7 +161,7 @@ public:
virtual void buildJacobian() {};
///internal method used by the constraint solver, don't use them directly
virtual void setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
virtual void setupSolverConstraint(b3ConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
{
(void)ca;
(void)solverBodyA;
@@ -170,10 +170,10 @@ public:
}
///internal method used by the constraint solver, don't use them directly
virtual void getInfo1 (btConstraintInfo1* info)=0;
virtual void getInfo1 (b3ConstraintInfo1* info)=0;
///internal method used by the constraint solver, don't use them directly
virtual void getInfo2 (btConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;
virtual void getInfo2 (b3ConstraintInfo2* info, const b3RigidBodyCL* bodies)=0;
///internal method used by the constraint solver, don't use them directly
void internalSetAppliedImpulse(b3Scalar appliedImpulse)
@@ -261,17 +261,17 @@ public:
return m_userConstraintPtr;
}
void setJointFeedback(btJointFeedback* jointFeedback)
void setJointFeedback(b3JointFeedback* jointFeedback)
{
m_jointFeedback = jointFeedback;
}
const btJointFeedback* getJointFeedback() const
const b3JointFeedback* getJointFeedback() const
{
return m_jointFeedback;
}
btJointFeedback* getJointFeedback()
b3JointFeedback* getJointFeedback()
{
return m_jointFeedback;
}
@@ -298,13 +298,13 @@ public:
///This feedback could be used to determine breaking constraints or playing sounds.
b3Scalar getAppliedImpulse() const
{
btAssert(m_needsFeedback);
b3Assert(m_needsFeedback);
return m_appliedImpulse;
}
btTypedConstraintType getConstraintType () const
b3TypedConstraintType getConstraintType () const
{
return btTypedConstraintType(m_objectType);
return b3TypedConstraintType(m_objectType);
}
void setDbgDrawSize(b3Scalar dbgDrawSize)
@@ -326,13 +326,13 @@ public:
// virtual int calculateSerializeBufferSize() const;
///fills the dataBuffer and returns the struct name (and 0 on failure)
//virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
//virtual const char* serialize(void* dataBuffer, b3Serializer* serializer) const;
};
// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits
// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
SIMD_FORCE_INLINE b3Scalar b3AdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
{
if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
{
@@ -340,14 +340,14 @@ SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scal
}
else if(angleInRadians < angleLowerLimitInRadians)
{
b3Scalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleLowerLimitInRadians - angleInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleUpperLimitInRadians - angleInRadians));
return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
}
else if(angleInRadians > angleUpperLimitInRadians)
{
b3Scalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
b3Scalar diffHi = b3Fabs(b3NormalizeAngle(angleInRadians - angleUpperLimitInRadians));
b3Scalar diffLo = b3Fabs(b3NormalizeAngle(angleInRadians - angleLowerLimitInRadians));
return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
}
else
@@ -357,7 +357,7 @@ SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scal
}
///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
struct btTypedConstraintData
struct b3TypedConstraintData
{
int m_bodyA;
int m_bodyB;
@@ -381,12 +381,12 @@ struct btTypedConstraintData
/*SIMD_FORCE_INLINE int b3TypedConstraint::calculateSerializeBufferSize() const
{
return sizeof(btTypedConstraintData);
return sizeof(b3TypedConstraintData);
}
*/
class btAngularLimit
class b3AngularLimit
{
private:
b3Scalar
@@ -403,7 +403,7 @@ private:
public:
/// Default constructor initializes limit as inactive, allowing free constraint movement
btAngularLimit()
b3AngularLimit()
:m_center(0.0f),
m_halfRange(-1.0f),
m_softness(0.9f),
@@ -480,4 +480,4 @@ public:
#endif //BT_TYPED_CONSTRAINT_H
#endif //B3_TYPED_CONSTRAINT_H

42
src/Bullet3Geometry/b3AabbUtil.h
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
#ifndef BT_AABB_UTIL2
#define BT_AABB_UTIL2
#ifndef B3_AABB_UTIL2
#define B3_AABB_UTIL2
#include "Bullet3Common/b3Transform.h"
#include "Bullet3Common/b3Vector3.h"
@@ -63,19 +63,19 @@ SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const b3Vector3 *vertices,
const b3Vector3 &p2 = vertices[1];
const b3Vector3 &p3 = vertices[2];
if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
if (b3Min(b3Min(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (b3Max(b3Max(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
if (b3Min(b3Min(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
if (b3Max(b3Max(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
if (b3Min(b3Min(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
if (b3Max(b3Max(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
return true;
}
SIMD_FORCE_INLINE int btOutcode(const b3Vector3& p,const b3Vector3& halfExtent)
SIMD_FORCE_INLINE int b3Outcode(const b3Vector3& p,const b3Vector3& halfExtent)
{
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
@@ -87,7 +87,7 @@ SIMD_FORCE_INLINE int btOutcode(const b3Vector3& p,const b3Vector3& halfExtent)
SIMD_FORCE_INLINE bool btRayAabb2(const b3Vector3& rayFrom,
SIMD_FORCE_INLINE bool b3RayAabb2(const b3Vector3& rayFrom,
const b3Vector3& rayInvDirection,
const unsigned int raySign[3],
const b3Vector3 bounds[2],
@@ -122,7 +122,7 @@ SIMD_FORCE_INLINE bool btRayAabb2(const b3Vector3& rayFrom,
return ( (tmin < lambda_max) && (tmax > lambda_min) );
}
SIMD_FORCE_INLINE bool btRayAabb(const b3Vector3& rayFrom,
SIMD_FORCE_INLINE bool b3RayAabb(const b3Vector3& rayFrom,
const b3Vector3& rayTo,
const b3Vector3& aabbMin,
const b3Vector3& aabbMax,
@@ -132,8 +132,8 @@ SIMD_FORCE_INLINE bool btRayAabb(const b3Vector3& rayFrom,
b3Vector3 aabbCenter = (aabbMax+aabbMin)* b3Scalar(0.5);
b3Vector3 source = rayFrom - aabbCenter;
b3Vector3 target = rayTo - aabbCenter;
int sourceOutcode = btOutcode(source,aabbHalfExtent);
int targetOutcode = btOutcode(target,aabbHalfExtent);
int sourceOutcode = b3Outcode(source,aabbHalfExtent);
int targetOutcode = b3Outcode(target,aabbHalfExtent);
if ((sourceOutcode & targetOutcode) == 0x0)
{
b3Scalar lambda_enter = b3Scalar(0.0);
@@ -161,7 +161,7 @@ SIMD_FORCE_INLINE bool btRayAabb(const b3Vector3& rayFrom,
else if (targetOutcode & bit)
{
b3Scalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
btSetMin(lambda_exit, lambda);
b3SetMin(lambda_exit, lambda);
}
bit<<=1;
}
@@ -179,7 +179,7 @@ SIMD_FORCE_INLINE bool btRayAabb(const b3Vector3& rayFrom,
SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
SIMD_FORCE_INLINE void b3TransformAabb(const b3Vector3& halfExtents, b3Scalar margin,const b3Transform& t,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
{
b3Vector3 halfExtentsWithMargin = halfExtents+b3Vector3(margin,margin,margin);
b3Matrix3x3 abs_b = t.getBasis().absolute();
@@ -190,11 +190,11 @@ SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& halfExtents, b3Scalar ma
}
SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
SIMD_FORCE_INLINE void b3TransformAabb(const b3Vector3& localAabbMin,const b3Vector3& localAabbMax, b3Scalar margin,const b3Transform& trans,b3Vector3& aabbMinOut,b3Vector3& aabbMaxOut)
{
btAssert(localAabbMin.getX() <= localAabbMax.getX());
btAssert(localAabbMin.getY() <= localAabbMax.getY());
btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
b3Assert(localAabbMin.getX() <= localAabbMax.getX());
b3Assert(localAabbMin.getY() <= localAabbMax.getY());
b3Assert(localAabbMin.getZ() <= localAabbMax.getZ());
b3Vector3 localHalfExtents = b3Scalar(0.5)*(localAabbMax-localAabbMin);
localHalfExtents+=b3Vector3(margin,margin,margin);
@@ -211,7 +211,7 @@ SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& localAabbMin,const b3Vec
//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)
SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(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>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
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));
@@ -227,6 +227,6 @@ SIMD_FORCE_INLINE void btTransformAabb(const b3Vector3& localAabbMin,const b3Vec
}
#endif //USE_BANCHLESS
#endif //BT_AABB_UTIL2
#endif //B3_AABB_UTIL2

138
src/Bullet3Geometry/b3ConvexHullComputer.cpp
View File

@@ -49,7 +49,7 @@ subject to the following restrictions:
// Convex hull implementation based on Preparata and Hong
// Ole Kniemeyer, MAXON Computer GmbH
class btConvexHullInternal
class b3ConvexHullInternal
{
public:
@@ -495,7 +495,7 @@ class btConvexHullInternal
}
for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex)
{
btAssert(f->nearbyVertex == src);
b3Assert(f->nearbyVertex == src);
f->nearbyVertex = this;
}
src->firstNearbyFace = NULL;
@@ -525,7 +525,7 @@ class btConvexHullInternal
void link(Edge* n)
{
btAssert(reverse->target == n->reverse->target);
b3Assert(reverse->target == n->reverse->target);
next = n;
n->prev = this;
}
@@ -673,12 +673,12 @@ class btConvexHullInternal
PoolArray(int size): size(size), next(NULL)
{
array = (T*) btAlignedAlloc(sizeof(T) * size, 16);
array = (T*) b3AlignedAlloc(sizeof(T) * size, 16);
}
~PoolArray()
{
btAlignedFree(array);
b3AlignedFree(array);
}
T* init()
@@ -712,7 +712,7 @@ class btConvexHullInternal
PoolArray<T>* p = arrays;
arrays = p->next;
p->~PoolArray<T>();
btAlignedFree(p);
b3AlignedFree(p);
}
}
@@ -739,7 +739,7 @@ class btConvexHullInternal
}
else
{
p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
p = new(b3AlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize);
p->next = arrays;
arrays = p;
}
@@ -781,7 +781,7 @@ class btConvexHullInternal
Edge* n = edge->next;
Edge* r = edge->reverse;
btAssert(edge->target && r->target);
b3Assert(edge->target && r->target);
if (n != edge)
{
@@ -835,7 +835,7 @@ class btConvexHullInternal
};
btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const
b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::operator*(int64_t b) const
{
bool negative = (int64_t) high < 0;
Int128 a = negative ? -*this : *this;
@@ -849,7 +849,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b)
return negative ? -result : result;
}
btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b)
b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(int64_t a, int64_t b)
{
Int128 result;
@@ -876,7 +876,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_
#endif
}
btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
b3ConvexHullInternal::Int128 b3ConvexHullInternal::Int128::mul(uint64_t a, uint64_t b)
{
Int128 result;
@@ -893,7 +893,7 @@ btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint6
return result;
}
int btConvexHullInternal::Rational64::compare(const Rational64& b) const
int b3ConvexHullInternal::Rational64::compare(const Rational64& b) const
{
if (sign != b.sign)
{
@@ -937,7 +937,7 @@ int btConvexHullInternal::Rational64::compare(const Rational64& b) const
#endif
}
int btConvexHullInternal::Rational128::compare(const Rational128& b) const
int b3ConvexHullInternal::Rational128::compare(const Rational128& b) const
{
if (sign != b.sign)
{
@@ -964,7 +964,7 @@ int btConvexHullInternal::Rational128::compare(const Rational128& b) const
return nbdLow.ucmp(dbnLow) * sign;
}
int btConvexHullInternal::Rational128::compare(int64_t b) const
int b3ConvexHullInternal::Rational128::compare(int64_t b) const
{
if (isInt64)
{
@@ -995,9 +995,9 @@ int btConvexHullInternal::Rational128::compare(int64_t b) const
}
btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
b3ConvexHullInternal::Edge* b3ConvexHullInternal::newEdgePair(Vertex* from, Vertex* to)
{
btAssert(from && to);
b3Assert(from && to);
Edge* e = edgePool.newObject();
Edge* r = edgePool.newObject();
e->reverse = r;
@@ -1016,22 +1016,22 @@ btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vert
return e;
}
bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
bool b3ConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1)
{
Vertex* v0 = h0.maxYx;
Vertex* v1 = h1.minYx;
if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y))
{
btAssert(v0->point.z < v1->point.z);
b3Assert(v0->point.z < v1->point.z);
Vertex* v1p = v1->prev;
if (v1p == v1)
{
c0 = v0;
if (v1->edges)
{
btAssert(v1->edges->next == v1->edges);
b3Assert(v1->edges->next == v1->edges);
v1 = v1->edges->target;
btAssert(v1->edges->next == v1->edges);
b3Assert(v1->edges->next == v1->edges);
}
c1 = v1;
return false;
@@ -1201,7 +1201,7 @@ bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHul
return true;
}
void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
void b3ConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result)
{
int n = end - start;
switch (n)
@@ -1229,7 +1229,7 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
w = v;
v = t;
}
btAssert(v->point.z < w->point.z);
b3Assert(v->point.z < w->point.z);
v->next = v;
v->prev = v;
result.minXy = v;
@@ -1318,7 +1318,7 @@ void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull&
}
#ifdef DEBUG_CONVEX_HULL
void btConvexHullInternal::IntermediateHull::print()
void b3ConvexHullInternal::IntermediateHull::print()
{
printf(" Hull\n");
for (Vertex* v = minXy; v; )
@@ -1355,7 +1355,7 @@ void btConvexHullInternal::IntermediateHull::print()
}
}
void btConvexHullInternal::Vertex::printGraph()
void b3ConvexHullInternal::Vertex::printGraph()
{
print();
printf("\nEdges\n");
@@ -1382,18 +1382,18 @@ void btConvexHullInternal::Vertex::printGraph()
}
#endif
btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
b3ConvexHullInternal::Orientation b3ConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t)
{
btAssert(prev->reverse->target == next->reverse->target);
b3Assert(prev->reverse->target == next->reverse->target);
if (prev->next == next)
{
if (prev->prev == next)
{
Point64 n = t.cross(s);
Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target);
btAssert(!m.isZero());
b3Assert(!m.isZero());
int64_t dot = n.dot(m);
btAssert(dot != 0);
b3Assert(dot != 0);
return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE;
}
return COUNTER_CLOCKWISE;
@@ -1408,7 +1408,7 @@ btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edg
}
}
btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
b3ConvexHullInternal::Edge* b3ConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot)
{
Edge* minEdge = NULL;
@@ -1425,12 +1425,12 @@ btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const V
Point32 t = *e->target - *start;
Rational64 cot(t.dot(sxrxs), t.dot(rxs));
#ifdef DEBUG_CONVEX_HULL
printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN());
printf(" Angle is %f (%d) for ", (float) b3Atan(cot.toScalar()), (int) cot.isNaN());
e->print();
#endif
if (cot.isNaN())
{
btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
b3Assert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0));
}
else
{
@@ -1460,7 +1460,7 @@ btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const V
return minEdge;
}
void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
void b3ConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1)
{
Edge* start0 = e0;
Edge* start1 = e1;
@@ -1469,9 +1469,9 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
Point32 s = c1->point - c0->point;
Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s);
int64_t dist = c0->point.dot(normal);
btAssert(!start1 || (start1->target->point.dot(normal) == dist));
b3Assert(!start1 || (start1->target->point.dot(normal) == dist));
Point64 perp = s.cross(normal);
btAssert(!perp.isZero());
b3Assert(!perp.isZero());
#ifdef DEBUG_CONVEX_HULL
printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1);
@@ -1487,7 +1487,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
{
break;
}
btAssert(e->target->point.dot(normal) == dist);
b3Assert(e->target->point.dot(normal) == dist);
if (e->copy == mergeStamp)
{
break;
@@ -1513,7 +1513,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
{
break;
}
btAssert(e->target->point.dot(normal) == dist);
b3Assert(e->target->point.dot(normal) == dist);
if (e->copy == mergeStamp)
{
break;
@@ -1578,7 +1578,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
}
else
{
btAssert((e1 == start1) && (d1.dot(normal) < 0));
b3Assert((e1 == start1) && (d1.dot(normal) < 0));
}
}
}
@@ -1630,7 +1630,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
}
else
{
btAssert((e0 == start0) && (d0.dot(normal) < 0));
b3Assert((e0 == start0) && (d0.dot(normal) < 0));
}
}
}
@@ -1644,7 +1644,7 @@ void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge
}
void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
void b3ConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
{
if (!h1.maxXy)
{
@@ -1675,7 +1675,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
Point32 s = *c1 - *c0;
Point64 normal = Point32(0, 0, -1).cross(s);
Point64 t = s.cross(normal);
btAssert(!t.isZero());
b3Assert(!t.isZero());
Edge* e = c0->edges;
Edge* start0 = NULL;
@@ -1684,7 +1684,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
do
{
int64_t dot = (*e->target - *c0).dot(normal);
btAssert(dot <= 0);
b3Assert(dot <= 0);
if ((dot == 0) && ((*e->target - *c0).dot(t) > 0))
{
if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE))
@@ -1703,7 +1703,7 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
do
{
int64_t dot = (*e->target - *c1).dot(normal);
btAssert(dot <= 0);
b3Assert(dot <= 0);
if ((dot == 0) && ((*e->target - *c1).dot(t) > 0))
{
if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE))
@@ -1932,12 +1932,12 @@ void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1)
}
static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q)
static bool pointCmp(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))));
}
void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
void b3ConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count)
{
b3Vector3 min(b3Scalar(1e30), b3Scalar(1e30), b3Scalar(1e30)), max(b3Scalar(-1e30), b3Scalar(-1e30), b3Scalar(-1e30));
const char* ptr = (const char*) coords;
@@ -2058,7 +2058,7 @@ void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int st
#endif
}
b3Vector3 btConvexHullInternal::toBtVector(const Point32& v)
b3Vector3 b3ConvexHullInternal::toBtVector(const Point32& v)
{
b3Vector3 p;
p[medAxis] = b3Scalar(v.x);
@@ -2067,12 +2067,12 @@ b3Vector3 btConvexHullInternal::toBtVector(const Point32& v)
return p * scaling;
}
b3Vector3 btConvexHullInternal::getBtNormal(Face* face)
b3Vector3 b3ConvexHullInternal::getBtNormal(Face* face)
{
return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized();
}
b3Vector3 btConvexHullInternal::getCoordinates(const Vertex* v)
b3Vector3 b3ConvexHullInternal::getCoordinates(const Vertex* v)
{
b3Vector3 p;
p[medAxis] = v->xvalue();
@@ -2081,7 +2081,7 @@ b3Vector3 btConvexHullInternal::getCoordinates(const Vertex* v)
return p * scaling + center;
}
b3Scalar btConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
b3Scalar b3ConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
{
if (!vertexList)
{
@@ -2127,7 +2127,7 @@ b3Scalar btConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
if (a && b)
{
int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref));
btAssert(vol >= 0);
b3Assert(vol >= 0);
Point32 c = v->point + a->point + b->point + ref;
hullCenterX += vol * c.x;
hullCenterY += vol * c.y;
@@ -2135,7 +2135,7 @@ b3Scalar btConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
volume += vol;
}
btAssert(f->copy != stamp);
b3Assert(f->copy != stamp);
f->copy = stamp;
f->face = face;
@@ -2182,13 +2182,13 @@ b3Scalar btConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
return 0;
}
amount = btMin(amount, minDist * clampAmount);
amount = b3Min(amount, minDist * clampAmount);
}
unsigned int seed = 243703;
for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223)
{
btSwap(faces[i], faces[seed % faceCount]);
b3Swap(faces[i], faces[seed % faceCount]);
}
for (int i = 0; i < faceCount; i++)
@@ -2202,7 +2202,7 @@ b3Scalar btConvexHullInternal::shrink(b3Scalar amount, b3Scalar clampAmount)
return amount;
}
bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack)
bool b3ConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjectArray<Vertex*> stack)
{
b3Vector3 origShift = getBtNormal(face) * -amount;
if (scaling[0] != 0)
@@ -2230,7 +2230,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
int64_t origDot = face->origin.dot(normal);
Point32 shiftedOrigin = face->origin + shift;
int64_t shiftedDot = shiftedOrigin.dot(normal);
btAssert(shiftedDot <= origDot);
b3Assert(shiftedDot <= origDot);
if (shiftedDot >= origDot)
{
return false;
@@ -2258,7 +2258,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
n++;
#endif
Rational128 dot = e->target->dot(normal);
btAssert(dot.compare(origDot) <= 0);
b3Assert(dot.compare(origDot) <= 0);
#ifdef DEBUG_CONVEX_HULL
printf("Moving downwards, edge is ");
e->print();
@@ -2294,7 +2294,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
n++;
#endif
Rational128 dot = e->target->dot(normal);
btAssert(dot.compare(origDot) <= 0);
b3Assert(dot.compare(origDot) <= 0);
#ifdef DEBUG_CONVEX_HULL
printf("Moving upwards, edge is ");
e->print();
@@ -2426,7 +2426,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
n++;
#endif
e = e->reverse->prev;
btAssert(e != intersection->reverse);
b3Assert(e != intersection->reverse);
cmp = e->target->dot(normal).compare(shiftedDot);
#ifdef DEBUG_CONVEX_HULL
printf("Testing edge ");
@@ -2470,7 +2470,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0);
int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1);
Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10);
btAssert(det.getSign() != 0);
b3Assert(det.getSign() != 0);
Vertex* v = vertexPool.newObject();
v->point.index = -1;
v->copy = -1;
@@ -2568,7 +2568,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
stack.push_back(NULL);
}
btAssert(stack.size() > 0);
b3Assert(stack.size() > 0);
vertexList = stack[0];
#ifdef DEBUG_CONVEX_HULL
@@ -2623,7 +2623,7 @@ bool btConvexHullInternal::shiftFace(Face* face, b3Scalar amount, b3AlignedObjec
}
static int getVertexCopy(btConvexHullInternal::Vertex* vertex, b3AlignedObjectArray<btConvexHullInternal::Vertex*>& vertices)
static int getVertexCopy(b3ConvexHullInternal::Vertex* vertex, b3AlignedObjectArray<b3ConvexHullInternal::Vertex*>& vertices)
{
int index = vertex->copy;
if (index < 0)
@@ -2648,7 +2648,7 @@ b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, in
return 0;
}
btConvexHullInternal hull;
b3ConvexHullInternal hull;
hull.compute(coords, doubleCoords, stride, count);
b3Scalar shift = 0;
@@ -2664,19 +2664,19 @@ b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, in
edges.resize(0);
faces.resize(0);
b3AlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices;
b3AlignedObjectArray<b3ConvexHullInternal::Vertex*> oldVertices;
getVertexCopy(hull.vertexList, oldVertices);
int copied = 0;
while (copied < oldVertices.size())
{
btConvexHullInternal::Vertex* v = oldVertices[copied];
b3ConvexHullInternal::Vertex* v = oldVertices[copied];
vertices.push_back(hull.getCoordinates(v));
btConvexHullInternal::Edge* firstEdge = v->edges;
b3ConvexHullInternal::Edge* firstEdge = v->edges;
if (firstEdge)
{
int firstCopy = -1;
int prevCopy = -1;
btConvexHullInternal::Edge* e = firstEdge;
b3ConvexHullInternal::Edge* e = firstEdge;
do
{
if (e->copy < 0)
@@ -2714,11 +2714,11 @@ b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, in
for (int i = 0; i < copied; i++)
{
btConvexHullInternal::Vertex* v = oldVertices[i];
btConvexHullInternal::Edge* firstEdge = v->edges;
b3ConvexHullInternal::Vertex* v = oldVertices[i];
b3ConvexHullInternal::Edge* firstEdge = v->edges;
if (firstEdge)
{
btConvexHullInternal::Edge* e = firstEdge;
b3ConvexHullInternal::Edge* e = firstEdge;
do
{
if (e->copy >= 0)
@@ -2727,7 +2727,7 @@ b3Scalar b3ConvexHullComputer::compute(const void* coords, bool doubleCoords, in
printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex());
#endif
faces.push_back(e->copy);
btConvexHullInternal::Edge* f = e;
b3ConvexHullInternal::Edge* f = e;
do
{
#ifdef DEBUG_CONVEX_HULL

6
src/Bullet3Geometry/b3ConvexHullComputer.h
View File

@@ -12,8 +12,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_CONVEX_HULL_COMPUTER_H
#define BT_CONVEX_HULL_COMPUTER_H
#ifndef B3_CONVEX_HULL_COMPUTER_H
#define B3_CONVEX_HULL_COMPUTER_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
@@ -99,5 +99,5 @@ class b3ConvexHullComputer
};
#endif //BT_CONVEX_HULL_COMPUTER_H
#endif //B3_CONVEX_HULL_COMPUTER_H

6
src/Bullet3Geometry/b3GeometryUtil.cpp
View File

@@ -24,9 +24,9 @@ subject to the following restrictions:
*/
extern "C"
{
void btBulletMathProbe ();
void b3BulletMathProbe ();
void btBulletMathProbe () {}
void b3BulletMathProbe () {}
}
@@ -160,7 +160,7 @@ void b3GeometryUtil::getVerticesFromPlaneEquations(const b3AlignedObjectArray<b3
b3Scalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > b3Scalar(0.000001))
if (b3Fabs(quotient) > b3Scalar(0.000001))
{
quotient = b3Scalar(-1.) / quotient;
n2n3 *= N1[3];

6
src/Bullet3Geometry/b3GeometryUtil.h
View File

@@ -13,8 +13,8 @@ subject to the following restrictions:
*/
#ifndef BT_GEOMETRY_UTIL_H
#define BT_GEOMETRY_UTIL_H
#ifndef B3_GEOMETRY_UTIL_H
#define B3_GEOMETRY_UTIL_H
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
@@ -38,5 +38,5 @@ class b3GeometryUtil
};
#endif //BT_GEOMETRY_UTIL_H
#endif //B3_GEOMETRY_UTIL_H

12
src/Bullet3Geometry/b3GrahamScan2dConvexHull.h
View File

@@ -33,9 +33,9 @@ struct GrahamVector3 : public b3Vector3
};
struct btAngleCompareFunc {
struct b3AngleCompareFunc {
b3Vector3 m_anchor;
btAngleCompareFunc(const b3Vector3& anchor)
b3AngleCompareFunc(const b3Vector3& anchor)
: m_anchor(anchor)
{
}
@@ -59,7 +59,7 @@ struct btAngleCompareFunc {
inline void GrahamScanConvexHull2D(b3AlignedObjectArray<GrahamVector3>& originalPoints, b3AlignedObjectArray<GrahamVector3>& hull, const b3Vector3& normalAxis)
{
b3Vector3 axis0,axis1;
btPlaneSpace1(normalAxis,axis0,axis1);
b3PlaneSpace1(normalAxis,axis0,axis1);
if (originalPoints.size()<=1)
@@ -87,11 +87,11 @@ inline void GrahamScanConvexHull2D(b3AlignedObjectArray<GrahamVector3>& original
{
b3Vector3 xvec = axis0;
b3Vector3 ar = originalPoints[i]-originalPoints[0];
originalPoints[i].m_angle = btCross(xvec, ar).dot(normalAxis) / ar.length();
originalPoints[i].m_angle = b3Cross(xvec, ar).dot(normalAxis) / ar.length();
}
//step 2: sort all points, based on 'angle' with this anchor
btAngleCompareFunc comp(originalPoints[0]);
b3AngleCompareFunc comp(originalPoints[0]);
originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
int i;
@@ -105,7 +105,7 @@ inline void GrahamScanConvexHull2D(b3AlignedObjectArray<GrahamVector3>& original
while (!isConvex&& hull.size()>1) {
b3Vector3& a = hull[hull.size()-2];
b3Vector3& b = hull[hull.size()-1];
isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
isConvex = b3Cross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
if (!isConvex)
hull.pop_back();
else

518
src/Bullet3Serialize/Bullet2FileLoader/autogenerated/bullet2.h
View File

@@ -27,59 +27,59 @@ typedef struct bInvalidHandle {
}bInvalidHandle;
class PointerArray;
class btPhysicsSystem;
class b3PhysicsSystem;
class ListBase;
class btVector3FloatData;
class btVector3DoubleData;
class btMatrix3x3FloatData;
class btMatrix3x3DoubleData;
class btTransformFloatData;
class btTransformDoubleData;
class btBvhSubtreeInfoData;
class btOptimizedBvhNodeFloatData;
class btOptimizedBvhNodeDoubleData;
class btQuantizedBvhNodeData;
class btQuantizedBvhFloatData;
class btQuantizedBvhDoubleData;
class btCollisionShapeData;
class btStaticPlaneShapeData;
class btConvexInternalShapeData;
class btPositionAndRadius;
class btMultiSphereShapeData;
class btIntIndexData;
class btShortIntIndexData;
class btShortIntIndexTripletData;
class btCharIndexTripletData;
class btMeshPartData;
class btStridingMeshInterfaceData;
class btTriangleMeshShapeData;
class btScaledTriangleMeshShapeData;
class btCompoundShapeChildData;
class btCompoundShapeData;
class btCylinderShapeData;
class btCapsuleShapeData;
class btTriangleInfoData;
class btTriangleInfoMapData;
class btGImpactMeshShapeData;
class btConvexHullShapeData;
class btCollisionObjectDoubleData;
class btCollisionObjectFloatData;
class btDynamicsWorldDoubleData;
class btDynamicsWorldFloatData;
class btRigidBodyFloatData;
class btRigidBodyDoubleData;
class btConstraintInfo1;
class btTypedConstraintData;
class btPoint2PointConstraintFloatData;
class btPoint2PointConstraintDoubleData;
class btHingeConstraintDoubleData;
class btHingeConstraintFloatData;
class btConeTwistConstraintData;
class btGeneric6DofConstraintData;
class btGeneric6DofSpringConstraintData;
class btSliderConstraintData;
class btContactSolverInfoDoubleData;
class btContactSolverInfoFloatData;
class b3Vector3FloatData;
class b3Vector3DoubleData;
class b3Matrix3x3FloatData;
class b3Matrix3x3DoubleData;
class b3TransformFloatData;
class b3TransformDoubleData;
class b3BvhSubtreeInfoData;
class b3OptimizedBvhNodeFloatData;
class b3OptimizedBvhNodeDoubleData;
class b3QuantizedBvhNodeData;
class b3QuantizedBvhFloatData;
class b3QuantizedBvhDoubleData;
class b3CollisionShapeData;
class b3StaticPlaneShapeData;
class b3ConvexInternalShapeData;
class b3PositionAndRadius;
class b3MultiSphereShapeData;
class b3IntIndexData;
class b3ShortIntIndexData;
class b3ShortIntIndexTripletData;
class b3CharIndexTripletData;
class b3MeshPartData;
class b3StridingMeshInterfaceData;
class b3TriangleMeshShapeData;
class b3ScaledTriangleMeshShapeData;
class b3CompoundShapeChildData;
class b3CompoundShapeData;
class b3CylinderShapeData;
class b3CapsuleShapeData;
class b3TriangleInfoData;
class b3TriangleInfoMapData;
class b3GImpactMeshShapeData;
class b3ConvexHullShapeData;
class b3CollisionObjectDoubleData;
class b3CollisionObjectFloatData;
class b3DynamicsWorldDoubleData;
class b3DynamicsWorldFloatData;
class b3RigidBodyFloatData;
class b3RigidBodyDoubleData;
class b3ConstraintInfo1;
class b3TypedConstraintData;
class b3Point2PointConstraintFloatData;
class b3Point2PointConstraintDoubleData;
class b3HingeConstraintDoubleData;
class b3HingeConstraintFloatData;
class b3ConeTwistConstraintData;
class b3Generic6DofConstraintData;
class b3Generic6DofSpringConstraintData;
class b3SliderConstraintData;
class b3ContactSolverInfoDoubleData;
class b3ContactSolverInfoFloatData;
class SoftBodyMaterialData;
class SoftBodyNodeData;
class SoftBodyLinkData;
@@ -89,8 +89,8 @@ typedef struct bInvalidHandle {
class SoftBodyConfigData;
class SoftBodyPoseData;
class SoftBodyClusterData;
class btSoftBodyJointData;
class btSoftBodyFloatData;
class b3SoftBodyJointData;
class b3SoftBodyFloatData;
// -------------------------------------------------- //
class PointerArray
{
@@ -102,7 +102,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btPhysicsSystem
class b3PhysicsSystem
{
public:
PointerArray m_collisionShapes;
@@ -121,7 +121,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btVector3FloatData
class b3Vector3FloatData
{
public:
float m_floats[4];
@@ -129,7 +129,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btVector3DoubleData
class b3Vector3DoubleData
{
public:
double m_floats[4];
@@ -137,41 +137,41 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btMatrix3x3FloatData
class b3Matrix3x3FloatData
{
public:
btVector3FloatData m_el[3];
b3Vector3FloatData m_el[3];
};
// -------------------------------------------------- //
class btMatrix3x3DoubleData
class b3Matrix3x3DoubleData
{
public:
btVector3DoubleData m_el[3];
b3Vector3DoubleData m_el[3];
};
// -------------------------------------------------- //
class btTransformFloatData
class b3TransformFloatData
{
public:
btMatrix3x3FloatData m_basis;
btVector3FloatData m_origin;
b3Matrix3x3FloatData m_basis;
b3Vector3FloatData m_origin;
};
// -------------------------------------------------- //
class btTransformDoubleData
class b3TransformDoubleData
{
public:
btMatrix3x3DoubleData m_basis;
btVector3DoubleData m_origin;
b3Matrix3x3DoubleData m_basis;
b3Vector3DoubleData m_origin;
};
// -------------------------------------------------- //
class btBvhSubtreeInfoData
class b3BvhSubtreeInfoData
{
public:
int m_rootNodeIndex;
@@ -182,11 +182,11 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btOptimizedBvhNodeFloatData
class b3OptimizedBvhNodeFloatData
{
public:
btVector3FloatData m_aabbMinOrg;
btVector3FloatData m_aabbMaxOrg;
b3Vector3FloatData m_aabbMinOrg;
b3Vector3FloatData m_aabbMaxOrg;
int m_escapeIndex;
int m_subPart;
int m_triangleIndex;
@@ -195,11 +195,11 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btOptimizedBvhNodeDoubleData
class b3OptimizedBvhNodeDoubleData
{
public:
btVector3DoubleData m_aabbMinOrg;
btVector3DoubleData m_aabbMaxOrg;
b3Vector3DoubleData m_aabbMinOrg;
b3Vector3DoubleData m_aabbMaxOrg;
int m_escapeIndex;
int m_subPart;
int m_triangleIndex;
@@ -208,7 +208,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btQuantizedBvhNodeData
class b3QuantizedBvhNodeData
{
public:
short m_quantizedAabbMin[3];
@@ -218,45 +218,45 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btQuantizedBvhFloatData
class b3QuantizedBvhFloatData
{
public:
btVector3FloatData m_bvhAabbMin;
btVector3FloatData m_bvhAabbMax;
btVector3FloatData m_bvhQuantization;
b3Vector3FloatData m_bvhAabbMin;
b3Vector3FloatData m_bvhAabbMax;
b3Vector3FloatData m_bvhQuantization;
int m_curNodeIndex;
int m_useQuantization;
int m_numContiguousLeafNodes;
int m_numQuantizedContiguousNodes;
btOptimizedBvhNodeFloatData *m_contiguousNodesPtr;
btQuantizedBvhNodeData *m_quantizedContiguousNodesPtr;
btBvhSubtreeInfoData *m_subTreeInfoPtr;
b3OptimizedBvhNodeFloatData *m_contiguousNodesPtr;
b3QuantizedBvhNodeData *m_quantizedContiguousNodesPtr;
b3BvhSubtreeInfoData *m_subTreeInfoPtr;
int m_traversalMode;
int m_numSubtreeHeaders;
};
// -------------------------------------------------- //
class btQuantizedBvhDoubleData
class b3QuantizedBvhDoubleData
{
public:
btVector3DoubleData m_bvhAabbMin;
btVector3DoubleData m_bvhAabbMax;
btVector3DoubleData m_bvhQuantization;
b3Vector3DoubleData m_bvhAabbMin;
b3Vector3DoubleData m_bvhAabbMax;
b3Vector3DoubleData m_bvhQuantization;
int m_curNodeIndex;
int m_useQuantization;
int m_numContiguousLeafNodes;
int m_numQuantizedContiguousNodes;
btOptimizedBvhNodeDoubleData *m_contiguousNodesPtr;
btQuantizedBvhNodeData *m_quantizedContiguousNodesPtr;
b3OptimizedBvhNodeDoubleData *m_contiguousNodesPtr;
b3QuantizedBvhNodeData *m_quantizedContiguousNodesPtr;
int m_traversalMode;
int m_numSubtreeHeaders;
btBvhSubtreeInfoData *m_subTreeInfoPtr;
b3BvhSubtreeInfoData *m_subTreeInfoPtr;
};
// -------------------------------------------------- //
class btCollisionShapeData
class b3CollisionShapeData
{
public:
char *m_name;
@@ -266,51 +266,51 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btStaticPlaneShapeData
class b3StaticPlaneShapeData
{
public:
btCollisionShapeData m_collisionShapeData;
btVector3FloatData m_localScaling;
btVector3FloatData m_planeNormal;
b3CollisionShapeData m_collisionShapeData;
b3Vector3FloatData m_localScaling;
b3Vector3FloatData m_planeNormal;
float m_planeConstant;
char m_pad[4];
};
// -------------------------------------------------- //
class btConvexInternalShapeData
class b3ConvexInternalShapeData
{
public:
btCollisionShapeData m_collisionShapeData;
btVector3FloatData m_localScaling;
btVector3FloatData m_implicitShapeDimensions;
b3CollisionShapeData m_collisionShapeData;
b3Vector3FloatData m_localScaling;
b3Vector3FloatData m_implicitShapeDimensions;
float m_collisionMargin;
int m_padding;
};
// -------------------------------------------------- //
class btPositionAndRadius
class b3PositionAndRadius
{
public:
btVector3FloatData m_pos;
b3Vector3FloatData m_pos;
float m_radius;
};
// -------------------------------------------------- //
class btMultiSphereShapeData
class b3MultiSphereShapeData
{
public:
btConvexInternalShapeData m_convexInternalShapeData;
btPositionAndRadius *m_localPositionArrayPtr;
b3ConvexInternalShapeData m_convexInternalShapeData;
b3PositionAndRadius *m_localPositionArrayPtr;
int m_localPositionArraySize;
char m_padding[4];
};
// -------------------------------------------------- //
class btIntIndexData
class b3IntIndexData
{
public:
int m_value;
@@ -318,7 +318,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btShortIntIndexData
class b3ShortIntIndexData
{
public:
short m_value;
@@ -327,7 +327,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btShortIntIndexTripletData
class b3ShortIntIndexTripletData
{
public:
short m_values[3];
@@ -336,7 +336,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btCharIndexTripletData
class b3CharIndexTripletData
{
public:
char m_values[3];
@@ -345,98 +345,98 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btMeshPartData
class b3MeshPartData
{
public:
btVector3FloatData *m_vertices3f;
btVector3DoubleData *m_vertices3d;
btIntIndexData *m_indices32;
btShortIntIndexTripletData *m_3indices16;
btCharIndexTripletData *m_3indices8;
btShortIntIndexData *m_indices16;
b3Vector3FloatData *m_vertices3f;
b3Vector3DoubleData *m_vertices3d;
b3IntIndexData *m_indices32;
b3ShortIntIndexTripletData *m_3indices16;
b3CharIndexTripletData *m_3indices8;
b3ShortIntIndexData *m_indices16;
int m_numTriangles;
int m_numVertices;
};
// -------------------------------------------------- //
class btStridingMeshInterfaceData
class b3StridingMeshInterfaceData
{
public:
btMeshPartData *m_meshPartsPtr;
btVector3FloatData m_scaling;
b3MeshPartData *m_meshPartsPtr;
b3Vector3FloatData m_scaling;
int m_numMeshParts;
char m_padding[4];
};
// -------------------------------------------------- //
class btTriangleMeshShapeData
class b3TriangleMeshShapeData
{
public:
btCollisionShapeData m_collisionShapeData;
btStridingMeshInterfaceData m_meshInterface;
btQuantizedBvhFloatData *m_quantizedFloatBvh;
btQuantizedBvhDoubleData *m_quantizedDoubleBvh;
btTriangleInfoMapData *m_triangleInfoMap;
b3CollisionShapeData m_collisionShapeData;
b3StridingMeshInterfaceData m_meshInterface;
b3QuantizedBvhFloatData *m_quantizedFloatBvh;
b3QuantizedBvhDoubleData *m_quantizedDoubleBvh;
b3TriangleInfoMapData *m_triangleInfoMap;
float m_collisionMargin;
char m_pad3[4];
};
// -------------------------------------------------- //
class btScaledTriangleMeshShapeData
class b3ScaledTriangleMeshShapeData
{
public:
btTriangleMeshShapeData m_trimeshShapeData;
btVector3FloatData m_localScaling;
b3TriangleMeshShapeData m_trimeshShapeData;
b3Vector3FloatData m_localScaling;
};
// -------------------------------------------------- //
class btCompoundShapeChildData
class b3CompoundShapeChildData
{
public:
btTransformFloatData m_transform;
btCollisionShapeData *m_childShape;
b3TransformFloatData m_transform;
b3CollisionShapeData *m_childShape;
int m_childShapeType;
float m_childMargin;
};
// -------------------------------------------------- //
class btCompoundShapeData
class b3CompoundShapeData
{
public:
btCollisionShapeData m_collisionShapeData;
btCompoundShapeChildData *m_childShapePtr;
b3CollisionShapeData m_collisionShapeData;
b3CompoundShapeChildData *m_childShapePtr;
int m_numChildShapes;
float m_collisionMargin;
};
// -------------------------------------------------- //
class btCylinderShapeData
class b3CylinderShapeData
{
public:
btConvexInternalShapeData m_convexInternalShapeData;
b3ConvexInternalShapeData m_convexInternalShapeData;
int m_upAxis;
char m_padding[4];
};
// -------------------------------------------------- //
class btCapsuleShapeData
class b3CapsuleShapeData
{
public:
btConvexInternalShapeData m_convexInternalShapeData;
b3ConvexInternalShapeData m_convexInternalShapeData;
int m_upAxis;
char m_padding[4];
};
// -------------------------------------------------- //
class btTriangleInfoData
class b3TriangleInfoData
{
public:
int m_flags;
@@ -447,12 +447,12 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btTriangleInfoMapData
class b3TriangleInfoMapData
{
public:
int *m_hashTablePtr;
int *m_nextPtr;
btTriangleInfoData *m_valueArrayPtr;
b3TriangleInfoData *m_valueArrayPtr;
int *m_keyArrayPtr;
float m_convexEpsilon;
float m_planarEpsilon;
@@ -468,42 +468,42 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btGImpactMeshShapeData
class b3GImpactMeshShapeData
{
public:
btCollisionShapeData m_collisionShapeData;
btStridingMeshInterfaceData m_meshInterface;
btVector3FloatData m_localScaling;
b3CollisionShapeData m_collisionShapeData;
b3StridingMeshInterfaceData m_meshInterface;
b3Vector3FloatData m_localScaling;
float m_collisionMargin;
int m_gimpactSubType;
};
// -------------------------------------------------- //
class btConvexHullShapeData
class b3ConvexHullShapeData
{
public:
btConvexInternalShapeData m_convexInternalShapeData;
btVector3FloatData *m_unscaledPointsFloatPtr;
btVector3DoubleData *m_unscaledPointsDoublePtr;
b3ConvexInternalShapeData m_convexInternalShapeData;
b3Vector3FloatData *m_unscaledPointsFloatPtr;
b3Vector3DoubleData *m_unscaledPointsDoublePtr;
int m_numUnscaledPoints;
char m_padding3[4];
};
// -------------------------------------------------- //
class btCollisionObjectDoubleData
class b3CollisionObjectDoubleData
{
public:
void *m_broadphaseHandle;
void *m_collisionShape;
btCollisionShapeData *m_rootCollisionShape;
b3CollisionShapeData *m_rootCollisionShape;
char *m_name;
btTransformDoubleData m_worldTransform;
btTransformDoubleData m_interpolationWorldTransform;
btVector3DoubleData m_interpolationLinearVelocity;
btVector3DoubleData m_interpolationAngularVelocity;
btVector3DoubleData m_anisotropicFriction;
b3TransformDoubleData m_worldTransform;
b3TransformDoubleData m_interpolationWorldTransform;
b3Vector3DoubleData m_interpolationLinearVelocity;
b3Vector3DoubleData m_interpolationAngularVelocity;
b3Vector3DoubleData m_anisotropicFriction;
double m_contactProcessingThreshold;
double m_deactivationTime;
double m_friction;
@@ -524,18 +524,18 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btCollisionObjectFloatData
class b3CollisionObjectFloatData
{
public:
void *m_broadphaseHandle;
void *m_collisionShape;
btCollisionShapeData *m_rootCollisionShape;
b3CollisionShapeData *m_rootCollisionShape;
char *m_name;
btTransformFloatData m_worldTransform;
btTransformFloatData m_interpolationWorldTransform;
btVector3FloatData m_interpolationLinearVelocity;
btVector3FloatData m_interpolationAngularVelocity;
btVector3FloatData m_anisotropicFriction;
b3TransformFloatData m_worldTransform;
b3TransformFloatData m_interpolationWorldTransform;
b3Vector3FloatData m_interpolationLinearVelocity;
b3Vector3FloatData m_interpolationAngularVelocity;
b3Vector3FloatData m_anisotropicFriction;
float m_contactProcessingThreshold;
float m_deactivationTime;
float m_friction;
@@ -557,20 +557,20 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btRigidBodyFloatData
class b3RigidBodyFloatData
{
public:
btCollisionObjectFloatData m_collisionObjectData;
btMatrix3x3FloatData m_invInertiaTensorWorld;
btVector3FloatData m_linearVelocity;
btVector3FloatData m_angularVelocity;
btVector3FloatData m_angularFactor;
btVector3FloatData m_linearFactor;
btVector3FloatData m_gravity;
btVector3FloatData m_gravity_acceleration;
btVector3FloatData m_invInertiaLocal;
btVector3FloatData m_totalForce;
btVector3FloatData m_totalTorque;
b3CollisionObjectFloatData m_collisionObjectData;
b3Matrix3x3FloatData m_invInertiaTensorWorld;
b3Vector3FloatData m_linearVelocity;
b3Vector3FloatData m_angularVelocity;
b3Vector3FloatData m_angularFactor;
b3Vector3FloatData m_linearFactor;
b3Vector3FloatData m_gravity;
b3Vector3FloatData m_gravity_acceleration;
b3Vector3FloatData m_invInertiaLocal;
b3Vector3FloatData m_totalForce;
b3Vector3FloatData m_totalTorque;
float m_inverseMass;
float m_linearDamping;
float m_angularDamping;
@@ -585,20 +585,20 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btRigidBodyDoubleData
class b3RigidBodyDoubleData
{
public:
btCollisionObjectDoubleData m_collisionObjectData;
btMatrix3x3DoubleData m_invInertiaTensorWorld;
btVector3DoubleData m_linearVelocity;
btVector3DoubleData m_angularVelocity;
btVector3DoubleData m_angularFactor;
btVector3DoubleData m_linearFactor;
btVector3DoubleData m_gravity;
btVector3DoubleData m_gravity_acceleration;
btVector3DoubleData m_invInertiaLocal;
btVector3DoubleData m_totalForce;
btVector3DoubleData m_totalTorque;
b3CollisionObjectDoubleData m_collisionObjectData;
b3Matrix3x3DoubleData m_invInertiaTensorWorld;
b3Vector3DoubleData m_linearVelocity;
b3Vector3DoubleData m_angularVelocity;
b3Vector3DoubleData m_angularFactor;
b3Vector3DoubleData m_linearFactor;
b3Vector3DoubleData m_gravity;
b3Vector3DoubleData m_gravity_acceleration;
b3Vector3DoubleData m_invInertiaLocal;
b3Vector3DoubleData m_totalForce;
b3Vector3DoubleData m_totalTorque;
double m_inverseMass;
double m_linearDamping;
double m_angularDamping;
@@ -614,7 +614,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btConstraintInfo1
class b3ConstraintInfo1
{
public:
int m_numConstraintRows;
@@ -623,7 +623,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btTypedConstraintData
class b3TypedConstraintData
{
public:
bInvalidHandle *m_rbA;
@@ -643,32 +643,32 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btPoint2PointConstraintFloatData
class b3Point2PointConstraintFloatData
{
public:
btTypedConstraintData m_typeConstraintData;
btVector3FloatData m_pivotInA;
btVector3FloatData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3FloatData m_pivotInA;
b3Vector3FloatData m_pivotInB;
};
// -------------------------------------------------- //
class btPoint2PointConstraintDoubleData
class b3Point2PointConstraintDoubleData
{
public:
btTypedConstraintData m_typeConstraintData;
btVector3DoubleData m_pivotInA;
btVector3DoubleData m_pivotInB;
b3TypedConstraintData m_typeConstraintData;
b3Vector3DoubleData m_pivotInA;
b3Vector3DoubleData m_pivotInB;
};
// -------------------------------------------------- //
class btHingeConstraintDoubleData
class b3HingeConstraintDoubleData
{
public:
btTypedConstraintData m_typeConstraintData;
btTransformDoubleData m_rbAFrame;
btTransformDoubleData m_rbBFrame;
b3TypedConstraintData m_typeConstraintData;
b3TransformDoubleData m_rbAFrame;
b3TransformDoubleData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
@@ -683,12 +683,12 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btHingeConstraintFloatData
class b3HingeConstraintFloatData
{
public:
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
b3TypedConstraintData m_typeConstraintData;
b3TransformFloatData m_rbAFrame;
b3TransformFloatData m_rbBFrame;
int m_useReferenceFrameA;
int m_angularOnly;
int m_enableAngularMotor;
@@ -703,12 +703,12 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btConeTwistConstraintData
class b3ConeTwistConstraintData
{
public:
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
b3TypedConstraintData m_typeConstraintData;
b3TransformFloatData m_rbAFrame;
b3TransformFloatData m_rbBFrame;
float m_swingSpan1;
float m_swingSpan2;
float m_twistSpan;
@@ -721,26 +721,26 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btGeneric6DofConstraintData
class b3Generic6DofConstraintData
{
public:
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
btVector3FloatData m_linearUpperLimit;
btVector3FloatData m_linearLowerLimit;
btVector3FloatData m_angularUpperLimit;
btVector3FloatData m_angularLowerLimit;
b3TypedConstraintData m_typeConstraintData;
b3TransformFloatData m_rbAFrame;
b3TransformFloatData m_rbBFrame;
b3Vector3FloatData m_linearUpperLimit;
b3Vector3FloatData m_linearLowerLimit;
b3Vector3FloatData m_angularUpperLimit;
b3Vector3FloatData m_angularLowerLimit;
int m_useLinearReferenceFrameA;
int m_useOffsetForConstraintFrame;
};
// -------------------------------------------------- //
class btGeneric6DofSpringConstraintData
class b3Generic6DofSpringConstraintData
{
public:
btGeneric6DofConstraintData m_6dofData;
b3Generic6DofConstraintData m_6dofData;
int m_springEnabled[6];
float m_equilibriumPoint[6];
float m_springStiffness[6];
@@ -749,12 +749,12 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btSliderConstraintData
class b3SliderConstraintData
{
public:
btTypedConstraintData m_typeConstraintData;
btTransformFloatData m_rbAFrame;
btTransformFloatData m_rbBFrame;
b3TypedConstraintData m_typeConstraintData;
b3TransformFloatData m_rbAFrame;
b3TransformFloatData m_rbBFrame;
float m_linearUpperLimit;
float m_linearLowerLimit;
float m_angularUpperLimit;
@@ -765,7 +765,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btContactSolverInfoDoubleData
class b3ContactSolverInfoDoubleData
{
public:
double m_tau;
@@ -794,7 +794,7 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btContactSolverInfoFloatData
class b3ContactSolverInfoFloatData
{
public:
float m_tau;
@@ -823,20 +823,20 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btDynamicsWorldDoubleData
class b3DynamicsWorldDoubleData
{
public:
btContactSolverInfoDoubleData m_solverInfo;
btVector3DoubleData m_gravity;
b3ContactSolverInfoDoubleData m_solverInfo;
b3Vector3DoubleData m_gravity;
};
// -------------------------------------------------- //
class btDynamicsWorldFloatData
class b3DynamicsWorldFloatData
{
public:
btContactSolverInfoFloatData m_solverInfo;
btVector3FloatData m_gravity;
b3ContactSolverInfoFloatData m_solverInfo;
b3Vector3FloatData m_gravity;
};
@@ -857,11 +857,11 @@ typedef struct bInvalidHandle {
{
public:
SoftBodyMaterialData *m_material;
btVector3FloatData m_position;
btVector3FloatData m_previousPosition;
btVector3FloatData m_velocity;
btVector3FloatData m_accumulatedForce;
btVector3FloatData m_normal;
b3Vector3FloatData m_position;
b3Vector3FloatData m_previousPosition;
b3Vector3FloatData m_velocity;
b3Vector3FloatData m_accumulatedForce;
b3Vector3FloatData m_normal;
float m_inverseMass;
float m_area;
int m_attach;
@@ -884,7 +884,7 @@ typedef struct bInvalidHandle {
class SoftBodyFaceData
{
public:
btVector3FloatData m_normal;
b3Vector3FloatData m_normal;
SoftBodyMaterialData *m_material;
int m_nodeIndices[3];
float m_restArea;
@@ -895,7 +895,7 @@ typedef struct bInvalidHandle {
class SoftBodyTetraData
{
public:
btVector3FloatData m_c0[4];
b3Vector3FloatData m_c0[4];
SoftBodyMaterialData *m_material;
int m_nodeIndices[4];
float m_restVolume;
@@ -909,9 +909,9 @@ typedef struct bInvalidHandle {
class SoftRigidAnchorData
{
public:
btMatrix3x3FloatData m_c0;
btVector3FloatData m_c1;
btVector3FloatData m_localFrame;
b3Matrix3x3FloatData m_c0;
b3Vector3FloatData m_c1;
b3Vector3FloatData m_localFrame;
bInvalidHandle *m_rigidBody;
int m_nodeIndex;
float m_c2;
@@ -955,11 +955,11 @@ typedef struct bInvalidHandle {
class SoftBodyPoseData
{
public:
btMatrix3x3FloatData m_rot;
btMatrix3x3FloatData m_scale;
btMatrix3x3FloatData m_aqq;
btVector3FloatData m_com;
btVector3FloatData *m_positions;
b3Matrix3x3FloatData m_rot;
b3Matrix3x3FloatData m_scale;
b3Matrix3x3FloatData m_aqq;
b3Vector3FloatData m_com;
b3Vector3FloatData *m_positions;
float *m_weights;
int m_numPositions;
int m_numWeigts;
@@ -974,15 +974,15 @@ typedef struct bInvalidHandle {
class SoftBodyClusterData
{
public:
btTransformFloatData m_framexform;
btMatrix3x3FloatData m_locii;
btMatrix3x3FloatData m_invwi;
btVector3FloatData m_com;
btVector3FloatData m_vimpulses[2];
btVector3FloatData m_dimpulses[2];
btVector3FloatData m_lv;
btVector3FloatData m_av;
btVector3FloatData *m_framerefs;
b3TransformFloatData m_framexform;
b3Matrix3x3FloatData m_locii;
b3Matrix3x3FloatData m_invwi;
b3Vector3FloatData m_com;
b3Vector3FloatData m_vimpulses[2];
b3Vector3FloatData m_dimpulses[2];
b3Vector3FloatData m_lv;
b3Vector3FloatData m_av;
b3Vector3FloatData *m_framerefs;
int *m_nodeIndices;
float *m_masses;
int m_numFrameRefs;
@@ -1005,17 +1005,17 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btSoftBodyJointData
class b3SoftBodyJointData
{
public:
void *m_bodyA;
void *m_bodyB;
btVector3FloatData m_refs[2];
b3Vector3FloatData m_refs[2];
float m_cfm;
float m_erp;
float m_split;
int m_delete;
btVector3FloatData m_relPosition[2];
b3Vector3FloatData m_relPosition[2];
int m_bodyAtype;
int m_bodyBtype;
int m_jointType;
@@ -1024,10 +1024,10 @@ typedef struct bInvalidHandle {
// -------------------------------------------------- //
class btSoftBodyFloatData
class b3SoftBodyFloatData
{
public:
btCollisionObjectFloatData m_collisionObjectData;
b3CollisionObjectFloatData m_collisionObjectData;
SoftBodyPoseData *m_pose;
SoftBodyMaterialData **m_materials;
SoftBodyNodeData *m_nodes;
@@ -1036,7 +1036,7 @@ typedef struct bInvalidHandle {
SoftBodyTetraData *m_tetrahedra;
SoftRigidAnchorData *m_anchors;
SoftBodyClusterData *m_clusters;
btSoftBodyJointData *m_joints;
b3SoftBodyJointData *m_joints;
int m_numMaterials;
int m_numNodes;
int m_numLinks;

92
src/Bullet3Serialize/Bullet2FileLoader/btBulletFile.cpp → src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.cpp
View File

@@ -13,7 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btBulletFile.h"
#include "b3BulletFile.h"
#include "bDefines.h"
#include "bDNA.h"
@@ -24,7 +24,7 @@ subject to the following restrictions:
// 32 && 64 bit versions
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef _WIN64
extern char sBulletDNAstr64[];
extern int sBulletDNAlen64;
@@ -32,18 +32,18 @@ extern int sBulletDNAlen64;
extern char sBulletDNAstr[];
extern int sBulletDNAlen;
#endif //_WIN64
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
extern char sBulletDNAstr64[];
extern int sBulletDNAlen64;
extern char sBulletDNAstr[];
extern int sBulletDNAlen;
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif //B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
using namespace bParse;
btBulletFile::btBulletFile()
b3BulletFile::b3BulletFile()
:bFile("", "BULLET ")
{
mMemoryDNA = new bDNA(); //this memory gets released in the bFile::~bFile destructor,@todo not consistent with the rule 'who allocates it, has to deallocate it"
@@ -51,35 +51,35 @@ btBulletFile::btBulletFile()
m_DnaCopy = 0;
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef _WIN64
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64,16);
m_DnaCopy = (char*)b3AlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen64);
#else//_WIN64
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen,16);
m_DnaCopy = (char*)b3AlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen);
#endif//_WIN64
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
m_DnaCopy = (char*) btAlignedAlloc(sBulletDNAlen64,16);
m_DnaCopy = (char*) b3AlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen64);
}
else
{
m_DnaCopy =(char*) btAlignedAlloc(sBulletDNAlen,16);
m_DnaCopy =(char*) b3AlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
mMemoryDNA->init(m_DnaCopy,sBulletDNAlen);
}
#endif//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
}
btBulletFile::btBulletFile(const char* fileName)
b3BulletFile::b3BulletFile(const char* fileName)
:bFile(fileName, "BULLET ")
{
m_DnaCopy = 0;
@@ -87,17 +87,17 @@ btBulletFile::btBulletFile(const char* fileName)
btBulletFile::btBulletFile(char *memoryBuffer, int len)
b3BulletFile::b3BulletFile(char *memoryBuffer, int len)
:bFile(memoryBuffer,len, "BULLET ")
{
m_DnaCopy = 0;
}
btBulletFile::~btBulletFile()
b3BulletFile::~b3BulletFile()
{
if (m_DnaCopy)
btAlignedFree(m_DnaCopy);
b3AlignedFree(m_DnaCopy);
while (m_dataBlocks.size())
@@ -112,7 +112,7 @@ btBulletFile::~btBulletFile()
// ----------------------------------------------------- //
void btBulletFile::parseData()
void b3BulletFile::parseData()
{
// printf ("Building datablocks");
// printf ("Chunk size = %d",CHUNK_HEADER_LEN);
@@ -143,7 +143,7 @@ void btBulletFile::parseData()
while (dataChunk.code != DNA1)
{
if (!brokenDNA || (dataChunk.code != BT_QUANTIZED_BVH_CODE) )
if (!brokenDNA || (dataChunk.code != B3_QUANTIZED_BVH_CODE) )
{
// one behind
@@ -169,42 +169,42 @@ void btBulletFile::parseData()
// listID->push_back((bStructHandle*)id);
}
if (dataChunk.code == BT_SOFTBODY_CODE)
if (dataChunk.code == B3_SOFTBODY_CODE)
{
m_softBodies.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_RIGIDBODY_CODE)
if (dataChunk.code == B3_RIGIDBODY_CODE)
{
m_rigidBodies.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_DYNAMICSWORLD_CODE)
if (dataChunk.code == B3_DYNAMICSWORLD_CODE)
{
m_dynamicsWorldInfo.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_CONSTRAINT_CODE)
if (dataChunk.code == B3_CONSTRAINT_CODE)
{
m_constraints.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_QUANTIZED_BVH_CODE)
if (dataChunk.code == B3_QUANTIZED_BVH_CODE)
{
m_bvhs.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_TRIANLGE_INFO_MAP)
if (dataChunk.code == B3_TRIANLGE_INFO_MAP)
{
m_triangleInfoMaps.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_COLLISIONOBJECT_CODE)
if (dataChunk.code == B3_COLLISIONOBJECT_CODE)
{
m_collisionObjects.push_back((bStructHandle*) id);
}
if (dataChunk.code == BT_SHAPE_CODE)
if (dataChunk.code == B3_SHAPE_CODE)
{
m_collisionShapes.push_back((bStructHandle*) id);
}
@@ -221,7 +221,7 @@ void btBulletFile::parseData()
}
} else
{
printf("skipping BT_QUANTIZED_BVH_CODE due to broken DNA\n");
printf("skipping B3_QUANTIZED_BVH_CODE due to broken DNA\n");
}
@@ -237,7 +237,7 @@ void btBulletFile::parseData()
}
void btBulletFile::addDataBlock(char* dataBlock)
void b3BulletFile::addDataBlock(char* dataBlock)
{
m_dataBlocks.push_back(dataBlock);
@@ -246,14 +246,14 @@ void btBulletFile::addDataBlock(char* dataBlock)
void btBulletFile::writeDNA(FILE* fp)
void b3BulletFile::writeDNA(FILE* fp)
{
bChunkInd dataChunk;
dataChunk.code = DNA1;
dataChunk.dna_nr = 0;
dataChunk.nr = 1;
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
#ifdef _WIN64
@@ -262,7 +262,7 @@ void btBulletFile::writeDNA(FILE* fp)
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr64, sBulletDNAlen64,1,fp);
#else
btAssert(0);
b3Assert(0);
#endif
}
else
@@ -273,10 +273,10 @@ void btBulletFile::writeDNA(FILE* fp)
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr, sBulletDNAlen,1,fp);
#else//_WIN64
btAssert(0);
b3Assert(0);
#endif//_WIN64
}
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
dataChunk.len = sBulletDNAlen64;
@@ -291,24 +291,24 @@ void btBulletFile::writeDNA(FILE* fp)
fwrite(&dataChunk,sizeof(bChunkInd),1,fp);
fwrite(sBulletDNAstr, sBulletDNAlen,1,fp);
}
#endif//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
}
void btBulletFile::parse(int verboseMode)
void b3BulletFile::parse(int verboseMode)
{
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
#ifdef _WIN64
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64,16);
m_DnaCopy = (char*)b3AlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
parseInternal(verboseMode,(char*)sBulletDNAstr64,sBulletDNAlen64);
#else
btAssert(0);
b3Assert(0);
#endif
}
else
@@ -317,19 +317,19 @@ void btBulletFile::parse(int verboseMode)
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen,16);
m_DnaCopy = (char*)b3AlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
parseInternal(verboseMode,m_DnaCopy,sBulletDNAlen);
#else
btAssert(0);
b3Assert(0);
#endif
}
#else//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64,16);
m_DnaCopy = (char*)b3AlignedAlloc(sBulletDNAlen64,16);
memcpy(m_DnaCopy,sBulletDNAstr64,sBulletDNAlen64);
parseInternal(verboseMode,m_DnaCopy,sBulletDNAlen64);
}
@@ -337,11 +337,11 @@ void btBulletFile::parse(int verboseMode)
{
if (m_DnaCopy)
delete m_DnaCopy;
m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen,16);
m_DnaCopy = (char*)b3AlignedAlloc(sBulletDNAlen,16);
memcpy(m_DnaCopy,sBulletDNAstr,sBulletDNAlen);
parseInternal(verboseMode,m_DnaCopy,sBulletDNAlen);
}
#endif//BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif//B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
//the parsing will convert to cpu endian
mFlags &=~FD_ENDIAN_SWAP;
@@ -354,7 +354,7 @@ void btBulletFile::parse(int verboseMode)
}
// experimental
int btBulletFile::write(const char* fileName, bool fixupPointers)
int b3BulletFile::write(const char* fileName, bool fixupPointers)
{
FILE *fp = fopen(fileName, "wb");
if (fp)
@@ -401,7 +401,7 @@ int btBulletFile::write(const char* fileName, bool fixupPointers)
void btBulletFile::addStruct(const char* structType,void* data, int len, void* oldPtr, int code)
void b3BulletFile::addStruct(const char* structType,void* data, int len, void* oldPtr, int code)
{
bParse::bChunkInd dataChunk;

18
src/Bullet3Serialize/Bullet2FileLoader/btBulletFile.h → src/Bullet3Serialize/Bullet2FileLoader/b3BulletFile.h
View File

@@ -13,22 +13,22 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_BULLET_FILE_H
#define BT_BULLET_FILE_H
#ifndef B3_BULLET_FILE_H
#define B3_BULLET_FILE_H
#include "bFile.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "bDefines.h"
#include "Bullet3Serialize/Bullet2FileLoader/btSerializer.h"
#include "Bullet3Serialize/Bullet2FileLoader/b3Serializer.h"
namespace bParse {
// ----------------------------------------------------- //
class btBulletFile : public bFile
class b3BulletFile : public bFile
{
@@ -55,13 +55,13 @@ namespace bParse {
b3AlignedObjectArray<bStructHandle*> m_dynamicsWorldInfo;
b3AlignedObjectArray<char*> m_dataBlocks;
btBulletFile();
b3BulletFile();
btBulletFile(const char* fileName);
b3BulletFile(const char* fileName);
btBulletFile(char *memoryBuffer, int len);
b3BulletFile(char *memoryBuffer, int len);
virtual ~btBulletFile();
virtual ~b3BulletFile();
virtual void addDataBlock(char* dataBlock);
@@ -80,4 +80,4 @@ namespace bParse {
};
};
#endif //BT_BULLET_FILE_H
#endif //B3_BULLET_FILE_H

0
src/Bullet3Serialize/Bullet2FileLoader/btSerializer.cpp → src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.cpp
View File

180
src/Bullet3Serialize/Bullet2FileLoader/btSerializer.h → src/Bullet3Serialize/Bullet2FileLoader/b3Serializer.h
View File

@@ -13,8 +13,8 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SERIALIZER_H
#define BT_SERIALIZER_H
#ifndef B3_SERIALIZER_H
#define B3_SERIALIZER_H
#include "Bullet3Common/b3Scalar.h" // has definitions like SIMD_FORCE_INLINE
#include "Bullet3Common/b3StackAlloc.h"
@@ -33,7 +33,7 @@ extern int sBulletDNAlen;
extern char sBulletDNAstr64[];
extern int sBulletDNAlen64;
SIMD_FORCE_INLINE int btStrLen(const char* str)
SIMD_FORCE_INLINE int b3StrLen(const char* str)
{
if (!str)
return(0);
@@ -49,7 +49,7 @@ SIMD_FORCE_INLINE int btStrLen(const char* str)
}
class btChunk
class b3Chunk
{
public:
int m_chunkCode;
@@ -59,27 +59,27 @@ public:
int m_number;
};
enum btSerializationFlags
enum b3SerializationFlags
{
BT_SERIALIZE_NO_BVH = 1,
BT_SERIALIZE_NO_TRIANGLEINFOMAP = 2,
BT_SERIALIZE_NO_DUPLICATE_ASSERT = 4
B3_SERIALIZE_NO_BVH = 1,
B3_SERIALIZE_NO_TRIANGLEINFOMAP = 2,
B3_SERIALIZE_NO_DUPLICATE_ASSERT = 4
};
class btSerializer
class b3Serializer
{
public:
virtual ~btSerializer() {}
virtual ~b3Serializer() {}
virtual const unsigned char* getBufferPointer() const = 0;
virtual int getCurrentBufferSize() const = 0;
virtual btChunk* allocate(size_t size, int numElements) = 0;
virtual b3Chunk* allocate(size_t size, int numElements) = 0;
virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)= 0;
virtual void finalizeChunk(b3Chunk* chunk, const char* structType, int chunkCode,void* oldPtr)= 0;
virtual void* findPointer(void* oldPtr) = 0;
@@ -104,29 +104,29 @@ public:
#define BT_HEADER_LENGTH 12
#define B3_HEADER_LENGTH 12
#if defined(__sgi) || defined (__sparc) || defined (__sparc__) || defined (__PPC__) || defined (__ppc__) || defined (__BIG_ENDIAN__)
# define BT_MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) )
# define B3_MAKE_ID(a,b,c,d) ( (int)(a)<<24 | (int)(b)<<16 | (c)<<8 | (d) )
#else
# define BT_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) )
# define B3_MAKE_ID(a,b,c,d) ( (int)(d)<<24 | (int)(c)<<16 | (b)<<8 | (a) )
#endif
#define BT_SOFTBODY_CODE BT_MAKE_ID('S','B','D','Y')
#define BT_COLLISIONOBJECT_CODE BT_MAKE_ID('C','O','B','J')
#define BT_RIGIDBODY_CODE BT_MAKE_ID('R','B','D','Y')
#define BT_CONSTRAINT_CODE BT_MAKE_ID('C','O','N','S')
#define BT_BOXSHAPE_CODE BT_MAKE_ID('B','O','X','S')
#define BT_QUANTIZED_BVH_CODE BT_MAKE_ID('Q','B','V','H')
#define BT_TRIANLGE_INFO_MAP BT_MAKE_ID('T','M','A','P')
#define BT_SHAPE_CODE BT_MAKE_ID('S','H','A','P')
#define BT_ARRAY_CODE BT_MAKE_ID('A','R','A','Y')
#define BT_SBMATERIAL_CODE BT_MAKE_ID('S','B','M','T')
#define BT_SBNODE_CODE BT_MAKE_ID('S','B','N','D')
#define BT_DYNAMICSWORLD_CODE BT_MAKE_ID('D','W','L','D')
#define BT_DNA_CODE BT_MAKE_ID('D','N','A','1')
#define B3_SOFTBODY_CODE B3_MAKE_ID('S','B','D','Y')
#define B3_COLLISIONOBJECT_CODE B3_MAKE_ID('C','O','B','J')
#define B3_RIGIDBODY_CODE B3_MAKE_ID('R','B','D','Y')
#define B3_CONSTRAINT_CODE B3_MAKE_ID('C','O','N','S')
#define B3_BOXSHAPE_CODE B3_MAKE_ID('B','O','X','S')
#define B3_QUANTIZED_BVH_CODE B3_MAKE_ID('Q','B','V','H')
#define B3_TRIANLGE_INFO_MAP B3_MAKE_ID('T','M','A','P')
#define B3_SHAPE_CODE B3_MAKE_ID('S','H','A','P')
#define B3_ARRAY_CODE B3_MAKE_ID('A','R','A','Y')
#define B3_SBMATERIAL_CODE B3_MAKE_ID('S','B','M','T')
#define B3_SBNODE_CODE B3_MAKE_ID('S','B','N','D')
#define B3_DYNAMICSWORLD_CODE B3_MAKE_ID('D','W','L','D')
#define B3_DNA_CODE B3_MAKE_ID('D','N','A','1')
struct btPointerUid
struct b3PointerUid
{
union
{
@@ -135,24 +135,24 @@ struct btPointerUid
};
};
///The btDefaultSerializer is the main Bullet serialization class.
///The b3DefaultSerializer is the main Bullet serialization class.
///The constructor takes an optional argument for backwards compatibility, it is recommended to leave this empty/zero.
class btDefaultSerializer : public btSerializer
class b3DefaultSerializer : public b3Serializer
{
b3AlignedObjectArray<char*> mTypes;
b3AlignedObjectArray<short*> mStructs;
b3AlignedObjectArray<short> mTlens;
b3HashMap<btHashInt, int> mStructReverse;
b3HashMap<btHashString,int> mTypeLookup;
b3HashMap<b3HashInt, int> mStructReverse;
b3HashMap<b3HashString,int> mTypeLookup;
b3HashMap<btHashPtr,void*> m_chunkP;
b3HashMap<b3HashPtr,void*> m_chunkP;
b3HashMap<btHashPtr,const char*> m_nameMap;
b3HashMap<b3HashPtr,const char*> m_nameMap;
b3HashMap<btHashPtr,btPointerUid> m_uniquePointers;
b3HashMap<b3HashPtr,b3PointerUid> m_uniquePointers;
int m_uniqueIdGenerator;
int m_totalSize;
@@ -164,7 +164,7 @@ class btDefaultSerializer : public btSerializer
int m_serializationFlags;
b3AlignedObjectArray<btChunk*> m_chunkPtrs;
b3AlignedObjectArray<b3Chunk*> m_chunkPtrs;
protected:
@@ -182,15 +182,15 @@ protected:
void writeDNA()
{
btChunk* dnaChunk = allocate(m_dnaLength,1);
b3Chunk* dnaChunk = allocate(m_dnaLength,1);
memcpy(dnaChunk->m_oldPtr,m_dna,m_dnaLength);
finalizeChunk(dnaChunk,"DNA1",BT_DNA_CODE, m_dna);
finalizeChunk(dnaChunk,"DNA1",B3_DNA_CODE, m_dna);
}
int getReverseType(const char *type) const
{
btHashString key(type);
b3HashString key(type);
const int* valuePtr = mTypeLookup.find(key);
if (valuePtr)
return *valuePtr;
@@ -208,7 +208,7 @@ protected:
littleEndian= ((char*)&littleEndian)[0];
m_dna = btAlignedAlloc(dnalen,16);
m_dna = b3AlignedAlloc(dnalen,16);
memcpy(m_dna,bdnaOrg,dnalen);
m_dnaLength = dnalen;
@@ -233,7 +233,7 @@ protected:
// Parse names
if (!littleEndian)
*intPtr = btSwapEndian(*intPtr);
*intPtr = b3SwapEndian(*intPtr);
dataLen = *intPtr;
@@ -247,7 +247,7 @@ protected:
while (*cp)cp++;
cp++;
}
cp = btAlignPointer(cp,4);
cp = b3AlignPointer(cp,4);
/*
TYPE (4 bytes)
@@ -257,10 +257,10 @@ protected:
*/
intPtr = (int*)cp;
btAssert(strncmp(cp, "TYPE", 4)==0); intPtr++;
b3Assert(strncmp(cp, "TYPE", 4)==0); intPtr++;
if (!littleEndian)
*intPtr = btSwapEndian(*intPtr);
*intPtr = b3SwapEndian(*intPtr);
dataLen = *intPtr;
intPtr++;
@@ -274,7 +274,7 @@ protected:
cp++;
}
cp = btAlignPointer(cp,4);
cp = b3AlignPointer(cp,4);
/*
@@ -285,7 +285,7 @@ protected:
// Parse type lens
intPtr = (int*)cp;
btAssert(strncmp(cp, "TLEN", 4)==0); intPtr++;
b3Assert(strncmp(cp, "TLEN", 4)==0); intPtr++;
dataLen = (int)mTypes.size();
@@ -293,7 +293,7 @@ protected:
for (i=0; i<dataLen; i++, shtPtr++)
{
if (!littleEndian)
shtPtr[0] = btSwapEndian(shtPtr[0]);
shtPtr[0] = b3SwapEndian(shtPtr[0]);
mTlens.push_back(shtPtr[0]);
}
@@ -312,10 +312,10 @@ protected:
intPtr = (int*)shtPtr;
cp = (char*)intPtr;
btAssert(strncmp(cp, "STRC", 4)==0); intPtr++;
b3Assert(strncmp(cp, "STRC", 4)==0); intPtr++;
if (!littleEndian)
*intPtr = btSwapEndian(*intPtr);
*intPtr = b3SwapEndian(*intPtr);
dataLen = *intPtr ;
intPtr++;
@@ -327,16 +327,16 @@ protected:
if (!littleEndian)
{
shtPtr[0]= btSwapEndian(shtPtr[0]);
shtPtr[1]= btSwapEndian(shtPtr[1]);
shtPtr[0]= b3SwapEndian(shtPtr[0]);
shtPtr[1]= b3SwapEndian(shtPtr[1]);
int len = shtPtr[1];
shtPtr+= 2;
for (int a=0; a<len; a++, shtPtr+=2)
{
shtPtr[0]= btSwapEndian(shtPtr[0]);
shtPtr[1]= btSwapEndian(shtPtr[1]);
shtPtr[0]= b3SwapEndian(shtPtr[0]);
shtPtr[1]= b3SwapEndian(shtPtr[1]);
}
} else
@@ -350,7 +350,7 @@ protected:
{
short *strc = mStructs.at(i);
mStructReverse.insert(strc[0], i);
mTypeLookup.insert(btHashString(mTypes[strc[0]]),i);
mTypeLookup.insert(b3HashString(mTypes[strc[0]]),i);
}
}
@@ -359,35 +359,35 @@ public:
btDefaultSerializer(int totalSize=0)
b3DefaultSerializer(int totalSize=0)
:m_totalSize(totalSize),
m_currentSize(0),
m_dna(0),
m_dnaLength(0),
m_serializationFlags(0)
{
m_buffer = m_totalSize?(unsigned char*)btAlignedAlloc(totalSize,16):0;
m_buffer = m_totalSize?(unsigned char*)b3AlignedAlloc(totalSize,16):0;
const bool VOID_IS_8 = ((sizeof(void*)==8));
#ifdef BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#ifdef B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
#if _WIN64
initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64);
#else
btAssert(0);
b3Assert(0);
#endif
} else
{
#ifndef _WIN64
initDNA((const char*)sBulletDNAstr,sBulletDNAlen);
#else
btAssert(0);
b3Assert(0);
#endif
}
#else //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#else //B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
if (VOID_IS_8)
{
initDNA((const char*)sBulletDNAstr64,sBulletDNAlen64);
@@ -395,27 +395,27 @@ public:
{
initDNA((const char*)sBulletDNAstr,sBulletDNAlen);
}
#endif //BT_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
#endif //B3_INTERNAL_UPDATE_SERIALIZATION_STRUCTURES
}
virtual ~btDefaultSerializer()
virtual ~b3DefaultSerializer()
{
if (m_buffer)
btAlignedFree(m_buffer);
b3AlignedFree(m_buffer);
if (m_dna)
btAlignedFree(m_dna);
b3AlignedFree(m_dna);
}
void writeHeader(unsigned char* buffer) const
{
#ifdef BT_USE_DOUBLE_PRECISION
#ifdef B3_USE_DOUBLE_PRECISION
memcpy(buffer, "BULLETd", 7);
#else
memcpy(buffer, "BULLETf", 7);
#endif //BT_USE_DOUBLE_PRECISION
#endif //B3_USE_DOUBLE_PRECISION
int littleEndian= 1;
littleEndian= ((char*)&littleEndian)[0];
@@ -448,7 +448,7 @@ public:
m_uniqueIdGenerator= 1;
if (m_totalSize)
{
unsigned char* buffer = internalAlloc(BT_HEADER_LENGTH);
unsigned char* buffer = internalAlloc(B3_HEADER_LENGTH);
writeHeader(buffer);
}
@@ -463,20 +463,20 @@ public:
if (!m_totalSize)
{
if (m_buffer)
btAlignedFree(m_buffer);
b3AlignedFree(m_buffer);
m_currentSize += BT_HEADER_LENGTH;
m_buffer = (unsigned char*)btAlignedAlloc(m_currentSize,16);
m_currentSize += B3_HEADER_LENGTH;
m_buffer = (unsigned char*)b3AlignedAlloc(m_currentSize,16);
unsigned char* currentPtr = m_buffer;
writeHeader(m_buffer);
currentPtr += BT_HEADER_LENGTH;
mysize+=BT_HEADER_LENGTH;
currentPtr += B3_HEADER_LENGTH;
mysize+=B3_HEADER_LENGTH;
for (int i=0;i< m_chunkPtrs.size();i++)
{
int curLength = sizeof(btChunk)+m_chunkPtrs[i]->m_length;
int curLength = sizeof(b3Chunk)+m_chunkPtrs[i]->m_length;
memcpy(currentPtr,m_chunkPtrs[i], curLength);
btAlignedFree(m_chunkPtrs[i]);
b3AlignedFree(m_chunkPtrs[i]);
currentPtr+=curLength;
mysize+=curLength;
}
@@ -498,14 +498,14 @@ public:
if (!oldPtr)
return 0;
btPointerUid* uptr = (btPointerUid*)m_uniquePointers.find(oldPtr);
b3PointerUid* uptr = (b3PointerUid*)m_uniquePointers.find(oldPtr);
if (uptr)
{
return uptr->m_ptr;
}
m_uniqueIdGenerator++;
btPointerUid uid;
b3PointerUid uid;
uid.m_uniqueIds[0] = m_uniqueIdGenerator;
uid.m_uniqueIds[1] = m_uniqueIdGenerator;
m_uniquePointers.insert(oldPtr,uid);
@@ -523,11 +523,11 @@ public:
return m_currentSize;
}
virtual void finalizeChunk(btChunk* chunk, const char* structType, int chunkCode,void* oldPtr)
virtual void finalizeChunk(b3Chunk* chunk, const char* structType, int chunkCode,void* oldPtr)
{
if (!(m_serializationFlags&BT_SERIALIZE_NO_DUPLICATE_ASSERT))
if (!(m_serializationFlags&B3_SERIALIZE_NO_DUPLICATE_ASSERT))
{
btAssert(!findPointer(oldPtr));
b3Assert(!findPointer(oldPtr));
}
chunk->m_dna_nr = getReverseType(structType);
@@ -550,10 +550,10 @@ public:
{
ptr = m_buffer+m_currentSize;
m_currentSize += int(size);
btAssert(m_currentSize<m_totalSize);
b3Assert(m_currentSize<m_totalSize);
} else
{
ptr = (unsigned char*)btAlignedAlloc(size,16);
ptr = (unsigned char*)b3AlignedAlloc(size,16);
m_currentSize += int(size);
}
return ptr;
@@ -561,14 +561,14 @@ public:
virtual btChunk* allocate(size_t size, int numElements)
virtual b3Chunk* allocate(size_t size, int numElements)
{
unsigned char* ptr = internalAlloc(int(size)*numElements+sizeof(btChunk));
unsigned char* ptr = internalAlloc(int(size)*numElements+sizeof(b3Chunk));
unsigned char* data = ptr + sizeof(btChunk);
unsigned char* data = ptr + sizeof(b3Chunk);
btChunk* chunk = (btChunk*)ptr;
b3Chunk* chunk = (b3Chunk*)ptr;
chunk->m_chunkCode = 0;
chunk->m_oldPtr = data;
chunk->m_length = int(size)*numElements;
@@ -602,7 +602,7 @@ public:
if (findPointer((void*)name))
return;
int len = btStrLen(name);
int len = b3StrLen(name);
if (len)
{
@@ -611,14 +611,14 @@ public:
newLen += padding;
//serialize name string now
btChunk* chunk = allocate(sizeof(char),newLen);
b3Chunk* chunk = allocate(sizeof(char),newLen);
char* destinationName = (char*)chunk->m_oldPtr;
for (int i=0;i<len;i++)
{
destinationName[i] = name[i];
}
destinationName[len] = 0;
finalizeChunk(chunk,"char",BT_ARRAY_CODE,(void*)name);
finalizeChunk(chunk,"char",B3_ARRAY_CODE,(void*)name);
}
}
}
@@ -636,5 +636,5 @@ public:
};
#endif //BT_SERIALIZER_H
#endif //B3_SERIALIZER_H

2
src/Bullet3Serialize/Bullet2FileLoader/bCommon.h
View File

@@ -32,7 +32,7 @@ namespace bParse {
// delete void* undefined
typedef struct bStructHandle {int unused;}bStructHandle;
typedef b3AlignedObjectArray<bStructHandle*> bListBasePtr;
typedef b3HashMap<btHashPtr, bStructHandle*> bPtrMap;
typedef b3HashMap<b3HashPtr, bStructHandle*> bPtrMap;
}

4
src/Bullet3Serialize/Bullet2FileLoader/bDNA.cpp
View File

@@ -93,7 +93,7 @@ int bDNA::getReverseType(short type)
int bDNA::getReverseType(const char *type)
{
btHashString key(type);
b3HashString key(type);
int* valuePtr = mTypeLookup.find(key);
if (valuePtr)
return *valuePtr;
@@ -514,7 +514,7 @@ void bDNA::init(char *data, int len, bool swap)
}
mStructReverse.insert(strc[0], i);
mTypeLookup.insert(btHashString(mTypes[strc[0]]),i);
mTypeLookup.insert(b3HashString(mTypes[strc[0]]),i);
}
}

4
src/Bullet3Serialize/Bullet2FileLoader/bDNA.h
View File

@@ -95,8 +95,8 @@ namespace bParse {
b3AlignedObjectArray<char*> mTypes;
b3AlignedObjectArray<short*> mStructs;
b3AlignedObjectArray<short> mTlens;
b3HashMap<btHashInt, int> mStructReverse;
b3HashMap<btHashString,int> mTypeLookup;
b3HashMap<b3HashInt, int> mStructReverse;
b3HashMap<b3HashString,int> mTypeLookup;
int mPtrLen;

20
src/Bullet3Serialize/Bullet2FileLoader/bFile.cpp
View File

@@ -20,7 +20,7 @@ subject to the following restrictions:
#include <string.h>
#include <stdlib.h>
#include "bDefines.h"
#include "Bullet3Serialize/Bullet2FileLoader/btSerializer.h"
#include "Bullet3Serialize/Bullet2FileLoader/b3Serializer.h"
#include "Bullet3Common/b3AlignedAllocator.h"
#include "Bullet3Common/b3MinMax.h"
@@ -33,7 +33,7 @@ const char* getCleanName(const char* memName, char* buffer)
{
int slen = strlen(memName);
assert(slen<MAX_STRLEN);
slen=btMin(slen,MAX_STRLEN);
slen=b3Min(slen,MAX_STRLEN);
for (int i=0;i<slen;i++)
{
if (memName[i]==']'||memName[i]=='[')
@@ -662,11 +662,11 @@ char* bFile::readStruct(char *head, bChunkInd& dataChunk)
if ((mFlags&FD_BROKEN_DNA)!=0)
{
if ((strcmp(oldType,"btQuantizedBvhNodeData")==0)&&oldLen==20)
if ((strcmp(oldType,"b3QuantizedBvhNodeData")==0)&&oldLen==20)
{
return 0;
}
if ((strcmp(oldType,"btShortIntIndexData")==0))
if ((strcmp(oldType,"b3ShortIntIndexData")==0))
{
int allocLen = 2;
char *dataAlloc = new char[(dataChunk.nr*allocLen)+1];
@@ -935,8 +935,8 @@ void bFile::safeSwapPtr(char *dst, const char *src)
}
else if (ptrMem==4 && ptrFile==8)
{
btPointerUid* oldPtr = (btPointerUid*)src;
btPointerUid* newPtr = (btPointerUid*)dst;
b3PointerUid* oldPtr = (b3PointerUid*)src;
b3PointerUid* newPtr = (b3PointerUid*)dst;
if (oldPtr->m_uniqueIds[0] == oldPtr->m_uniqueIds[1])
{
@@ -957,8 +957,8 @@ void bFile::safeSwapPtr(char *dst, const char *src)
}
else if (ptrMem==8 && ptrFile==4)
{
btPointerUid* oldPtr = (btPointerUid*)src;
btPointerUid* newPtr = (btPointerUid*)dst;
b3PointerUid* oldPtr = (b3PointerUid*)src;
b3PointerUid* newPtr = (b3PointerUid*)dst;
if (oldPtr->m_uniqueIds[0] == oldPtr->m_uniqueIds[1])
{
newPtr->m_uniqueIds[0] = oldPtr->m_uniqueIds[0];
@@ -1192,7 +1192,7 @@ void bFile::resolvePointersMismatch()
int p = 0;
while (blockLen-- > 0)
{
btPointerUid dp = {0};
b3PointerUid dp = {0};
safeSwapPtr((char*)dp.m_uniqueIds, oldPtr);
void **tptr = (void**)(newPtr + p * ptrMem);
@@ -1456,7 +1456,7 @@ void bFile::resolvePointers(int verboseMode)
{
printf("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n");
int numitems = m_chunks.size();
printf("<bullet_physics version=%d itemcount = %d>\n", btGetVersion(), numitems);
printf("<bullet_physics version=%d itemcount = %d>\n", b3GetVersion(), numitems);
}
for (int i=0;i<m_chunks.size();i++)
{

2
src/Bullet3Serialize/Bullet2FileLoader/bFile.h
View File

@@ -66,7 +66,7 @@ namespace bParse {
b3AlignedObjectArray<char*> m_pointerPtrFixupArray;
b3AlignedObjectArray<bChunkInd> m_chunks;
b3HashMap<btHashPtr, bChunkInd> m_chunkPtrPtrMap;
b3HashMap<b3HashPtr, bChunkInd> m_chunkPtrPtrMap;
//