diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch')
15 files changed, 361 insertions, 89 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index c81af95672..e5bac8438e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -132,6 +132,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po else { // Could be inside one of the contact capsules btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold; + btScalar minDistSqr = contactCapsuleRadiusSqr; btVector3 nearestOnEdge; for (int i = 0; i < m_triangle->getNumEdges(); i++) { @@ -141,8 +142,9 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po m_triangle->getEdge(i, pa, pb); btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge); - if (distanceSqr < contactCapsuleRadiusSqr) { - // Yep, we're inside a capsule + if (distanceSqr < minDistSqr) { + // Yep, we're inside a capsule, and record the capsule with smallest distance + minDistSqr = distanceSqr; hasContact = true; contactPoint = nearestOnEdge; } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp index 2c36277821..cabbb0bf6a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp @@ -151,8 +151,8 @@ static btScalar EdgeSeparation(const btBox2dShape* poly1, const btTransform& xf1 int index = 0; btScalar minDot = BT_LARGE_FLOAT; - if( count2 > 0 ) - index = (int) normal1.minDot( vertices2, count2, minDot); + if( count2 > 0 ) + index = (int) normal1.minDot( vertices2, count2, minDot); btVector3 v1 = b2Mul(xf1, vertices1[edge1]); btVector3 v2 = b2Mul(xf2, vertices2[index]); @@ -175,8 +175,8 @@ static btScalar FindMaxSeparation(int* edgeIndex, // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; btScalar maxDot; - if( count1 > 0 ) - edge = (int) dLocal1.maxDot( normals1, count1, maxDot); + if( count1 > 0 ) + edge = (int) dLocal1.maxDot( normals1, count1, maxDot); // Get the separation for the edge normal. btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2); diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 5739a1ef01..f8794dec47 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -27,8 +27,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" -int gNumManifold = 0; - #ifdef BT_DEBUG #include <stdio.h> #endif @@ -77,8 +75,6 @@ btCollisionDispatcher::~btCollisionDispatcher() btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0,const btCollisionObject* body1) { - gNumManifold++; - //btAssert(gNumManifold < 65535); @@ -121,7 +117,6 @@ void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold) void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) { - gNumManifold--; //printf("releaseManifold: gNumManifold %d\n",gNumManifold); clearManifold(manifold); @@ -246,13 +241,17 @@ public: + void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,const btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) { //m_blockedForChanges = true; btCollisionPairCallback collisionCallback(dispatchInfo,this); - pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); + { + BT_PROFILE("processAllOverlappingPairs"); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher, dispatchInfo); + } //m_blockedForChanges = false; diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp index b595c56bc5..05f96a14bc 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -16,6 +16,7 @@ subject to the following restrictions: #include "btCollisionObject.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" btCollisionObject::btCollisionObject() : m_interpolationLinearVelocity(0.f, 0.f, 0.f), @@ -38,7 +39,7 @@ btCollisionObject::btCollisionObject() m_rollingFriction(0.0f), m_spinningFriction(0.f), m_contactDamping(.1), - m_contactStiffness(1e4), + m_contactStiffness(BT_LARGE_FLOAT), m_internalType(CO_COLLISION_OBJECT), m_userObjectPointer(0), m_userIndex2(-1), @@ -114,10 +115,18 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali dataOut->m_ccdSweptSphereRadius = m_ccdSweptSphereRadius; dataOut->m_ccdMotionThreshold = m_ccdMotionThreshold; dataOut->m_checkCollideWith = m_checkCollideWith; - - // Fill padding with zeros to appease msan. - memset(dataOut->m_padding, 0, sizeof(dataOut->m_padding)); - + if (m_broadphaseHandle) + { + dataOut->m_collisionFilterGroup = m_broadphaseHandle->m_collisionFilterGroup; + dataOut->m_collisionFilterMask = m_broadphaseHandle->m_collisionFilterMask; + dataOut->m_uniqueId = m_broadphaseHandle->m_uniqueId; + } + else + { + dataOut->m_collisionFilterGroup = 0; + dataOut->m_collisionFilterMask = 0; + dataOut->m_uniqueId = -1; + } return btCollisionObjectDataName; } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h index fec831bffc..135f8a033c 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -621,7 +621,6 @@ struct btCollisionObjectDoubleData double m_hitFraction; double m_ccdSweptSphereRadius; double m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -629,8 +628,9 @@ struct btCollisionObjectDoubleData int m_activationState1; int m_internalType; int m_checkCollideWith; - - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 @@ -650,13 +650,12 @@ struct btCollisionObjectFloatData float m_deactivationTime; float m_friction; float m_rollingFriction; - float m_contactDamping; + float m_contactDamping; float m_contactStiffness; float m_restitution; float m_hitFraction; float m_ccdSweptSphereRadius; float m_ccdMotionThreshold; - int m_hasAnisotropicFriction; int m_collisionFlags; int m_islandTag1; @@ -664,7 +663,9 @@ struct btCollisionObjectFloatData int m_activationState1; int m_internalType; int m_checkCollideWith; - char m_padding[4]; + int m_collisionFilterGroup; + int m_collisionFilterMask; + int m_uniqueId; }; diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index c3e912fdca..3de8d6995e 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1646,7 +1646,7 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) for (i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; - if ((colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) || (colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)) + if (colObj->getInternalType() == btCollisionObject::CO_COLLISION_OBJECT) { colObj->serializeSingleObject(serializer); } @@ -1654,12 +1654,36 @@ void btCollisionWorld::serializeCollisionObjects(btSerializer* serializer) } + +void btCollisionWorld::serializeContactManifolds(btSerializer* serializer) +{ + if (serializer->getSerializationFlags() & BT_SERIALIZE_CONTACT_MANIFOLDS) + { + int numManifolds = getDispatcher()->getNumManifolds(); + for (int i = 0; i < numManifolds; i++) + { + const btPersistentManifold* manifold = getDispatcher()->getInternalManifoldPointer()[i]; + //don't serialize empty manifolds, they just take space + //(may have to do it anyway if it destroys determinism) + if (manifold->getNumContacts() == 0) + continue; + + btChunk* chunk = serializer->allocate(manifold->calculateSerializeBufferSize(), 1); + const char* structType = manifold->serialize(manifold, chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONTACTMANIFOLD_CODE, (void*)manifold); + } + } +} + + void btCollisionWorld::serialize(btSerializer* serializer) { serializer->startSerialization(); serializeCollisionObjects(serializer); + + serializeContactManifolds(serializer); serializer->finishSerialization(); } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h index eede2b28ca..886476e8ad 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -107,6 +107,9 @@ protected: void serializeCollisionObjects(btSerializer* serializer); + void serializeContactManifolds(btSerializer* serializer); + + public: //this constructor doesn't own the dispatcher and paircache/broadphase diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 7f4dea1c6d..91b7809c17 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -156,10 +156,12 @@ public: btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); btCollisionAlgorithm* algo = 0; + bool allocatedAlgorithm = false; if (m_resultOut->m_closestPointDistanceThreshold > 0) { algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + allocatedAlgorithm = true; } else { @@ -204,7 +206,11 @@ public: { m_resultOut->setBody1Wrap(tmpWrap); } - + if(allocatedAlgorithm) + { + algo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(algo); + } } } void Process(const btDbvtNode* leaf) @@ -253,9 +259,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap m_compoundShapeRevision = compoundShape->getUpdateRevision(); } - if (m_childCollisionAlgorithms.size()==0) - return; - + if (m_childCollisionAlgorithms.size()==0) + return; + const btDbvt* tree = compoundShape->getDynamicAabbTree(); //use a dynamic aabb tree to cull potential child-overlaps btCompoundLeafCallback callback(colObjWrap,otherObjWrap,m_dispatcher,dispatchInfo,resultOut,&m_childCollisionAlgorithms[0],m_sharedManifold); diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index d4a1aa78e4..20b542f670 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -181,11 +181,12 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); - + bool removePair = false; btCollisionAlgorithm* colAlgo = 0; if (m_resultOut->m_closestPointDistanceThreshold > 0) { colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + removePair = true; } else { @@ -223,7 +224,11 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide m_resultOut->setBody0Wrap(tmpWrap0); m_resultOut->setBody1Wrap(tmpWrap1); - + if (removePair) + { + colAlgo->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(colAlgo); + } } } @@ -396,32 +401,24 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb btCollisionAlgorithm* algo = (btCollisionAlgorithm*)pairs[i].m_userPointer; { - btTransform orgTrans0; const btCollisionShape* childShape0 = 0; btTransform newChildWorldTrans0; - btTransform orgInterpolationTrans0; childShape0 = compoundShape0->getChildShape(pairs[i].m_indexA); - orgTrans0 = col0ObjWrap->getWorldTransform(); - orgInterpolationTrans0 = col0ObjWrap->getWorldTransform(); const btTransform& childTrans0 = compoundShape0->getChildTransform(pairs[i].m_indexA); - newChildWorldTrans0 = orgTrans0*childTrans0 ; + newChildWorldTrans0 = col0ObjWrap->getWorldTransform()*childTrans0 ; childShape0->getAabb(newChildWorldTrans0,aabbMin0,aabbMax0); } btVector3 thresholdVec(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); aabbMin0 -= thresholdVec; aabbMax0 += thresholdVec; { - btTransform orgInterpolationTrans1; const btCollisionShape* childShape1 = 0; - btTransform orgTrans1; btTransform newChildWorldTrans1; childShape1 = compoundShape1->getChildShape(pairs[i].m_indexB); - orgTrans1 = col1ObjWrap->getWorldTransform(); - orgInterpolationTrans1 = col1ObjWrap->getWorldTransform(); const btTransform& childTrans1 = compoundShape1->getChildTransform(pairs[i].m_indexB); - newChildWorldTrans1 = orgTrans1*childTrans1 ; + newChildWorldTrans1 = col1ObjWrap->getWorldTransform()*childTrans1 ; childShape1->getAabb(newChildWorldTrans1,aabbMin1,aabbMax1); } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 39ff7934d9..d8cbe96142 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -27,6 +27,7 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h" +#include "BulletCollision/CollisionShapes/btSdfCollisionShape.h" btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped) : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap), @@ -152,7 +153,7 @@ partId, int triangleIndex) { m_resultOut->setBody1Wrap(tmpWrap); } - + colAlgo->~btCollisionAlgorithm(); @@ -163,7 +164,7 @@ partId, int triangleIndex) -void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) +void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut) { m_convexBodyWrap = convexBodyWrap; m_triBodyWrap = triBodyWrap; @@ -177,14 +178,14 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform(); const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); - convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold; - - btVector3 extra(extraMargin,extraMargin,extraMargin); + convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax); + btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold; + + btVector3 extra(extraMargin, extraMargin, extraMargin); m_aabbMax += extra; m_aabbMin -= extra; - + } void btConvexConcaveCollisionAlgorithm::clearCache() @@ -193,35 +194,99 @@ void btConvexConcaveCollisionAlgorithm::clearCache() } -void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) +void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut) { BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision"); - + const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap; const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap; if (triBodyWrap->getCollisionShape()->isConcave()) { + if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE) + { + btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape(); + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape(); + btAlignedObjectArray<btVector3> queryVertices; + + if (convex->isPolyhedral()) + { + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < poly->getNumVertices(); v++) + { + btVector3 vtx; + poly->getVertex(v, vtx); + queryVertices.push_back(vtx); + } + } + btScalar maxDist = SIMD_EPSILON; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + queryVertices.push_back(btVector3(0, 0, 0)); + btSphereShape* sphere = (btSphereShape*)convex; + maxDist = sphere->getRadius() + SIMD_EPSILON; + + } + if (queryVertices.size()) + { + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold(); + + btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; + for (int v = 0; v < queryVertices.size(); v++) + { + const btVector3& vtx = queryVertices[v]; + btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform()*vtx; + btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace); + + btVector3 normalLocal; + btScalar dist; + if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal)) + { + if (dist <= maxDist) + { + normalLocal.safeNormalize(); + btVector3 normal = triBodyWrap->getWorldTransform().getBasis()*normalLocal; + + if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE) + { + btSphereShape* sphere = (btSphereShape*)convex; + dist -= sphere->getRadius(); + vtxWorldSpace -= sphere->getRadius()*normal; + + } + resultOut->addContactPoint(normal,vtxWorldSpace-normal*dist, dist); + } + } + } + resultOut->refreshContactPoints(); + } - - const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape()); - - if (convexBodyWrap->getCollisionShape()->isConvex()) + } + } else { - btScalar collisionMarginTriangle = concaveShape->getMargin(); + const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape()); + + if (convexBodyWrap->getCollisionShape()->isConvex()) + { + btScalar collisionMarginTriangle = concaveShape->getMargin(); - resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); - m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); + resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr); + m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut); - m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); + m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject()); - concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); + concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax()); - resultOut->refreshContactPoints(); + resultOut->refreshContactPoints(); - m_btConvexTriangleCallback.clearWrapperData(); + m_btConvexTriangleCallback.clearWrapperData(); + } } } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index b54bd48932..3e8bc6e426 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -28,6 +28,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" @@ -259,7 +260,7 @@ struct btPerturbedContactResult : public btManifoldResult btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth; endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg); newDepth = (endPt - pointInWorld).dot(normalOnBInWorld); - startPt = endPt+normalOnBInWorld*newDepth; + startPt = endPt - normalOnBInWorld*newDepth; } else { endPt = pointInWorld + normalOnBInWorld*orgDepth; @@ -442,10 +443,26 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result { + btVector3 m_normalOnBInWorld; + btVector3 m_pointInWorld; + btScalar m_depth; + bool m_hasContact; + + + btDummyResult() + : m_hasContact(false) + { + } + + virtual void setShapeIdentifiersA(int partId0,int index0){} virtual void setShapeIdentifiersB(int partId1,int index1){} virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) { + m_hasContact = true; + m_normalOnBInWorld = normalOnBInWorld; + m_pointInWorld = pointInWorld; + m_depth = depth; } }; @@ -560,15 +577,18 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* } else { + + //we can also deal with convex versus triangle (without connectivity data) - if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + if (dispatchInfo.m_enableSatConvex && polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) { - btVertexArray vertices; + + btVertexArray worldSpaceVertices; btTriangleShape* tri = (btTriangleShape*)polyhedronB; - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); - vertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); + worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[0]); + worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[1]); + worldSpaceVertices.push_back( body1Wrap->getWorldTransform()*tri->m_vertices1[2]); //tri->initializePolyhedralFeatures(); @@ -579,17 +599,99 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* btScalar maxDist = threshold; bool foundSepAxis = false; - if (0) + bool useSatSepNormal = true; + + if (useSatSepNormal) { - polyhedronB->initializePolyhedralFeatures(); +#if 0 + if (0) + { + //initializePolyhedralFeatures performs a convex hull computation, not needed for a single triangle + polyhedronB->initializePolyhedralFeatures(); + } else +#endif + { + + btVector3 uniqueEdges[3] = {tri->m_vertices1[1]-tri->m_vertices1[0], + tri->m_vertices1[2]-tri->m_vertices1[1], + tri->m_vertices1[0]-tri->m_vertices1[2]}; + + uniqueEdges[0].normalize(); + uniqueEdges[1].normalize(); + uniqueEdges[2].normalize(); + + btConvexPolyhedron polyhedron; + polyhedron.m_vertices.push_back(tri->m_vertices1[2]); + polyhedron.m_vertices.push_back(tri->m_vertices1[0]); + polyhedron.m_vertices.push_back(tri->m_vertices1[1]); + + + { + btFace combinedFaceA; + combinedFaceA.m_indices.push_back(0); + combinedFaceA.m_indices.push_back(1); + combinedFaceA.m_indices.push_back(2); + btVector3 faceNormal = uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq=1e30f; + for (int v=0;v<combinedFaceA.m_indices.size();v++) + { + btScalar eq = tri->m_vertices1[combinedFaceA.m_indices[v]].dot(faceNormal); + if (planeEq>eq) + { + planeEq=eq; + } + } + combinedFaceA.m_plane[0] = faceNormal[0]; + combinedFaceA.m_plane[1] = faceNormal[1]; + combinedFaceA.m_plane[2] = faceNormal[2]; + combinedFaceA.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceA); + } + { + btFace combinedFaceB; + combinedFaceB.m_indices.push_back(0); + combinedFaceB.m_indices.push_back(2); + combinedFaceB.m_indices.push_back(1); + btVector3 faceNormal = -uniqueEdges[0].cross(uniqueEdges[1]); + faceNormal.normalize(); + btScalar planeEq=1e30f; + for (int v=0;v<combinedFaceB.m_indices.size();v++) + { + btScalar eq = tri->m_vertices1[combinedFaceB.m_indices[v]].dot(faceNormal); + if (planeEq>eq) + { + planeEq=eq; + } + } + + combinedFaceB.m_plane[0] = faceNormal[0]; + combinedFaceB.m_plane[1] = faceNormal[1]; + combinedFaceB.m_plane[2] = faceNormal[2]; + combinedFaceB.m_plane[3] = -planeEq; + polyhedron.m_faces.push_back(combinedFaceB); + } + + + polyhedron.m_uniqueEdges.push_back(uniqueEdges[0]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[1]); + polyhedron.m_uniqueEdges.push_back(uniqueEdges[2]); + polyhedron.initialize2(); + + polyhedronB->setPolyhedralFeatures(polyhedron); + } + + + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( - *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), - body1Wrap->getWorldTransform(), - sepNormalWorldSpace,*resultOut); + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0Wrap->getWorldTransform(), + body1Wrap->getWorldTransform(), + sepNormalWorldSpace,*resultOut); // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); - } else + } + else { #ifdef ZERO_MARGIN gjkPairDetector.setIgnoreMargin(true); @@ -598,6 +700,24 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); #endif//ZERO_MARGIN + if (dummy.m_hasContact && dummy.m_depth<0) + { + + if (foundSepAxis) + { + if (dummy.m_normalOnBInWorld.dot(sepNormalWorldSpace)<0.99) + { + printf("?\n"); + } + } else + { + printf("!\n"); + } + sepNormalWorldSpace.setValue(0,0,1);// = dummy.m_normalOnBInWorld; + //minDist = dummy.m_depth; + foundSepAxis = true; + } +#if 0 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); if (l2>SIMD_EPSILON) { @@ -607,6 +727,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); foundSepAxis = true; } +#endif } @@ -614,7 +735,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* { worldVertsB2.resize(0); btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), - body0Wrap->getWorldTransform(), vertices, worldVertsB2,minDist-threshold, maxDist, *resultOut); + body0Wrap->getWorldTransform(), worldSpaceVertices, worldVertsB2,minDist-threshold, maxDist, *resultOut); } @@ -625,6 +746,7 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* return; } + } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp index 8c8a7c3c1e..8271981b29 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp @@ -20,11 +20,12 @@ subject to the following restrictions: #include <stdio.h> +#ifdef BT_DEBUG_COLLISION_PAIRS int gOverlappingSimplePairs = 0; int gRemoveSimplePairs =0; int gAddedSimplePairs =0; int gFindSimplePairs =0; - +#endif //BT_DEBUG_COLLISION_PAIRS @@ -61,7 +62,9 @@ void btHashedSimplePairCache::removeAllPairs() btSimplePair* btHashedSimplePairCache::findPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gFindSimplePairs++; +#endif /*if (indexA > indexB) @@ -172,7 +175,9 @@ btSimplePair* btHashedSimplePairCache::internalAddPair(int indexA, int indexB) void* btHashedSimplePairCache::removeOverlappingPair(int indexA, int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gRemoveSimplePairs++; +#endif /*if (indexA > indexB) diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h index 2aaf6201f3..318981cda1 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h @@ -43,12 +43,12 @@ struct btSimplePair typedef btAlignedObjectArray<btSimplePair> btSimplePairArray; - +#ifdef BT_DEBUG_COLLISION_PAIRS extern int gOverlappingSimplePairs; extern int gRemoveSimplePairs; extern int gAddedSimplePairs; extern int gFindSimplePairs; - +#endif //BT_DEBUG_COLLISION_PAIRS @@ -75,7 +75,9 @@ public: // no new pair is created and the old one is returned. virtual btSimplePair* addOverlappingPair(int indexA,int indexB) { +#ifdef BT_DEBUG_COLLISION_PAIRS gAddedSimplePairs++; +#endif return internalAddPair(indexA,indexB); } diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 6cba442ca5..898320ee1a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -455,11 +455,20 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObjectWr btBvhTriangleMeshShape* trimesh = 0; if( colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) - trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); - else - trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); - - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + { + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape())->getChildShape(); + } + else + { + if (colObj0Wrap->getCollisionObject()->getCollisionShape()->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + trimesh = (btBvhTriangleMeshShape*)colObj0Wrap->getCollisionObject()->getCollisionShape(); + } + } + if (trimesh==0) + return; + + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 1344782257..91c76a8dac 100644 --- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -199,6 +199,22 @@ class btPersistentManifoldSortPredicate } }; +class btPersistentManifoldSortPredicateDeterministic +{ +public: + + SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const + { + return ( + (getIslandId(lhs) < getIslandId(rhs)) + || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) + ||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && + (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId)) + ); + + } +}; + void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld) { @@ -245,13 +261,11 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); if (colObj0->getIslandTag() == islandId) { - if (colObj0->getActivationState()== ACTIVE_TAG) - { - allSleeping = false; - } - if (colObj0->getActivationState()== DISABLE_DEACTIVATION) + if (colObj0->getActivationState()== ACTIVE_TAG || + colObj0->getActivationState()== DISABLE_DEACTIVATION) { allSleeping = false; + break; } } } @@ -318,7 +332,12 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio for (i=0;i<maxNumManifolds ;i++) { btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); - + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + if (manifold->getNumContacts() == 0) + continue; + } + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); @@ -379,7 +398,16 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, //tried a radix sort, but quicksort/heapsort seems still faster //@todo rewrite island management - m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //btPersistentManifoldSortPredicateDeterministic sorts contact manifolds based on islandid, + //but also based on object0 unique id and object1 unique id + if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs) + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic()); + } else + { + m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + } + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) |