Merge remote-tracking branch 'origin/dev' into python-dev

libsrc/bullet3-3.24/BulletCollision/BroadphaseCollision/btRSBroadphase.cpp

@@ -22,6 +22,8 @@ subject to the following restrictions:
 #include "../../LinearMath/btMatrix3x3.h"
 #include "../../LinearMath/btAabbUtil2.h"
 
+#include "../CollisionShapes/btBvhTriangleMeshShape.h"
+
 #include <new>
 #include <string>
 #include <stdexcept>
@@ -103,10 +105,48 @@ void _UpdateCellsStatic(btRSBroadphase* _this, btRSBroadphaseProxy* proxy) {
 	_this->GetCellIndices(proxy->m_aabbMin, iMin, jMin, kMin);
 	_this->GetCellIndices(aabbMax, iMax, jMax, kMax);
 
+	btCollisionObject* colObj = (btCollisionObject*)proxy->m_clientObject;
+
+	// We should check if each cell actually collides with the object
+	bool isTriMesh = colObj && colObj->m_collisionShape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE;
+
+	// For checking if an AABB has any containing triangles
+	struct BoolHitTriangleCallback : public btTriangleCallback {
+
+		bool hit = false;
+
+		BoolHitTriangleCallback() {}
+		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) {
+			hit = true;
+		}
+	};
+	BoolHitTriangleCallback callbackInst = {};
+
+	int numSkipped = 0;
 	for (int i = iMin; i <= iMax; i++) {
 		for (int j = jMin; j <= jMax; j++) {
 			for (int k = kMin; k <= kMax; k++) {
 				auto& cell = _this->GetCell(i, j, k);
+
+				if (isTriMesh) {
+					auto triMeshShape = (btTriangleMeshShape*)colObj->m_collisionShape;
+					btVector3 cellMin = _this->GetCellMinPos(i, j, k);
+					btVector3 cellMax = cellMin + btVector3(_this->cellSize, _this->cellSize, _this->cellSize);
+
+					callbackInst.hit = false;
+					triMeshShape->processAllTriangles(&callbackInst, cellMin, cellMax);
+
+					if (!callbackInst.hit) {
+						numSkipped++;
+
+						if (ADD) {
+							continue; // No tris in this AABB, ignore
+						} else {
+							// Remove it anyway
+						}
+					}
+				}
+
 				if (ADD) {
 					cell.staticHandles.push_back(proxy);
 				} else {
@@ -272,9 +312,11 @@ void btRSBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, b
 
 		Cell& cell = cells[GetCellIdx(rayFrom)];
 		for (auto& otherProxy : cell.staticHandles)
-			rayCallback.process(otherProxy);
+			if (otherProxy->m_clientObject)
+				rayCallback.process(otherProxy);
 		for (auto& otherProxy : cell.dynHandles)
-			rayCallback.process(otherProxy);
+			if (otherProxy->m_clientObject)
+				rayCallback.process(otherProxy);
 	} else {
 		static std::once_flag onceFlag;
 		std::call_once(onceFlag, 
@@ -302,19 +344,17 @@ void btRSBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax
 
 	for (int i = 0; i <= m_LastHandleIndex; i++) {
 		btRSBroadphaseProxy* proxy = &m_pHandles[i];
-		if (!proxy->m_clientObject) {
+		if (!proxy->m_clientObject)
 			continue;
-		}
+
 		if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax)) {
 			callback.process(proxy);
 		}
 	}
 }
 
 bool btRSBroadphase::aabbOverlap(btRSBroadphaseProxy* proxy0, btRSBroadphaseProxy* proxy1) {
-	return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
-		proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
-		proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
+	return TestAabbAgainstAabb2(proxy0->m_aabbMin, proxy0->m_aabbMax, proxy1->m_aabbMin, proxy1->m_aabbMax);
 }
 
 //then remove non-overlapping ones
@@ -327,6 +367,7 @@ class CheckOverlapCallback : public btOverlapCallback
 };
 
 void btRSBroadphase::calculateOverlappingPairs(btCollisionDispatcher* dispatcher) {
+
 	bool shouldRemove = !m_pairCache->hasDeferredRemoval();
 	if (m_numHandles >= 0) {
 		int new_largest_index = -1;
@@ -345,6 +386,9 @@ void btRSBroadphase::calculateOverlappingPairs(btCollisionDispatcher* dispatcher
 			Cell& cell = cells[proxy->cellIdx];
 
 			for (auto& otherProxy : cell.staticHandles) {
+				if (!otherProxy->m_clientObject)
+					continue;
+
 				totalStaticPairs++;
 
 				if (aabbOverlap(proxy, otherProxy)) {
@@ -367,6 +411,9 @@ void btRSBroadphase::calculateOverlappingPairs(btCollisionDispatcher* dispatcher
 						if (otherProxy == proxy)
 							continue;
 
+						if (!otherProxy->m_clientObject)
+							continue;
+
 						totalDynPairs++;
 
 						if (aabbOverlap(proxy, otherProxy)) {
@@ -400,5 +447,5 @@ bool btRSBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProx
 }
 
 void btRSBroadphase::resetPool(btCollisionDispatcher* dispatcher) {
-	//not yet
+	// TODO: ?
 }

libsrc/bullet3-3.24/BulletCollision/BroadphaseCollision/btRSBroadphase.h

@@ -95,6 +95,10 @@ class btRSBroadphase : public btBroadphaseInterface
 		btClamp(k, 0, cellsZ - 1);
 	}
 
+	btVector3 GetCellMinPos(int i, int j, int k) const {
+		return minPos + btVector3(i, j, k) * cellSize;
+	}
+
 	int GetCellIdx(const btVector3& pos) const {
 		int i, j, k;
 		GetCellIndices(pos, i, j, k);

libsrc/bullet3-3.24/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -84,6 +84,50 @@ btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const bt
 	return diff.dot(diff);
 }
 
+// From https://github.com/RenderKit/embree/blob/master/tutorials/common/math/closest_point.h (thanks VirxEC)
+btVector3 closestPointTriangle(btVector3 const& p, btVector3 const& a, btVector3 const& b, btVector3 const& c) {
+	const btVector3 ab = b - a;
+	const btVector3 ac = c - a;
+	const btVector3 ap = p - a;
+
+	const float d1 = ab.dot(ap);
+	const float d2 = ac.dot(ap);
+	if (d1 <= 0.f && d2 <= 0.f) return a; //#1
+
+	const btVector3 bp = p - b;
+	const float d3 = ab.dot(bp);
+	const float d4 = ac.dot(bp);
+	if (d3 >= 0.f && d4 <= d3) return b; //#2
+
+	const btVector3 cp = p - c;
+	const float d5 = ab.dot(cp);
+	const float d6 = ac.dot(cp);
+	if (d6 >= 0.f && d5 <= d6) return c; //#3
+
+	const float vc = d1 * d4 - d3 * d2;
+	if (vc <= 0.f && d1 >= 0.f && d3 <= 0.f) {
+		const float v = d1 / (d1 - d3);
+		return a + v * ab; //#4
+	}
+
+	const float vb = d5 * d2 - d1 * d6;
+	if (vb <= 0.f && d2 >= 0.f && d6 <= 0.f) {
+		const float v = d2 / (d2 - d6);
+		return a + v * ac; //#5
+	}
+
+	const float va = d3 * d6 - d5 * d4;
+	if (va <= 0.f && (d4 - d3) >= 0.f && (d5 - d6) >= 0.f) {
+		const float v = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+		return b + v * (c - b); //#6
+	}
+
+	const float denom = 1.f / (va + vb + vc);
+	const float v = vb * denom;
+	const float w = vc * denom;
+	return a + v * ab + w * ac; //#0
+}
+
 bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal)
 {
 	btVector3 lp(p);
@@ -136,8 +180,10 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& p
 				// Could be inside one of the contact capsules
 				btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
 				btScalar minDistSqr = contactCapsuleRadiusSqr;
+
+#if 0 // Bullet's method
 				btVector3 nearestOnEdge;
-				for (int i = 0; i < m_triangle->getNumEdges(); i++)
+				for (int i = 0; i < 3; i++)
 				{
 					btVector3 pa;
 					btVector3 pb;
@@ -153,8 +199,19 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& p
 						contactPoint = nearestOnEdge;
 					}
 				}
+#else // Faster method (thanks VirxEC)
+				// https://github.com/VirxEC/rl_ball_sym/blob/99b50b381cd529e567c9a33ab10464b89484227a/src/simulation/geometry.rs
+				btVector3 nearestOnEdge = closestPointTriangle(sphereCenter, vertices[0], vertices[1], vertices[2]);
+				btScalar distanceSqr = nearestOnEdge.distance2(sphereCenter);
+				if (distanceSqr < minDistSqr) {
+					minDistSqr = distanceSqr;
+					hasContact = true;
+					contactPoint = nearestOnEdge;
+				}
+#endif
 			}
 		}
+
 	}
 
 	if (hasContact)
@@ -187,6 +244,8 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& p
 
 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p)
 {
+
+#if 0 // Bullet's method
 	const btVector3* p1 = &vertices[0];
 	const btVector3* p2 = &vertices[1];
 	const btVector3* p3 = &vertices[2];
@@ -211,4 +270,27 @@ bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const b
 		(r1 <= 0 && r2 <= 0 && r3 <= 0))
 		return true;
 	return false;
+#else // A faster method from https://gamedev.stackexchange.com/questions/28781/easy-way-to-project-point-onto-triangle-or-plane/152476#152476 (thanks VirxEC)
+
+	const btVector3&
+		p0 = vertices[0],
+		p1 = vertices[1],
+		p2 = vertices[2];
+
+	btVector3 u = p1 - p0;
+	btVector3 v = p2 - p0;
+	btVector3 n = u.cross(v);
+	float nLenSq = n.dot(n);
+
+	btVector3 w = *p - p0;
+
+	float gamma = u.cross(w).dot(n) / nLenSq;
+
+	float beta = w.cross(v).dot(n) / nLenSq;
+	float alpha = 1 - gamma - beta;
+
+	return ((0 <= alpha) && (alpha <= 1) &&
+		(0 <= beta) && (beta <= 1) &&
+		(0 <= gamma) && (gamma <= 1));
+#endif
 }

