diff options
Diffstat (limited to 'thirdparty/bullet/BulletCollision')
46 files changed, 2877 insertions, 397 deletions
diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h index 2c4d41bc04..323aa96dca 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h @@ -628,7 +628,6 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher* } -extern int gOverlappingPairs; //#include <stdio.h> template <typename BP_FP_INT_TYPE> @@ -695,10 +694,9 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatche pair.m_pProxy0 = 0; pair.m_pProxy1 = 0; m_invalidPair++; - gOverlappingPairs--; - } - - } + } + + } ///if you don't like to skip the invalid pairs in the array, execute following code: #define CLEAN_INVALID_PAIRS 1 diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index adaf083a21..f6e1202a69 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -66,6 +66,7 @@ CONCAVE_SHAPES_START_HERE, EMPTY_SHAPE_PROXYTYPE, STATIC_PLANE_PROXYTYPE, CUSTOM_CONCAVE_SHAPE_TYPE, + SDF_SHAPE_PROXYTYPE=CUSTOM_CONCAVE_SHAPE_TYPE, CONCAVE_SHAPES_END_HERE, COMPOUND_SHAPE_PROXYTYPE, diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 4d12b1c9c7..14cd1a31ea 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -17,7 +17,7 @@ subject to the following restrictions: #include "btDbvtBroadphase.h" #include "LinearMath/btThreads.h" - +btScalar gDbvtMargin = btScalar(0.05); // // Profiling // @@ -332,12 +332,9 @@ void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, if(delta[0]<0) velocity[0]=-velocity[0]; if(delta[1]<0) velocity[1]=-velocity[1]; if(delta[2]<0) velocity[2]=-velocity[2]; - if ( -#ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) -#else - m_sets[0].update(proxy->leaf,aabb,velocity) -#endif + if ( + m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin) + ) { ++m_updates_done; diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index 8feb95d51f..90b333d846 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -29,7 +29,8 @@ subject to the following restrictions: #define DBVT_BP_PREVENTFALSEUPDATE 0 #define DBVT_BP_ACCURATESLEEPING 0 #define DBVT_BP_ENABLE_BENCHMARK 0 -#define DBVT_BP_MARGIN (btScalar)0.05 +//#define DBVT_BP_MARGIN (btScalar)0.05 +extern btScalar gDbvtMargin; #if DBVT_BP_PROFILE #define DBVT_BP_PROFILING_RATE 256 diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h index 7b0f9489af..a0e4c18927 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -46,7 +46,8 @@ struct btDispatcherInfo m_useEpa(true), m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), - m_convexConservativeDistanceThreshold(0.0f) + m_convexConservativeDistanceThreshold(0.0f), + m_deterministicOverlappingPairs(false) { } @@ -62,6 +63,7 @@ struct btDispatcherInfo btScalar m_allowedCcdPenetration; bool m_useConvexConservativeDistanceUtil; btScalar m_convexConservativeDistanceThreshold; + bool m_deterministicOverlappingPairs; }; enum ebtDispatcherQueryType diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index 55ebf06f1e..9e3337c5f6 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -23,11 +23,6 @@ subject to the following restrictions: #include <stdio.h> -int gOverlappingPairs = 0; - -int gRemovePairs =0; -int gAddedPairs =0; -int gFindPairs =0; @@ -134,13 +129,12 @@ void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroad btBroadphasePair* btHashedOverlappingPairCache::findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) { - gFindPairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) + if(proxy0->m_uniqueId>proxy1->m_uniqueId) btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - /*if (proxyId1 > proxyId2) + /*if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2);*/ int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); @@ -271,13 +265,12 @@ btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(btBroadphaseProx void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher) { - gRemovePairs++; - if(proxy0->m_uniqueId>proxy1->m_uniqueId) + if(proxy0->m_uniqueId>proxy1->m_uniqueId) btSwap(proxy0,proxy1); int proxyId1 = proxy0->getUid(); int proxyId2 = proxy1->getUid(); - /*if (proxyId1 > proxyId2) + /*if (proxyId1 > proxyId2) btSwap(proxyId1, proxyId2);*/ int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); @@ -386,8 +379,6 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* if (callback->processOverlap(*pair)) { removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); - - gOverlappingPairs--; } else { i++; @@ -395,6 +386,70 @@ void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* } } +struct MyPairIndex +{ + int m_orgIndex; + int m_uidA0; + int m_uidA1; +}; + +class MyPairIndeSortPredicate +{ +public: + + bool operator() ( const MyPairIndex& a, const MyPairIndex& b ) const + { + const int uidA0 = a.m_uidA0; + const int uidB0 = b.m_uidA0; + const int uidA1 = a.m_uidA1; + const int uidB1 = b.m_uidA1; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) +{ + if (dispatchInfo.m_deterministicOverlappingPairs) + { + btBroadphasePairArray& pa = getOverlappingPairArray(); + btAlignedObjectArray<MyPairIndex> indices; + { + BT_PROFILE("sortOverlappingPairs"); + indices.resize(pa.size()); + for (int i=0;i<indices.size();i++) + { + const btBroadphasePair& p = pa[i]; + const int uidA0 = p.m_pProxy0 ? p.m_pProxy0->m_uniqueId : -1; + const int uidA1 = p.m_pProxy1 ? p.m_pProxy1->m_uniqueId : -1; + + indices[i].m_uidA0 = uidA0; + indices[i].m_uidA1 = uidA1; + indices[i].m_orgIndex = i; + } + indices.quickSort(MyPairIndeSortPredicate()); + } + { + BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs"); + int i; + for (i=0;i<indices.size();) + { + btBroadphasePair* pair = &pa[indices[i].m_orgIndex]; + if (callback->processOverlap(*pair)) + { + removeOverlappingPair(pair->m_pProxy0,pair->m_pProxy1,dispatcher); + } else + { + i++; + } + } + } + } else + { + processAllOverlappingPairs(callback, dispatcher); + } +} + + void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) { ///need to keep hashmap in sync with pair address, so rebuild all @@ -435,7 +490,6 @@ void* btSortedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro int findIndex = m_overlappingPairArray.findLinearSearch(findPair); if (findIndex < m_overlappingPairArray.size()) { - gOverlappingPairs--; btBroadphasePair& pair = m_overlappingPairArray[findIndex]; void* userData = pair.m_internalInfo1; cleanOverlappingPair(pair,dispatcher); @@ -468,11 +522,8 @@ btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(btBroadphaseP void* mem = &m_overlappingPairArray.expandNonInitializing(); btBroadphasePair* pair = new (mem) btBroadphasePair(*proxy0,*proxy1); - - gOverlappingPairs++; - gAddedPairs++; - - if (m_ghostPairCallback) + + if (m_ghostPairCallback) m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); return pair; @@ -526,7 +577,6 @@ void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* pair->m_pProxy1 = 0; m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); m_overlappingPairArray.pop_back(); - gOverlappingPairs--; } else { i++; @@ -559,7 +609,6 @@ void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,b pair.m_algorithm->~btCollisionAlgorithm(); dispatcher->freeCollisionAlgorithm(pair.m_algorithm); pair.m_algorithm=0; - gRemovePairs--; } } } diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index f7be7d45b3..7a38d34f05 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -49,10 +49,6 @@ struct btOverlapFilterCallback -extern int gRemovePairs; -extern int gAddedPairs; -extern int gFindPairs; - const int BT_NULL_PAIR=0xffffffff; ///The btOverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. @@ -78,6 +74,10 @@ public: virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo) + { + processAllOverlappingPairs(callback, dispatcher); + } virtual btBroadphasePair* findPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) = 0; virtual bool hasDeferredRemoval() = 0; @@ -129,8 +129,6 @@ public: // no new pair is created and the old one is returned. virtual btBroadphasePair* addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) { - gAddedPairs++; - if (!needsBroadphaseCollision(proxy0,proxy1)) return 0; @@ -144,6 +142,8 @@ public: virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + virtual void processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher, const struct btDispatcherInfo& dispatchInfo); + virtual btBroadphasePair* getOverlappingPairArrayPtr() { return &m_overlappingPairArray[0]; diff --git a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index f1d5f5476e..5f89f960e8 100644 --- a/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -24,8 +24,6 @@ subject to the following restrictions: #include <new> -extern int gOverlappingPairs; - void btSimpleBroadphase::validate() { for (int i=0;i<m_numHandles;i++) @@ -315,8 +313,7 @@ void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) pair.m_pProxy0 = 0; pair.m_pProxy1 = 0; m_invalidPair++; - gOverlappingPairs--; - } + } } 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) diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h index 7d64b46abf..5a3362834a 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h @@ -49,6 +49,7 @@ public: virtual void setMargin(btScalar collisionMargin) { //don't override the margin for capsules, their entire radius == margin + (void)collisionMargin; } virtual void getAabb (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp index e8c8c336cd..85572da307 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp @@ -213,7 +213,7 @@ void btCompoundShape::calculateLocalInertia(btScalar mass,btVector3& inertia) co -void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const +void btCompoundShape::calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const { int n = m_children.size(); diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h index 4eef8dba30..2cbcd1bfca 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h @@ -160,7 +160,7 @@ public: ///"principal" has to be applied inversely to all children transforms in order for the local coordinate system of the compound ///shape to be centered at the center of mass and to coincide with the principal axes. This also necessitates a correction of the world transform ///of the collision object by the principal transform. - void calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const; + void calculatePrincipalAxisTransform(const btScalar* masses, btTransform& principal, btVector3& inertia) const; int getUpdateRevision() const { diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp index 4f45319a83..0fea00df5c 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -104,9 +104,9 @@ void btConvexPolyhedron::initialize() btHashMap<btInternalVertexPair,btInternalEdge> edges; - btScalar TotalArea = 0.0f; - m_localCenter.setValue(0, 0, 0); + + for(int i=0;i<m_faces.size();i++) { int numVertices = m_faces[i].m_indices.size(); @@ -172,6 +172,13 @@ void btConvexPolyhedron::initialize() } #endif//USE_CONNECTED_FACES + initialize2(); +} + +void btConvexPolyhedron::initialize2() +{ + m_localCenter.setValue(0, 0, 0); + btScalar TotalArea = 0.0f; for(int i=0;i<m_faces.size();i++) { int numVertices = m_faces[i].m_indices.size(); @@ -274,7 +281,6 @@ void btConvexPolyhedron::initialize() } #endif } - void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const { minProj = FLT_MAX; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h index d3cd066ac8..c5aa20f453 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -54,6 +54,7 @@ ATTRIBUTE_ALIGNED16(class) btConvexPolyhedron btVector3 mE; void initialize(); + void initialize2(); bool testContainment() const; void project(const btTransform& trans, const btVector3& dir, btScalar& minProj, btScalar& maxProj, btVector3& witnesPtMin,btVector3& witnesPtMax) const; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp index 8d7fb054d6..2f84858598 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -118,9 +118,13 @@ static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector return supVec; #else - btScalar maxDot; - long ptIndex = vec.maxDot( points, numPoints, maxDot); + btScalar maxDot; + long ptIndex = vec.maxDot( points, numPoints, maxDot); btAssert(ptIndex >= 0); + if (ptIndex<0) + { + ptIndex = 0; + } btVector3 supVec = points[ptIndex] * localScaling; return supVec; #endif //__SPU__ diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp new file mode 100644 index 0000000000..afe45e1d2d --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp @@ -0,0 +1,532 @@ +#include "btMiniSDF.h" + +// +//Based on code from DiscreGrid, https://github.com/InteractiveComputerGraphics/Discregrid +//example: +//GenerateSDF.exe -r "32 32 32" -d "-1.6 -1.6 -.6 1.6 1.6 .6" concave_box.obj +//The MIT License (MIT) +// +//Copyright (c) 2017 Dan Koschier +// + +#include <limits.h> +#include <string.h> //memcpy + +struct btSdfDataStream +{ + const char* m_data; + int m_size; + + int m_currentOffset; + + btSdfDataStream(const char* data, int size) + :m_data(data), + m_size(size), + m_currentOffset(0) + { + + } + + template<class T> bool read(T& val) + { + int bytes = sizeof(T); + if (m_currentOffset+bytes<=m_size) + { + char* dest = (char*)&val; + memcpy(dest,&m_data[m_currentOffset],bytes); + m_currentOffset+=bytes; + return true; + } + btAssert(0); + return false; + } +}; + + +bool btMiniSDF::load(const char* data, int size) +{ + int fileSize = -1; + + btSdfDataStream ds(data,size); + { + double buf[6]; + ds.read(buf); + m_domain.m_min[0] = buf[0]; + m_domain.m_min[1] = buf[1]; + m_domain.m_min[2] = buf[2]; + m_domain.m_min[3] = 0; + m_domain.m_max[0] = buf[3]; + m_domain.m_max[1] = buf[4]; + m_domain.m_max[2] = buf[5]; + m_domain.m_max[3] = 0; + } + { + unsigned int buf2[3]; + ds.read(buf2); + m_resolution[0] = buf2[0]; + m_resolution[1] = buf2[1]; + m_resolution[2] = buf2[2]; + } + { + double buf[3]; + ds.read(buf); + m_cell_size[0] = buf[0]; + m_cell_size[1] = buf[1]; + m_cell_size[2] = buf[2]; + } + { + double buf[3]; + ds.read(buf); + m_inv_cell_size[0] = buf[0]; + m_inv_cell_size[1] = buf[1]; + m_inv_cell_size[2] = buf[2]; + } + { + unsigned long long int cells; + ds.read(cells); + m_n_cells = cells; + } + { + unsigned long long int fields; + ds.read(fields); + m_n_fields = fields; + } + + + unsigned long long int nodes0; + std::size_t n_nodes0; + ds.read(nodes0); + n_nodes0 = nodes0; + if (n_nodes0 > 1024 * 1024 * 1024) + { + return m_isValid; + } + m_nodes.resize(n_nodes0); + for (unsigned int i=0;i<n_nodes0;i++) + { + unsigned long long int n_nodes1; + ds.read(n_nodes1); + btAlignedObjectArray<double>& nodes = m_nodes[i]; + nodes.resize(n_nodes1); + for ( int j=0;j<nodes.size();j++) + { + double& node = nodes[j]; + ds.read(node); + } + } + + unsigned long long int n_cells0; + ds.read(n_cells0); + m_cells.resize(n_cells0); + for (int i=0;i<n_cells0;i++) + { + unsigned long long int n_cells1; + btAlignedObjectArray<btCell32 >& cells = m_cells[i]; + ds.read(n_cells1); + cells.resize(n_cells1); + for (int j=0;j<n_cells1;j++) + { + btCell32& cell = cells[j]; + ds.read(cell); + } + } + + { + unsigned long long int n_cell_maps0; + ds.read(n_cell_maps0); + + m_cell_map.resize(n_cell_maps0); + for (int i=0;i<n_cell_maps0;i++) + { + unsigned long long int n_cell_maps1; + btAlignedObjectArray<unsigned int>& cell_maps = m_cell_map[i]; + ds.read(n_cell_maps1); + cell_maps.resize(n_cell_maps1); + for (int j=0;j<n_cell_maps1;j++) + { + unsigned int& cell_map = cell_maps[j]; + ds.read(cell_map); + } + } + } + + m_isValid = (ds.m_currentOffset == ds.m_size); + return m_isValid; +} + + +unsigned int btMiniSDF::multiToSingleIndex(btMultiIndex const & ijk) const +{ + return m_resolution[1] * m_resolution[0] * ijk.ijk[2] + m_resolution[0] * ijk.ijk[1] + ijk.ijk[0]; +} + +btAlignedBox3d +btMiniSDF::subdomain(btMultiIndex const& ijk) const +{ + btAssert(m_isValid); + btVector3 tmp; + tmp.m_floats[0] = m_cell_size[0]*(double)ijk.ijk[0]; + tmp.m_floats[1] = m_cell_size[1]*(double)ijk.ijk[1]; + tmp.m_floats[2] = m_cell_size[2]*(double)ijk.ijk[2]; + + + btVector3 origin = m_domain.min() + tmp; + + btAlignedBox3d box = btAlignedBox3d (origin, origin + m_cell_size); + return box; +} + +btMultiIndex +btMiniSDF::singleToMultiIndex(unsigned int l) const +{ + btAssert(m_isValid); + unsigned int n01 = m_resolution[0] * m_resolution[1]; + unsigned int k = l / n01; + unsigned int temp = l % n01; + unsigned int j = temp / m_resolution[0]; + unsigned int i = temp % m_resolution[0]; + btMultiIndex mi; + mi.ijk[0] = i; + mi.ijk[1] = j; + mi.ijk[2] = k; + return mi; +} + +btAlignedBox3d +btMiniSDF::subdomain(unsigned int l) const +{ + btAssert(m_isValid); + return subdomain(singleToMultiIndex(l)); +} + + +btShapeMatrix +btMiniSDF::shape_function_(btVector3 const& xi, btShapeGradients* gradient) const +{ + btAssert(m_isValid); + btShapeMatrix res; + + btScalar x = xi[0]; + btScalar y = xi[1]; + btScalar z = xi[2]; + + btScalar x2 = x*x; + btScalar y2 = y*y; + btScalar z2 = z*z; + + btScalar _1mx = 1.0 - x; + btScalar _1my = 1.0 - y; + btScalar _1mz = 1.0 - z; + + btScalar _1px = 1.0 + x; + btScalar _1py = 1.0 + y; + btScalar _1pz = 1.0 + z; + + btScalar _1m3x = 1.0 - 3.0 * x; + btScalar _1m3y = 1.0 - 3.0 * y; + btScalar _1m3z = 1.0 - 3.0 * z; + + btScalar _1p3x = 1.0 + 3.0 * x; + btScalar _1p3y = 1.0 + 3.0 * y; + btScalar _1p3z = 1.0 + 3.0 * z; + + btScalar _1mxt1my = _1mx * _1my; + btScalar _1mxt1py = _1mx * _1py; + btScalar _1pxt1my = _1px * _1my; + btScalar _1pxt1py = _1px * _1py; + + btScalar _1mxt1mz = _1mx * _1mz; + btScalar _1mxt1pz = _1mx * _1pz; + btScalar _1pxt1mz = _1px * _1mz; + btScalar _1pxt1pz = _1px * _1pz; + + btScalar _1myt1mz = _1my * _1mz; + btScalar _1myt1pz = _1my * _1pz; + btScalar _1pyt1mz = _1py * _1mz; + btScalar _1pyt1pz = _1py * _1pz; + + btScalar _1mx2 = 1.0 - x2; + btScalar _1my2 = 1.0 - y2; + btScalar _1mz2 = 1.0 - z2; + + + // Corner nodes. + btScalar fac = 1.0 / 64.0 * (9.0 * (x2 + y2 + z2) - 19.0); + res[0] = fac * _1mxt1my * _1mz; + res[1] = fac * _1pxt1my * _1mz; + res[2] = fac * _1mxt1py * _1mz; + res[3] = fac * _1pxt1py * _1mz; + res[4] = fac * _1mxt1my * _1pz; + res[5] = fac * _1pxt1my * _1pz; + res[6] = fac * _1mxt1py * _1pz; + res[7] = fac * _1pxt1py * _1pz; + + // Edge nodes. + + fac = 9.0 / 64.0 * _1mx2; + btScalar fact1m3x = fac * _1m3x; + btScalar fact1p3x = fac * _1p3x; + res[ 8] = fact1m3x * _1myt1mz; + res[ 9] = fact1p3x * _1myt1mz; + res[10] = fact1m3x * _1myt1pz; + res[11] = fact1p3x * _1myt1pz; + res[12] = fact1m3x * _1pyt1mz; + res[13] = fact1p3x * _1pyt1mz; + res[14] = fact1m3x * _1pyt1pz; + res[15] = fact1p3x * _1pyt1pz; + + fac = 9.0 / 64.0 * _1my2; + btScalar fact1m3y = fac * _1m3y; + btScalar fact1p3y = fac * _1p3y; + res[16] = fact1m3y * _1mxt1mz; + res[17] = fact1p3y * _1mxt1mz; + res[18] = fact1m3y * _1pxt1mz; + res[19] = fact1p3y * _1pxt1mz; + res[20] = fact1m3y * _1mxt1pz; + res[21] = fact1p3y * _1mxt1pz; + res[22] = fact1m3y * _1pxt1pz; + res[23] = fact1p3y * _1pxt1pz; + + fac = 9.0 / 64.0 * _1mz2; + btScalar fact1m3z = fac * _1m3z; + btScalar fact1p3z = fac * _1p3z; + res[24] = fact1m3z * _1mxt1my; + res[25] = fact1p3z * _1mxt1my; + res[26] = fact1m3z * _1mxt1py; + res[27] = fact1p3z * _1mxt1py; + res[28] = fact1m3z * _1pxt1my; + res[29] = fact1p3z * _1pxt1my; + res[30] = fact1m3z * _1pxt1py; + res[31] = fact1p3z * _1pxt1py; + + if (gradient) + { + btShapeGradients& dN = *gradient; + + btScalar _9t3x2py2pz2m19 = 9.0 * (3.0 * x2 + y2 + z2) - 19.0; + btScalar _9tx2p3y2pz2m19 = 9.0 * (x2 + 3.0 * y2 + z2) - 19.0; + btScalar _9tx2py2p3z2m19 = 9.0 * (x2 + y2 + 3.0 * z2) - 19.0; + btScalar _18x = 18.0 * x; + btScalar _18y = 18.0 * y; + btScalar _18z = 18.0 * z; + + btScalar _3m9x2 = 3.0 - 9.0 * x2; + btScalar _3m9y2 = 3.0 - 9.0 * y2; + btScalar _3m9z2 = 3.0 - 9.0 * z2; + + btScalar _2x = 2.0 * x; + btScalar _2y = 2.0 * y; + btScalar _2z = 2.0 * z; + + btScalar _18xm9t3x2py2pz2m19 = _18x - _9t3x2py2pz2m19; + btScalar _18xp9t3x2py2pz2m19 = _18x + _9t3x2py2pz2m19; + btScalar _18ym9tx2p3y2pz2m19 = _18y - _9tx2p3y2pz2m19; + btScalar _18yp9tx2p3y2pz2m19 = _18y + _9tx2p3y2pz2m19; + btScalar _18zm9tx2py2p3z2m19 = _18z - _9tx2py2p3z2m19; + btScalar _18zp9tx2py2p3z2m19 = _18z + _9tx2py2p3z2m19; + + dN(0,0) =_18xm9t3x2py2pz2m19 * _1myt1mz; + dN(0,1) =_1mxt1mz * _18ym9tx2p3y2pz2m19; + dN(0,2) =_1mxt1my * _18zm9tx2py2p3z2m19; + dN(1,0) =_18xp9t3x2py2pz2m19 * _1myt1mz; + dN(1,1) =_1pxt1mz * _18ym9tx2p3y2pz2m19; + dN(1,2) =_1pxt1my * _18zm9tx2py2p3z2m19; + dN(2,0) =_18xm9t3x2py2pz2m19 * _1pyt1mz; + dN(2,1) =_1mxt1mz * _18yp9tx2p3y2pz2m19; + dN(2,2) =_1mxt1py * _18zm9tx2py2p3z2m19; + dN(3,0) =_18xp9t3x2py2pz2m19 * _1pyt1mz; + dN(3,1) =_1pxt1mz * _18yp9tx2p3y2pz2m19; + dN(3,2) =_1pxt1py * _18zm9tx2py2p3z2m19; + dN(4,0) =_18xm9t3x2py2pz2m19 * _1myt1pz; + dN(4,1) =_1mxt1pz * _18ym9tx2p3y2pz2m19; + dN(4,2) =_1mxt1my * _18zp9tx2py2p3z2m19; + dN(5,0) =_18xp9t3x2py2pz2m19 * _1myt1pz; + dN(5,1) =_1pxt1pz * _18ym9tx2p3y2pz2m19; + dN(5,2) =_1pxt1my * _18zp9tx2py2p3z2m19; + dN(6,0) =_18xm9t3x2py2pz2m19 * _1pyt1pz; + dN(6,1) =_1mxt1pz * _18yp9tx2p3y2pz2m19; + dN(6,2) =_1mxt1py * _18zp9tx2py2p3z2m19; + dN(7,0) =_18xp9t3x2py2pz2m19 * _1pyt1pz; + dN(7,1) =_1pxt1pz * _18yp9tx2p3y2pz2m19; + dN(7,2) =_1pxt1py * _18zp9tx2py2p3z2m19; + + dN.topRowsDivide(8, 64.0); + + btScalar _m3m9x2m2x = -_3m9x2 - _2x; + btScalar _p3m9x2m2x = _3m9x2 - _2x; + btScalar _1mx2t1m3x = _1mx2 * _1m3x; + btScalar _1mx2t1p3x = _1mx2 * _1p3x; + dN( 8,0) = _m3m9x2m2x * _1myt1mz, + dN( 8,1) = -_1mx2t1m3x * _1mz, + dN( 8,2) = -_1mx2t1m3x * _1my; + dN( 9,0) = _p3m9x2m2x * _1myt1mz, + dN( 9,1) = -_1mx2t1p3x * _1mz, + dN( 9,2) = -_1mx2t1p3x * _1my; + dN(10,0) = _m3m9x2m2x * _1myt1pz, + dN(10,1) = -_1mx2t1m3x * _1pz, + dN(10,2) = _1mx2t1m3x * _1my; + dN(11,0) = _p3m9x2m2x * _1myt1pz, + dN(11,1) = -_1mx2t1p3x * _1pz, + dN(11,2) = _1mx2t1p3x * _1my; + dN(12,0) = _m3m9x2m2x * _1pyt1mz, + dN(12,1) = _1mx2t1m3x * _1mz, + dN(12,2) = -_1mx2t1m3x * _1py; + dN(13,0) = _p3m9x2m2x * _1pyt1mz, + dN(13,1) = _1mx2t1p3x * _1mz, + dN(13,2) = -_1mx2t1p3x * _1py; + dN(14,0) = _m3m9x2m2x * _1pyt1pz, + dN(14,1) = _1mx2t1m3x * _1pz, + dN(14,2) = _1mx2t1m3x * _1py; + dN(15,0) = _p3m9x2m2x * _1pyt1pz, + dN(15,1) = _1mx2t1p3x * _1pz, + dN(15,2) = _1mx2t1p3x * _1py; + + btScalar _m3m9y2m2y = -_3m9y2 - _2y; + btScalar _p3m9y2m2y = _3m9y2 - _2y; + btScalar _1my2t1m3y = _1my2 * _1m3y; + btScalar _1my2t1p3y = _1my2 * _1p3y; + dN(16,0) = -_1my2t1m3y * _1mz, + dN(16,1) = _m3m9y2m2y * _1mxt1mz, + dN(16,2) = -_1my2t1m3y * _1mx; + dN(17,0) = -_1my2t1p3y * _1mz, + dN(17,1) = _p3m9y2m2y * _1mxt1mz, + dN(17,2) = -_1my2t1p3y * _1mx; + dN(18,0) = _1my2t1m3y * _1mz, + dN(18,1) = _m3m9y2m2y * _1pxt1mz, + dN(18,2) = -_1my2t1m3y * _1px; + dN(19,0) = _1my2t1p3y * _1mz, + dN(19,1) = _p3m9y2m2y * _1pxt1mz, + dN(19,2) = -_1my2t1p3y * _1px; + dN(20,0) = -_1my2t1m3y * _1pz, + dN(20,1) = _m3m9y2m2y * _1mxt1pz, + dN(20,2) = _1my2t1m3y * _1mx; + dN(21,0) = -_1my2t1p3y * _1pz, + dN(21,1) = _p3m9y2m2y * _1mxt1pz, + dN(21,2) = _1my2t1p3y * _1mx; + dN(22,0) = _1my2t1m3y * _1pz, + dN(22,1) = _m3m9y2m2y * _1pxt1pz, + dN(22,2) = _1my2t1m3y * _1px; + dN(23,0) = _1my2t1p3y * _1pz, + dN(23,1) = _p3m9y2m2y * _1pxt1pz, + dN(23,2) = _1my2t1p3y * _1px; + + + btScalar _m3m9z2m2z = -_3m9z2 - _2z; + btScalar _p3m9z2m2z = _3m9z2 - _2z; + btScalar _1mz2t1m3z = _1mz2 * _1m3z; + btScalar _1mz2t1p3z = _1mz2 * _1p3z; + dN(24,0) = -_1mz2t1m3z * _1my, + dN(24,1) = -_1mz2t1m3z * _1mx, + dN(24,2) = _m3m9z2m2z * _1mxt1my; + dN(25,0) = -_1mz2t1p3z * _1my, + dN(25,1) = -_1mz2t1p3z * _1mx, + dN(25,2) = _p3m9z2m2z * _1mxt1my; + dN(26,0) = -_1mz2t1m3z * _1py, + dN(26,1) = _1mz2t1m3z * _1mx, + dN(26,2) = _m3m9z2m2z * _1mxt1py; + dN(27,0) = -_1mz2t1p3z * _1py, + dN(27,1) = _1mz2t1p3z * _1mx, + dN(27,2) = _p3m9z2m2z * _1mxt1py; + dN(28,0) = _1mz2t1m3z * _1my, + dN(28,1) = -_1mz2t1m3z * _1px, + dN(28,2) = _m3m9z2m2z * _1pxt1my; + dN(29,0) = _1mz2t1p3z * _1my, + dN(29,1) = -_1mz2t1p3z * _1px, + dN(29,2) = _p3m9z2m2z * _1pxt1my; + dN(30,0) = _1mz2t1m3z * _1py, + dN(30,1) = _1mz2t1m3z * _1px, + dN(30,2) = _m3m9z2m2z * _1pxt1py; + dN(31,0) = _1mz2t1p3z * _1py, + dN(31,1) = _1mz2t1p3z * _1px, + dN(31,2) = _p3m9z2m2z * _1pxt1py; + + dN.bottomRowsMul(32u - 8u, 9.0 / 64.0); + + } + + return res; +} + + + +bool btMiniSDF::interpolate(unsigned int field_id, double& dist, btVector3 const& x, + btVector3* gradient) const +{ + btAssert(m_isValid); + if (!m_isValid) + return false; + + if (!m_domain.contains(x)) + return false; + + btVector3 tmpmi = ((x - m_domain.min())*(m_inv_cell_size));//.cast<unsigned int>().eval(); + unsigned int mi[3] = {(unsigned int )tmpmi[0],(unsigned int )tmpmi[1],(unsigned int )tmpmi[2]}; + if (mi[0] >= m_resolution[0]) + mi[0] = m_resolution[0]-1; + if (mi[1] >= m_resolution[1]) + mi[1] = m_resolution[1]-1; + if (mi[2] >= m_resolution[2]) + mi[2] = m_resolution[2]-1; + btMultiIndex mui; + mui.ijk[0] = mi[0]; + mui.ijk[1] = mi[1]; + mui.ijk[2] = mi[2]; + int i = multiToSingleIndex(mui); + unsigned int i_ = m_cell_map[field_id][i]; + if (i_ == UINT_MAX) + return false; + + btAlignedBox3d sd = subdomain(i); + i = i_; + btVector3 d = sd.m_max-sd.m_min;//.diagonal().eval(); + + btVector3 denom = (sd.max() - sd.min()); + btVector3 c0 = btVector3(2.0,2.0,2.0)/denom; + btVector3 c1 = (sd.max() + sd.min())/denom; + btVector3 xi = (c0*x - c1); + + btCell32 const& cell = m_cells[field_id][i]; + if (!gradient) + { + //auto phi = m_coefficients[field_id][i].dot(shape_function_(xi, 0)); + double phi = 0.0; + btShapeMatrix N = shape_function_(xi, 0); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + return false;; + } + phi += c * N[j]; + } + + dist = phi; + return true; + } + + btShapeGradients dN; + btShapeMatrix N = shape_function_(xi, &dN); + + double phi = 0.0; + gradient->setZero(); + for (unsigned int j = 0u; j < 32u; ++j) + { + unsigned int v = cell.m_cells[j]; + double c = m_nodes[field_id][v]; + if (c == DBL_MAX) + { + gradient->setZero(); + return false; + } + phi += c * N[j]; + (*gradient)[0] += c * dN(j, 0); + (*gradient)[1] += c * dN(j, 1); + (*gradient)[2] += c * dN(j, 2); + } + (*gradient) *= c0; + dist = phi; + return true; +} + diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h new file mode 100644 index 0000000000..3de90e4f8a --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h @@ -0,0 +1,134 @@ +#ifndef MINISDF_H +#define MINISDF_H + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAabbUtil2.h" +#include "LinearMath/btAlignedObjectArray.h" + + +struct btMultiIndex +{ + unsigned int ijk[3]; +}; + +struct btAlignedBox3d +{ + btVector3 m_min; + btVector3 m_max; + + const btVector3& min() const + { + return m_min; + } + + const btVector3& max() const + { + return m_max; + } + + + bool contains(const btVector3& x) const + { + return TestPointAgainstAabb2(m_min, m_max, x); + } + + btAlignedBox3d(const btVector3& mn, const btVector3& mx) + :m_min(mn), + m_max(mx) + { + } + + btAlignedBox3d() + { + } +}; + +struct btShapeMatrix +{ + double m_vec[32]; + + inline double& operator[](int i) + { + return m_vec[i]; + } + + inline const double& operator[](int i) const + { + return m_vec[i]; + } + +}; + +struct btShapeGradients +{ + btVector3 m_vec[32]; + + void topRowsDivide(int row, double denom) + { + for (int i=0;i<row;i++) + { + m_vec[i] /= denom; + } + } + + void bottomRowsMul(int row, double val) + { + for (int i=32-row;i<32;i++) + { + m_vec[i] *= val; + } + } + + inline btScalar& operator()(int i, int j) + { + return m_vec[i][j]; + } +}; + +struct btCell32 +{ + unsigned int m_cells[32]; +}; + +struct btMiniSDF +{ + + btAlignedBox3d m_domain; + unsigned int m_resolution[3]; + btVector3 m_cell_size; + btVector3 m_inv_cell_size; + std::size_t m_n_cells; + std::size_t m_n_fields; + bool m_isValid; + + + btAlignedObjectArray<btAlignedObjectArray<double> > m_nodes; + btAlignedObjectArray<btAlignedObjectArray<btCell32 > > m_cells; + btAlignedObjectArray<btAlignedObjectArray<unsigned int> > m_cell_map; + + btMiniSDF() + :m_isValid(false) + { + } + bool load(const char* data, int size); + bool isValid() const + { + return m_isValid; + } + unsigned int multiToSingleIndex(btMultiIndex const & ijk) const; + + btAlignedBox3d subdomain(btMultiIndex const& ijk) const; + + btMultiIndex singleToMultiIndex(unsigned int l) const; + + btAlignedBox3d subdomain(unsigned int l) const; + + + btShapeMatrix + shape_function_(btVector3 const& xi, btShapeGradients* gradient = 0) const; + + bool interpolate(unsigned int field_id, double& dist, btVector3 const& x, btVector3* gradient) const; +}; + + +#endif //MINISDF_H diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index 4854f370f7..d51b6760fc 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -39,6 +39,17 @@ btPolyhedralConvexShape::~btPolyhedralConvexShape() } } +void btPolyhedralConvexShape::setPolyhedralFeatures(btConvexPolyhedron& polyhedron) +{ + if (m_polyhedron) + { + *m_polyhedron = polyhedron; + } else + { + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + m_polyhedron = new (mem) btConvexPolyhedron(polyhedron); + } +} bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMargin) { @@ -87,8 +98,72 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); } +#ifndef BT_RECONSTRUCT_FACES + + int numVertices = conv.vertices.size(); + m_polyhedron->m_vertices.resize(numVertices); + for (int p=0;p<numVertices;p++) + { + m_polyhedron->m_vertices[p] = conv.vertices[p]; + } + + int v0, v1; + for (int j = 0; j < conv.faces.size(); j++) + { + btVector3 edges[3]; + int numEdges = 0; + btFace combinedFace; + const btConvexHullComputer::Edge* edge = &conv.edges[conv.faces[j]]; + v0 = edge->getSourceVertex(); + int prevVertex=v0; + combinedFace.m_indices.push_back(v0); + v1 = edge->getTargetVertex(); + while (v1 != v0) + { + + btVector3 wa = conv.vertices[prevVertex]; + btVector3 wb = conv.vertices[v1]; + btVector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + //face->addIndex(v1); + combinedFace.m_indices.push_back(v1); + edge = edge->getNextEdgeOfFace(); + prevVertex = v1; + int v01 = edge->getSourceVertex(); + v1 = edge->getTargetVertex(); + + } + + btAssert(combinedFace.m_indices.size() > 2); + + btVector3 faceNormal = edges[0].cross(edges[1]); + faceNormal.normalize(); + + btScalar planeEq=1e30f; + + for (int v=0;v<combinedFace.m_indices.size();v++) + { + btScalar eq = m_polyhedron->m_vertices[combinedFace.m_indices[v]].dot(faceNormal); + if (planeEq>eq) + { + planeEq=eq; + } + } + combinedFace.m_plane[0] = faceNormal.getX(); + combinedFace.m_plane[1] = faceNormal.getY(); + combinedFace.m_plane[2] = faceNormal.getZ(); + combinedFace.m_plane[3] = -planeEq; + + m_polyhedron->m_faces.push_back(combinedFace); + } + + +#else//BT_RECONSTRUCT_FACES + btAlignedObjectArray<btVector3> faceNormals; int numFaces = conv.faces.size(); faceNormals.resize(numFaces); @@ -311,7 +386,9 @@ bool btPolyhedralConvexShape::initializePolyhedralFeatures(int shiftVerticesByMa } - + +#endif //BT_RECONSTRUCT_FACES + m_polyhedron->initialize(); return true; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 7bf8e01c1f..b7ddb6e060 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -43,6 +43,9 @@ public: ///experimental/work-in-progress virtual bool initializePolyhedralFeatures(int shiftVerticesByMargin=0); + virtual void setPolyhedralFeatures(btConvexPolyhedron& polyhedron); + + const btConvexPolyhedron* getConvexPolyhedron() const { return m_polyhedron; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp new file mode 100644 index 0000000000..828acda470 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp @@ -0,0 +1,99 @@ +#include "btSdfCollisionShape.h" +#include "btMiniSDF.h" +#include "LinearMath/btAabbUtil2.h" + +struct btSdfCollisionShapeInternalData +{ + btVector3 m_localScaling; + btScalar m_margin; + btMiniSDF m_sdf; + + btSdfCollisionShapeInternalData() + :m_localScaling(1,1,1), + m_margin(0) + { + + } +}; + +bool btSdfCollisionShape::initializeSDF(const char* sdfData, int sizeInBytes) +{ + bool valid = m_data->m_sdf.load(sdfData, sizeInBytes); + return valid; +} +btSdfCollisionShape::btSdfCollisionShape() +{ + m_shapeType = SDF_SHAPE_PROXYTYPE; + m_data = new btSdfCollisionShapeInternalData(); + + + + //"E:/develop/bullet3/data/toys/ground_hole64_64_8.cdf");//ground_cube.cdf"); + /*unsigned int field_id=0; + Eigen::Vector3d x (1,10,1); + Eigen::Vector3d gradient; + double dist = m_data->m_sdf.interpolate(field_id, x, &gradient); + printf("dist=%g\n", dist); + */ + +} +btSdfCollisionShape::~btSdfCollisionShape() +{ + delete m_data; +} + +void btSdfCollisionShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const +{ + btAssert(m_data->m_sdf.isValid()); + btVector3 localAabbMin = m_data->m_sdf.m_domain.m_min; + btVector3 localAabbMax = m_data->m_sdf.m_domain.m_max; + btScalar margin(0); + btTransformAabb(localAabbMin,localAabbMax,margin,t,aabbMin,aabbMax); + +} + + +void btSdfCollisionShape::setLocalScaling(const btVector3& scaling) +{ + m_data->m_localScaling = scaling; +} +const btVector3& btSdfCollisionShape::getLocalScaling() const +{ + return m_data->m_localScaling; +} +void btSdfCollisionShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const +{ + inertia.setValue(0,0,0); +} +const char* btSdfCollisionShape::getName()const +{ + return "btSdfCollisionShape"; +} +void btSdfCollisionShape::setMargin(btScalar margin) +{ + m_data->m_margin = margin; +} +btScalar btSdfCollisionShape::getMargin() const +{ + return m_data->m_margin; +} + +void btSdfCollisionShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const +{ + //not yet +} + + +bool btSdfCollisionShape::queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal) +{ + int field = 0; + btVector3 grad; + double dist; + bool hasResult = m_data->m_sdf.interpolate(field,dist, ptInSDF,&grad); + if (hasResult) + { + normal.setValue(grad[0],grad[1],grad[2]); + distOut= dist; + } + return hasResult; +} diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h new file mode 100644 index 0000000000..6e32db9cd8 --- /dev/null +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h @@ -0,0 +1,30 @@ +#ifndef BT_SDF_COLLISION_SHAPE_H +#define BT_SDF_COLLISION_SHAPE_H + +#include "btConcaveShape.h" + +class btSdfCollisionShape : public btConcaveShape +{ + struct btSdfCollisionShapeInternalData* m_data; + +public: + + btSdfCollisionShape(); + virtual ~btSdfCollisionShape(); + + bool initializeSDF(const char* sdfData, int sizeInBytes); + + virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const; + virtual void setLocalScaling(const btVector3& scaling); + virtual const btVector3& getLocalScaling() const; + virtual void calculateLocalInertia(btScalar mass,btVector3& inertia) const; + virtual const char* getName()const; + virtual void setMargin(btScalar margin); + virtual btScalar getMargin() const; + + virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; + + bool queryPoint(const btVector3& ptInSDF, btScalar& distOut, btVector3& normal); +}; + +#endif //BT_SDF_COLLISION_SHAPE_H diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp index 3beaf86580..9f712fe555 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btConvexHull.h" #define NUM_UNITSPHERE_POINTS 42 +#define NUM_UNITSPHERE_POINTS_HIGHRES 256 + btShapeHull::btShapeHull (const btConvexShape* shape) { @@ -36,9 +38,9 @@ btShapeHull::~btShapeHull () } bool -btShapeHull::buildHull (btScalar /*margin*/) +btShapeHull::buildHull (btScalar /*margin*/, int highres) { - int numSampleDirections = NUM_UNITSPHERE_POINTS; + int numSampleDirections = highres? NUM_UNITSPHERE_POINTS_HIGHRES:NUM_UNITSPHERE_POINTS; { int numPDA = m_shape->getNumPreferredPenetrationDirections(); if (numPDA) @@ -47,17 +49,17 @@ btShapeHull::buildHull (btScalar /*margin*/) { btVector3 norm; m_shape->getPreferredPenetrationDirection(i,norm); - getUnitSpherePoints()[numSampleDirections] = norm; + getUnitSpherePoints(highres)[numSampleDirections] = norm; numSampleDirections++; } } } - btVector3 supportPoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; + btVector3 supportPoints[NUM_UNITSPHERE_POINTS_HIGHRES+MAX_PREFERRED_PENETRATION_DIRECTIONS*2]; int i; for (i = 0; i < numSampleDirections; i++) { - supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints()[i]); + supportPoints[i] = m_shape->localGetSupportingVertex(getUnitSpherePoints(highres)[i]); } HullDesc hd; @@ -118,9 +120,268 @@ btShapeHull::numIndices () const } -btVector3* btShapeHull::getUnitSpherePoints() +btVector3* btShapeHull::getUnitSpherePoints(int highres) { - static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS+MAX_PREFERRED_PENETRATION_DIRECTIONS*2] = + static btVector3 sUnitSpherePointsHighres[NUM_UNITSPHERE_POINTS_HIGHRES + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = + { + btVector3(btScalar(0.997604), btScalar(0.067004), btScalar(0.017144)), + btVector3(btScalar(0.984139), btScalar(-0.086784), btScalar(-0.154427)), + btVector3(btScalar(0.971065), btScalar(0.124164), btScalar(-0.203224)), + btVector3(btScalar(0.955844), btScalar(0.291173), btScalar(-0.037704)), + btVector3(btScalar(0.957405), btScalar(0.212238), btScalar(0.195157)), + btVector3(btScalar(0.971650), btScalar(-0.012709), btScalar(0.235561)), + btVector3(btScalar(0.984920), btScalar(-0.161831), btScalar(0.059695)), + btVector3(btScalar(0.946673), btScalar(-0.299288), btScalar(-0.117536)), + btVector3(btScalar(0.922670), btScalar(-0.219186), btScalar(-0.317019)), + btVector3(btScalar(0.928134), btScalar(-0.007265), btScalar(-0.371867)), + btVector3(btScalar(0.875642), btScalar(0.198434), btScalar(-0.439988)), + btVector3(btScalar(0.908035), btScalar(0.325975), btScalar(-0.262562)), + btVector3(btScalar(0.864519), btScalar(0.488706), btScalar(-0.116755)), + btVector3(btScalar(0.893009), btScalar(0.428046), btScalar(0.137185)), + btVector3(btScalar(0.857494), btScalar(0.362137), btScalar(0.364776)), + btVector3(btScalar(0.900815), btScalar(0.132524), btScalar(0.412987)), + btVector3(btScalar(0.934964), btScalar(-0.241739), btScalar(0.259179)), + btVector3(btScalar(0.894570), btScalar(-0.103504), btScalar(0.434263)), + btVector3(btScalar(0.922085), btScalar(-0.376668), btScalar(0.086241)), + btVector3(btScalar(0.862177), btScalar(-0.499154), btScalar(-0.085330)), + btVector3(btScalar(0.861982), btScalar(-0.420218), btScalar(-0.282861)), + btVector3(btScalar(0.818076), btScalar(-0.328256), btScalar(-0.471804)), + btVector3(btScalar(0.762657), btScalar(-0.179329), btScalar(-0.621124)), + btVector3(btScalar(0.826857), btScalar(0.019760), btScalar(-0.561786)), + btVector3(btScalar(0.731434), btScalar(0.206599), btScalar(-0.649817)), + btVector3(btScalar(0.769486), btScalar(0.379052), btScalar(-0.513770)), + btVector3(btScalar(0.796806), btScalar(0.507176), btScalar(-0.328145)), + btVector3(btScalar(0.679722), btScalar(0.684101), btScalar(-0.264123)), + btVector3(btScalar(0.786854), btScalar(0.614886), btScalar(0.050912)), + btVector3(btScalar(0.769486), btScalar(0.571141), btScalar(0.285139)), + btVector3(btScalar(0.707432), btScalar(0.492789), btScalar(0.506288)), + btVector3(btScalar(0.774560), btScalar(0.268037), btScalar(0.572652)), + btVector3(btScalar(0.796220), btScalar(0.031230), btScalar(0.604077)), + btVector3(btScalar(0.837395), btScalar(-0.320285), btScalar(0.442461)), + btVector3(btScalar(0.848127), btScalar(-0.450548), btScalar(0.278307)), + btVector3(btScalar(0.775536), btScalar(-0.206354), btScalar(0.596465)), + btVector3(btScalar(0.816320), btScalar(-0.567007), btScalar(0.109469)), + btVector3(btScalar(0.741191), btScalar(-0.668690), btScalar(-0.056832)), + btVector3(btScalar(0.755632), btScalar(-0.602975), btScalar(-0.254949)), + btVector3(btScalar(0.720311), btScalar(-0.521318), btScalar(-0.457165)), + btVector3(btScalar(0.670746), btScalar(-0.386583), btScalar(-0.632835)), + btVector3(btScalar(0.587031), btScalar(-0.219769), btScalar(-0.778836)), + btVector3(btScalar(0.676015), btScalar(-0.003182), btScalar(-0.736676)), + btVector3(btScalar(0.566932), btScalar(0.186963), btScalar(-0.802064)), + btVector3(btScalar(0.618254), btScalar(0.398105), btScalar(-0.677533)), + btVector3(btScalar(0.653964), btScalar(0.575224), btScalar(-0.490933)), + btVector3(btScalar(0.525367), btScalar(0.743205), btScalar(-0.414028)), + btVector3(btScalar(0.506439), btScalar(0.836528), btScalar(-0.208885)), + btVector3(btScalar(0.651427), btScalar(0.756426), btScalar(-0.056247)), + btVector3(btScalar(0.641670), btScalar(0.745149), btScalar(0.180908)), + btVector3(btScalar(0.602643), btScalar(0.687211), btScalar(0.405180)), + btVector3(btScalar(0.516586), btScalar(0.596999), btScalar(0.613447)), + btVector3(btScalar(0.602252), btScalar(0.387801), btScalar(0.697573)), + btVector3(btScalar(0.646549), btScalar(0.153911), btScalar(0.746956)), + btVector3(btScalar(0.650842), btScalar(-0.087756), btScalar(0.753983)), + btVector3(btScalar(0.740411), btScalar(-0.497404), btScalar(0.451830)), + btVector3(btScalar(0.726946), btScalar(-0.619890), btScalar(0.295093)), + btVector3(btScalar(0.637768), btScalar(-0.313092), btScalar(0.703624)), + btVector3(btScalar(0.678942), btScalar(-0.722934), btScalar(0.126645)), + btVector3(btScalar(0.489072), btScalar(-0.867195), btScalar(-0.092942)), + btVector3(btScalar(0.622742), btScalar(-0.757541), btScalar(-0.194636)), + btVector3(btScalar(0.596788), btScalar(-0.693576), btScalar(-0.403098)), + btVector3(btScalar(0.550150), btScalar(-0.582172), btScalar(-0.598287)), + btVector3(btScalar(0.474436), btScalar(-0.429745), btScalar(-0.768101)), + btVector3(btScalar(0.372574), btScalar(-0.246016), btScalar(-0.894583)), + btVector3(btScalar(0.480095), btScalar(-0.026513), btScalar(-0.876626)), + btVector3(btScalar(0.352474), btScalar(0.177242), btScalar(-0.918787)), + btVector3(btScalar(0.441848), btScalar(0.374386), btScalar(-0.814946)), + btVector3(btScalar(0.492389), btScalar(0.582223), btScalar(-0.646693)), + btVector3(btScalar(0.343498), btScalar(0.866080), btScalar(-0.362693)), + btVector3(btScalar(0.362036), btScalar(0.745149), btScalar(-0.559639)), + btVector3(btScalar(0.334131), btScalar(0.937044), btScalar(-0.099774)), + btVector3(btScalar(0.486925), btScalar(0.871718), btScalar(0.052473)), + btVector3(btScalar(0.452776), btScalar(0.845665), btScalar(0.281820)), + btVector3(btScalar(0.399503), btScalar(0.771785), btScalar(0.494576)), + btVector3(btScalar(0.296469), btScalar(0.673018), btScalar(0.677469)), + btVector3(btScalar(0.392088), btScalar(0.479179), btScalar(0.785213)), + btVector3(btScalar(0.452190), btScalar(0.252094), btScalar(0.855286)), + btVector3(btScalar(0.478339), btScalar(0.013149), btScalar(0.877928)), + btVector3(btScalar(0.481656), btScalar(-0.219380), btScalar(0.848259)), + btVector3(btScalar(0.615327), btScalar(-0.494293), btScalar(0.613837)), + btVector3(btScalar(0.594642), btScalar(-0.650414), btScalar(0.472325)), + btVector3(btScalar(0.562249), btScalar(-0.771345), btScalar(0.297631)), + btVector3(btScalar(0.467411), btScalar(-0.437133), btScalar(0.768231)), + btVector3(btScalar(0.519513), btScalar(-0.847947), btScalar(0.103808)), + btVector3(btScalar(0.297640), btScalar(-0.938159), btScalar(-0.176288)), + btVector3(btScalar(0.446727), btScalar(-0.838615), btScalar(-0.311359)), + btVector3(btScalar(0.331790), btScalar(-0.942437), btScalar(0.040762)), + btVector3(btScalar(0.413358), btScalar(-0.748403), btScalar(-0.518259)), + btVector3(btScalar(0.347596), btScalar(-0.621640), btScalar(-0.701737)), + btVector3(btScalar(0.249831), btScalar(-0.456186), btScalar(-0.853984)), + btVector3(btScalar(0.131772), btScalar(-0.262931), btScalar(-0.955678)), + btVector3(btScalar(0.247099), btScalar(-0.042261), btScalar(-0.967975)), + btVector3(btScalar(0.113624), btScalar(0.165965), btScalar(-0.979491)), + btVector3(btScalar(0.217438), btScalar(0.374580), btScalar(-0.901220)), + btVector3(btScalar(0.307983), btScalar(0.554615), btScalar(-0.772786)), + btVector3(btScalar(0.166702), btScalar(0.953181), btScalar(-0.252021)), + btVector3(btScalar(0.172751), btScalar(0.844499), btScalar(-0.506743)), + btVector3(btScalar(0.177630), btScalar(0.711125), btScalar(-0.679876)), + btVector3(btScalar(0.120064), btScalar(0.992260), btScalar(-0.030482)), + btVector3(btScalar(0.289640), btScalar(0.949098), btScalar(0.122546)), + btVector3(btScalar(0.239879), btScalar(0.909047), btScalar(0.340377)), + btVector3(btScalar(0.181142), btScalar(0.821363), btScalar(0.540641)), + btVector3(btScalar(0.066986), btScalar(0.719097), btScalar(0.691327)), + btVector3(btScalar(0.156750), btScalar(0.545478), btScalar(0.823079)), + btVector3(btScalar(0.236172), btScalar(0.342306), btScalar(0.909353)), + btVector3(btScalar(0.277541), btScalar(0.112693), btScalar(0.953856)), + btVector3(btScalar(0.295299), btScalar(-0.121974), btScalar(0.947415)), + btVector3(btScalar(0.287883), btScalar(-0.349254), btScalar(0.891591)), + btVector3(btScalar(0.437165), btScalar(-0.634666), btScalar(0.636869)), + btVector3(btScalar(0.407113), btScalar(-0.784954), btScalar(0.466664)), + btVector3(btScalar(0.375111), btScalar(-0.888193), btScalar(0.264839)), + btVector3(btScalar(0.275394), btScalar(-0.560591), btScalar(0.780723)), + btVector3(btScalar(0.122015), btScalar(-0.992209), btScalar(-0.024821)), + btVector3(btScalar(0.087866), btScalar(-0.966156), btScalar(-0.241676)), + btVector3(btScalar(0.239489), btScalar(-0.885665), btScalar(-0.397437)), + btVector3(btScalar(0.167287), btScalar(-0.965184), btScalar(0.200817)), + btVector3(btScalar(0.201632), btScalar(-0.776789), btScalar(-0.596335)), + btVector3(btScalar(0.122015), btScalar(-0.637971), btScalar(-0.760098)), + btVector3(btScalar(0.008054), btScalar(-0.464741), btScalar(-0.885214)), + btVector3(btScalar(-0.116054), btScalar(-0.271096), btScalar(-0.955482)), + btVector3(btScalar(-0.000727), btScalar(-0.056065), btScalar(-0.998424)), + btVector3(btScalar(-0.134007), btScalar(0.152939), btScalar(-0.978905)), + btVector3(btScalar(-0.025900), btScalar(0.366026), btScalar(-0.930108)), + btVector3(btScalar(0.081231), btScalar(0.557337), btScalar(-0.826072)), + btVector3(btScalar(-0.002874), btScalar(0.917213), btScalar(-0.398023)), + btVector3(btScalar(-0.050683), btScalar(0.981761), btScalar(-0.182534)), + btVector3(btScalar(-0.040536), btScalar(0.710153), btScalar(-0.702713)), + btVector3(btScalar(-0.139081), btScalar(0.827973), btScalar(-0.543048)), + btVector3(btScalar(-0.101029), btScalar(0.994010), btScalar(0.041152)), + btVector3(btScalar(0.069328), btScalar(0.978067), btScalar(0.196133)), + btVector3(btScalar(0.023860), btScalar(0.911380), btScalar(0.410645)), + btVector3(btScalar(-0.153521), btScalar(0.736789), btScalar(0.658145)), + btVector3(btScalar(-0.070002), btScalar(0.591750), btScalar(0.802780)), + btVector3(btScalar(0.002590), btScalar(0.312948), btScalar(0.949562)), + btVector3(btScalar(0.090988), btScalar(-0.020680), btScalar(0.995627)), + btVector3(btScalar(0.088842), btScalar(-0.250099), btScalar(0.964006)), + btVector3(btScalar(0.083378), btScalar(-0.470185), btScalar(0.878318)), + btVector3(btScalar(0.240074), btScalar(-0.749764), btScalar(0.616374)), + btVector3(btScalar(0.210803), btScalar(-0.885860), btScalar(0.412987)), + btVector3(btScalar(0.077524), btScalar(-0.660524), btScalar(0.746565)), + btVector3(btScalar(-0.096736), btScalar(-0.990070), btScalar(-0.100945)), + btVector3(btScalar(-0.052634), btScalar(-0.990264), btScalar(0.127426)), + btVector3(btScalar(-0.106102), btScalar(-0.938354), btScalar(-0.328340)), + btVector3(btScalar(0.013323), btScalar(-0.863112), btScalar(-0.504596)), + btVector3(btScalar(-0.002093), btScalar(-0.936993), btScalar(0.349161)), + btVector3(btScalar(-0.106297), btScalar(-0.636610), btScalar(-0.763612)), + btVector3(btScalar(-0.229430), btScalar(-0.463769), btScalar(-0.855546)), + btVector3(btScalar(-0.245236), btScalar(-0.066175), btScalar(-0.966999)), + btVector3(btScalar(-0.351587), btScalar(-0.270513), btScalar(-0.896145)), + btVector3(btScalar(-0.370906), btScalar(0.133108), btScalar(-0.918982)), + btVector3(btScalar(-0.264360), btScalar(0.346000), btScalar(-0.900049)), + btVector3(btScalar(-0.151375), btScalar(0.543728), btScalar(-0.825291)), + btVector3(btScalar(-0.218697), btScalar(0.912741), btScalar(-0.344346)), + btVector3(btScalar(-0.274507), btScalar(0.953764), btScalar(-0.121635)), + btVector3(btScalar(-0.259677), btScalar(0.692266), btScalar(-0.673044)), + btVector3(btScalar(-0.350416), btScalar(0.798810), btScalar(-0.488786)), + btVector3(btScalar(-0.320170), btScalar(0.941127), btScalar(0.108297)), + btVector3(btScalar(-0.147667), btScalar(0.952792), btScalar(0.265034)), + btVector3(btScalar(-0.188061), btScalar(0.860636), btScalar(0.472910)), + btVector3(btScalar(-0.370906), btScalar(0.739900), btScalar(0.560941)), + btVector3(btScalar(-0.297143), btScalar(0.585334), btScalar(0.754178)), + btVector3(btScalar(-0.189622), btScalar(0.428241), btScalar(0.883393)), + btVector3(btScalar(-0.091272), btScalar(0.098695), btScalar(0.990747)), + btVector3(btScalar(-0.256945), btScalar(0.228375), btScalar(0.938827)), + btVector3(btScalar(-0.111761), btScalar(-0.133251), btScalar(0.984696)), + btVector3(btScalar(-0.118006), btScalar(-0.356253), btScalar(0.926725)), + btVector3(btScalar(-0.119372), btScalar(-0.563896), btScalar(0.817029)), + btVector3(btScalar(0.041228), btScalar(-0.833949), btScalar(0.550010)), + btVector3(btScalar(-0.121909), btScalar(-0.736543), btScalar(0.665172)), + btVector3(btScalar(-0.307681), btScalar(-0.931160), btScalar(-0.195026)), + btVector3(btScalar(-0.283679), btScalar(-0.957990), btScalar(0.041348)), + btVector3(btScalar(-0.227284), btScalar(-0.935243), btScalar(0.270890)), + btVector3(btScalar(-0.293436), btScalar(-0.858252), btScalar(-0.420860)), + btVector3(btScalar(-0.175767), btScalar(-0.780677), btScalar(-0.599262)), + btVector3(btScalar(-0.170108), btScalar(-0.858835), btScalar(0.482865)), + btVector3(btScalar(-0.332854), btScalar(-0.635055), btScalar(-0.696857)), + btVector3(btScalar(-0.447791), btScalar(-0.445299), btScalar(-0.775128)), + btVector3(btScalar(-0.470622), btScalar(-0.074146), btScalar(-0.879164)), + btVector3(btScalar(-0.639417), btScalar(-0.340505), btScalar(-0.689049)), + btVector3(btScalar(-0.598438), btScalar(0.104722), btScalar(-0.794256)), + btVector3(btScalar(-0.488575), btScalar(0.307699), btScalar(-0.816313)), + btVector3(btScalar(-0.379882), btScalar(0.513592), btScalar(-0.769077)), + btVector3(btScalar(-0.425740), btScalar(0.862775), btScalar(-0.272516)), + btVector3(btScalar(-0.480769), btScalar(0.875412), btScalar(-0.048439)), + btVector3(btScalar(-0.467890), btScalar(0.648716), btScalar(-0.600043)), + btVector3(btScalar(-0.543799), btScalar(0.730956), btScalar(-0.411881)), + btVector3(btScalar(-0.516284), btScalar(0.838277), btScalar(0.174076)), + btVector3(btScalar(-0.353343), btScalar(0.876384), btScalar(0.326519)), + btVector3(btScalar(-0.572875), btScalar(0.614497), btScalar(0.542007)), + btVector3(btScalar(-0.503600), btScalar(0.497261), btScalar(0.706161)), + btVector3(btScalar(-0.530920), btScalar(0.754870), btScalar(0.384685)), + btVector3(btScalar(-0.395884), btScalar(0.366414), btScalar(0.841818)), + btVector3(btScalar(-0.300656), btScalar(0.001678), btScalar(0.953661)), + btVector3(btScalar(-0.461060), btScalar(0.146912), btScalar(0.875000)), + btVector3(btScalar(-0.315486), btScalar(-0.232212), btScalar(0.919893)), + btVector3(btScalar(-0.323682), btScalar(-0.449187), btScalar(0.832644)), + btVector3(btScalar(-0.318999), btScalar(-0.639527), btScalar(0.699134)), + btVector3(btScalar(-0.496771), btScalar(-0.866029), btScalar(-0.055271)), + btVector3(btScalar(-0.496771), btScalar(-0.816257), btScalar(-0.294377)), + btVector3(btScalar(-0.456377), btScalar(-0.869528), btScalar(0.188130)), + btVector3(btScalar(-0.380858), btScalar(-0.827144), btScalar(0.412792)), + btVector3(btScalar(-0.449352), btScalar(-0.727405), btScalar(-0.518259)), + btVector3(btScalar(-0.570533), btScalar(-0.551064), btScalar(-0.608632)), + btVector3(btScalar(-0.656394), btScalar(-0.118280), btScalar(-0.744874)), + btVector3(btScalar(-0.756696), btScalar(-0.438105), btScalar(-0.484882)), + btVector3(btScalar(-0.801773), btScalar(-0.204798), btScalar(-0.561005)), + btVector3(btScalar(-0.785186), btScalar(0.038618), btScalar(-0.617805)), + btVector3(btScalar(-0.709082), btScalar(0.262399), btScalar(-0.654306)), + btVector3(btScalar(-0.583412), btScalar(0.462265), btScalar(-0.667383)), + btVector3(btScalar(-0.616001), btScalar(0.761286), btScalar(-0.201272)), + btVector3(btScalar(-0.660687), btScalar(0.750204), btScalar(0.020072)), + btVector3(btScalar(-0.744987), btScalar(0.435823), btScalar(-0.504791)), + btVector3(btScalar(-0.713765), btScalar(0.605554), btScalar(-0.351373)), + btVector3(btScalar(-0.686251), btScalar(0.687600), btScalar(0.236927)), + btVector3(btScalar(-0.680201), btScalar(0.429407), btScalar(0.593732)), + btVector3(btScalar(-0.733474), btScalar(0.546450), btScalar(0.403814)), + btVector3(btScalar(-0.591023), btScalar(0.292923), btScalar(0.751445)), + btVector3(btScalar(-0.500283), btScalar(-0.080757), btScalar(0.861922)), + btVector3(btScalar(-0.643710), btScalar(0.070115), btScalar(0.761985)), + btVector3(btScalar(-0.506332), btScalar(-0.308425), btScalar(0.805122)), + btVector3(btScalar(-0.503015), btScalar(-0.509847), btScalar(0.697573)), + btVector3(btScalar(-0.482525), btScalar(-0.682105), btScalar(0.549229)), + btVector3(btScalar(-0.680396), btScalar(-0.716323), btScalar(-0.153451)), + btVector3(btScalar(-0.658346), btScalar(-0.746264), btScalar(0.097562)), + btVector3(btScalar(-0.653272), btScalar(-0.646915), btScalar(-0.392948)), + btVector3(btScalar(-0.590828), btScalar(-0.732655), btScalar(0.337645)), + btVector3(btScalar(-0.819140), btScalar(-0.518013), btScalar(-0.246166)), + btVector3(btScalar(-0.900513), btScalar(-0.282178), btScalar(-0.330487)), + btVector3(btScalar(-0.914953), btScalar(-0.028652), btScalar(-0.402122)), + btVector3(btScalar(-0.859924), btScalar(0.220209), btScalar(-0.459898)), + btVector3(btScalar(-0.777185), btScalar(0.613720), btScalar(-0.137836)), + btVector3(btScalar(-0.805285), btScalar(0.586889), btScalar(0.082728)), + btVector3(btScalar(-0.872413), btScalar(0.406077), btScalar(-0.271735)), + btVector3(btScalar(-0.859339), btScalar(0.448072), btScalar(0.246101)), + btVector3(btScalar(-0.757671), btScalar(0.216320), btScalar(0.615594)), + btVector3(btScalar(-0.826165), btScalar(0.348139), btScalar(0.442851)), + btVector3(btScalar(-0.671810), btScalar(-0.162803), btScalar(0.722557)), + btVector3(btScalar(-0.796504), btScalar(-0.004543), btScalar(0.604468)), + btVector3(btScalar(-0.676298), btScalar(-0.378223), btScalar(0.631794)), + btVector3(btScalar(-0.668883), btScalar(-0.558258), btScalar(0.490673)), + btVector3(btScalar(-0.821287), btScalar(-0.570118), btScalar(0.006994)), + btVector3(btScalar(-0.767428), btScalar(-0.587810), btScalar(0.255470)), + btVector3(btScalar(-0.933296), btScalar(-0.349837), btScalar(-0.079865)), + btVector3(btScalar(-0.982667), btScalar(-0.100393), btScalar(-0.155208)), + btVector3(btScalar(-0.961396), btScalar(0.160910), btScalar(-0.222938)), + btVector3(btScalar(-0.934858), btScalar(0.354555), btScalar(-0.006864)), + btVector3(btScalar(-0.941687), btScalar(0.229736), btScalar(0.245711)), + btVector3(btScalar(-0.884317), btScalar(0.131552), btScalar(0.447536)), + btVector3(btScalar(-0.810359), btScalar(-0.219769), btScalar(0.542788)), + btVector3(btScalar(-0.915929), btScalar(-0.210048), btScalar(0.341743)), + btVector3(btScalar(-0.816799), btScalar(-0.407192), btScalar(0.408303)), + btVector3(btScalar(-0.903050), btScalar(-0.392416), btScalar(0.174076)), + btVector3(btScalar(-0.980325), btScalar(-0.170969), btScalar(0.096586)), + btVector3(btScalar(-0.995936), btScalar(0.084891), btScalar(0.029441)), + btVector3(btScalar(-0.960031), btScalar(0.002650), btScalar(0.279283)), + }; + static btVector3 sUnitSpherePoints[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] = { btVector3(btScalar(0.000000) , btScalar(-0.000000),btScalar(-1.000000)), btVector3(btScalar(0.723608) , btScalar(-0.525725),btScalar(-0.447219)), @@ -165,6 +426,8 @@ btVector3* btShapeHull::getUnitSpherePoints() btVector3(btScalar(-0.425323) , btScalar(0.309011),btScalar(0.850654)), btVector3(btScalar(0.162456) , btScalar(0.499995),btScalar(0.850654)) }; + if (highres) + return sUnitSpherePointsHighres; return sUnitSpherePoints; } diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h index e959f198b6..78ea4b6501 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h @@ -34,7 +34,7 @@ protected: unsigned int m_numIndices; const btConvexShape* m_shape; - static btVector3* getUnitSpherePoints(); + static btVector3* getUnitSpherePoints(int highres=0); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -42,7 +42,7 @@ public: btShapeHull (const btConvexShape* shape); ~btShapeHull (); - bool buildHull (btScalar margin); + bool buildHull (btScalar margin, int highres=0); int numTriangles () const; int numVertices () const; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp b/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp index d17141e3f2..b5e0e716d4 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp @@ -69,8 +69,6 @@ void btStaticPlaneShape::processAllTriangles(btTriangleCallback* callback,const //tangentDir0/tangentDir1 can be precalculated btPlaneSpace1(m_planeNormal,tangentDir0,tangentDir1); - btVector3 supVertex0,supVertex1; - btVector3 projectedCenter = center - (m_planeNormal.dot(center) - m_planeConstant)*m_planeNormal; btVector3 triangle[3]; diff --git a/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index 9e1544e87a..b7a6f74361 100644 --- a/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -63,7 +63,7 @@ typedef btAlignedObjectArray<btIndexedMesh> IndexedMeshArray; ///The btTriangleIndexVertexArray allows to access multiple triangle meshes, by indexing into existing triangle/index arrays. ///Additional meshes can be added using addIndexedMesh -///No duplcate is made of the vertex/index data, it only indexes into external vertex/index arrays. +///No duplicate is made of the vertex/index data, it only indexes into external vertex/index arrays. ///So keep those arrays around during the lifetime of this btTriangleIndexVertexArray. ATTRIBUTE_ALIGNED16( class) btTriangleIndexVertexArray : public btStridingMeshInterface { diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 940282f576..3481fec850 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -113,12 +113,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if ((relLinVelocLength+maxAngularProjectedVelocity) == 0.f) return false; - - btScalar lambda = btScalar(0.); - btVector3 v(1,0,0); - - int maxIter = MAX_ITERATIONS; btVector3 n; n.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); @@ -137,8 +132,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btPointCollector pointCollector1; - { - + { computeClosestPoints(fromA,fromB,pointCollector1); hasResult = pointCollector1.m_hasResult; @@ -172,28 +166,20 @@ bool btContinuousConvexCollision::calcTimeOfImpact( dLambda = dist / (projectedLinearVelocity+ maxAngularProjectedVelocity); - - - lambda = lambda + dLambda; + lambda += dLambda; - if (lambda > btScalar(1.)) + if (lambda > btScalar(1.) || lambda < btScalar(0.)) return false; - if (lambda < btScalar(0.)) - return false; - - //todo: next check with relative epsilon if (lambda <= lastLambda) { return false; //n.setValue(0,0,0); - break; + //break; } lastLambda = lambda; - - //interpolate to next lambda btTransform interpolatedTransA,interpolatedTransB,relativeTrans; @@ -223,7 +209,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } numIter++; - if (numIter > maxIter) + if (numIter > MAX_ITERATIONS) { result.reportFailure(-2, numIter); return false; @@ -237,6 +223,5 @@ bool btContinuousConvexCollision::calcTimeOfImpact( } return false; - } diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index bdc0572f75..528b5e0101 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -25,7 +25,7 @@ class btStaticPlaneShape; /// btContinuousConvexCollision implements angular and linear time of impact for convex objects. /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). -/// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent. +/// Algorithm operates in worldspace, in order to keep in between motion globally consistent. /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops class btContinuousConvexCollision : public btConvexCast { diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index 572ec36f56..b79f49d611 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp @@ -21,46 +21,64 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver, - const btConvexShape* pConvexA, const btConvexShape* pConvexB, - const btTransform& transformA, const btTransform& transformB, - btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, - class btIDebugDraw* debugDraw) +bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver, + const btConvexShape* pConvexA, const btConvexShape* pConvexB, + const btTransform& transformA, const btTransform& transformB, + btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB, + class btIDebugDraw* debugDraw) { (void)debugDraw; (void)v; (void)simplexSolver; -// const btScalar radialmargin(btScalar(0.)); - - btVector3 guessVector(transformB.getOrigin()-transformA.getOrigin()); - btGjkEpaSolver2::sResults results; - + btVector3 guessVectors[] = { + btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(), + btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(), + btVector3(0, 0, 1), + btVector3(0, 1, 0), + btVector3(1, 0, 0), + btVector3(1, 1, 0), + btVector3(1, 1, 1), + btVector3(0, 1, 1), + btVector3(1, 0, 1), + }; - if(btGjkEpaSolver2::Penetration(pConvexA,transformA, - pConvexB,transformB, - guessVector,results)) - - { - // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); - //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); - wWitnessOnA = results.witnesses[0]; - wWitnessOnB = results.witnesses[1]; - v = results.normal; - return true; - } else + int numVectors = sizeof(guessVectors) / sizeof(btVector3); + + for (int i = 0; i < numVectors; i++) { - if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results)) + simplexSolver.reset(); + btVector3 guessVector = guessVectors[i]; + + btGjkEpaSolver2::sResults results; + + if (btGjkEpaSolver2::Penetration(pConvexA, transformA, + pConvexB, transformB, + guessVector, results)) + { wWitnessOnA = results.witnesses[0]; wWitnessOnB = results.witnesses[1]; v = results.normal; - return false; + return true; + } + else + { + if (btGjkEpaSolver2::Distance(pConvexA, transformA, pConvexB, transformB, guessVector, results)) + { + wWitnessOnA = results.witnesses[0]; + wWitnessOnB = results.witnesses[1]; + v = results.normal; + return false; + } } } + //failed to find a distance/penetration + wWitnessOnA.setValue(0, 0, 0); + wWitnessOnB.setValue(0, 0, 0); + v.setValue(0, 0, 0); return false; } - diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp index 257b026d9b..a0b825f0e8 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp @@ -1,4 +1,4 @@ -/* +/* Bullet Continuous Collision Detection and Physics Library Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ @@ -32,7 +32,7 @@ subject to the following restrictions: //must be above the machine epsilon #ifdef BT_USE_DOUBLE_PRECISION #define REL_ERROR2 btScalar(1.0e-12) - btScalar gGjkEpaPenetrationTolerance = 1e-7; + btScalar gGjkEpaPenetrationTolerance = 1.0e-12; #else #define REL_ERROR2 btScalar(1.0e-6) btScalar gGjkEpaPenetrationTolerance = 0.001; @@ -83,6 +83,593 @@ void btGjkPairDetector::getClosestPoints(const ClosestPointInput& input,Result& getClosestPointsNonVirtual(input,output,debugDraw); } +static void btComputeSupport(const btConvexShape* convexA, const btTransform& localTransA, const btConvexShape* convexB, const btTransform& localTransB, const btVector3& dir, bool check2d, btVector3& supAworld, btVector3& supBworld, btVector3& aMinb) +{ + btVector3 seperatingAxisInA = (dir)* localTransA.getBasis(); + btVector3 seperatingAxisInB = (-dir)* localTransB.getBasis(); + + btVector3 pInANoMargin = convexA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInBNoMargin = convexB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + btVector3 pInA = pInANoMargin; + btVector3 qInB = qInBNoMargin; + + supAworld = localTransA(pInA); + supBworld = localTransB(qInB); + + if (check2d) + { + supAworld[2] = 0.f; + supBworld[2] = 0.f; + } + + aMinb = supAworld - supBworld; +} + +struct btSupportVector +{ + btVector3 v; //!< Support point in minkowski sum + btVector3 v1; //!< Support point in obj1 + btVector3 v2; //!< Support point in obj2 +}; + +struct btSimplex +{ + btSupportVector ps[4]; + int last; //!< index of last added point +}; + +static btVector3 ccd_vec3_origin(0, 0, 0); + + +inline void btSimplexInit(btSimplex *s) +{ + s->last = -1; +} + +inline int btSimplexSize(const btSimplex *s) +{ + return s->last + 1; +} + +inline const btSupportVector *btSimplexPoint(const btSimplex *s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} +inline void btSupportCopy(btSupportVector *d, const btSupportVector *s) +{ + *d = *s; +} + +inline void btVec3Copy(btVector3 *v, const btVector3* w) +{ + *v = *w; +} + +inline void ccdVec3Add(btVector3*v, const btVector3*w) +{ + v->m_floats[0] += w->m_floats[0]; + v->m_floats[1] += w->m_floats[1]; + v->m_floats[2] += w->m_floats[2]; +} + + +inline void ccdVec3Sub(btVector3 *v, const btVector3 *w) +{ + *v -= *w; +} +inline void btVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w) +{ + *d = (*v) - (*w); + +} +inline btScalar btVec3Dot(const btVector3 *a, const btVector3 *b) +{ + btScalar dot; + dot = a->dot(*b); + + return dot; +} + +inline btScalar ccdVec3Dist2(const btVector3 *a, const btVector3*b) +{ + btVector3 ab; + btVec3Sub2(&ab, a, b); + return btVec3Dot(&ab, &ab); +} + + +inline void btVec3Scale(btVector3 *d, btScalar k) +{ + d->m_floats[0] *= k; + d->m_floats[1] *= k; + d->m_floats[2] *= k; +} + +inline void btVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b) +{ + d->m_floats[0] = (a->m_floats[1] * b->m_floats[2]) - (a->m_floats[2] * b->m_floats[1]); + d->m_floats[1] = (a->m_floats[2] * b->m_floats[0]) - (a->m_floats[0] * b->m_floats[2]); + d->m_floats[2] = (a->m_floats[0] * b->m_floats[1]) - (a->m_floats[1] * b->m_floats[0]); +} + +inline void btTripleCross(const btVector3 *a, const btVector3 *b, + const btVector3 *c, btVector3 *d) +{ + btVector3 e; + btVec3Cross(&e, a, b); + btVec3Cross(d, &e, c); +} + +inline int ccdEq(btScalar _a, btScalar _b) +{ + btScalar ab; + btScalar a, b; + + ab = btFabs(_a - _b); + if (btFabs(ab) < SIMD_EPSILON) + return 1; + + a = btFabs(_a); + b = btFabs(_b); + if (b > a) { + return ab < SIMD_EPSILON * b; + } + else { + return ab < SIMD_EPSILON * a; + } +} + +btScalar ccdVec3X(const btVector3* v) +{ + return v->x(); +} + +btScalar ccdVec3Y(const btVector3* v) +{ + return v->y(); +} + +btScalar ccdVec3Z(const btVector3* v) +{ + return v->z(); +} +inline int btVec3Eq(const btVector3 *a, const btVector3 *b) +{ + return ccdEq(ccdVec3X(a), ccdVec3X(b)) + && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) + && ccdEq(ccdVec3Z(a), ccdVec3Z(b)); +} + + +inline void btSimplexAdd(btSimplex *s, const btSupportVector *v) +{ + // here is no check on boundaries in sake of speed + ++s->last; + btSupportCopy(s->ps + s->last, v); +} + + +inline void btSimplexSet(btSimplex *s, size_t pos, const btSupportVector *a) +{ + btSupportCopy(s->ps + pos, a); +} + +inline void btSimplexSetSize(btSimplex *s, int size) +{ + s->last = size - 1; +} + +inline const btSupportVector *ccdSimplexLast(const btSimplex *s) +{ + return btSimplexPoint(s, s->last); +} + +inline int ccdSign(btScalar val) +{ + if (btFuzzyZero(val)) { + return 0; + } + else if (val < btScalar(0)) { + return -1; + } + return 1; +} + + +inline btScalar btVec3PointSegmentDist2(const btVector3 *P, + const btVector3 *x0, + const btVector3 *b, + btVector3 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + btScalar dist, t; + btVector3 d, a; + + // direction of segment + btVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + btVec3Sub2(&a, x0, P); + + t = -btScalar(1.) * btVec3Dot(&a, &d); + t /= btVec3Dot(&d, &d); + + if (t < btScalar(0) || btFuzzyZero(t)) { + dist = ccdVec3Dist2(x0, P); + if (witness) + btVec3Copy(witness, x0); + } + else if (t > btScalar(1) || ccdEq(t, btScalar(1))) { + dist = ccdVec3Dist2(b, P); + if (witness) + btVec3Copy(witness, b); + } + else { + if (witness) { + btVec3Copy(witness, &d); + btVec3Scale(witness, t); + ccdVec3Add(witness, x0); + dist = ccdVec3Dist2(witness, P); + } + else { + // recycling variables + btVec3Scale(&d, t); + ccdVec3Add(&d, &a); + dist = btVec3Dot(&d, &d); + } + } + + return dist; +} + + +btScalar btVec3PointTriDist2(const btVector3 *P, + const btVector3 *x0, const btVector3 *B, + const btVector3 *C, + btVector3 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + btVector3 d1, d2, a; + double u, v, w, p, q, r; + double s, t, dist, dist2; + btVector3 witness2; + + btVec3Sub2(&d1, B, x0); + btVec3Sub2(&d2, C, x0); + btVec3Sub2(&a, x0, P); + + u = btVec3Dot(&a, &a); + v = btVec3Dot(&d1, &d1); + w = btVec3Dot(&d2, &d2); + p = btVec3Dot(&a, &d1); + q = btVec3Dot(&a, &d2); + r = btVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((btFuzzyZero(s) || s > btScalar(0)) + && (ccdEq(s, btScalar(1)) || s < btScalar(1)) + && (btFuzzyZero(t) || t > btScalar(0)) + && (ccdEq(t, btScalar(1)) || t < btScalar(1)) + && (ccdEq(t + s, btScalar(1)) || t + s < btScalar(1))) { + + if (witness) + { + btVec3Scale(&d1, s); + btVec3Scale(&d2, t); + btVec3Copy(witness, x0); + ccdVec3Add(witness, &d1); + ccdVec3Add(witness, &d2); + + dist = ccdVec3Dist2(witness, P); + } + else + { + dist = s * s * v; + dist += t * t * w; + dist += btScalar(2.) * s * t * r; + dist += btScalar(2.) * s * p; + dist += btScalar(2.) * t * q; + dist += u; + } + } + else { + dist = btVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = btVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist) { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + + dist2 = btVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist) { + dist = dist2; + if (witness) + btVec3Copy(witness, &witness2); + } + } + + return dist; +} + + +static int btDoSimplex2(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B; + btVector3 AB, AO, tmp; + btScalar dot; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other point + B = btSimplexPoint(simplex, 0); + // compute AB oriented segment + btVec3Sub2(&AB, &B->v, &A->v); + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // dot product AB . AO + dot = btVec3Dot(&AB, &AO); + + // check if origin doesn't lie on AB segment + btVec3Cross(&tmp, &AB, &AO); + if (btFuzzyZero(btVec3Dot(&tmp, &tmp)) && dot > btScalar(0)) { + return 1; + } + + // check if origin is in area where AB segment is + if (btFuzzyZero(dot) || dot < btScalar(0)) { + // origin is in outside are of A + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + else { + // origin is in area where AB segment is + + // keep simplex untouched and set direction to + // AB x AO x AB + btTripleCross(&AB, &AO, &AB, dir); + } + + return 0; +} + + + +static int btDoSimplex3(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C; + btVector3 AO, AB, AC, ABC, tmp; + btScalar dot, dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 1); + C = btSimplexPoint(simplex, 0); + + // check touching contact + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) { + return 1; + } + + // check if triangle is really triangle (has area > 0) + // if not simplex can't be expanded and thus no itersection is found + if (btVec3Eq(&A->v, &B->v) || btVec3Eq(&A->v, &C->v)) { + return -1; + } + + // compute AO vector + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + + // compute AB and AC segments and ABC vector (perpendircular to triangle) + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + + btVec3Cross(&tmp, &ABC, &AC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) { + dot = btVec3Dot(&AC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) { + // C is already in place + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AC, &AO, &AC, dir); + } + else { + + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + } + else { + btVec3Cross(&tmp, &AB, &ABC); + dot = btVec3Dot(&tmp, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) + { + dot = btVec3Dot(&AB, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) { + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, A); + btSimplexSetSize(simplex, 2); + btTripleCross(&AB, &AO, &AB, dir); + } + else { + btSimplexSet(simplex, 0, A); + btSimplexSetSize(simplex, 1); + btVec3Copy(dir, &AO); + } + } + else { + dot = btVec3Dot(&ABC, &AO); + if (btFuzzyZero(dot) || dot > btScalar(0)) { + btVec3Copy(dir, &ABC); + } + else { + btSupportVector tmp; + btSupportCopy(&tmp, C); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 1, &tmp); + + btVec3Copy(dir, &ABC); + btVec3Scale(dir, -btScalar(1)); + } + } + } + + return 0; +} + +static int btDoSimplex4(btSimplex *simplex, btVector3 *dir) +{ + const btSupportVector *A, *B, *C, *D; + btVector3 AO, AB, AC, AD, ABC, ACD, ADB; + int B_on_ACD, C_on_ADB, D_on_ABC; + int AB_O, AC_O, AD_O; + btScalar dist; + + // get last added as A + A = ccdSimplexLast(simplex); + // get the other points + B = btSimplexPoint(simplex, 2); + C = btSimplexPoint(simplex, 1); + D = btSimplexPoint(simplex, 0); + + // check if tetrahedron is really tetrahedron (has volume > 0) + // if it is not simplex can't be expanded and thus no intersection is + // found + dist = btVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) { + return -1; + } + + // check if origin lies on some of tetrahedron's face - if so objects + // intersect + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &C->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &A->v, &B->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + dist = btVec3PointTriDist2(&ccd_vec3_origin, &B->v, &C->v, &D->v, 0); + if (btFuzzyZero(dist)) + return 1; + + // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors + btVec3Copy(&AO, &A->v); + btVec3Scale(&AO, -btScalar(1)); + btVec3Sub2(&AB, &B->v, &A->v); + btVec3Sub2(&AC, &C->v, &A->v); + btVec3Sub2(&AD, &D->v, &A->v); + btVec3Cross(&ABC, &AB, &AC); + btVec3Cross(&ACD, &AC, &AD); + btVec3Cross(&ADB, &AD, &AB); + + // side (positive or negative) of B, C, D relative to planes ACD, ADB + // and ABC respectively + B_on_ACD = ccdSign(btVec3Dot(&ACD, &AB)); + C_on_ADB = ccdSign(btVec3Dot(&ADB, &AC)); + D_on_ABC = ccdSign(btVec3Dot(&ABC, &AD)); + + // whether origin is on same side of ACD, ADB, ABC as B, C, D + // respectively + AB_O = ccdSign(btVec3Dot(&ACD, &AO)) == B_on_ACD; + AC_O = ccdSign(btVec3Dot(&ADB, &AO)) == C_on_ADB; + AD_O = ccdSign(btVec3Dot(&ABC, &AO)) == D_on_ABC; + + if (AB_O && AC_O && AD_O) { + // origin is in tetrahedron + return 1; + // rearrange simplex to triangle and call btDoSimplex3() + } + else if (!AB_O) { + // B is farthest from the origin among all of the tetrahedron's + // points, so remove it from the list and go on with the triangle + // case + + // D and C are in place + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else if (!AC_O) { + // C is farthest + btSimplexSet(simplex, 1, D); + btSimplexSet(simplex, 0, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + else { // (!AD_O) + btSimplexSet(simplex, 0, C); + btSimplexSet(simplex, 1, B); + btSimplexSet(simplex, 2, A); + btSimplexSetSize(simplex, 3); + } + + return btDoSimplex3(simplex, dir); +} + +static int btDoSimplex(btSimplex *simplex, btVector3 *dir) +{ + if (btSimplexSize(simplex) == 2) { + // simplex contains segment only one segment + return btDoSimplex2(simplex, dir); + } + else if (btSimplexSize(simplex) == 3) { + // simplex contains triangle + return btDoSimplex3(simplex, dir); + } + else { // btSimplexSize(simplex) == 4 + // tetrahedron - this is the only shape which can encapsule origin + // so btDoSimplex4() also contains test on it + return btDoSimplex4(simplex, dir); + } +} + #ifdef __SPU__ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw) #else @@ -123,193 +710,308 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu bool checkSimplex = false; bool checkPenetration = true; m_degenerateSimplex = 0; - + m_lastUsedMethod = -1; - + int status = -2; + btVector3 orgNormalInB(0, 0, 0); + btScalar margin = marginA + marginB; + + //we add a separate implementation to check if the convex shapes intersect + //See also "Real-time Collision Detection with Implicit Objects" by Leif Olvang + //Todo: integrate the simplex penetration check directly inside the Bullet btVoronoiSimplexSolver + //and remove this temporary code from libCCD + //this fixes issue https://github.com/bulletphysics/bullet3/issues/1703 + //note, for large differences in shapes, use double precision build! { btScalar squaredDistance = BT_LARGE_FLOAT; btScalar delta = btScalar(0.); - - btScalar margin = marginA + marginB; - - - m_simplexSolver->reset(); + + - for ( ; ; ) - //while (true) - { + btSimplex simplex1; + btSimplex* simplex = &simplex1; + btSimplexInit(simplex); - btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = m_cachedSeparatingAxis* input.m_transformB.getBasis(); + btVector3 dir(1, 0, 0); + { - btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); - btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + btVector3 lastSupV; + btVector3 supAworld; + btVector3 supBworld; + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); + + btSupportVector last; + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; - btVector3 pWorld = localTransA(pInA); - btVector3 qWorld = localTransB(qInB); + btSimplexAdd(simplex, &last); + dir = -lastSupV; - if (check2d) + + + // start iterations + for (int iterations = 0; iterations <gGjkMaxIter; iterations++) { - pWorld[2] = 0.f; - qWorld[2] = 0.f; - } + // obtain support point + btComputeSupport(m_minkowskiA, localTransA, m_minkowskiB, localTransB, dir, check2d, supAworld, supBworld, lastSupV); + + // check if farthest point in Minkowski difference in direction dir + // isn't somewhere before origin (the test on negative dot product) + // - because if it is, objects are not intersecting at all. + btScalar delta = lastSupV.dot(dir); + if (delta < 0) + { + //no intersection, besides margin + status = -1; + break; + } + + // add last support vector to simplex + last.v = lastSupV; + last.v1 = supAworld; + last.v2 = supBworld; - btVector3 w = pWorld - qWorld; - delta = m_cachedSeparatingAxis.dot(w); + btSimplexAdd(simplex, &last); - // potential exit, they don't overlap - if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) - { - m_degenerateSimplex = 10; - checkSimplex=true; - //checkPenetration = false; - break; - } + // if btDoSimplex returns 1 if objects intersect, -1 if objects don't + // intersect and 0 if algorithm should continue - //exit 0: the new point is already in the simplex, or we didn't come any closer - if (m_simplexSolver->inSimplex(w)) - { - m_degenerateSimplex = 1; - checkSimplex = true; - break; - } - // are we getting any closer ? - btScalar f0 = squaredDistance - delta; - btScalar f1 = squaredDistance * REL_ERROR2; + btVector3 newDir; + int do_simplex_res = btDoSimplex(simplex, &dir); - if (f0 <= f1) - { - if (f0 <= btScalar(0.)) + if (do_simplex_res == 1) { - m_degenerateSimplex = 2; - } else + status = 0; // intersection found + break; + } + else if (do_simplex_res == -1) + { + // intersection not found + status = -1; + break; + } + + if (btFuzzyZero(btVec3Dot(&dir, &dir))) + { + // intersection not found + status = -1; + } + + if (dir.length2() < SIMD_EPSILON) { - m_degenerateSimplex = 11; + //no intersection, besides margin + status = -1; + break; + } + + if (dir.fuzzyZero()) + { + // intersection not found + status = -1; + break; } - checkSimplex = true; - break; } - //add current vertex to simplex - m_simplexSolver->addVertex(w, pWorld, qWorld); - btVector3 newCachedSeparatingAxis; + } + + m_simplexSolver->reset(); + if (status == 0) + { + //status = 0; + //printf("Intersect!\n"); + } - //calculate the closest point to the origin (update vector v) - if (!m_simplexSolver->closest(newCachedSeparatingAxis)) + if (status==-1) + { + //printf("not intersect\n"); + } + //printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]); + if (1) + { + for (; ; ) + //while (true) { - m_degenerateSimplex = 3; - checkSimplex = true; - break; - } - if(newCachedSeparatingAxis.length2()<REL_ERROR2) - { - m_cachedSeparatingAxis = newCachedSeparatingAxis; - m_degenerateSimplex = 6; - checkSimplex = true; - break; - } + btVector3 seperatingAxisInA = (-m_cachedSeparatingAxis)* localTransA.getBasis(); + btVector3 seperatingAxisInB = m_cachedSeparatingAxis* localTransB.getBasis(); + + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); - btScalar previousSquaredDistance = squaredDistance; - squaredDistance = newCachedSeparatingAxis.length2(); + + if (check2d) + { + pWorld[2] = 0.f; + qWorld[2] = 0.f; + } + + btVector3 w = pWorld - qWorld; + delta = m_cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > btScalar(0.0)) && (delta * delta > squaredDistance * input.m_maximumDistanceSquared)) + { + 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 (m_simplexSolver->inSimplex(w)) + { + m_degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + btScalar f0 = squaredDistance - delta; + btScalar f1 = squaredDistance * REL_ERROR2; + + if (f0 <= f1) + { + if (f0 <= btScalar(0.)) + { + m_degenerateSimplex = 2; + } + else + { + m_degenerateSimplex = 11; + } + checkSimplex = true; + break; + } + + //add current vertex to simplex + m_simplexSolver->addVertex(w, pWorld, qWorld); + btVector3 newCachedSeparatingAxis; + + //calculate the closest point to the origin (update vector v) + if (!m_simplexSolver->closest(newCachedSeparatingAxis)) + { + m_degenerateSimplex = 3; + checkSimplex = true; + break; + } + + if (newCachedSeparatingAxis.length2() < REL_ERROR2) + { + m_cachedSeparatingAxis = newCachedSeparatingAxis; + m_degenerateSimplex = 6; + checkSimplex = true; + break; + } + + btScalar previousSquaredDistance = squaredDistance; + squaredDistance = newCachedSeparatingAxis.length2(); #if 0 -///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo - if (squaredDistance>previousSquaredDistance) - { - m_degenerateSimplex = 7; - squaredDistance = previousSquaredDistance; - checkSimplex = false; - break; - } + ///warning: this termination condition leads to some problems in 2d test case see Bullet/Demos/Box2dDemo + if (squaredDistance > previousSquaredDistance) + { + m_degenerateSimplex = 7; + squaredDistance = previousSquaredDistance; + checkSimplex = false; + break; + } #endif // - - //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - //are we getting any closer ? - if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) - { -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - checkSimplex = true; - m_degenerateSimplex = 12; - - break; - } + //redundant m_simplexSolver->compute_points(pointOnA, pointOnB); - m_cachedSeparatingAxis = newCachedSeparatingAxis; + //are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= SIMD_EPSILON * previousSquaredDistance) + { + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + checkSimplex = true; + m_degenerateSimplex = 12; + + break; + } - //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject - if (m_curIter++ > gGjkMaxIter) - { - #if defined(DEBUG) || defined (_DEBUG) + m_cachedSeparatingAxis = newCachedSeparatingAxis; - printf("btGjkPairDetector maxIter exceeded:%i\n",m_curIter); - printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", - m_cachedSeparatingAxis.getX(), - m_cachedSeparatingAxis.getY(), - m_cachedSeparatingAxis.getZ(), - squaredDistance, - m_minkowskiA->getShapeType(), - m_minkowskiB->getShapeType()); + //degeneracy, this is typically due to invalid/uninitialized worldtransforms for a btCollisionObject + if (m_curIter++ > gGjkMaxIter) + { +#if defined(DEBUG) || defined (_DEBUG) - #endif - break; + printf("btGjkPairDetector maxIter exceeded:%i\n", m_curIter); + printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", + m_cachedSeparatingAxis.getX(), + m_cachedSeparatingAxis.getY(), + m_cachedSeparatingAxis.getZ(), + squaredDistance, + m_minkowskiA->getShapeType(), + m_minkowskiB->getShapeType()); - } +#endif + break; + } - bool check = (!m_simplexSolver->fullSimplex()); - //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); - if (!check) - { - //do we need this backup_closest here ? -// m_simplexSolver->backup_closest(m_cachedSeparatingAxis); - m_degenerateSimplex = 13; - break; + bool check = (!m_simplexSolver->fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) + { + //do we need this backup_closest here ? + // m_simplexSolver->backup_closest(m_cachedSeparatingAxis); + m_degenerateSimplex = 13; + break; + } } - } - if (checkSimplex) - { + if (checkSimplex) + { m_simplexSolver->compute_points(pointOnA, pointOnB); normalInB = m_cachedSeparatingAxis; btScalar lenSqr =m_cachedSeparatingAxis.length2(); - //valid normal - if (lenSqr < REL_ERROR2) - { - m_degenerateSimplex = 5; - } - if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) - { - btScalar rlen = btScalar(1.) / btSqrt(lenSqr ); - normalInB *= rlen; //normalize + //valid normal + if (lenSqr < REL_ERROR2) + { + m_degenerateSimplex = 5; + } + if (lenSqr > SIMD_EPSILON*SIMD_EPSILON) + { + btScalar rlen = btScalar(1.) / btSqrt(lenSqr); + normalInB *= rlen; //normalize - btScalar s = btSqrt(squaredDistance); - - btAssert(s > btScalar(0.0)); - pointOnA -= m_cachedSeparatingAxis * (marginA / s); - pointOnB += m_cachedSeparatingAxis * (marginB / s); - distance = ((btScalar(1.)/rlen) - margin); - isValid = true; - - m_lastUsedMethod = 1; - } else - { - m_lastUsedMethod = 2; + btScalar s = btSqrt(squaredDistance); + + btAssert(s > btScalar(0.0)); + pointOnA -= m_cachedSeparatingAxis * (marginA / s); + pointOnB += m_cachedSeparatingAxis * (marginB / s); + distance = ((btScalar(1.) / rlen) - margin); + isValid = true; + orgNormalInB = normalInB; + + m_lastUsedMethod = 1; + } + else + { + m_lastUsedMethod = 2; + } } } + + bool catchDegeneratePenetrationCase = (m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance)); //if (checkPenetration && !isValid) - if (checkPenetration && (!isValid || catchDegeneratePenetrationCase )) + if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase )) || (status == 0)) { //penetration case @@ -331,70 +1033,79 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu ); - if (isValid2) + if (m_cachedSeparatingAxis.length2()) { - btVector3 tmpNormalInB = tmpPointOnB-tmpPointOnA; - btScalar lenSqr = tmpNormalInB.length2(); - if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + if (isValid2) { - tmpNormalInB = m_cachedSeparatingAxis; - lenSqr = m_cachedSeparatingAxis.length2(); - } + btVector3 tmpNormalInB = tmpPointOnB - tmpPointOnA; + btScalar lenSqr = tmpNormalInB.length2(); + if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON)) + { + tmpNormalInB = m_cachedSeparatingAxis; + lenSqr = m_cachedSeparatingAxis.length2(); + } - if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) - { - tmpNormalInB /= btSqrt(lenSqr); - btScalar distance2 = -(tmpPointOnA-tmpPointOnB).length(); - m_lastUsedMethod = 3; - //only replace valid penetrations when the result is deeper (check) - if (!isValid || (distance2 < distance)) + if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON)) { - distance = distance2; - pointOnA = tmpPointOnA; - pointOnB = tmpPointOnB; - normalInB = tmpNormalInB; - - isValid = true; - - } else + tmpNormalInB /= btSqrt(lenSqr); + btScalar distance2 = -(tmpPointOnA - tmpPointOnB).length(); + m_lastUsedMethod = 3; + //only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) + { + distance = distance2; + pointOnA = tmpPointOnA; + pointOnB = tmpPointOnB; + normalInB = tmpNormalInB; + isValid = true; + + } + else + { + m_lastUsedMethod = 8; + } + } + else { - m_lastUsedMethod = 8; + m_lastUsedMethod = 9; } - } else - { - 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 + else - - if (m_cachedSeparatingAxis.length2() > btScalar(0.)) { - btScalar 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 -= m_cachedSeparatingAxis * marginA ; - pointOnB += m_cachedSeparatingAxis * marginB ; - normalInB = m_cachedSeparatingAxis; - normalInB.normalize(); - - isValid = true; - m_lastUsedMethod = 6; - } 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 (m_cachedSeparatingAxis.length2() > btScalar(0.)) { - m_lastUsedMethod = 5; + btScalar 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 -= m_cachedSeparatingAxis * marginA; + pointOnB += m_cachedSeparatingAxis * marginB; + normalInB = m_cachedSeparatingAxis; + normalInB.normalize(); + + isValid = true; + m_lastUsedMethod = 6; + } + else + { + m_lastUsedMethod = 5; + } } } + } else + { + //printf("EPA didn't return a valid value\n"); } } @@ -409,17 +1120,33 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu m_cachedSeparatingAxis = normalInB; m_cachedSeparatingDistance = distance; - + if (1) { ///todo: need to track down this EPA penetration solver degeneracy ///the penetration solver reports penetration but the contact normal ///connecting the contact points is pointing in the opposite direction ///until then, detect the issue and revert the normal + btScalar d2 = 0.f; + { + btVector3 seperatingAxisInA = (-orgNormalInB)* localTransA.getBasis(); + btVector3 seperatingAxisInB = orgNormalInB* localTransB.getBasis(); + + + btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); + btVector3 qInB = m_minkowskiB->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInB); + + btVector3 pWorld = localTransA(pInA); + btVector3 qWorld = localTransB(qInB); + btVector3 w = pWorld - qWorld; + d2 = orgNormalInB.dot(w)- margin; + } + btScalar d1=0; { - btVector3 seperatingAxisInA = (normalInB)* input.m_transformA.getBasis(); - btVector3 seperatingAxisInB = -normalInB* input.m_transformB.getBasis(); + + btVector3 seperatingAxisInA = (normalInB)* localTransA.getBasis(); + btVector3 seperatingAxisInB = -normalInB* localTransB.getBasis(); btVector3 pInA = m_minkowskiA->localGetSupportVertexWithoutMarginNonVirtual(seperatingAxisInA); @@ -428,7 +1155,8 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btVector3 pWorld = localTransA(pInA); btVector3 qWorld = localTransB(qInB); btVector3 w = pWorld - qWorld; - d1 = (-normalInB).dot(w); + d1 = (-normalInB).dot(w)- margin; + } btScalar d0 = 0.f; { @@ -442,21 +1170,37 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu btVector3 pWorld = localTransA(pInA); btVector3 qWorld = localTransB(qInB); btVector3 w = pWorld - qWorld; - d0 = normalInB.dot(w); + d0 = normalInB.dot(w)-margin; } + if (d1>d0) { m_lastUsedMethod = 10; normalInB*=-1; } + if (orgNormalInB.length2()) + { + if (d2 > d0 && d2 > d1 && d2 > distance) + { + + normalInB = orgNormalInB; + distance = d2; + } + } } + + output.addContactPoint( normalInB, pointOnB+positionOffset, distance); } + else + { + //printf("invalid gjk query\n"); + } } diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 23aaece22b..9603a8bbdc 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -16,7 +16,13 @@ subject to the following restrictions: #include "btPersistentManifold.h" #include "LinearMath/btTransform.h" +#include "LinearMath/btSerializer.h" +#ifdef BT_USE_DOUBLE_PRECISION +#define btCollisionObjectData btCollisionObjectDoubleData +#else +#define btCollisionObjectData btCollisionObjectFloatData +#endif btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; @@ -33,6 +39,8 @@ btPersistentManifold::btPersistentManifold() m_body0(0), m_body1(0), m_cachedPoints (0), +m_companionIdA(0), +m_companionIdB(0), m_index1a(0) { } @@ -303,6 +311,149 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT } +int btPersistentManifold::calculateSerializeBufferSize() const +{ + return sizeof(btPersistentManifoldData); +} + +const char* btPersistentManifold::serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const +{ + btPersistentManifoldData* dataOut = (btPersistentManifoldData*)dataBuffer; + memset(dataOut, 0, sizeof(btPersistentManifoldData)); + + dataOut->m_body0 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody0()); + dataOut->m_body1 = (btCollisionObjectData*)serializer->getUniquePointer((void*)manifold->getBody1()); + dataOut->m_contactBreakingThreshold = manifold->getContactBreakingThreshold(); + dataOut->m_contactProcessingThreshold = manifold->getContactProcessingThreshold(); + dataOut->m_numCachedPoints = manifold->getNumContacts(); + dataOut->m_companionIdA = manifold->m_companionIdA; + dataOut->m_companionIdB = manifold->m_companionIdB; + dataOut->m_index1a = manifold->m_index1a; + dataOut->m_objectType = manifold->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + const btManifoldPoint& pt = manifold->getContactPoint(i); + dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; + dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; + dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; + pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); + pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.serialize(dataOut->m_pointCacheNormalWorldOnB[i]); + dataOut->m_pointCacheDistance[i] = pt.m_distance1; + dataOut->m_pointCacheCombinedContactDamping1[i] = pt.m_combinedContactDamping1; + dataOut->m_pointCacheCombinedContactStiffness1[i] = pt.m_combinedContactStiffness1; + dataOut->m_pointCacheLifeTime[i] = pt.m_lifeTime; + dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM; + dataOut->m_pointCacheContactERP[i] = pt.m_contactERP; + dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM; + dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags; + dataOut->m_pointCacheIndex0[i] = pt.m_index0; + dataOut->m_pointCacheIndex1[i] = pt.m_index1; + dataOut->m_pointCachePartId0[i] = pt.m_partId0; + dataOut->m_pointCachePartId1[i] = pt.m_partId1; + pt.m_positionWorldOnA.serialize(dataOut->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.serialize(dataOut->m_pointCachePositionWorldOnB[i]); + dataOut->m_pointCacheCombinedFriction[i] = pt.m_combinedFriction; + pt.m_lateralFrictionDir1.serialize(dataOut->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.serialize(dataOut->m_pointCacheLateralFrictionDir2[i]); + dataOut->m_pointCacheCombinedRollingFriction[i] = pt.m_combinedRollingFriction; + dataOut->m_pointCacheCombinedSpinningFriction[i] = pt.m_combinedSpinningFriction; + dataOut->m_pointCacheCombinedRestitution[i] = pt.m_combinedRestitution; + dataOut->m_pointCacheContactMotion1[i] = pt.m_contactMotion1; + dataOut->m_pointCacheContactMotion2[i] = pt.m_contactMotion2; + } + return btPersistentManifoldDataName; +} + +void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerializeDouble(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerializeDouble(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } +} +void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr) +{ + m_contactBreakingThreshold = manifoldDataPtr->m_contactBreakingThreshold; + m_contactProcessingThreshold = manifoldDataPtr->m_contactProcessingThreshold; + m_cachedPoints = manifoldDataPtr->m_numCachedPoints; + m_companionIdA = manifoldDataPtr->m_companionIdA; + m_companionIdB = manifoldDataPtr->m_companionIdB; + //m_index1a = manifoldDataPtr->m_index1a; + m_objectType = manifoldDataPtr->m_objectType; + + for (int i = 0; i < this->getNumContacts(); i++) + { + btManifoldPoint& pt = m_pointCache[i]; + + pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i]; + pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i]; + pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i]; + pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]); + pt.m_localPointB.deSerialize(manifoldDataPtr->m_pointCacheLocalPointB[i]); + pt.m_normalWorldOnB.deSerialize(manifoldDataPtr->m_pointCacheNormalWorldOnB[i]); + pt.m_distance1 = manifoldDataPtr->m_pointCacheDistance[i]; + pt.m_combinedContactDamping1 = manifoldDataPtr->m_pointCacheCombinedContactDamping1[i]; + pt.m_combinedContactStiffness1 = manifoldDataPtr->m_pointCacheCombinedContactStiffness1[i]; + pt.m_lifeTime = manifoldDataPtr->m_pointCacheLifeTime[i]; + pt.m_frictionCFM = manifoldDataPtr->m_pointCacheFrictionCFM[i]; + pt.m_contactERP = manifoldDataPtr->m_pointCacheContactERP[i]; + pt.m_contactCFM = manifoldDataPtr->m_pointCacheContactCFM[i]; + pt.m_contactPointFlags = manifoldDataPtr->m_pointCacheContactPointFlags[i]; + pt.m_index0 = manifoldDataPtr->m_pointCacheIndex0[i]; + pt.m_index1 = manifoldDataPtr->m_pointCacheIndex1[i]; + pt.m_partId0 = manifoldDataPtr->m_pointCachePartId0[i]; + pt.m_partId1 = manifoldDataPtr->m_pointCachePartId1[i]; + pt.m_positionWorldOnA.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnA[i]); + pt.m_positionWorldOnB.deSerialize(manifoldDataPtr->m_pointCachePositionWorldOnB[i]); + pt.m_combinedFriction = manifoldDataPtr->m_pointCacheCombinedFriction[i]; + pt.m_lateralFrictionDir1.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir1[i]); + pt.m_lateralFrictionDir2.deSerialize(manifoldDataPtr->m_pointCacheLateralFrictionDir2[i]); + pt.m_combinedRollingFriction = manifoldDataPtr->m_pointCacheCombinedRollingFriction[i]; + pt.m_combinedSpinningFriction = manifoldDataPtr->m_pointCacheCombinedSpinningFriction[i]; + pt.m_combinedRestitution = manifoldDataPtr->m_pointCacheCombinedRestitution[i]; + pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i]; + pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i]; + } +}
\ No newline at end of file diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index f872c8e1c9..67be0c48eb 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -24,6 +24,8 @@ class btCollisionObject; #include "LinearMath/btAlignedAllocator.h" struct btCollisionResult; +struct btCollisionObjectDoubleData; +struct btCollisionObjectFloatData; ///maximum contact breaking and merging threshold extern btScalar gContactBreakingThreshold; @@ -95,7 +97,10 @@ public: : btTypedObject(BT_PERSISTENT_MANIFOLD_TYPE), m_body0(body0),m_body1(body1),m_cachedPoints(0), m_contactBreakingThreshold(contactBreakingThreshold), - m_contactProcessingThreshold(contactProcessingThreshold) + m_contactProcessingThreshold(contactProcessingThreshold), + m_companionIdA(0), + m_companionIdB(0), + m_index1a(0) { } @@ -256,10 +261,115 @@ public: m_cachedPoints = 0; } + int calculateSerializeBufferSize() const; + const char* serialize(const class btPersistentManifold* manifold, void* dataBuffer, class btSerializer* serializer) const; + void deSerialize(const struct btPersistentManifoldDoubleData* manifoldDataPtr); + void deSerialize(const struct btPersistentManifoldFloatData* manifoldDataPtr); -} -; +}; + + + +struct btPersistentManifoldDoubleData +{ + btVector3DoubleData m_pointCacheLocalPointA[4]; + btVector3DoubleData m_pointCacheLocalPointB[4]; + btVector3DoubleData m_pointCachePositionWorldOnA[4]; + btVector3DoubleData m_pointCachePositionWorldOnB[4]; + btVector3DoubleData m_pointCacheNormalWorldOnB[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir1[4]; + btVector3DoubleData m_pointCacheLateralFrictionDir2[4]; + double m_pointCacheDistance[4]; + double m_pointCacheAppliedImpulse[4]; + double m_pointCacheCombinedFriction[4]; + double m_pointCacheCombinedRollingFriction[4]; + double m_pointCacheCombinedSpinningFriction[4]; + double m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + double m_pointCacheAppliedImpulseLateral1[4]; + double m_pointCacheAppliedImpulseLateral2[4]; + double m_pointCacheContactMotion1[4]; + double m_pointCacheContactMotion2[4]; + double m_pointCacheContactCFM[4]; + double m_pointCacheCombinedContactStiffness1[4]; + double m_pointCacheContactERP[4]; + double m_pointCacheCombinedContactDamping1[4]; + double m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + + int m_objectType; + double m_contactBreakingThreshold; + double m_contactProcessingThreshold; + int m_padding; + + btCollisionObjectDoubleData *m_body0; + btCollisionObjectDoubleData *m_body1; +}; + + +struct btPersistentManifoldFloatData +{ + btVector3FloatData m_pointCacheLocalPointA[4]; + btVector3FloatData m_pointCacheLocalPointB[4]; + btVector3FloatData m_pointCachePositionWorldOnA[4]; + btVector3FloatData m_pointCachePositionWorldOnB[4]; + btVector3FloatData m_pointCacheNormalWorldOnB[4]; + btVector3FloatData m_pointCacheLateralFrictionDir1[4]; + btVector3FloatData m_pointCacheLateralFrictionDir2[4]; + float m_pointCacheDistance[4]; + float m_pointCacheAppliedImpulse[4]; + float m_pointCacheCombinedFriction[4]; + float m_pointCacheCombinedRollingFriction[4]; + float m_pointCacheCombinedSpinningFriction[4]; + float m_pointCacheCombinedRestitution[4]; + int m_pointCachePartId0[4]; + int m_pointCachePartId1[4]; + int m_pointCacheIndex0[4]; + int m_pointCacheIndex1[4]; + int m_pointCacheContactPointFlags[4]; + float m_pointCacheAppliedImpulseLateral1[4]; + float m_pointCacheAppliedImpulseLateral2[4]; + float m_pointCacheContactMotion1[4]; + float m_pointCacheContactMotion2[4]; + float m_pointCacheContactCFM[4]; + float m_pointCacheCombinedContactStiffness1[4]; + float m_pointCacheContactERP[4]; + float m_pointCacheCombinedContactDamping1[4]; + float m_pointCacheFrictionCFM[4]; + int m_pointCacheLifeTime[4]; + + int m_numCachedPoints; + int m_companionIdA; + int m_companionIdB; + int m_index1a; + + int m_objectType; + float m_contactBreakingThreshold; + float m_contactProcessingThreshold; + int m_padding; + + btCollisionObjectFloatData *m_body0; + btCollisionObjectFloatData *m_body1; +}; + +#ifdef BT_USE_DOUBLE_PRECISION +#define btPersistentManifoldData btPersistentManifoldDoubleData +#define btPersistentManifoldDataName "btPersistentManifoldDoubleData" +#else +#define btPersistentManifoldData btPersistentManifoldFloatData +#define btPersistentManifoldDataName "btPersistentManifoldFloatData" +#endif //BT_USE_DOUBLE_PRECISION + diff --git a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index ec638f60ba..08d6e6de86 100644 --- a/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp +++ b/thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp @@ -72,11 +72,18 @@ bool btSubsimplexConvexCast::calcTimeOfImpact( btScalar dist2 = v.length2(); + #ifdef BT_USE_DOUBLE_PRECISION - btScalar epsilon = btScalar(0.0001); + btScalar epsilon = SIMD_EPSILON * 10; #else +//todo: epsilon kept for backward compatibility of unit tests. +//will need to digg deeper to make the algorithm more robust +//since, a large epsilon can cause an early termination with false +//positive results (ray intersections that shouldn't be there) btScalar epsilon = btScalar(0.0001); #endif //BT_USE_DOUBLE_PRECISION + + btVector3 w,p; btScalar VdotR; |