47         top_out = rotation_matrix * top_in;
 
   48         bottom_out = -displacement.
cross(top_out) + rotation_matrix * bottom_in;
 
   56     void InverseSpatialTransform(
const btMatrix3x3 &rotation_matrix,
 
   63         top_out = rotation_matrix.
transpose() * top_in;
 
   64         bottom_out = rotation_matrix.
transpose() * (bottom_in + displacement.
cross(top_in));            
 
   72         return a_bottom.
dot(b_top) + a_top.
dot(b_bottom);
 
   75         void SpatialCrossProduct(
const btVector3 &a_top,
 
   82                 top_out = a_top.
cross(b_top);
 
   83                 bottom_out = a_bottom.
cross(b_top) + a_top.
cross(b_bottom);
 
  102           m_baseQuat(0, 0, 0, 1),
 
  104           m_baseInertia(inertia),
 
  106           m_fixedBase(fixedBase),
 
  108           m_canSleep(canSleep),
 
  110           m_userObjectPointer(0),
 
  114           m_linearDamping(0.04f),
 
  115           m_angularDamping(0.04f),
 
  117           m_maxAppliedImpulse(1000.f),
 
  118           m_maxCoordinateVelocity(100.f),
 
  119           m_hasSelfCollision(true),
 
  124           m_useGlobalVelocities(false),
 
  125           m_internalNeedsJointFeedback(false)
 
  152                                                          const btVector3 &parentComToThisPivotOffset,
 
  153                                                          const btVector3 &thisPivotToThisComOffset, 
bool )
 
  156         m_links[i].m_inertiaLocal = inertia;
 
  158         m_links[i].setAxisTop(0, 0., 0., 0.);
 
  160         m_links[i].m_zeroRotParentToThis = rotParentToThis;
 
  161         m_links[i].m_dVector = thisPivotToThisComOffset;
 
  162         m_links[i].m_eVector = parentComToThisPivotOffset;
 
  170         m_links[i].updateCacheMultiDof();
 
  181                                                                  const btVector3 &parentComToThisPivotOffset,
 
  182                                                                  const btVector3 &thisPivotToThisComOffset,
 
  183                                                                  bool disableParentCollision)
 
  189         m_links[i].m_inertiaLocal = inertia;
 
  191         m_links[i].m_zeroRotParentToThis = rotParentToThis;
 
  192         m_links[i].setAxisTop(0, 0., 0., 0.);
 
  193         m_links[i].setAxisBottom(0, jointAxis);
 
  194         m_links[i].m_eVector = parentComToThisPivotOffset;
 
  195         m_links[i].m_dVector = thisPivotToThisComOffset;
 
  196         m_links[i].m_cachedRotParentToThis = rotParentToThis;
 
  201         m_links[i].m_jointPos[0] = 0.f;
 
  202         m_links[i].m_jointTorque[0] = 0.f;
 
  204         if (disableParentCollision)
 
  208         m_links[i].updateCacheMultiDof();
 
  219                                                                 const btVector3 &parentComToThisPivotOffset,
 
  220                                                                 const btVector3 &thisPivotToThisComOffset,
 
  221                                                                 bool disableParentCollision)
 
  227         m_links[i].m_inertiaLocal = inertia;
 
  229         m_links[i].m_zeroRotParentToThis = rotParentToThis;
 
  230         m_links[i].setAxisTop(0, jointAxis);
 
  231         m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
 
  232         m_links[i].m_dVector = thisPivotToThisComOffset;
 
  233         m_links[i].m_eVector = parentComToThisPivotOffset;
 
  238         m_links[i].m_jointPos[0] = 0.f;
 
  239         m_links[i].m_jointTorque[0] = 0.f;
 
  241         if (disableParentCollision)
 
  244         m_links[i].updateCacheMultiDof();
 
  254                                                                  const btVector3 &parentComToThisPivotOffset,
 
  255                                                                  const btVector3 &thisPivotToThisComOffset,
 
  256                                                                  bool disableParentCollision)
 
  262         m_links[i].m_inertiaLocal = inertia;
 
  264         m_links[i].m_zeroRotParentToThis = rotParentToThis;
 
  265         m_links[i].m_dVector = thisPivotToThisComOffset;
 
  266         m_links[i].m_eVector = parentComToThisPivotOffset;
 
  271         m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
 
  272         m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
 
  273         m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
 
  274         m_links[i].setAxisBottom(0, 
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
 
  275         m_links[i].setAxisBottom(1, 
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
 
  276         m_links[i].setAxisBottom(2, 
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
 
  278         m_links[i].m_jointPos[3] = 1.f;
 
  281         if (disableParentCollision)
 
  284         m_links[i].updateCacheMultiDof();
 
  295                                                           const btVector3 &parentComToThisComOffset,
 
  296                                                           bool disableParentCollision)
 
  302         m_links[i].m_inertiaLocal = inertia;
 
  304         m_links[i].m_zeroRotParentToThis = rotParentToThis;
 
  305         m_links[i].m_dVector.setZero();
 
  306         m_links[i].m_eVector = parentComToThisComOffset;
 
  309         btVector3 vecNonParallelToRotAxis(1, 0, 0);
 
  310         if (rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
 
  311                 vecNonParallelToRotAxis.
setValue(0, 1, 0);
 
  318         m_links[i].setAxisTop(0, n[0], n[1], n[2]);
 
  319         m_links[i].setAxisTop(1, 0, 0, 0);
 
  320         m_links[i].setAxisTop(2, 0, 0, 0);
 
  321         m_links[i].setAxisBottom(0, 0, 0, 0);
 
  323         m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
 
  325         m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
 
  329         if (disableParentCollision)
 
  332         m_links[i].updateCacheMultiDof();
 
  336         m_links[i].setAxisBottom(1, 
m_links[i].getAxisBottom(1).normalized());
 
  337         m_links[i].setAxisBottom(2, 
m_links[i].getAxisBottom(2).normalized());
 
  365         return m_links[i].m_inertiaLocal;
 
  370         return m_links[i].m_jointPos[0];
 
  380         return &
m_links[i].m_jointPos[0];
 
  390         return &
m_links[i].m_jointPos[0];
 
  401         m_links[i].updateCacheMultiDof();
 
  407         for (
int pos = 0; pos < 
m_links[i].m_posVarCount; ++pos)
 
  410         m_links[i].updateCacheMultiDof();
 
  415         for (
int pos = 0; pos < 
m_links[i].m_posVarCount; ++pos)
 
  418         m_links[i].updateCacheMultiDof();
 
  430         for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
  436         for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
  442         return m_links[i].m_cachedRVector;
 
  447         return m_links[i].m_cachedRotParentToThis;
 
  540         result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
 
  552         for (
int i = 0; i < num_links; ++i)
 
  559                         omega[parent + 1], vel[parent + 1],
 
  560                         omega[i + 1], vel[i + 1]);
 
  564                 for (
int dof = 0; dof < link.
m_dofCount; ++dof)
 
  566                         omega[i + 1] += jointVel[dof] * link.
getAxisTop(dof);
 
  577         omega.
resize(num_links + 1);
 
  579         vel.
resize(num_links + 1);
 
  586         for (
int i = 0; i < num_links; ++i)
 
  588                 result += 
m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
 
  589                 result += omega[i + 1].dot(
m_links[i].m_inertiaLocal * omega[i + 1]);
 
  592         return 0.5f * result;
 
  600         omega.
resize(num_links + 1);
 
  602         vel.
resize(num_links + 1);
 
  604         rot_from_world.
resize(num_links + 1);
 
  610         for (
int i = 0; i < num_links; ++i)
 
  612                 rot_from_world[i + 1] = 
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent + 1];
 
  626                 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
 
  627                 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
 
  637                 m_links[i].m_appliedForce.setValue(0, 0, 0);
 
  638                 m_links[i].m_appliedTorque.setValue(0, 0, 0);
 
  652         m_links[i].m_appliedForce += f;
 
  657         m_links[i].m_appliedTorque += t;
 
  662         m_links[i].m_appliedConstraintForce += f;
 
  667         m_links[i].m_appliedConstraintTorque += t;
 
  672         m_links[i].m_jointTorque[0] += Q;
 
  677         m_links[i].m_jointTorque[dof] += Q;
 
  682         for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
  683                 m_links[i].m_jointTorque[dof] = Q[dof];
 
  688         return m_links[i].m_appliedForce;
 
  693         return m_links[i].m_appliedTorque;
 
  698         return m_links[i].m_jointTorque[0];
 
  703         return &
m_links[i].m_jointTorque[0];
 
  722                                   row1[0], row1[1], row1[2],
 
  723                                   row2[0], row2[1], row2[2]);
 
  727 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) 
  734         bool isConstraintPass,
 
  735         bool jointFeedbackInWorldSpace,
 
  736         bool jointFeedbackInJointFrame)
 
  769         scratch_v.
