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 
29 
31 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
32 m_isSwapped(isSwapped),
33 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
34 {
35 }
36 
38 {
39 }
40 
42 {
44  {
46  }
47 }
48 
49 
51  m_dispatcher(dispatcher),
52  m_dispatchInfoPtr(0)
53 {
54  m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
55  m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
56 
57  //
58  // create the manifold from the dispatcher 'manifold pool'
59  //
61 
62  clearCache();
63 }
64 
66 {
67  clearCache();
69 
70 }
71 
72 
74 {
76 }
77 
78 
80 partId, int triangleIndex)
81 {
82 
84  {
85  return;
86  }
87 
88  //just for debugging purposes
89  //printf("triangle %d",m_triangleCount++);
90 
91 
92 
95 
96 
97 
98 #if 0
99 
102  {
104  btVector3 color(1,1,0);
105  btTransform& tr = ob->getWorldTransform();
106  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
107  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
108  m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
109  }
110 #endif
111 
113  {
114  btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
116 
117 
118  btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
120 
121  const btCollisionObjectWrapper* tmpWrap = 0;
122 
124  {
125  tmpWrap = m_resultOut->getBody0Wrap();
126  m_resultOut->setBody0Wrap(&triObWrap);
127  m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
128  }
129  else
130  {
131  tmpWrap = m_resultOut->getBody1Wrap();
132  m_resultOut->setBody1Wrap(&triObWrap);
133  m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
134  }
135 
136  colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
137 
139  {
140  m_resultOut->setBody0Wrap(tmpWrap);
141  } else
142  {
143  m_resultOut->setBody1Wrap(tmpWrap);
144  }
145 
146 
147 
148  colAlgo->~btCollisionAlgorithm();
150  }
151 
152 }
153 
154 
155 
156 void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
157 {
158  m_convexBodyWrap = convexBodyWrap;
159  m_triBodyWrap = triBodyWrap;
160 
161  m_dispatchInfoPtr = &dispatchInfo;
162  m_collisionMarginTriangle = collisionMarginTriangle;
163  m_resultOut = resultOut;
164 
165  //recalc aabbs
166  btTransform convexInTriangleSpace;
168  const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
169  //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
170  convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
171  btScalar extraMargin = collisionMarginTriangle;
172  btVector3 extra(extraMargin,extraMargin,extraMargin);
173 
174  m_aabbMax += extra;
175  m_aabbMin -= extra;
176 
177 }
178 
180 {
181  m_btConvexTriangleCallback.clearCache();
182 
183 }
184 
186 {
187 
188 
189  const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
190  const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
191 
192  if (triBodyWrap->getCollisionShape()->isConcave())
193  {
194 
195 
196 
197  const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
198 
199  if (convexBodyWrap->getCollisionShape()->isConvex())
200  {
201  btScalar collisionMarginTriangle = concaveShape->getMargin();
202 
203  resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
204  m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
205 
206  m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
207 
208  concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
209 
210  resultOut->refreshContactPoints();
211 
212  m_btConvexTriangleCallback.clearWrapperData();
213 
214  }
215 
216  }
217 
218 }
219 
220 
222 {
223  (void)resultOut;
224  (void)dispatchInfo;
225  btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
226  btCollisionObject* triBody = m_isSwapped ? body0 : body1;
227 
228 
229  //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
230 
231  //only perform CCD above a certain threshold, this prevents blocking on the long run
232  //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
233  btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
234  if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
235  {
236  return btScalar(1.);
237  }
238 
239  //const btVector3& from = convexbody->m_worldTransform.getOrigin();
240  //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
241  //todo: only do if the motion exceeds the 'radius'
242 
243  btTransform triInv = triBody->getWorldTransform().inverse();
244  btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
245  btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
246 
247  struct LocalTriangleSphereCastCallback : public btTriangleCallback
248  {
249  btTransform m_ccdSphereFromTrans;
250  btTransform m_ccdSphereToTrans;
251  btTransform m_meshTransform;
252 
253  btScalar m_ccdSphereRadius;
254  btScalar m_hitFraction;
255 
256 
257  LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
258  :m_ccdSphereFromTrans(from),
259  m_ccdSphereToTrans(to),
260  m_ccdSphereRadius(ccdSphereRadius),
261  m_hitFraction(hitFraction)
262  {
263  }
264 
265 
266  virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
267  {
268  (void)partId;
269  (void)triangleIndex;
270  //do a swept sphere for now
271  btTransform ident;
272  ident.setIdentity();
273  btConvexCast::CastResult castResult;
274  castResult.m_fraction = m_hitFraction;
275  btSphereShape pointShape(m_ccdSphereRadius);
276  btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
277  btVoronoiSimplexSolver simplexSolver;
278  btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
279  //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
280  //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
281  //local space?
282 
283  if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
284  ident,ident,castResult))
285  {
286  if (m_hitFraction > castResult.m_fraction)
287  m_hitFraction = castResult.m_fraction;
288  }
289 
290  }
291 
292  };
293 
294 
295 
296 
297 
298  if (triBody->getCollisionShape()->isConcave())
299  {
300  btVector3 rayAabbMin = convexFromLocal.getOrigin();
301  rayAabbMin.setMin(convexToLocal.getOrigin());
302  btVector3 rayAabbMax = convexFromLocal.getOrigin();
303  rayAabbMax.setMax(convexToLocal.getOrigin());
304  btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
305  rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
306  rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
307 
308  btScalar curHitFraction = btScalar(1.); //is this available?
309  LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
310  convexbody->getCcdSweptSphereRadius(),curHitFraction);
311 
312  raycastCallback.m_hitFraction = convexbody->getHitFraction();
313 
314  btCollisionObject* concavebody = triBody;
315 
316  btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
317 
318  if (triangleMesh)
319  {
320  triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
321  }
322 
323 
324 
325  if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
326  {
327  convexbody->setHitFraction( raycastCallback.m_hitFraction);
328  return raycastCallback.m_hitFraction;
329  }
330  }
331 
332  return btScalar(1.);
333 
334 }
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
btScalar getCcdSweptSphereRadius() const
Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold=0)=0
virtual void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)=0
virtual int getDebugMode() const =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 btCollisionObjectWrapper * getBody1Wrap() const
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const =0
const btTransform & getInterpolationWorldTransform() const
virtual void setShapeIdentifiersA(int partId0, int index0)
setShapeIdentifiersA/B provides experimental support for per-triangle material / custom material comb...
bool isConcave() const
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 btCollisionObjectWrapper * getBody0Wrap() 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 (...
const btTransform & getWorldTransform() const
btCollisionObject can be used to manage collision detection objects.
void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo &dispatchInfo, const btCollisionObjectWrapper *convexBodyWrap, const btCollisionObjectWrapper *triBodyWrap, btManifoldResult *resultOut)
const btCollisionObject * getBody0Internal() const
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=0
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
virtual void setShapeIdentifiersB(int partId1, int index1)
const btCollisionShape * getCollisionShape() const
btScalar getHitFraction() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
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...
bool isConvex() const
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
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)
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:609
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...
const btCollisionShape * getCollisionShape() const
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...
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:626
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
const btCollisionObject * getCollisionObject() const
virtual void clearManifold(btPersistentManifold *manifold)=0