SphereTriangleDetector.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "LinearMath/btScalar.h"
00017 #include "SphereTriangleDetector.h"
00018 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00019 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00020 
00021 
00022 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
00023 :m_sphere(sphere),
00024 m_triangle(triangle),
00025 m_contactBreakingThreshold(contactBreakingThreshold)
00026 {
00027 
00028 }
00029 
00030 void    SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
00031 {
00032 
00033         (void)debugDraw;
00034         const btTransform& transformA = input.m_transformA;
00035         const btTransform& transformB = input.m_transformB;
00036 
00037         btVector3 point,normal;
00038         btScalar timeOfImpact = btScalar(1.);
00039         btScalar depth = btScalar(0.);
00040 //      output.m_distance = btScalar(BT_LARGE_FLOAT);
00041         //move sphere into triangle space
00042         btTransform     sphereInTr = transformB.inverseTimes(transformA);
00043 
00044         if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
00045         {
00046                 if (swapResults)
00047                 {
00048                         btVector3 normalOnB = transformB.getBasis()*normal;
00049                         btVector3 normalOnA = -normalOnB;
00050                         btVector3 pointOnA = transformB*point+normalOnB*depth;
00051                         output.addContactPoint(normalOnA,pointOnA,depth);
00052                 } else
00053                 {
00054                         output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
00055                 }
00056         }
00057 
00058 }
00059 
00060 
00061 
00062 // See also geometrictools.com
00063 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
00064 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
00065 
00066 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
00067         btVector3 diff = p - from;
00068         btVector3 v = to - from;
00069         btScalar t = v.dot(diff);
00070         
00071         if (t > 0) {
00072                 btScalar dotVV = v.dot(v);
00073                 if (t < dotVV) {
00074                         t /= dotVV;
00075                         diff -= t*v;
00076                 } else {
00077                         t = 1;
00078                         diff -= v;
00079                 }
00080         } else
00081                 t = 0;
00082 
00083         nearest = from + t*v;
00084         return diff.dot(diff);  
00085 }
00086 
00087 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
00088         btVector3 lp(p);
00089         btVector3 lnormal(normal);
00090         
00091         return pointInTriangle(vertices, lnormal, &lp);
00092 }
00093 
00094 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
00095 {
00096 
00097         const btVector3* vertices = &m_triangle->getVertexPtr(0);
00098         
00099         btScalar radius = m_sphere->getRadius();
00100         btScalar radiusWithThreshold = radius + contactBreakingThreshold;
00101 
00102         btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
00103         normal.normalize();
00104         btVector3 p1ToCentre = sphereCenter - vertices[0];
00105         btScalar distanceFromPlane = p1ToCentre.dot(normal);
00106 
00107         if (distanceFromPlane < btScalar(0.))
00108         {
00109                 //triangle facing the other way
00110                 distanceFromPlane *= btScalar(-1.);
00111                 normal *= btScalar(-1.);
00112         }
00113 
00114         bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
00115         
00116         // Check for contact / intersection
00117         bool hasContact = false;
00118         btVector3 contactPoint;
00119         if (isInsideContactPlane) {
00120                 if (facecontains(sphereCenter,vertices,normal)) {
00121                         // Inside the contact wedge - touches a point on the shell plane
00122                         hasContact = true;
00123                         contactPoint = sphereCenter - normal*distanceFromPlane;
00124                 } else {
00125                         // Could be inside one of the contact capsules
00126                         btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
00127                         btVector3 nearestOnEdge;
00128                         for (int i = 0; i < m_triangle->getNumEdges(); i++) {
00129                                 
00130                                 btVector3 pa;
00131                                 btVector3 pb;
00132                                 
00133                                 m_triangle->getEdge(i,pa,pb);
00134 
00135                                 btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, nearestOnEdge);
00136                                 if (distanceSqr < contactCapsuleRadiusSqr) {
00137                                         // Yep, we're inside a capsule
00138                                         hasContact = true;
00139                                         contactPoint = nearestOnEdge;
00140                                 }
00141                                 
00142                         }
00143                 }
00144         }
00145 
00146         if (hasContact) {
00147                 btVector3 contactToCentre = sphereCenter - contactPoint;
00148                 btScalar distanceSqr = contactToCentre.length2();
00149 
00150                 if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
00151                 {
00152                         if (distanceSqr>SIMD_EPSILON)
00153                         {
00154                                 btScalar distance = btSqrt(distanceSqr);
00155                                 resultNormal = contactToCentre;
00156                                 resultNormal.normalize();
00157                                 point = contactPoint;
00158                                 depth = -(radius-distance);
00159                         } else
00160                         {
00161                                 resultNormal = normal;
00162                                 point = contactPoint;
00163                                 depth = -radius;
00164                         }
00165                         return true;
00166                 }
00167         }
00168         
00169         return false;
00170 }
00171 
00172 
00173 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
00174 {
00175         const btVector3* p1 = &vertices[0];
00176         const btVector3* p2 = &vertices[1];
00177         const btVector3* p3 = &vertices[2];
00178 
00179         btVector3 edge1( *p2 - *p1 );
00180         btVector3 edge2( *p3 - *p2 );
00181         btVector3 edge3( *p1 - *p3 );
00182 
00183         btVector3 p1_to_p( *p - *p1 );
00184         btVector3 p2_to_p( *p - *p2 );
00185         btVector3 p3_to_p( *p - *p3 );
00186 
00187         btVector3 edge1_normal( edge1.cross(normal));
00188         btVector3 edge2_normal( edge2.cross(normal));
00189         btVector3 edge3_normal( edge3.cross(normal));
00190         
00191         btScalar r1, r2, r3;
00192         r1 = edge1_normal.dot( p1_to_p );
00193         r2 = edge2_normal.dot( p2_to_p );
00194         r3 = edge3_normal.dot( p3_to_p );
00195         if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
00196              ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
00197                 return true;
00198         return false;
00199 
00200 }