resize(8 * num_links + 6);
 
  770         scratch_m.
resize(4 * num_links + 4);
 
  778         v_ptr += num_links * 2 + 2;
 
  782         v_ptr += num_links * 2 + 2;
 
  786         v_ptr += num_links * 2;
 
  799         v_ptr += num_links * 2 + 2;
 
  829         spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
 
  840                 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
 
  843                 const btScalar linDampMult = 1., angDampMult = 1.;
 
  844                 zeroAccSpatFrc[0].
addVector(angDampMult * 
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
 
  845                                                                         linDampMult * 
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
 
  852                 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
 
  866         rot_from_world[0] = rot_from_parent[0];
 
  869         for (
int i = 0; i < num_links; ++i)
 
  871                 const int parent = 
m_links[i].m_parent;
 
  873                 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
 
  875                 fromParent.
m_rotMat = rot_from_parent[i + 1];
 
  877                 fromWorld.
m_rotMat = rot_from_world[i + 1];
 
  878                 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
 
  886                         for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
  890                         spatVel[i + 1] += spatJointVel;
 
  903                 spatVel[i + 1].
cross(spatJointVel, spatCoriolisAcc[i]);
 
  908                 btVector3 linkAppliedForce = isConstraintPass ? 
m_links[i].m_appliedConstraintForce : 
m_links[i].m_appliedForce;
 
  909                 btVector3 linkAppliedTorque = isConstraintPass ? 
m_links[i].m_appliedConstraintTorque : 
m_links[i].m_appliedTorque;
 
  911                 zeroAccSpatFrc[i + 1].
setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
 
  916                         b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
 
  918                         zeroAccSpatFrc[i+1].m_topVec[0],
 
  919                         zeroAccSpatFrc[i+1].m_topVec[1],
 
  920                         zeroAccSpatFrc[i+1].m_topVec[2],
 
  922                         zeroAccSpatFrc[i+1].m_bottomVec[0],
 
  923                         zeroAccSpatFrc[i+1].m_bottomVec[1],
 
  924                         zeroAccSpatFrc[i+1].m_bottomVec[2]);
 
  929                 btScalar linDampMult = 1., angDampMult = 1.;
 
  930                 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()),
 
  931                                                                                 linDampMult * 
