Home | History | Annotate | Download | only in Featherstone
      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 (at) solarflare.org.uk>, 2011-2013
      9  *   Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
     10 
     11  This software is provided 'as-is', without any express or implied warranty.
     12  In no event will the authors be held liable for any damages arising from the use of this software.
     13  Permission is granted to anyone to use this software for any purpose,
     14  including commercial applications, and to alter it and redistribute it freely,
     15  subject to the following restrictions:
     16 
     17  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.
     18  2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
     19  3. This notice may not be removed or altered from any source distribution.
     20 
     21  */
     22 
     23 
     24 #include "btMultiBody.h"
     25 #include "btMultiBodyLink.h"
     26 #include "btMultiBodyLinkCollider.h"
     27 #include "btMultiBodyJointFeedback.h"
     28 #include "LinearMath/btTransformUtil.h"
     29 #include "LinearMath/btSerializer.h"
     30 // #define INCLUDE_GYRO_TERM
     31 
     32 ///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
     33 bool gJointFeedbackInWorldSpace = false;
     34 bool gJointFeedbackInJointFrame = false;
     35 
     36 namespace {
     37     const btScalar SLEEP_EPSILON = btScalar(0.05);  // this is a squared velocity (m^2 s^-2)
     38     const btScalar SLEEP_TIMEOUT = btScalar(2);     // in seconds
     39 }
     40 
     41 namespace {
     42     void SpatialTransform(const btMatrix3x3 &rotation_matrix,  // rotates vectors in 'from' frame to vectors in 'to' frame
     43                           const btVector3 &displacement,     // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
     44                           const btVector3 &top_in,       // top part of input vector
     45                           const btVector3 &bottom_in,    // bottom part of input vector
     46                           btVector3 &top_out,         // top part of output vector
     47                           btVector3 &bottom_out)      // bottom part of output vector
     48     {
     49         top_out = rotation_matrix * top_in;
     50         bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
     51     }
     52 
     53     void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
     54                                  const btVector3 &displacement,
     55                                  const btVector3 &top_in,
     56                                  const btVector3 &bottom_in,
     57                                  btVector3 &top_out,
     58                                  btVector3 &bottom_out)
     59     {
     60         top_out = rotation_matrix.transpose() * top_in;
     61         bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
     62     }
     63 
     64     btScalar SpatialDotProduct(const btVector3 &a_top,
     65                             const btVector3 &a_bottom,
     66                             const btVector3 &b_top,
     67                             const btVector3 &b_bottom)
     68     {
     69         return a_bottom.dot(b_top) + a_top.dot(b_bottom);
     70     }
     71 
     72 	void SpatialCrossProduct(const btVector3 &a_top,
     73                             const btVector3 &a_bottom,
     74                             const btVector3 &b_top,
     75                             const btVector3 &b_bottom,
     76 							btVector3 &top_out,
     77 							btVector3 &bottom_out)
     78 	{
     79 		top_out = a_top.cross(b_top);
     80 		bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
     81 	}
     82 }
     83 
     84 
     85 //
     86 // Implementation of class btMultiBody
     87 //
     88 
     89 btMultiBody::btMultiBody(int n_links,
     90                      btScalar mass,
     91                      const btVector3 &inertia,
     92                      bool fixedBase,
     93                      bool canSleep,
     94 					 bool multiDof)
     95     :
     96     	m_baseCollider(0),
     97 		m_baseName(0),
     98     	m_basePos(0,0,0),
     99     	m_baseQuat(0, 0, 0, 1),
    100       m_baseMass(mass),
    101       m_baseInertia(inertia),
    102 
    103 		m_fixedBase(fixedBase),
    104 		m_awake(true),
    105 		m_canSleep(canSleep),
    106 		m_sleepTimer(0),
    107 
    108 		m_linearDamping(0.04f),
    109 		m_angularDamping(0.04f),
    110 		m_useGyroTerm(true),
    111 			m_maxAppliedImpulse(1000.f),
    112 		m_maxCoordinateVelocity(100.f),
    113 			m_hasSelfCollision(true),
    114 		m_isMultiDof(multiDof),
    115 		__posUpdated(false),
    116 			m_dofCount(0),
    117 		m_posVarCnt(0),
    118 		m_useRK4(false),
    119 		m_useGlobalVelocities(false),
    120 		m_internalNeedsJointFeedback(false)
    121 {
    122 
    123 	if(!m_isMultiDof)
    124 	{
    125 		m_vectorBuf.resize(2*n_links);
    126 		m_realBuf.resize(6 + 2*n_links);
    127 		m_posVarCnt = n_links;
    128 	}
    129 
    130 	m_links.resize(n_links);
    131 	m_matrixBuf.resize(n_links + 1);
    132 
    133 
    134     m_baseForce.setValue(0, 0, 0);
    135     m_baseTorque.setValue(0, 0, 0);
    136 }
    137 
    138 btMultiBody::~btMultiBody()
    139 {
    140 }
    141 
    142 void btMultiBody::setupFixed(int i,
    143 						   btScalar mass,
    144 						   const btVector3 &inertia,
    145 						   int parent,
    146 						   const btQuaternion &rotParentToThis,
    147 						   const btVector3 &parentComToThisPivotOffset,
    148                            const btVector3 &thisPivotToThisComOffset,
    149 						   bool disableParentCollision)
    150 {
    151 	btAssert(m_isMultiDof);
    152 
    153 	m_links[i].m_mass = mass;
    154     m_links[i].m_inertiaLocal = inertia;
    155     m_links[i].m_parent = parent;
    156     m_links[i].m_zeroRotParentToThis = rotParentToThis;
    157 	m_links[i].m_dVector = thisPivotToThisComOffset;
    158     m_links[i].m_eVector = parentComToThisPivotOffset;
    159 
    160 	m_links[i].m_jointType = btMultibodyLink::eFixed;
    161 	m_links[i].m_dofCount = 0;
    162 	m_links[i].m_posVarCount = 0;
    163 
    164 	if (disableParentCollision)
    165 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
    166 	//
    167 	m_links[i].updateCacheMultiDof();
    168 
    169 	//
    170 	//if(m_isMultiDof)
    171 	//	resizeInternalMultiDofBuffers();
    172 	//
    173 	updateLinksDofOffsets();
    174 
    175 }
    176 
    177 
    178 void btMultiBody::setupPrismatic(int i,
    179                                btScalar mass,
    180                                const btVector3 &inertia,
    181                                int parent,
    182                                const btQuaternion &rotParentToThis,
    183                                const btVector3 &jointAxis,
    184                                const btVector3 &parentComToThisPivotOffset,
    185 							   const btVector3 &thisPivotToThisComOffset,
    186 							   bool disableParentCollision)
    187 {
    188 	if(m_isMultiDof)
    189 	{
    190 		m_dofCount += 1;
    191 		m_posVarCnt += 1;
    192 	}
    193 
    194     m_links[i].m_mass = mass;
    195     m_links[i].m_inertiaLocal = inertia;
    196     m_links[i].m_parent = parent;
    197     m_links[i].m_zeroRotParentToThis = rotParentToThis;
    198     m_links[i].setAxisTop(0, 0., 0., 0.);
    199     m_links[i].setAxisBottom(0, jointAxis);
    200     m_links[i].m_eVector = parentComToThisPivotOffset;
    201 	m_links[i].m_dVector = thisPivotToThisComOffset;
    202     m_links[i].m_cachedRotParentToThis = rotParentToThis;
    203 
    204 	m_links[i].m_jointType = btMultibodyLink::ePrismatic;
    205 	m_links[i].m_dofCount = 1;
    206 	m_links[i].m_posVarCount = 1;
    207 	m_links[i].m_jointPos[0] = 0.f;
    208 	m_links[i].m_jointTorque[0] = 0.f;
    209 
    210 	if (disableParentCollision)
    211 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
    212 	//
    213 	if(m_isMultiDof)
    214 		m_links[i].updateCacheMultiDof();
    215 	else
    216 		m_links[i].updateCache();
    217 	//
    218 	if(m_isMultiDof)
    219 		updateLinksDofOffsets();
    220 }
    221 
    222 void btMultiBody::setupRevolute(int i,
    223                               btScalar mass,
    224                               const btVector3 &inertia,
    225                               int parent,
    226                               const btQuaternion &rotParentToThis,
    227                               const btVector3 &jointAxis,
    228                               const btVector3 &parentComToThisPivotOffset,
    229                               const btVector3 &thisPivotToThisComOffset,
    230 							  bool disableParentCollision)
    231 {
    232 	if(m_isMultiDof)
    233 	{
    234 		m_dofCount += 1;
    235 		m_posVarCnt += 1;
    236 	}
    237 
    238     m_links[i].m_mass = mass;
    239     m_links[i].m_inertiaLocal = inertia;
    240     m_links[i].m_parent = parent;
    241     m_links[i].m_zeroRotParentToThis = rotParentToThis;
    242     m_links[i].setAxisTop(0, jointAxis);
    243     m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
    244     m_links[i].m_dVector = thisPivotToThisComOffset;
    245     m_links[i].m_eVector = parentComToThisPivotOffset;
    246 
    247 	m_links[i].m_jointType = btMultibodyLink::eRevolute;
    248 	m_links[i].m_dofCount = 1;
    249 	m_links[i].m_posVarCount = 1;
    250 	m_links[i].m_jointPos[0] = 0.f;
    251 	m_links[i].m_jointTorque[0] = 0.f;
    252 
    253 	if (disableParentCollision)
    254 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
    255     //
    256 	if(m_isMultiDof)
    257 		m_links[i].updateCacheMultiDof();
    258 	else
    259 		m_links[i].updateCache();
    260 	//
    261 	if(m_isMultiDof)
    262 		updateLinksDofOffsets();
    263 }
    264 
    265 
    266 
    267 void btMultiBody::setupSpherical(int i,
    268 						   btScalar mass,
    269 						   const btVector3 &inertia,
    270 						   int parent,
    271 						   const btQuaternion &rotParentToThis,
    272 						   const btVector3 &parentComToThisPivotOffset,
    273 						   const btVector3 &thisPivotToThisComOffset,
    274 						   bool disableParentCollision)
    275 {
    276 	btAssert(m_isMultiDof);
    277 	m_dofCount += 3;
    278 	m_posVarCnt += 4;
    279 
    280 	m_links[i].m_mass = mass;
    281     m_links[i].m_inertiaLocal = inertia;
    282     m_links[i].m_parent = parent;
    283     m_links[i].m_zeroRotParentToThis = rotParentToThis;
    284     m_links[i].m_dVector = thisPivotToThisComOffset;
    285     m_links[i].m_eVector = parentComToThisPivotOffset;
    286 
    287 	m_links[i].m_jointType = btMultibodyLink::eSpherical;
    288 	m_links[i].m_dofCount = 3;
    289 	m_links[i].m_posVarCount = 4;
    290 	m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
    291 	m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
    292 	m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
    293 	m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
    294 	m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
    295 	m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
    296 	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;
    297 	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
    298 
    299 
    300 	if (disableParentCollision)
    301 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
    302 	//
    303 	m_links[i].updateCacheMultiDof();
    304 	//
    305 	updateLinksDofOffsets();
    306 }
    307 
    308 void btMultiBody::setupPlanar(int i,
    309 						   btScalar mass,
    310 						   const btVector3 &inertia,
    311 						   int parent,
    312 						   const btQuaternion &rotParentToThis,
    313 						   const btVector3 &rotationAxis,
    314 						   const btVector3 &parentComToThisComOffset,
    315 						   bool disableParentCollision)
    316 {
    317 	btAssert(m_isMultiDof);
    318 	m_dofCount += 3;
    319 	m_posVarCnt += 3;
    320 
    321 	m_links[i].m_mass = mass;
    322     m_links[i].m_inertiaLocal = inertia;
    323     m_links[i].m_parent = parent;
    324     m_links[i].m_zeroRotParentToThis = rotParentToThis;
    325 	m_links[i].m_dVector.setZero();
    326     m_links[i].m_eVector = parentComToThisComOffset;
    327 
    328 	//
    329 	static btVector3 vecNonParallelToRotAxis(1, 0, 0);
    330 	if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
    331 		vecNonParallelToRotAxis.setValue(0, 1, 0);
    332 	//
    333 
    334 	m_links[i].m_jointType = btMultibodyLink::ePlanar;
    335 	m_links[i].m_dofCount = 3;
    336 	m_links[i].m_posVarCount = 3;
    337 	btVector3 n=rotationAxis.normalized();
    338 	m_links[i].setAxisTop(0, n[0],n[1],n[2]);
    339 	m_links[i].setAxisTop(1,0,0,0);
    340 	m_links[i].setAxisTop(2,0,0,0);
    341 	m_links[i].setAxisBottom(0,0,0,0);
    342 	btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
    343 	m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
    344 	cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
    345 	m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
    346 	m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
    347 	m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
    348 
    349 	if (disableParentCollision)
    350 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
    351     //
    352 	m_links[i].updateCacheMultiDof();
    353 	//
    354 	updateLinksDofOffsets();
    355 }
    356 
    357 void btMultiBody::finalizeMultiDof()
    358 {
    359 	btAssert(m_isMultiDof);
    360 	m_deltaV.resize(0);
    361 	m_deltaV.resize(6 + m_dofCount);
    362 	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")
    363 	m_vectorBuf.resize(2 * m_dofCount);													//two 3-vectors (i.e. one six-vector) for each system dof	("h" matrices)
    364 
    365 	updateLinksDofOffsets();
    366 }
    367 
    368 int btMultiBody::getParent(int i) const
    369 {
    370     return m_links[i].m_parent;
    371 }
    372 
    373 btScalar btMultiBody::getLinkMass(int i) const
    374 {
    375     return m_links[i].m_mass;
    376 }
    377 
    378 const btVector3 & btMultiBody::getLinkInertia(int i) const
    379 {
    380     return m_links[i].m_inertiaLocal;
    381 }
    382 
    383 btScalar btMultiBody::getJointPos(int i) const
    384 {
    385     return m_links[i].m_jointPos[0];
    386 }
    387 
    388 btScalar btMultiBody::getJointVel(int i) const
    389 {
    390     return m_realBuf[6 + i];
    391 }
    392 
    393 btScalar * btMultiBody::getJointPosMultiDof(int i)
    394 {
    395 	return &m_links[i].m_jointPos[0];
    396 }
    397 
    398 btScalar * btMultiBody::getJointVelMultiDof(int i)
    399 {
    400 	return &m_realBuf[6 + m_links[i].m_dofOffset];
    401 }
    402 
    403 const btScalar * btMultiBody::getJointPosMultiDof(int i) const
    404 {
    405 	return &m_links[i].m_jointPos[0];
    406 }
    407 
    408 const btScalar * btMultiBody::getJointVelMultiDof(int i) const
    409 {
    410 	return &m_realBuf[6 + m_links[i].m_dofOffset];
    411 }
    412 
    413 
    414 void btMultiBody::setJointPos(int i, btScalar q)
    415 {
    416     m_links[i].m_jointPos[0] = q;
    417     m_links[i].updateCache();
    418 }
    419 
    420 void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
    421 {
    422 	for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
    423 		m_links[i].m_jointPos[pos] = q[pos];
    424 
    425     m_links[i].updateCacheMultiDof();
    426 }
    427 
    428 void btMultiBody::setJointVel(int i, btScalar qdot)
    429 {
    430     m_realBuf[6 + i] = qdot;
    431 }
    432 
    433 void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
    434 {
    435 	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    436 		m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
    437 }
    438 
    439 const btVector3 & btMultiBody::getRVector(int i) const
    440 {
    441     return m_links[i].m_cachedRVector;
    442 }
    443 
    444 const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
    445 {
    446     return m_links[i].m_cachedRotParentToThis;
    447 }
    448 
    449 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
    450 {
    451     btVector3 result = local_pos;
    452     while (i != -1) {
    453         // 'result' is in frame i. transform it to frame parent(i)
    454         result += getRVector(i);
    455         result = quatRotate(getParentToLocalRot(i).inverse(),result);
    456         i = getParent(i);
    457     }
    458 
    459     // 'result' is now in the base frame. transform it to world frame
    460     result = quatRotate(getWorldToBaseRot().inverse() ,result);
    461     result += getBasePos();
    462 
    463     return result;
    464 }
    465 
    466 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
    467 {
    468     if (i == -1) {
    469         // world to base
    470         return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
    471     } else {
    472         // find position in parent frame, then transform to current frame
    473         return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
    474     }
    475 }
    476 
    477 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
    478 {
    479     btVector3 result = local_dir;
    480     while (i != -1) {
    481         result = quatRotate(getParentToLocalRot(i).inverse() , result);
    482         i = getParent(i);
    483     }
    484     result = quatRotate(getWorldToBaseRot().inverse() , result);
    485     return result;
    486 }
    487 
    488 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
    489 {
    490     if (i == -1) {
    491         return quatRotate(getWorldToBaseRot(), world_dir);
    492     } else {
    493         return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
    494     }
    495 }
    496 
    497 void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
    498 {
    499 	int num_links = getNumLinks();
    500     // Calculates the velocities of each link (and the base) in its local frame
    501     omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
    502     vel[0] = quatRotate(m_baseQuat ,getBaseVel());
    503 
    504     for (int i = 0; i < num_links; ++i) {
    505         const int parent = m_links[i].m_parent;
    506 
    507         // transform parent vel into this frame, store in omega[i+1], vel[i+1]
    508         SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
    509                          omega[parent+1], vel[parent+1],
    510                          omega[i+1], vel[i+1]);
    511 
    512         // now add qidot * shat_i
    513         omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
    514         vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
    515     }
    516 }
    517 
    518 btScalar btMultiBody::getKineticEnergy() const
    519 {
    520 	int num_links = getNumLinks();
    521     // TODO: would be better not to allocate memory here
    522     btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
    523 	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
    524     compTreeLinkVelocities(&omega[0], &vel[0]);
    525 
    526     // we will do the factor of 0.5 at the end
    527     btScalar result = m_baseMass * vel[0].dot(vel[0]);
    528     result += omega[0].dot(m_baseInertia * omega[0]);
    529 
    530     for (int i = 0; i < num_links; ++i) {
    531         result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
    532         result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
    533     }
    534 
    535     return 0.5f * result;
    536 }
    537 
    538 btVector3 btMultiBody::getAngularMomentum() const
    539 {
    540 	int num_links = getNumLinks();
    541     // TODO: would be better not to allocate memory here
    542     btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
    543 	btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
    544     btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
    545     compTreeLinkVelocities(&omega[0], &vel[0]);
    546 
    547     rot_from_world[0] = m_baseQuat;
    548     btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
    549 
    550     for (int i = 0; i < num_links; ++i) {
    551         rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
    552         result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
    553     }
    554 
    555     return result;
    556 }
    557 
    558 void btMultiBody::clearConstraintForces()
    559 {
    560 	m_baseConstraintForce.setValue(0, 0, 0);
    561 	m_baseConstraintTorque.setValue(0, 0, 0);
    562 
    563 
    564     for (int i = 0; i < getNumLinks(); ++i) {
    565         m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
    566         m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
    567     }
    568 }
    569 void btMultiBody::clearForcesAndTorques()
    570 {
    571     m_baseForce.setValue(0, 0, 0);
    572     m_baseTorque.setValue(0, 0, 0);
    573 
    574 
    575     for (int i = 0; i < getNumLinks(); ++i) {
    576         m_links[i].m_appliedForce.setValue(0, 0, 0);
    577         m_links[i].m_appliedTorque.setValue(0, 0, 0);
    578 		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;
    579     }
    580 }
    581 
    582 void btMultiBody::clearVelocities()
    583 {
    584 	for (int i = 0; i < 6 + getNumLinks(); ++i)
    585 	{
    586 		m_realBuf[i] = 0.f;
    587 	}
    588 }
    589 void btMultiBody::addLinkForce(int i, const btVector3 &f)
    590 {
    591     m_links[i].m_appliedForce += f;
    592 }
    593 
    594 void btMultiBody::addLinkTorque(int i, const btVector3 &t)
    595 {
    596     m_links[i].m_appliedTorque += t;
    597 }
    598 
    599 void btMultiBody::addLinkConstraintForce(int i, const btVector3 &f)
    600 {
    601     m_links[i].m_appliedConstraintForce += f;
    602 }
    603 
    604 void btMultiBody::addLinkConstraintTorque(int i, const btVector3 &t)
    605 {
    606     m_links[i].m_appliedConstraintTorque += t;
    607 }
    608 
    609 
    610 
    611 void btMultiBody::addJointTorque(int i, btScalar Q)
    612 {
    613     m_links[i].m_jointTorque[0] += Q;
    614 }
    615 
    616 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
    617 {
    618 	m_links[i].m_jointTorque[dof] += Q;
    619 }
    620 
    621 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
    622 {
    623 	for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    624 		m_links[i].m_jointTorque[dof] = Q[dof];
    625 }
    626 
    627 const btVector3 & btMultiBody::getLinkForce(int i) const
    628 {
    629     return m_links[i].m_appliedForce;
    630 }
    631 
    632 const btVector3 & btMultiBody::getLinkTorque(int i) const
    633 {
    634     return m_links[i].m_appliedTorque;
    635 }
    636 
    637 btScalar btMultiBody::getJointTorque(int i) const
    638 {
    639     return m_links[i].m_jointTorque[0];
    640 }
    641 
    642 btScalar * btMultiBody::getJointTorqueMultiDof(int i)
    643 {
    644     return &m_links[i].m_jointTorque[0];
    645 }
    646 
    647 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?
    648 {
    649 		btVector3 row0 = btVector3(
    650 			v0.x() * v1.x(),
    651 			v0.x() * v1.y(),
    652 			v0.x() * v1.z());
    653 		btVector3 row1 = btVector3(
    654 			v0.y() * v1.x(),
    655 			v0.y() * v1.y(),
    656 			v0.y() * v1.z());
    657 		btVector3 row2 = btVector3(
    658 			v0.z() * v1.x(),
    659 			v0.z() * v1.y(),
    660 			v0.z() * v1.z());
    661 
    662         btMatrix3x3 m(row0[0],row0[1],row0[2],
    663 						row1[0],row1[1],row1[2],
    664 						row2[0],row2[1],row2[2]);
    665 		return m;
    666 }
    667 
    668 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
    669 //
    670 
    671 void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
    672                                btAlignedObjectArray<btScalar> &scratch_r,
    673                                btAlignedObjectArray<btVector3> &scratch_v,
    674                                btAlignedObjectArray<btMatrix3x3> &scratch_m,
    675 				bool isConstraintPass)
    676 {
    677     // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
    678     // and the base linear & angular accelerations.
    679 
    680     // We apply damping forces in this routine as well as any external forces specified by the
    681     // caller (via addBaseForce etc).
    682 
    683     // output should point to an array of 6 + num_links reals.
    684     // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
    685     // num_links joint acceleration values.
    686 
    687 	m_internalNeedsJointFeedback = false;
    688 
    689 	int num_links = getNumLinks();
    690 
    691     const btScalar DAMPING_K1_LINEAR = m_linearDamping;
    692 	const btScalar DAMPING_K2_LINEAR = m_linearDamping;
    693 
    694 	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
    695 	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
    696 
    697     btVector3 base_vel = getBaseVel();
    698     btVector3 base_omega = getBaseOmega();
    699 
    700     // Temporary matrices/vectors -- use scratch space from caller
    701     // so that we don't have to keep reallocating every frame
    702 
    703     scratch_r.resize(2*m_dofCount + 6);				//multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
    704     scratch_v.resize(8*num_links + 6);
    705     scratch_m.resize(4*num_links + 4);
    706 
    707 	//btScalar * r_ptr = &scratch_r[0];
    708     btScalar * output = &scratch_r[m_dofCount];  // "output" holds the q_double_dot results
    709     btVector3 * v_ptr = &scratch_v[0];
    710 
    711     // vhat_i  (top = angular, bottom = linear part)
    712 	btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
    713 	v_ptr += num_links * 2 + 2;
    714 	//
    715     // zhat_i^A
    716 	btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
    717 	v_ptr += num_links * 2 + 2;
    718 	//
    719     // chat_i  (note NOT defined for the base)
    720 	btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
    721 	v_ptr += num_links * 2;
    722 	//
    723     // Ihat_i^A.
    724 	btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
    725 
    726     // Cached 3x3 rotation matrices from parent frame to this frame.
    727     btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
    728     btMatrix3x3 * rot_from_world = &scratch_m[0];
    729 
    730     // hhat_i, ahat_i
    731     // hhat is NOT stored for the base (but ahat is)
    732 	btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
    733 	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
    734 	v_ptr += num_links * 2 + 2;
    735 	//
    736     // Y_i, invD_i
    737     btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
    738 	btScalar * Y = &scratch_r[0];
    739 	//
    740 	//aux variables
    741 	static btSpatialMotionVector spatJointVel;					//spatial velocity due to the joint motion (i.e. without predecessors' influence)
    742 	static btScalar D[36];										//"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
    743 	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
    744 	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
    745 	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
    746 	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
    747 	static btSpatialTransformationMatrix fromParent;				//spatial transform from parent to child
    748 	static btSymmetricSpatialDyad dyadTemp;						//inertia matrix temp
    749 	static btSpatialTransformationMatrix fromWorld;
    750 	fromWorld.m_trnVec.setZero();
    751 	/////////////////
    752 
    753     // ptr to the joint accel part of the output
    754     btScalar * joint_accel = output + 6;
    755 
    756     // Start of the algorithm proper.
    757 
    758     // First 'upward' loop.
    759     // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
    760 
    761     rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
    762 
    763 	//create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
    764 	spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
    765 
    766     if (m_fixedBase)
    767 	{
    768 		zeroAccSpatFrc[0].setZero();
    769     }
    770 	else
    771 	{
    772 		btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
    773 		btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
    774 		//external forces
    775 		zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
    776 
    777 		//adding damping terms (only)
    778 		btScalar linDampMult = 1., angDampMult = 1.;
    779 		zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
    780 								   linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
    781 
    782 		//
    783 		//p += vhat x Ihat vhat - done in a simpler way
    784 		if (m_useGyroTerm)
    785 			zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
    786 		//
    787 		zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
    788     }
    789 
    790 
    791 	//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
    792 	spatInertia[0].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
    793 								//
    794 								btMatrix3x3(m_baseMass, 0, 0,
    795 											0, m_baseMass, 0,
    796 											0, 0, m_baseMass),
    797 								//
    798 								btMatrix3x3(m_baseInertia[0], 0, 0,
    799 											0, m_baseInertia[1], 0,
    800 											0, 0, m_baseInertia[2])
    801 							);
    802 
    803     rot_from_world[0] = rot_from_parent[0];
    804 
    805 	//
    806     for (int i = 0; i < num_links; ++i) {
    807         const int parent = m_links[i].m_parent;
    808         rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
    809         rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
    810 
    811 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
    812 		fromWorld.m_rotMat = rot_from_world[i+1];
    813 		fromParent.transform(spatVel[parent+1], spatVel[i+1]);
    814 
    815 		// now set vhat_i to its true value by doing
    816         // vhat_i += qidot * shat_i
    817 		if(!m_useGlobalVelocities)
    818 		{
    819 			spatJointVel.setZero();
    820 
    821 			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    822 				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
    823 
    824 			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
    825 			spatVel[i+1] += spatJointVel;
    826 
    827 			//
    828 			// vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
    829 			//spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
    830 
    831 		}
    832 		else
    833 		{
    834 			fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
    835 			fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
    836 		}
    837 
    838 		// we can now calculate chat_i
    839 		spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);
    840 
    841         // calculate zhat_i^A
    842 		//
    843 		//external forces
    844 		btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
    845 		btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
    846 
    847 		zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
    848 
    849 #if 0
    850 		{
    851 
    852 			b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
    853 			i+1,
    854 			zeroAccSpatFrc[i+1].m_topVec[0],
    855 			zeroAccSpatFrc[i+1].m_topVec[1],
    856 			zeroAccSpatFrc[i+1].m_topVec[2],
    857 
    858 			zeroAccSpatFrc[i+1].m_bottomVec[0],
    859 			zeroAccSpatFrc[i+1].m_bottomVec[1],
    860 			zeroAccSpatFrc[i+1].m_bottomVec[2]);
    861 		}
    862 #endif
    863 		//
    864 		//adding damping terms (only)
    865 		btScalar linDampMult = 1., angDampMult = 1.;
    866 		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()),
    867 									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
    868 
    869         // calculate Ihat_i^A
    870 		//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
    871 		spatInertia[i+1].setMatrix(	btMatrix3x3(0,0,0,0,0,0,0,0,0),
    872 									//
    873 									btMatrix3x3(m_links[i].m_mass, 0, 0,
    874 												0, m_links[i].m_mass, 0,
    875 												0, 0, m_links[i].m_mass),
    876 									//
    877 									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
    878 												0, m_links[i].m_inertiaLocal[1], 0,
    879 												0, 0, m_links[i].m_inertiaLocal[2])
    880 								);
    881 		//
    882 		//p += vhat x Ihat vhat - done in a simpler way
    883 		if(m_useGyroTerm)
    884 			zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
    885 		//
    886 		zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
    887 		//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
    888 		////clamp parent's omega
    889 		//btScalar parOmegaMod = temp.length();
    890 		//btScalar parOmegaModMax = 1000;
    891 		//if(parOmegaMod > parOmegaModMax)
    892 		//	temp *= parOmegaModMax / parOmegaMod;
    893 		//zeroAccSpatFrc[i+1].addLinear(temp);
    894 		//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
    895 		//temp = spatCoriolisAcc[i].getLinear();
    896 		//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
    897 
    898 
    899 
    900 		//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());
    901 		//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());
    902 		//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
    903     }
    904 
    905     // 'Downward' loop.
    906     // (part of TreeForwardDynamics in Mirtich.)
    907     for (int i = num_links - 1; i >= 0; --i)
    908 	{
    909 		const int parent = m_links[i].m_parent;
    910 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
    911 
    912 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    913 		{
    914 			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
    915 			//
    916 			hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
    917 			//
    918 			Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
    919 			- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
    920 			- spatCoriolisAcc[i].dot(hDof)
    921 			;
    922 		}
    923 
    924 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    925 		{
    926 			btScalar *D_row = &D[dof * m_links[i].m_dofCount];
    927 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
    928 			{
    929 				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
    930 				D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
    931 			}
    932 		}
    933 
    934         btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
    935 		switch(m_links[i].m_jointType)
    936 		{
    937 			case btMultibodyLink::ePrismatic:
    938 			case btMultibodyLink::eRevolute:
    939 			{
    940 				invDi[0] = 1.0f / D[0];
    941 				break;
    942 			}
    943 			case btMultibodyLink::eSpherical:
    944 			case btMultibodyLink::ePlanar:
    945 			{
    946 				static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
    947 				static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
    948 
    949 				//unroll the loop?
    950 				for(int row = 0; row < 3; ++row)
    951 				{
    952 					for(int col = 0; col < 3; ++col)
    953 					{
    954 						invDi[row * 3 + col] = invD3x3[row][col];
    955 					}
    956 				}
    957 
    958 				break;
    959 			}
    960 			default:
    961 			{
    962 
    963 			}
    964 		}
    965 
    966 		//determine h*D^{-1}
    967 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    968 		{
    969 			spatForceVecTemps[dof].setZero();
    970 
    971 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
    972 			{
    973 				btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
    974 				//
    975 				spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
    976 			}
    977 		}
    978 
    979 		dyadTemp = spatInertia[i+1];
    980 
    981 		//determine (h*D^{-1}) * h^{T}
    982 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    983 		{
    984 			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
    985 			//
    986 			dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
    987 		}
    988 
    989 		fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
    990 
    991 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
    992 		{
    993 			invD_times_Y[dof] = 0.f;
    994 
    995 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
    996 			{
    997 				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
    998 			}
    999 		}
   1000 
   1001 		spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
   1002 
   1003 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1004 		{
   1005 			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
   1006 			//
   1007 			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
   1008 		}
   1009 
   1010 		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
   1011 
   1012 		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
   1013     }
   1014 
   1015 
   1016     // Second 'upward' loop
   1017     // (part of TreeForwardDynamics in Mirtich)
   1018 
   1019     if (m_fixedBase)
   1020 	{
   1021         spatAcc[0].setZero();
   1022     }
   1023 	else
   1024 	{
   1025         if (num_links > 0)
   1026 		{
   1027 			m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
   1028 			m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
   1029 			m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
   1030 			m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
   1031 
   1032         }
   1033 
   1034 		solveImatrix(zeroAccSpatFrc[0], result);
   1035 		spatAcc[0] = -result;
   1036     }
   1037 
   1038 
   1039     // now do the loop over the m_links
   1040     for (int i = 0; i < num_links; ++i)
   1041 	{
   1042 		//	qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
   1043 		//	a = apar + cor + Sqdd
   1044 		//or
   1045 		//	qdd = D^{-1} * (Y - h^{T}*(apar+cor))
   1046 		//	a = apar + Sqdd
   1047 
   1048         const int parent = m_links[i].m_parent;
   1049 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
   1050 
   1051 		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
   1052 
   1053 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1054 		{
   1055 			btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
   1056 			//
   1057 			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
   1058 		}
   1059 
   1060 		btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
   1061 		//D^{-1} * (Y - h^{T}*apar)
   1062 		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]);
   1063 
   1064 		spatAcc[i+1] += spatCoriolisAcc[i];
   1065 
   1066 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1067 			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
   1068 
   1069 		if (m_links[i].m_jointFeedback)
   1070 		{
   1071 			m_internalNeedsJointFeedback = true;
   1072 
   1073 			btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
   1074 			btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
   1075 
   1076 			if (gJointFeedbackInJointFrame)
   1077 			{
   1078 				//shift the reaction forces to the joint frame
   1079 				//linear (force) component is the same
   1080 				//shift the angular (torque, moment) component using the relative position,  m_links[i].m_dVector
   1081 				 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
   1082 			}
   1083 
   1084 
   1085 			if (gJointFeedbackInWorldSpace)
   1086 			{
   1087 				if (isConstraintPass)
   1088 				{
   1089  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
   1090                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
   1091 				} else
   1092 				{
   1093 					m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
   1094 					m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
   1095 				}
   1096 			} else
   1097 			{
   1098 				if (isConstraintPass)
   1099 				{
   1100 					  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
   1101                                 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
   1102 
   1103 				}
   1104 				else
   1105 				{
   1106 				m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
   1107 				m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
   1108 				}
   1109 			}
   1110 	}
   1111 
   1112     }
   1113 
   1114     // transform base accelerations back to the world frame.
   1115     btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
   1116 	output[0] = omegadot_out[0];
   1117 	output[1] = omegadot_out[1];
   1118 	output[2] = omegadot_out[2];
   1119 
   1120     btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
   1121 	output[3] = vdot_out[0];
   1122 	output[4] = vdot_out[1];
   1123 	output[5] = vdot_out[2];
   1124 
   1125 	/////////////////
   1126 	//printf("q = [");
   1127 	//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());
   1128 	//for(int link = 0; link < getNumLinks(); ++link)
   1129 	//	for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
   1130 	//		printf("%.6f ", m_links[link].m_jointPos[dof]);
   1131 	//printf("]\n");
   1132 	////
   1133 	//printf("qd = [");
   1134 	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
   1135 	//	printf("%.6f ", m_realBuf[dof]);
   1136 	//printf("]\n");
   1137 	//printf("qdd = [");
   1138 	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
   1139 	//	printf("%.6f ", output[dof]);
   1140 	//printf("]\n");
   1141 	/////////////////
   1142 
   1143     // Final step: add the accelerations (times dt) to the velocities.
   1144 
   1145 	if (!isConstraintPass)
   1146 	{
   1147 	if(dt > 0.)
   1148 		applyDeltaVeeMultiDof(output, dt);
   1149 
   1150 	}
   1151 	/////
   1152 	//btScalar angularThres = 1;
   1153 	//btScalar maxAngVel = 0.;
   1154 	//bool scaleDown = 1.;
   1155 	//for(int link = 0; link < m_links.size(); ++link)
   1156 	//{
   1157 	//	if(spatVel[link+1].getAngular().length() > maxAngVel)
   1158 	//	{
   1159 	//		maxAngVel = spatVel[link+1].getAngular().length();
   1160 	//		scaleDown = angularThres / spatVel[link+1].getAngular().length();
   1161 	//		break;
   1162 	//	}
   1163 	//}
   1164 
   1165 	//if(scaleDown != 1.)
   1166 	//{
   1167 	//	for(int link = 0; link < m_links.size(); ++link)
   1168 	//	{
   1169 	//		if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
   1170 	//		{
   1171 	//			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
   1172 	//				getJointVelMultiDof(link)[dof] *= scaleDown;
   1173 	//		}
   1174 	//	}
   1175 	//}
   1176 	/////
   1177 
   1178 	/////////////////////
   1179 	if(m_useGlobalVelocities)
   1180 	{
   1181 		for (int i = 0; i < num_links; ++i)
   1182 		{
   1183 			const int parent = m_links[i].m_parent;
   1184 			//rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);    /// <- done
   1185 			//rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];		/// <- done
   1186 
   1187 			fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
   1188 			fromWorld.m_rotMat = rot_from_world[i+1];
   1189 
   1190 			// vhat_i = i_xhat_p(i) * vhat_p(i)
   1191 			fromParent.transform(spatVel[parent+1], spatVel[i+1]);
   1192 			//nice alternative below (using operator *) but it generates temps
   1193 			/////////////////////////////////////////////////////////////
   1194 
   1195 			// now set vhat_i to its true value by doing
   1196 			// vhat_i += qidot * shat_i
   1197 			spatJointVel.setZero();
   1198 
   1199 			for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1200 				spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
   1201 
   1202 			// remember vhat_i is really vhat_p(i) (but in current frame) at this point	=> we need to add velocity across the inboard joint
   1203 			spatVel[i+1] += spatJointVel;
   1204 
   1205 
   1206 			fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
   1207 			fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
   1208 		}
   1209 	}
   1210 
   1211 }
   1212 
   1213 
   1214 void btMultiBody::stepVelocities(btScalar dt,
   1215                                btAlignedObjectArray<btScalar> &scratch_r,
   1216                                btAlignedObjectArray<btVector3> &scratch_v,
   1217                                btAlignedObjectArray<btMatrix3x3> &scratch_m)
   1218 {
   1219     // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
   1220     // and the base linear & angular accelerations.
   1221 
   1222     // We apply damping forces in this routine as well as any external forces specified by the
   1223     // caller (via addBaseForce etc).
   1224 
   1225     // output should point to an array of 6 + num_links reals.
   1226     // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
   1227     // num_links joint acceleration values.
   1228 
   1229 	int num_links = getNumLinks();
   1230 
   1231     const btScalar DAMPING_K1_LINEAR = m_linearDamping;
   1232 	const btScalar DAMPING_K2_LINEAR = m_linearDamping;
   1233 
   1234 	const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
   1235 	const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
   1236 
   1237     btVector3 base_vel = getBaseVel();
   1238     btVector3 base_omega = getBaseOmega();
   1239 
   1240     // Temporary matrices/vectors -- use scratch space from caller
   1241     // so that we don't have to keep reallocating every frame
   1242 
   1243     scratch_r.resize(2*num_links + 6);
   1244     scratch_v.resize(8*num_links + 6);
   1245     scratch_m.resize(4*num_links + 4);
   1246 
   1247     btScalar * r_ptr = &scratch_r[0];
   1248     btScalar * output = &scratch_r[num_links];  // "output" holds the q_double_dot results
   1249     btVector3 * v_ptr = &scratch_v[0];
   1250 
   1251     // vhat_i  (top = angular, bottom = linear part)
   1252     btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
   1253     btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
   1254 
   1255     // zhat_i^A
   1256     btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
   1257     btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
   1258 
   1259     // chat_i  (note NOT defined for the base)
   1260     btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
   1261     btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
   1262 
   1263     // top left, top right and bottom left blocks of Ihat_i^A.
   1264     // bottom right block = transpose of top left block and is not stored.
   1265     // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
   1266     btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
   1267     btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
   1268     btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
   1269 
   1270     // Cached 3x3 rotation matrices from parent frame to this frame.
   1271     btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
   1272     btMatrix3x3 * rot_from_world = &scratch_m[0];
   1273 
   1274     // hhat_i, ahat_i
   1275     // hhat is NOT stored for the base (but ahat is)
   1276     btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
   1277     btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
   1278     btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
   1279     btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
   1280 
   1281     // Y_i, D_i
   1282     btScalar * Y1 = r_ptr; r_ptr += num_links;
   1283     btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
   1284 
   1285     // ptr to the joint accel part of the output
   1286     btScalar * joint_accel = output + 6;
   1287 
   1288 
   1289     // Start of the algorithm proper.
   1290 
   1291     // First 'upward' loop.
   1292     // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
   1293 
   1294     rot_from_parent[0] = btMatrix3x3(m_baseQuat);
   1295 
   1296     vel_top_angular[0] = rot_from_parent[0] * base_omega;
   1297     vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
   1298 
   1299     if (m_fixedBase) {
   1300         f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
   1301     } else {
   1302         f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
   1303                                                    - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
   1304 
   1305         f_zero_acc_bottom_linear[0] =
   1306             - (rot_from_parent[0] * m_baseTorque);
   1307 
   1308 		if (m_useGyroTerm)
   1309 			f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
   1310 
   1311         f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
   1312 
   1313     }
   1314 
   1315 
   1316 
   1317     inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
   1318 
   1319 
   1320     inertia_top_right[0].setValue(m_baseMass, 0, 0,
   1321                             0, m_baseMass, 0,
   1322                             0, 0, m_baseMass);
   1323     inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
   1324                               0, m_baseInertia[1], 0,
   1325                               0, 0, m_baseInertia[2]);
   1326 
   1327     rot_from_world[0] = rot_from_parent[0];
   1328 
   1329     for (int i = 0; i < num_links; ++i) {
   1330         const int parent = m_links[i].m_parent;
   1331         rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
   1332 
   1333 
   1334         rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
   1335 
   1336         // vhat_i = i_xhat_p(i) * vhat_p(i)
   1337         SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
   1338                          vel_top_angular[parent+1], vel_bottom_linear[parent+1],
   1339                          vel_top_angular[i+1], vel_bottom_linear[i+1]);
   1340 
   1341         // we can now calculate chat_i
   1342         // remember vhat_i is really vhat_p(i) (but in current frame) at this point
   1343         coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector))
   1344             + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
   1345 		if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
   1346             coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
   1347             coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
   1348         } else {
   1349             coriolis_top_angular[i] = btVector3(0,0,0);
   1350         }
   1351 
   1352         // now set vhat_i to its true value by doing
   1353         // vhat_i += qidot * shat_i
   1354         vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
   1355         vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
   1356 
   1357         // calculate zhat_i^A
   1358         f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
   1359         f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
   1360 
   1361         f_zero_acc_bottom_linear[i+1] =
   1362             - (rot_from_world[i+1] * m_links[i].m_appliedTorque);
   1363 		if (m_useGyroTerm)
   1364 		{
   1365 			f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
   1366 		}
   1367 
   1368         f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
   1369 
   1370         // calculate Ihat_i^A
   1371         inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
   1372         inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
   1373                                   0, m_links[i].m_mass, 0,
   1374                                   0, 0, m_links[i].m_mass);
   1375         inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
   1376                                     0, m_links[i].m_inertiaLocal[1], 0,
   1377                                     0, 0, m_links[i].m_inertiaLocal[2]);
   1378     }
   1379 
   1380 
   1381     // 'Downward' loop.
   1382     // (part of TreeForwardDynamics in Mirtich.)
   1383     for (int i = num_links - 1; i >= 0; --i) {
   1384 
   1385         h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
   1386         h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
   1387 		btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
   1388         D[i] = val;
   1389 		//Y1 = joint torque - zero acceleration force - coriolis force
   1390 		Y1[i] = m_links[i].m_jointTorque[0]
   1391             - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
   1392             - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
   1393 
   1394         const int parent = m_links[i].m_parent;
   1395 
   1396         btAssert(D[i]!=0.f);
   1397         // Ip += pXi * (Ii - hi hi' / Di) * iXp
   1398         const btScalar one_over_di = 1.0f / D[i];
   1399 
   1400 
   1401 
   1402 
   1403 		const btMatrix3x3 TL = inertia_top_left[i+1]   - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
   1404         const btMatrix3x3 TR = inertia_top_right[i+1]  - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
   1405         const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
   1406 
   1407 
   1408         btMatrix3x3 r_cross;
   1409         r_cross.setValue(
   1410             0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
   1411             m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
   1412             -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
   1413 
   1414         inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
   1415         inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
   1416         inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
   1417             (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
   1418 
   1419 
   1420         // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
   1421         btVector3 in_top, in_bottom, out_top, out_bottom;
   1422         const btScalar Y_over_D = Y1[i] * one_over_di;
   1423         in_top = f_zero_acc_top_angular[i+1]
   1424             + inertia_top_left[i+1] * coriolis_top_angular[i]
   1425             + inertia_top_right[i+1] * coriolis_bottom_linear[i]
   1426             + Y_over_D * h_top[i];
   1427         in_bottom = f_zero_acc_bottom_linear[i+1]
   1428             + inertia_bottom_left[i+1] * coriolis_top_angular[i]
   1429             + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
   1430             + Y_over_D * h_bottom[i];
   1431         InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
   1432                                 in_top, in_bottom, out_top, out_bottom);
   1433         f_zero_acc_top_angular[parent+1] += out_top;
   1434         f_zero_acc_bottom_linear[parent+1] += out_bottom;
   1435     }
   1436 
   1437 
   1438     // Second 'upward' loop
   1439     // (part of TreeForwardDynamics in Mirtich)
   1440 
   1441     if (m_fixedBase)
   1442 	{
   1443         accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
   1444     }
   1445 	else
   1446 	{
   1447         if (num_links > 0)
   1448 		{
   1449             //Matrix<btScalar, 6, 6> Imatrix;
   1450             //Imatrix.block<3,3>(0,0) = inertia_top_left[0];
   1451             //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
   1452             //Imatrix.block<3,3>(0,3) = inertia_top_right[0];
   1453             //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
   1454             //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix));  // TODO: Avoid memory allocation here?
   1455 
   1456 			m_cachedInertiaTopLeft = inertia_top_left[0];
   1457 			m_cachedInertiaTopRight = inertia_top_right[0];
   1458 			m_cachedInertiaLowerLeft = inertia_bottom_left[0];
   1459 			m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
   1460 
   1461         }
   1462 		btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
   1463 		btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
   1464         float result[6];
   1465 
   1466 		solveImatrix(rhs_top, rhs_bot, result);
   1467 //		printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
   1468         for (int i = 0; i < 3; ++i) {
   1469             accel_top[0][i] = -result[i];
   1470             accel_bottom[0][i] = -result[i+3];
   1471         }
   1472 
   1473     }
   1474 
   1475     // now do the loop over the m_links
   1476     for (int i = 0; i < num_links; ++i)
   1477 	{
   1478 		//acceleration of the child link = acceleration of the parent transformed into child frame +
   1479 		//							+ coriolis acceleration
   1480 		//							+ joint acceleration
   1481         const int parent = m_links[i].m_parent;
   1482         SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
   1483                          accel_top[parent+1], accel_bottom[parent+1],
   1484                          accel_top[i+1], accel_bottom[i+1]);
   1485 
   1486 
   1487         joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
   1488         accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
   1489         accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
   1490     }
   1491 
   1492     // transform base accelerations back to the world frame.
   1493     btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
   1494 	output[0] = omegadot_out[0];
   1495 	output[1] = omegadot_out[1];
   1496 	output[2] = omegadot_out[2];
   1497 
   1498     btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
   1499 	output[3] = vdot_out[0];
   1500 	output[4] = vdot_out[1];
   1501 	output[5] = vdot_out[2];
   1502 
   1503 	/////////////////
   1504 	//printf("q = [");
   1505 	//printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
   1506 	//for(int link = 0; link < getNumLinks(); ++link)
   1507 	//	printf("%.2f ", m_links[link].m_jointPos[0]);
   1508 	//printf("]\n");
   1509 	////
   1510 	//printf("qd = [");
   1511 	//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
   1512 	//	printf("%.2f ", m_realBuf[dof]);
   1513 	//printf("]\n");
   1514 	////
   1515 	//printf("qdd = [");
   1516 	//for(int dof = 0; dof < getNumLinks() + 6; ++dof)
   1517 	//	printf("%.2f ", output[dof]);
   1518 	//printf("]\n");
   1519 	/////////////////
   1520 
   1521     // Final step: add the accelerations (times dt) to the velocities.
   1522 	//todo: do we already need to update the velocities, or can we move this into the constraint solver?
   1523 	//the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
   1524     applyDeltaVee(output, dt);
   1525 
   1526 
   1527 }
   1528 
   1529 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
   1530 {
   1531 	int num_links = getNumLinks();
   1532 	///solve I * x = rhs, so the result = invI * rhs
   1533     if (num_links == 0)
   1534 	{
   1535 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
   1536         result[0] = rhs_bot[0] / m_baseInertia[0];
   1537         result[1] = rhs_bot[1] / m_baseInertia[1];
   1538         result[2] = rhs_bot[2] / m_baseInertia[2];
   1539         result[3] = rhs_top[0] / m_baseMass;
   1540         result[4] = rhs_top[1] / m_baseMass;
   1541         result[5] = rhs_top[2] / m_baseMass;
   1542     } else
   1543 	{
   1544 		/// Special routine for calculating the inverse of a spatial inertia matrix
   1545 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
   1546 		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
   1547 		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
   1548 		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
   1549 		tmp = invIupper_right * m_cachedInertiaLowerRight;
   1550 		btMatrix3x3 invI_upper_left = (tmp * Binv);
   1551 		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
   1552 		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
   1553 		tmp[0][0]-= 1.0;
   1554 		tmp[1][1]-= 1.0;
   1555 		tmp[2][2]-= 1.0;
   1556 		btMatrix3x3 invI_lower_left = (Binv * tmp);
   1557 
   1558 		//multiply result = invI * rhs
   1559 		{
   1560 		  btVector3 vtop = invI_upper_left*rhs_top;
   1561 		  btVector3 tmp;
   1562 		  tmp = invIupper_right * rhs_bot;
   1563 		  vtop += tmp;
   1564 		  btVector3 vbot = invI_lower_left*rhs_top;
   1565 		  tmp = invI_lower_right * rhs_bot;
   1566 		  vbot += tmp;
   1567 		  result[0] = vtop[0];
   1568 		  result[1] = vtop[1];
   1569 		  result[2] = vtop[2];
   1570 		  result[3] = vbot[0];
   1571 		  result[4] = vbot[1];
   1572 		  result[5] = vbot[2];
   1573 		}
   1574 
   1575     }
   1576 }
   1577 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
   1578 {
   1579 	int num_links = getNumLinks();
   1580 	///solve I * x = rhs, so the result = invI * rhs
   1581     if (num_links == 0)
   1582 	{
   1583 		// in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
   1584 		result.setAngular(rhs.getAngular() / m_baseInertia);
   1585 		result.setLinear(rhs.getLinear() / m_baseMass);
   1586     } else
   1587 	{
   1588 		/// Special routine for calculating the inverse of a spatial inertia matrix
   1589 		///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
   1590 		btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
   1591 		btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
   1592 		btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
   1593 		tmp = invIupper_right * m_cachedInertiaLowerRight;
   1594 		btMatrix3x3 invI_upper_left = (tmp * Binv);
   1595 		btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
   1596 		tmp = m_cachedInertiaTopLeft  * invI_upper_left;
   1597 		tmp[0][0]-= 1.0;
   1598 		tmp[1][1]-= 1.0;
   1599 		tmp[2][2]-= 1.0;
   1600 		btMatrix3x3 invI_lower_left = (Binv * tmp);
   1601 
   1602 		//multiply result = invI * rhs
   1603 		{
   1604 		  btVector3 vtop = invI_upper_left*rhs.getLinear();
   1605 		  btVector3 tmp;
   1606 		  tmp = invIupper_right * rhs.getAngular();
   1607 		  vtop += tmp;
   1608 		  btVector3 vbot = invI_lower_left*rhs.getLinear();
   1609 		  tmp = invI_lower_right * rhs.getAngular();
   1610 		  vbot += tmp;
   1611 		  result.setVector(vtop, vbot);
   1612 		}
   1613 
   1614     }
   1615 }
   1616 
   1617 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
   1618 {
   1619 	for (int row = 0; row < rowsA; row++)
   1620 	{
   1621 		for (int col = 0; col < colsB; col++)
   1622 		{
   1623 			pC[row * colsB + col] = 0.f;
   1624 			for (int inner = 0; inner < rowsB; inner++)
   1625 			{
   1626 				pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
   1627 			}
   1628 		}
   1629 	}
   1630 }
   1631 
   1632 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
   1633                                        btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
   1634 {
   1635     // Temporary matrices/vectors -- use scratch space from caller
   1636     // so that we don't have to keep reallocating every frame
   1637 
   1638 
   1639 	int num_links = getNumLinks();
   1640     scratch_r.resize(m_dofCount);
   1641     scratch_v.resize(4*num_links + 4);
   1642 
   1643     btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
   1644     btVector3 * v_ptr = &scratch_v[0];
   1645 
   1646     // zhat_i^A (scratch space)
   1647     btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
   1648 	v_ptr += num_links * 2 + 2;
   1649 
   1650     // rot_from_parent (cached from calcAccelerations)
   1651     const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
   1652 
   1653     // hhat (cached), accel (scratch)
   1654     // hhat is NOT stored for the base (but ahat is)
   1655 	const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
   1656 	btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
   1657 	v_ptr += num_links * 2 + 2;
   1658 
   1659     // Y_i (scratch), invD_i (cached)
   1660     const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
   1661 	btScalar * Y = r_ptr;
   1662 	////////////////
   1663 	//aux variables
   1664 	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
   1665 	static btSpatialMotionVector result;							//holds results of the SolveImatrix op; it is a spatial motion vector (accel)
   1666 	static btScalar Y_minus_hT_a[6];							//Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
   1667 	static btSpatialForceVector spatForceVecTemps[6];				//6 temporary spatial force vectors
   1668 	static btSpatialTransformationMatrix fromParent;
   1669 	/////////////////
   1670 
   1671     // First 'upward' loop.
   1672     // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
   1673 
   1674 	// Fill in zero_acc
   1675     // -- set to force/torque on the base, zero otherwise
   1676     if (m_fixedBase)
   1677 	{
   1678         zeroAccSpatFrc[0].setZero();
   1679     } else
   1680 	{
   1681 		//test forces
   1682 		fromParent.m_rotMat = rot_from_parent[0];
   1683 		fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
   1684     }
   1685     for (int i = 0; i < num_links; ++i)
   1686 	{
   1687 		zeroAccSpatFrc[i+1].setZero();
   1688     }
   1689 
   1690 	// 'Downward' loop.
   1691     // (part of TreeForwardDynamics in Mirtich.)
   1692     for (int i = num_links - 1; i >= 0; --i)
   1693 	{
   1694 		const int parent = m_links[i].m_parent;
   1695 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
   1696 
   1697 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1698 		{
   1699 			Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
   1700 											- m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
   1701 											;
   1702 		}
   1703 
   1704 		btVector3 in_top, in_bottom, out_top, out_bottom;
   1705 		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
   1706 
   1707 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1708 		{
   1709 			invD_times_Y[dof] = 0.f;
   1710 
   1711 			for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
   1712 			{
   1713 				invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
   1714 			}
   1715 		}
   1716 
   1717 		 // Zp += pXi * (Zi + hi*Yi/Di)
   1718 		spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
   1719 
   1720 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1721 		{
   1722 			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
   1723 			//
   1724 			spatForceVecTemps[0] += hDof * invD_times_Y[dof];
   1725 		}
   1726 
   1727 
   1728 		fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
   1729 
   1730 		zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
   1731     }
   1732 
   1733 	// ptr to the joint accel part of the output
   1734     btScalar * joint_accel = output + 6;
   1735 
   1736 
   1737     // Second 'upward' loop
   1738     // (part of TreeForwardDynamics in Mirtich)
   1739 
   1740     if (m_fixedBase)
   1741 	{
   1742         spatAcc[0].setZero();
   1743     }
   1744 	else
   1745 	{
   1746 		solveImatrix(zeroAccSpatFrc[0], result);
   1747 		spatAcc[0] = -result;
   1748 
   1749     }
   1750 
   1751     // now do the loop over the m_links
   1752     for (int i = 0; i < num_links; ++i)
   1753 	{
   1754         const int parent = m_links[i].m_parent;
   1755 		fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
   1756 
   1757 		fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
   1758 
   1759 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1760 		{
   1761 			const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
   1762 			//
   1763 			Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
   1764 		}
   1765 
   1766 		const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
   1767 		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]);
   1768 
   1769 		for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
   1770 			spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
   1771     }
   1772 
   1773     // transform base accelerations back to the world frame.
   1774     btVector3 omegadot_out;
   1775     omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
   1776 	output[0] = omegadot_out[0];
   1777 	output[1] = omegadot_out[1];
   1778 	output[2] = omegadot_out[2];
   1779 
   1780     btVector3 vdot_out;
   1781     vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
   1782 	output[3] = vdot_out[0];
   1783 	output[4] = vdot_out[1];
   1784 	output[5] = vdot_out[2];
   1785 
   1786 	/////////////////
   1787 	//printf("delta = [");
   1788 	//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
   1789 	//	printf("%.2f ", output[dof]);
   1790 	//printf("]\n");
   1791 	/////////////////
   1792 }
   1793 
   1794 
   1795 void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
   1796                                        btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
   1797 {
   1798     // Temporary matrices/vectors -- use scratch space from caller
   1799     // so that we don't have to keep reallocating every frame
   1800 	int num_links = getNumLinks();
   1801     scratch_r.resize(num_links);
   1802     scratch_v.resize(4*num_links + 4);
   1803 
   1804     btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
   1805     btVector3 * v_ptr = &scratch_v[0];
   1806 
   1807     // zhat_i^A (scratch space)
   1808     btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
   1809     btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
   1810 
   1811     // rot_from_parent (cached from calcAccelerations)
   1812     const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
   1813 
   1814     // hhat (cached), accel (scratch)
   1815     const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
   1816     const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
   1817     btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
   1818     btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
   1819 
   1820     // Y_i (scratch), D_i (cached)
   1821     btScalar * Y = r_ptr; r_ptr += num_links;
   1822     const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
   1823 
   1824     btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
   1825     btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
   1826 
   1827 
   1828 
   1829     // First 'upward' loop.
   1830     // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
   1831 
   1832     btVector3 input_force(force[3],force[4],force[5]);
   1833     btVector3 input_torque(force[0],force[1],force[2]);
   1834 
   1835     // Fill in zero_acc
   1836     // -- set to force/torque on the base, zero otherwise
   1837     if (m_fixedBase)
   1838 	{
   1839         zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
   1840     } else
   1841 	{
   1842         zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
   1843         zero_acc_bottom_linear[0] =  - (rot_from_parent[0] * input_torque);
   1844     }
   1845     for (int i = 0; i < num_links; ++i)
   1846 	{
   1847         zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
   1848     }
   1849 
   1850     // 'Downward' loop.
   1851     for (int i = num_links - 1; i >= 0; --i)
   1852 	{
   1853 //		btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
   1854         Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
   1855         Y[i] += force[6 + i];  // add joint torque
   1856 
   1857         const int parent = m_links[i].m_parent;
   1858 
   1859         // Zp += pXi * (Zi + hi*Yi/Di)
   1860         btVector3 in_top, in_bottom, out_top, out_bottom;
   1861         const btScalar Y_over_D = Y[i] / D[i];
   1862         in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
   1863         in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
   1864         InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
   1865                                 in_top, in_bottom, out_top, out_bottom);
   1866         zero_acc_top_angular[parent+1] += out_top;
   1867         zero_acc_bottom_linear[parent+1] += out_bottom;
   1868     }
   1869 
   1870     // ptr to the joint accel part of the output
   1871     btScalar * joint_accel = output + 6;
   1872 
   1873     // Second 'upward' loop
   1874     if (m_fixedBase)
   1875 	{
   1876         accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
   1877     } else
   1878 	{
   1879 		btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
   1880 		btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
   1881 
   1882 		float result[6];
   1883         solveImatrix(rhs_top,rhs_bot, result);
   1884 	//	printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
   1885 
   1886         for (int i = 0; i < 3; ++i) {
   1887             accel_top[0][i] = -result[i];
   1888             accel_bottom[0][i] = -result[i+3];
   1889         }
   1890 
   1891     }
   1892 
   1893     // now do the loop over the m_links
   1894     for (int i = 0; i < num_links; ++i) {
   1895         const int parent = m_links[i].m_parent;
   1896         SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
   1897                          accel_top[parent+1], accel_bottom[parent+1],
   1898                          accel_top[i+1], accel_bottom[i+1]);
   1899 		btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
   1900         joint_accel[i] = Y_minus_hT_a / D[i];
   1901         accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
   1902         accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
   1903     }
   1904 
   1905     // transform base accelerations back to the world frame.
   1906     btVector3 omegadot_out;
   1907     omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
   1908 	output[0] = omegadot_out[0];
   1909 	output[1] = omegadot_out[1];
   1910 	output[2] = omegadot_out[2];
   1911 
   1912     btVector3 vdot_out;
   1913     vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
   1914 
   1915 	output[3] = vdot_out[0];
   1916 	output[4] = vdot_out[1];
   1917 	output[5] = vdot_out[2];
   1918 
   1919 
   1920 	/////////////////
   1921 	/*
   1922 	int ndof = getNumLinks() + 6;
   1923 	printf("test force(impulse) (%d) = [\n",ndof);
   1924 
   1925 	for (int i=0;i<ndof;i++)
   1926 	{
   1927 			printf("%.2f ", force[i]);
   1928 		printf("]\n");
   1929 	}
   1930 
   1931 	printf("delta(%d) = [",ndof);
   1932 	for(int dof = 0; dof < getNumLinks() + 6; ++dof)
   1933 		printf("%.2f ", output[dof]);
   1934 	printf("]\n");
   1935 	/////////////////
   1936 */
   1937 
   1938 	//int dummy = 0;
   1939 }
   1940 
   1941 void btMultiBody::stepPositions(btScalar dt)
   1942 {
   1943 	int num_links = getNumLinks();
   1944     // step position by adding dt * velocity
   1945 	btVector3 v = getBaseVel();
   1946     m_basePos += dt * v;
   1947 
   1948     // "exponential map" method for the rotation
   1949     btVector3 base_omega = getBaseOmega();
   1950     const btScalar omega_norm = base_omega.norm();
   1951     const btScalar omega_times_dt = omega_norm * dt;
   1952     const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
   1953     if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
   1954 	{
   1955         const btScalar xsq = omega_times_dt * omega_times_dt;     // |omega|^2 * dt^2
   1956         const btScalar sin_term = dt * (xsq / 48.0f - 0.5f);      // -sin(0.5*dt*|omega|) / |omega|
   1957         const btScalar cos_term = 1.0f - xsq / 8.0f;              // cos(0.5*dt*|omega|)
   1958         m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
   1959     } else
   1960 	{
   1961         m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
   1962     }
   1963 
   1964     // Make sure the quaternion represents a valid rotation.
   1965     // (Not strictly necessary, but helps prevent any round-off errors from building up.)
   1966     m_baseQuat.normalize();
   1967 
   1968     // Finally we can update m_jointPos for each of the m_links
   1969     for (int i = 0; i < num_links; ++i)
   1970 	{
   1971 		float jointVel = getJointVel(i);
   1972         m_links[i].m_jointPos[0] += dt * jointVel;
   1973         m_links[i].updateCache();
   1974     }
   1975 }
   1976 
   1977 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
   1978 {
   1979 	int num_links = getNumLinks();
   1980     // step position by adding dt * velocity
   1981 	//btVector3 v = getBaseVel();
   1982     //m_basePos += dt * v;
   1983 	//
   1984 	btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
   1985 	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)
   1986 	//
   1987 	pBasePos[0] += dt * pBaseVel[0];
   1988 	pBasePos[1] += dt * pBaseVel[1];
   1989 	pBasePos[2] += dt * pBaseVel[2];
   1990 
   1991 	///////////////////////////////
   1992 	//local functor for quaternion integration (to avoid error prone redundancy)
   1993 	struct
   1994 	{
   1995 		//"exponential map" based on btTransformUtil::integrateTransform(..)
   1996 		void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
   1997 		{
   1998 			//baseBody	=>	quat is alias and omega is global coor
   1999 			//!baseBody	=>	quat is alibi and omega is local coor
   2000 
   2001 			btVector3 axis;
   2002 			btVector3 angvel;
   2003 
   2004 			if(!baseBody)
   2005 				angvel = quatRotate(quat, omega);				//if quat is not m_baseQuat, it is alibi => ok
   2006 			else
   2007 				angvel = omega;
   2008 
   2009 			btScalar fAngle = angvel.length();
   2010 			//limit the angular motion
   2011 			if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
   2012 			{
   2013 				fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
   2014 			}
   2015 
   2016 			if ( fAngle < btScalar(0.001) )
   2017 			{
   2018 				// use Taylor's expansions of sync function
   2019 				axis   = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
   2020 			}
   2021 			else
   2022 			{
   2023 				// sync(fAngle) = sin(c*fAngle)/t
   2024 				axis   = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
   2025 			}
   2026 
   2027 			if(!baseBody)
   2028 				quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;
   2029 			else
   2030 				quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
   2031 				//equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
   2032 
   2033 			quat.normalize();
   2034 		}
   2035 	} pQuatUpdateFun;
   2036 	///////////////////////////////
   2037 
   2038 	//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
   2039 	//
   2040 	btScalar *pBaseQuat = pq ? pq : m_baseQuat;
   2041 	btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];		//note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
   2042 	//
   2043 	static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
   2044 	static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
   2045 	pQuatUpdateFun(baseOmega, baseQuat, true, dt);
   2046 	pBaseQuat[0] = baseQuat.x();
   2047 	pBaseQuat[1] = baseQuat.y();
   2048 	pBaseQuat[2] = baseQuat.z();
   2049 	pBaseQuat[3] = baseQuat.w();
   2050 
   2051 
   2052 	//printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
   2053 	//printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
   2054 	//printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
   2055 
   2056 	if(pq)
   2057 		pq += 7;
   2058 	if(pqd)
   2059 		pqd += 6;
   2060 
   2061 	// Finally we can update m_jointPos for each of the m_links
   2062     for (int i = 0; i < num_links; ++i)
   2063 	{
   2064 		btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
   2065 		btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
   2066 
   2067 		switch(m_links[i].m_jointType)
   2068 		{
   2069 			case btMultibodyLink::ePrismatic:
   2070 			case btMultibodyLink::eRevolute:
   2071 			{
   2072 				btScalar jointVel = pJointVel[0];
   2073 				pJointPos[0] += dt * jointVel;
   2074 				break;
   2075 			}
   2076 			case btMultibodyLink::eSpherical:
   2077 			{
   2078 				static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
   2079 				static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
   2080 				pQuatUpdateFun(jointVel, jointOri, false, dt);
   2081 				pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
   2082 				break;
   2083 			}
   2084 			case btMultibodyLink::ePlanar:
   2085 			{
   2086 				pJointPos[0] += dt * getJointVelMultiDof(i)[0];
   2087 
   2088 				btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
   2089 				btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
   2090 				pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
   2091 				pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
   2092 
   2093 				break;
   2094 			}
   2095 			default:
   2096 			{
   2097 			}
   2098 
   2099 		}
   2100 
   2101 		m_links[i].updateCacheMultiDof(pq);
   2102 
   2103 		if(pq)
   2104 			pq += m_links[i].m_posVarCount;
   2105 		if(pqd)
   2106 			pqd += m_links[i].m_dofCount;
   2107     }
   2108 }
   2109 
   2110 void btMultiBody::filConstraintJacobianMultiDof(int link,
   2111                                     const btVector3 &contact_point,
   2112 									const btVector3 &normal_ang,
   2113                                     const btVector3 &normal_lin,
   2114                                     btScalar *jac,
   2115                                     btAlignedObjectArray<btScalar> &scratch_r,
   2116                                     btAlignedObjectArray<btVector3> &scratch_v,
   2117                                     btAlignedObjectArray<btMatrix3x3> &scratch_m) const
   2118 {
   2119     // temporary space
   2120 	int num_links = getNumLinks();
   2121 	int m_dofCount = getNumDofs();
   2122     scratch_v.resize(3*num_links + 3);			//(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
   2123     scratch_m.resize(num_links + 1);
   2124 
   2125     btVector3 * v_ptr = &scratch_v[0];
   2126     btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
   2127     btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
   2128 	btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
   2129     btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
   2130 
   2131     scratch_r.resize(m_dofCount);
   2132     btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
   2133 
   2134     btMatrix3x3 * rot_from_world = &scratch_m[0];
   2135 
   2136     const btVector3 p_minus_com_world = contact_point - m_basePos;
   2137 	const btVector3 &normal_lin_world = normal_lin;							//convenience
   2138 	const btVector3 &normal_ang_world = normal_ang;
   2139 
   2140     rot_from_world[0] = btMatrix3x3(m_baseQuat);
   2141 
   2142     // omega coeffients first.
   2143     btVector3 omega_coeffs_world;
   2144     omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
   2145 	jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
   2146 	jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
   2147 	jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
   2148     // then v coefficients
   2149     jac[3] = normal_lin_world[0];
   2150     jac[4] = normal_lin_world[1];
   2151     jac[5] = normal_lin_world[2];
   2152 
   2153 	//create link-local versions of p_minus_com and normal
   2154 	p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
   2155     n_local_lin[0] = rot_from_world[0] * normal_lin_world;
   2156 	n_local_ang[0] = rot_from_world[0] * normal_ang_world;
   2157 
   2158     // Set remaining jac values to zero for now.
   2159     for (int i = 6; i < 6 + m_dofCount; ++i)
   2160 	{
   2161         jac[i] = 0;
   2162     }
   2163 
   2164     // Qdot coefficients, if necessary.
   2165     if (num_links > 0 && link > -1) {
   2166 
   2167         // TODO: speed this up -- don't calculate for m_links we don't need.
   2168         // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
   2169         // which is resulting in repeated work being done...)
   2170 
   2171         // calculate required normals & positions in the local frames.
   2172         for (int i = 0; i < num_links; ++i) {
   2173 
   2174             // transform to local frame
   2175             const int parent = m_links[i].m_parent;
   2176             const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
   2177             rot_from_world[i+1] = mtx * rot_from_world[parent+1];
   2178 
   2179             n_local_lin[i+1] = mtx * n_local_lin[parent+1];
   2180 			n_local_ang[i+1] = mtx * n_local_ang[parent+1];
   2181             p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
   2182 
   2183 			// calculate the jacobian entry
   2184 			switch(m_links[i].m_jointType)
   2185 			{
   2186 				case btMultibodyLink::eRevolute:
   2187 				{
   2188 					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));
   2189 					results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
   2190 					break;
   2191 				}
   2192 				case btMultibodyLink::ePrismatic:
   2193 				{
   2194 					results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
   2195 					break;
   2196 				}
   2197 				case btMultibodyLink::eSpherical:
   2198 				{
   2199 					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));
   2200 					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));
   2201 					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));
   2202 
   2203 					results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
   2204 					results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
   2205 					results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
   2206 
   2207 					break;
   2208 				}
   2209 				case btMultibodyLink::ePlanar:
   2210 				{
   2211 					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));
   2212 					results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
   2213 					results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
   2214 
   2215 					break;
   2216 				}
   2217 				default:
   2218 				{
   2219 				}
   2220 			}
   2221 
   2222         }
   2223 
   2224         // Now copy through to output.
   2225 		//printf("jac[%d] = ", link);
   2226         while (link != -1)
   2227 		{
   2228 			for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
   2229 			{
   2230 				jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
   2231 				//printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
   2232 			}
   2233 
   2234 			link = m_links[link].m_parent;
   2235         }
   2236 		//printf("]\n");
   2237     }
   2238 }
   2239 
   2240 void btMultiBody::fillContactJacobian(int link,
   2241                                     const btVector3 &contact_point,
   2242                                     const btVector3 &normal,
   2243                                     btScalar *jac,
   2244                                     btAlignedObjectArray<btScalar> &scratch_r,
   2245                                     btAlignedObjectArray<btVector3> &scratch_v,
   2246                                     btAlignedObjectArray<btMatrix3x3> &scratch_m) const
   2247 {
   2248     // temporary space
   2249 	int num_links = getNumLinks();
   2250     scratch_v.resize(2*num_links + 2);
   2251     scratch_m.resize(num_links + 1);
   2252 
   2253     btVector3 * v_ptr = &scratch_v[0];
   2254     btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
   2255     btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
   2256     btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
   2257 
   2258     scratch_r.resize(num_links);
   2259     btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
   2260 
   2261     btMatrix3x3 * rot_from_world = &scratch_m[0];
   2262 
   2263     const btVector3 p_minus_com_world = contact_point - m_basePos;
   2264 
   2265     rot_from_world[0] = btMatrix3x3(m_baseQuat);
   2266 
   2267     p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
   2268     n_local[0] = rot_from_world[0] * normal;
   2269 
   2270     // omega coeffients first.
   2271 	if (this->m_fixedBase)
   2272 	{
   2273 		for (int i=0;i<6;i++)
   2274 		{
   2275 			jac[i]=0;
   2276 		}
   2277 	} else
   2278 	{
   2279 		    btVector3 omega_coeffs;
   2280 
   2281 		omega_coeffs = p_minus_com_world.cross(normal);
   2282 		jac[0] = omega_coeffs[0];
   2283 		jac[1] = omega_coeffs[1];
   2284 		jac[2] = omega_coeffs[2];
   2285 		// then v coefficients
   2286 		jac[3] = normal[0];
   2287 		jac[4] = normal[1];
   2288 		jac[5] = normal[2];
   2289 	}
   2290     // Set remaining jac values to zero for now.
   2291     for (int i = 6; i < 6 + num_links; ++i) {
   2292         jac[i] = 0;
   2293     }
   2294 
   2295     // Qdot coefficients, if necessary.
   2296     if (num_links > 0 && link > -1) {
   2297 
   2298         // TODO: speed this up -- don't calculate for m_links we don't need.
   2299         // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
   2300         // which is resulting in repeated work being done...)
   2301 
   2302         // calculate required normals & positions in the local frames.
   2303         for (int i = 0; i < num_links; ++i) {
   2304 
   2305             // transform to local frame
   2306             const int parent = m_links[i].m_parent;
   2307             const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
   2308             rot_from_world[i+1] = mtx * rot_from_world[parent+1];
   2309 
   2310             n_local[i+1] = mtx * n_local[parent+1];
   2311             p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
   2312 
   2313             // calculate the jacobian entry
   2314 			if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
   2315                 results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
   2316             } else {
   2317                 results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
   2318             }
   2319         }
   2320 
   2321         // Now copy through to output.
   2322 		//printf("jac[%d] = ", link);
   2323         while (link != -1)
   2324 		{
   2325             jac[6 + link] = results[link];
   2326 			//printf("%.2f\t", jac[6 + link]);
   2327             link = m_links[link].m_parent;
   2328         }
   2329 		//printf("]\n");
   2330     }
   2331 }
   2332 
   2333 void btMultiBody::wakeUp()
   2334 {
   2335     m_awake = true;
   2336 }
   2337 
   2338 void btMultiBody::goToSleep()
   2339 {
   2340     m_awake = false;
   2341 }
   2342 
   2343 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
   2344 {
   2345 	int num_links = getNumLinks();
   2346 	extern bool gDisableDeactivation;
   2347     if (!m_canSleep || gDisableDeactivation)
   2348 	{
   2349 		m_awake = true;
   2350 		m_sleepTimer = 0;
   2351 		return;
   2352 	}
   2353 
   2354     // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
   2355     btScalar motion = 0;
   2356 	if(m_isMultiDof)
   2357 	{
   2358 		for (int i = 0; i < 6 + m_dofCount; ++i)
   2359 			motion += m_realBuf[i] * m_realBuf[i];
   2360 	}
   2361 	else
   2362 	{
   2363 		for (int i = 0; i < 6 + num_links; ++i)
   2364 			motion += m_realBuf[i] * m_realBuf[i];
   2365 	}
   2366 
   2367 
   2368     if (motion < SLEEP_EPSILON) {
   2369         m_sleepTimer += timestep;
   2370         if (m_sleepTimer > SLEEP_TIMEOUT) {
   2371             goToSleep();
   2372         }
   2373     } else {
   2374         m_sleepTimer = 0;
   2375 		if (!m_awake)
   2376 			wakeUp();
   2377     }
   2378 }
   2379 
   2380 
   2381 void	btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
   2382 {
   2383 
   2384 	int num_links = getNumLinks();
   2385 
   2386 	// Cached 3x3 rotation matrices from parent frame to this frame.
   2387 	btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
   2388 
   2389 	rot_from_parent[0] = btMatrix3x3(m_baseQuat);				//m_baseQuat assumed to be alias!?
   2390 
   2391 	for (int i = 0; i < num_links; ++i)
   2392 	{
   2393 		rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
   2394 	}
   2395 
   2396 	int nLinks = getNumLinks();
   2397 	///base + num m_links
   2398 	world_to_local.resize(nLinks+1);
   2399 	local_origin.resize(nLinks+1);
   2400 
   2401 	world_to_local[0] = getWorldToBaseRot();
   2402 	local_origin[0] = getBasePos();
   2403 
   2404 	for (int k=0;k<getNumLinks();k++)
   2405 	{
   2406 		const int parent = getParent(k);
   2407 		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
   2408 		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
   2409 	}
   2410 
   2411 	for (int link=0;link<getNumLinks();link++)
   2412 	{
   2413 		int index = link+1;
   2414 
   2415 		btVector3 posr = local_origin[index];
   2416 		btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
   2417 		btTransform tr;
   2418 		tr.setIdentity();
   2419 		tr.setOrigin(posr);
   2420 		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
   2421 		getLink(link).m_cachedWorldTransform = tr;
   2422 
   2423 	}
   2424 
   2425 }
   2426 
   2427 void	btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
   2428 {
   2429 	world_to_local.resize(getNumLinks()+1);
   2430 	local_origin.resize(getNumLinks()+1);
   2431 
   2432 	world_to_local[0] = getWorldToBaseRot();
   2433 	local_origin[0] = getBasePos();
   2434 
   2435 	if (getBaseCollider())
   2436 	{
   2437 		btVector3 posr = local_origin[0];
   2438 		//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
   2439 		btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
   2440 		btTransform tr;
   2441 		tr.setIdentity();
   2442 		tr.setOrigin(posr);
   2443 		tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
   2444 
   2445 		getBaseCollider()->setWorldTransform(tr);
   2446 
   2447 	}
   2448 
   2449 	for (int k=0;k<getNumLinks();k++)
   2450 	{
   2451 		const int parent = getParent(k);
   2452 		world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
   2453 		local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
   2454 	}
   2455 
   2456 
   2457 	for (int m=0;m<getNumLinks();m++)
   2458 	{
   2459 		btMultiBodyLinkCollider* col = getLink(m).m_collider;
   2460 		if (col)
   2461 		{
   2462 			int link = col->m_link;
   2463 			btAssert(link == m);
   2464 
   2465 			int index = link+1;
   2466 
   2467 			btVector3 posr = local_origin[index];
   2468 			//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
   2469 			btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
   2470 			btTransform tr;
   2471 			tr.setIdentity();
   2472 			tr.setOrigin(posr);
   2473 			tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
   2474 
   2475 			col->setWorldTransform(tr);
   2476 		}
   2477 	}
   2478 }
   2479 
   2480 int	btMultiBody::calculateSerializeBufferSize()	const
   2481 {
   2482 	int sz = sizeof(btMultiBodyData);
   2483 	return sz;
   2484 }
   2485 
   2486 	///fills the dataBuffer and returns the struct name (and 0 on failure)
   2487 const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
   2488 {
   2489 		btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
   2490 		getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
   2491 		mbd->m_baseMass = this->getBaseMass();
   2492 		getBaseInertia().serialize(mbd->m_baseInertia);
   2493 		{
   2494 			char* name = (char*) serializer->findNameForPointer(m_baseName);
   2495 			mbd->m_baseName = (char*)serializer->getUniquePointer(name);
   2496 			if (mbd->m_baseName)
   2497 			{
   2498 				serializer->serializeName(name);
   2499 			}
   2500 		}
   2501 		mbd->m_numLinks = this->getNumLinks();
   2502 		if (mbd->m_numLinks)
   2503 		{
   2504 			int sz = sizeof(btMultiBodyLinkData);
   2505 			int numElem = mbd->m_numLinks;
   2506 			btChunk* chunk = serializer->allocate(sz,numElem);
   2507 			btMultiBodyLinkData* memPtr = (btMultiBodyLinkData*)chunk->m_oldPtr;
   2508 			for (int i=0;i<numElem;i++,memPtr++)
   2509 			{
   2510 
   2511 				memPtr->m_jointType = getLink(i).m_jointType;
   2512 				memPtr->m_dofCount = getLink(i).m_dofCount;
   2513 				memPtr->m_posVarCount = getLink(i).m_posVarCount;
   2514 
   2515 				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
   2516 				memPtr->m_linkMass = getLink(i).m_mass;
   2517 				memPtr->m_parentIndex = getLink(i).m_parent;
   2518 				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
   2519 				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
   2520 				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
   2521 				btAssert(memPtr->m_dofCount<=3);
   2522 				for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
   2523 				{
   2524 					getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
   2525 					getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
   2526 
   2527 					memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
   2528 					memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
   2529 
   2530 				}
   2531 				int numPosVar = getLink(i).m_posVarCount;
   2532 				for (int posvar = 0; posvar < numPosVar;posvar++)
   2533 				{
   2534 					memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
   2535 				}
   2536 
   2537 
   2538 				{
   2539 					char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
   2540 					memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
   2541 					if (memPtr->m_linkName)
   2542 					{
   2543 						serializer->serializeName(name);
   2544 					}
   2545 				}
   2546 				{
   2547 					char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
   2548 					memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
   2549 					if (memPtr->m_jointName)
   2550 					{
   2551 						serializer->serializeName(name);
   2552 					}
   2553 				}
   2554 				memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
   2555 
   2556 			}
   2557 			serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
   2558 		}
   2559 		mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
   2560 
   2561 		return btMultiBodyDataName;
   2562 }
   2563