diff --git a/Demos3/GpuDemos/rigidbody/GpuConvexScene.h b/Demos3/GpuDemos/rigidbody/GpuConvexScene.h index 7e6d970de..7a79e5760 100644 --- a/Demos3/GpuDemos/rigidbody/GpuConvexScene.h +++ b/Demos3/GpuDemos/rigidbody/GpuConvexScene.h @@ -72,7 +72,7 @@ public: virtual ~GpuBoxPlaneScene(){} virtual const char* getName() { - return "BoxPlane"; + return "BoxBox"; } static GpuDemo* MyCreateFunc() diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h index 4ec51ded1..a27cb620c 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h +++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h @@ -18,6 +18,9 @@ subject to the following restrictions: #include "Bullet3Common/b3Int4.h" +#define B3_NEW_PAIR_MARKER -1 +#define B3_REMOVED_PAIR_MARKER -2 + //typedef b3Int2 b3BroadphasePair; struct b3BroadphasePair : public b3Int4 { @@ -34,6 +37,8 @@ struct b3BroadphasePair : public b3Int4 x = yy; y = xx; } + z = B3_NEW_PAIR_MARKER; + w = B3_NEW_PAIR_MARKER; } }; diff --git a/src/Bullet3Common/b3Vector3.h b/src/Bullet3Common/b3Vector3.h index be9efd3de..aa053297b 100644 --- a/src/Bullet3Common/b3Vector3.h +++ b/src/Bullet3Common/b3Vector3.h @@ -992,7 +992,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_ #if defined (B3_USE_SSE) || defined (B3_USE_NEON) #if defined _WIN32 || defined (B3_USE_SSE) const long scalar_cutoff = 10; - long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + long b3_maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); #elif defined B3_USE_NEON const long scalar_cutoff = 4; extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut ); @@ -1020,7 +1020,7 @@ B3_FORCE_INLINE long b3Vector3::maxDot( const b3Vector3 *array, long array_ return ptIndex; } #if defined (B3_USE_SSE) || defined (B3_USE_NEON) - return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); + return b3_maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); #endif } @@ -1029,10 +1029,10 @@ B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_ #if defined (B3_USE_SSE) || defined (B3_USE_NEON) #if defined B3_USE_SSE const long scalar_cutoff = 10; - long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + long b3_mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut ); #elif defined B3_USE_NEON const long scalar_cutoff = 4; - extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut ); + extern long (*b3_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut ); #else #error unhandled arch! #endif @@ -1060,7 +1060,7 @@ B3_FORCE_INLINE long b3Vector3::minDot( const b3Vector3 *array, long array_ return ptIndex; } #if defined (B3_USE_SSE) || defined (B3_USE_NEON) - return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); + return b3_mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut ); #endif } diff --git a/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl b/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl index f2f273663..74baa30bd 100644 --- a/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl +++ b/src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl @@ -13,6 +13,7 @@ subject to the following restrictions: */ //Originally written by Erwin Coumans +#define NEW_PAIR_MARKER -1 typedef struct { @@ -87,6 +88,9 @@ __kernel void computePairsKernelTwoArrays( __global const btAabbCL* unsortedAa myPair.x = xIndex; myPair.y = yIndex; + myPair.z = NEW_PAIR_MARKER; + myPair.w = NEW_PAIR_MARKER; + int curPair = atomic_inc (pairCount); if (curPair* bodyBuf, - b3AlignedObjectArray* contactOut, + b3AlignedObjectArray* globalContactOut, int& nContacts, const b3AlignedObjectArray& hostConvexDataA, @@ -1443,9 +1448,10 @@ void clipHullHullSingle( const b3AlignedObjectArray& hostCollidablesA, const b3AlignedObjectArray& hostCollidablesB, - const b3Vector3& sepNormalWorldSpace ) + const b3Vector3& sepNormalWorldSpace, + int maxContactCapacity ) { - + int contactIndex = -1; b3ConvexPolyhedronCL hullA, hullB; b3Collidable colA = hostCollidablesA[collidableIndexA]; @@ -1459,7 +1465,7 @@ void clipHullHullSingle( float4 contactsOut[MAX_VERTS]; - int contactCapacity = MAX_VERTS; + int localContactCapacity = MAX_VERTS; #ifdef _WIN32 b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x)); @@ -1507,7 +1513,7 @@ void clipHullHullSingle( (float4*)&verticesA[0], &facesA[0],&indicesA[0], (float4*)&verticesB[0], &facesB[0],&indicesB[0], - contactsOut,contactCapacity); + contactsOut,localContactCapacity); if (numContactsOut>0) { @@ -1531,32 +1537,42 @@ void clipHullHullSingle( b3Assert(numPoints); - - contactOut->expand(); - b3Contact4& contact = contactOut->at(nContacts); - contact.m_batchIdx = 0;//i; - contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; - contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; - - contact.m_frictionCoeffCmp = 45874; - contact.m_restituitionCoeffCmp = 0; - - float distance = 0.f; - for (int p=0;pexpand(); + b3Contact4& contact = globalContactOut->at(nContacts); + contact.m_batchIdx = 0;//i; + contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + contact.m_frictionCoeffCmp = 45874; + contact.m_restituitionCoeffCmp = 0; + + float distance = 0.f; + for (int p=0;p& rigidBodies, @@ -1568,43 +1584,98 @@ void computeContactConvexConvex( const b3AlignedObjectArray& faces, b3AlignedObjectArray& globalContactsOut, int& nGlobalContactsOut, - int maxContactCapacity) -{ + int maxContactCapacity, + const b3AlignedObjectArray& oldContacts + ) +{ + int contactIndex = -1; + b3VoronoiSimplexSolver simplexSolver; + b3GjkEpaSolver2 epaSolver; + + b3GjkPairDetector gjkDetector(&simplexSolver,&epaSolver); + b3Transform transA; + transA.setOrigin(rigidBodies[bodyIndexA].m_pos); + transA.setRotation(rigidBodies[bodyIndexA].m_quat); + + b3Transform transB; + transB.setOrigin(rigidBodies[bodyIndexB].m_pos); + transB.setRotation(rigidBodies[bodyIndexB].m_quat); + float maximumDistanceSquared = 1e30f; + + b3Vector3 resultPointOnB; + b3Vector3 sepAxis2(0,1,0); + b3Scalar distance2 = 1e30f; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + bool result2 = getClosestPoints(&gjkDetector, transA, transB, + convexShapes[shapeIndexA], convexShapes[shapeIndexB], + convexVertices,convexVertices, + maximumDistanceSquared, + sepAxis2, + distance2, + resultPointOnB); + + if (result2) + { + if (nGlobalContactsOut& rigidBodies, + const b3AlignedObjectArray& collidables, + const b3AlignedObjectArray& convexShapes, + const b3AlignedObjectArray& convexVertices, + const b3AlignedObjectArray& uniqueEdges, + const b3AlignedObjectArray& convexIndices, + const b3AlignedObjectArray& faces, + b3AlignedObjectArray& globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity, + const b3AlignedObjectArray& oldContacts + ) +{ + int contactIndex = -1; b3Vector3 posA = rigidBodies[bodyIndexA].m_pos; b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; b3Vector3 posB = rigidBodies[bodyIndexB].m_pos; b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; - /*int bodyIndexA, int bodyIndexB, - const float4& posA, - const float4& ornA, - const float4& posB, - const float4& ornB, - int collidableIndexA, int collidableIndexB, - - const b3AlignedObjectArray* bodyBuf, - b3AlignedObjectArray* contactOut, - int& nContacts, - - const b3AlignedObjectArray& hostConvexDataA, - const b3AlignedObjectArray& hostConvexDataB, - - const b3AlignedObjectArray& verticesA, - const b3AlignedObjectArray& uniqueEdgesA, - const b3AlignedObjectArray& facesA, - const b3AlignedObjectArray& indicesA, - - const b3AlignedObjectArray& verticesB, - const b3AlignedObjectArray& uniqueEdgesB, - const b3AlignedObjectArray& facesB, - const b3AlignedObjectArray& indicesB, - - const b3AlignedObjectArray& hostCollidablesA, - const b3AlignedObjectArray& hostCollidablesB - ) - */ - b3ConvexPolyhedronCL hullA, hullB; @@ -1649,7 +1720,7 @@ void computeContactConvexConvex( { - clipHullHullSingle( + contactIndex = clipHullHullSingle( bodyIndexA, bodyIndexB, posA,ornA, posB,ornB, @@ -1673,17 +1744,19 @@ void computeContactConvexConvex( collidables, collidables, - sepNormalWorldSpace); + sepNormalWorldSpace, + maxContactCapacity); } - + return contactIndex; } -void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray* pairs, int nPairs, +void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs, int nPairs, const b3OpenCLArray* bodyBuf, b3OpenCLArray* contactOut, int& nContacts, + const b3OpenCLArray* oldContacts, int maxContactCapacity, int compoundPairCapacity, const b3OpenCLArray& convexData, @@ -1754,6 +1827,13 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArraycopyToHost(hostContacts); } + b3AlignedObjectArray oldHostContacts; + if (oldContacts->size()) + { + oldContacts->copyToHost(oldHostContacts); + } + + hostContacts.resize(nPairs); for (int i=0;i=0) + { + hostPairs[i].z = contactIndex; + } // printf("plane-convex\n"); } @@ -1828,6 +1915,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArraycopyFromHost(hostPairs); + } + if (nContacts) { hostContacts.resize(nContacts); @@ -2380,7 +2472,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const b3OpenCLArray= maxContactCapacity) + { + b3Error("Exceeded contact capacity (%d/%d)\n",nContacts,maxContactCapacity); + nContacts = maxContactCapacity; + } contactOut->resize(nContacts); } diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h index 326acec66..a78063766 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h @@ -75,9 +75,10 @@ struct GpuSatCollision virtual ~GpuSatCollision(); - void computeConvexConvexContactsGPUSAT( const b3OpenCLArray* pairs, int nPairs, + void computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs, int nPairs, const b3OpenCLArray* bodyBuf, b3OpenCLArray* contactOut, int& nContacts, + const b3OpenCLArray* oldContacts, int maxContactCapacity, int compoundPairCapacity, const b3OpenCLArray& hostConvexData, diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp new file mode 100644 index 000000000..6d7cf4c1c --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.cpp @@ -0,0 +1,1011 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ + +#include "b3GjkEpa.h" + +#include "b3SupportMappings.h" + +namespace gjkepa2_impl +{ + + // Config + + /* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((b3Scalar)0.0001) +#define GJK_MIN_DISTANCE ((b3Scalar)0.0001) +#define GJK_DUPLICATED_EPS ((b3Scalar)0.0001) +#define GJK_SIMPLEX2_EPS ((b3Scalar)0.0) +#define GJK_SIMPLEX3_EPS ((b3Scalar)0.0) +#define GJK_SIMPLEX4_EPS ((b3Scalar)0.0) + + /* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((b3Scalar)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((b3Scalar)0.00001) +#define EPA_INSIDE_EPS ((b3Scalar)0.01) + + + // Shorthands + + + // MinkowskiDiff + struct MinkowskiDiff + { + + + const b3ConvexPolyhedronCL* m_shapes[2]; + + + b3Matrix3x3 m_toshape1; + b3Transform m_toshape0; + + bool m_enableMargin; + + + void EnableMargin(bool enable) + { + m_enableMargin = enable; + } + inline b3Vector3 Support0(const b3Vector3& d, const b3AlignedObjectArray& verticesA) const + { + if (m_enableMargin) + { + return localGetSupportVertexWithMargin(d,m_shapes[0],verticesA,0.f); + } else + { + return localGetSupportVertexWithoutMargin(d,m_shapes[0],verticesA); + } + } + inline b3Vector3 Support1(const b3Vector3& d, const b3AlignedObjectArray& verticesB) const + { + if (m_enableMargin) + { + return m_toshape0*(localGetSupportVertexWithMargin(m_toshape1*d,m_shapes[1],verticesB,0.f)); + } else + { + return m_toshape0*(localGetSupportVertexWithoutMargin(m_toshape1*d,m_shapes[1],verticesB)); + } + } + + inline b3Vector3 Support(const b3Vector3& d, const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB) const + { + return(Support0(d,verticesA)-Support1(-d,verticesB)); + } + b3Vector3 Support(const b3Vector3& d,unsigned int index,const b3AlignedObjectArray& verticesA, const b3AlignedObjectArray& verticesB) const + { + if(index) + return(Support1(d,verticesA)); + else + return(Support0(d,verticesB)); + } + }; + + typedef MinkowskiDiff tShape; + + + // GJK + struct GJK + { + /* Types */ + struct sSV + { + b3Vector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + b3Scalar p[4]; + unsigned int rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + const b3AlignedObjectArray& m_verticesA; + const b3AlignedObjectArray& m_verticesB; + b3Vector3 m_ray; + b3Scalar m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + unsigned int m_nfree; + unsigned int m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK(const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB) + :m_verticesA(verticesA),m_verticesB(verticesB) + { + Initialize(); + } + void Initialize() + { + m_ray = b3Vector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg,const b3Vector3& guess) + { + unsigned int iterations=0; + b3Scalar sqdist=0; + b3Scalar alpha=0; + b3Vector3 lastw[4]; + unsigned int clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const b3Scalar sqrl= m_ray.length2(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:b3Vector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const unsigned int next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const b3Scalar rl=m_ray.length(); + if(rlw; + bool found=false; + for(unsigned int i=0;i<4;++i) + { + if((w-lastw[i]).length2()w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = b3Vector3(0,0,0); + m_current = next; + for(unsigned int i=0,ni=cs.rank;iw*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)rank) + { + case 1: + { + for(unsigned int i=0;i<3;++i) + { + b3Vector3 axis=b3Vector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const b3Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(unsigned int i=0;i<3;++i) + { + b3Vector3 axis=b3Vector3(0,0,0); + axis[i]=1; + const b3Vector3 p=b3Cross(d,axis); + if(p.length2()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const b3Vector3 n=b3Cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length2()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(b3Fabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } + /* Internals */ + void getsupport(const b3Vector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d,m_verticesA,m_verticesB); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const b3Vector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static b3Scalar det(const b3Vector3& a,const b3Vector3& b,const b3Vector3& c) + { + return( a.y*b.z*c.x+a.z*b.x*c.y- + a.x*b.z*c.y-a.y*b.x*c.z+ + a.x*b.y*c.z-a.z*b.y*c.x); + } + static b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + b3Scalar* w,unsigned int& m) + { + const b3Vector3 d=b-a; + const b3Scalar l=d.length2(); + if(l>GJK_SIMPLEX2_EPS) + { + const b3Scalar t(l>0?-b3Dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); } + } + return(-1); + } + static b3Scalar projectorigin( const b3Vector3& a, + const b3Vector3& b, + const b3Vector3& c, + b3Scalar* w,unsigned int& m) + { + static const unsigned int imd3[]={1,2,0}; + const b3Vector3* vt[]={&a,&b,&c}; + const b3Vector3 dl[]={a-b,b-c,c-a}; + const b3Vector3 n=b3Cross(dl[0],dl[1]); + const b3Scalar l=n.length2(); + if(l>GJK_SIMPLEX3_EPS) + { + b3Scalar mindist=-1; + b3Scalar subw[2]={0.f,0.f}; + unsigned int subm(0); + for(unsigned int i=0;i<3;++i) + { + if(b3Dot(*vt[i],b3Cross(dl[i],n))>0) + { + const unsigned int j=imd3[i]; + const b3Scalar subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd(((subm&1)?1<GJK_SIMPLEX4_EPS)) + { + b3Scalar mindist=-1; + b3Scalar subw[3]={0.f,0.f,0.f}; + unsigned int subm(0); + for(unsigned int i=0;i<3;++i) + { + const unsigned int j=imd3[i]; + const b3Scalar s=vl*b3Dot(d,b3Cross(dl[i],dl[j])); + if(s>0) + { + const b3Scalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd((subm&1?1<e[ea]=(unsigned char)eb;fa->f[ea]=fb; + fb->e[eb]=(unsigned char)ea;fb->f[eb]=fa; + } + static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = b3Vector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(unsigned int i=0;i1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + b3Swap(simplex.c[0],simplex.c[1]); + b3Swap(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + unsigned int pass=0; + unsigned int iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterationspass = (unsigned char)(++pass); + gjk.getsupport(best->n,*w); + const b3Scalar wdist=b3Dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(unsigned int j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const b3Vector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = b3Cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = b3Cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = b3Cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const b3Scalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const b3Scalar nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = b3Vector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } + bool getedgedist(sFace* face, sSV* a, sSV* b, b3Scalar& dist) + { + const b3Vector3 ba = b->w - a->w; + const b3Vector3 n_ab = b3Cross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const b3Scalar a_dot_nab = b3Dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if(a_dot_nab < 0) + { + // Outside of edge a->b + + const b3Scalar ba_l2 = ba.length2(); + const b3Scalar a_dot_ba = b3Dot(a->w, ba); + const b3Scalar b_dot_ba = b3Dot(b->w, ba); + + if(a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if(b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const b3Scalar a_dot_b = b3Dot(a->w, b->w); + dist = b3Sqrt(b3Max((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (b3Scalar)0)); + } + + return true; + } + + return false; + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = b3Cross(b->w-a->w,c->w-a->w); + const b3Scalar l=face->n.length(); + const bool v=l>EPA_ACCURACY; + + if(v) + { + if(!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = b3Dot(a->w, face->n) / l; + } + + face->n /= l; + if(forced || (face->d >= -EPA_PLANE_EPS)) + { + return face; + } + else + m_status=eStatus::NonConvex; + } + else + m_status=eStatus::Degenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + + } + m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; + return 0; + } + sFace* findbest() + { + sFace* minf=m_hull.root; + b3Scalar mind=minf->d*minf->d; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const b3Scalar sqd=f->d*f->d; + if(sqdpass!=pass) + { + const unsigned int e1=i1m3[e]; + if((b3Dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const unsigned int e2=i2m3[e]; + f->pass = (unsigned char)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + + }; + + // + static void Initialize(const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + b3GjkEpaSolver2::sResults& results, + tShape& shape, + bool withmargins) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = b3Vector3(0,0,0); + results.status = b3GjkEpaSolver2::sResults::Separated; + /* Shape */ + shape.m_shapes[0] = hullA; + shape.m_shapes[1] = hullB; + shape.m_toshape1 = transB.getBasis().transposeTimes(transA.getBasis()); + shape.m_toshape0 = transA.inverseTimes(transB); + shape.EnableMargin(withmargins); + } + +} + +// +// Api +// + +using namespace gjkepa2_impl; + +// +int b3GjkEpaSolver2::StackSizeRequirement() +{ + return(sizeof(GJK)+sizeof(EPA)); +} + +// +bool b3GjkEpaSolver2::Distance( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results) +{ + tShape shape; + Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,false); + GJK gjk(verticesA,verticesB); + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==GJK::eStatus::Valid) + { + b3Vector3 w0=b3Vector3(0,0,0); + b3Vector3 w1=b3Vector3(0,0,0); + for(unsigned int i=0;irank;++i) + { + const b3Scalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0,verticesA,verticesB)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1,verticesA,verticesB)*p; + } + results.witnesses[0] = transA*w0; + results.witnesses[1] = transA*w1; + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool b3GjkEpaSolver2::Penetration( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results, + bool usemargins) +{ + + tShape shape; + Initialize(transA,transB,hullA,hullB,verticesA,verticesB,results,shape,usemargins); + GJK gjk(verticesA,verticesB); + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + b3Vector3 w0=b3Vector3(0,0,0); + for(unsigned int i=0;id,0,verticesA,verticesB)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = transA*w0; + results.witnesses[1] = transA*(w0-epa.m_normal*epa.m_depth); + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + default: + { + } + } + return(false); +} + + +#if 0 +// +b3Scalar b3GjkEpaSolver2::SignedDistance(const b3Vector3& position, + b3Scalar margin, + const b3Transform& transA, + const b3ConvexPolyhedronCL& hullA, + const b3AlignedObjectArray& verticesA, + sResults& results) +{ + tShape shape; + btSphereShape shape1(margin); + b3Transform wtrs1(b3Quaternion(0,0,0,1),position); + Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,b3Vector3(1,1,1)); + if(gjk_status==GJK::eStatus::Valid) + { + b3Vector3 w0=b3Vector3(0,0,0); + b3Vector3 w1=b3Vector3(0,0,0); + for(unsigned int i=0;irank;++i) + { + const b3Scalar p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0*w0; + results.witnesses[1] = wtrs0*w1; + const b3Vector3 delta= results.witnesses[1]- + results.witnesses[0]; + const b3Scalar margin= shape0->getMarginNonVirtual()+ + shape1.getMarginNonVirtual(); + const b3Scalar length= delta.length(); + results.normal = delta/length; + results.witnesses[0] += results.normal*margin; + return(length-margin); + } + else + { + if(gjk_status==GJK::eStatus::Inside) + { + if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results)) + { + const b3Vector3 delta= results.witnesses[0]- + results.witnesses[1]; + const b3Scalar length= delta.length(); + if (length >= B3_EPSILON) + results.normal = delta/length; + return(-length); + } + } + } + return(B3_INFINITY); +} + +// +bool b3GjkEpaSolver2::SignedDistance(const btConvexShape* shape0, + const b3Transform& wtrs0, + const btConvexShape* shape1, + const b3Transform& wtrs1, + const b3Vector3& guess, + sResults& results) +{ + if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results)) + return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false)); + else + return(true); +} +#endif + + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h new file mode 100644 index 000000000..367c782bb --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkEpa.h @@ -0,0 +1,85 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the +use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it +freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not +claim that you wrote the original software. If you use this software in a +product, an acknowledgment in the product documentation would be appreciated +but is not required. +2. Altered source versions must be plainly marked as such, and must not be +misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +/* +GJK-EPA collision solver by Nathanael Presson, 2008 +*/ +#ifndef B3_GJK_EPA2_H +#define B3_GJK_EPA2_H + +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "Bullet3Common/b3Transform.h" +#include "b3ConvexPolyhedronCL.h" + +///btGjkEpaSolver contributed under zlib by Nathanael Presson +struct b3GjkEpaSolver2 +{ +struct sResults + { + enum eStatus + { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + b3Vector3 witnesses[2]; + b3Vector3 normal; + b3Scalar distance; + }; + +static int StackSizeRequirement(); + +static bool Distance( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results); + +static bool Penetration( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL* hullA, const b3ConvexPolyhedronCL* hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results, + bool usemargins=true); +#if 0 +static b3Scalar SignedDistance( const b3Vector3& position, + b3Scalar margin, + const b3Transform& transA, + const b3ConvexPolyhedronCL& hullA, + const b3AlignedObjectArray& verticesA, + + sResults& results); + +static bool SignedDistance( const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + const b3Vector3& guess, + sResults& results); +#endif //0 + +}; + +#endif //B3_GJK_EPA2_H + diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp new file mode 100644 index 000000000..ab7420385 --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.cpp @@ -0,0 +1,664 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "b3GjkPairDetector.h" +#include "Bullet3Common/b3Transform.h" +#include "b3VoronoiSimplexSolver.h" +#include "b3ConvexPolyhedronCL.h" +#include "b3VectorFloat4.h" +#include "b3GjkEpa.h" +#include "b3SupportMappings.h" + +//must be above the machine epsilon +#define REL_ERROR2 b3Scalar(1.0e-6) + +//temp globals, to improve GJK/EPA/penetration calculations +int gNumDeepPenetrationChecks = 0; +int gNumGjkChecks = 0; +int gGjkSeparatingAxis=0; +int gEpaSeparatingAxis=0; + + + +b3GjkPairDetector::b3GjkPairDetector(b3VoronoiSimplexSolver* simplexSolver,b3GjkEpaSolver2* penetrationDepthSolver) +:m_cachedSeparatingAxis(b3Scalar(0.),b3Scalar(-1.),b3Scalar(0.)), +m_penetrationDepthSolver(penetrationDepthSolver), +m_simplexSolver(simplexSolver), +m_ignoreMargin(false), +m_lastUsedMethod(-1), +m_catchDegeneracies(1), +m_fixContactNormalDirection(1) +{ +} + + + + +bool calcPenDepth( b3VoronoiSimplexSolver& simplexSolver, + const b3Transform& transformA, const b3Transform& transformB, + const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + b3Vector3& v, b3Vector3& wWitnessOnA, b3Vector3& wWitnessOnB) +{ + + (void)v; + (void)simplexSolver; + + b3Vector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); + b3GjkEpaSolver2::sResults results; + + + if(b3GjkEpaSolver2::Penetration(transformA,transformB,&hullA,&hullB,verticesA,verticesB,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return true; + } + else + { + if(b3GjkEpaSolver2::Distance(transformA,transformB,&hullA,&hullB,verticesA,verticesB,guessVector,results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } + } + + return false; +} +#define dot3F4 b3Dot + +inline void project(const b3ConvexPolyhedronCL& hull, const float4& pos, const b3Quaternion& orn, const float4& dir, const b3AlignedObjectArray& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = dot3F4(pos,dir); + + for(int i=0;i max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +static bool TestSepAxis(const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const float4& posA,const b3Quaternion& ornA, + const float4& posB,const b3Quaternion& ornB, + float4& sep_axis, const b3AlignedObjectArray& verticesA,const b3AlignedObjectArray& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + project(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + project(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0=0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1>=0.0f); + if (d0& verticesA, + const b3AlignedObjectArray& verticesB, + b3Scalar maximumDistanceSquared, + b3Vector3& resultSepNormal, + float& resultSepDistance, + b3Vector3& resultPointOnB) +{ + //resultSepDistance = maximumDistanceSquared; + + gjkDetector->m_cachedSeparatingDistance = 0.f; + + b3Scalar distance=b3Scalar(0.); + b3Vector3 normalInB(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + b3Vector3 pointOnA,pointOnB; + + b3Transform localTransA = transA; + b3Transform localTransB = transB; + + b3Vector3 positionOffset(0,0,0);// = (localTransA.getOrigin() + localTransB.getOrigin()) * b3Scalar(0.5); + localTransA.getOrigin() -= positionOffset; + localTransB.getOrigin() -= positionOffset; + + bool check2d = false;//m_minkowskiA->isConvex2d() && m_minkowskiB->isConvex2d(); + + b3Scalar marginA = 0.f;//m_marginA; + b3Scalar marginB = 0.f;//m_marginB; + + gNumGjkChecks++; + + + //for CCD we don't use margins + if (gjkDetector->m_ignoreMargin) + { + marginA = b3Scalar(0.); + marginB = b3Scalar(0.); + } + + gjkDetector->m_curIter = 0; + int gGjkMaxIter = 1000;//this is to catch invalid input, perhaps check for #NaN? + gjkDetector->m_cachedSeparatingAxis.setValue(1,1,1);//0,0,0); + + bool isValid = false; + bool checkSimplex = false; + bool checkPenetration = true; + gjkDetector->m_degenerateSimplex = 0; + + gjkDetector->m_lastUsedMethod = -1; + + { + b3Scalar squaredDistance = B3_LARGE_FLOAT; + b3Scalar delta = -1e30f;//b3Scalar(0.); + b3Scalar prevDelta = -1e30f;//b3Scalar(0.); + + b3Scalar margin = marginA + marginB; + b3Scalar bestDeltaN = -1e30f; + b3Vector3 bestSepAxis(0,0,0); + b3Vector3 bestPointOnA; + b3Vector3 bestPointOnB; + + + + gjkDetector->m_simplexSolver->reset(); + + for ( ; ; ) + //while (true) + { + + + b3Vector3 seperatingAxisInA = (-gjkDetector->m_cachedSeparatingAxis)* localTransA.getBasis(); + b3Vector3 seperatingAxisInB = gjkDetector->m_cachedSeparatingAxis* localTransB.getBasis(); + + b3Vector3 pInA = localGetSupportVertexWithoutMargin(seperatingAxisInA,&hullA,verticesA); + b3Vector3 qInB = localGetSupportVertexWithoutMargin(seperatingAxisInB,&hullB,verticesB); + + b3Vector3 pWorld = localTransA(pInA); + b3Vector3 qWorld = localTransB(qInB); + + { + b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2(); + if (l2>B3_EPSILON*B3_EPSILON) + { + + b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2)); + float computedDepth=1e30f; + if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), + transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) + { + return false; + } + + + + if(computedDepthB3_EPSILON*B3_EPSILON) + { + resultSepDistance = computedDepth; + resultSepNormal = testAxis; + } + } + } + } + + + + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + b3Vector3 w = pWorld - qWorld; + delta = gjkDetector->m_cachedSeparatingAxis.dot(w); + if (delta>0) + return false; + b3Scalar deltaN = gjkDetector->m_cachedSeparatingAxis.normalized().dot(w.normalized()); + + + if (deltaN < bestDeltaN) + { + bestDeltaN = deltaN; + //printf("new solution?\n"); + bestSepAxis = gjkDetector->m_cachedSeparatingAxis; + gjkDetector->m_simplexSolver->compute_points(bestPointOnA, bestPointOnB); + } + prevDelta = delta; + b3Scalar dist = 0; + if (delta<0) + dist = -b3Sqrt(b3Fabs(delta)); + else + dist = b3Sqrt(delta); + + //printf("gjkDetector->m_cachedSeparatingAxis = %f,%f,%f delta/dist = %f\n",gjkDetector->m_cachedSeparatingAxis.x,gjkDetector->m_cachedSeparatingAxis.y,gjkDetector->m_cachedSeparatingAxis.z,dist); + // potential exit, they don't overlap + if ((delta > b3Scalar(0.0)) && (delta * delta > squaredDistance * maximumDistanceSquared)) + { + gjkDetector->m_degenerateSimplex = 10; + checkSimplex=true; + //checkPenetration = false; + break; + } + + //exit 0: the new point is already in the simplex, or we didn't come any closer + if (gjkDetector->m_simplexSolver->inSimplex(w)) + { + gjkDetector->m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + b3Scalar f0 = squaredDistance - delta; + b3Scalar f1 = squaredDistance * REL_ERROR2; + + if (f0 <= f1) + { + if (f0 <= b3Scalar(0.)) + { + gjkDetector->m_degenerateSimplex = 2; + } else + { + gjkDetector->m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + gjkDetector->m_simplexSolver->addVertex(w, pWorld, qWorld); + b3Vector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!gjkDetector->m_simplexSolver->closest(newCachedSeparatingAxis)) + { + gjkDetector->m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if(0)//newCachedSeparatingAxis.length2()m_cachedSeparatingAxis = newCachedSeparatingAxis; + } + gjkDetector->m_degenerateSimplex = 6; + checkSimplex = true; + break; + } + + b3Scalar previousSquaredDistance = squaredDistance; + squaredDistance = newCachedSeparatingAxis.length2(); + + b3Vector3 sepAxis=newCachedSeparatingAxis.normalized(); + + + + + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + + //are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= B3_EPSILON * previousSquaredDistance) + { + checkSimplex = true; + gjkDetector->m_degenerateSimplex = 12; + + break; + } + + gjkDetector->m_cachedSeparatingAxis = newCachedSeparatingAxis; + + { + b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2(); + if (l2>B3_EPSILON*B3_EPSILON) + { + + b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2)); + float computedDepth=1e30f; + if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), + transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) + { + return false; + } + + + + if(computedDepthB3_EPSILON*B3_EPSILON) + { + resultSepDistance = computedDepth; + resultSepNormal = testAxis; + } + } + } + } + + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (gjkDetector->m_curIter++ > gGjkMaxIter) + { + break; + } + + + bool check = (!gjkDetector->m_simplexSolver->fullSimplex()); + + float projectedDepth = 0; + + if (delta<0) + { + projectedDepth = -b3Sqrt(b3Fabs(delta)); + } else + { + projectedDepth = b3Sqrt(delta); + } + + //printf("dist2 = %f dist= %f projectedDepth = %f\n", squaredDistance,b3Sqrt(squaredDistance),projectedDepth); + + if (!check) + { + gjkDetector->m_degenerateSimplex = 13; + break; + } + } + + if (checkSimplex) + { + if (bestSepAxis.length2()) + { + pointOnA = bestPointOnA; + pointOnB = bestPointOnB; + gjkDetector->m_cachedSeparatingAxis = bestSepAxis; + } else + { + gjkDetector->m_simplexSolver->compute_points(pointOnA, pointOnB); + } + + normalInB = gjkDetector->m_cachedSeparatingAxis; + b3Scalar lenSqr =gjkDetector->m_cachedSeparatingAxis.length2(); + + //valid normal + if (lenSqr < 0.0001) + { + gjkDetector->m_degenerateSimplex = 5; + } + if (lenSqr > B3_EPSILON*B3_EPSILON) + { + b3Scalar rlen = b3Scalar(1.) / b3Sqrt(lenSqr ); + normalInB *= rlen; //normalize + b3Scalar s = b3Sqrt(squaredDistance); + + b3Assert(s > b3Scalar(0.0)); + pointOnA -= gjkDetector->m_cachedSeparatingAxis * (marginA / s); + pointOnB += gjkDetector->m_cachedSeparatingAxis * (marginB / s); + distance = ((b3Scalar(1.)/rlen) - margin); + isValid = true; + + gjkDetector->m_lastUsedMethod = 1; + } else + { + gjkDetector->m_lastUsedMethod = 2; + } + } + + bool catchDegeneratePenetrationCase = (gjkDetector->m_catchDegeneracies && gjkDetector->m_penetrationDepthSolver && gjkDetector->m_degenerateSimplex && ((distance+margin) < 0.01)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + { + //penetration case + + //if there is no way to handle penetrations, bail out + if (gjkDetector->m_penetrationDepthSolver) + { + // Penetration depth case. + b3Vector3 tmpPointOnA,tmpPointOnB; + + gNumDeepPenetrationChecks++; + gjkDetector->m_cachedSeparatingAxis.setZero(); + + bool isValid2 = calcPenDepth( + *gjkDetector->m_simplexSolver, + transA,transB,hullA,hullB,verticesA,verticesB, + gjkDetector->m_cachedSeparatingAxis, tmpPointOnA, tmpPointOnB + ); + + + if (isValid2) + { + b3Vector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; + b3Scalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (B3_EPSILON*B3_EPSILON)) + { + tmpNormalInB = gjkDetector->m_cachedSeparatingAxis; + lenSqr = gjkDetector->m_cachedSeparatingAxis.length2(); + } + + if (lenSqr > (B3_EPSILON*B3_EPSILON)) + { + tmpNormalInB /= b3Sqrt(lenSqr); + b3Scalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + gjkDetector->m_lastUsedMethod = 3; + } else + { + gjkDetector->m_lastUsedMethod = 8; + } + } else + { + gjkDetector->m_lastUsedMethod = 9; + } + } else + + { + ///this is another degenerate case, where the initial GJK calculation reports a degenerate case + ///EPA reports no penetration, and the second GJK (using the supporting vector without margin) + ///reports a valid positive distance. Use the results of the second GJK instead of failing. + ///thanks to Jacob.Langford for the reproduction case + ///http://code.google.com/p/bullet/issues/detail?id=250 + + + if (gjkDetector->m_cachedSeparatingAxis.length2() > b3Scalar(0.)) + { + b3Scalar distance2 = (tmpPointOnA-tmpPointOnB).length()-margin; + //only replace valid distances when the distance is less + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + pointOnA -= gjkDetector->m_cachedSeparatingAxis * marginA ; + pointOnB += gjkDetector->m_cachedSeparatingAxis * marginB ; + normalInB = gjkDetector->m_cachedSeparatingAxis; + normalInB.normalize(); + isValid = true; + gjkDetector->m_lastUsedMethod = 6; + } else + { + gjkDetector->m_lastUsedMethod = 5; + } + } + } + + } + + } + } + + + + if (isValid && ((distance < 0) || (distance*distance < maximumDistanceSquared))) + { + + if (gjkDetector->m_fixContactNormalDirection) + { + ///@workaround for sticky convex collisions + //in some degenerate cases (usually when the use uses very small margins) + //the contact normal is pointing the wrong direction + //so fix it now (until we can deal with all degenerate cases in GJK and EPA) + //contact normals need to point from B to A in all cases, so we can simply check if the contact normal really points from B to A + //We like to use a dot product of the normal against the difference of the centroids, + //once the centroid is available in the API + //until then we use the center of the aabb to approximate the centroid + b3Vector3 aabbMin,aabbMax; + //m_minkowskiA->getAabb(localTransA,aabbMin,aabbMax); + //b3Vector3 posA = (aabbMax+aabbMin)*b3Scalar(0.5); + + //m_minkowskiB->getAabb(localTransB,aabbMin,aabbMax); + //b3Vector3 posB = (aabbMin+aabbMax)*b3Scalar(0.5); + + + b3Vector3 diff = transA.getOrigin()-transB.getOrigin(); + if (diff.dot(normalInB) < 0.f) + normalInB *= -1.f; + } + gjkDetector->m_cachedSeparatingAxis = normalInB; + gjkDetector->m_cachedSeparatingDistance = distance; + + { + b3Scalar l2 = gjkDetector->m_cachedSeparatingAxis.length2(); + if (l2>B3_EPSILON*B3_EPSILON) + { + + b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2)); + float computedDepth=1e30f; + if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), + transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) + { + return false; + } + + + + if(computedDepthB3_EPSILON*B3_EPSILON) + { + resultSepDistance = computedDepth; + resultSepNormal = testAxis; + } + } + } + } + + + resultSepNormal = normalInB; + //printf("normalInB = %f,%f,%f, distance = %f\n",normalInB.x,normalInB.y,normalInB.z,distance); + resultSepDistance = distance; + + + b3Scalar lsqr = resultSepNormal.length2(); + b3Assert(lsqr>B3_EPSILON*B3_EPSILON); + if (lsqrB3_EPSILON*B3_EPSILON) + { + + b3Vector3 testAxis = gjkDetector->m_cachedSeparatingAxis*(1./b3Sqrt(l2)); + float computedDepth=1e30f; + if (!TestSepAxis(hullA,hullB,transA.getOrigin(),transA.getRotation(), + transB.getOrigin(),transB.getRotation(),testAxis,verticesA,verticesB,computedDepth)) + { + return false; + } + + + + if(computedDepthB3_EPSILON*B3_EPSILON) + { + resultSepDistance = computedDepth; + resultSepNormal = testAxis; + } + } + } + } + } +#endif + resultPointOnB = pointOnB+positionOffset; + return true; + } + return false; + +} + + + + + diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h new file mode 100644 index 000000000..477ddd3fd --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3GjkPairDetector.h @@ -0,0 +1,84 @@ + + + + +#ifndef B3_GJK_PAIR_DETECTOR_H +#define B3_GJK_PAIR_DETECTOR_H + + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +struct b3Transform; +struct b3GjkEpaSolver2; +class b3VoronoiSimplexSolver; +struct b3ConvexPolyhedronCL; + +B3_ATTRIBUTE_ALIGNED16(struct) b3GjkPairDetector +{ + + + b3Vector3 m_cachedSeparatingAxis; + b3GjkEpaSolver2* m_penetrationDepthSolver; + b3VoronoiSimplexSolver* m_simplexSolver; + + + bool m_ignoreMargin; + b3Scalar m_cachedSeparatingDistance; + + +public: + + //some debugging to fix degeneracy problems + int m_lastUsedMethod; + int m_curIter; + int m_degenerateSimplex; + int m_catchDegeneracies; + int m_fixContactNormalDirection; + + b3GjkPairDetector(b3VoronoiSimplexSolver* simplexSolver,b3GjkEpaSolver2* penetrationDepthSolver); + + virtual ~b3GjkPairDetector() {}; + + + //void getClosestPoints(,Result& output); + + void setCachedSeperatingAxis(const b3Vector3& seperatingAxis) + { + m_cachedSeparatingAxis = seperatingAxis; + } + + const b3Vector3& getCachedSeparatingAxis() const + { + return m_cachedSeparatingAxis; + } + b3Scalar getCachedSeparatingDistance() const + { + return m_cachedSeparatingDistance; + } + + void setPenetrationDepthSolver(b3GjkEpaSolver2* penetrationDepthSolver) + { + m_penetrationDepthSolver = penetrationDepthSolver; + } + + ///don't use setIgnoreMargin, it's for Bullet's internal use + void setIgnoreMargin(bool ignoreMargin) + { + m_ignoreMargin = ignoreMargin; + } + + +}; + + +bool getClosestPoints(b3GjkPairDetector* gjkDetector, const b3Transform& transA, const b3Transform& transB, + const b3ConvexPolyhedronCL& hullA, const b3ConvexPolyhedronCL& hullB, + const b3AlignedObjectArray& verticesA, + const b3AlignedObjectArray& verticesB, + b3Scalar maximumDistanceSquared, + b3Vector3& resultSepNormal, + float& resultSepDistance, + b3Vector3& resultPointOnB); + +#endif //B3_GJK_PAIR_DETECTOR_H diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h new file mode 100644 index 000000000..1fa27c4e6 --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3SupportMappings.h @@ -0,0 +1,36 @@ + +#ifndef B3_SUPPORT_MAPPINGS_H +#define B3_SUPPORT_MAPPINGS_H + +#include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "b3VectorFloat4.h" + +struct b3ConvexPolyhedronCL; +struct b3GjkPairDetector; + +inline b3Vector3 localGetSupportVertexWithMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull, + const b3AlignedObjectArray& verticesA, b3Scalar margin) +{ + b3Vector3 supVec(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + b3Scalar maxDot = b3Scalar(-B3_LARGE_FLOAT); + + // Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically. + if( 0 < hull->m_numVertices ) + { + const b3Vector3 scaled = supportVec; + int index = (int) scaled.maxDot( &verticesA[hull->m_vertexOffset], hull->m_numVertices, maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + +inline b3Vector3 localGetSupportVertexWithoutMargin(const float4& supportVec,const struct b3ConvexPolyhedronCL* hull, + const b3AlignedObjectArray& verticesA) +{ + return localGetSupportVertexWithMargin(supportVec,hull,verticesA,0.f); +} + +#endif //B3_SUPPORT_MAPPINGS_H diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h new file mode 100644 index 000000000..3ae074f28 --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VectorFloat4.h @@ -0,0 +1,20 @@ +#ifndef B3_VECTOR_FLOAT4_H +#define B3_VECTOR_FLOAT4_H + +#include "Bullet3Common/b3Transform.h" + +#define cross3(a,b) (a.cross(b)) +#define float4 b3Vector3 +#define make_float4(x,y,z,w) b3Vector4(x,y,z,w) + +inline b3Vector3 transform(const b3Vector3* v, const b3Vector3* pos, const b3Quaternion* orn) +{ + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(*pos); + tr.setRotation(*orn); + b3Vector3 res = tr(*v); + return res; +} + +#endif //B3_VECTOR_FLOAT4_H diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp new file mode 100644 index 000000000..bc9a18f22 --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp @@ -0,0 +1,609 @@ + +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. + + Elsevier CDROM license agreements grants nonexclusive license to use the software + for any purpose, commercial or non-commercial as long as the following credit is included + identifying the original source of the software: + + Parts of the source are "from the book Real-Time Collision Detection by + Christer Ericson, published by Morgan Kaufmann Publishers, + (c) 2005 Elsevier Inc." + +*/ + + +#include "b3VoronoiSimplexSolver.h" + +#define VERTA 0 +#define VERTB 1 +#define VERTC 2 +#define VERTD 3 + +#define B3_CATCH_DEGENERATE_TETRAHEDRON 1 +void b3VoronoiSimplexSolver::removeVertex(int index) +{ + + b3Assert(m_numVertices>0); + m_numVertices--; + m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; + m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; + m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; +} + +void b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& usedVerts) +{ + if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) + removeVertex(3); + + if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) + removeVertex(2); + + if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) + removeVertex(1); + + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) + removeVertex(0); + +} + + + + + +//clear the simplex, remove all the vertices +void b3VoronoiSimplexSolver::reset() +{ + m_cachedValidClosest = false; + m_numVertices = 0; + m_needsUpdate = true; + m_lastW = b3Vector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT)); + m_cachedBC.reset(); +} + + + + //add a vertex +void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q) +{ + m_lastW = w; + m_needsUpdate = true; + + m_simplexVectorW[m_numVertices] = w; + m_simplexPointsP[m_numVertices] = p; + m_simplexPointsQ[m_numVertices] = q; + + m_numVertices++; +} + +bool b3VoronoiSimplexSolver::updateClosestVectorAndPoints() +{ + + if (m_needsUpdate) + { + m_cachedBC.reset(); + + m_needsUpdate = false; + + switch (numVertices()) + { + case 0: + m_cachedValidClosest = false; + break; + case 1: + { + m_cachedP1 = m_simplexPointsP[0]; + m_cachedP2 = m_simplexPointsQ[0]; + m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] + m_cachedBC.reset(); + m_cachedBC.setBarycentricCoordinates(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + m_cachedValidClosest = m_cachedBC.isValid(); + break; + }; + case 2: + { + //closest point origin from line segment + const b3Vector3& from = m_simplexVectorW[0]; + const b3Vector3& to = m_simplexVectorW[1]; + b3Vector3 nearest; + + b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + b3Vector3 diff = p - from; + b3Vector3 v = to - from; + b3Scalar t = v.dot(diff); + + if (t > 0) { + b3Scalar dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + diff -= t*v; + m_cachedBC.m_usedVertices.usedVertexA = true; + m_cachedBC.m_usedVertices.usedVertexB = true; + } else { + t = 1; + diff -= v; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexB = true; + } + } else + { + t = 0; + //reduce to 1 point + m_cachedBC.m_usedVertices.usedVertexA = true; + } + m_cachedBC.setBarycentricCoordinates(1-t,t); + nearest = from + t*v; + + m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); + m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); + m_cachedV = m_cachedP1 - m_cachedP2; + + reduceVertices(m_cachedBC.m_usedVertices); + + m_cachedValidClosest = m_cachedBC.isValid(); + break; + } + case 3: + { + //closest point origin from triangle + b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + + const b3Vector3& a = m_simplexVectorW[0]; + const b3Vector3& b = m_simplexVectorW[1]; + const b3Vector3& c = m_simplexVectorW[2]; + + closestPtPointTriangle(p,a,b,c,m_cachedBC); + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; + + m_cachedV = m_cachedP1-m_cachedP2; + + reduceVertices (m_cachedBC.m_usedVertices); + m_cachedValidClosest = m_cachedBC.isValid(); + + break; + } + case 4: + { + + + b3Vector3 p (b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + + const b3Vector3& a = m_simplexVectorW[0]; + const b3Vector3& b = m_simplexVectorW[1]; + const b3Vector3& c = m_simplexVectorW[2]; + const b3Vector3& d = m_simplexVectorW[3]; + + bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); + + if (hasSeperation) + { + + m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + + m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + + m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + + m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; + + m_cachedV = m_cachedP1-m_cachedP2; + reduceVertices (m_cachedBC.m_usedVertices); + } else + { +// printf("sub distance got penetration\n"); + + if (m_cachedBC.m_degenerate) + { + m_cachedValidClosest = false; + } else + { + m_cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + m_cachedV.setValue(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.)); + } + break; + } + + m_cachedValidClosest = m_cachedBC.isValid(); + + //closest point origin from tetrahedron + break; + } + default: + { + m_cachedValidClosest = false; + } + }; + } + + return m_cachedValidClosest; + +} + +//return/calculate the closest vertex +bool b3VoronoiSimplexSolver::closest(b3Vector3& v) +{ + bool succes = updateClosestVectorAndPoints(); + v = m_cachedV; + return succes; +} + + + +b3Scalar b3VoronoiSimplexSolver::maxVertex() +{ + int i, numverts = numVertices(); + b3Scalar maxV = b3Scalar(0.); + for (i=0;i= b3Scalar(0.0) && d4 <= d3) + { + result.m_closestPointOnSimplex = b; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(0,1,0); + + return true; // b; // barycentric coordinates (0,1,0) + } + // Check if P in edge region of AB, if so return projection of P onto AB + b3Scalar vc = d1*d4 - d3*d2; + if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0)) { + b3Scalar v = d1 / (d1 - d3); + result.m_closestPointOnSimplex = a + v * ab; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(1-v,v,0); + return true; + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + b3Vector3 cp = p - c; + b3Scalar d5 = ab.dot(cp); + b3Scalar d6 = ac.dot(cp); + if (d6 >= b3Scalar(0.0) && d5 <= d6) + { + result.m_closestPointOnSimplex = c; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,0,1); + return true;//c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + b3Scalar vb = d5*d2 - d1*d6; + if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0)) { + b3Scalar w = d2 / (d2 - d6); + result.m_closestPointOnSimplex = a + w * ac; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-w,0,w); + return true; + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + b3Scalar va = d3*d6 - d5*d4; + if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0)) { + b3Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + result.m_closestPointOnSimplex = b + w * (c - b); + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0,1-w,w); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + b3Scalar denom = b3Scalar(1.0) / (va + vb + vc); + b3Scalar v = vb * denom; + b3Scalar w = vc * denom; + + result.m_closestPointOnSimplex = a + ab * v + ac * w; + result.m_usedVertices.usedVertexA = true; + result.m_usedVertices.usedVertexB = true; + result.m_usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1-v-w,v,w); + + return true; +// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = b3Scalar(1.0) - v - w + +} + + + + + +/// Test if point p and d lie on opposite sides of plane through abc +int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d) +{ + b3Vector3 normal = (b-a).cross(c-a); + + b3Scalar signp = (p - a).dot(normal); // [AP AB AC] + b3Scalar signd = (d - a).dot( normal); // [AD AB AC] + +#ifdef B3_CATCH_DEGENERATE_TETRAHEDRON +#ifdef BT_USE_DOUBLE_PRECISION +if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8))) + { + return -1; + } +#else + if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4))) + { +// printf("affine dependent/degenerate\n");// + return -1; + } +#endif + +#endif + // Points on opposite sides if expression signs are opposite + return signp * signd < b3Scalar(0.); +} + + +bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult) +{ + b3SubSimplexClosestResult tempResult; + + // Start out assuming point inside all halfspaces, so closest to itself + finalResult.m_closestPointOnSimplex = p; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = true; + finalResult.m_usedVertices.usedVertexB = true; + finalResult.m_usedVertices.usedVertexC = true; + finalResult.m_usedVertices.usedVertexD = true; + + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); + + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.m_degenerate = true; + return false; + } + + if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) + { + return false; + } + + + b3Scalar bestSqDist = FLT_MAX; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC) + { + closestPtPointTriangle(p, a, b, c,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + + b3Scalar sqDist = (q - p).dot( q - p); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + //convert result bitmask! + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC], + 0 + ); + + } + } + + + // Repeat test for face acd + if (pointOutsideACD) + { + closestPtPointTriangle(p, a, c, d,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + b3Scalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + 0, + tempResult.m_barycentricCoords[VERTB], + tempResult.m_barycentricCoords[VERTC] + ); + + } + } + // Repeat test for face adb + + + if (pointOutsideADB) + { + closestPtPointTriangle(p, a, d, b,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + + b3Scalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; + + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + finalResult.setBarycentricCoordinates( + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + 0, + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + // Repeat test for face bdc + + + if (pointOutsideBDC) + { + closestPtPointTriangle(p, b, d, c,tempResult); + b3Vector3 q = tempResult.m_closestPointOnSimplex; + //convert result bitmask! + b3Scalar sqDist = (q - p).dot( q - p); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.m_closestPointOnSimplex = q; + finalResult.m_usedVertices.reset(); + // + finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; + finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; + finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; + + finalResult.setBarycentricCoordinates( + 0, + tempResult.m_barycentricCoords[VERTA], + tempResult.m_barycentricCoords[VERTC], + tempResult.m_barycentricCoords[VERTB] + ); + + } + } + + //help! we ended up full ! + + if (finalResult.m_usedVertices.usedVertexA && + finalResult.m_usedVertices.usedVertexB && + finalResult.m_usedVertices.usedVertexC && + finalResult.m_usedVertices.usedVertexD) + { + return true; + } + + return true; +} + diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h new file mode 100644 index 000000000..a6e27667d --- /dev/null +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.h @@ -0,0 +1,177 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#ifndef B3_VORONOI_SIMPLEX_SOLVER_H +#define B3_VORONOI_SIMPLEX_SOLVER_H + +#include "Bullet3Common/b3Vector3.h" + + +#define VORONOI_SIMPLEX_MAX_VERTS 5 + +///disable next define, or use defaultCollisionConfiguration->getSimplexSolver()->setEqualVertexThreshold(0.f) to disable/configure +//#define BT_USE_EQUAL_VERTEX_THRESHOLD +#define VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD 0.0001f + + +struct b3UsageBitfield{ + b3UsageBitfield() + { + reset(); + } + + void reset() + { + usedVertexA = false; + usedVertexB = false; + usedVertexC = false; + usedVertexD = false; + } + unsigned short usedVertexA : 1; + unsigned short usedVertexB : 1; + unsigned short usedVertexC : 1; + unsigned short usedVertexD : 1; + unsigned short unused1 : 1; + unsigned short unused2 : 1; + unsigned short unused3 : 1; + unsigned short unused4 : 1; +}; + + +struct b3SubSimplexClosestResult +{ + b3Vector3 m_closestPointOnSimplex; + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + b3UsageBitfield m_usedVertices; + b3Scalar m_barycentricCoords[4]; + bool m_degenerate; + + void reset() + { + m_degenerate = false; + setBarycentricCoordinates(); + m_usedVertices.reset(); + } + bool isValid() + { + bool valid = (m_barycentricCoords[0] >= b3Scalar(0.)) && + (m_barycentricCoords[1] >= b3Scalar(0.)) && + (m_barycentricCoords[2] >= b3Scalar(0.)) && + (m_barycentricCoords[3] >= b3Scalar(0.)); + + + return valid; + } + void setBarycentricCoordinates(b3Scalar a=b3Scalar(0.),b3Scalar b=b3Scalar(0.),b3Scalar c=b3Scalar(0.),b3Scalar d=b3Scalar(0.)) + { + m_barycentricCoords[0] = a; + m_barycentricCoords[1] = b; + m_barycentricCoords[2] = c; + m_barycentricCoords[3] = d; + } + +}; + +/// b3VoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points simplex to the origin. +/// Can be used with GJK, as an alternative to Johnson distance algorithm. + +B3_ATTRIBUTE_ALIGNED16(class) b3VoronoiSimplexSolver +{ +public: + + B3_DECLARE_ALIGNED_ALLOCATOR(); + + int m_numVertices; + + b3Vector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]; + b3Vector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]; + b3Vector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]; + + + + b3Vector3 m_cachedP1; + b3Vector3 m_cachedP2; + b3Vector3 m_cachedV; + b3Vector3 m_lastW; + + b3Scalar m_equalVertexThreshold; + bool m_cachedValidClosest; + + + b3SubSimplexClosestResult m_cachedBC; + + bool m_needsUpdate; + + void removeVertex(int index); + void reduceVertices (const b3UsageBitfield& usedVerts); + bool updateClosestVectorAndPoints(); + + bool closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult); + int pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d); + bool closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result); + +public: + + b3VoronoiSimplexSolver() + : m_equalVertexThreshold(VORONOI_DEFAULT_EQUAL_VERTEX_THRESHOLD) + { + } + void reset(); + + void addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& q); + + void setEqualVertexThreshold(b3Scalar threshold) + { + m_equalVertexThreshold = threshold; + } + + b3Scalar getEqualVertexThreshold() const + { + return m_equalVertexThreshold; + } + + bool closest(b3Vector3& v); + + b3Scalar maxVertex(); + + bool fullSimplex() const + { + return (m_numVertices == 4); + } + + int getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *yBuf) const; + + bool inSimplex(const b3Vector3& w); + + void backup_closest(b3Vector3& v) ; + + bool emptySimplex() const ; + + void compute_points(b3Vector3& p1, b3Vector3& p2) ; + + int numVertices() const + { + return m_numVertices; + } + + +}; + +#endif //B3_VORONOI_SIMPLEX_SOLVER_H + diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl index 1f37e3cbf..bf961ed3d 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl @@ -597,7 +597,7 @@ int extractManifoldSequential(const float4* p, int nPoints, float4 nearNormal, i #define MAX_PLANE_CONVEX_POINTS 64 -void computeContactPlaneConvex(int pairIndex, +int computeContactPlaneConvex(int pairIndex, int bodyIndexA, int bodyIndexB, int collidableIndexA, int collidableIndexB, __global const BodyData* rigidBodies, @@ -613,6 +613,7 @@ void computeContactPlaneConvex(int pairIndex, Quaternion ornB ) { + int resultIndex=-1; int shapeIndex = collidables[collidableIndexB].m_shapeIndex; __global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex]; @@ -706,6 +707,7 @@ void computeContactPlaneConvex(int pairIndex, if (dstIdx < maxContactCapacity) { + resultIndex = dstIdx; __global Contact4* c = &globalContactsOut[dstIdx]; c->m_worldNormal = planeNormalWorld; //c->setFrictionCoeff(0.7); @@ -735,6 +737,8 @@ void computeContactPlaneConvex(int pairIndex, GET_NPOINTS(*c) = numReducedPoints; }//if (dstIdx < numPairs) } + + return resultIndex; } @@ -802,7 +806,7 @@ void computeContactPlaneSphere(int pairIndex, } -__kernel void primitiveContactsKernel( __global const int4* pairs, +__kernel void primitiveContactsKernel( __global int4* pairs, __global const BodyData* rigidBodies, __global const btCollidableGpu* collidables, __global const ConvexPolyhedronCL* convexShapes, @@ -845,9 +849,12 @@ __kernel void primitiveContactsKernel( __global const int4* pairs, posB = rigidBodies[bodyIndexB].m_pos; Quaternion ornB; ornB = rigidBodies[bodyIndexB].m_quat; - computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, + int contactIndex = computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, rigidBodies,collidables,convexShapes,vertices,indices, faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB); + if (contactIndex>=0) + pairs[pairIndex].z = contactIndex; + return; } @@ -862,10 +869,13 @@ __kernel void primitiveContactsKernel( __global const int4* pairs, ornA = rigidBodies[bodyIndexA].m_quat; - computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, + int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, rigidBodies,collidables,convexShapes,vertices,indices, faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA); + if (contactIndex>=0) + pairs[pairIndex].z = contactIndex; + return; } diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h index c5bdbc97a..fe4db508a 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h @@ -599,7 +599,7 @@ static const char* primitiveContactsKernelsCL= \ "\n" "#define MAX_PLANE_CONVEX_POINTS 64\n" "\n" -"void computeContactPlaneConvex(int pairIndex,\n" +"int computeContactPlaneConvex(int pairIndex,\n" " int bodyIndexA, int bodyIndexB, \n" " int collidableIndexA, int collidableIndexB, \n" " __global const BodyData* rigidBodies, \n" @@ -615,6 +615,7 @@ static const char* primitiveContactsKernelsCL= \ " Quaternion ornB\n" " )\n" "{\n" +" int resultIndex=-1;\n" "\n" " int shapeIndex = collidables[collidableIndexB].m_shapeIndex;\n" " __global const ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex];\n" @@ -708,6 +709,7 @@ static const char* primitiveContactsKernelsCL= \ "\n" " if (dstIdx < maxContactCapacity)\n" " {\n" +" resultIndex = dstIdx;\n" " __global Contact4* c = &globalContactsOut[dstIdx];\n" " c->m_worldNormal = planeNormalWorld;\n" " //c->setFrictionCoeff(0.7);\n" @@ -737,6 +739,8 @@ static const char* primitiveContactsKernelsCL= \ " GET_NPOINTS(*c) = numReducedPoints;\n" " }//if (dstIdx < numPairs)\n" " } \n" +"\n" +" return resultIndex;\n" "}\n" "\n" "\n" @@ -804,7 +808,7 @@ static const char* primitiveContactsKernelsCL= \ "}\n" "\n" "\n" -"__kernel void primitiveContactsKernel( __global const int4* pairs, \n" +"__kernel void primitiveContactsKernel( __global int4* pairs, \n" " __global const BodyData* rigidBodies, \n" " __global const btCollidableGpu* collidables,\n" " __global const ConvexPolyhedronCL* convexShapes, \n" @@ -847,9 +851,12 @@ static const char* primitiveContactsKernelsCL= \ " posB = rigidBodies[bodyIndexB].m_pos;\n" " Quaternion ornB;\n" " ornB = rigidBodies[bodyIndexB].m_quat;\n" -" computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" +" int contactIndex = computeContactPlaneConvex(pairIndex, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, \n" " rigidBodies,collidables,convexShapes,vertices,indices,\n" " faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity, posB,ornB);\n" +" if (contactIndex>=0)\n" +" pairs[pairIndex].z = contactIndex;\n" +"\n" " return;\n" " }\n" "\n" @@ -864,10 +871,13 @@ static const char* primitiveContactsKernelsCL= \ " ornA = rigidBodies[bodyIndexA].m_quat;\n" "\n" "\n" -" computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n" +" int contactIndex = computeContactPlaneConvex( pairIndex, bodyIndexB,bodyIndexA, collidableIndexB,collidableIndexA, \n" " rigidBodies,collidables,convexShapes,vertices,indices,\n" " faces, globalContactsOut, nGlobalContactsOut,maxContactCapacity,posA,ornA);\n" "\n" +" if (contactIndex>=0)\n" +" pairs[pairIndex].z = contactIndex;\n" +"\n" " return;\n" " }\n" "\n" diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl index e5f8c85b4..1d48b39af 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl @@ -45,15 +45,15 @@ typedef struct { float4 m_worldPos[4]; float4 m_worldNormal; // w: m_nPoints + u32 m_coeffs; u32 m_batchIdx; - int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr int m_bodyBPtrAndSignBit; int m_childIndexA; int m_childIndexB; - int m_unused1; + float m_unused1; int m_unused2; } Contact4; @@ -960,7 +960,7 @@ void trMul(float4 translationA, Quaternion orientationA, -__kernel void clipHullHullKernel( __global const int4* pairs, +__kernel void clipHullHullKernel( __global int4* pairs, __global const BodyData* rigidBodies, __global const btCollidableGpu* collidables, __global const ConvexPolyhedronCL* convexShapes, @@ -972,7 +972,8 @@ __kernel void clipHullHullKernel( __global const int4* pairs, __global const int* hasSeparatingAxis, __global Contact4* restrict globalContactsOut, counter32_t nGlobalContactsOut, - int numPairs) + int numPairs, + int contactCapacity) { int i = get_global_id(0); @@ -1032,8 +1033,10 @@ __kernel void clipHullHullKernel( __global const int4* pairs, int dstIdx; AppendInc( nGlobalContactsOut, dstIdx ); - //if ((dstIdx+nReducedContacts) < capacity) + if (dstIdxm_worldNormal = normal; c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16); @@ -1740,7 +1743,7 @@ __kernel void findClippingFacesKernel( __global const int4* pairs, -__kernel void clipFacesAndContactReductionKernel( __global const int4* pairs, +__kernel void clipFacesAndContactReductionKernel( __global int4* pairs, __global const BodyData* rigidBodies, __global const float4* separatingNormals, __global const int* hasSeparatingAxis, @@ -1853,7 +1856,7 @@ __kernel void clipFacesAndContactReductionKernel( __global const int4* pairs, -__kernel void newContactReductionKernel( __global const int4* pairs, +__kernel void newContactReductionKernel( __global int4* pairs, __global const BodyData* rigidBodies, __global const float4* separatingNormals, __global const int* hasSeparatingAxis, @@ -1897,12 +1900,16 @@ __kernel void newContactReductionKernel( __global const int4* pairs, if (dstIdx < numPairs) { + __global Contact4* c = &globalContactsOut[dstIdx]; c->m_worldNormal = normal; c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16); c->m_batchIdx = pairIndex; int bodyA = pairs[pairIndex].x; int bodyB = pairs[pairIndex].y; + + pairs[pairIndex].w = dstIdx; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; c->m_childIndexA =-1; diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h index 5496cc90e..ec4c581ed 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h @@ -962,7 +962,7 @@ static const char* satClipKernelsCL= \ "\n" "\n" "\n" -"__kernel void clipHullHullKernel( __global const int4* pairs, \n" +"__kernel void clipHullHullKernel( __global int4* pairs, \n" " __global const BodyData* rigidBodies, \n" " __global const btCollidableGpu* collidables,\n" " __global const ConvexPolyhedronCL* convexShapes, \n" @@ -974,7 +974,8 @@ static const char* satClipKernelsCL= \ " __global const int* hasSeparatingAxis,\n" " __global Contact4* restrict globalContactsOut,\n" " counter32_t nGlobalContactsOut,\n" -" int numPairs)\n" +" int numPairs,\n" +" int contactCapacity)\n" "{\n" "\n" " int i = get_global_id(0);\n" @@ -1034,8 +1035,10 @@ static const char* satClipKernelsCL= \ " \n" " int dstIdx;\n" " AppendInc( nGlobalContactsOut, dstIdx );\n" -" //if ((dstIdx+nReducedContacts) < capacity)\n" +" if (dstIdxm_worldNormal = normal;\n" " c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n" @@ -1742,7 +1745,7 @@ static const char* satClipKernelsCL= \ "\n" "\n" "\n" -"__kernel void clipFacesAndContactReductionKernel( __global const int4* pairs,\n" +"__kernel void clipFacesAndContactReductionKernel( __global int4* pairs,\n" " __global const BodyData* rigidBodies,\n" " __global const float4* separatingNormals,\n" " __global const int* hasSeparatingAxis,\n" @@ -1855,7 +1858,7 @@ static const char* satClipKernelsCL= \ "\n" "\n" "\n" -"__kernel void newContactReductionKernel( __global const int4* pairs,\n" +"__kernel void newContactReductionKernel( __global int4* pairs,\n" " __global const BodyData* rigidBodies,\n" " __global const float4* separatingNormals,\n" " __global const int* hasSeparatingAxis,\n" @@ -1899,12 +1902,16 @@ static const char* satClipKernelsCL= \ " \n" " if (dstIdx < numPairs)\n" " {\n" +"\n" " __global Contact4* c = &globalContactsOut[dstIdx];\n" " c->m_worldNormal = normal;\n" " c->m_coeffs = (u32)(0.f*0xffff) | ((u32)(0.7f*0xffff)<<16);\n" " c->m_batchIdx = pairIndex;\n" " int bodyA = pairs[pairIndex].x;\n" " int bodyB = pairs[pairIndex].y;\n" +"\n" +" pairs[pairIndex].w = dstIdx;\n" +"\n" " c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n" " c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n" " c->m_childIndexA =-1;\n" diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp index 27926d221..e6dca3085 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp @@ -25,14 +25,16 @@ m_queue(queue) { m_data = new b3GpuNarrowPhaseInternalData(); + m_data->m_currentContactBuffer = 0; + memset(m_data,0,sizeof(b3GpuNarrowPhaseInternalData)); m_data->m_config = config; m_data->m_gpuSatCollision = new GpuSatCollision(ctx,device,queue); - m_data->m_pBufPairsCPU = new b3AlignedObjectArray; - m_data->m_pBufPairsCPU->resize(config.m_maxBroadphasePairs); + + m_data->m_triangleConvexPairs = new b3OpenCLArray(m_context,m_queue, config.m_maxTriConvexPairCapacity); @@ -47,7 +49,8 @@ m_queue(queue) m_data->m_inertiaBufferCPU = new b3AlignedObjectArray(); m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies); - m_data->m_pBufContactOutGPU = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); + m_data->m_pBufContactBuffersGPU[0] = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); + m_data->m_pBufContactBuffersGPU[1] = new b3OpenCLArray(ctx,queue, config.m_maxContactCapacity,true); m_data->m_inertiaBufferGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexBodies,false); m_data->m_collidablesGPU = new b3OpenCLArray(ctx,queue,config.m_maxConvexShapes); @@ -111,14 +114,17 @@ m_queue(queue) b3GpuNarrowPhase::~b3GpuNarrowPhase() { delete m_data->m_gpuSatCollision; - delete m_data->m_pBufPairsCPU; + delete m_data->m_triangleConvexPairs; //delete m_data->m_convexPairsOutGPU; //delete m_data->m_planePairs; delete m_data->m_pBufContactOutCPU; delete m_data->m_bodyBufferCPU; delete m_data->m_inertiaBufferCPU; - delete m_data->m_pBufContactOutGPU; + delete m_data->m_pBufContactBuffersGPU[0]; + delete m_data->m_pBufContactBuffersGPU[1]; + + delete m_data->m_inertiaBufferGPU; delete m_data->m_collidablesGPU; delete m_data->m_localShapeAABBCPU; @@ -707,16 +713,16 @@ int b3GpuNarrowPhase::getNumCollidablesGpu() const int b3GpuNarrowPhase::getNumContactsGpu() const { - return m_data->m_pBufContactOutGPU->size(); + return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); } cl_mem b3GpuNarrowPhase::getContactsGpu() { - return m_data->m_pBufContactOutGPU->getBufferCL(); + return m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->getBufferCL(); } const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const { - m_data->m_pBufContactOutGPU->copyToHost(*m_data->m_pBufContactOutCPU); + m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->copyToHost(*m_data->m_pBufContactOutCPU); return &m_data->m_pBufContactOutCPU->at(0); } @@ -724,19 +730,29 @@ void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase { int nContactOut = 0; + //swap buffer + m_data->m_currentContactBuffer=1-m_data->m_currentContactBuffer; + + int curSize = m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer]->size(); + int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity; int numTriConvexPairsOut=0; b3OpenCLArray broadphasePairsGPU(m_context,m_queue); broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs); + + + + b3OpenCLArray clAabbArray(this->m_context,this->m_queue); clAabbArray.setFromOpenCLBuffer(aabbsWS,numObjects); m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT( &broadphasePairsGPU, numBroadphasePairs, m_data->m_bodyBufferGPU, - m_data->m_pBufContactOutGPU, + m_data->m_pBufContactBuffersGPU[m_data->m_currentContactBuffer], nContactOut, + m_data->m_pBufContactBuffersGPU[1-m_data->m_currentContactBuffer], m_data->m_config.m_maxContactCapacity, m_data->m_config.m_compoundPairCapacity, *m_data->m_convexPolyhedraGPU, @@ -762,6 +778,10 @@ void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphase numTriConvexPairsOut ); + /*b3AlignedObjectArray broadphasePairsCPU; + broadphasePairsGPU.copyToHost(broadphasePairsCPU); + printf("checking pairs\n"); + */ } const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h index 45937a524..20d35388c 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h +++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h @@ -50,13 +50,12 @@ struct b3GpuNarrowPhaseInternalData struct GpuSatCollision* m_gpuSatCollision; - b3AlignedObjectArray* m_pBufPairsCPU; + b3OpenCLArray* m_triangleConvexPairs; - //b3OpenCLArray* m_convexPairsOutGPU; - //b3OpenCLArray* m_planePairs; - - b3OpenCLArray* m_pBufContactOutGPU; + + b3OpenCLArray* m_pBufContactBuffersGPU[2]; + int m_currentContactBuffer; b3AlignedObjectArray* m_pBufContactOutCPU; diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp index 5af4cfb39..5f9aaf1fb 100644 --- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -276,6 +276,12 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies); numContacts = m_data->m_narrowphase->getNumContactsGpu(); + if (useDbvt) + { + ///store the cached information (contact locations in the 'z' component) + B3_PROFILE("m_overlappingPairsGPU->copyToHost"); + m_data->m_overlappingPairsGPU->copyToHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); + } if (dumpContactStats && numContacts) { m_data->m_narrowphase->getContactsGpu();