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

Implemented btDiscreteDynamicsWorld::addSpeculativeContacts, using conservative advancement to find contact point ahead of time

Browse Source 
make Extras/ConvexDecomposition thread safe, Issue 501
some improvements to the btInternalEdgeUtility, patch from Issue 501
This commit is contained in:
erwin.coumans
2011-04-07 06:23:34 +00:00
parent 49630e9c77
commit a4e8213ede
8 changed files with 304 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
Extras/ConvexDecomposition/cd_hull.cpp
View File

@@ -992,14 +992,14 @@ Quaternion YawPitchRoll( float yaw, float pitch, float roll )
float Yaw( const Quaternion& q ) float Yaw( const Quaternion& q )
{ {
static float3 v; float3 v;
v=q.ydir(); v=q.ydir();
return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG; return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
} }
float Pitch( const Quaternion& q ) float Pitch( const Quaternion& q )
{ {
static float3 v; float3 v;
v=q.ydir(); v=q.ydir();
return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG; return atan2f(v.z,sqrtf(sqr(v.x)+sqr(v.y)))*RAD2DEG;
} }
@@ -1028,8 +1028,8 @@ float Pitch( const float3& v )
void Plane::Transform(const float3 &position, const Quaternion &orientation) { void Plane::Transform(const float3 &position, const Quaternion &orientation) {
// Transforms the plane to the space defined by the // Transforms the plane to the space defined by the
// given position/orientation. // given position/orientation.
static float3 newnormal; float3 newnormal;
static float3 origin; float3 origin;
newnormal = Inverse(orientation)*normal; newnormal = Inverse(orientation)*normal;
origin = Inverse(orientation)*(-normal*dist - position); origin = Inverse(orientation)*(-normal*dist - position);
@@ -1048,7 +1048,7 @@ void Plane::Transform(const float3 &position, const Quaternion &orientation) {
// returns quaternion q where q*v0==v1. // returns quaternion q where q*v0==v1.
// Routine taken from game programming gems. // Routine taken from game programming gems.
Quaternion RotationArc(float3 v0,float3 v1){ Quaternion RotationArc(float3 v0,float3 v1){
static Quaternion q; Quaternion q;
v0 = normalize(v0); // Comment these two lines out if you know its not needed. v0 = normalize(v0); // Comment these two lines out if you know its not needed.
v1 = normalize(v1); // If vector is already unit length then why do it again? v1 = normalize(v1); // If vector is already unit length then why do it again?
float3 c = cross(v0,v1); float3 c = cross(v0,v1);
@@ -1100,7 +1100,7 @@ float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v)
float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1) float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1)
{ {
// returns the point where the line p0-p1 intersects the plane n&d // returns the point where the line p0-p1 intersects the plane n&d
static float3 dif; float3 dif;
dif = p1-p0; dif = p1-p0;
float dn= dot(plane.normal,dif); float dn= dot(plane.normal,dif);
float t = -(plane.dist+dot(plane.normal,p0) )/dn; float t = -(plane.dist+dot(plane.normal,p0) )/dn;
@@ -1248,7 +1248,7 @@ int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const f
float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint) float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint)
{ {
static float3 cp; float3 cp;
cp = normalize(cross(udir,vdir)); cp = normalize(cross(udir,vdir));
float distu = -dot(cp,ustart); float distu = -dot(cp,ustart);
@@ -1343,7 +1343,7 @@ int PolyHit(const float3 *vert, const int n, const float3 &v0, const float3 &v1,
return 0; return 0;
} }
static float3 the_point; float3 the_point;
// By using the cached plane distances d0 and d1 // By using the cached plane distances d0 and d1
// we can optimize the following: // we can optimize the following:
// the_point = planelineintersection(nrml,dist,v0,v1); // the_point = planelineintersection(nrml,dist,v0,v1);
@@ -1853,9 +1853,9 @@ ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice)
int i; int i;
int vertcountunder=0; int vertcountunder=0;
int vertcountover =0; int vertcountover =0;
static Array<int> vertscoplanar; // existing vertex members of convex that are coplanar Array<int> vertscoplanar; // existing vertex members of convex that are coplanar
vertscoplanar.count=0; vertscoplanar.count=0;
static Array<int> edgesplit; // existing edges that members of convex that cross the splitplane Array<int> edgesplit; // existing edges that members of convex that cross the splitplane
edgesplit.count=0; edgesplit.count=0;
assert(convex.edges.count<480); assert(convex.edges.count<480);
@@ -2332,7 +2332,7 @@ int shareedge(const int3 &a,const int3 &b)
class btHullTriangle; class btHullTriangle;
Array<btHullTriangle*> tris; //Array<btHullTriangle*> tris;
class btHullTriangle : public int3 class btHullTriangle : public int3
{ {
@@ -2341,17 +2341,19 @@ public:
int id; int id;
int vmax; int vmax;
float rise; float rise;
btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) Array<btHullTriangle*>* tris;
btHullTriangle(int a,int b,int c, Array<btHullTriangle*>* pTris):int3(a,b,c),n(-1,-1,-1)
{ {
id = tris.count; tris = pTris;
tris.Add(this); id = tris->count;
tris->Add(this);
vmax=-1; vmax=-1;
rise = 0.0f; rise = 0.0f;
} }
~btHullTriangle() ~btHullTriangle()
{ {
assert(tris[id]==this); assert((*tris)[id]==this);
tris[id]=NULL; (*tris)[id]=NULL;
} }
int &neib(int a,int b); int &neib(int a,int b);
}; };
@@ -2371,7 +2373,7 @@ int &btHullTriangle::neib(int a,int b)
assert(0); assert(0);
return er; return er;
} }
void b2bfix(btHullTriangle* s,btHullTriangle*t) void b2bfix(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
{ {
int i; int i;
for(i=0;i<3;i++) for(i=0;i<3;i++)
@@ -2387,14 +2389,14 @@ void b2bfix(btHullTriangle* s,btHullTriangle*t)
} }
} }
void removeb2b(btHullTriangle* s,btHullTriangle*t) void removeb2b(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
{ {
b2bfix(s,t); b2bfix(s,t, tris);
delete s; delete s;
delete t; delete t;
} }
void checkit(btHullTriangle *t) void checkit(btHullTriangle *t, Array<btHullTriangle*>& tris)
{ {
int i; int i;
assert(tris[t->id]==t); assert(tris[t->id]==t);
@@ -2408,30 +2410,30 @@ void checkit(btHullTriangle *t)
assert( tris[t->n[i]]->neib(b,a) == t->id); assert( tris[t->n[i]]->neib(b,a) == t->id);
} }
} }
void extrude(btHullTriangle *t0,int v) void extrude(btHullTriangle *t0,int v, Array<btHullTriangle*>& tris)
{ {
int3 t= *t0; int3 t= *t0;
int n = tris.count; int n = tris.count;
btHullTriangle* ta = new btHullTriangle(v,t[1],t[2]); btHullTriangle* ta = new btHullTriangle(v,t[1],t[2], &tris);
ta->n = int3(t0->n[0],n+1,n+2); ta->n = int3(t0->n[0],n+1,n+2);
tris[t0->n[0]]->neib(t[1],t[2]) = n+0; tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
btHullTriangle* tb = new btHullTriangle(v,t[2],t[0]); btHullTriangle* tb = new btHullTriangle(v,t[2],t[0], &tris);
tb->n = int3(t0->n[1],n+2,n+0); tb->n = int3(t0->n[1],n+2,n+0);
tris[t0->n[1]]->neib(t[2],t[0]) = n+1; tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
btHullTriangle* tc = new btHullTriangle(v,t[0],t[1]); btHullTriangle* tc = new btHullTriangle(v,t[0],t[1], &tris);
tc->n = int3(t0->n[2],n+0,n+1); tc->n = int3(t0->n[2],n+0,n+1);
tris[t0->n[2]]->neib(t[0],t[1]) = n+2; tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
checkit(ta); checkit(ta, tris);
checkit(tb); checkit(tb, tris);
checkit(tc); checkit(tc, tris);
if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]); if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]], tris);
if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]); if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]], tris);
if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]); if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]], tris);
delete t0; delete t0;
} }
btHullTriangle *extrudable(float epsilon) btHullTriangle *extrudable(float epsilon, Array<btHullTriangle*>& tris)
{ {
int i; int i;
btHullTriangle *t=NULL; btHullTriangle *t=NULL;
@@ -2487,7 +2489,7 @@ int4 FindSimplex(float3 *verts,int verts_count,Array<int> &allow)
return int4(p0,p1,p2,p3); return int4(p0,p1,p2,p3);
} }
int calchullgen(float3 *verts,int verts_count, int vlimit) int calchullgen(float3 *verts,int verts_count, int vlimit,Array<btHullTriangle*>& tris)
{ {
if(verts_count <4) return 0; if(verts_count <4) return 0;
if(vlimit==0) vlimit=1000000000; if(vlimit==0) vlimit=1000000000;
@@ -2511,12 +2513,12 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
float3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f; // a valid interior point float3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f; // a valid interior point
btHullTriangle *t0 = new btHullTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); btHullTriangle *t0 = new btHullTriangle(p[2],p[3],p[1], &tris); t0->n=int3(2,3,1);
btHullTriangle *t1 = new btHullTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); btHullTriangle *t1 = new btHullTriangle(p[3],p[2],p[0], &tris); t1->n=int3(3,2,0);
btHullTriangle *t2 = new btHullTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); btHullTriangle *t2 = new btHullTriangle(p[0],p[1],p[3], &tris); t2->n=int3(0,1,3);
btHullTriangle *t3 = new btHullTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); btHullTriangle *t3 = new btHullTriangle(p[1],p[0],p[2], &tris); t3->n=int3(1,0,2);
isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
checkit(t0);checkit(t1);checkit(t2);checkit(t3); checkit(t0, tris);checkit(t1, tris);checkit(t2, tris);checkit(t3, tris);
for(j=0;j<tris.count;j++) for(j=0;j<tris.count;j++)
{ {
@@ -2529,7 +2531,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
} }
btHullTriangle *te; btHullTriangle *te;
vlimit-=4; vlimit-=4;
while(vlimit >0 && (te=extrudable(epsilon))) while(vlimit >0 && (te=extrudable(epsilon, tris)))
{ {
int3 ti=*te; int3 ti=*te;
int v=te->vmax; int v=te->vmax;
@@ -2542,7 +2544,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
int3 t=*tris[j]; int3 t=*tris[j];
if(above(verts,t,verts[v],0.01f*epsilon)) if(above(verts,t,verts[v],0.01f*epsilon))
{ {
extrude(tris[j],v); extrude(tris[j],v, tris);
} }
} }
// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
@@ -2556,7 +2558,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
{ {
btHullTriangle *nb = tris[tris[j]->n[0]]; btHullTriangle *nb = tris[tris[j]->n[0]];
assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j); assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j);
extrude(nb,v); extrude(nb,v, tris);
j=tris.count; j=tris.count;
} }
} }
@@ -2582,9 +2584,9 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
return 1; return 1;
} }
int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit) int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit, Array<btHullTriangle*>& tris)
{ {
int rc=calchullgen(verts,verts_count, vlimit) ; int rc=calchullgen(verts,verts_count, vlimit, tris) ;
if(!rc) return 0; if(!rc) return 0;
Array<int> ts; Array<int> ts;
for(int i=0;i<tris.count;i++)if(tris[i]) for(int i=0;i<tris.count;i++)if(tris[i])
@@ -2599,11 +2601,11 @@ int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int
return 1; return 1;
} }
int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle) int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris)
{ {
int i,j; int i,j;
planes.count=0; planes.count=0;
int rc = calchullgen(verts,verts_count,vlimit); int rc = calchullgen(verts,verts_count,vlimit, tris);
if(!rc) return 0; if(!rc) return 0;
for(i=0;i<tris.count;i++)if(tris[i]) for(i=0;i<tris.count;i++)if(tris[i])
{ {
@@ -2702,18 +2704,18 @@ int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int m
} }
int overhullv(float3 *verts, int verts_count,int maxplanes, int overhullv(float3 *verts, int verts_count,int maxplanes,
float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate,float bevangle,int vlimit) float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate,float bevangle,int vlimit, Array<btHullTriangle*>& tris)
{ {
if(!verts_count) return 0; if(!verts_count) return 0;
extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle) ; extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array<Plane> &planes,float bevangle, Array<btHullTriangle*>& tris) ;
Array<Plane> planes; Array<Plane> planes;
int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle) ; int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle, tris) ;
if(!rc) return 0; if(!rc) return 0;
return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate); return overhull(planes.element,planes.count,verts,verts_count,maxplanes,verts_out,verts_count_out,faces_out,faces_count_out,inflate);
} }
bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,unsigned int vlimit,float inflate) bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,unsigned int vlimit,float inflate, Array<btHullTriangle*>& arrtris)
{ {
int index_count; int index_count;
@@ -2725,7 +2727,7 @@ bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,u
{ {
int *tris_out; int *tris_out;
int tris_count; int tris_count;
int ret = calchull( (float3 *) vertices, (int) vcount, tris_out, tris_count, vlimit ); int ret = calchull( (float3 *) vertices, (int) vcount, tris_out, tris_count, vlimit, arrtris );
if(!ret) return false; if(!ret) return false;
result.mIndexCount = (unsigned int) (tris_count*3); result.mIndexCount = (unsigned int) (tris_count*3);
result.mFaceCount = (unsigned int) tris_count; result.mFaceCount = (unsigned int) tris_count;
@@ -2735,7 +2737,7 @@ bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,u
return true; return true;
} }
int ret = overhullv((float3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit); int ret = overhullv((float3*)vertices,vcount,35,verts_out,verts_count_out,faces,index_count,inflate,120.0f,vlimit, arrtris);
if(!ret) return false; if(!ret) return false;
Array<int3> tris; Array<int3> tris;
@@ -2826,7 +2828,8 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
if ( desc.HasHullFlag(QF_SKIN_WIDTH) ) if ( desc.HasHullFlag(QF_SKIN_WIDTH) )
skinwidth = desc.mSkinWidth; skinwidth = desc.mSkinWidth;
ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth); Array<btHullTriangle*> tris;
ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth, tris);
if ( ok ) if ( ok )
{ {

2
Extras/ConvexDecomposition/cd_vector.h
View File

@@ -797,7 +797,7 @@ public:
else else
{ {
static Vector3d axis[3]; Vector3d axis[3];
axis[0].NearestPointInLine(triangle0, triangle1, triangle2); axis[0].NearestPointInLine(triangle0, triangle1, triangle2);
axis[1].NearestPointInLine(triangle1, triangle0, triangle2); axis[1].NearestPointInLine(triangle1, triangle0, triangle2);
axis[2].NearestPointInLine(triangle2, triangle0, triangle1); axis[2].NearestPointInLine(triangle2, triangle0, triangle1);

2
Extras/ConvexDecomposition/cd_wavefront.cpp
View File

@@ -450,7 +450,7 @@ const char ** InPlaceParser::GetArglist(char *line,int &count) // convert source
{ {
const char **ret = 0; const char **ret = 0;
static const char *argv[MAXARGS]; const char *argv[MAXARGS];
int argc = 0; int argc = 0;
char *foo = line; char *foo = line;

59
Extras/ConvexDecomposition/vlookup.cpp
View File

@@ -143,49 +143,70 @@ public:
float mPos[3]; float mPos[3];
}; };
typedef std::vector< VertexPosition > VertexVector;
class VertexLess struct Tracker
{ {
public: VertexPosition mFind; // vertice to locate.
typedef std::vector< VertexPosition > VertexVector; VertexVector *mList;
bool operator()(int v1,int v2) const; Tracker()
{
mList = 0;
}
static void SetSearch(const VertexPosition& match,VertexVector *list) void SetSearch(const VertexPosition& match,VertexVector *list)
{ {
mFind = match; mFind = match;
mList = list; mList = list;
}; };
};
struct VertexID
{
int mID;
Tracker* mTracker;
VertexID(int ID, Tracker* Tracker)
{
mID = ID;
mTracker = Tracker;
}
};
class VertexLess
{
public:
bool operator()(VertexID v1,VertexID v2) const;
private: private:
const VertexPosition& Get(int index) const const VertexPosition& Get(VertexID index) const
{ {
if ( index == -1 ) return mFind; if ( index.mID == -1 ) return index.mTracker->mFind;
VertexVector &vlist = *mList; VertexVector &vlist = *index.mTracker->mList;
return vlist[index]; return vlist[index.mID];
} }
static VertexPosition mFind; // vertice to locate.
static VertexVector *mList;
}; };
template <class Type> class VertexPool template <class Type> class VertexPool
{ {
public: public:
typedef std::set<int, VertexLess > VertexSet; typedef std::set<VertexID, VertexLess > VertexSet;
typedef std::vector< Type > VertexVector; typedef std::vector< Type > VertexVector;
int getVertex(const Type& vtx) int getVertex(const Type& vtx)
{ {
VertexLess::SetSearch(vtx,&mVtxs); mTracker.SetSearch(vtx,&mVtxs);
VertexSet::iterator found; VertexSet::iterator found;
found = mVertSet.find( -1 ); found = mVertSet.find( VertexID(-1,&mTracker) );
if ( found != mVertSet.end() ) if ( found != mVertSet.end() )
{ {
return *found; return found->mID;
} }
int idx = (int)mVtxs.size(); int idx = (int)mVtxs.size();
mVtxs.push_back( vtx ); mVtxs.push_back( vtx );
mVertSet.insert( idx ); mVertSet.insert( VertexID(idx,&mTracker) );
return idx; return idx;
}; };
@@ -233,13 +254,11 @@ public:
private: private:
VertexSet mVertSet; // ordered list. VertexSet mVertSet; // ordered list.
VertexVector mVtxs; // set of vertices. VertexVector mVtxs; // set of vertices.
Tracker mTracker;
}; };
VertexPosition VertexLess::mFind; bool VertexLess::operator()(VertexID v1,VertexID v2) const
std::vector<VertexPosition > *VertexLess::mList=0;
bool VertexLess::operator()(int v1,int v2) const
{ {
const VertexPosition& a = Get(v1); const VertexPosition& a = Get(v1);

96
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
View File

@@ -1,6 +1,7 @@
#include "btInternalEdgeUtility.h" #include "btInternalEdgeUtility.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
@@ -9,7 +10,6 @@
//#define DEBUG_INTERNAL_EDGE //#define DEBUG_INTERNAL_EDGE
#ifdef DEBUG_INTERNAL_EDGE #ifdef DEBUG_INTERNAL_EDGE
#include <stdio.h> #include <stdio.h>
#endif //DEBUG_INTERNAL_EDGE #endif //DEBUG_INTERNAL_EDGE
@@ -456,8 +456,14 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return; return;
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); btBvhTriangleMeshShape* trimesh = 0;
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if( colObj0->getRootCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0->getRootCollisionShape())->getChildShape();
else
trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr) if (!triangleInfoMapPtr)
return; return;
@@ -501,14 +507,63 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.normalize();//is this necessary? localContactNormalOnB.normalize();//is this necessary?
if ((info->m_edgeV0V1Angle)< SIMD_2_PI) // Get closest edge
int bestedge=-1;
float disttobestedge=BT_LARGE_FLOAT;
//
// Edge 0 -> 1
if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
float len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=0;
disttobestedge=len;
}
}
// Edge 1 -> 2
if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
float len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=1;
disttobestedge=len;
}
}
// Edge 2 -> 0
if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
float len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=2;
disttobestedge=len;
}
}
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
#endif
//if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{ {
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif #endif
btScalar len = (contact-nearest).length(); btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) //if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==0 )
{ {
btVector3 edge(v0-v1); btVector3 edge(v0-v1);
isNearEdge = true; isNearEdge = true;
@@ -577,7 +632,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
if ((info->m_edgeV1V2Angle)< SIMD_2_PI) #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
#endif
//if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{ {
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
@@ -586,7 +645,8 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
btScalar len = (contact-nearest).length(); btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) //if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==1 )
{ {
isNearEdge = true; isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
@@ -658,8 +718,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
#endif
if ((info->m_edgeV2V0Angle)< SIMD_2_PI) //if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{ {
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
@@ -667,7 +730,8 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW #endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btScalar len = (contact-nearest).length(); btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) //if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==2 )
{ {
isNearEdge = true; isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
@@ -759,11 +823,17 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal; cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
} else } else
{ {
btVector3 newNormal = tri_normal *frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
btScalar d = newNormal.dot(localContactNormalOnB) ;
if (d< 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal) //modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal;
} }
// Reproject collision point along normal. // Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);

2
src/BulletCollision/CollisionShapes/btTriangleInfoMap.h
View File

@@ -61,6 +61,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap
btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle
btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared'
btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge"
btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold
btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold)
@@ -71,6 +72,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap
m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001); m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001);
m_edgeDistanceThreshold = btScalar(0.1); m_edgeDistanceThreshold = btScalar(0.1);
m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001);
m_maxEdgeAngleThreshold = SIMD_2_PI;
} }
virtual ~btTriangleInfoMap() {} virtual ~btTriangleInfoMap() {}

