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
SphereTriangleDetector(btSphereShape * sphere,btTriangleShape * triangle,btScalar contactBreakingThreshold)21 SphereTriangleDetector::SphereTriangleDetector(btSphereShape* sphere, btTriangleShape* triangle, btScalar contactBreakingThreshold)
22 : m_sphere(sphere),
23 m_triangle(triangle),
24 m_contactBreakingThreshold(contactBreakingThreshold)
25 {
26 }
27
getClosestPoints(const ClosestPointInput & input,Result & output,class btIDebugDraw * debugDraw,bool swapResults)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
SegmentSqrDistance(const btVector3 & from,const btVector3 & to,const btVector3 & p,btVector3 & nearest)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
facecontains(const btVector3 & p,const btVector3 * vertices,btVector3 & normal)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
collide(const btVector3 & sphereCenter,btVector3 & point,btVector3 & resultNormal,btScalar & depth,btScalar & timeOfImpact,btScalar contactBreakingThreshold)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
pointInTriangle(const btVector3 vertices[],const btVector3 & normal,btVector3 * p)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 }
216