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
17 #include "btConvexConcaveCollisionAlgorithm.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
btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)30 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
31 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
32 m_isSwapped(isSwapped),
33 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
34 {
35 }
36
~btConvexConcaveCollisionAlgorithm()37 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
38 {
39 }
40
getAllContactManifolds(btManifoldArray & manifoldArray)41 void btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray& manifoldArray)
42 {
43 if (m_btConvexTriangleCallback.m_manifoldPtr)
44 {
45 manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
46 }
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):
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 //
60 m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
61
62 clearCache();
63 }
64
~btConvexTriangleCallback()65 btConvexTriangleCallback::~btConvexTriangleCallback()
66 {
67 clearCache();
68 m_dispatcher->releaseManifold( m_manifoldPtr );
69
70 }
71
72
clearCache()73 void btConvexTriangleCallback::clearCache()
74 {
75 m_dispatcher->clearManifold(m_manifoldPtr);
76 }
77
78
processTriangle(btVector3 * triangle,int partId,int triangleIndex)79 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
80 partId, int triangleIndex)
81 {
82
83 if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
84 {
85 return;
86 }
87
88 //just for debugging purposes
89 //printf("triangle %d",m_triangleCount++);
90
91
92
93 btCollisionAlgorithmConstructionInfo ci;
94 ci.m_dispatcher1 = m_dispatcher;
95
96
97
98 #if 0
99
100 ///debug drawing of the overlapping triangles
101 if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
102 {
103 const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
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
112 if (m_convexBodyWrap->getCollisionShape()->isConvex())
113 {
114 btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
115 tm.setMargin(m_collisionMarginTriangle);
116
117
118 btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
119 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr);
120
121 const btCollisionObjectWrapper* tmpWrap = 0;
122
123 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
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
138 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
139 {
140 m_resultOut->setBody0Wrap(tmpWrap);
141 } else
142 {
143 m_resultOut->setBody1Wrap(tmpWrap);
144 }
145
146
147
148 colAlgo->~btCollisionAlgorithm();
149 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
150 }
151
152 }
153
154
155
setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo & dispatchInfo,const btCollisionObjectWrapper * convexBodyWrap,const btCollisionObjectWrapper * triBodyWrap,btManifoldResult * resultOut)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;
167 convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
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
clearCache()179 void btConvexConcaveCollisionAlgorithm::clearCache()
180 {
181 m_btConvexTriangleCallback.clearCache();
182
183 }
184
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)185 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
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
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)221 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
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 }
335