124
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -46,6 +46,13 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h" #include "LinearMath/btSerializer.h"
#if 0
btAlignedObjectArray<btVector3> debugContacts;
btAlignedObjectArray<btVector3> debugNormals;
int startHit=2;
int firstHit=startHit;
#endif
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
@@ -314,9 +321,13 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
dispatchInfo.m_stepCount = 0; dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer(); dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection ///perform collision detection
performDiscreteCollisionDetection(); performDiscreteCollisionDetection();
addSpeculativeContacts(timeStep);
calculateSimulationIslands(); calculateSimulationIslands();
@@ -745,12 +756,13 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
{ {
public:
btCollisionObject* m_me; btCollisionObject* m_me;
btScalar m_allowedPenetration; btScalar m_allowedPenetration;
btOverlappingPairCache* m_pairCache; btOverlappingPairCache* m_pairCache;
btDispatcher* m_dispatcher; btDispatcher* m_dispatcher;
public: public:
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btCollisionWorld::ClosestConvexResultCallback(fromA,toA), btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
@@ -828,7 +840,6 @@ public:
///internal debugging variable. this value shouldn't be too high ///internal debugging variable. this value shouldn't be too high
int gNumClampedCcdMotions=0; int gNumClampedCcdMotions=0;
//#include "stdio.h"
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{ {
BT_PROFILE("integrateTransforms"); BT_PROFILE("integrateTransforms");
@@ -875,6 +886,115 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
} }
} }
void btDiscreteDynamicsWorld::addSpeculativeContacts(btScalar timeStep)
{
BT_PROFILE("addSpeculativeContacts");
btTransform predictedTrans;
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
body->predictIntegratedTransform(timeStep, predictedTrans);
btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("search speculative contacts");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
btTransform modifiedPredictedTrans;
modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
btBroadphaseProxy* proxy0 = body->getBroadphaseHandle();
btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle();
btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1);
if (pair)
{
if (pair->m_algorithm)
{
btManifoldArray contacts;
pair->m_algorithm->getAllContactManifolds(contacts);
if (contacts.size())
{
btManifoldResult result(body,sweepResults.m_hitCollisionObject);
result.setPersistentManifold(contacts[0]);
btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin());
vec*=sweepResults.m_closestHitFraction;
btScalar lenSqr = vec.length2();
btScalar depth = 0.f;
btVector3 pointWorld = sweepResults.m_hitPointWorld;
if (lenSqr>SIMD_EPSILON)
{
depth = btSqrt(lenSqr);
pointWorld -= vec;
vec /= depth;
}
if (contacts[0]->getBody0()==body)
{
result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth);
#if 0
debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld);
debugNormals.push_back(sweepResults.m_hitNormalWorld);
#endif
} else
{
//swapped
result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth);
//sweepResults.m_hitPointWorld,depth);
#if 0
if (1)//firstHit==1)
{
firstHit=0;
debugNormals.push_back(sweepResults.m_hitNormalWorld);
debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld);
debugNormals.push_back(sweepResults.m_hitNormalWorld);
debugContacts.push_back(sweepResults.m_hitPointWorld);
}
firstHit--;
#endif
}
}
} else
{
//no algorithm, use dispatcher to create one
}
} else
{
//add an overlapping pair
//printf("pair missing\n");
}
}
}
}
}
}
}

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -62,6 +62,8 @@ protected:
virtual void integrateTransforms(btScalar timeStep); virtual void integrateTransforms(btScalar timeStep);
virtual void addSpeculativeContacts(btScalar timeStep);
virtual void calculateSimulationIslands(); virtual void calculateSimulationIslands();
virtual void solveConstraints(btContactSolverInfo& solverInfo); virtual void solveConstraints(btContactSolverInfo& solverInfo);