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 "LinearMath/btScalar.h"
  17 #include "SphereTriangleDetector.h"
  18 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  19 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  20
  21
  22 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere,btTriangleShape* triangle,btScalar contactBreakingThreshold)
  23 :m_sphere(sphere),
  24 m_triangle(triangle),
  25 m_contactBreakingThreshold(contactBreakingThreshold)
  26 {
  27
  28 }
  29
  30 void    SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input,Result& output,class btIDebugDraw* debugDraw,bool swapResults)
  31 {
  32
  33         (void)debugDraw;
  34         const btTransform& transformA = input.m_transformA;
  35         const btTransform& transformB = input.m_transformB;
  36
  37         btVector3 point,normal;
  38         btScalar timeOfImpact = btScalar(1.);
  39         btScalar depth = btScalar(0.);
  40 //      output.m_distance = btScalar(BT_LARGE_FLOAT);
  41         //move sphere into triangle space
  42         btTransform     sphereInTr = transformB.inverseTimes(transformA);
  43
  44         if (collide(sphereInTr.getOrigin(),point,normal,depth,timeOfImpact,m_contactBreakingThreshold))
  45         {
  46                 if (swapResults)
  47                 {
  48                         btVector3 normalOnB = transformB.getBasis()*normal;
  49                         btVector3 normalOnA = -normalOnB;
  50                         btVector3 pointOnA = transformB*point+normalOnB*depth;
  51                         output.addContactPoint(normalOnA,pointOnA,depth);
  52                 } else
  53                 {
  54                         output.addContactPoint(transformB.getBasis()*normal,transformB*point,depth);
  55                 }
  56         }
  57
  58 }
  59
  60
  61
  62 // See also geometrictools.com
  63 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
  64 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest);
  65
  66 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to,const btVector3 &p, btVector3 &nearest) {
  67         btVector3 diff = p - from;
  68         btVector3 v = to - from;
  69         btScalar t = v.dot(diff);
  70
  71         if (t > 0) {
  72                 btScalar dotVV = v.dot(v);
  73                 if (t < dotVV) {
  74                         t /= dotVV;
  75                         diff -= t*v;
  76                 } else {
  77                         t = 1;
  78                         diff -= v;
  79                 }
  80         } else
  81                 t = 0;
  82
  83         nearest = from + t*v;
  84         return diff.dot(diff);
  85 }
  86
  87 bool SphereTriangleDetector::facecontains(const btVector3 &p,const btVector3* vertices,btVector3& normal)  {
  88         btVector3 lp(p);
  89         btVector3 lnormal(normal);
  90
  91         return pointInTriangle(vertices, lnormal, &lp);
  92 }
  93
  94 bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &point, btVector3& resultNormal, btScalar& depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
  95 {
  96
  97         const btVector3* vertices = &m_triangle->getVertexPtr(0);
  98
  99         btScalar radius = m_sphere->getRadius();
 100         btScalar radiusWithThreshold = radius + contactBreakingThreshold;
 101
 102         btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]);
 103         normal.normalize();
 104         btVector3 p1ToCentre = sphereCenter - vertices[0];
 105         btScalar distanceFromPlane = p1ToCentre.dot(normal);
 106
 107         if (distanceFromPlane < btScalar(0.))
 108         {
 109                 //triangle facing the other way
 110                 distanceFromPlane *= btScalar(-1.);
 111                 normal *= btScalar(-1.);
 112         }
 113
 114         bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
 115
 116         // Check for contact / intersection
 117         bool hasContact = false;
 118         btVector3 contactPoint;
 119         if (isInsideContactPlane) {
 120                 if (facecontains(sphereCenter,vertices,normal)) {
 121                         // Inside the contact wedge - touches a point on the shell plane
 122                         hasContact = true;
 123                         contactPoint = sphereCenter - normal*distanceFromPlane;
 124                 } else {
 125                         // Could be inside one of the contact capsules
 126                         btScalar contactCapsuleRadiusSqr = radiusWithThreshold*radiusWithThreshold;
 127                         btVector3 nearestOnEdge;
 128                         for (int i = 0; i < m_triangle->getNumEdges(); i++) {
 129
 130                                 btVector3 pa;
 131                                 btVector3 pb;
 132
 133                                 m_triangle->getEdge(i,pa,pb);
 134
 135                                 btScalar distanceSqr = SegmentSqrDistance(pa,pb,sphereCenter, 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 = sphereCenter - contactPoint;
 148                 btScalar distanceSqr = contactToCentre.length2();
 149
 150                 if (distanceSqr < radiusWithThreshold*radiusWithThreshold)
 151                 {
 152                         if (distanceSqr>SIMD_EPSILON)
 153                         {
 154                                 btScalar distance = btSqrt(distanceSqr);
 155                                 resultNormal = contactToCentre;
 156                                 resultNormal.normalize();
 157                                 point = contactPoint;
 158                                 depth = -(radius-distance);
 159                         } else
 160                         {
 161                                 btScalar distance = 0.f;
 162                                 resultNormal = normal;
 163                                 point = contactPoint;
 164                                 depth = -radius;
 165                         }
 166                         return true;
 167                 }
 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         btScalar 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 }