m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
 
  942                                                                                                  0, 
m_links[i].m_inertiaLocal[1], 0,
 
  943                                                                                                  0, 0, 
m_links[i].m_inertiaLocal[2]));
 
  947                         zeroAccSpatFrc[i + 1].
addAngular(spatVel[i + 1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
 
  949                 zeroAccSpatFrc[i + 1].
addLinear(
m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
 
  968         for (
int i = num_links - 1; i >= 0; --i)
 
  970                 const int parent = 
m_links[i].m_parent;
 
  971                 fromParent.
m_rotMat = rot_from_parent[i + 1];
 
  974                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
  978                         hDof = spatInertia[i + 1] * 
m_links[i].m_axes[dof];
 
  980                         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);
 
  982                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
  985                         for (
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
 
  988                                 D_row[dof2] = 
m_links[i].m_axes[dof].dot(hDof2);
 
  993                 switch (
m_links[i].m_jointType)
 
 1000                                         invDi[0] = 1.0f / D[0];
 
 1011                                 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
 
 1015                                 for (
int row = 0; row < 3; ++row)
 
 1017                                         for (
int col = 0; col < 3; ++col)
 
 1019                                                 invDi[row * 3 + col] = invD3x3[row][col];
 
 1031                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1033                         spatForceVecTemps[dof].
setZero();
 
 1035                         for (
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
 
 1039                                 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * 
m_links[i].m_dofCount + dof];
 
 1043                 dyadTemp = spatInertia[i + 1];
 
 1046                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1055                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1057                         invD_times_Y[dof] = 0.f;
 
 1059                         for (
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
 
 1061                                 invD_times_Y[dof] += invDi[dof * 
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
 
 1065                 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
 
 1067                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1071                         spatForceVecTemps[0] += hDof * invD_times_Y[dof];
 
 1076                 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
 
 1098                 spatAcc[0] = -result;
 
 1102         for (
int i = 0; i < num_links; ++i)
 
 1110                 const int parent = 
m_links[i].m_parent;
 
 1111                 fromParent.
m_rotMat = rot_from_parent[i + 1];
 
 1114                 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
 
 1116                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1120                         Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
 
 1127                 spatAcc[i + 1] += spatCoriolisAcc[i];
 
 1129                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1130                         spatAcc[i + 1] += 
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
 
 1132                 if (
m_links[i].m_jointFeedback)
 
 1136                         btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
 
 1137                         btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
 
 1139                         if (jointFeedbackInJointFrame)
 
 1144                                 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
 
 1147                         if (jointFeedbackInWorldSpace)
 
 1149                                 if (isConstraintPass)
 
 1151                                         m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += 
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
 
 1152                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec += 
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
 
 1156                                         m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = 
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
 
 1157                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec = 
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
 
 1162                                 if (isConstraintPass)
 
 1164                                         m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
 
 1165                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
 
 1169                                         m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
 
 1170                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
 
 1178         output[0] = omegadot_out[0];
 
 1179         output[1] = omegadot_out[1];
 
 1180         output[2] = omegadot_out[2];
 
 1207         if (!isConstraintPass)
 
 1242                 for (
int i = 0; i < num_links; ++i)
 
 1244                         const int parent = 
m_links[i].m_parent;
 
 1248                         fromParent.
m_rotMat = rot_from_parent[i + 1];
 
 1250                         fromWorld.
m_rotMat = rot_from_world[i + 1];
 
 1253                         fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
 
 1261                         for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1265                         spatVel[i + 1] += spatJointVel;
 
 1310                         for (
int i = 0; i < 6; i++)
 
 1323                 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
 
 1332                         btVector3 vtop = invI_upper_left * rhs_top;
 
 1334                         tmp = invIupper_right * rhs_bot;
 
 1336                         btVector3 vbot = invI_lower_left * rhs_top;
 
 1337                         tmp = invI_lower_right * rhs_bot;
 
 1339                         result[0] = vtop[0];
 
 1340                         result[1] = vtop[1];
 
 1341                         result[2] = vtop[2];
 
 1342                         result[3] = vbot[0];
 
 1343                         result[4] = vbot[1];
 
 1344                         result[5] = vbot[2];
 
 1388                 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
 
 1411         for (
int row = 0; row < rowsA; row++)
 
 1413                 for (
int col = 0; col < colsB; col++)
 
 1415                         pC[row * colsB + col] = 0.f;
 
 1416                         for (
int inner = 0; inner < rowsB; inner++)
 
 1418                                 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
 
 1432         scratch_v.
resize(4 * num_links + 4);
 
 1439         v_ptr += num_links * 2 + 2;
 
 1448         v_ptr += num_links * 2 + 2;
 
 1474                 fromParent.
m_rotMat = rot_from_parent[0];
 
 1477         for (
int i = 0; i < num_links; ++i)
 
 1479                 zeroAccSpatFrc[i + 1].
setZero();
 
 1484         for (
int i = num_links - 1; i >= 0; --i)
 
 1486                 const int parent = 
m_links[i].m_parent;
 
 1487                 fromParent.
m_rotMat = rot_from_parent[i + 1];
 
 1490                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1492                         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]);
 
 1495                 btVector3 in_top, in_bottom, out_top, out_bottom;
 
 1498                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1500                         invD_times_Y[dof] = 0.f;
 
 1502                         for (
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
 
 1504                                 invD_times_Y[dof] += invDi[dof * 
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
 
 1509                 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
 
 1511                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1515                         spatForceVecTemps[0] += hDof * invD_times_Y[dof];
 
 1520                 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
 
 1536                 spatAcc[0] = -result;
 
 1540         for (
int i = 0; i < num_links; ++i)
 
 1542                 const int parent = 
m_links[i].m_parent;
 
 1543                 fromParent.
m_rotMat = rot_from_parent[i + 1];
 
 1546                 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
 
 1548                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1552                         Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
 
 1558                 for (
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
 
 1559                         spatAcc[i + 1] += 
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
 
 1565         output[0] = omegadot_out[0];
 
 1566         output[1] = omegadot_out[1];
 
 1567         output[2] = omegadot_out[2];
 
 1593         pBasePos[0] += dt * pBaseVel[0];
 
 1594         pBasePos[1] += dt * pBaseVel[1];
 
 1595         pBasePos[2] += dt * pBaseVel[2];
 
 1625                                 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
 
 1630                                 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
 
 1650         baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
 
 1652         baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
 
 1653         pQuatUpdateFun(baseOmega, baseQuat, 
true, dt);
 
 1654         pBaseQuat[0] = baseQuat.
x();
 
 1655         pBaseQuat[1] = baseQuat.
y();
 
 1656         pBaseQuat[2] = baseQuat.
z();
 
 1657         pBaseQuat[3] = baseQuat.
w();
 
 1669         for (
int i = 0; i < num_links; ++i)
 
 1674                 switch (
m_links[i].m_jointType)
 
 1680                                 pJointPos[0] += dt * jointVel;
 
 1686                                 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
 
 1688                                 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
 
 1689                                 pQuatUpdateFun(jointVel, jointOri, 
false, dt);
 
 1690                                 pJointPos[0] = jointOri.
x();
 
 1691                                 pJointPos[1] = jointOri.
y();
 
 1692                                 pJointPos[2] = jointOri.
z();
 
 1693                                 pJointPos[3] = jointOri.
w();
 
 1702                                 pJointPos[1] += 
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
 
 1703                                 pJointPos[2] += 
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
 
 1712                 m_links[i].updateCacheMultiDof(pq);
 
 1715                         pq += 
m_links[i].m_posVarCount;
 
 1733         scratch_v.
resize(3 * num_links + 3);  
 
 1734         scratch_m.
resize(num_links + 1);
 
 1738         v_ptr += num_links + 1;
 
 1740         v_ptr += num_links + 1;
 
 1742         v_ptr += num_links + 1;
 
 1751     int numLinksChildToRoot=0;
 
 1755         links[numLinksChildToRoot++]=l;
 
 1762         const btVector3 &normal_lin_world = normal_lin;  
 
 1763         const btVector3 &normal_ang_world = normal_ang;
 
 1769         omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
 
 1770         jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
 
 1771         jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
 
 1772         jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
 
 1774         jac[3] = normal_lin_world[0];
 
 1775         jac[4] = normal_lin_world[1];
 
 1776         jac[5] = normal_lin_world[2];
 
 1779         p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
 
 1780         n_local_lin[0] = rot_from_world[0] * normal_lin_world;
 
 1781         n_local_ang[0] = rot_from_world[0] * normal_ang_world;
 
 1790         if (num_links > 0 && link > -1)
 
 1796         for (
int a = 0; a < numLinksChildToRoot; a++)
 
 1798             int i = links[numLinksChildToRoot-1-a];
 
 1800                         const int parent = 
m_links[i].m_parent;
 
 1802                         rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
 
 1804                         n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
 
 1805                         n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
 
 1806                         p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - 
m_links[i].m_cachedRVector;
 
 1809                         switch (
m_links[i].m_jointType)
 
 1813                                         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));
 
 1814                                         results[
m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
 
 1819                                         results[
m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(0));
 
 1824                                         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));
 
 1825                                         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));
 
 1826                                         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));
 
 1828                                         results[
m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
 
 1829                                         results[
m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(1));
 
 1830                                         results[
m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(2));
 
 1836                                         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]));  
 
 1837                                         results[
m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(1));
 
 1838                                         results[
m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(2));
 
 1852                         for (
int dof = 0; dof < 
m_links[link].m_dofCount; ++dof)
 
 1854                                 jac[6 + 
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
 
 1858                         link = 
m_links[link].m_parent;
 
 1892         if (motion < SLEEP_EPSILON)
 
 1917         for (
int i = 0; i < num_links; ++i)
 
 1924         world_to_local.
resize(nLinks + 1);
 
 1925         local_origin.
resize(nLinks + 1);
 
 1939                 int index = link + 1;
 
 1942                 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
 
 1963                 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
 
 1987                         int index = link + 1;
 
 1991                         btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
 
 2022                 if (mbd->m_baseName)
 
 2028         if (mbd->m_numLinks)
 
 2031                 int numElem = mbd->m_numLinks;
 
 2034                 for (
int i = 0; i < numElem; i++, memPtr++)
 
 2069                         for (
int posvar = 0; posvar < numPosVar; posvar++)
 
 2077                                 if (memPtr->m_linkName)
 
 2085                                 if (memPtr->m_jointName)
 
 2097 #ifdef BT_USE_DOUBLE_PRECISION 
 2098         memset(mbd->m_padding, 0, 
sizeof(mbd->m_padding));