← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

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