Blender  V3.3
btConvexConcaveCollisionAlgorithm.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 
17 #include "LinearMath/btQuickprof.h"
30 
32  : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
33  m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped),
34  m_isSwapped(isSwapped)
35 {
36 }
37 
39 {
40 }
41 
43 {
44  if (m_btConvexTriangleCallback.m_manifoldPtr)
45  {
46  manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
47  }
48 }
49 
50 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
52 {
53  m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
54  m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
55 
56  //
57  // create the manifold from the dispatcher 'manifold pool'
58  //
60 
61  clearCache();
62 }
63 
65 {
66  clearCache();
68 }
69 
71 {
73 }
74 
75 void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
76 {
77  BT_PROFILE("btConvexTriangleCallback::processTriangle");
78 
80  {
81  return;
82  }
83 
84  //just for debugging purposes
85  //printf("triangle %d",m_triangleCount++);
86 
89 
90 #if 0
91 
94  {
96  btVector3 color(1,1,0);
97  btTransform& tr = ob->getWorldTransform();
98  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
99  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
100  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
101  }
102 #endif
103 
104  if (m_convexBodyWrap->getCollisionShape()->isConvex())
105  {
106  btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
107  tm.setMargin(m_collisionMarginTriangle);
108 
109  btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform?
110  btCollisionAlgorithm* colAlgo = 0;
111 
113  {
115  }
116  else
117  {
119  }
120  const btCollisionObjectWrapper* tmpWrap = 0;
121 
123  {
124  tmpWrap = m_resultOut->getBody0Wrap();
125  m_resultOut->setBody0Wrap(&triObWrap);
126  m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
127  }
128  else
129  {
130  tmpWrap = m_resultOut->getBody1Wrap();
131  m_resultOut->setBody1Wrap(&triObWrap);
132  m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
133  }
134 
136 
138  {
139  m_resultOut->setBody0Wrap(tmpWrap);
140  }
141  else
142  {
143  m_resultOut->setBody1Wrap(tmpWrap);
144  }
145 
146  colAlgo->~btCollisionAlgorithm();
148  }
149 }
150 
151 void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
152 {
153  m_convexBodyWrap = convexBodyWrap;
154  m_triBodyWrap = triBodyWrap;
155 
156  m_dispatchInfoPtr = &dispatchInfo;
157  m_collisionMarginTriangle = collisionMarginTriangle;
158  m_resultOut = resultOut;
159 
160  //recalc aabbs
161  btTransform convexInTriangleSpace;
162  convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
163  const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
164  //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
165  convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
166  btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
167 
168  btVector3 extra(extraMargin, extraMargin, extraMargin);
169 
170  m_aabbMax += extra;
171  m_aabbMin -= extra;
172 }
173 
175 {
176  m_btConvexTriangleCallback.clearCache();
177 }
178 
180 {
181  BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
182 
183  const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
184  const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
185 
186  if (triBodyWrap->getCollisionShape()->isConcave())
187  {
188  if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
189  {
190  btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
191  if (convexBodyWrap->getCollisionShape()->isConvex())
192  {
193  btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
194  btAlignedObjectArray<btVector3> queryVertices;
195 
196  if (convex->isPolyhedral())
197  {
199  for (int v = 0; v < poly->getNumVertices(); v++)
200  {
201  btVector3 vtx;
202  poly->getVertex(v, vtx);
203  queryVertices.push_back(vtx);
204  }
205  }
206  btScalar maxDist = SIMD_EPSILON;
207 
208  if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
209  {
210  queryVertices.push_back(btVector3(0, 0, 0));
211  btSphereShape* sphere = (btSphereShape*)convex;
212  maxDist = sphere->getRadius() + SIMD_EPSILON;
213  }
214  if (queryVertices.size())
215  {
216  resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
217  //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
218 
220  for (int v = 0; v < queryVertices.size(); v++)
221  {
222  const btVector3& vtx = queryVertices[v];
223  btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx;
224  btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
225 
226  btVector3 normalLocal;
227  btScalar dist;
228  if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
229  {
230  if (dist <= maxDist)
231  {
232  normalLocal.safeNormalize();
233  btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal;
234 
235  if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
236  {
237  btSphereShape* sphere = (btSphereShape*)convex;
238  dist -= sphere->getRadius();
239  vtxWorldSpace -= sphere->getRadius() * normal;
240  }
241  resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist);
242  }
243  }
244  }
245  resultOut->refreshContactPoints();
246  }
247  }
248  }
249  else
250  {
251  const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape());
252 
253  if (convexBodyWrap->getCollisionShape()->isConvex())
254  {
255  btScalar collisionMarginTriangle = concaveShape->getMargin();
256 
257  resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
258  m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
259 
260  m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
261 
262  concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
263 
264  resultOut->refreshContactPoints();
265 
266  m_btConvexTriangleCallback.clearWrapperData();
267  }
268  }
269  }
270 }
271 
273 {
274  (void)resultOut;
275  (void)dispatchInfo;
276  btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
277  btCollisionObject* triBody = m_isSwapped ? body0 : body1;
278 
279  //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
280 
281  //only perform CCD above a certain threshold, this prevents blocking on the long run
282  //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
283  btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
284  if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
285  {
286  return btScalar(1.);
287  }
288 
289  //const btVector3& from = convexbody->m_worldTransform.getOrigin();
290  //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
291  //todo: only do if the motion exceeds the 'radius'
292 
293  btTransform triInv = triBody->getWorldTransform().inverse();
294  btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
295  btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
296 
297  struct LocalTriangleSphereCastCallback : public btTriangleCallback
298  {
299  btTransform m_ccdSphereFromTrans;
300  btTransform m_ccdSphereToTrans;
301  btTransform m_meshTransform;
302 
303  btScalar m_ccdSphereRadius;
305 
306  LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
307  : m_ccdSphereFromTrans(from),
308  m_ccdSphereToTrans(to),
309  m_ccdSphereRadius(ccdSphereRadius),
310  m_hitFraction(hitFraction)
311  {
312  }
313 
314  virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
315  {
316  BT_PROFILE("processTriangle");
317  (void)partId;
318  (void)triangleIndex;
319  //do a swept sphere for now
320  btTransform ident;
321  ident.setIdentity();
322  btConvexCast::CastResult castResult;
323  castResult.m_fraction = m_hitFraction;
324  btSphereShape pointShape(m_ccdSphereRadius);
325  btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
326  btVoronoiSimplexSolver simplexSolver;
327  btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
328  //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
329  //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
330  //local space?
331 
332  if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
333  ident, ident, castResult))
334  {
335  if (m_hitFraction > castResult.m_fraction)
336  m_hitFraction = castResult.m_fraction;
337  }
338  }
339  };
340 
341  if (triBody->getCollisionShape()->isConcave())
342  {
343  btVector3 rayAabbMin = convexFromLocal.getOrigin();
344  rayAabbMin.setMin(convexToLocal.getOrigin());
345  btVector3 rayAabbMax = convexFromLocal.getOrigin();
346  rayAabbMax.setMax(convexToLocal.getOrigin());
347  btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
348  rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
349  rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
350 
351  btScalar curHitFraction = btScalar(1.); //is this available?
352  LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
353  convexbody->getCcdSweptSphereRadius(), curHitFraction);
354 
355  raycastCallback.m_hitFraction = convexbody->getHitFraction();
356 
357  btCollisionObject* concavebody = triBody;
358 
359  btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
360 
361  if (triangleMesh)
362  {
363  triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
364  }
365 
366  if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
367  {
368  convexbody->setHitFraction(raycastCallback.m_hitFraction);
369  return raycastCallback.m_hitFraction;
370  }
371  }
372 
373  return btScalar(1.);
374 }
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
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices, const btVector3 &aabbMin, const btVector3 &aabbMax)
conservative test for overlap between triangle and aabb
Definition: btAabbUtil2.h:54
btVector3 m_aabbMax
@ SDF_SHAPE_PROXYTYPE
@ SPHERE_SHAPE_PROXYTYPE
btVector3 m_aabbMin
btCollisionObject
btScalar m_hitFraction
time of impact calculation
btScalar getCcdSquareMotionThreshold() const
btCollisionShape
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
btConcaveShape()
btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
virtual ~btConvexConcaveCollisionAlgorithm()
btManifoldResult * m_resultOut
btPersistentManifold * m_manifoldPtr
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
virtual ~btConvexTriangleCallback()
btScalar m_collisionMarginTriangle
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
btConvexTriangleCallback(btDispatcher *dispatcher, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
void clearCache()
const btCollisionObjectWrapper * m_triBodyWrap
const btDispatcherInfo * m_dispatchInfoPtr
btDispatcher * m_dispatcher
void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo &dispatchInfo, const btCollisionObjectWrapper *convexBodyWrap, const btCollisionObjectWrapper *triBodyWrap, btManifoldResult *resultOut)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
const btCollisionObjectWrapper * m_convexBodyWrap
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
btConvexShape()
not supported on IBM SDK, until we fix the alignment of btVector3
@ BT_CLOSEST_POINT_ALGORITHMS
Definition: btDispatcher.h:71
@ BT_CONTACT_POINT_ALGORITHMS
Definition: btDispatcher.h:70
btPolyhedralConvexShape()
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_EPSILON
Definition: btScalar.h:543
btSphereShape(btScalar radius)
Definition: btSphereShape.h:29
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTriangleShape()
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
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
btVoronoiSimplexSolver
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void push_back(const T &_Val)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
virtual void clearManifold(btPersistentManifold *manifold)=0
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual int getDebugMode() const =0
btManifoldResult is a helper class to manage contact results.
virtual void setShapeIdentifiersA(int partId0, int index0)
setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material comb...
void setBody0Wrap(const btCollisionObjectWrapper *obj0Wrap)
const btCollisionObject * getBody0Internal() const
const btCollisionObjectWrapper * getBody1Wrap() const
const btCollisionObjectWrapper * getBody0Wrap() const
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
void setPersistentManifold(btPersistentManifold *manifoldPtr)
virtual void setShapeIdentifiersB(int partId1, int index1)
btScalar m_closestPointDistanceThreshold
SIMD_FORCE_INLINE void refreshContactPoints()
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
bool queryPoint(const btVector3 &ptInSDF, btScalar &distOut, btVector3 &normal)
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)=0
StackEntry * from
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
SIMD_FORCE_INLINE const btCollisionShape * getCollisionShape() const
SIMD_FORCE_INLINE const btTransform & getWorldTransform() const
SIMD_FORCE_INLINE const btCollisionObject * getCollisionObject() const
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58