Merge pull request #940 from erwincoumans/master

fix sphere-triangle for degenerate triangles (zero area/normal), fix 'safeNormalize' (probably should remove it)

src/Bullet3Common/b3Vector3.h

@@ -249,14 +249,16 @@ public:
 
 	B3_FORCE_INLINE b3Vector3& safeNormalize()
 	{
-		b3Vector3 absVec = this->absolute();
-		int maxIndex = absVec.maxAxis();
-		if (absVec[maxIndex]>0)
+		b3Scalar l2 = length2();
+		//triNormal.normalize();
+		if (l2 >= B3_EPSILON*B3_EPSILON)
 		{
-			*this /= absVec[maxIndex];
-			return *this /= length();
+			(*this) /= b3Sqrt(l2);
+		}
+		else
+		{
+			setValue(1, 0, 0);
 		}
-		setValue(1,0,0);
 		return *this;
 	}
 

src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -100,45 +100,54 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 	btScalar radiusWithThreshold = radius + contactBreakingThreshold;
 
 	btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
-	normal.safeNormalize();
-	btVector3 p1ToCentre = sphereCenter - vertices[0];
-	btScalar distanceFromPlane = p1ToCentre.dot(normal);
 
-	if (distanceFromPlane < btScalar(0.))
-	{
-		//triangle facing the other way
-		distanceFromPlane *= btScalar(-1.);
-		normal *= btScalar(-1.);
-	}
-
-	bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
-	
-	// Check for contact / intersection
+	btScalar l2 = normal.length2();
 	bool hasContact = false;
 	btVector3 contactPoint;
-	if (isInsideContactPlane) {
-		if (facecontains(sphereCenter,vertices,normal)) {
-			// Inside the contact wedge - touches a point on the shell plane
-			hasContact = true;
-			contactPoint = sphereCenter - normal*distanceFromPlane;
-		} else {
-			// Could be inside one of the contact capsules
-			btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
-			btVector3 nearestOnEdge;
-			for (int i = 0; i < m_triangle->getNumEdges(); i++) {
-				
-				btVector3 pa;
-				btVector3 pb;
-				
-				m_triangle->getEdge(i,pa,pb);
 
-				btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
-				if (distanceSqr < contactCapsuleRadiusSqr) {
-					// Yep, we're inside a capsule
-					hasContact = true;
-					contactPoint = nearestOnEdge;
+	if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
+	{
+		normal /= btSqrt(l2);
+
+		btVector3 p1ToCentre = sphereCenter - vertices[0];
+		btScalar distanceFromPlane = p1ToCentre.dot(normal);
+
+		if (distanceFromPlane < btScalar(0.))
+		{
+			//triangle facing the other way
+			distanceFromPlane *= btScalar(-1.);
+			normal *= btScalar(-1.);
+		}
+
+		bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
+	
+		// Check for contact / intersection
+	
+		if (isInsideContactPlane) {
+			if (facecontains(sphereCenter, vertices, normal)) {
+				// Inside the contact wedge - touches a point on the shell plane
+				hasContact = true;
+				contactPoint = sphereCenter - normal*distanceFromPlane;
+			}
+			else {
+				// Could be inside one of the contact capsules
+				btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
+				btVector3 nearestOnEdge;
+				for (int i = 0; i < m_triangle->getNumEdges(); i++) {
+
+					btVector3 pa;
+					btVector3 pb;
+
+					m_triangle->getEdge(i, pa, pb);
+
+					btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
+					if (distanceSqr < contactCapsuleRadiusSqr) {
+						// Yep, we're inside a capsule
+						hasContact = true;
+						contactPoint = nearestOnEdge;
+					}
+
 				}
-				
 			}
 		}
 	}

src/LinearMath/btVector3.h

@@ -291,14 +291,16 @@ public:
 
 	SIMD_FORCE_INLINE btVector3& safeNormalize() 
 	{
-		btVector3 absVec = this->absolute();
-		int maxIndex = absVec.maxAxis();
-		if (absVec[maxIndex]>0)
+		btScalar l2 = length2();
+		//triNormal.normalize();
+		if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
 		{
-			*this /= absVec[maxIndex];
-			return *this /= length();
+			(*this) /= btSqrt(l2);
+		}
+		else
+		{
+			setValue(1, 0, 0);
 		}
-		setValue(1,0,0);
 		return *this;
 	}