diff --git a/Extras/ConvexDecomposition/cd_hull.cpp b/Extras/ConvexDecomposition/cd_hull.cpp index b2f6d8b24..b85b64941 100644 --- a/Extras/ConvexDecomposition/cd_hull.cpp +++ b/Extras/ConvexDecomposition/cd_hull.cpp @@ -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 vertscoplanar; // existing vertex members of convex that are coplanar + Array vertscoplanar; // existing vertex members of convex that are coplanar vertscoplanar.count=0; - static Array edgesplit; // existing edges that members of convex that cross the splitplane + Array 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 tris; +//Array 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* tris; + btHullTriangle(int a,int b,int c, Array* 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& 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& tris) { - b2bfix(s,t); + b2bfix(s,t, tris); delete s; delete t; } -void checkit(btHullTriangle *t) +void checkit(btHullTriangle *t, Array& 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& 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& tris) { int i; btHullTriangle *t=NULL; @@ -2487,7 +2489,7 @@ int4 FindSimplex(float3 *verts,int verts_count,Array &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& 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;j0 && (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& tris) { - int rc=calchullgen(verts,verts_count, vlimit) ; + int rc=calchullgen(verts,verts_count, vlimit, tris) ; if(!rc) return 0; Array ts; for(int i=0;i &planes,float bevangle) +int calchullpbev(float3 *verts,int verts_count,int vlimit, Array &planes,float bevangle, Array& 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) { if(!verts_count) return 0; - extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array &planes,float bevangle) ; + extern int calchullpbev(float3 *verts,int verts_count,int vlimit, Array &planes,float bevangle, Array& tris) ; Array 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& 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 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 tris; + ok = ComputeHull(ovcount,vsource,hr,desc.mMaxVertices,skinwidth, tris); if ( ok ) { diff --git a/Extras/ConvexDecomposition/cd_vector.h b/Extras/ConvexDecomposition/cd_vector.h index 382ba7534..bd1eff25b 100644 --- a/Extras/ConvexDecomposition/cd_vector.h +++ b/Extras/ConvexDecomposition/cd_vector.h @@ -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); diff --git a/Extras/ConvexDecomposition/cd_wavefront.cpp b/Extras/ConvexDecomposition/cd_wavefront.cpp index 98d2bd25c..54e8bdf68 100644 --- a/Extras/ConvexDecomposition/cd_wavefront.cpp +++ b/Extras/ConvexDecomposition/cd_wavefront.cpp @@ -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; diff --git a/Extras/ConvexDecomposition/vlookup.cpp b/Extras/ConvexDecomposition/vlookup.cpp index 8ce4e452e..3b9e928ca 100644 --- a/Extras/ConvexDecomposition/vlookup.cpp +++ b/Extras/ConvexDecomposition/vlookup.cpp @@ -143,49 +143,70 @@ public: float mPos[3]; }; +typedef std::vector< VertexPosition > VertexVector; -class VertexLess +struct Tracker { -public: - typedef std::vector< VertexPosition > VertexVector; + VertexPosition mFind; // vertice to locate. + 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; 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 VertexPool { public: - typedef std::set VertexSet; + typedef std::set 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 *VertexLess::mList=0; - -bool VertexLess::operator()(int v1,int v2) const +bool VertexLess::operator()(VertexID v1,VertexID v2) const { const VertexPosition& a = Get(v1); diff --git a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 5cceb04db..83295016e 100644 --- a/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -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 #endif //DEBUG_INTERNAL_EDGE @@ -456,8 +456,14 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + 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; @@ -501,14 +507,63 @@ 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(lenm_edgeDistanceThreshold) + //if(lenm_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(lenm_edgeDistanceThreshold) + //if(lenm_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(lenm_edgeDistanceThreshold) + //if(lenm_edgeDistanceThreshold) + if( bestedge==2 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -759,11 +823,17 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal; } 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) - cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); + cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal; } - - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); diff --git a/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h index 282a7702e..5aadfc670 100644 --- a/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ b/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -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() {} diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 761152d56..ce5f6f746 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -46,6 +46,13 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" +#if 0 +btAlignedObjectArray debugContacts; +btAlignedObjectArray 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;isetHitFraction(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(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast(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"); + + } + } + } + } + + } + } +} + diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index df47c2904..3ba39a1c4 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -62,6 +62,8 @@ protected: virtual void integrateTransforms(btScalar timeStep); + virtual void addSpeculativeContacts(btScalar timeStep); + virtual void calculateSimulationIslands(); virtual void solveConstraints(btContactSolverInfo& solverInfo);