Bullet Collision Detection & Physics Library: btMinkowskiPenetrationDepthSolver.cpp Source File

Bullet Collision Detection & Physics Library

btMinkowskiPenetrationDepthSolver.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 "btMinkowskiPenetrationDepthSolver.h"
17 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
18 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
19 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
20 #include "BulletCollision/CollisionShapes/btConvexShape.h"
21
22 #define NUM_UNITSPHERE_POINTS 42
23
24 bool btMinkowskiPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simplexSolver,
25 const btConvexShape* convexA, const btConvexShape* convexB,
26 const btTransform& transA, const btTransform& transB,
27 btVector3& v, btVector3& pa, btVector3& pb,
28 class btIDebugDraw* debugDraw)
29 {
30 (void)v;
31
32 bool check2d = convexA->isConvex2d() && convexB->isConvex2d();
33
34 struct btIntermediateResult : public btDiscreteCollisionDetectorInterface::Result
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 {
47 (void)partId0;
48 (void)index0;
49 }
50 virtual void setShapeIdentifiersB(int partId1, int index1)
51 {
52 (void)partId1;
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
65 btScalar minProj = btScalar(BT_LARGE_FLOAT);
66 btVector3 minNorm(btScalar(0.), btScalar(0.), btScalar(0.));
67 btVector3 minA, minB;
68 btVector3 separatingAxisInA, separatingAxisInB;
69 btVector3 pInA, qInB, pWorld, qWorld, w;
70
71 #ifndef __SPU__
72 #define USE_BATCHED_SUPPORT 1
73 #endif
74 #ifdef USE_BATCHED_SUPPORT
75
76 btVector3 supportVerticesABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
77 btVector3 supportVerticesBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
78 btVector3 separatingAxisInABatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
79 btVector3 separatingAxisInBBatch[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2];
80 int i;
81
82 int numSampleDirections = NUM_UNITSPHERE_POINTS;
83
84 for (i = 0; i < numSampleDirections; i++)
85 {
86 btVector3 norm = getPenetrationDirections()[i];
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;
100 getPenetrationDirections()[numSampleDirections] = norm;
101 separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
102 separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
103 numSampleDirections++;
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;
117 getPenetrationDirections()[numSampleDirections] = norm;
118 separatingAxisInABatch[numSampleDirections] = (-norm) * transA.getBasis();
119 separatingAxisInBBatch[numSampleDirections] = norm * transB.getBasis();
120 numSampleDirections++;
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 {
130 btVector3 norm = getPenetrationDirections()[i];
131 if (check2d)
132 {
133 norm[2] = 0.f;
134 }
135 if (norm.length2() > 0.01)
136 {
137 separatingAxisInA = separatingAxisInABatch[i];
138 separatingAxisInB = separatingAxisInBBatch[i];
139
140 pInA = supportVerticesABatch[i];
141 qInB = supportVerticesBBatch[i];
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
165 int numSampleDirections = NUM_UNITSPHERE_POINTS;
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;
177 getPenetrationDirections()[numSampleDirections] = norm;
178 numSampleDirections++;
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;
192 getPenetrationDirections()[numSampleDirections] = norm;
193 numSampleDirections++;
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
229 btScalar extraSeparation = 0.5f;
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);
239 btVector3 vec = minB - minA;
240 btScalar prj2 = minNorm.dot(vec);
241 debugDraw->drawLine(minA, minA + (minNorm * minProj), color);
242 }
243 #endif //DEBUG_DRAW
244
245 btGjkPairDetector gjkdet(convexA, convexB, &simplexSolver, 0);
246
247 btScalar offsetDist = minProj;
248 btVector3 offset = minNorm * offsetDist;
249
250 btGjkPairDetector::ClosestPointInput input;
251
252 btVector3 newOrg = transA.getOrigin() + offset;
253
254 btTransform displacedTrans = transA;
255 displacedTrans.setOrigin(newOrg);
256
257 input.m_transformA = displacedTrans;
258 input.m_transformB = transB;
259 input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT); //minProj;
260
261 btIntermediateResult res;
262 gjkdet.setCachedSeparatingAxis(-minNorm);
263 gjkdet.getClosestPoints(input, res, debugDraw);
264
265 btScalar correctedMinNorm = minProj - res.m_depth;
266
267 //the penetration depth is over-estimated, relax it
268 btScalar penetration_relaxation = btScalar(1.);
269 minNorm *= penetration_relaxation;
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
288 btVector3* btMinkowskiPenetrationDepthSolver::getPenetrationDirections()
289 {
290 static btVector3 sPenetrationDirections[NUM_UNITSPHERE_POINTS + MAX_PREFERRED_PENETRATION_DIRECTIONS * 2] =
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
335 return sPenetrationDirections;
336 }
btConvexShape::getNumPreferredPenetrationDirections
virtual int getNumPreferredPenetrationDirections() const =0
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btSubSimplexConvexCast.h
btDiscreteCollisionDetectorInterface::Result
Definition: btDiscreteCollisionDetectorInterface.h:30
btConvexShape.h
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btMinkowskiPenetrationDepthSolver::calcPenDepth
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)
Definition: btMinkowskiPenetrationDepthSolver.cpp:24
btGjkPairDetector.h
btConvexShape::getPreferredPenetrationDirection
virtual void getPreferredPenetrationDirection(int index, btVector3 &penetrationVector) const =0
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btDiscreteCollisionDetectorInterface.h:48
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformA
btTransform m_transformA
Definition: btDiscreteCollisionDetectorInterface.h:46
btConvexShape::localGetSupportVertexWithoutMarginNonVirtual
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
Definition: btConvexShape.cpp:131
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btDiscreteCollisionDetectorInterface::ClosestPointInput::m_transformB
btTransform m_transformB
Definition: btDiscreteCollisionDetectorInterface.h:47
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btGjkPairDetector
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
Definition: btGjkPairDetector.h:28
MAX_PREFERRED_PENETRATION_DIRECTIONS
#define MAX_PREFERRED_PENETRATION_DIRECTIONS
Definition: btConvexShape.h:27
btConvexShape::getMarginNonVirtual
btScalar getMarginNonVirtual() const
Definition: btConvexShape.cpp:320
btVoronoiSimplexSolver.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btMinkowskiPenetrationDepthSolver.h
btIDebugDraw::drawLine
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
btMinkowskiPenetrationDepthSolver::getPenetrationDirections
static btVector3 * getPenetrationDirections()
Definition: btMinkowskiPenetrationDepthSolver.cpp:288
btConvexShape
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
btConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin
virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 *vectors, btVector3 *supportVerticesOut, int numVectors) const =0
btCollisionShape::isConvex2d
bool isConvex2d() const
Definition: btCollisionShape.h:65
btGjkPairDetector::setCachedSeparatingAxis
void setCachedSeparatingAxis(const btVector3 &separatingAxis)
Definition: btGjkPairDetector.h:67
btGjkPairDetector::getClosestPoints
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
Definition: btGjkPairDetector.cpp:73
btSimplexSolverInterface
#define btSimplexSolverInterface
Definition: btSimplexSolverInterface.h:24
NUM_UNITSPHERE_POINTS
#define NUM_UNITSPHERE_POINTS
Definition: btMinkowskiPenetrationDepthSolver.cpp:22
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btDiscreteCollisionDetectorInterface::ClosestPointInput
Definition: btDiscreteCollisionDetectorInterface.h:40
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251