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_isSwapped(isSwapped),
34 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,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?
122 
123  const btCollisionObjectWrapper* tmpWrap = 0;
124 
126  {
127  tmpWrap = m_resultOut->getBody0Wrap();
128  m_resultOut->setBody0Wrap(&triObWrap);
129  m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
130  }
131  else
132  {
133  tmpWrap = m_resultOut->getBody1Wrap();
134  m_resultOut->setBody1Wrap(&triObWrap);
135  m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
136  }
137 
138  colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
139 
141  {
142  m_resultOut->setBody0Wrap(tmpWrap);
143  } else
144  {
145  m_resultOut->setBody1Wrap(tmpWrap);
146  }
147 
148 
149 
150  colAlgo->~btCollisionAlgorithm();
152  }
153 
154 }
155 
156 
157 
158 void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
159 {
160  m_convexBodyWrap = convexBodyWrap;
161  m_triBodyWrap = triBodyWrap;
162 
163  m_dispatchInfoPtr = &dispatchInfo;
164  m_collisionMarginTriangle = collisionMarginTriangle;
165  m_resultOut = resultOut;
166 
167  //recalc aabbs
168  btTransform convexInTriangleSpace;
170  const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
171  //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
172  convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
173  btScalar extraMargin = collisionMarginTriangle;
174  btVector3 extra(extraMargin,extraMargin,extraMargin);
175 
176  m_aabbMax += extra;
177  m_aabbMin -= extra;
178 
179 }
180 
182 {
183  m_btConvexTriangleCallback.clearCache();
184 
185 }
186 
188 {
189  BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
190 
191  const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
192  const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
193 
194  if (triBodyWrap->getCollisionShape()->isConcave())
195  {
196 
197 
198 
199  const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
200 
201  if (convexBodyWrap->getCollisionShape()->isConvex())
202  {
203  btScalar collisionMarginTriangle = concaveShape->getMargin();
204 
205  resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
206  m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
207 
208  m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
209 
210  concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
211 
212  resultOut->refreshContactPoints();
213 
214  m_btConvexTriangleCallback.clearWrapperData();
215 
216  }
217 
218  }
219 
220 }
221 
222 
224 {
225  (void)resultOut;
226  (void)dispatchInfo;
227  btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
228  btCollisionObject* triBody = m_isSwapped ? body0 : body1;
229 
230 
231  //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
232 
233  //only perform CCD above a certain threshold, this prevents blocking on the long run
234  //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
235  btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
236  if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
237  {
238  return btScalar(1.);
239  }
240 
241  //const btVector3& from = convexbody->m_worldTransform.getOrigin();
242  //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
243  //todo: only do if the motion exceeds the 'radius'
244 
245  btTransform triInv = triBody->getWorldTransform().inverse();
246  btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
247  btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
248 
249  struct LocalTriangleSphereCastCallback : public btTriangleCallback
250  {
251  btTransform m_ccdSphereFromTrans;
252  btTransform m_ccdSphereToTrans;
253  btTransform m_meshTransform;
254 
255  btScalar m_ccdSphereRadius;
256  btScalar m_hitFraction;
257 
258 
259  LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
260  :m_ccdSphereFromTrans(from),
261  m_ccdSphereToTrans(to),
262  m_ccdSphereRadius(ccdSphereRadius),
263  m_hitFraction(hitFraction)
264  {
265  }
266 
267 
268  virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
269  {
270  BT_PROFILE("processTriangle");
271  (void)partId;
272  (void)triangleIndex;
273  //do a swept sphere for now
274  btTransform ident;
275  ident.setIdentity();
276  btConvexCast::CastResult castResult;
277  castResult.m_fraction = m_hitFraction;
278  btSphereShape pointShape(m_ccdSphereRadius);
279  btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
280  btVoronoiSimplexSolver simplexSolver;
281  btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
282  //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
283  //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
284  //local space?
285 
286  if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
287  ident,ident,castResult))
288  {
289  if (m_hitFraction > castResult.m_fraction)
290  m_hitFraction = castResult.m_fraction;
291  }
292 
293  }
294 
295  };
296 
297 
298 
299 
300 
301  if (triBody->getCollisionShape()->isConcave())
302  {
303  btVector3 rayAabbMin = convexFromLocal.getOrigin();
304  rayAabbMin.setMin(convexToLocal.getOrigin());
305  btVector3 rayAabbMax = convexFromLocal.getOrigin();
306  rayAabbMax.setMax(convexToLocal.getOrigin());
307  btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
308  rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
309  rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
310 
311  btScalar curHitFraction = btScalar(1.); //is this available?
312  LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
313  convexbody->getCcdSweptSphereRadius(),curHitFraction);
314 
315  raycastCallback.m_hitFraction = convexbody->getHitFraction();
316 
317  btCollisionObject* concavebody = triBody;
318 
319  btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
320 
321  if (triangleMesh)
322  {
323  triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
324  }
325 
326 
327 
328  if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
329  {
330  convexbody->setHitFraction( raycastCallback.m_hitFraction);
331  return raycastCallback.m_hitFraction;
332  }
333  }
334 
335  return btScalar(1.);
336 
337 }
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
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold=0)=0
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
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
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:203
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:619
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:69
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:636
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
const btCollisionShape * getCollisionShape() const
virtual void clearManifold(btPersistentManifold *manifold)=0