summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision')
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h8
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h1
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp11
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h3
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btDispatcher.h4
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp91
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h12
-rw-r--r--thirdparty/bullet/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp5
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp6
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp8
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp11
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.cpp19
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionObject.h13
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.cpp26
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCollisionWorld.h3
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp14
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp21
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp105
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp150
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.cpp7
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btHashedSimplePairCache.h6
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp19
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp42
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btCapsuleShape.h1
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.cpp2
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btCompoundShape.h2
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp12
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btConvexPolyhedron.h1
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btConvexShape.cpp8
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.cpp532
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btMiniSDF.h134
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp79
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h3
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.cpp99
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btSdfCollisionShape.h30
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.cpp277
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btShapeHull.h4
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btStaticPlaneShape.cpp2
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h2
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp25
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h2
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp68
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp1130
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp151
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h116
-rw-r--r--thirdparty/bullet/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp9
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;