| /* | |
| Bullet Continuous Collision Detection and Physics Library | |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ | |
| This software is provided 'as-is', without any express or implied warranty. | |
| In no event will the authors be held liable for any damages arising from the use of this software. | |
| Permission is granted to anyone to use this software for any purpose, | |
| including commercial applications, and to alter it and redistribute it freely, | |
| subject to the following restrictions: | |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | |
| 3. This notice may not be removed or altered from any source distribution. | |
| */ | |
| #include "LinearMath/btScalar.h" | |
| #include "BulletCollision/CollisionDispatch/SphereTriangleDetector.h" | |
| #include "BulletCollision/CollisionShapes/btTriangleShape.h" | |
| #include "BulletCollision/CollisionShapes/btSphereShape.h" | |
| SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold) | |
| :m_sphere(sphere), | |
| m_triangle(triangle), | |
| m_contactBreakingThreshold(contactBreakingThreshold) | |
| { | |
| } | |
| void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults) | |
| { | |
| (void)debugDraw; | |
| const btTransform& transformA = input.m_transformA; | |
| const btTransform& transformB = input.m_transformB; | |
| btVector3 point,normal; | |
| btScalar timeOfImpact = btScalar(1.); | |
| btScalar depth = btScalar(0.); | |
| // output.m_distance = btScalar(BT_LARGE_FLOAT); | |
| //move sphere into triangle space | |
| btTransform sphereInTr = transformB.inverseTimes(transformA); | |
| if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold)) | |
| { | |
| if (swapResults) | |
| { | |
| btVector3 normalOnB = transformB.getBasis()*normal; | |
| btVector3 normalOnA = -normalOnB; | |
| btVector3 pointOnA = transformB*point+normalOnB*depth; | |
| output.addContactPoint(normalOnA,pointOnA,depth); | |
| } else | |
| { | |
| output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth); | |
| } | |
| } | |
| } | |
| #define MAX_OVERLAP btScalar(0.) | |
| // See also geometrictools.com | |
| // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv | |
| btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest); | |
| btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) { | |
| btVector3 diff = p - from; | |
| btVector3 v = to - from; | |
| btScalar t = v.dot(diff); | |
| if (t > 0) { | |
| btScalar dotVV = v.dot(v); | |
| if (t < dotVV) { | |
| t /= dotVV; | |
| diff -= t*v; | |
| } else { | |
| t = 1; | |
| diff -= v; | |
| } | |
| } else | |
| t = 0; | |
| nearest = from + t*v; | |
| return diff.dot(diff); | |
| } | |
| bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal) { | |
| btVector3 lp(p); | |
| btVector3 lnormal(normal); | |
| 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) | |
| { | |
| const btVector3* vertices = &m_triangle->getVertexPtr(0); | |
| const btVector3& c = sphereCenter; | |
| btScalar r = m_sphere->getRadius(); | |
| btVector3 delta (0,0,0); | |
| btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); | |
| normal.normalize(); | |
| btVector3 p1ToCentre = c - vertices[0]; | |
| btScalar distanceFromPlane = p1ToCentre.dot(normal); | |
| if (distanceFromPlane < btScalar(0.)) | |
| { | |
| //triangle facing the other way | |
| distanceFromPlane *= btScalar(-1.); | |
| normal *= btScalar(-1.); | |
| } | |
| btScalar contactMargin = contactBreakingThreshold; | |
| 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 | |
| bool hasContact = false; | |
| btVector3 contactPoint; | |
| if (isInsideContactPlane) { | |
| if (facecontains(c,vertices,normal)) { | |
| // Inside the contact wedge - touches a point on the shell plane | |
| hasContact = true; | |
| contactPoint = c - normal*distanceFromPlane; | |
| } else { | |
| // Could be inside one of the contact capsules | |
| btScalar contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin); | |
| 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,c, nearestOnEdge); | |
| if (distanceSqr < contactCapsuleRadiusSqr) { | |
| // Yep, we're inside a capsule | |
| hasContact = true; | |
| contactPoint = nearestOnEdge; | |
| } | |
| } | |
| } | |
| } | |
| if (hasContact) { | |
| btVector3 contactToCentre = c - contactPoint; | |
| btScalar distanceSqr = contactToCentre.length2(); | |
| if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) { | |
| btScalar distance = btSqrt(distanceSqr); | |
| resultNormal = contactToCentre; | |
| resultNormal.normalize(); | |
| point = contactPoint; | |
| depth = -(r-distance); | |
| 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; | |
| } | |
| bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p ) | |
| { | |
| const btVector3* p1 = &vertices[0]; | |
| const btVector3* p2 = &vertices[1]; | |
| const btVector3* p3 = &vertices[2]; | |
| btVector3 edge1( *p2 - *p1 ); | |
| btVector3 edge2( *p3 - *p2 ); | |
| btVector3 edge3( *p1 - *p3 ); | |
| btVector3 p1_to_p( *p - *p1 ); | |
| btVector3 p2_to_p( *p - *p2 ); | |
| btVector3 p3_to_p( *p - *p3 ); | |
| btVector3 edge1_normal( edge1.cross(normal)); | |
| btVector3 edge2_normal( edge2.cross(normal)); | |
| btVector3 edge3_normal( edge3.cross(normal)); | |
| btScalar r1, r2, r3; | |
| r1 = edge1_normal.dot( p1_to_p ); | |
| r2 = edge2_normal.dot( p2_to_p ); | |
| r3 = edge3_normal.dot( p3_to_p ); | |
| if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) || | |
| ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) ) | |
| return true; | |
| return false; | |
| } |