← Back to branch summary

~siretart/ubuntu/utopic/blender/libav10

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

  • Committer: Reinhard Tartler
  • Date: 2014-05-31 01:50:05 UTC
  • mfrom: (14.2.27 sid)
  • Revision ID: siretart@tauware.de-20140531015005-ml6druahuj82nsav
merge from debian

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
 
1
/*
 
2
Bullet Continuous Collision Detection and Physics Library
 
3
Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
 
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 "btMultiBodyDynamicsWorld.h"
 
17
#include "btMultiBodyConstraintSolver.h"
 
18
#include "btMultiBody.h"
 
19
#include "btMultiBodyLinkCollider.h"
 
20
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 
21
#include "LinearMath/btQuickprof.h"
 
22
#include "btMultiBodyConstraint.h"
 
23
 
 
24
        
 
25
 
 
26
 
 
27
void    btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask)
 
28
{
 
29
        m_multiBodies.push_back(body);
 
30
 
 
31
}
 
32
 
 
33
void    btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
 
34
{
 
35
        m_multiBodies.remove(body);
 
36
}
 
37
 
 
38
void    btMultiBodyDynamicsWorld::calculateSimulationIslands()
 
39
{
 
40
        BT_PROFILE("calculateSimulationIslands");
 
41
 
 
42
        getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
 
43
 
 
44
    {
 
45
        //merge islands based on speculative contact manifolds too
 
46
        for (int i=0;i<this->m_predictiveManifolds.size();i++)
 
47
        {
 
48
            btPersistentManifold* manifold = m_predictiveManifolds[i];
 
49
            
 
50
            const btCollisionObject* colObj0 = manifold->getBody0();
 
51
            const btCollisionObject* colObj1 = manifold->getBody1();
 
52
            
 
53
            if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 
54
                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 
55
            {
 
56
                                getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
 
57
            }
 
58
        }
 
59
    }
 
60
    
 
61
        {
 
62
                int i;
 
63
                int numConstraints = int(m_constraints.size());
 
64
                for (i=0;i< numConstraints ; i++ )
 
65
                {
 
66
                        btTypedConstraint* constraint = m_constraints[i];
 
67
                        if (constraint->isEnabled())
 
68
                        {
 
69
                                const btRigidBody* colObj0 = &constraint->getRigidBodyA();
 
70
                                const btRigidBody* colObj1 = &constraint->getRigidBodyB();
 
71
 
 
72
                                if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 
73
                                        ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
 
74
                                {
 
75
                                        getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag());
 
76
                                }
 
77
                        }
 
78
                }
 
79
        }
 
80
 
 
81
        //merge islands linked by Featherstone link colliders
 
82
        for (int i=0;i<m_multiBodies.size();i++)
 
83
        {
 
84
                btMultiBody* body = m_multiBodies[i];
 
85
                {
 
86
                        btMultiBodyLinkCollider* prev = body->getBaseCollider();
 
87
 
 
88
                        for (int b=0;b<body->getNumLinks();b++)
 
89
                        {
 
90
                                btMultiBodyLinkCollider* cur = body->getLink(b).m_collider;
 
91
                                
 
92
                                if (((cur) && (!(cur)->isStaticOrKinematicObject())) &&
 
93
                                        ((prev) && (!(prev)->isStaticOrKinematicObject())))
 
94
                                {
 
95
                                        int tagPrev = prev->getIslandTag();
 
96
                                        int tagCur = cur->getIslandTag();
 
97
                                        getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur);
 
98
                                }
 
99
                                if (cur && !cur->isStaticOrKinematicObject())
 
100
                                        prev = cur;
 
101
                                
 
102
                        }
 
103
                }
 
104
        }
 
105
 
 
106
        //merge islands linked by multibody constraints
 
107
        {
 
108
                for (int i=0;i<this->m_multiBodyConstraints.size();i++)
 
109
                {
 
110
                        btMultiBodyConstraint* c = m_multiBodyConstraints[i];
 
111
                        int tagA = c->getIslandIdA();
 
112
                        int tagB = c->getIslandIdB();
 
113
                        if (tagA>=0 && tagB>=0)
 
114
                                getSimulationIslandManager()->getUnionFind().unite(tagA, tagB);
 
115
                }
 
116
        }
 
117
 
 
118
        //Store the island id in each body
 
119
        getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
 
120
 
 
121
}
 
122
 
 
123
 
 
124
void    btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 
125
{
 
126
        BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState");
 
127
 
 
128
        
 
129
        
 
130
        for ( int i=0;i<m_multiBodies.size();i++)
 
131
        {
 
132
                btMultiBody* body = m_multiBodies[i];
 
133
                if (body)
 
134
                {
 
135
                        body->checkMotionAndSleepIfRequired(timeStep);
 
136
                        if (!body->isAwake())
 
137
                        {
 
138
                                btMultiBodyLinkCollider* col = body->getBaseCollider();
 
139
                                if (col && col->getActivationState() == ACTIVE_TAG)
 
140
                                {
 
141
                                        col->setActivationState( WANTS_DEACTIVATION);
 
142
                                        col->setDeactivationTime(0.f);
 
143
                                }
 
144
                                for (int b=0;b<body->getNumLinks();b++)
 
145
                                {
 
146
                                        btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
 
147
                                        if (col && col->getActivationState() == ACTIVE_TAG)
 
148
                                        {
 
149
                                                col->setActivationState( WANTS_DEACTIVATION);
 
150
                                                col->setDeactivationTime(0.f);
 
151
                                        }
 
152
                                }
 
153
                        } else
 
154
                        {
 
155
                                btMultiBodyLinkCollider* col = body->getBaseCollider();
 
156
                                if (col && col->getActivationState() != DISABLE_DEACTIVATION)
 
157
                                        col->setActivationState( ACTIVE_TAG );
 
158
 
 
159
                                for (int b=0;b<body->getNumLinks();b++)
 
160
                                {
 
161
                                        btMultiBodyLinkCollider* col = body->getLink(b).m_collider;
 
162
                                        if (col && col->getActivationState() != DISABLE_DEACTIVATION)
 
163
                                                col->setActivationState( ACTIVE_TAG );
 
164
                                }
 
165
                        }
 
166
 
 
167
                }
 
168
        }
 
169
 
 
170
        btDiscreteDynamicsWorld::updateActivationState(timeStep);
 
171
}
 
172
 
 
173
 
 
174
SIMD_FORCE_INLINE       int     btGetConstraintIslandId2(const btTypedConstraint* lhs)
 
175
{
 
176
        int islandId;
 
177
        
 
178
        const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
 
179
        const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
 
180
        islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
 
181
        return islandId;
 
182
 
 
183
}
 
184
 
 
185
 
 
186
class btSortConstraintOnIslandPredicate2
 
187
{
 
188
        public:
 
189
 
 
190
                bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const
 
191
                {
 
192
                        int rIslandId0,lIslandId0;
 
193
                        rIslandId0 = btGetConstraintIslandId2(rhs);
 
194
                        lIslandId0 = btGetConstraintIslandId2(lhs);
 
195
                        return lIslandId0 < rIslandId0;
 
196
                }
 
197
};
 
198
 
 
199
 
 
200
 
 
201
SIMD_FORCE_INLINE       int     btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs)
 
202
{
 
203
        int islandId;
 
204
        
 
205
        int islandTagA = lhs->getIslandIdA();
 
206
        int islandTagB = lhs->getIslandIdB();
 
207
        islandId= islandTagA>=0?islandTagA:islandTagB;
 
208
        return islandId;
 
209
 
 
210
}
 
211
 
 
212
 
 
213
class btSortMultiBodyConstraintOnIslandPredicate
 
214
{
 
215
        public:
 
216
 
 
217
                bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const
 
218
                {
 
219
                        int rIslandId0,lIslandId0;
 
220
                        rIslandId0 = btGetMultiBodyConstraintIslandId(rhs);
 
221
                        lIslandId0 = btGetMultiBodyConstraintIslandId(lhs);
 
222
                        return lIslandId0 < rIslandId0;
 
223
                }
 
224
};
 
225
 
 
226
struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 
227
{
 
228
        btContactSolverInfo*    m_solverInfo;
 
229
        btMultiBodyConstraintSolver*            m_solver;
 
230
        btMultiBodyConstraint**         m_multiBodySortedConstraints;
 
231
        int                                                     m_numMultiBodyConstraints;
 
232
        
 
233
        btTypedConstraint**             m_sortedConstraints;
 
234
        int                                             m_numConstraints;
 
235
        btIDebugDraw*                   m_debugDrawer;
 
236
        btDispatcher*                   m_dispatcher;
 
237
        
 
238
        btAlignedObjectArray<btCollisionObject*> m_bodies;
 
239
        btAlignedObjectArray<btPersistentManifold*> m_manifolds;
 
240
        btAlignedObjectArray<btTypedConstraint*> m_constraints;
 
241
        btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
 
242
 
 
243
 
 
244
        MultiBodyInplaceSolverIslandCallback(   btMultiBodyConstraintSolver*    solver,
 
245
                                                                        btDispatcher* dispatcher)
 
246
                :m_solverInfo(NULL),
 
247
                m_solver(solver),
 
248
                m_multiBodySortedConstraints(NULL),
 
249
                m_numConstraints(0),
 
250
                m_debugDrawer(NULL),
 
251
                m_dispatcher(dispatcher)
 
252
        {
 
253
 
 
254
        }
 
255
 
 
256
        MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other)
 
257
        {
 
258
                btAssert(0);
 
259
                (void)other;
 
260
                return *this;
 
261
        }
 
262
 
 
263
        SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints,  int     numMultiBodyConstraints,        btIDebugDraw* debugDrawer)
 
264
        {
 
265
                btAssert(solverInfo);
 
266
                m_solverInfo = solverInfo;
 
267
 
 
268
                m_multiBodySortedConstraints = sortedMultiBodyConstraints;
 
269
                m_numMultiBodyConstraints = numMultiBodyConstraints;
 
270
                m_sortedConstraints = sortedConstraints;
 
271
                m_numConstraints = numConstraints;
 
272
 
 
273
                m_debugDrawer = debugDrawer;
 
274
                m_bodies.resize (0);
 
275
                m_manifolds.resize (0);
 
276
                m_constraints.resize (0);
 
277
                m_multiBodyConstraints.resize(0);
 
278
        }
 
279
 
 
280
        
 
281
        virtual void    processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**   manifolds,int numManifolds, int islandId)
 
282
        {
 
283
                if (islandId<0)
 
284
                {
 
285
                        ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 
286
                        m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
 
287
                } else
 
288
                {
 
289
                                //also add all non-contact constraints/joints for this island
 
290
                        btTypedConstraint** startConstraint = 0;
 
291
                        btMultiBodyConstraint** startMultiBodyConstraint = 0;
 
292
 
 
293
                        int numCurConstraints = 0;
 
294
                        int numCurMultiBodyConstraints = 0;
 
295
 
 
296
                        int i;
 
297
                        
 
298
                        //find the first constraint for this island
 
299
 
 
300
                        for (i=0;i<m_numConstraints;i++)
 
301
                        {
 
302
                                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
 
303
                                {
 
304
                                        startConstraint = &m_sortedConstraints[i];
 
305
                                        break;
 
306
                                }
 
307
                        }
 
308
                        //count the number of constraints in this island
 
309
                        for (;i<m_numConstraints;i++)
 
310
                        {
 
311
                                if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId)
 
312
                                {
 
313
                                        numCurConstraints++;
 
314
                                }
 
315
                        }
 
316
 
 
317
                        for (i=0;i<m_numMultiBodyConstraints;i++)
 
318
                        {
 
319
                                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
 
320
                                {
 
321
                                        
 
322
                                        startMultiBodyConstraint = &m_multiBodySortedConstraints[i];
 
323
                                        break;
 
324
                                }
 
325
                        }
 
326
                        //count the number of multi body constraints in this island
 
327
                        for (;i<m_numMultiBodyConstraints;i++)
 
328
                        {
 
329
                                if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId)
 
330
                                {
 
331
                                        numCurMultiBodyConstraints++;
 
332
                                }
 
333
                        }
 
334
 
 
335
                        if (m_solverInfo->m_minimumSolverBatchSize<=1)
 
336
                        {
 
337
                                m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
 
338
                        } else
 
339
                        {
 
340
                                
 
341
                                for (i=0;i<numBodies;i++)
 
342
                                        m_bodies.push_back(bodies[i]);
 
343
                                for (i=0;i<numManifolds;i++)
 
344
                                        m_manifolds.push_back(manifolds[i]);
 
345
                                for (i=0;i<numCurConstraints;i++)
 
346
                                        m_constraints.push_back(startConstraint[i]);
 
347
                                
 
348
                                for (i=0;i<numCurMultiBodyConstraints;i++)
 
349
                                        m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]);
 
350
                                
 
351
                                if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize)
 
352
                                {
 
353
                                        processConstraints();
 
354
                                } else
 
355
                                {
 
356
                                        //printf("deferred\n");
 
357
                                }
 
358
                        }
 
359
                }
 
360
        }
 
361
        void    processConstraints()
 
362
        {
 
363
 
 
364
                btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0;
 
365
                btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0;
 
366
                btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0;
 
367
                btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0;
 
368
        
 
369
                m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher);
 
370
                m_bodies.resize(0);
 
371
                m_manifolds.resize(0);
 
372
                m_constraints.resize(0);
 
373
                m_multiBodyConstraints.resize(0);
 
374
        }
 
375
 
 
376
};
 
377
 
 
378
 
 
379
 
 
380
btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration)
 
381
        :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
 
382
        m_multiBodyConstraintSolver(constraintSolver)
 
383
{
 
384
        //split impulse is not yet supported for Featherstone hierarchies
 
385
        getSolverInfo().m_splitImpulse = false;
 
386
        getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS;
 
387
        m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher);
 
388
}
 
389
 
 
390
btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
 
391
{
 
392
        delete m_solverMultiBodyIslandCallback;
 
393
}
 
394
 
 
395
 
 
396
 
 
397
 
 
398
void    btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 
399
{
 
400
 
 
401
        btAlignedObjectArray<btScalar> scratch_r;
 
402
        btAlignedObjectArray<btVector3> scratch_v;
 
403
        btAlignedObjectArray<btMatrix3x3> scratch_m;
 
404
 
 
405
 
 
406
        BT_PROFILE("solveConstraints");
 
407
        
 
408
        m_sortedConstraints.resize( m_constraints.size());
 
409
        int i; 
 
410
        for (i=0;i<getNumConstraints();i++)
 
411
        {
 
412
                m_sortedConstraints[i] = m_constraints[i];
 
413
        }
 
414
        m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
 
415
        btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
 
416
 
 
417
        m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
 
418
        for (i=0;i<m_multiBodyConstraints.size();i++)
 
419
        {
 
420
                m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
 
421
        }
 
422
        m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
 
423
 
 
424
        btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ?  &m_sortedMultiBodyConstraints[0] : 0;
 
425
        
 
426
 
 
427
        m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer());
 
428
        m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
 
429
        
 
430
        /// solve all the constraints for this island
 
431
        m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback);
 
432
 
 
433
 
 
434
        {
 
435
                BT_PROFILE("btMultiBody addForce and stepVelocities");
 
436
                for (int i=0;i<this->m_multiBodies.size();i++)
 
437
                {
 
438
                        btMultiBody* bod = m_multiBodies[i];
 
439
 
 
440
                        bool isSleeping = false;
 
441
                        
 
442
                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 
443
                        {
 
444
                                isSleeping = true;
 
445
                        } 
 
446
                        for (int b=0;b<bod->getNumLinks();b++)
 
447
                        {
 
448
                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
 
449
                                        isSleeping = true;
 
450
                        } 
 
451
 
 
452
                        if (!isSleeping)
 
453
                        {
 
454
                                scratch_r.resize(bod->getNumLinks()+1);
 
455
                                scratch_v.resize(bod->getNumLinks()+1);
 
456
                                scratch_m.resize(bod->getNumLinks()+1);
 
457
 
 
458
                                bod->clearForcesAndTorques();
 
459
                                bod->addBaseForce(m_gravity * bod->getBaseMass());
 
460
 
 
461
                                for (int j = 0; j < bod->getNumLinks(); ++j) 
 
462
                                {
 
463
                                        bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 
464
                                }
 
465
 
 
466
                                bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m);
 
467
                        }
 
468
                }
 
469
        }
 
470
 
 
471
        m_solverMultiBodyIslandCallback->processConstraints();
 
472
        
 
473
        m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
 
474
 
 
475
}
 
476
 
 
477
void    btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 
478
{
 
479
        btDiscreteDynamicsWorld::integrateTransforms(timeStep);
 
480
 
 
481
        {
 
482
                BT_PROFILE("btMultiBody stepPositions");
 
483
                //integrate and update the Featherstone hierarchies
 
484
                btAlignedObjectArray<btQuaternion> world_to_local;
 
485
                btAlignedObjectArray<btVector3> local_origin;
 
486
 
 
487
                for (int b=0;b<m_multiBodies.size();b++)
 
488
                {
 
489
                        btMultiBody* bod = m_multiBodies[b];
 
490
                        bool isSleeping = false;
 
491
                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 
492
                        {
 
493
                                isSleeping = true;
 
494
                        } 
 
495
                        for (int b=0;b<bod->getNumLinks();b++)
 
496
                        {
 
497
                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
 
498
                                        isSleeping = true;
 
499
                        }
 
500
 
 
501
 
 
502
                        if (!isSleeping)
 
503
                        {
 
504
                                int nLinks = bod->getNumLinks();
 
505
 
 
506
                                ///base + num links
 
507
                                world_to_local.resize(nLinks+1);
 
508
                                local_origin.resize(nLinks+1);
 
509
 
 
510
                                bod->stepPositions(timeStep);
 
511
 
 
512
         
 
513
 
 
514
                                world_to_local[0] = bod->getWorldToBaseRot();
 
515
                                local_origin[0] = bod->getBasePos();
 
516
 
 
517
                                if (bod->getBaseCollider())
 
518
                                {
 
519
                                        btVector3 posr = local_origin[0];
 
520
                                        float pos[4]={posr.x(),posr.y(),posr.z(),1};
 
521
                                        float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
 
522
                                        btTransform tr;
 
523
                                        tr.setIdentity();
 
524
                                        tr.setOrigin(posr);
 
525
                                        tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
 
526
 
 
527
                                        bod->getBaseCollider()->setWorldTransform(tr);
 
528
 
 
529
                                }
 
530
      
 
531
                                for (int k=0;k<bod->getNumLinks();k++)
 
532
                                {
 
533
                                        const int parent = bod->getParent(k);
 
534
                                        world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
 
535
                                        local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
 
536
                                }
 
537
 
 
538
 
 
539
                                for (int m=0;m<bod->getNumLinks();m++)
 
540
                                {
 
541
                                        btMultiBodyLinkCollider* col = bod->getLink(m).m_collider;
 
542
                                        if (col)
 
543
                                        {
 
544
                                                int link = col->m_link;
 
545
                                                btAssert(link == m);
 
546
 
 
547
                                                int index = link+1;
 
548
 
 
549
                                                btVector3 posr = local_origin[index];
 
550
                                                float pos[4]={posr.x(),posr.y(),posr.z(),1};
 
551
                                                float quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
 
552
                                                btTransform tr;
 
553
                                                tr.setIdentity();
 
554
                                                tr.setOrigin(posr);
 
555
                                                tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
 
556
 
 
557
                                                col->setWorldTransform(tr);
 
558
                                        }
 
559
                                }
 
560
                        } else
 
561
                        {
 
562
                                bod->clearVelocities();
 
563
                        }
 
564
                }
 
565
        }
 
566
}
 
567
 
 
568
 
 
569
 
 
570
void    btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint)
 
571
{
 
572
        m_multiBodyConstraints.push_back(constraint);
 
573
}
 
574
 
 
575
void    btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint)
 
576
{
 
577
        m_multiBodyConstraints.remove(constraint);
 
578
}