extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "SphereTriangleDetector.h"
  17 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  18 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  19
  20
  21 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle)
  22 :m_sphere(sphere),
  23 m_triangle(triangle)
  24 {
  25
  26 }
  27
  28 void    SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw)
  29 {
  30
  31         const btTransform& transformA = input.m_transformA;
  32         const btTransform& transformB = input.m_transformB;
  33
  34         btVector3 point,normal;
  35         btScalar timeOfImpact = 1.f;
  36         btScalar depth = 0.f;
  37 //      output.m_distance = 1e30f;
  38         //move sphere into triangle space
  39         btTransform     sphereInTr = transformB.inverseTimes(transformA);
  40
  41         if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact))
  42         {
  43                 output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
  44         }
  45
  46 }
  47
  48 #define MAX_OVERLAP 0.f
  49
  50
  51
  52 // See also geometrictools.com
  53 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
  54 float SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
  55         btVector3 diff = p - from;
  56         btVector3 v = to - from;
  57         float t = v.dot(diff);
  58
  59         if (t > 0) {
  60                 float dotVV = v.dot(v);
  61                 if (t < dotVV) {
  62                         t /= dotVV;
  63                         diff -= t*v;
  64                 } else {
  65                         t = 1;
  66                         diff -= v;
  67                 }
  68         } else
  69                 t = 0;
  70
  71         nearest = from + t*v;
  72         return diff.dot(diff);
  73 }
  74
  75 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
  76         btVector3 lp(p);
  77         btVector3 lnormal(normal);
  78
  79         return pointInTriangle(vertices, lnormal, &lp);
  80 }
  81
  82 ///combined discrete/continuous sphere-triangle
  83 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, float &timeOfImpact)
  84 {
  85
  86         const btVector3* vertices = &m_triangle->getVertexPtr(0);
  87         const btVector3& c = sphereCenter;
  88         btScalar r = m_sphere->getRadius();
  89
  90         btVector3 delta (0,0,0);
  91
  92         btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
  93         normal.normalize();
  94         btVector3 p1ToCentre = c - vertices[0];
  95         float distanceFromPlane = p1ToCentre.dot(normal);
  96
  97         if (distanceFromPlane < 0.f)
  98         {
  99                 //triangle facing the other way
 100
 101                 distanceFromPlane *= -1.f;
 102                 normal *= -1.f;
 103         }
 104
 105         ///todo: move this gContactBreakingThreshold into a proper structure
 106         extern float gContactBreakingThreshold;
 107
 108         float contactMargin = gContactBreakingThreshold;
 109         bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
 110         bool isInsideShellPlane = distanceFromPlane < r;
 111
 112         float deltaDotNormal = delta.dot(normal);
 113         if (!isInsideShellPlane && deltaDotNormal >= 0.0f)
 114                 return false;
 115
 116         // Check for contact / intersection
 117         bool hasContact = false;
 118         btVector3 contactPoint;
 119         if (isInsideContactPlane) {
 120                 if (facecontains(c,vertices,normal)) {
 121                         // Inside the contact wedge - touches a point on the shell plane
 122                         hasContact = true;
 123                         contactPoint = c - normal*distanceFromPlane;
 124                 } else {
 125                         // Could be inside one of the contact capsules
 126                         float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
 127                         btVector3 nearestOnEdge;
 128                         for (int i = 0; i < m_triangle->getNumEdges(); i++) {
 129
 130                                 btPoint3 pa;
 131                                 btPoint3 pb;
 132
 133                                 m_triangle->getEdge(i,pa,pb);
 134
 135                                 float distanceSqr = SegmentSqrDistance(pa,pb,c, nearestOnEdge);
 136                                 if (distanceSqr < contactCapsuleRadiusSqr) {
 137                                         // Yep, we're inside a capsule
 138                                         hasContact = true;
 139                                         contactPoint = nearestOnEdge;
 140                                 }
 141
 142                         }
 143                 }
 144         }
 145
 146         if (hasContact) {
 147                 btVector3 contactToCentre = c - contactPoint;
 148                 float distanceSqr = contactToCentre.length2();
 149                 if (distanceSqr < (r - MAX_OVERLAP)*(r - MAX_OVERLAP)) {
 150                         float distance = btSqrt(distanceSqr);
 151                         if (1)
 152                         {
 153                                 resultNormal = contactToCentre;
 154                                 resultNormal.normalize();
 155                         }
 156                         point = contactPoint;
 157                         depth = -(r-distance);
 158                         return true;
 159                 }
 160
 161                 if (delta.dot(contactToCentre) >= 0.0f)
 162                         return false;
 163
 164                 // Moving towards the contact point -> collision
 165                 point = contactPoint;
 166                 timeOfImpact = 0.0f;
 167                 return true;
 168         }
 169
 170         return false;
 171 }
 172
 173
 174 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p )
 175 {
 176         const btVector3* p1 = &vertices[0];
 177         const btVector3* p2 = &vertices[1];
 178         const btVector3* p3 = &vertices[2];
 179
 180         btVector3 edge1( *p2 - *p1 );
 181         btVector3 edge2( *p3 - *p2 );
 182         btVector3 edge3( *p1 - *p3 );
 183
 184         btVector3 p1_to_p( *p - *p1 );
 185         btVector3 p2_to_p( *p - *p2 );
 186         btVector3 p3_to_p( *p - *p3 );
 187
 188         btVector3 edge1_normal( edge1.cross(normal));
 189         btVector3 edge2_normal( edge2.cross(normal));
 190         btVector3 edge3_normal( edge3.cross(normal));
 191
 192         float r1, r2, r3;
 193         r1 = edge1_normal.dot( p1_to_p );
 194         r2 = edge2_normal.dot( p2_to_p );
 195         r3 = edge3_normal.dot( p3_to_p );
 196         if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
 197              ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
 198                 return true;
 199         return false;
 200
 201 }