Blender  V3.3
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 
21 
22 #define NUM_UNITSPHERE_POINTS 42
23 
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;
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 
80  int i;
81 
82  int numSampleDirections = NUM_UNITSPHERE_POINTS;
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  {
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  {
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  {
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 
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 
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 }
Group Output data from inside of a node group A color picker Mix two input colors RGB to Convert a color s luminance to a grayscale value Generate a normal vector and a dot product Bright Control the brightness and contrast of the input color Vector Map an input vectors to used to fine tune the interpolation of the input Camera Retrieve information about the camera and how it relates to the current shading point s position Clamp a value between a minimum and a maximum Vector Perform vector math operation Invert a color
ATTR_WARN_UNUSED_RESULT const BMVert * v
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
#define MAX_PREFERRED_PENETRATION_DIRECTIONS
Definition: btConvexShape.h:27
void debugDraw(btIDebugDraw *debugDrawer)
btActionInterface interface
#define NUM_UNITSPHERE_POINTS
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
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 btSimplexSolverInterface
btVector3 m_depth
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
btGjkPairDetector uses GJK to implement the btDiscreteCollisionDetectorInterface
virtual void getClosestPoints(const ClosestPointInput &input, Result &output, class btIDebugDraw *debugDraw, bool swapResults=false)
void setCachedSeparatingAxis(const btVector3 &separatingAxis)
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)
SyclQueue void void size_t num_bytes void
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset
ccl_global KernelShaderEvalInput * input