Merge pull request #1695 from erwincoumans/master

Fix Issue #1623, SphereTriangleDetector

setup.py

@@ -450,7 +450,7 @@ print("-----")
 
 setup(
 	name = 'pybullet',
-	version='1.9.8',
+	version='1.9.9',
 	description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',

src/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;
 					}