From 72dd8285e81db8fabe7af0571d667e63386de977 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 30 Jan 2017 18:12:09 -0800 Subject: [PATCH] fix 'safeNormalize' (probably should remove it) Fix for Issue 953: cull degenerate triangles https://github.com/bulletphysics/bullet3/issues/935 Thanks to Oleg Klimov for the report and reproduction case. --- .../CollisionShape2TriangleMesh.cpp | 19 +++-- src/Bullet3Common/b3Vector3.h | 14 ++-- .../SphereTriangleDetector.cpp | 77 +++++++++++-------- src/LinearMath/btVector3.h | 14 ++-- 4 files changed, 70 insertions(+), 54 deletions(-) diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index 2bc8a0ea4..f53cba232 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/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) { - - btVector3 pos =parentTransform*triangleVerts[v]; - indicesOut.push_back(vertexPositions.size()); - vertexPositions.push_back(pos); - vertexNormals.push_back(triNormal); + triNormal /= btSqrt(dot); + for (int v = 0; v < 3; v++) + { + btVector3 pos = parentTransform*triangleVerts[v]; + indicesOut.push_back(vertexPositions.size()); + vertexPositions.push_back(pos); + vertexNormals.push_back(triNormal); + } } - } } diff --git a/src/Bullet3Common/b3Vector3.h b/src/Bullet3Common/b3Vector3.h index f6919934d..50504c7f7 100644 --- a/src/Bullet3Common/b3Vector3.h +++ b/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; } diff --git a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 006cc65a2..c81af9567 100644 --- a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/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; + } + } - } } } diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 487670009..8d0b73f88 100644 --- a/src/LinearMath/btVector3.h +++ b/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; }