libsrc/bullet3-3.24/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -16,6 +16,8 @@ subject to the following restrictions:
 #include "btCollisionObject.h"
 #include "../BroadphaseCollision/btBroadphaseProxy.h"
 
+#include "../CollisionShapes/btCollisionShape.h"
+
 btCollisionObject::btCollisionObject()
 	: m_interpolationLinearVelocity(0.f, 0.f, 0.f),
 	  m_interpolationAngularVelocity(0.f, 0.f, 0.f),
@@ -75,4 +77,11 @@ void btCollisionObject::activate(bool forceActivation) const
 		setActivationState(ACTIVE_TAG);
 		m_deactivationTime = btScalar(0.);
 	}
+}
+
+void btCollisionObject::setWorldTransform(const btTransform& worldTrans) {
+	m_updateRevision++;
+	m_worldTransform = worldTrans;
+	if (m_collisionShape)
+		m_collisionShape->m_aabbCached = false;
 }

libsrc/bullet3-3.24/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -49,9 +49,10 @@ typedef btAlignedObjectArray<class btCollisionObject*> btCollisionObjectArray;
 ATTRIBUTE_ALIGNED16(class)
 btCollisionObject
 {
-public:
+private:
 	btTransform m_worldTransform;
 
+public:
 	///m_interpolationWorldTransform is used for CCD and interpolation
 	///it can be either previous or future (predicted) transform
 	btTransform m_interpolationWorldTransform;
@@ -400,11 +401,7 @@ btCollisionObject
 		return m_worldTransform;
 	}
 
-	void setWorldTransform(const btTransform& worldTrans)
-	{
-		m_updateRevision++;
-		m_worldTransform = worldTrans;
-	}
+	void setWorldTransform(const btTransform& worldTrans);
 
 	SIMD_FORCE_INLINE btBroadphaseProxy* getBroadphaseHandle()
 	{