Merge pull request #940 from erwincoumans/master

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

examples/Benchmarks/BenchmarkDemo.cpp

@@ -320,7 +320,7 @@ public:
 				indices.push_back(indices.size());
 			}
 
-			m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0],lineColor,points.size(),sizeof(btVector3),&indices[0],indices.size(),1);
+			m_guiHelper->getRenderInterface()->drawLines(&points[0].m_floats[0],lineColor,points.size(),sizeof(btVector3FloatData),&indices[0],indices.size(),1);
 		}
 													 
 #if 0

examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp

@@ -111,18 +111,21 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans
 					vertices.push_back(triangleVerts[2]);
 
 					btVector3 triNormal = (triangleVerts[1]-triangleVerts[0]).cross(triangleVerts[2]-triangleVerts[0]);
-					triNormal.normalize();
+					btScalar dot = triNormal.dot(triNormal);
 
-					for (int v=0;v<3;v++)
+					//cull degenerate triangles
+					if (dot >= SIMD_EPSILON*SIMD_EPSILON)
+					{
+						triNormal /= btSqrt(dot);
+						for (int v = 0; v < 3; v++)
 						{
 
-						btVector3 pos =parentTransform*triangleVerts[v];
+							btVector3 pos = parentTransform*triangleVerts[v];
 							indicesOut.push_back(vertexPositions.size());
 							vertexPositions.push_back(pos);
 							vertexNormals.push_back(triNormal);
-
 						}
-
+					}
 					
 				}
 			}

src/Bullet3Common/b3Vector3.h

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

src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

@@ -100,7 +100,15 @@ 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();
+
+	btScalar l2 = normal.length2();
+	bool hasContact = false;
+	btVector3 contactPoint;
+
+	if (l2 >= SIMD_EPSILON*SIMD_EPSILON)
+	{
+		normal /= btSqrt(l2);
+
 		btVector3 p1ToCentre = sphereCenter - vertices[0];
 		btScalar distanceFromPlane = p1ToCentre.dot(normal);
 
@@ -114,14 +122,14 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 		bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
 	
 		// Check for contact / intersection
-	bool hasContact = false;
+	
-	btVector3 contactPoint;
 		if (isInsideContactPlane) {
-		if (facecontains(sphereCenter,vertices,normal)) {
+			if (facecontains(sphereCenter, vertices, normal)) {
 				// Inside the contact wedge - touches a point on the shell plane
 				hasContact = true;
 				contactPoint = sphereCenter - normal*distanceFromPlane;
-		} else {
+			}
+			else {
 				// Could be inside one of the contact capsules
 				btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
 				btVector3 nearestOnEdge;
@@ -130,9 +138,9 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 					btVector3 pa;
 					btVector3 pb;
 
-				m_triangle->getEdge(i,pa,pb);
+					m_triangle->getEdge(i, pa, pb);
 
-				btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
+					btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
 					if (distanceSqr < contactCapsuleRadiusSqr) {
 						// Yep, we're inside a capsule
 						hasContact = true;
@@ -142,6 +150,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
 				}
 			}
 		}
+	}
 
 	if (hasContact) {
 		btVector3 contactToCentre = sphereCenter - contactPoint;

src/LinearMath/btVector3.h

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