Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fixes in SphereTriangleDetector, needs review/test:

Browse Source 
+ remove obsolete comment and code
+ use contact breaking threshold
This commit is contained in:
erwin.coumans
2010-10-02 00:03:01 +00:00
parent 3ccb101ba8
commit a061306bd2
1 changed files with 27 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp
View File

@@ -57,8 +57,6 @@ void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Res
} }
#define MAX_OVERLAP btScalar(0.)
// See also geometrictools.com // See also geometrictools.com
@@ -93,48 +91,39 @@ bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* ve
return pointInTriangle(vertices, lnormal, &lp); return pointInTriangle(vertices, lnormal, &lp);
} }
///combined discrete/continuous sphere-triangle
bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold) bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
{ {
const btVector3* vertices = &m_triangle->getVertexPtr(0); const btVector3* vertices = &m_triangle->getVertexPtr(0);
const btVector3& c = sphereCenter;
btScalar r = m_sphere->getRadius();
btVector3 delta (0,0,0); btScalar radius = m_sphere->getRadius();
btScalar radiusWithThreshold = radius + contactBreakingThreshold;
btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
normal.normalize(); normal.normalize();
btVector3 p1ToCentre = c - vertices[0]; btVector3 p1ToCentre = sphereCenter - vertices[0];
btScalar distanceFromPlane = p1ToCentre.dot(normal); btScalar distanceFromPlane = p1ToCentre.dot(normal);
if (distanceFromPlane < btScalar(0.)) if (distanceFromPlane < btScalar(0.))
{ {
//triangle facing the other way //triangle facing the other way
distanceFromPlane *= btScalar(-1.); distanceFromPlane *= btScalar(-1.);
normal *= btScalar(-1.); normal *= btScalar(-1.);
} }
btScalar contactMargin = contactBreakingThreshold; bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
bool isInsideShellPlane = distanceFromPlane < r;
btScalar deltaDotNormal = delta.dot(normal);
if (!isInsideShellPlane && deltaDotNormal >= btScalar(0.0))
return false;
// Check for contact / intersection // Check for contact / intersection
bool hasContact = false; bool hasContact = false;
btVector3 contactPoint; btVector3 contactPoint;
if (isInsideContactPlane) { if (isInsideContactPlane) {
if (facecontains(c,vertices,normal)) { if (facecontains(sphereCenter,vertices,normal)) {
// Inside the contact wedge - touches a point on the shell plane // Inside the contact wedge - touches a point on the shell plane
hasContact = true; hasContact = true;
contactPoint = c - normal*distanceFromPlane; contactPoint = sphereCenter - normal*distanceFromPlane;
} else { } else {
// Could be inside one of the contact capsules // Could be inside one of the contact capsules
btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
btVector3 nearestOnEdge; btVector3 nearestOnEdge;
for (int i = 0; i < m_triangle->getNumEdges(); i++) { for (int i = 0; i < m_triangle->getNumEdges(); i++) {
@@ -143,7 +132,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
m_triangle->getEdge(i,pa,pb); m_triangle->getEdge(i,pa,pb);
btScalar distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge); btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
if (distanceSqr < contactCapsuleRadiusSqr) { if (distanceSqr < contactCapsuleRadiusSqr) {
// Yep, we're inside a capsule // Yep, we're inside a capsule
hasContact = true; hasContact = true;
@@ -155,24 +144,27 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po
} }
if (hasContact) { if (hasContact) {
btVector3 contactToCentre = c - contactPoint; btVector3 contactToCentre = sphereCenter - contactPoint;
btScalar distanceSqr = contactToCentre.length2(); btScalar distanceSqr = contactToCentre.length2();
if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
{
if (distanceSqr>SIMD_EPSILON)
{
btScalar distance = btSqrt(distanceSqr); btScalar distance = btSqrt(distanceSqr);
resultNormal = contactToCentre; resultNormal = contactToCentre;
resultNormal.normalize(); resultNormal.normalize();
point = contactPoint; point = contactPoint;
depth = -(r-distance); depth = -(radius-distance);
} else
{
btScalar distance = 0.f;
resultNormal = normal;
point = contactPoint;
depth = -radius;
}
return true; return true;
} }
if (delta.dot(contactToCentre) >= btScalar(0.0))
return false;
// Moving towards the contact point -> collision
point = contactPoint;
timeOfImpact = btScalar(0.0);
return true;
} }
return false; return false;