dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBody.cpp

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