Silence some annoying warnings when doing full build with strict flags
[blender.git] / extern / bullet2 / src / BulletDynamics / Featherstone / btMultiBody.cpp
1 /*
2  * PURPOSE:
3  *   Class representing an articulated rigid body. Stores the body's
4  *   current state, allows forces and torques to be set, handles
5  *   timestepping and implements Featherstone's algorithm.
6  *   
7  * COPYRIGHT:
8  *   Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
9  *   Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10  *   Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
11
12  This software is provided 'as-is', without any express or implied warranty.
13  In no event will the authors be held liable for any damages arising from the use of this software.
14  Permission is granted to anyone to use this software for any purpose,
15  including commercial applications, and to alter it and redistribute it freely,
16  subject to the following restrictions:
17  
18  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.
19  2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
20  3. This notice may not be removed or altered from any source distribution.
21  
22  */
23
24
25 #include "btMultiBody.h"
26 #include "btMultiBodyLink.h"
27 #include "btMultiBodyLinkCollider.h"
28 #include "btMultiBodyJointFeedback.h"
29 #include "LinearMath/btTransformUtil.h"
30 #include "LinearMath/btSerializer.h"
31 // #define INCLUDE_GYRO_TERM 
32
33 ///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
34 bool gJointFeedbackInWorldSpace = false;
35 bool gJointFeedbackInJointFrame = false;
36
37 namespace {
38     const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
39     const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
40 }
41
42 namespace {
43     void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
44                           const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
45                           const btVector3 &top_in,       // top part of input vector
46                           const btVector3 &bottom_in,    // bottom part of input vector
47                           btVector3 &top_out,         // top part of output vector
48                           btVector3 &bottom_out)      // bottom part of output vector
49     {
50         top_out = rotation_matrix * top_in;
51         bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
52     }
53
54 /*
55     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
56                                  const btVector3 &displacement,
57                                  const btVector3 &top_in,
58                                  const btVector3 &bottom_in,
59                                  btVector3 &top_out,
60                                  btVector3 &bottom_out)
61     {
62         top_out = rotation_matrix.transpose() * top_in;
63         bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));            
64     }
65
66     btScalar SpatialDotProduct(const btVector3 &a_top,
67                             const btVector3 &a_bottom,
68                             const btVector3 &b_top,
69                             const btVector3 &b_bottom)
70     {
71         return a_bottom.dot(b_top) + a_top.dot(b_bottom);
72     }
73
74         void SpatialCrossProduct(const btVector3 &a_top,
75                             const btVector3 &a_bottom,
76                             const btVector3 &b_top,
77                             const btVector3 &b_bottom,
78                                                         btVector3 &top_out,
79                                                         btVector3 &bottom_out)
80         {
81                 top_out = a_top.cross(b_top);
82                 bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
83         }
84 */
85 }
86
87
88 //
89 // Implementation of class btMultiBody
90 //
91
92 btMultiBody::btMultiBody(int n_links,
93                      btScalar mass,
94                      const btVector3 &inertia,
95                      bool fixedBase,
96                      bool canSleep,
97                      bool /*deprecatedUseMultiDof*/)
98     : 
99         m_baseCollider(0),
100                 m_baseName(0),
101         m_basePos(0,0,0),
102         m_baseQuat(0, 0, 0, 1),
103       m_baseMass(mass),
104       m_baseInertia(inertia),
105     
106                 m_fixedBase(fixedBase),
107                 m_awake(true),
108                 m_canSleep(canSleep),
109                 m_sleepTimer(0),
110                 
111                 m_linearDamping(0.04f),
112                 m_angularDamping(0.04f),
113                 m_useGyroTerm(true),
114                         m_maxAppliedImpulse(1000.f),
115                 m_maxCoordinateVelocity(100.f),
116                         m_hasSelfCollision(true),               
117                 __posUpdated(false),
118                         m_dofCount(0),
119                 m_posVarCnt(0),
120                 m_useRK4(false),        
121                 m_useGlobalVelocities(false),
122                 m_internalNeedsJointFeedback(false)
123 {
124         m_links.resize(n_links);
125         m_matrixBuf.resize(n_links + 1);
126
127
128     m_baseForce.setValue(0, 0, 0);
129     m_baseTorque.setValue(0, 0, 0);
130 }
131
132 btMultiBody::~btMultiBody()
133 {
134 }
135
136 void btMultiBody::setupFixed(int i,
137                                                    btScalar mass,
138                                                    const btVector3 &inertia,
139                                                    int parent,
140                                                    const btQuaternion &rotParentToThis,
141                                                    const btVector3 &parentComToThisPivotOffset,
142                            const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
143 {
144         
145         m_links[i].m_mass = mass;
146     m_links[i].m_inertiaLocal = inertia;
147     m_links[i].m_parent = parent;
148     m_links[i].m_zeroRotParentToThis = rotParentToThis;
149         m_links[i].m_dVector = thisPivotToThisComOffset;
150     m_links[i].m_eVector = parentComToThisPivotOffset;    
151
152         m_links[i].m_jointType = btMultibodyLink::eFixed;
153         m_links[i].m_dofCount = 0;
154         m_links[i].m_posVarCount = 0;
155
156         m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
157         
158         m_links[i].updateCacheMultiDof();
159
160         updateLinksDofOffsets();
161
162 }
163
164
165 void btMultiBody::setupPrismatic(int i,
166                                btScalar mass,
167                                const btVector3 &inertia,
168                                int parent,
169                                const btQuaternion &rotParentToThis,
170                                const btVector3 &jointAxis,
171                                const btVector3 &parentComToThisPivotOffset,
172                                                            const btVector3 &thisPivotToThisComOffset,
173                                                            bool disableParentCollision)
174 {
175         m_dofCount += 1;
176         m_posVarCnt += 1;
177         
178     m_links[i].m_mass = mass;
179     m_links[i].m_inertiaLocal = inertia;
180     m_links[i].m_parent = parent;
181     m_links[i].m_zeroRotParentToThis = rotParentToThis;
182     m_links[i].setAxisTop(0, 0., 0., 0.);
183     m_links[i].setAxisBottom(0, jointAxis);
184     m_links[i].m_eVector = parentComToThisPivotOffset;
185         m_links[i].m_dVector = thisPivotToThisComOffset;
186     m_links[i].m_cachedRotParentToThis = rotParentToThis;
187
188         m_links[i].m_jointType = btMultibodyLink::ePrismatic;
189         m_links[i].m_dofCount = 1;
190         m_links[i].m_posVarCount = 1;   
191         m_links[i].m_jointPos[0] = 0.f;
192         m_links[i].m_jointTorque[0] = 0.f;
193
194         if (disableParentCollision)
195                 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
196         //
197         
198         m_links[i].updateCacheMultiDof();
199         
200         updateLinksDofOffsets();
201 }
202
203 void btMultiBody::setupRevolute(int i,
204                               btScalar mass,
205                               const btVector3 &inertia,
206                               int parent,
207                               const btQuaternion &rotParentToThis,
208                               const btVector3 &jointAxis,
209                               const btVector3 &parentComToThisPivotOffset,
210                               const btVector3 &thisPivotToThisComOffset,
211                                                           bool disableParentCollision)
212 {
213         m_dofCount += 1;
214         m_posVarCnt += 1;
215         
216     m_links[i].m_mass = mass;
217     m_links[i].m_inertiaLocal = inertia;
218     m_links[i].m_parent = parent;
219     m_links[i].m_zeroRotParentToThis = rotParentToThis;
220     m_links[i].setAxisTop(0, jointAxis);
221     m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
222     m_links[i].m_dVector = thisPivotToThisComOffset;
223     m_links[i].m_eVector = parentComToThisPivotOffset;
224
225         m_links[i].m_jointType = btMultibodyLink::eRevolute;
226         m_links[i].m_dofCount = 1;
227         m_links[i].m_posVarCount = 1;   
228         m_links[i].m_jointPos[0] = 0.f;
229         m_links[i].m_jointTorque[0] = 0.f;
230
231         if (disableParentCollision)
232                 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
233     //
234         m_links[i].updateCacheMultiDof();
235         //
236         updateLinksDofOffsets();
237 }
238
239
240
241 void btMultiBody::setupSpherical(int i,
242                                                    btScalar mass,
243                                                    const btVector3 &inertia,
244                                                    int parent,
245                                                    const btQuaternion &rotParentToThis,
246                                                    const btVector3 &parentComToThisPivotOffset,
247                                                    const btVector3 &thisPivotToThisComOffset,
248                                                    bool disableParentCollision)
249 {
250         
251         m_dofCount += 3;
252         m_posVarCnt += 4;
253
254         m_links[i].m_mass = mass;
255     m_links[i].m_inertiaLocal = inertia;
256     m_links[i].m_parent = parent;
257     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
258     m_links[i].m_dVector = thisPivotToThisComOffset;
259     m_links[i].m_eVector = parentComToThisPivotOffset;    
260
261         m_links[i].m_jointType = btMultibodyLink::eSpherical;
262         m_links[i].m_dofCount = 3;
263         m_links[i].m_posVarCount = 4;
264         m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
265         m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
266         m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
267         m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
268         m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
269         m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
270         m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
271         m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
272
273
274         if (disableParentCollision)
275                 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
276         //
277         m_links[i].updateCacheMultiDof();       
278         //
279         updateLinksDofOffsets();
280 }
281
282 void btMultiBody::setupPlanar(int i,
283                                                    btScalar mass,
284                                                    const btVector3 &inertia,
285                                                    int parent,
286                                                    const btQuaternion &rotParentToThis,
287                                                    const btVector3 &rotationAxis,
288                                                    const btVector3 &parentComToThisComOffset,                                              
289                                                    bool disableParentCollision)
290 {
291         
292         m_dofCount += 3;
293         m_posVarCnt += 3;
294
295         m_links[i].m_mass = mass;
296     m_links[i].m_inertiaLocal = inertia;
297     m_links[i].m_parent = parent;
298     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
299         m_links[i].m_dVector.setZero();
300     m_links[i].m_eVector = parentComToThisComOffset;
301
302         //
303         static btVector3 vecNonParallelToRotAxis(1, 0, 0);
304         if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
305                 vecNonParallelToRotAxis.setValue(0, 1, 0);
306         //
307
308         m_links[i].m_jointType = btMultibodyLink::ePlanar;
309         m_links[i].m_dofCount = 3;
310         m_links[i].m_posVarCount = 3;
311         btVector3 n=rotationAxis.normalized();
312         m_links[i].setAxisTop(0, n[0],n[1],n[2]);
313         m_links[i].setAxisTop(1,0,0,0);
314         m_links[i].setAxisTop(2,0,0,0);
315         m_links[i].setAxisBottom(0,0,0,0);
316         btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
317         m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
318         cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
319         m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
320         m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
321         m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
322
323         if (disableParentCollision)
324                 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
325     //
326         m_links[i].updateCacheMultiDof();
327         //
328         updateLinksDofOffsets();
329 }
330
331 void btMultiBody::finalizeMultiDof()
332 {
333         m_deltaV.resize(0);
334         m_deltaV.resize(6 + m_dofCount);
335         m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount);                      //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
336         m_vectorBuf.resize(2 * m_dofCount);                                                                                                     //two 3-vectors (i.e. one six-vector) for each system dof       ("h" matrices)
337
338         updateLinksDofOffsets();
339 }
340         
341 int btMultiBody::getParent(int i) const
342 {
343     return m_links[i].m_parent;
344 }
345
346 btScalar btMultiBody::getLinkMass(int i) const
347 {
348     return m_links[i].m_mass;
349 }
350
351 const btVector3 & btMultiBody::getLinkInertia(int i) const
352 {
353     return m_links[i].m_inertiaLocal;
354 }
355
356 btScalar btMultiBody::getJointPos(int i) const
357 {
358     return m_links[i].m_jointPos[0];
359 }
360
361 btScalar btMultiBody::getJointVel(int i) const
362 {
363     return m_realBuf[6 + m_links[i].m_dofOffset];
364 }
365
366 btScalar * btMultiBody::getJointPosMultiDof(int i)
367 {
368         return &m_links[i].m_jointPos[0];
369 }
370
371 btScalar * btMultiBody::getJointVelMultiDof(int i)
372 {
373         return &m_realBuf[6 + m_links[i].m_dofOffset];
374 }
375
376 const btScalar * btMultiBody::getJointPosMultiDof(int i) const 
377 {
378         return &m_links[i].m_jointPos[0];
379 }
380
381 const btScalar * btMultiBody::getJointVelMultiDof(int i) const 
382 {
383         return &m_realBuf[6 + m_links[i].m_dofOffset];
384 }
385
386
387 void btMultiBody::setJointPos(int i, btScalar q)
388 {
389     m_links[i].m_jointPos[0] = q;
390     m_links[i].updateCacheMultiDof();
391 }
392
393 void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
394 {
395         for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
396                 m_links[i].m_jointPos[pos] = q[pos];
397
398     m_links[i].updateCacheMultiDof();
399 }
400
401 void btMultiBody::setJointVel(int i, btScalar qdot)
402 {
403     m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
404 }
405
406 void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
407 {
408         for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
409                 m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
410 }
411
412 const btVector3 & btMultiBody::getRVector(int i) const
413 {
414     return m_links[i].m_cachedRVector;
415 }
416
417 const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
418 {
419     return m_links[i].m_cachedRotParentToThis;
420 }
421
422 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
423 {
424     btVector3 result = local_pos;
425     while (i != -1) {
426         // 'result' is in frame i. transform it to frame parent(i)
427         result += getRVector(i);
428         result = quatRotate(getParentToLocalRot(i).inverse(),result);
429         i = getParent(i);
430     }
431
432     // 'result' is now in the base frame. transform it to world frame
433     result = quatRotate(getWorldToBaseRot().inverse() ,result);
434     result += getBasePos();
435
436     return result;
437 }
438
439 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
440 {
441     if (i == -1) {
442         // world to base
443         return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
444     } else {
445         // find position in parent frame, then transform to current frame
446         return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
447     }
448 }
449
450 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
451 {
452     btVector3 result = local_dir;
453     while (i != -1) {
454         result = quatRotate(getParentToLocalRot(i).inverse() , result);
455         i = getParent(i);
456     }
457     result = quatRotate(getWorldToBaseRot().inverse() , result);
458     return result;
459 }
460
461 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
462 {
463     if (i == -1) {
464         return quatRotate(getWorldToBaseRot(), world_dir);
465     } else {
466         return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
467     }
468 }
469
470 void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
471 {
472         int num_links = getNumLinks();
473     // Calculates the velocities of each link (and the base) in its local frame
474     omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
475     vel[0] = quatRotate(m_baseQuat ,getBaseVel());
476     
477     for (int i = 0; i < num_links; ++i) {
478         const int parent = m_links[i].m_parent;
479
480         // transform parent vel into this frame, store in omega[i+1], vel[i+1]
481         SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
482                          omega[parent+1], vel[parent+1],
483                          omega[i+1], vel[i+1]);
484
485         // now add qidot * shat_i
486         omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
487         vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
488     }
489 }
490
491 btScalar btMultiBody::getKineticEnergy() const
492 {
493         int num_links = getNumLinks();
494     // TODO: would be better not to allocate memory here
495     btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
496         btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
497     compTreeLinkVelocities(&omega[0], &vel[0]);
498
499     // we will do the factor of 0.5 at the end
500     btScalar result = m_baseMass * vel[0].dot(vel[0]);
501     result += omega[0].dot(m_baseInertia * omega[0]);
502     
503     for (int i = 0; i < num_links; ++i) {
504         result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
505         result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
506     }
507
508     return 0.5f * result;
509 }
510
511 btVector3 btMultiBody::getAngularMomentum() const
512 {
513         int num_links = getNumLinks();
514     // TODO: would be better not to allocate memory here
515     btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
516         btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
517     btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
518     compTreeLinkVelocities(&omega[0], &vel[0]);
519
520     rot_from_world[0] = m_baseQuat;
521     btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
522
523     for (int i = 0; i < num_links; ++i) {
524         rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
525         result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
526     }
527
528     return result;
529 }
530
531 void btMultiBody::clearConstraintForces()
532 {
533         m_baseConstraintForce.setValue(0, 0, 0);
534         m_baseConstraintTorque.setValue(0, 0, 0);
535
536
537     for (int i = 0; i < getNumLinks(); ++i) {
538         m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
539         m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
540     }
541 }
542 void btMultiBody::clearForcesAndTorques()
543 {
544     m_baseForce.setValue(0, 0, 0);
545     m_baseTorque.setValue(0, 0, 0);
546
547         
548     for (int i = 0; i < getNumLinks(); ++i) {
549         m_links[i].m_appliedForce.setValue(0, 0, 0);
550         m_links[i].m_appliedTorque.setValue(0, 0, 0);
551                 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
552     }
553 }
554
555 void btMultiBody::clearVelocities()
556 {
557         for (int i = 0; i < 6 + getNumLinks(); ++i) 
558         {
559                 m_realBuf[i] = 0.f;
560         }
561 }
562 void btMultiBody::addLinkForce(int i, const btVector3 &f)
563 {
564     m_links[i].m_appliedForce += f;
565 }
566
567 void btMultiBody::addLinkTorque(int i, const btVector3 &t)
568 {
569     m_links[i].m_appliedTorque += t;
570 }
571
572 void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
573 {
574     m_links[i].m_appliedConstraintForce += f;
575 }
576
577 void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
578 {
579     m_links[i].m_appliedConstraintTorque += t;
580 }
581
582
583
584 void btMultiBody::addJointTorque(int i, btScalar Q)
585 {
586     m_links[i].m_jointTorque[0] += Q;
587 }
588
589 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
590 {
591         m_links[i].m_jointTorque[dof] += Q;
592 }
593
594 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
595 {
596         for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
597                 m_links[i].m_jointTorque[dof] = Q[dof];
598 }
599
600 const btVector3 & btMultiBody::getLinkForce(int i) const
601 {
602     return m_links[i].m_appliedForce;
603 }
604
605 const btVector3 & btMultiBody::getLinkTorque(int i) const
606 {
607     return m_links[i].m_appliedTorque;
608 }
609
610 btScalar btMultiBody::getJointTorque(int i) const
611 {
612     return m_links[i].m_jointTorque[0];
613 }
614
615 btScalar * btMultiBody::getJointTorqueMultiDof(int i)
616 {
617     return &m_links[i].m_jointTorque[0];
618 }
619
620 inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1)                               //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
621 {
622                 btVector3 row0 = btVector3( 
623                         v0.x() * v1.x(),
624                         v0.x() * v1.y(),
625                         v0.x() * v1.z());
626                 btVector3 row1 = btVector3( 
627                         v0.y() * v1.x(),
628                         v0.y() * v1.y(),
629                         v0.y() * v1.z());
630                 btVector3 row2 = btVector3( 
631                         v0.z() * v1.x(),
632                         v0.z() * v1.y(),
633                         v0.z() * v1.z());
634
635         btMatrix3x3 m(row0[0],row0[1],row0[2],
636                                                 row1[0],row1[1],row1[2],
637                                                 row2[0],row2[1],row2[2]);
638                 return m;
639 }
640
641 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
642 //
643
644 void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
645                                btAlignedObjectArray<btScalar> &scratch_r,
646                                btAlignedObjectArray<btVector3> &scratch_v,
647                                btAlignedObjectArray<btMatrix3x3> &scratch_m,
648                                 bool isConstraintPass)
649 {
650     // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
651     // and the base linear & angular accelerations.
652
653     // We apply damping forces in this routine as well as any external forces specified by the 
654     // caller (via addBaseForce etc).
655
656     // output should point to an array of 6 + num_links reals.
657     // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
658     // num_links joint acceleration values.
659     
660         // We added support for multi degree of freedom (multi dof) joints.
661         // In addition we also can compute the joint reaction forces. This is performed in a second pass,
662         // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
663
664         m_internalNeedsJointFeedback = false;
665
666         int num_links = getNumLinks();
667
668     const btScalar DAMPING_K1_LINEAR = m_linearDamping;
669         const btScalar DAMPING_K2_LINEAR = m_linearDamping;
670
671         const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
672         const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
673
674     btVector3 base_vel = getBaseVel();
675     btVector3 base_omega = getBaseOmega();
676
677     // Temporary matrices/vectors -- use scratch space from caller
678     // so that we don't have to keep reallocating every frame
679
680     scratch_r.resize(2*m_dofCount + 6);                         //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
681     scratch_v.resize(8*num_links + 6);
682     scratch_m.resize(4*num_links + 4);
683
684         //btScalar * r_ptr = &scratch_r[0];
685     btScalar * output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
686     btVector3 * v_ptr = &scratch_v[0];
687     
688     // vhat_i  (top = angular, bottom = linear part)    
689         btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
690         v_ptr += num_links * 2 + 2;
691         //
692     // zhat_i^A    
693         btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
694         v_ptr += num_links * 2 + 2;
695         //
696     // chat_i  (note NOT defined for the base)    
697         btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
698         v_ptr += num_links * 2;
699         //
700     // Ihat_i^A.    
701         btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
702
703     // Cached 3x3 rotation matrices from parent frame to this frame.
704     btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
705     btMatrix3x3 * rot_from_world = &scratch_m[0];
706
707     // hhat_i, ahat_i
708     // hhat is NOT stored for the base (but ahat is)    
709         btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
710         btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
711         v_ptr += num_links * 2 + 2;
712         //
713     // Y_i, invD_i
714     btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
715         btScalar * Y = &scratch_r[0];
716         //
717         //aux variables 
718         static btSpatialMotionVector spatJointVel;                                      //spatial velocity due to the joint motion (i.e. without predecessors' influence)
719         static btScalar D[36];                                                                          //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do        
720         static btScalar invD_times_Y[6];                                                        //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies       
721         static btSpatialMotionVector result;                                                    //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
722         static btScalar Y_minus_hT_a[6];                                                        //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough        
723         static btSpatialForceVector spatForceVecTemps[6];                               //6 temporary spatial force vectors
724         static btSpatialTransformationMatrix fromParent;                                //spatial transform from parent to child
725         static btSymmetricSpatialDyad dyadTemp;                                         //inertia matrix temp
726         static btSpatialTransformationMatrix fromWorld;
727         fromWorld.m_trnVec.setZero();
728         /////////////////
729
730     // ptr to the joint accel part of the output
731     btScalar * joint_accel = output + 6;
732
733     // Start of the algorithm proper.
734     
735     // First 'upward' loop.
736     // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
737
738     rot_from_parent[0] = btMatrix3x3(m_baseQuat);                               //m_baseQuat assumed to be alias!?
739
740         //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
741         spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
742
743     if (m_fixedBase) 
744         {        
745                 zeroAccSpatFrc[0].setZero();
746     }
747         else 
748         {
749                 btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
750                 btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
751                 //external forces               
752                 zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));     
753
754                 //adding damping terms (only)
755                 btScalar linDampMult = 1., angDampMult = 1.;
756                 zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
757                                                                    linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
758
759                 //
760                 //p += vhat x Ihat vhat - done in a simpler way
761                 if (m_useGyroTerm)
762                         zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
763                 //
764                 zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
765     }
766
767
768         //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
769         spatInertia[0].setMatrix(       btMatrix3x3(0,0,0,0,0,0,0,0,0),
770                                                                 //
771                                                                 btMatrix3x3(m_baseMass, 0, 0,
772                                                                                         0, m_baseMass, 0,
773                                                                                         0, 0, m_baseMass),
774                                                                 //
775                                                                 btMatrix3x3(m_baseInertia[0], 0, 0,
776                                                                                         0, m_baseInertia[1], 0,
777                                                                                         0, 0, m_baseInertia[2])
778                                                         );
779
780     rot_from_world[0] = rot_from_parent[0];
781
782         //
783     for (int i = 0; i < num_links; ++i) {               
784         const int parent = m_links[i].m_parent;
785         rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
786         rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
787
788                 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
789                 fromWorld.m_rotMat = rot_from_world[i+1];
790                 fromParent.transform(spatVel[parent+1], spatVel[i+1]);
791
792                 // now set vhat_i to its true value by doing
793         // vhat_i += qidot * shat_i                     
794                 if(!m_useGlobalVelocities)
795                 {
796                         spatJointVel.setZero();
797
798                         for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)            
799                                 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
800
801                         // remember vhat_i is really vhat_p(i) (but in current frame) at this point     => we need to add velocity across the inboard joint
802                         spatVel[i+1] += spatJointVel;
803
804                         //
805                         // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
806                         //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
807
808                 }
809                 else
810                 {
811                         fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
812                         fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
813                 }
814
815                 // we can now calculate chat_i          
816                 spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);           
817
818         // calculate zhat_i^A
819                 //
820                 //external forces               
821                 btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
822                 btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
823  
824                 zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
825         
826 #if 0   
827                 {
828
829                         b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
830                         i+1,
831                         zeroAccSpatFrc[i+1].m_topVec[0],
832                         zeroAccSpatFrc[i+1].m_topVec[1],
833                         zeroAccSpatFrc[i+1].m_topVec[2],
834
835                         zeroAccSpatFrc[i+1].m_bottomVec[0],
836                         zeroAccSpatFrc[i+1].m_bottomVec[1],
837                         zeroAccSpatFrc[i+1].m_bottomVec[2]);
838                 }
839 #endif
840                 //
841                 //adding damping terms (only)
842                 btScalar linDampMult = 1., angDampMult = 1.;
843                 zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
844                                                                          linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
845                 
846         // calculate Ihat_i^A
847                 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
848                 spatInertia[i+1].setMatrix(     btMatrix3x3(0,0,0,0,0,0,0,0,0),
849                                                                         //
850                                                                         btMatrix3x3(m_links[i].m_mass, 0, 0,
851                                                                                                 0, m_links[i].m_mass, 0,
852                                                                                                 0, 0, m_links[i].m_mass),
853                                                                         //
854                                                                         btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
855                                                                                                 0, m_links[i].m_inertiaLocal[1], 0,
856                                                                                                 0, 0, m_links[i].m_inertiaLocal[2])
857                                                                 );
858                 //
859                 //p += vhat x Ihat vhat - done in a simpler way
860                 if(m_useGyroTerm)
861                         zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));                 
862                 //              
863                 zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
864                 //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
865                 ////clamp parent's omega
866                 //btScalar parOmegaMod = temp.length();
867                 //btScalar parOmegaModMax = 1000;
868                 //if(parOmegaMod > parOmegaModMax)
869                 //      temp *= parOmegaModMax / parOmegaMod;
870                 //zeroAccSpatFrc[i+1].addLinear(temp);
871                 //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
872                 //temp = spatCoriolisAcc[i].getLinear();
873                 //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
874                 
875                 
876
877                 //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
878                 //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());          
879                 //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
880     }
881         
882     // 'Downward' loop.
883     // (part of TreeForwardDynamics in Mirtich.)
884     for (int i = num_links - 1; i >= 0; --i)
885         {
886                 const int parent = m_links[i].m_parent;
887                 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
888
889                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
890                 {
891                         btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
892                         //
893                         hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
894                         //
895                         Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
896                         - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
897                         - spatCoriolisAcc[i].dot(hDof)
898                         ;
899                 }
900
901                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
902                 {
903                         btScalar *D_row = &D[dof * m_links[i].m_dofCount];                      
904                         for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
905                         {
906                                 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
907                                 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
908                         }
909                 }
910
911         btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
912                 switch(m_links[i].m_jointType)
913                 {
914                         case btMultibodyLink::ePrismatic:
915                         case btMultibodyLink::eRevolute:
916                         {
917                                 invDi[0] = 1.0f / D[0];
918                                 break;
919                         }
920                         case btMultibodyLink::eSpherical:
921                         case btMultibodyLink::ePlanar:
922                         {
923                                 static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
924                                 static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
925
926                                 //unroll the loop?
927                                 for(int row = 0; row < 3; ++row)
928                                 {
929                                         for(int col = 0; col < 3; ++col)
930                                         {                                               
931                                                 invDi[row * 3 + col] = invD3x3[row][col];
932                                         }
933                                 }
934
935                                 break;
936                         }
937                         default:
938                         {
939                         
940                         }
941                 }
942
943                 //determine h*D^{-1}
944                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
945                 {
946                         spatForceVecTemps[dof].setZero();
947
948                         for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
949                         {                               
950                                 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
951                                 //                              
952                                 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
953                         }
954                 }
955
956                 dyadTemp = spatInertia[i+1];
957
958                 //determine (h*D^{-1}) * h^{T}
959                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
960                 {                       
961                         btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
962                         //
963                         dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
964                 }
965
966                 fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
967         
968                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
969                 {
970                         invD_times_Y[dof] = 0.f;
971
972                         for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
973                         {
974                                 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];                              
975                         }       
976                 }
977                 
978                 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];             
979
980                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
981                 {                               
982                         btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
983                         //
984                         spatForceVecTemps[0] += hDof * invD_times_Y[dof];                       
985                 }
986                 
987                 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
988                 
989                 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
990     }
991
992
993     // Second 'upward' loop
994     // (part of TreeForwardDynamics in Mirtich)
995
996     if (m_fixedBase) 
997         {
998         spatAcc[0].setZero();
999     } 
1000         else 
1001         {
1002         if (num_links > 0) 
1003                 {
1004                         m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
1005                         m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
1006                         m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
1007                         m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
1008
1009         }               
1010                 
1011                 solveImatrix(zeroAccSpatFrc[0], result);
1012                 spatAcc[0] = -result;
1013     }
1014         
1015         
1016     // now do the loop over the m_links
1017     for (int i = 0; i < num_links; ++i) 
1018         {
1019                 //      qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
1020                 //      a = apar + cor + Sqdd
1021                 //or
1022                 //      qdd = D^{-1} * (Y - h^{T}*(apar+cor))
1023                 //      a = apar + Sqdd
1024
1025         const int parent = m_links[i].m_parent;
1026                 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1027
1028                 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1029                 
1030                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1031                 {
1032                         btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1033                         //                      
1034                         Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);                   
1035                 }
1036
1037                 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1038                 //D^{-1} * (Y - h^{T}*apar)
1039                 mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1040
1041                 spatAcc[i+1] += spatCoriolisAcc[i];             
1042
1043                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)            
1044                         spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1045
1046                 if (m_links[i].m_jointFeedback)
1047                 {
1048                         m_internalNeedsJointFeedback = true;
1049
1050                         btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
1051                         btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
1052
1053                         if (gJointFeedbackInJointFrame)
1054                         {
1055                                 //shift the reaction forces to the joint frame
1056                                 //linear (force) component is the same
1057                                 //shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
1058                                  angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
1059                         }
1060                         
1061
1062                         if (gJointFeedbackInWorldSpace)
1063                         {
1064                                 if (isConstraintPass)
1065                                 {
1066  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1067                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1068                                 } else
1069                                 {
1070                                         m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1071                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1072                                 }
1073                         } else
1074                         {
1075                                 if (isConstraintPass)
1076                                 {
1077                                           m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;                        
1078                                 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1079
1080                                 }
1081                                 else
1082                                 {
1083                                 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1084                                 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1085                                 }               
1086                         }       
1087         }
1088
1089     }
1090
1091     // transform base accelerations back to the world frame.
1092     btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1093         output[0] = omegadot_out[0];
1094         output[1] = omegadot_out[1];
1095         output[2] = omegadot_out[2];
1096
1097     btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1098         output[3] = vdot_out[0];
1099         output[4] = vdot_out[1];
1100         output[5] = vdot_out[2];
1101
1102         /////////////////
1103         //printf("q = [");
1104         //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1105         //for(int link = 0; link < getNumLinks(); ++link)
1106         //      for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1107         //              printf("%.6f ", m_links[link].m_jointPos[dof]);
1108         //printf("]\n");
1109         ////
1110         //printf("qd = [");
1111         //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1112         //      printf("%.6f ", m_realBuf[dof]);
1113         //printf("]\n");
1114         //printf("qdd = [");
1115         //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1116         //      printf("%.6f ", output[dof]);
1117         //printf("]\n");
1118         /////////////////
1119
1120     // Final step: add the accelerations (times dt) to the velocities.
1121
1122         if (!isConstraintPass)
1123         {
1124         if(dt > 0.)
1125                 applyDeltaVeeMultiDof(output, dt);
1126
1127         }
1128         /////
1129         //btScalar angularThres = 1;
1130         //btScalar maxAngVel = 0.;              
1131         //bool scaleDown = 1.;
1132         //for(int link = 0; link < m_links.size(); ++link)
1133         //{             
1134         //      if(spatVel[link+1].getAngular().length() > maxAngVel)
1135         //      {
1136         //              maxAngVel = spatVel[link+1].getAngular().length();
1137         //              scaleDown = angularThres / spatVel[link+1].getAngular().length();
1138         //              break;
1139         //      }               
1140         //}
1141
1142         //if(scaleDown != 1.)
1143         //{
1144         //      for(int link = 0; link < m_links.size(); ++link)
1145         //      {
1146         //              if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
1147         //              {
1148         //                      for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1149         //                              getJointVelMultiDof(link)[dof] *= scaleDown;
1150         //              }
1151         //      }
1152         //}
1153         /////
1154
1155         /////////////////////
1156         if(m_useGlobalVelocities)
1157         {
1158                 for (int i = 0; i < num_links; ++i) 
1159                 {
1160                         const int parent = m_links[i].m_parent;
1161                         //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
1162                         //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];                /// <- done
1163                 
1164                         fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1165                         fromWorld.m_rotMat = rot_from_world[i+1];                       
1166         
1167                         // vhat_i = i_xhat_p(i) * vhat_p(i)             
1168                         fromParent.transform(spatVel[parent+1], spatVel[i+1]);
1169                         //nice alternative below (using operator *) but it generates temps
1170                         /////////////////////////////////////////////////////////////
1171
1172                         // now set vhat_i to its true value by doing
1173                         // vhat_i += qidot * shat_i                     
1174                         spatJointVel.setZero();
1175
1176                         for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)            
1177                                 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1178                 
1179                         // remember vhat_i is really vhat_p(i) (but in current frame) at this point     => we need to add velocity across the inboard joint
1180                         spatVel[i+1] += spatJointVel;
1181
1182
1183                         fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
1184                         fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
1185                 }
1186         }
1187         
1188 }
1189
1190
1191
1192 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
1193 {
1194         int num_links = getNumLinks();
1195         ///solve I * x = rhs, so the result = invI * rhs
1196     if (num_links == 0) 
1197         {
1198                 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1199         result[0] = rhs_bot[0] / m_baseInertia[0];
1200         result[1] = rhs_bot[1] / m_baseInertia[1];
1201         result[2] = rhs_bot[2] / m_baseInertia[2];
1202         result[3] = rhs_top[0] / m_baseMass;
1203         result[4] = rhs_top[1] / m_baseMass;
1204         result[5] = rhs_top[2] / m_baseMass;
1205     } else 
1206         {
1207                 /// Special routine for calculating the inverse of a spatial inertia matrix
1208                 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
1209                 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
1210                 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1211                 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
1212                 tmp = invIupper_right * m_cachedInertiaLowerRight;
1213                 btMatrix3x3 invI_upper_left = (tmp * Binv);
1214                 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1215                 tmp = m_cachedInertiaTopLeft  * invI_upper_left;
1216                 tmp[0][0]-= 1.0;
1217                 tmp[1][1]-= 1.0;
1218                 tmp[2][2]-= 1.0;
1219                 btMatrix3x3 invI_lower_left = (Binv * tmp);
1220
1221                 //multiply result = invI * rhs
1222                 {
1223                   btVector3 vtop = invI_upper_left*rhs_top;
1224                   btVector3 tmp;
1225                   tmp = invIupper_right * rhs_bot;
1226                   vtop += tmp;
1227                   btVector3 vbot = invI_lower_left*rhs_top;
1228                   tmp = invI_lower_right * rhs_bot;
1229                   vbot += tmp;
1230                   result[0] = vtop[0];
1231                   result[1] = vtop[1];
1232                   result[2] = vtop[2];
1233                   result[3] = vbot[0];
1234                   result[4] = vbot[1];
1235                   result[5] = vbot[2];
1236                 }
1237                 
1238     }
1239 }
1240 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
1241 {
1242         int num_links = getNumLinks();
1243         ///solve I * x = rhs, so the result = invI * rhs
1244     if (num_links == 0) 
1245         {
1246                 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1247                 result.setAngular(rhs.getAngular() / m_baseInertia);
1248                 result.setLinear(rhs.getLinear() / m_baseMass);         
1249     } else 
1250         {
1251                 /// Special routine for calculating the inverse of a spatial inertia matrix
1252                 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
1253                 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
1254                 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1255                 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
1256                 tmp = invIupper_right * m_cachedInertiaLowerRight;
1257                 btMatrix3x3 invI_upper_left = (tmp * Binv);
1258                 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1259                 tmp = m_cachedInertiaTopLeft  * invI_upper_left;
1260                 tmp[0][0]-= 1.0;
1261                 tmp[1][1]-= 1.0;
1262                 tmp[2][2]-= 1.0;
1263                 btMatrix3x3 invI_lower_left = (Binv * tmp);
1264
1265                 //multiply result = invI * rhs
1266                 {
1267                   btVector3 vtop = invI_upper_left*rhs.getLinear();
1268                   btVector3 tmp;
1269                   tmp = invIupper_right * rhs.getAngular();
1270                   vtop += tmp;
1271                   btVector3 vbot = invI_lower_left*rhs.getLinear();
1272                   tmp = invI_lower_right * rhs.getAngular();
1273                   vbot += tmp;
1274                   result.setVector(vtop, vbot);           
1275                 }
1276                 
1277     }
1278 }
1279
1280 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
1281 {
1282         for (int row = 0; row < rowsA; row++)
1283         {
1284                 for (int col = 0; col < colsB; col++)
1285                 {
1286                         pC[row * colsB + col] = 0.f;
1287                         for (int inner = 0; inner < rowsB; inner++)
1288                         {
1289                                 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1290                         }
1291                 }
1292         }
1293 }
1294
1295 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
1296                                        btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
1297 {
1298     // Temporary matrices/vectors -- use scratch space from caller
1299     // so that we don't have to keep reallocating every frame
1300
1301         
1302         int num_links = getNumLinks();  
1303     scratch_r.resize(m_dofCount);
1304     scratch_v.resize(4*num_links + 4);      
1305
1306     btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
1307     btVector3 * v_ptr = &scratch_v[0];
1308
1309     // zhat_i^A (scratch space)
1310     btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
1311         v_ptr += num_links * 2 + 2;
1312
1313     // rot_from_parent (cached from calcAccelerations)
1314     const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1315
1316     // hhat (cached), accel (scratch)
1317     // hhat is NOT stored for the base (but ahat is) 
1318         const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
1319         btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
1320         v_ptr += num_links * 2 + 2;
1321
1322     // Y_i (scratch), invD_i (cached)
1323     const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1324         btScalar * Y = r_ptr; 
1325         ////////////////
1326         //aux variables
1327         static btScalar invD_times_Y[6];                                                        //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
1328         static btSpatialMotionVector result;                                                    //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
1329         static btScalar Y_minus_hT_a[6];                                                        //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough        
1330         static btSpatialForceVector spatForceVecTemps[6];                               //6 temporary spatial force vectors
1331         static btSpatialTransformationMatrix fromParent;        
1332         /////////////////
1333
1334     // First 'upward' loop.
1335     // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1336         
1337         // Fill in zero_acc
1338     // -- set to force/torque on the base, zero otherwise
1339     if (m_fixedBase) 
1340         {
1341         zeroAccSpatFrc[0].setZero();
1342     } else 
1343         {       
1344                 //test forces
1345                 fromParent.m_rotMat = rot_from_parent[0];
1346                 fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
1347     }
1348     for (int i = 0; i < num_links; ++i) 
1349         {
1350                 zeroAccSpatFrc[i+1].setZero();
1351     }    
1352
1353         // 'Downward' loop.
1354     // (part of TreeForwardDynamics in Mirtich.)
1355     for (int i = num_links - 1; i >= 0; --i)
1356         {
1357                 const int parent = m_links[i].m_parent;
1358                 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1359
1360                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1361                 {
1362                         Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
1363                                                                                         - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1364                                                                                         ;
1365                 }
1366
1367                 btVector3 in_top, in_bottom, out_top, out_bottom;
1368                 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1369                 
1370                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1371                 {
1372                         invD_times_Y[dof] = 0.f;
1373
1374                         for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1375                         {
1376                                 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];                              
1377                         }       
1378                 }
1379                 
1380                  // Zp += pXi * (Zi + hi*Yi/Di)
1381                 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
1382
1383                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1384                 {
1385                         const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1386                         //
1387                         spatForceVecTemps[0] += hDof * invD_times_Y[dof];               
1388                 }
1389                 
1390
1391                 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1392                         
1393                 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1394     }
1395         
1396         // ptr to the joint accel part of the output
1397     btScalar * joint_accel = output + 6;
1398
1399
1400     // Second 'upward' loop
1401     // (part of TreeForwardDynamics in Mirtich)
1402
1403     if (m_fixedBase) 
1404         {
1405         spatAcc[0].setZero();
1406     } 
1407         else 
1408         {
1409                 solveImatrix(zeroAccSpatFrc[0], result);
1410                 spatAcc[0] = -result;
1411
1412     }
1413         
1414     // now do the loop over the m_links
1415     for (int i = 0; i < num_links; ++i)
1416         {
1417         const int parent = m_links[i].m_parent;
1418                 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1419
1420                 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1421                 
1422                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1423                 {
1424                         const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1425                         //                      
1426                         Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
1427                 }
1428
1429                 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1430                 mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1431
1432                 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)            
1433                         spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];      
1434     }
1435
1436     // transform base accelerations back to the world frame.
1437     btVector3 omegadot_out;
1438     omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1439         output[0] = omegadot_out[0];
1440         output[1] = omegadot_out[1];
1441         output[2] = omegadot_out[2];
1442
1443     btVector3 vdot_out;
1444     vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
1445         output[3] = vdot_out[0];
1446         output[4] = vdot_out[1];
1447         output[5] = vdot_out[2];
1448
1449         /////////////////
1450         //printf("delta = [");
1451         //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1452         //      printf("%.2f ", output[dof]);
1453         //printf("]\n");
1454         /////////////////
1455 }
1456
1457
1458
1459
1460 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
1461 {       
1462         int num_links = getNumLinks();
1463     // step position by adding dt * velocity
1464         //btVector3 v = getBaseVel();   
1465     //m_basePos += dt * v;
1466         //
1467         btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
1468         btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);                   //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
1469         //      
1470         pBasePos[0] += dt * pBaseVel[0];
1471         pBasePos[1] += dt * pBaseVel[1];
1472         pBasePos[2] += dt * pBaseVel[2];
1473
1474         ///////////////////////////////
1475         //local functor for quaternion integration (to avoid error prone redundancy)
1476         struct
1477         {
1478                 //"exponential map" based on btTransformUtil::integrateTransform(..)
1479                 void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
1480                 {
1481                         //baseBody      =>      quat is alias and omega is global coor
1482                         //!baseBody     =>      quat is alibi and omega is local coor   
1483                         
1484                         btVector3 axis;
1485                         btVector3 angvel;
1486
1487                         if(!baseBody)                   
1488                                 angvel = quatRotate(quat, omega);                               //if quat is not m_baseQuat, it is alibi => ok                  
1489                         else
1490                                 angvel = omega;
1491                 
1492                         btScalar fAngle = angvel.length();              
1493                         //limit the angular motion
1494                         if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
1495                         {
1496                                 fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
1497                         }
1498
1499                         if ( fAngle < btScalar(0.001) )
1500                         {
1501                                 // use Taylor's expansions of sync function
1502                                 axis   = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
1503                         }
1504                         else
1505                         {
1506                                 // sync(fAngle) = sin(c*fAngle)/t
1507                                 axis   = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
1508                         }
1509                         
1510                         if(!baseBody)                           
1511                                 quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;                        
1512                         else                    
1513                                 quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
1514                                 //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();                 
1515                 
1516                         quat.normalize();
1517                 }
1518         } pQuatUpdateFun;
1519         ///////////////////////////////
1520
1521         //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
1522         //      
1523         btScalar *pBaseQuat = pq ? pq : m_baseQuat;     
1524         btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];               //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
1525         //
1526         static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1527         static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1528         pQuatUpdateFun(baseOmega, baseQuat, true, dt);
1529         pBaseQuat[0] = baseQuat.x();
1530         pBaseQuat[1] = baseQuat.y();
1531         pBaseQuat[2] = baseQuat.z();
1532         pBaseQuat[3] = baseQuat.w();
1533
1534
1535         //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
1536         //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
1537         //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
1538
1539         if(pq)          
1540                 pq += 7;
1541         if(pqd)
1542                 pqd += 6;
1543
1544         // Finally we can update m_jointPos for each of the m_links
1545     for (int i = 0; i < num_links; ++i) 
1546         {
1547                 btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);            
1548                 btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
1549
1550                 switch(m_links[i].m_jointType)
1551                 {
1552                         case btMultibodyLink::ePrismatic:
1553                         case btMultibodyLink::eRevolute:
1554                         {
1555                                 btScalar jointVel = pJointVel[0];       
1556                                 pJointPos[0] += dt * jointVel;
1557                                 break;
1558                         }
1559                         case btMultibodyLink::eSpherical:
1560                         {
1561                                 static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1562                                 static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1563                                 pQuatUpdateFun(jointVel, jointOri, false, dt);
1564                                 pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
1565                                 break;
1566                         }
1567                         case btMultibodyLink::ePlanar:
1568                         {
1569                                 pJointPos[0] += dt * getJointVelMultiDof(i)[0];
1570
1571                                 btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
1572                                 btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
1573                                 pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1574                                 pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1575
1576                                 break;
1577                         }
1578                         default:
1579                         {
1580                         }
1581
1582                 }
1583
1584                 m_links[i].updateCacheMultiDof(pq);
1585
1586                 if(pq)          
1587                         pq += m_links[i].m_posVarCount;
1588                 if(pqd)
1589                         pqd += m_links[i].m_dofCount;
1590     }
1591 }
1592
1593 void btMultiBody::fillConstraintJacobianMultiDof(int link,
1594                                     const btVector3 &contact_point,
1595                                                                         const btVector3 &normal_ang,
1596                                     const btVector3 &normal_lin,
1597                                     btScalar *jac,
1598                                     btAlignedObjectArray<btScalar> &scratch_r,
1599                                     btAlignedObjectArray<btVector3> &scratch_v,
1600                                     btAlignedObjectArray<btMatrix3x3> &scratch_m) const
1601 {
1602     // temporary space
1603         int num_links = getNumLinks();
1604         int m_dofCount = getNumDofs();
1605     scratch_v.resize(3*num_links + 3);                  //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
1606     scratch_m.resize(num_links + 1);
1607
1608     btVector3 * v_ptr = &scratch_v[0];
1609     btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
1610     btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
1611         btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
1612     btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
1613
1614     scratch_r.resize(m_dofCount);
1615     btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
1616
1617     btMatrix3x3 * rot_from_world = &scratch_m[0];
1618
1619     const btVector3 p_minus_com_world = contact_point - m_basePos;
1620         const btVector3 &normal_lin_world = normal_lin;                                                 //convenience
1621         const btVector3 &normal_ang_world = normal_ang;
1622
1623     rot_from_world[0] = btMatrix3x3(m_baseQuat);    
1624     
1625     // omega coeffients first.
1626     btVector3 omega_coeffs_world;
1627     omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
1628         jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1629         jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1630         jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1631     // then v coefficients
1632     jac[3] = normal_lin_world[0];
1633     jac[4] = normal_lin_world[1];
1634     jac[5] = normal_lin_world[2];
1635
1636         //create link-local versions of p_minus_com and normal
1637         p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1638     n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1639         n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1640
1641     // Set remaining jac values to zero for now.
1642     for (int i = 6; i < 6 + m_dofCount; ++i) 
1643         {
1644         jac[i] = 0;
1645     }
1646
1647     // Qdot coefficients, if necessary.
1648     if (num_links > 0 && link > -1) {
1649
1650         // TODO: speed this up -- don't calculate for m_links we don't need.
1651         // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
1652         // which is resulting in repeated work being done...)
1653
1654         // calculate required normals & positions in the local frames.
1655         for (int i = 0; i < num_links; ++i) {
1656
1657             // transform to local frame
1658             const int parent = m_links[i].m_parent;
1659             const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
1660             rot_from_world[i+1] = mtx * rot_from_world[parent+1];
1661
1662             n_local_lin[i+1] = mtx * n_local_lin[parent+1];
1663                         n_local_ang[i+1] = mtx * n_local_ang[parent+1];
1664             p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
1665
1666                         // calculate the jacobian entry
1667                         switch(m_links[i].m_jointType)
1668                         {
1669                                 case btMultibodyLink::eRevolute:
1670                                 {
1671                                         results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
1672                                         results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
1673                                         break;
1674                                 }
1675                                 case btMultibodyLink::ePrismatic:
1676                                 {
1677                                         results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
1678                                         break;
1679                                 }
1680                                 case btMultibodyLink::eSpherical:
1681                                 {
1682                                         results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
1683                                         results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
1684                                         results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
1685                                                                                 
1686                                         results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
1687                                         results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
1688                                         results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
1689
1690                                         break;
1691                                 }
1692                                 case btMultibodyLink::ePlanar:
1693                                 {
1694                                         results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
1695                                         results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
1696                                         results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
1697
1698                                         break;
1699                                 }
1700                                 default:
1701                                 {
1702                                 }
1703                         }
1704             
1705         }
1706
1707         // Now copy through to output.
1708                 //printf("jac[%d] = ", link);
1709         while (link != -1) 
1710                 {
1711                         for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1712                         {
1713                                 jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
1714                                 //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
1715                         }
1716             
1717                         link = m_links[link].m_parent;
1718         }
1719                 //printf("]\n");
1720     }
1721 }
1722
1723
1724 void btMultiBody::wakeUp()
1725 {
1726     m_awake = true;
1727 }
1728
1729 void btMultiBody::goToSleep()
1730 {
1731     m_awake = false;
1732 }
1733
1734 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
1735 {
1736         extern bool gDisableDeactivation;
1737     if (!m_canSleep || gDisableDeactivation) 
1738         {
1739                 m_awake = true;
1740                 m_sleepTimer = 0;
1741                 return;
1742         }
1743
1744     // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
1745     btScalar motion = 0;
1746         {
1747                 for (int i = 0; i < 6 + m_dofCount; ++i)                
1748                         motion += m_realBuf[i] * m_realBuf[i];
1749         }
1750         
1751
1752     if (motion < SLEEP_EPSILON) {
1753         m_sleepTimer += timestep;
1754         if (m_sleepTimer > SLEEP_TIMEOUT) {
1755             goToSleep();
1756         }
1757     } else {
1758         m_sleepTimer = 0;
1759                 if (!m_awake)
1760                         wakeUp();
1761     }
1762 }
1763
1764
1765 void    btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
1766 {
1767         
1768         int num_links = getNumLinks();
1769
1770         // Cached 3x3 rotation matrices from parent frame to this frame.
1771         btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
1772
1773         rot_from_parent[0] = btMatrix3x3(m_baseQuat);                           //m_baseQuat assumed to be alias!?
1774         
1775         for (int i = 0; i < num_links; ++i) 
1776         {
1777                 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
1778         }
1779                 
1780         int nLinks = getNumLinks();
1781         ///base + num m_links
1782         world_to_local.resize(nLinks+1);
1783         local_origin.resize(nLinks+1);
1784
1785         world_to_local[0] = getWorldToBaseRot();
1786         local_origin[0] = getBasePos();
1787         
1788         for (int k=0;k<getNumLinks();k++)
1789         {
1790                 const int parent = getParent(k);
1791                 world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
1792                 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
1793         }
1794
1795         for (int link=0;link<getNumLinks();link++)
1796         {
1797                 int index = link+1;
1798
1799                 btVector3 posr = local_origin[index];
1800                 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
1801                 btTransform tr;
1802                 tr.setIdentity();
1803                 tr.setOrigin(posr);
1804                 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
1805                 getLink(link).m_cachedWorldTransform = tr;
1806                         
1807         }
1808
1809 }
1810
1811 void    btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
1812 {
1813         world_to_local.resize(getNumLinks()+1);
1814         local_origin.resize(getNumLinks()+1);
1815         
1816         world_to_local[0] = getWorldToBaseRot();
1817         local_origin[0] = getBasePos();
1818         
1819         if (getBaseCollider())
1820         {
1821                 btVector3 posr = local_origin[0];
1822                 //      float pos[4]={posr.x(),posr.y(),posr.z(),1};
1823                 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
1824                 btTransform tr;
1825                 tr.setIdentity();
1826                 tr.setOrigin(posr);
1827                 tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
1828                 
1829                 getBaseCollider()->setWorldTransform(tr);
1830                 
1831         }
1832         
1833         for (int k=0;k<getNumLinks();k++)
1834         {
1835                 const int parent = getParent(k);
1836                 world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
1837                 local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
1838         }
1839         
1840         
1841         for (int m=0;m<getNumLinks();m++)
1842         {
1843                 btMultiBodyLinkCollider* col = getLink(m).m_collider;
1844                 if (col)
1845                 {
1846                         int link = col->m_link;
1847                         btAssert(link == m);
1848                         
1849                         int index = link+1;
1850                         
1851                         btVector3 posr = local_origin[index];
1852                         //                      float pos[4]={posr.x(),posr.y(),posr.z(),1};
1853                         btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
1854                         btTransform tr;
1855                         tr.setIdentity();
1856                         tr.setOrigin(posr);
1857                         tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
1858                         
1859                         col->setWorldTransform(tr);
1860                 }
1861         }
1862 }
1863
1864 int     btMultiBody::calculateSerializeBufferSize()     const
1865 {
1866         int sz = sizeof(btMultiBodyData);
1867         return sz;
1868 }
1869
1870         ///fills the dataBuffer and returns the struct name (and 0 on failure)
1871 const char*     btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
1872 {
1873                 btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
1874                 getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
1875                 mbd->m_baseMass = this->getBaseMass();
1876                 getBaseInertia().serialize(mbd->m_baseInertia);
1877                 {
1878                         char* name = (char*) serializer->findNameForPointer(m_baseName);
1879                         mbd->m_baseName = (char*)serializer->getUniquePointer(name);
1880                         if (mbd->m_baseName)
1881                         {
1882                                 serializer->serializeName(name);
1883                         }
1884                 }
1885                 mbd->m_numLinks = this->getNumLinks();
1886                 if (mbd->m_numLinks)
1887                 {
1888                         int sz = sizeof(btMultiBodyLinkData);
1889                         int numElem = mbd->m_numLinks;
1890                         btChunk* chunk = serializer->allocate(sz,numElem);
1891                         btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
1892                         for (int i=0;i<numElem;i++,memPtr++)
1893                         {
1894
1895                                 memPtr->m_jointType = getLink(i).m_jointType;
1896                                 memPtr->m_dofCount = getLink(i).m_dofCount;
1897                                 memPtr->m_posVarCount = getLink(i).m_posVarCount;
1898                                 
1899                                 getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
1900                                 memPtr->m_linkMass = getLink(i).m_mass;
1901                                 memPtr->m_parentIndex = getLink(i).m_parent;
1902                                 getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
1903                                 getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
1904                                 getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
1905                                 btAssert(memPtr->m_dofCount<=3);
1906                                 for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
1907                                 {
1908                                         getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
1909                                         getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
1910                                         
1911                                         memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
1912                                         memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
1913
1914                                 }
1915                                 int numPosVar = getLink(i).m_posVarCount;
1916                                 for (int posvar = 0; posvar < numPosVar;posvar++)
1917                                 {
1918                                         memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
1919                                 }
1920                                 
1921                                 
1922                                 {
1923                                         char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
1924                                         memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
1925                                         if (memPtr->m_linkName)
1926                                         {
1927                                                 serializer->serializeName(name);
1928                                         }
1929                                 }
1930                                 {
1931                                         char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
1932                                         memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
1933                                         if (memPtr->m_jointName)
1934                                         {
1935                                                 serializer->serializeName(name);
1936                                         }
1937                                 }
1938                                 memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
1939
1940                         }
1941                         serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
1942                 }
1943                 mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
1944
1945                 return btMultiBodyDataName;
1946 }