Implemented btDiscreteDynamicsWorld::addSpeculativeContacts, using conservative advancement to find contact point ahead of time
make Extras/ConvexDecomposition thread safe, Issue 501 some improvements to the btInternalEdgeUtility, patch from Issue 501
This commit is contained in:
erwin.coumans
@@ -992,14 +992,14 @@ Quaternion YawPitchRoll( float yaw, float pitch, float roll )
|
||||
|
||||
float Yaw( const Quaternion& q )
|
||||
{
|
||||
static float3 v;
|
||||
float3 v;
|
||||
v=q.ydir();
|
||||
return (v.y==0.0&&v.x==0.0) ? 0.0f: atan2f(-v.x,v.y)*RAD2DEG;
|
||||
}
|
||||
|
||||
float Pitch( const Quaternion& q )
|
||||
{
|
||||
static float3 v;
|
||||
float3 v;
|
||||
v=q.ydir();
|
||||
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) {
|
||||
// Transforms the plane to the space defined by the
|
||||
// given position/orientation.
|
||||
static float3 newnormal;
|
||||
static float3 origin;
|
||||
float3 newnormal;
|
||||
float3 origin;
|
||||
|
||||
newnormal = Inverse(orientation)*normal;
|
||||
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.
|
||||
// Routine taken from game programming gems.
|
||||
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.
|
||||
v1 = normalize(v1); // If vector is already unit length then why do it again?
|
||||
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)
|
||||
{
|
||||
// returns the point where the line p0-p1 intersects the plane n&d
|
||||
static float3 dif;
|
||||
float3 dif;
|
||||
dif = p1-p0;
|
||||
float dn= dot(plane.normal,dif);
|
||||
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)
|
||||
{
|
||||
static float3 cp;
|
||||
float3 cp;
|
||||
cp = normalize(cross(udir,vdir));
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
static float3 the_point;
|
||||
float3 the_point;
|
||||
// By using the cached plane distances d0 and d1
|
||||
// we can optimize the following:
|
||||
// the_point = planelineintersection(nrml,dist,v0,v1);
|
||||
@@ -1853,9 +1853,9 @@ ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice)
|
||||
int i;
|
||||
int vertcountunder=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;
|
||||
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;
|
||||
|
||||
assert(convex.edges.count<480);
|
||||
@@ -2332,7 +2332,7 @@ int shareedge(const int3 &a,const int3 &b)
|
||||
|
||||
class btHullTriangle;
|
||||
|
||||
Array<btHullTriangle*> tris;
|
||||
//Array<btHullTriangle*> tris;
|
||||
|
||||
class btHullTriangle : public int3
|
||||
{
|
||||
@@ -2341,17 +2341,19 @@ public:
|
||||
int id;
|
||||
int vmax;
|
||||
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.Add(this);
|
||||
tris = pTris;
|
||||
id = tris->count;
|
||||
tris->Add(this);
|
||||
vmax=-1;
|
||||
rise = 0.0f;
|
||||
}
|
||||
~btHullTriangle()
|
||||
{
|
||||
assert(tris[id]==this);
|
||||
tris[id]=NULL;
|
||||
assert((*tris)[id]==this);
|
||||
(*tris)[id]=NULL;
|
||||
}
|
||||
int &neib(int a,int b);
|
||||
};
|
||||
@@ -2371,7 +2373,7 @@ int &btHullTriangle::neib(int a,int b)
|
||||
assert(0);
|
||||
return er;
|
||||
}
|
||||
void b2bfix(btHullTriangle* s,btHullTriangle*t)
|
||||
void b2bfix(btHullTriangle* s,btHullTriangle*t, Array<btHullTriangle*>& tris)
|
||||
{
|
||||
int 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 t;
|
||||
}
|
||||
|
||||
void checkit(btHullTriangle *t)
|
||||
void checkit(btHullTriangle *t, Array<btHullTriangle*>& tris)
|
||||
{
|
||||
int i;
|
||||
assert(tris[t->id]==t);
|
||||
@@ -2408,30 +2410,30 @@ void checkit(btHullTriangle *t)
|
||||
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;
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
|
||||
checkit(ta);
|
||||
checkit(tb);
|
||||
checkit(tc);
|
||||
if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]);
|
||||
if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]);
|
||||
if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]);
|
||||
checkit(ta, tris);
|
||||
checkit(tb, tris);
|
||||
checkit(tc, tris);
|
||||
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]], tris);
|
||||
if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]], tris);
|
||||
delete t0;
|
||||
|
||||
}
|
||||
|
||||
btHullTriangle *extrudable(float epsilon)
|
||||
btHullTriangle *extrudable(float epsilon, Array<btHullTriangle*>& tris)
|
||||
{
|
||||
int i;
|
||||
btHullTriangle *t=NULL;
|
||||
@@ -2487,7 +2489,7 @@ int4 FindSimplex(float3 *verts,int verts_count,Array<int> &allow)
|
||||
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(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
|
||||
btHullTriangle *t0 = new btHullTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
|
||||
btHullTriangle *t1 = new btHullTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
|
||||
btHullTriangle *t2 = new btHullTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
|
||||
btHullTriangle *t3 = new btHullTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
|
||||
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], &tris); t1->n=int3(3,2,0);
|
||||
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], &tris); t3->n=int3(1,0,2);
|
||||
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++)
|
||||
{
|
||||
@@ -2529,7 +2531,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
|
||||
}
|
||||
btHullTriangle *te;
|
||||
vlimit-=4;
|
||||
while(vlimit >0 && (te=extrudable(epsilon)))
|
||||
while(vlimit >0 && (te=extrudable(epsilon, tris)))
|
||||
{
|
||||
int3 ti=*te;
|
||||
int v=te->vmax;
|
||||
@@ -2542,7 +2544,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
|
||||
int3 t=*tris[j];
|
||||
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
|
||||
@@ -2556,7 +2558,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
|
||||
{
|
||||
btHullTriangle *nb = tris[tris[j]->n[0]];
|
||||
assert(nb);assert(!hasvert(*nb,v));assert(nb->id<j);
|
||||
extrude(nb,v);
|
||||
extrude(nb,v, tris);
|
||||
j=tris.count;
|
||||
}
|
||||
}
|
||||
@@ -2582,9 +2584,9 @@ int calchullgen(float3 *verts,int verts_count, int vlimit)
|
||||
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;
|
||||
Array<int> ts;
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
planes.count=0;
|
||||
int rc = calchullgen(verts,verts_count,vlimit);
|
||||
int rc = calchullgen(verts,verts_count,vlimit, tris);
|
||||
if(!rc) return 0;
|
||||
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,
|
||||
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;
|
||||
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;
|
||||
int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle) ;
|
||||
int rc=calchullpbev(verts,verts_count,vlimit,planes,bevangle, tris) ;
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
@@ -2725,7 +2727,7 @@ bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,u
|
||||
{
|
||||
int *tris_out;
|
||||
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;
|
||||
result.mIndexCount = (unsigned int) (tris_count*3);
|
||||
result.mFaceCount = (unsigned int) tris_count;
|
||||
@@ -2735,7 +2737,7 @@ bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,u
|
||||
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;
|
||||
|
||||
Array<int3> tris;
|
||||
@@ -2826,7 +2828,8 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, //
|
||||
if ( desc.HasHullFlag(QF_SKIN_WIDTH) )
|
||||
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 )
|
||||
{
|
||||
|
||||
@@ -797,7 +797,7 @@ public:
|
||||
|
||||
else
|
||||
{
|
||||
static Vector3d axis[3];
|
||||
Vector3d axis[3];
|
||||
axis[0].NearestPointInLine(triangle0, triangle1, triangle2);
|
||||
axis[1].NearestPointInLine(triangle1, triangle0, triangle2);
|
||||
axis[2].NearestPointInLine(triangle2, triangle0, triangle1);
|
||||
|
||||
@@ -450,7 +450,7 @@ const char ** InPlaceParser::GetArglist(char *line,int &count) // convert source
|
||||
{
|
||||
const char **ret = 0;
|
||||
|
||||
static const char *argv[MAXARGS];
|
||||
const char *argv[MAXARGS];
|
||||
int argc = 0;
|
||||
|
||||
char *foo = line;
|
||||
|
||||
@@ -143,49 +143,70 @@ public:
|
||||
float mPos[3];
|
||||
};
|
||||
|
||||
|
||||
class VertexLess
|
||||
{
|
||||
public:
|
||||
typedef std::vector< VertexPosition > VertexVector;
|
||||
|
||||
bool operator()(int v1,int v2) const;
|
||||
struct Tracker
|
||||
{
|
||||
VertexPosition mFind; // vertice to locate.
|
||||
VertexVector *mList;
|
||||
|
||||
static void SetSearch(const VertexPosition& match,VertexVector *list)
|
||||
Tracker()
|
||||
{
|
||||
mList = 0;
|
||||
}
|
||||
|
||||
void SetSearch(const VertexPosition& match,VertexVector *list)
|
||||
{
|
||||
mFind = match;
|
||||
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:
|
||||
const VertexPosition& Get(int index) const
|
||||
const VertexPosition& Get(VertexID index) const
|
||||
{
|
||||
if ( index == -1 ) return mFind;
|
||||
VertexVector &vlist = *mList;
|
||||
return vlist[index];
|
||||
if ( index.mID == -1 ) return index.mTracker->mFind;
|
||||
VertexVector &vlist = *index.mTracker->mList;
|
||||
return vlist[index.mID];
|
||||
}
|
||||
static VertexPosition mFind; // vertice to locate.
|
||||
static VertexVector *mList;
|
||||
};
|
||||
|
||||
template <class Type> class VertexPool
|
||||
{
|
||||
public:
|
||||
typedef std::set<int, VertexLess > VertexSet;
|
||||
typedef std::set<VertexID, VertexLess > VertexSet;
|
||||
typedef std::vector< Type > VertexVector;
|
||||
|
||||
int getVertex(const Type& vtx)
|
||||
{
|
||||
VertexLess::SetSearch(vtx,&mVtxs);
|
||||
mTracker.SetSearch(vtx,&mVtxs);
|
||||
VertexSet::iterator found;
|
||||
found = mVertSet.find( -1 );
|
||||
found = mVertSet.find( VertexID(-1,&mTracker) );
|
||||
if ( found != mVertSet.end() )
|
||||
{
|
||||
return *found;
|
||||
return found->mID;
|
||||
}
|
||||
int idx = (int)mVtxs.size();
|
||||
mVtxs.push_back( vtx );
|
||||
mVertSet.insert( idx );
|
||||
mVertSet.insert( VertexID(idx,&mTracker) );
|
||||
return idx;
|
||||
};
|
||||
|
||||
@@ -233,13 +254,11 @@ public:
|
||||
private:
|
||||
VertexSet mVertSet; // ordered list.
|
||||
VertexVector mVtxs; // set of vertices.
|
||||
Tracker mTracker;
|
||||
};
|
||||
|
||||
|
||||
VertexPosition VertexLess::mFind;
|
||||
std::vector<VertexPosition > *VertexLess::mList=0;
|
||||
|
||||
bool VertexLess::operator()(int v1,int v2) const
|
||||
bool VertexLess::operator()(VertexID v1,VertexID v2) const
|
||||
{
|
||||
|
||||
const VertexPosition& a = Get(v1);
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#include "btInternalEdgeUtility.h"
|
||||
|
||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
|
||||
@@ -9,7 +10,6 @@
|
||||
|
||||
//#define DEBUG_INTERNAL_EDGE
|
||||
|
||||
|
||||
#ifdef DEBUG_INTERNAL_EDGE
|
||||
#include <stdio.h>
|
||||
#endif //DEBUG_INTERNAL_EDGE
|
||||
@@ -456,7 +456,13 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
|
||||
if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
|
||||
return;
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
|
||||
btBvhTriangleMeshShape* trimesh = 0;
|
||||
|
||||
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)
|
||||
return;
|
||||
@@ -502,13 +508,62 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
|
||||
btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
|
||||
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
|
||||
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
|
||||
#endif
|
||||
btScalar len = (contact-nearest).length();
|
||||
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
|
||||
//if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
|
||||
if( bestedge==0 )
|
||||
{
|
||||
btVector3 edge(v0-v1);
|
||||
isNearEdge = true;
|
||||
@@ -577,7 +632,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
|
||||
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
|
||||
#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
|
||||
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();
|
||||
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
|
||||
//if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
|
||||
if( bestedge==1 )
|
||||
{
|
||||
isNearEdge = true;
|
||||
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
@@ -658,8 +718,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
|
||||
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
|
||||
#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
|
||||
@@ -667,7 +730,8 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
|
||||
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
|
||||
btScalar len = (contact-nearest).length();
|
||||
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
|
||||
//if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
|
||||
if( bestedge==2 )
|
||||
{
|
||||
isNearEdge = true;
|
||||
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
|
||||
@@ -759,10 +823,16 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
|
||||
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
|
||||
} else
|
||||
{
|
||||
//modify the normal to be the triangle normal (or backfacing normal)
|
||||
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing);
|
||||
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)
|
||||
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal;
|
||||
}
|
||||
|
||||
|
||||
// Reproject collision point along normal.
|
||||
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
|
||||
|
||||
@@ -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_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_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)
|
||||
|
||||
|
||||
@@ -71,6 +72,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap
|
||||
m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001);
|
||||
m_edgeDistanceThreshold = btScalar(0.1);
|
||||
m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001);
|
||||
m_maxEdgeAngleThreshold = SIMD_2_PI;
|
||||
}
|
||||
virtual ~btTriangleInfoMap() {}
|
||||
|
||||
|
||||
@@ -46,6 +46,13 @@ subject to the following restrictions:
|
||||
|
||||
#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)
|
||||
@@ -314,9 +321,13 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
|
||||
dispatchInfo.m_stepCount = 0;
|
||||
dispatchInfo.m_debugDraw = getDebugDrawer();
|
||||
|
||||
|
||||
///perform collision detection
|
||||
performDiscreteCollisionDetection();
|
||||
|
||||
addSpeculativeContacts(timeStep);
|
||||
|
||||
|
||||
calculateSimulationIslands();
|
||||
|
||||
|
||||
@@ -745,12 +756,13 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
|
||||
|
||||
class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
|
||||
{
|
||||
public:
|
||||
|
||||
btCollisionObject* m_me;
|
||||
btScalar m_allowedPenetration;
|
||||
btOverlappingPairCache* m_pairCache;
|
||||
btDispatcher* m_dispatcher;
|
||||
|
||||
|
||||
public:
|
||||
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
|
||||
btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
|
||||
@@ -828,7 +840,6 @@ public:
|
||||
///internal debugging variable. this value shouldn't be too high
|
||||
int gNumClampedCcdMotions=0;
|
||||
|
||||
//#include "stdio.h"
|
||||
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||
{
|
||||
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");
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -62,6 +62,8 @@ protected:
|
||||
|
||||
virtual void integrateTransforms(btScalar timeStep);
|
||||
|
||||
virtual void addSpeculativeContacts(btScalar timeStep);
|
||||
|
||||
virtual void calculateSimulationIslands();
|
||||
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
Reference in New Issue
Block a user