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
41
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
69
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#ifdef BT_ENABLE_CONVEX_CONCAVE_EARLY_OUT
107 //todo: check this issue https://github.com/bulletphysics/bullet3/issues/4263
108 //an early out optimisation if the object is separated from the triangle
109 //projected on the triangle normal)
110 {
111 const btVector3 v0 = m_triBodyWrap->getWorldTransform()*triangle[0];
112 const btVector3 v1 = m_triBodyWrap->getWorldTransform()*triangle[1];
113 const btVector3 v2 = m_triBodyWrap->getWorldTransform()*triangle[2];
114
115 btVector3 triangle_normal_world = ( v1 - v0).cross(v2 - v0);
116 triangle_normal_world.normalize();
117
119
120 btVector3 localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world);
121 btVector3 worldPt = m_convexBodyWrap->getWorldTransform()*localPt;
122 //now check if this is fully on one side of the triangle
123 btScalar proj_distPt = triangle_normal_world.dot(worldPt);
124 btScalar proj_distTr = triangle_normal_world.dot(v0);
126 btScalar dist = proj_distTr - proj_distPt;
127 if (dist > contact_threshold)
128 return;
129
130 //also check the other side of the triangle
131 triangle_normal_world*=-1;
132
133 localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world);
134 worldPt = m_convexBodyWrap->getWorldTransform()*localPt;
135 //now check if this is fully on one side of the triangle
136 proj_distPt = triangle_normal_world.dot(worldPt);
137 proj_distTr = triangle_normal_world.dot(v0);
138
139 dist = proj_distTr - proj_distPt;
140 if (dist > contact_threshold)
141 return;
142 }
143#endif //BT_ENABLE_CONVEX_CONCAVE_EARLY_OUT
144
145 btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
147
148 btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform?
149 btCollisionAlgorithm* colAlgo = 0;
150
152 {
154 }
155 else
156 {
158 }
159 const btCollisionObjectWrapper* tmpWrap = 0;
160
162 {
163 tmpWrap = m_resultOut->getBody0Wrap();
164 m_resultOut->setBody0Wrap(&triObWrap);
165 m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
166 }
167 else
168 {
169 tmpWrap = m_resultOut->getBody1Wrap();
170 m_resultOut->setBody1Wrap(&triObWrap);
171 m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
172 }
173
174 {
175 BT_PROFILE("processCollision (GJK?)");
177 }
178
180 {
181 m_resultOut->setBody0Wrap(tmpWrap);
182 }
183 else
184 {
185 m_resultOut->setBody1Wrap(tmpWrap);
186 }
187
188 colAlgo->~btCollisionAlgorithm();
190 }
191}
192
193void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
194{
195 m_convexBodyWrap = convexBodyWrap;
196 m_triBodyWrap = triBodyWrap;
197
198 m_dispatchInfoPtr = &dispatchInfo;
199 m_collisionMarginTriangle = collisionMarginTriangle;
200 m_resultOut = resultOut;
201
202 //recalc aabbs
203 btTransform convexInTriangleSpace;
205 const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
206 //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
207 convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
208 btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
209
210 btVector3 extra(extraMargin, extraMargin, extraMargin);
211
212 m_aabbMax += extra;
213 m_aabbMin -= extra;
214}
215
220
222{
223 BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
224
225 const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
226 const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
227
228 if (triBodyWrap->getCollisionShape()->isConcave())
229 {
230 if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
231 {
232 btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
233 if (convexBodyWrap->getCollisionShape()->isConvex())
234 {
235 btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
237
238 if (convex->isPolyhedral())
239 {
241 for (int v = 0; v < poly->getNumVertices(); v++)
242 {
243 btVector3 vtx;
244 poly->getVertex(v, vtx);
245 queryVertices.push_back(vtx);
246 }
247 }
248 btScalar maxDist = SIMD_EPSILON;
249
250 if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
251 {
252 queryVertices.push_back(btVector3(0, 0, 0));
253 btSphereShape* sphere = (btSphereShape*)convex;
254 maxDist = sphere->getRadius() + SIMD_EPSILON;
255 }
256 if (queryVertices.size())
257 {
259 //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
260
262 for (int v = 0; v < queryVertices.size(); v++)
263 {
264 const btVector3& vtx = queryVertices[v];
265 btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx;
266 btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
267
268 btVector3 normalLocal;
269 btScalar dist;
270 if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
271 {
272 if (dist <= maxDist)
273 {
274 normalLocal.safeNormalize();
275 btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal;
276
277 if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
278 {
279 btSphereShape* sphere = (btSphereShape*)convex;
280 dist -= sphere->getRadius();
281 vtxWorldSpace -= sphere->getRadius() * normal;
282 }
283 resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist);
284 }
285 }
286 }
287 resultOut->refreshContactPoints();
288 }
289 }
290 }
291 else
292 {
293 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape());
294
295 if (convexBodyWrap->getCollisionShape()->isConvex())
296 {
297 btScalar collisionMarginTriangle = concaveShape->getMargin();
298
300 m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
301
303
305
306 resultOut->refreshContactPoints();
307
309 }
310 }
311 }
312}
313
315{
316 (void)resultOut;
317 (void)dispatchInfo;
318 btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
319 btCollisionObject* triBody = m_isSwapped ? body0 : body1;
320
321 //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
322
323 //only perform CCD above a certain threshold, this prevents blocking on the long run
324 //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
325 btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
326 if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
327 {
328 return btScalar(1.);
329 }
330
331 //const btVector3& from = convexbody->m_worldTransform.getOrigin();
332 //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
333 //todo: only do if the motion exceeds the 'radius'
334
335 btTransform triInv = triBody->getWorldTransform().inverse();
336 btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
337 btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
338
339 struct LocalTriangleSphereCastCallback : public btTriangleCallback
340 {
341 btTransform m_ccdSphereFromTrans;
342 btTransform m_ccdSphereToTrans;
343 btTransform m_meshTransform;
344
345 btScalar m_ccdSphereRadius;
346 btScalar m_hitFraction;
347
348 LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
349 : m_ccdSphereFromTrans(from),
350 m_ccdSphereToTrans(to),
351 m_ccdSphereRadius(ccdSphereRadius),
352 m_hitFraction(hitFraction)
353 {
354 }
355
356 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
357 {
358 BT_PROFILE("processTriangle");
359 (void)partId;
360 (void)triangleIndex;
361 //do a swept sphere for now
362 btTransform ident;
363 ident.setIdentity();
364 btConvexCast::CastResult castResult;
365 castResult.m_fraction = m_hitFraction;
366 btSphereShape pointShape(m_ccdSphereRadius);
367 btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
368 btVoronoiSimplexSolver simplexSolver;
369 btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
370 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
371 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
372 //local space?
373
374 if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
375 ident, ident, castResult))
376 {
377 if (m_hitFraction > castResult.m_fraction)
378 m_hitFraction = castResult.m_fraction;
379 }
380 }
381 };
382
383 if (triBody->getCollisionShape()->isConcave())
384 {
385 btVector3 rayAabbMin = convexFromLocal.getOrigin();
386 rayAabbMin.setMin(convexToLocal.getOrigin());
387 btVector3 rayAabbMax = convexFromLocal.getOrigin();
388 rayAabbMax.setMax(convexToLocal.getOrigin());
389 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
390 rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
391 rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
392
393 btScalar curHitFraction = btScalar(1.); //is this available?
394 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
395 convexbody->getCcdSweptSphereRadius(), curHitFraction);
396
397 raycastCallback.m_hitFraction = convexbody->getHitFraction();
398
399 btCollisionObject* concavebody = triBody;
400
401 btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
402
403 if (triangleMesh)
404 {
405 triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
406 }
407
408 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
409 {
410 convexbody->setHitFraction(raycastCallback.m_hitFraction);
411 return raycastCallback.m_hitFraction;
412 }
413 }
414
415 return btScalar(1.);
416}
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
@ BT_CONTACT_POINT_ALGORITHMS
#define BT_PROFILE(name)
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...
virtual btVector3 localGetSupportingVertex(const btVector3 &vec) const =0
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 btCollisionObjectWrapper * m_convexBodyWrap
const btCollisionObjectWrapper * m_triBodyWrap
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
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)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
void setBodies(const btCollisionObject *body0, const btCollisionObject *body1)
btScalar getContactBreakingThreshold() const
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.
btScalar getRadius() const
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.
btVector3 invXform(const btVector3 &inVec) const
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
void setIdentity()
Set this transformation to the identity.
btVector3 & getOrigin()
Return the origin vector translation.
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
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition btVector3.h:229
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
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition btVector3.h:303
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 ...
class btIDebugDraw * m_debugDraw