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 #include "btConvexConcaveCollisionAlgorithm.h"
17 #include "LinearMath/btQuickprof.h"
18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
19 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
20 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
21 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
22 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
23 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
24 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
25 #include "BulletCollision/CollisionShapes/btSphereShape.h"
26 #include "LinearMath/btIDebugDraw.h"
27 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
28 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
29 #include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
30
btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)31 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
32 : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
33 m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped),
34 m_isSwapped(isSwapped)
35 {
36 }
37
~btConvexConcaveCollisionAlgorithm()38 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
39 {
40 }
41
getAllContactManifolds(btManifoldArray & manifoldArray)42 void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
43 {
44 if (m_btConvexTriangleCallback.m_manifoldPtr)
45 {
46 manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
47 }
48 }
49
btConvexTriangleCallback(btDispatcher * dispatcher,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)50 btConvexTriangleCallback::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 //
59 m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject());
60
61 clearCache();
62 }
63
~btConvexTriangleCallback()64 btConvexTriangleCallback::~btConvexTriangleCallback()
65 {
66 clearCache();
67 m_dispatcher->releaseManifold(m_manifoldPtr);
68 }
69
clearCache()70 void btConvexTriangleCallback::clearCache()
71 {
72 m_dispatcher->clearManifold(m_manifoldPtr);
73 }
74
processTriangle(btVector3 * triangle,int partId,int triangleIndex)75 void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
76 {
77 BT_PROFILE("btConvexTriangleCallback::processTriangle");
78
79 if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
80 {
81 return;
82 }
83
84 //just for debugging purposes
85 //printf("triangle %d",m_triangleCount++);
86
87 btCollisionAlgorithmConstructionInfo ci;
88 ci.m_dispatcher1 = m_dispatcher;
89
90 #if 0
91
92 ///debug drawing of the overlapping triangles
93 if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
94 {
95 const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
96 btVector3 color(1,1,0);
97 btTransform& tr = ob->getWorldTransform();
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
104 if (m_convexBodyWrap->getCollisionShape()->isConvex())
105 {
106 btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
107 tm.setMargin(m_collisionMarginTriangle);
108
109 btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex); //correct transform?
110 btCollisionAlgorithm* colAlgo = 0;
111
112 if (m_resultOut->m_closestPointDistanceThreshold > 0)
113 {
114 colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
115 }
116 else
117 {
118 colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
119 }
120 const btCollisionObjectWrapper* tmpWrap = 0;
121
122 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
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
135 colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
136
137 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
138 {
139 m_resultOut->setBody0Wrap(tmpWrap);
140 }
141 else
142 {
143 m_resultOut->setBody1Wrap(tmpWrap);
144 }
145
146 colAlgo->~btCollisionAlgorithm();
147 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
148 }
149 }
150
setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo & dispatchInfo,const btCollisionObjectWrapper * convexBodyWrap,const btCollisionObjectWrapper * triBodyWrap,btManifoldResult * resultOut)151 void 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;
162 convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
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
clearCache()174 void btConvexConcaveCollisionAlgorithm::clearCache()
175 {
176 m_btConvexTriangleCallback.clearCache();
177 }
178
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)179 void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
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();
194 btAlignedObjectArray<btVector3> queryVertices;
195
196 if (convex->isPolyhedral())
197 {
198 btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
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 {
216 resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
217 //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
218
219 btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
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
257 resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
258 m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
259
260 m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
261
262 concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
263
264 resultOut->refreshContactPoints();
265
266 m_btConvexTriangleCallback.clearWrapperData();
267 }
268 }
269 }
270 }
271
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)272 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
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 }
375