Bullet Collision Detection & Physics Library
btConvexConcaveCollisionAlgorithm.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
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
50btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
51 m_dispatchInfoPtr(0)
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
75void 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);
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
105 {
106 btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
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
151void 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;
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{
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();
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 {
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
258 m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
259
261
263
264 resultOut->refreshContactPoints();
265
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;
304 btScalar m_hitFraction;
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}
bool TestTriangleAgainstAabb2(const btVector3 *vertices, const btVector3 &aabbMin, const btVector3 &aabbMax)
conservative test for overlap between triangle and aabb
Definition: btAabbUtil2.h:54
@ SDF_SHAPE_PROXYTYPE
@ SPHERE_SHAPE_PROXYTYPE
@ BT_CLOSEST_POINT_ALGORITHMS
Definition: btDispatcher.h:71
@ BT_CONTACT_POINT_ALGORITHMS
Definition: btDispatcher.h:70
#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
This class is not enabled yet (work-in-progress) to more aggressively activate objects.
int size() const
return the number of elements in the array
void push_back(const T &_Val)
btCollisionAlgorithm is an collision interface that is compatible with the Broadphase and btDispatche...
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)=0
btCollisionObject can be used to manage collision detection objects.
btScalar getHitFraction() const
btTransform & getWorldTransform()
const btTransform & getInterpolationWorldTransform() const
const btCollisionShape * getCollisionShape() const
void setHitFraction(btScalar hitFraction)
btScalar getCcdSweptSphereRadius() const
Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
The btCollisionShape class provides an interface for collision shapes that can be shared among btColl...
int getShapeType() const
bool isConvex() 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 isConcave() const
bool isPolyhedral() const
The btConcaveShape class provides an interface for non-moving (static) concave shapes.
virtual btScalar getMargin() const
virtual void processAllTriangles(btTriangleCallback *callback, const btVector3 &aabbMin, const btVector3 &aabbMax) const =0
btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo &ci, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
btScalar calculateTimeOfImpact(btCollisionObject *body0, btCollisionObject *body1, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
virtual void getAllContactManifolds(btManifoldArray &manifoldArray)
virtual void processCollision(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, const btDispatcherInfo &dispatchInfo, btManifoldResult *resultOut)
virtual void setMargin(btScalar margin)
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:33
btConvexTriangleCallback(btDispatcher *dispatcher, const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, bool isSwapped)
virtual void processTriangle(btVector3 *triangle, int partId, int triangleIndex)
void setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo &dispatchInfo, const btCollisionObjectWrapper *convexBodyWrap, const btCollisionObjectWrapper *triBodyWrap, btManifoldResult *resultOut)
const btDispatcherInfo * m_dispatchInfoPtr
const btCollisionObjectWrapper * m_convexBodyWrap
const btCollisionObjectWrapper * m_triBodyWrap
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
virtual void clearManifold(btPersistentManifold *manifold)=0
virtual void releaseManifold(btPersistentManifold *manifold)=0
virtual void freeCollisionAlgorithm(void *ptr)=0
virtual btCollisionAlgorithm * findAlgorithm(const btCollisionObjectWrapper *body0Wrap, const btCollisionObjectWrapper *body1Wrap, btPersistentManifold *sharedManifold, ebtDispatcherQueryType queryType)=0
virtual btPersistentManifold * getNewManifold(const btCollisionObject *b0, const btCollisionObject *b1)=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 btCollisionObjectWrapper * getBody1Wrap() const
void setBody1Wrap(const btCollisionObjectWrapper *obj1Wrap)
void setPersistentManifold(btPersistentManifold *manifoldPtr)
const btCollisionObject * getBody0Internal() const
virtual void setShapeIdentifiersB(int partId1, int index1)
btScalar m_closestPointDistanceThreshold
const btCollisionObjectWrapper * getBody0Wrap() const
virtual void addContactPoint(const btVector3 &normalOnBInWorld, const btVector3 &pointInWorld, btScalar depth)
void setBodies(const btCollisionObject *body0, const btCollisionObject *body1)
The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes.
virtual void getVertex(int i, btVector3 &vtx) const =0
virtual int getNumVertices() const =0
bool queryPoint(const btVector3 &ptInSDF, btScalar &distOut, btVector3 &normal)
The btSphereShape implements an implicit sphere, centered around a local origin with radius.
Definition: btSphereShape.h:25
btScalar getRadius() const
Definition: btSphereShape.h:48
btSubsimplexConvexCast implements Gino van den Bergens' paper "Ray Casting against bteral Convex Obje...
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 (...
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btVector3 invXform(const btVector3 &inVec) const
Definition: btTransform.h:215
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
The btTriangleCallback provides a callback for each overlapping triangle when calling processAllTrian...
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
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
btVector3 & safeNormalize()
Definition: btVector3.h:286
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
btVoronoiSimplexSolver is an implementation of the closest point distance algorithm from a 1-4 points...
const btCollisionShape * getCollisionShape() const
const btCollisionObject * getCollisionObject() const
const btTransform & getWorldTransform() const
RayResult stores the closest result alternatively, add a callback method to decide about closest/all ...
Definition: btConvexCast.h:47
class btIDebugDraw * m_debugDraw
Definition: btDispatcher.h:58