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 "btSoftBodyConcaveCollisionAlgorithm.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 "BulletCollision/CollisionShapes/btTetrahedronShape.h"
27 #include "BulletCollision/CollisionShapes/btConvexHullShape.h"
28 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
29
30
31 #include "LinearMath/btIDebugDraw.h"
32 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
33 #include "BulletSoftBody/btSoftBody.h"
34
35 #define BT_SOFTBODY_TRIANGLE_EXTRUSION btScalar(0.06)//make this configurable
36
btSoftBodyConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo & ci,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)37 btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
38 : btCollisionAlgorithm(ci),
39 m_isSwapped(isSwapped),
40 m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
41 {
42 }
43
44
45
~btSoftBodyConcaveCollisionAlgorithm()46 btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
47 {
48 }
49
50
51
btSoftBodyTriangleCallback(btDispatcher * dispatcher,const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,bool isSwapped)52 btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
53 m_dispatcher(dispatcher),
54 m_dispatchInfoPtr(0)
55 {
56 m_softBody = (isSwapped? (btSoftBody*)body1Wrap->getCollisionObject():(btSoftBody*)body0Wrap->getCollisionObject());
57 m_triBody = isSwapped? body0Wrap->getCollisionObject():body1Wrap->getCollisionObject();
58
59 //
60 // create the manifold from the dispatcher 'manifold pool'
61 //
62 // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody);
63
64 clearCache();
65 }
66
~btSoftBodyTriangleCallback()67 btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
68 {
69 clearCache();
70 // m_dispatcher->releaseManifold( m_manifoldPtr );
71
72 }
73
74
clearCache()75 void btSoftBodyTriangleCallback::clearCache()
76 {
77 for (int i=0;i<m_shapeCache.size();i++)
78 {
79 btTriIndex* tmp = m_shapeCache.getAtIndex(i);
80 btAssert(tmp);
81 btAssert(tmp->m_childShape);
82 m_softBody->getWorldInfo()->m_sparsesdf.RemoveReferences(tmp->m_childShape);//necessary?
83 delete tmp->m_childShape;
84 }
85 m_shapeCache.clear();
86 }
87
88
processTriangle(btVector3 * triangle,int partId,int triangleIndex)89 void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
90 {
91 //just for debugging purposes
92 //printf("triangle %d",m_triangleCount++);
93
94 btCollisionAlgorithmConstructionInfo ci;
95 ci.m_dispatcher1 = m_dispatcher;
96
97 ///debug drawing of the overlapping triangles
98 if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe))
99 {
100 btVector3 color(1,1,0);
101 const btTransform& tr = m_triBody->getWorldTransform();
102 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
103 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
104 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
105 }
106
107 btTriIndex triIndex(partId,triangleIndex,0);
108 btHashKey<btTriIndex> triKey(triIndex.getUid());
109
110
111 btTriIndex* shapeIndex = m_shapeCache[triKey];
112 if (shapeIndex)
113 {
114 btCollisionShape* tm = shapeIndex->m_childShape;
115 btAssert(tm);
116
117 //copy over user pointers to temporary shape
118 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
119
120 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
121 //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//??
122 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);
123
124 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
125
126 colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
127 colAlgo->~btCollisionAlgorithm();
128 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
129
130 return;
131 }
132
133 //aabb filter is already applied!
134
135 //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
136
137 // if (m_softBody->getCollisionShape()->getShapeType()==
138 {
139 // btVector3 other;
140 btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]);
141 normal.normalize();
142 normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION;
143 // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f;
144 // other+=normal*22.f;
145 btVector3 pts[6] = {triangle[0]+normal,
146 triangle[1]+normal,
147 triangle[2]+normal,
148 triangle[0]-normal,
149 triangle[1]-normal,
150 triangle[2]-normal};
151
152 btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6);
153
154
155 // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other);
156
157 //btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
158 // tm.setMargin(m_collisionMarginTriangle);
159
160 //copy over user pointers to temporary shape
161 tm->setUserPointer(m_triBody->getCollisionShape()->getUserPointer());
162
163
164 btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1);
165 btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//??
166
167 btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr);
168
169 colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut);
170 colAlgo->~btCollisionAlgorithm();
171 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
172
173 triIndex.m_childShape = tm;
174 m_shapeCache.insert(triKey,triIndex);
175
176 }
177
178
179
180 }
181
182
183
setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper * triBodyWrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)184 void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btCollisionObjectWrapper* triBodyWrap, const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
185 {
186 m_dispatchInfoPtr = &dispatchInfo;
187 m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
188 m_resultOut = resultOut;
189
190
191 btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax;
192 m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
193 btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
194 btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
195
196 btTransform softTransform;
197 softTransform.setIdentity();
198 softTransform.setOrigin(softBodyCenter);
199
200 btTransform convexInTriangleSpace;
201 convexInTriangleSpace = triBodyWrap->getWorldTransform().inverse() * softTransform;
202 btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
203 }
204
clearCache()205 void btSoftBodyConcaveCollisionAlgorithm::clearCache()
206 {
207 m_btSoftBodyTriangleCallback.clearCache();
208
209 }
210
processCollision(const btCollisionObjectWrapper * body0Wrap,const btCollisionObjectWrapper * body1Wrap,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)211 void btSoftBodyConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
212 {
213
214
215 //btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
216 const btCollisionObjectWrapper* triBody = m_isSwapped ? body0Wrap : body1Wrap;
217
218 if (triBody->getCollisionShape()->isConcave())
219 {
220
221
222 const btCollisionObject* triOb = triBody->getCollisionObject();
223 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triOb->getCollisionShape());
224
225 // if (convexBody->getCollisionShape()->isConvex())
226 {
227 btScalar collisionMarginTriangle = concaveShape->getMargin();
228
229 // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
230 m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,triBody,dispatchInfo,resultOut);
231
232
233 concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
234
235 // resultOut->refreshContactPoints();
236
237 }
238
239 }
240
241 }
242
243
calculateTimeOfImpact(btCollisionObject * body0,btCollisionObject * body1,const btDispatcherInfo & dispatchInfo,btManifoldResult * resultOut)244 btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
245 {
246 (void)resultOut;
247 (void)dispatchInfo;
248 btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
249 btCollisionObject* triBody = m_isSwapped ? body0 : body1;
250
251
252 //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
253
254 //only perform CCD above a certain threshold, this prevents blocking on the long run
255 //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
256 btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
257 if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
258 {
259 return btScalar(1.);
260 }
261
262 //const btVector3& from = convexbody->m_worldTransform.getOrigin();
263 //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
264 //todo: only do if the motion exceeds the 'radius'
265
266 btTransform triInv = triBody->getWorldTransform().inverse();
267 btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
268 btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
269
270 struct LocalTriangleSphereCastCallback : public btTriangleCallback
271 {
272 btTransform m_ccdSphereFromTrans;
273 btTransform m_ccdSphereToTrans;
274 btTransform m_meshTransform;
275
276 btScalar m_ccdSphereRadius;
277 btScalar m_hitFraction;
278
279
280 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
281 :m_ccdSphereFromTrans(from),
282 m_ccdSphereToTrans(to),
283 m_ccdSphereRadius(ccdSphereRadius),
284 m_hitFraction(hitFraction)
285 {
286 }
287
288
289 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
290 {
291 (void)partId;
292 (void)triangleIndex;
293 //do a swept sphere for now
294 btTransform ident;
295 ident.setIdentity();
296 btConvexCast::CastResult castResult;
297 castResult.m_fraction = m_hitFraction;
298 btSphereShape pointShape(m_ccdSphereRadius);
299 btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
300 btVoronoiSimplexSolver simplexSolver;
301 btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
302 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
303 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
304 //local space?
305
306 if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
307 ident,ident,castResult))
308 {
309 if (m_hitFraction > castResult.m_fraction)
310 m_hitFraction = castResult.m_fraction;
311 }
312
313 }
314
315 };
316
317
318
319
320
321 if (triBody->getCollisionShape()->isConcave())
322 {
323 btVector3 rayAabbMin = convexFromLocal.getOrigin();
324 rayAabbMin.setMin(convexToLocal.getOrigin());
325 btVector3 rayAabbMax = convexFromLocal.getOrigin();
326 rayAabbMax.setMax(convexToLocal.getOrigin());
327 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
328 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
329 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
330
331 btScalar curHitFraction = btScalar(1.); //is this available?
332 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
333 convexbody->getCcdSweptSphereRadius(),curHitFraction);
334
335 raycastCallback.m_hitFraction = convexbody->getHitFraction();
336
337 btCollisionObject* concavebody = triBody;
338
339 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
340
341 if (triangleMesh)
342 {
343 triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
344 }
345
346
347
348 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
349 {
350 convexbody->setHitFraction( raycastCallback.m_hitFraction);
351 return raycastCallback.m_hitFraction;
352 }
353 }
354
355 return btScalar(1.);
356
357 }
358