Bullet Collision Detection & Physics Library
btMinkowskiPenetrationDepthSolver.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
21
22#define NUM_UNITSPHERE_POINTS 42
23
26 const btTransform& transA, const btTransform& transB,
28 class btIDebugDraw* debugDraw)
29{
30 (void)v;
31
32 bool check2d = convexA->isConvex2d() && convexB->isConvex2d();
33
35 {
36 btIntermediateResult() : m_hasResult(false)
37 {
38 }
39
40 btVector3 m_normalOnBInWorld;
41 btVector3 m_pointInWorld;
42 btScalar m_depth;
43 bool m_hasResult;
44
45 virtual void setShapeIdentifiersA(int partId0, int index0)
46 {
48 (void)index0;
49 }
50 virtual void setShapeIdentifiersB(int partId1, int index1)
51 {
53 (void)index1;
54 }
55 void addContactPoint(const btVector3& normalOnBInWorld, const btVector3& pointInWorld, btScalar depth)
56 {
57 m_normalOnBInWorld = normalOnBInWorld;
58 m_pointInWorld = pointInWorld;
59 m_depth = depth;
60 m_hasResult = true;
61 }
62 };
63
64 //just take fixed number of orientation, and sample the penetration depth in that direction
70
71#ifndef __SPU__
72#define USE_BATCHED_SUPPORT 1
73#endif
74#ifdef USE_BATCHED_SUPPORT
75
80 int i;
81
83
84 for (i = 0; i < numSampleDirections; i++)
85 {
87 separatingAxisInABatch[i] = (-norm) * transA.getBasis();
88 separatingAxisInBBatch[i] = norm * transB.getBasis();
89 }
90
91 {
92 int numPDA = convexA->getNumPreferredPenetrationDirections();
93 if (numPDA)
94 {
95 for (int i = 0; i < numPDA; i++)
96 {
97 btVector3 norm;
98 convexA->getPreferredPenetrationDirection(i, norm);
99 norm = transA.getBasis() * norm;
101 separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
104 }
105 }
106 }
107
108 {
109 int numPDB = convexB->getNumPreferredPenetrationDirections();
110 if (numPDB)
111 {
112 for (int i = 0; i < numPDB; i++)
113 {
114 btVector3 norm;
115 convexB->getPreferredPenetrationDirection(i, norm);
116 norm = transB.getBasis() * norm;
118 separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
121 }
122 }
123 }
124
125 convexA->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInABatch, supportVerticesABatch, numSampleDirections);
126 convexB->batchedUnitVectorGetSupportingVertexWithoutMargin(separatingAxisInBBatch, supportVerticesBBatch, numSampleDirections);
127
128 for (i = 0; i < numSampleDirections; i++)
129 {
131 if (check2d)
132 {
133 norm[2] = 0.f;
134 }
135 if (norm.length2() > 0.01)
136 {
139
142
143 pWorld = transA(pInA);
144 qWorld = transB(qInB);
145 if (check2d)
146 {
147 pWorld[2] = 0.f;
148 qWorld[2] = 0.f;
149 }
150
151 w = qWorld - pWorld;
152 btScalar delta = norm.dot(w);
153 //find smallest delta
154 if (delta < minProj)
155 {
156 minProj = delta;
157 minNorm = norm;
158 minA = pWorld;
159 minB = qWorld;
160 }
161 }
162 }
163#else
164
166
167#ifndef __SPU__
168 {
169 int numPDA = convexA->getNumPreferredPenetrationDirections();
170 if (numPDA)
171 {
172 for (int i = 0; i < numPDA; i++)
173 {
174 btVector3 norm;
175 convexA->getPreferredPenetrationDirection(i, norm);
176 norm = transA.getBasis() * norm;
179 }
180 }
181 }
182
183 {
184 int numPDB = convexB->getNumPreferredPenetrationDirections();
185 if (numPDB)
186 {
187 for (int i = 0; i < numPDB; i++)
188 {
189 btVector3 norm;
190 convexB->getPreferredPenetrationDirection(i, norm);
191 norm = transB.getBasis() * norm;
194 }
195 }
196 }
197#endif // __SPU__
198
199 for (int i = 0; i < numSampleDirections; i++)
200 {
201 const btVector3& norm = getPenetrationDirections()[i];
202 separatingAxisInA = (-norm) * transA.getBasis();
203 separatingAxisInB = norm * transB.getBasis();
204 pInA = convexA->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInA);
205 qInB = convexB->localGetSupportVertexWithoutMarginNonVirtual(separatingAxisInB);
206 pWorld = transA(pInA);
207 qWorld = transB(qInB);
208 w = qWorld - pWorld;
209 btScalar delta = norm.dot(w);
210 //find smallest delta
211 if (delta < minProj)
212 {
213 minProj = delta;
214 minNorm = norm;
215 minA = pWorld;
216 minB = qWorld;
217 }
218 }
219#endif //USE_BATCHED_SUPPORT
220
221 //add the margins
222
223 minA += minNorm * convexA->getMarginNonVirtual();
224 minB -= minNorm * convexB->getMarginNonVirtual();
225 //no penetration
226 if (minProj < btScalar(0.))
227 return false;
228
230 minProj += extraSeparation + (convexA->getMarginNonVirtual() + convexB->getMarginNonVirtual());
231
232//#define DEBUG_DRAW 1
233#ifdef DEBUG_DRAW
234 if (debugDraw)
235 {
236 btVector3 color(0, 1, 0);
237 debugDraw->drawLine(minA, minB, color);
238 color = btVector3(1, 1, 1);
241 debugDraw->drawLine(minA, minA + (minNorm * minProj), color);
242 }
243#endif //DEBUG_DRAW
244
246
249
251
252 btVector3 newOrg = transA.getOrigin() + offset;
253
256
257 input.m_transformA = displacedTrans;
258 input.m_transformB = transB;
259 input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj;
260
262 gjkdet.setCachedSeparatingAxis(-minNorm);
263 gjkdet.getClosestPoints(input, res, debugDraw);
264
266
267 //the penetration depth is over-estimated, relax it
270
271 if (res.m_hasResult)
272 {
273 pa = res.m_pointInWorld - minNorm * correctedMinNorm;
274 pb = res.m_pointInWorld;
275 v = minNorm;
276
277#ifdef DEBUG_DRAW
278 if (debugDraw)
279 {
280 btVector3 color(1, 0, 0);
281 debugDraw->drawLine(pa, pb, color);
282 }
283#endif //DEBUG_DRAW
284 }
285 return res.m_hasResult;
286}
287
289{
291 {
292 btVector3(btScalar(0.000000), btScalar(-0.000000), btScalar(-1.000000)),
293 btVector3(btScalar(0.723608), btScalar(-0.525725), btScalar(-0.447219)),
294 btVector3(btScalar(-0.276388), btScalar(-0.850649), btScalar(-0.447219)),
295 btVector3(btScalar(-0.894426), btScalar(-0.000000), btScalar(-0.447216)),
296 btVector3(btScalar(-0.276388), btScalar(0.850649), btScalar(-0.447220)),
297 btVector3(btScalar(0.723608), btScalar(0.525725), btScalar(-0.447219)),
298 btVector3(btScalar(0.276388), btScalar(-0.850649), btScalar(0.447220)),
299 btVector3(btScalar(-0.723608), btScalar(-0.525725), btScalar(0.447219)),
300 btVector3(btScalar(-0.723608), btScalar(0.525725), btScalar(0.447219)),
301 btVector3(btScalar(0.276388), btScalar(0.850649), btScalar(0.447219)),
302 btVector3(btScalar(0.894426), btScalar(0.000000), btScalar(0.447216)),
303 btVector3(btScalar(-0.000000), btScalar(0.000000), btScalar(1.000000)),
304 btVector3(btScalar(0.425323), btScalar(-0.309011), btScalar(-0.850654)),
305 btVector3(btScalar(-0.162456), btScalar(-0.499995), btScalar(-0.850654)),
306 btVector3(btScalar(0.262869), btScalar(-0.809012), btScalar(-0.525738)),
307 btVector3(btScalar(0.425323), btScalar(0.309011), btScalar(-0.850654)),
308 btVector3(btScalar(0.850648), btScalar(-0.000000), btScalar(-0.525736)),
309 btVector3(btScalar(-0.525730), btScalar(-0.000000), btScalar(-0.850652)),
310 btVector3(btScalar(-0.688190), btScalar(-0.499997), btScalar(-0.525736)),
311 btVector3(btScalar(-0.162456), btScalar(0.499995), btScalar(-0.850654)),
312 btVector3(btScalar(-0.688190), btScalar(0.499997), btScalar(-0.525736)),
313 btVector3(btScalar(0.262869), btScalar(0.809012), btScalar(-0.525738)),
314 btVector3(btScalar(0.951058), btScalar(0.309013), btScalar(0.000000)),
315 btVector3(btScalar(0.951058), btScalar(-0.309013), btScalar(0.000000)),
316 btVector3(btScalar(0.587786), btScalar(-0.809017), btScalar(0.000000)),
317 btVector3(btScalar(0.000000), btScalar(-1.000000), btScalar(0.000000)),
318 btVector3(btScalar(-0.587786), btScalar(-0.809017), btScalar(0.000000)),
319 btVector3(btScalar(-0.951058), btScalar(-0.309013), btScalar(-0.000000)),
320 btVector3(btScalar(-0.951058), btScalar(0.309013), btScalar(-0.000000)),
321 btVector3(btScalar(-0.587786), btScalar(0.809017), btScalar(-0.000000)),
322 btVector3(btScalar(-0.000000), btScalar(1.000000), btScalar(-0.000000)),
323 btVector3(btScalar(0.587786), btScalar(0.809017), btScalar(-0.000000)),
324 btVector3(btScalar(0.688190), btScalar(-0.499997), btScalar(0.525736)),
325 btVector3(btScalar(-0.262869), btScalar(-0.809012), btScalar(0.525738)),
326 btVector3(btScalar(-0.850648), btScalar(0.000000), btScalar(0.525736)),
327 btVector3(btScalar(-0.262869), btScalar(0.809012), btScalar(0.525738)),
328 btVector3(btScalar(0.688190), btScalar(0.499997), btScalar(0.525736)),
329 btVector3(btScalar(0.525730), btScalar(0.000000), btScalar(0.850652)),
330 btVector3(btScalar(0.162456), btScalar(-0.499995), btScalar(0.850654)),
331 btVector3(btScalar(-0.425323), btScalar(-0.309011), btScalar(0.850654)),
332 btVector3(btScalar(-0.425323), btScalar(0.309011), btScalar(0.850654)),
333 btVector3(btScalar(0.162456), btScalar(0.499995), btScalar(0.850654))};
334
336}
#define MAX_PREFERRED_PENETRATION_DIRECTIONS
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:27
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
#define BT_LARGE_FLOAT
Definition btScalar.h:316
#define NUM_UNITSPHERE_POINTS
#define btSimplexSolverInterface
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual bool calcPenDepth(btSimplexSolverInterface &simplexSolver, const btConvexShape *convexA, const btConvexShape *convexB, const btTransform &transA, const btTransform &transB, btVector3 &v, btVector3 &pa, btVector3 &pb, class btIDebugDraw *debugDraw)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:30
void setOrigin(const btVector3 &origin)
Set the translational element.
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:82
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