dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold)
  22         : m_sphere(sphere),
  23           m_triangle(triangle),
  24           m_contactBreakingThreshold(contactBreakingThreshold)
  25 {
  26 }
  27
  28 void SphereTriangleDetector::getClosestPoints(const ClosestPointInput& input, Result& output, class btIDebugDraw* debugDraw, bool swapResults)
  29 {
  30         (void)debugDraw;
  31         const btTransform& transformA = input.m_transformA;
  32         const btTransform& transformB = input.m_transformB;
  33
  34         btVector3 point, normal;
  35         btScalar timeOfImpact = btScalar(1.);
  36         btScalar depth = btScalar(0.);
  37         //      output.m_distance = btScalar(BT_LARGE_FLOAT);
  38         //move sphere into triangle space
  39         btTransform sphereInTr = transformB.inverseTimes(transformA);
  40
  41         if (collide(sphereInTr.getOrigin(), point, normal, depth, timeOfImpact, m_contactBreakingThreshold))
  42         {
  43                 if (swapResults)
  44                 {
  45                         btVector3 normalOnB = transformB.getBasis() * normal;
  46                         btVector3 normalOnA = -normalOnB;
  47                         btVector3 pointOnA = transformB * point + normalOnB * depth;
  48                         output.addContactPoint(normalOnA, pointOnA, depth);
  49                 }
  50                 else
  51                 {
  52                         output.addContactPoint(transformB.getBasis() * normal, transformB * point, depth);
  53                 }
  54         }
  55 }
  56
  57 // See also geometrictools.com
  58 // Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
  59 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest);
  60
  61 btScalar SegmentSqrDistance(const btVector3& from, const btVector3& to, const btVector3& p, btVector3& nearest)
  62 {
  63         btVector3 diff = p - from;
  64         btVector3 v = to - from;
  65         btScalar t = v.dot(diff);
  66
  67         if (t > 0)
  68         {
  69                 btScalar dotVV = v.dot(v);
  70                 if (t < dotVV)
  71                 {
  72                         t /= dotVV;
  73                         diff -= t * v;
  74                 }
  75                 else
  76                 {
  77                         t = 1;
  78                         diff -= v;
  79                 }
  80         }
  81         else
  82                 t = 0;
  83
  84         nearest = from + t * v;
  85         return diff.dot(diff);
  86 }
  87
  88 bool SphereTriangleDetector::facecontains(const btVector3& p, const btVector3* vertices, btVector3& normal)
  89 {
  90         btVector3 lp(p);
  91         btVector3 lnormal(normal);
  92
  93         return pointInTriangle(vertices, lnormal, &lp);
  94 }
  95
  96 bool SphereTriangleDetector::collide(const btVector3& sphereCenter, btVector3& point, btVector3& resultNormal, btScalar& depth, btScalar& timeOfImpact, btScalar contactBreakingThreshold)
  97 {
  98         const btVector3* vertices = &m_triangle->getVertexPtr(0);
  99
 100         btScalar radius = m_sphere->getRadius();
 101         btScalar radiusWithThreshold = radius + contactBreakingThreshold;
 102
 103         btVector3 normal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
 104
 105         btScalar l2 = normal.length2();
 106         bool hasContact = false;
 107         btVector3 contactPoint;
 108
 109         if (l2 >= SIMD_EPSILON * SIMD_EPSILON)
 110         {
 111                 normal /= btSqrt(l2);
 112
 113                 btVector3 p1ToCentre = sphereCenter - vertices[0];
 114                 btScalar distanceFromPlane = p1ToCentre.dot(normal);
 115
 116                 if (distanceFromPlane < btScalar(0.))
 117                 {
 118                         //triangle facing the other way
 119                         distanceFromPlane *= btScalar(-1.);
 120                         normal *= btScalar(-1.);
 121                 }
 122
 123                 bool isInsideContactPlane = distanceFromPlane < radiusWithThreshold;
 124
 125                 // Check for contact / intersection
 126
 127                 if (isInsideContactPlane)
 128                 {
 129                         if (facecontains(sphereCenter, vertices, normal))
 130                         {
 131                                 // Inside the contact wedge - touches a point on the shell plane
 132                                 hasContact = true;
 133                                 contactPoint = sphereCenter - normal * distanceFromPlane;
 134                         }
 135                         else
 136                         {
 137                                 // Could be inside one of the contact capsules
 138                                 btScalar contactCapsuleRadiusSqr = radiusWithThreshold * radiusWithThreshold;
 139                                 btScalar minDistSqr = contactCapsuleRadiusSqr;
 140                                 btVector3 nearestOnEdge;
 141                                 for (int i = 0; i < m_triangle->getNumEdges(); i++)
 142                                 {
 143                                         btVector3 pa;
 144                                         btVector3 pb;
 145
 146                                         m_triangle->getEdge(i, pa, pb);
 147
 148                                         btScalar distanceSqr = SegmentSqrDistance(pa, pb, sphereCenter, nearestOnEdge);
 149                                         if (distanceSqr < minDistSqr)
 150                                         {
 151                                                 // Yep, we're inside a capsule, and record the capsule with smallest distance
 152                                                 minDistSqr = distanceSqr;
 153                                                 hasContact = true;
 154                                                 contactPoint = nearestOnEdge;
 155                                         }
 156                                 }
 157                         }
 158                 }
 159         }
 160
 161         if (hasContact)
 162         {
 163                 btVector3 contactToCentre = sphereCenter - contactPoint;
 164                 btScalar distanceSqr = contactToCentre.length2();
 165
 166                 if (distanceSqr < radiusWithThreshold * radiusWithThreshold)
 167                 {
 168                         if (distanceSqr > SIMD_EPSILON)
 169                         {
 170                                 btScalar distance = btSqrt(distanceSqr);
 171                                 resultNormal = contactToCentre;
 172                                 resultNormal.normalize();
 173                                 point = contactPoint;
 174                                 depth = -(radius - distance);
 175                         }
 176                         else
 177                         {
 178                                 resultNormal = normal;
 179                                 point = contactPoint;
 180                                 depth = -radius;
 181                         }
 182                         return true;
 183                 }
 184         }
 185
 186         return false;
 187 }
 188
 189 bool SphereTriangleDetector::pointInTriangle(const btVector3 vertices[], const btVector3& normal, btVector3* p)
 190 {
 191         const btVector3* p1 = &vertices[0];
 192         const btVector3* p2 = &vertices[1];
 193         const btVector3* p3 = &vertices[2];
 194
 195         btVector3 edge1(*p2 - *p1);
 196         btVector3 edge2(*p3 - *p2);
 197         btVector3 edge3(*p1 - *p3);
 198
 199         btVector3 p1_to_p(*p - *p1);
 200         btVector3 p2_to_p(*p - *p2);
 201         btVector3 p3_to_p(*p - *p3);
 202
 203         btVector3 edge1_normal(edge1.cross(normal));
 204         btVector3 edge2_normal(edge2.cross(normal));
 205         btVector3 edge3_normal(edge3.cross(normal));
 206
 207         btScalar r1, r2, r3;
 208         r1 = edge1_normal.dot(p1_to_p);
 209         r2 = edge2_normal.dot(p2_to_p);
 210         r3 = edge3_normal.dot(p3_to_p);
 211         if ((r1 > 0 && r2 > 0 && r3 > 0) ||
 212                 (r1 <= 0 && r2 <= 0 && r3 <= 0))
 213                 return true;
 214         return false;
 215 }