Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/bullet/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @ 2119

Last change on this file since 2119 was 2119, checked in by rgrieder, 16 years ago

Merged physics branch into physics_new branch.

  • Property svn:eol-style set to native
File size: 12.6 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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
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
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
37btSoftBodyConcaveCollisionAlgorithm::btSoftBodyConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped)
38: btCollisionAlgorithm(ci),
39m_isSwapped(isSwapped),
40m_btSoftBodyTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped)
41{
42}
43
44
45
46btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm()
47{
48}
49
50
51
52btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher*  dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped):
53          m_dispatcher(dispatcher),
54        m_dispatchInfoPtr(0)
55{
56        m_softBody = (btSoftBody*) (isSwapped? body1:body0);
57        m_triBody = isSwapped? body0:body1;
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
67btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback()
68{
69        clearCache();
70//      m_dispatcher->releaseManifold( m_manifoldPtr );
71 
72}
73 
74
75void    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
89void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex)
90{
91        //just for debugging purposes
92        //printf("triangle %d",m_triangleCount++);
93        btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody);
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() > 0)
99        {
100                btVector3 color(255,255,0);
101                btTransform& tr = ob->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(ob->getRootCollisionShape()->getUserPointer());
119               
120                btCollisionShape* tmpShape = ob->getCollisionShape();
121                ob->internalSetTemporaryCollisionShape( tm );
122               
123
124                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
125               
126                colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
127                colAlgo->~btCollisionAlgorithm();
128                ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
129                ob->internalSetTemporaryCollisionShape( tmpShape);
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(ob->getRootCollisionShape()->getUserPointer());
162               
163                btCollisionShape* tmpShape = ob->getCollisionShape();
164                ob->internalSetTemporaryCollisionShape( tm );
165               
166
167                btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr);
168                ///this should use the btDispatcher, so the actual registered algorithm is used
169                //              btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
170
171                //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex);
172        //      cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
173//              cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
174                colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
175                colAlgo->~btCollisionAlgorithm();
176                ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
177               
178               
179                ob->internalSetTemporaryCollisionShape( tmpShape );
180                triIndex.m_childShape = tm;
181                m_shapeCache.insert(triKey,triIndex);
182
183        }
184
185       
186
187}
188
189
190
191void    btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
192{
193        m_dispatchInfoPtr = &dispatchInfo;
194        m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION);
195        m_resultOut = resultOut;
196
197       
198        btVector3       aabbWorldSpaceMin,aabbWorldSpaceMax;
199        m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax);
200        btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5);
201        btVector3 softBodyCenter = (aabbWorldSpaceMax+aabbWorldSpaceMin)*btScalar(0.5);
202
203        btTransform softTransform;
204        softTransform.setIdentity();
205        softTransform.setOrigin(softBodyCenter);
206
207        btTransform convexInTriangleSpace;
208        convexInTriangleSpace = m_triBody->getWorldTransform().inverse() * softTransform;
209        btTransformAabb(halfExtents,m_collisionMarginTriangle,convexInTriangleSpace,m_aabbMin,m_aabbMax);
210}
211
212void btSoftBodyConcaveCollisionAlgorithm::clearCache()
213{
214        m_btSoftBodyTriangleCallback.clearCache();
215
216}
217
218void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
219{
220       
221       
222        btCollisionObject* convexBody = m_isSwapped ? body1 : body0;
223        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
224
225        if (triBody->getCollisionShape()->isConcave())
226        {
227
228
229                btCollisionObject*      triOb = triBody;
230                btConcaveShape* concaveShape = static_cast<btConcaveShape*>( triOb->getCollisionShape());
231               
232        //      if (convexBody->getCollisionShape()->isConvex())
233                {
234                        btScalar collisionMarginTriangle = concaveShape->getMargin();
235                                       
236//                      resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
237                        m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut);
238
239                        //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
240                        //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr);
241
242//                      m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody);
243
244
245                        concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax());
246                       
247                //      resultOut->refreshContactPoints();
248       
249                }
250       
251        }
252
253}
254
255
256btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
257{
258        (void)resultOut;
259        (void)dispatchInfo;
260        btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
261        btCollisionObject* triBody = m_isSwapped ? body0 : body1;
262
263
264        //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
265
266        //only perform CCD above a certain threshold, this prevents blocking on the long run
267        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
268        btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
269        if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
270        {
271                return btScalar(1.);
272        }
273
274        //const btVector3& from = convexbody->m_worldTransform.getOrigin();
275        //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
276        //todo: only do if the motion exceeds the 'radius'
277
278        btTransform triInv = triBody->getWorldTransform().inverse();
279        btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
280        btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
281
282        struct LocalTriangleSphereCastCallback  : public btTriangleCallback
283        {
284                btTransform m_ccdSphereFromTrans;
285                btTransform m_ccdSphereToTrans;
286                btTransform     m_meshTransform;
287
288                btScalar        m_ccdSphereRadius;
289                btScalar        m_hitFraction;
290       
291
292                LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
293                        :m_ccdSphereFromTrans(from),
294                        m_ccdSphereToTrans(to),
295                        m_ccdSphereRadius(ccdSphereRadius),
296                        m_hitFraction(hitFraction)
297                {                       
298                }
299               
300               
301                virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
302                {
303                        (void)partId;
304                        (void)triangleIndex;
305                        //do a swept sphere for now
306                        btTransform ident;
307                        ident.setIdentity();
308                        btConvexCast::CastResult castResult;
309                        castResult.m_fraction = m_hitFraction;
310                        btSphereShape   pointShape(m_ccdSphereRadius);
311                        btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
312                        btVoronoiSimplexSolver  simplexSolver;
313                        btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
314                        //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
315                        //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
316                        //local space?
317
318                        if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
319                                ident,ident,castResult))
320                        {
321                                if (m_hitFraction > castResult.m_fraction)
322                                        m_hitFraction = castResult.m_fraction;
323                        }
324
325                }
326
327        };
328
329
330       
331
332       
333        if (triBody->getCollisionShape()->isConcave())
334        {
335                btVector3 rayAabbMin = convexFromLocal.getOrigin();
336                rayAabbMin.setMin(convexToLocal.getOrigin());
337                btVector3 rayAabbMax = convexFromLocal.getOrigin();
338                rayAabbMax.setMax(convexToLocal.getOrigin());
339                btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
340                rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
341                rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
342
343                btScalar curHitFraction = btScalar(1.); //is this available?
344                LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
345                        convexbody->getCcdSweptSphereRadius(),curHitFraction);
346
347                raycastCallback.m_hitFraction = convexbody->getHitFraction();
348
349                btCollisionObject* concavebody = triBody;
350
351                btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
352               
353                if (triangleMesh)
354                {
355                        triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
356                }
357       
358
359
360                if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
361                {
362                        convexbody->setHitFraction( raycastCallback.m_hitFraction);
363                        return raycastCallback.m_hitFraction;
364                }
365        }
366
367        return btScalar(1.);
368
369}
Note: See TracBrowser for help on using the repository browser.