Bullet Collision Detection & Physics Library
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 
16 
18 #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 {
45  {
47  }
48 }
49 
50 
52  m_dispatcher(dispatcher),
53  m_dispatchInfoPtr(0)
54 {
55  m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
56  m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
57 
58  //
59  // create the manifold from the dispatcher 'manifold pool'
60  //
62 
63  clearCache();
64 }
65 
67 {
68  clearCache();
70 
71 }
72 
73 
75 {
77 }
78 
79 
81 partId, int triangleIndex)
82 {
83  BT_PROFILE("btConvexTriangleCallback::processTriangle");
84 
86  {
87  return;
88  }
89 
90  //just for debugging purposes
91  //printf("triangle %d",m_triangleCount++);
92 
93 
94 
97 
98 
99 
100 #if 0
101 
104  {
106  btVector3 color(1,1,0);
107  btTransform& tr = ob->getWorldTransform();
108  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
109  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
110  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
111  }
112 #endif
113 
115  {
116  btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
118 
119 
120  btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
121  btCollisionAlgorithm* colAlgo = 0;
122 
124  {
126  }
127  else
128  {
130  }
131  const btCollisionObjectWrapper* tmpWrap = 0;
132 
134  {
135  tmpWrap = m_resultOut->getBody0Wrap();
136  m_resultOut->setBody0Wrap(&triObWrap);
137  m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
138  }
139  else
140  {
141  tmpWrap = m_resultOut->getBody1Wrap();
142  m_resultOut->setBody1Wrap(&triObWrap);
143  m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
144  }
145 
146  colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
147 
149  {
150  m_resultOut->setBody0Wrap(tmpWrap);
151  } else
152  {
153  m_resultOut->setBody1Wrap(tmpWrap);
154  }
155 
156 
157 
158  colAlgo->~btCollisionAlgorithm();
160  }
161 
162 }
163 
164 
165 
166 void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
167 {
168  m_convexBodyWrap = convexBodyWrap;
169  m_triBodyWrap = triBodyWrap;
170 
171  m_dispatchInfoPtr = &dispatchInfo;
172  m_collisionMarginTriangle = collisionMarginTriangle;
173  m_resultOut = resultOut;
174 
175  //recalc aabbs
176  btTransform convexInTriangleSpace;
178  const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
179  //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
180  convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
181  btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
182 
183  btVector3 extra(extraMargin,extraMargin,extraMargin);
184 
185  m_aabbMax += extra;
186  m_aabbMin -= extra;
187 
188 }
189 
191 {
192  m_btConvexTriangleCallback.clearCache();
193 
194 }
195 
197 {
198  BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
199 
200  const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
201  const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
202 
203  if (triBodyWrap->getCollisionShape()->isConcave())
204  {
205 
206 
207 
208  const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
209 
210  if (convexBodyWrap->getCollisionShape()->isConvex())
211  {
212  btScalar collisionMarginTriangle = concaveShape->getMargin();
213 
214  resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
215  m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
216 
217  m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
218 
219  concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
220 
221  resultOut->refreshContactPoints();
222 
223  m_btConvexTriangleCallback.clearWrapperData();
224 
225  }
226 
227  }
228 
229 }
230 
231 
233 {
234  (void)resultOut;
235  (void)dispatchInfo;
236  btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
237  btCollisionObject* triBody = m_isSwapped ? body0 : body1;
238 
239 
240  //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
241 
242  //only perform CCD above a certain threshold, this prevents blocking on the long run
243  //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
244  btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
245  if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
246  {
247  return btScalar(1.);
248  }
249 
250  //const btVector3& from = convexbody->m_worldTransform.getOrigin();
251  //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
252  //todo: only do if the motion exceeds the 'radius'
253 
254  btTransform triInv = triBody->getWorldTransform().inverse();
255  btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
256  btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
257 
258  struct LocalTriangleSphereCastCallback : public btTriangleCallback
259  {
260  btTransform m_ccdSphereFromTrans;
261  btTransform m_ccdSphereToTrans;
262  btTransform m_meshTransform;
263 
264  btScalar m_ccdSphereRadius;
265  btScalar m_hitFraction;
266 
267 
268  LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
269  :m_ccdSphereFromTrans(from),
270  m_ccdSphereToTrans(to),
271  m_ccdSphereRadius(ccdSphereRadius),
272  m_hitFraction(hitFraction)
273  {
274  }
275 
276 
277  virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
278  {
279  BT_PROFILE("processTriangle");
280  (void)partId;
281  (void)triangleIndex;
282  //do a swept sphere for now
283  btTransform ident;
284  ident.setIdentity();
285  btConvexCast::CastResult castResult;
286  castResult.m_fraction = m_hitFraction;
287  btSphereShape pointShape(m_ccdSphereRadius);
288  btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
289  btVoronoiSimplexSolver simplexSolver;
290  btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
291  //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
292  //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
293  //local space?
294 
295  if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
296  ident,ident,castResult))
297  {
298  if (m_hitFraction > castResult.m_fraction)
299  m_hitFraction = castResult.m_fraction;
300  }
301 
302  }
303 
304  };
305 
306 
307 
308 
309 
310  if (triBody->getCollisionShape()->isConcave())
311  {
312  btVector3 rayAabbMin = convexFromLocal.getOrigin();
313  rayAabbMin.setMin(convexToLocal.getOrigin());
314  btVector3 rayAabbMax = convexFromLocal.getOrigin();
315  rayAabbMax.setMax(convexToLocal.getOrigin());
316  btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
317  rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
318  rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
319 
320  btScalar curHitFraction = btScalar(1.); //is this available?
321  LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
322  convexbody->getCcdSweptSphereRadius(),curHitFraction);
323 
324  raycastCallback.m_hitFraction = convexbody->getHitFraction();
325 
326  btCollisionObject* concavebody = triBody;
327 
328  btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
329 
330  if (triangleMesh)
331  {
332  triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
333  }
334 
335 
336 
337  if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
338  {
339  convexbody->setHitFraction( raycastCallback.m_hitFraction);
340  return raycastCallback.m_hitFraction;
341  }
342  }
343 
344  return btScalar(1.);
345 
346 }
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
void push_back(const T &_Val)
const btDispatcherInfo * m_dispatchInfoPtr
const btCollisionObjectWrapper * getBody0Wrap() const
bool isConvex() const
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
void setPersistentManifold(btPersistentManifold *manifoldPtr)
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
void setHitFraction(btScalar hitFraction)
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
btManifoldResult is a helper class to manage contact results.
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58
const btCollisionShape * getCollisionShape() const
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const =0
virtual void getAabb(const btTransform &t, btVector3 &aabbMin, btVector3 &aabbMax) const =0
getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t...
virtual void setShapeIdentifiersA(int partId0, int index0)
setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material comb...
btTransform & getWorldTransform()
virtual void setMargin(btScalar margin)
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:36
const btCollisionObjectWrapper * m_convexBodyWrap
btScalar m_closestPointDistanceThreshold
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
const btCollisionObject * getCollisionObject() const
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
virtual bool calcTimeOfImpact(const btTransform &fromA, const btTransform &toA, const btTransform &fromB, const btTransform &toB, CastResult &result)
SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (...
btCollisionObject can be used to manage collision detection objects.
const btTransform & getInterpolationWorldTransform() const
bool isConcave() const
void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo &dispatchInfo, const btCollisionObjectWrapper *convexBodyWrap, const btCollisionObjectWrapper *triBodyWrap, btManifoldResult *resultOut)
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
virtual void setShapeIdentifiersB(int partId1, int index1)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btSubsimplexConvexCast implements Gino van den Bergens&#39; paper "Ray Casting against bteral Convex Obje...
btScalar getHitFraction() const
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
const btCollisionObject * getBody0Internal() const
virtual int getDebugMode() const =0
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
const btTransform & getWorldTransform() const
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
const btCollisionObjectWrapper * m_triBodyWrap
btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
btConvexTriangleCallback(btDispatcher *dispatcher, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
const btCollisionObjectWrapper * getBody1Wrap() const
virtual btScalar getMargin() const
void setBody0Wrap(const btCollisionObjectWrapper *obj0Wrap)
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
Definition: btVector3.h:621
btScalar getCcdSweptSphereRadius() const
Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
bool TestTriangleAgainstAabb2(const btVector3 *vertices, const btVector3 &aabbMin, const btVector3 &aabbMax)
conservative test for overlap between triangle and aabb
Definition: btAabbUtil2.h:59
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
Definition: btVector3.h:638
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
const btCollisionShape * getCollisionShape() const
virtual void clearManifold(btPersistentManifold *manifold)=0