Bullet Collision Detection & Physics Library
SphereTriangleDetector.cpp
Go to the documentation of this file.
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"
20 
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 }
btScalar SegmentSqrDistance(const btVector3 &from, const btVector3 &to, const btVector3 &p, btVector3 &nearest)
#define output
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_EPSILON
Definition: btScalar.h:543
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
btScalar getRadius() const
Definition: btSphereShape.h:48
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:222
virtual void getEdge(int i, btVector3 &pa, btVector3 &pb) const
btVector3 & getVertexPtr(int index)
virtual int getNumEdges() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
bool collide(const btVector3 &sphereCenter, btVector3 &point, btVector3 &resultNormal, btScalar &depth, btScalar &timeOfImpact, btScalar contactBreakingThreshold)
SphereTriangleDetector(btSphereShape *sphere, btTriangleShape *triangle, btScalar contactBreakingThreshold)
bool pointInTriangle(const btVector3 vertices[], const btVector3 &normal, btVector3 *p)
bool facecontains(const btVector3 &p, const btVector3 *vertices, btVector3 &normal)
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)