31 #include "Bullet3Common/b3Logging.h"    44     void SpatialTransform(
const btMatrix3x3 &rotation_matrix,  
    51         top_out = rotation_matrix * top_in;
    52         bottom_out = -displacement.
cross(top_out) + rotation_matrix * bottom_in;
    55     void InverseSpatialTransform(
const btMatrix3x3 &rotation_matrix,
    62         top_out = rotation_matrix.
transpose() * top_in;
    63         bottom_out = rotation_matrix.
transpose() * (bottom_in + displacement.
cross(top_in));            
    71         return a_bottom.
dot(b_top) + a_top.
dot(b_bottom);
    74         void SpatialCrossProduct(
const btVector3 &a_top,
    81                 top_out = a_top.
cross(b_top);
    82                 bottom_out = a_bottom.
cross(b_top) + a_top.
cross(b_bottom);
   101         m_baseQuat(0, 0, 0, 1),
   103       m_baseInertia(inertia),
   105                 m_fixedBase(fixedBase),
   107                 m_canSleep(canSleep),
   110                 m_linearDamping(0.04f),
   111                 m_angularDamping(0.04f),
   113                         m_maxAppliedImpulse(1000.f),
   114                 m_maxCoordinateVelocity(100.f),
   115                         m_hasSelfCollision(true),               
   120                 m_useGlobalVelocities(false),
   121                 m_internalNeedsJointFeedback(false)
   140                                                    const btVector3 &parentComToThisPivotOffset,
   141                            const btVector3 &thisPivotToThisComOffset, 
bool )
   145     m_links[i].m_inertiaLocal = inertia;
   147     m_links[i].m_zeroRotParentToThis = rotParentToThis;
   148         m_links[i].m_dVector = thisPivotToThisComOffset;
   149     m_links[i].m_eVector = parentComToThisPivotOffset;    
   157         m_links[i].updateCacheMultiDof();
   170                                const btVector3 &parentComToThisPivotOffset,
   171                                                            const btVector3 &thisPivotToThisComOffset,
   172                                                            bool disableParentCollision)
   178     m_links[i].m_inertiaLocal = inertia;
   180     m_links[i].m_zeroRotParentToThis = rotParentToThis;
   181     m_links[i].setAxisTop(0, 0., 0., 0.);
   182     m_links[i].setAxisBottom(0, jointAxis);
   183     m_links[i].m_eVector = parentComToThisPivotOffset;
   184         m_links[i].m_dVector = thisPivotToThisComOffset;
   185     m_links[i].m_cachedRotParentToThis = rotParentToThis;
   190         m_links[i].m_jointPos[0] = 0.f;
   191         m_links[i].m_jointTorque[0] = 0.f;
   193         if (disableParentCollision)
   197         m_links[i].updateCacheMultiDof();
   208                               const btVector3 &parentComToThisPivotOffset,
   209                               const btVector3 &thisPivotToThisComOffset,
   210                                                           bool disableParentCollision)
   216     m_links[i].m_inertiaLocal = inertia;
   218     m_links[i].m_zeroRotParentToThis = rotParentToThis;
   219     m_links[i].setAxisTop(0, jointAxis);
   220     m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
   221     m_links[i].m_dVector = thisPivotToThisComOffset;
   222     m_links[i].m_eVector = parentComToThisPivotOffset;
   227         m_links[i].m_jointPos[0] = 0.f;
   228         m_links[i].m_jointTorque[0] = 0.f;
   230         if (disableParentCollision)
   233         m_links[i].updateCacheMultiDof();
   245                                                    const btVector3 &parentComToThisPivotOffset,
   246                                                    const btVector3 &thisPivotToThisComOffset,
   247                                                    bool disableParentCollision)
   254     m_links[i].m_inertiaLocal = inertia;
   256     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
   257     m_links[i].m_dVector = thisPivotToThisComOffset;
   258     m_links[i].m_eVector = parentComToThisPivotOffset;    
   263         m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
   264         m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
   265         m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
   266         m_links[i].setAxisBottom(0, 
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
   267         m_links[i].setAxisBottom(1, 
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
   268         m_links[i].setAxisBottom(2, 
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
   273         if (disableParentCollision)
   276         m_links[i].updateCacheMultiDof();       
   287                                                    const btVector3 &parentComToThisComOffset,                                              
   288                                                    bool disableParentCollision)
   295     m_links[i].m_inertiaLocal = inertia;
   297     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
   298         m_links[i].m_dVector.setZero();
   299     m_links[i].m_eVector = parentComToThisComOffset;
   302         static btVector3 vecNonParallelToRotAxis(1, 0, 0);
   303         if(rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
   304                 vecNonParallelToRotAxis.
setValue(0, 1, 0);
   311         m_links[i].setAxisTop(0, n[0],n[1],n[2]);
   312         m_links[i].setAxisTop(1,0,0,0);
   313         m_links[i].setAxisTop(2,0,0,0);
   314         m_links[i].setAxisBottom(0,0,0,0);
   316         m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
   318         m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
   322         if (disableParentCollision)
   325         m_links[i].updateCacheMultiDof();
   352     return m_links[i].m_inertiaLocal;
   357     return m_links[i].m_jointPos[0];
   367         return &
m_links[i].m_jointPos[0];
   377         return &
m_links[i].m_jointPos[0];
   389     m_links[i].updateCacheMultiDof();
   394         for(
int pos = 0; pos < 
m_links[i].m_posVarCount; ++pos)
   395                 m_links[i].m_jointPos[pos] = q[pos];
   397     m_links[i].updateCacheMultiDof();
   407         for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   413     return m_links[i].m_cachedRVector;
   418     return m_links[i].m_cachedRotParentToThis;
   476     for (
int i = 0; i < num_links; ++i) {
   477         const int parent = 
m_links[i].m_parent;
   481                          omega[parent+1], vel[parent+1],
   482                          omega[i+1], vel[i+1]);
   502     for (
int i = 0; i < num_links; ++i) {
   503         result += 
m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
   504         result += omega[i+1].dot(
m_links[i].m_inertiaLocal * omega[i+1]);
   507     return 0.5f * result;
   522     for (
int i = 0; i < num_links; ++i) {
   523         rot_from_world[i+1] = 
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent+1];
   524         result += (
quatRotate(rot_from_world[i+1].
inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
   537         m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
   538         m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
   548         m_links[i].m_appliedForce.setValue(0, 0, 0);
   549         m_links[i].m_appliedTorque.setValue(0, 0, 0);
   563     m_links[i].m_appliedForce += f;
   568     m_links[i].m_appliedTorque += t;
   573     m_links[i].m_appliedConstraintForce += f;
   578     m_links[i].m_appliedConstraintTorque += t;
   585     m_links[i].m_jointTorque[0] += Q;
   590         m_links[i].m_jointTorque[dof] += Q;
   595         for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   596                 m_links[i].m_jointTorque[dof] = Q[dof];
   601     return m_links[i].m_appliedForce;
   606     return m_links[i].m_appliedTorque;
   611     return m_links[i].m_jointTorque[0];
   616     return &
m_links[i].m_jointTorque[0];
   635                                                 row1[0],row1[1],row1[2],
   636                                                 row2[0],row2[1],row2[2]);
   640 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)   647                                 bool isConstraintPass)
   680     scratch_v.
resize(8*num_links + 6);
   681     scratch_m.
resize(4*num_links + 4);
   689         v_ptr += num_links * 2 + 2;
   693         v_ptr += num_links * 2 + 2;
   697         v_ptr += num_links * 2;
   710         v_ptr += num_links * 2 + 2;
   730     btScalar * joint_accel = output + 6;
   740         spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
   751                 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));     
   754                 btScalar linDampMult = 1., angDampMult = 1.;
   755                 zeroAccSpatFrc[0].
addVector(angDampMult * 
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
   756                                                                    linDampMult * 
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
   763                 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
   779     rot_from_world[0] = rot_from_parent[0];
   782     for (
int i = 0; i < num_links; ++i) {               
   783         const int parent = 
m_links[i].m_parent;
   785         rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
   788                 fromWorld.
m_rotMat = rot_from_world[i+1];
   789                 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
   797                         for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)            
   801                         spatVel[i+1] += spatJointVel;
   815                 spatVel[i+1].
cross(spatJointVel, spatCoriolisAcc[i]);           
   820                 btVector3 linkAppliedForce = isConstraintPass? 
m_links[i].m_appliedConstraintForce : 
m_links[i].m_appliedForce;
   821                 btVector3 linkAppliedTorque =isConstraintPass ? 
m_links[i].m_appliedConstraintTorque : 
m_links[i].m_appliedTorque;
   823                 zeroAccSpatFrc[i+1].
setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
   828                         b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
   830                         zeroAccSpatFrc[i+1].m_topVec[0],
   831                         zeroAccSpatFrc[i+1].m_topVec[1],
   832                         zeroAccSpatFrc[i+1].m_topVec[2],
   834                         zeroAccSpatFrc[i+1].m_bottomVec[0],
   835                         zeroAccSpatFrc[i+1].m_bottomVec[1],
   836                         zeroAccSpatFrc[i+1].m_bottomVec[2]);
   841                 btScalar linDampMult = 1., angDampMult = 1.;
   842                 zeroAccSpatFrc[i+1].
addVector(angDampMult * 
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
   843                                                                          linDampMult * 
m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
   854                                                                                                 0, 
m_links[i].m_inertiaLocal[1], 0,
   855                                                                                                 0, 0, 
m_links[i].m_inertiaLocal[2])
   860                         zeroAccSpatFrc[i+1].
addAngular(spatVel[i+1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));                 
   862                 zeroAccSpatFrc[i+1].
addLinear(
m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
   883     for (
int i = num_links - 1; i >= 0; --i)
   885                 const int parent = 
m_links[i].m_parent;
   888                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   892                         hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
   894                         Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
   895                         - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
   896                         - spatCoriolisAcc[i].
dot(hDof)
   900                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   903                         for(
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
   906                                 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
   916                                 invDi[0] = 1.0f / D[0];
   922                                 static btMatrix3x3 D3x3; D3x3.
setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
   926                                 for(
int row = 0; row < 3; ++row)
   928                                         for(
int col = 0; col < 3; ++col)
   930                                                 invDi[row * 3 + col] = invD3x3[row][col];
   943                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   945                         spatForceVecTemps[dof].
setZero();
   947                         for(
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
   951                                 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
   955                 dyadTemp = spatInertia[i+1];
   958                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   967                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   969                         invD_times_Y[dof] = 0.f;
   971                         for(
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
   973                                 invD_times_Y[dof] += invDi[dof * 
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];                              
   977                 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];             
   979                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
   983                         spatForceVecTemps[0] += hDof * invD_times_Y[dof];                       
   988                 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
  1011                 spatAcc[0] = -result;
  1016     for (
int i = 0; i < num_links; ++i) 
  1024         const int parent = 
m_links[i].m_parent;
  1027                 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
  1029                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
  1033                         Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);                   
  1040                 spatAcc[i+1] += spatCoriolisAcc[i];             
  1042                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)            
  1043                         spatAcc[i+1] += 
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
  1045                 if (
m_links[i].m_jointFeedback)
  1049                         btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
  1050                         btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
  1057                                  angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
  1063                                 if (isConstraintPass)
  1065  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += 
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
  1066                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec += 
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
  1069                                         m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = 
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
  1070                                         m_links[i].m_jointFeedback->m_reactionForces.m_topVec = 
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
  1074                                 if (isConstraintPass)
  1076                                           m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;                        
  1077                                 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
  1082                                 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
  1083                                 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
  1092         output[0] = omegadot_out[0];
  1093         output[1] = omegadot_out[1];
  1094         output[2] = omegadot_out[2];
  1097         output[3] = vdot_out[0];
  1098         output[4] = vdot_out[1];
  1099         output[5] = vdot_out[2];
  1121         if (!isConstraintPass)
  1157                 for (
int i = 0; i < num_links; ++i) 
  1159                         const int parent = 
m_links[i].m_parent;
  1164                         fromWorld.
m_rotMat = rot_from_world[i+1];                       
  1167                         fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
  1175                         for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)            
  1179                         spatVel[i+1] += spatJointVel;
  1213                 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
  1222                   btVector3 vtop = invI_upper_left*rhs_top;
  1224                   tmp = invIupper_right * rhs_bot;
  1226                   btVector3 vbot = invI_lower_left*rhs_top;
  1227                   tmp = invI_lower_right * rhs_bot;
  1229                   result[0] = vtop[0];
  1230                   result[1] = vtop[1];
  1231                   result[2] = vtop[2];
  1232                   result[3] = vbot[0];
  1233                   result[4] = vbot[1];
  1234                   result[5] = vbot[2];
  1257                 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
  1281         for (
int row = 0; row < rowsA; row++)
  1283                 for (
int col = 0; col < colsB; col++)
  1285                         pC[row * colsB + col] = 0.f;
  1286                         for (
int inner = 0; inner < rowsB; inner++)
  1288                                 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
  1303     scratch_v.
resize(4*num_links + 4);      
  1310         v_ptr += num_links * 2 + 2;
  1319         v_ptr += num_links * 2 + 2;
  1344                 fromParent.
m_rotMat = rot_from_parent[0];
  1347     for (
int i = 0; i < num_links; ++i) 
  1349                 zeroAccSpatFrc[i+1].
setZero();
  1354     for (
int i = num_links - 1; i >= 0; --i)
  1356                 const int parent = 
m_links[i].m_parent;
  1359                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
  1361                         Y[
m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
  1362                                                                                         - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
  1366                 btVector3 in_top, in_bottom, out_top, out_bottom;
  1369                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
  1371                         invD_times_Y[dof] = 0.f;
  1373                         for(
int dof2 = 0; dof2 < 
m_links[i].m_dofCount; ++dof2)
  1375                                 invD_times_Y[dof] += invDi[dof * 
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];                              
  1380                 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
  1382                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
  1386                         spatForceVecTemps[0] += hDof * invD_times_Y[dof];               
  1392                 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
  1396     btScalar * joint_accel = output + 6;
  1409                 spatAcc[0] = -result;
  1414     for (
int i = 0; i < num_links; ++i)
  1416         const int parent = 
m_links[i].m_parent;
  1419                 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
  1421                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)
  1425                         Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
  1431                 for(
int dof = 0; dof < 
m_links[i].m_dofCount; ++dof)            
  1432                         spatAcc[i+1] += 
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];      
  1438         output[0] = omegadot_out[0];
  1439         output[1] = omegadot_out[1];
  1440         output[2] = omegadot_out[2];
  1444         output[3] = vdot_out[0];
  1445         output[4] = vdot_out[1];
  1446         output[5] = vdot_out[2];
  1469         pBasePos[0] += dt * pBaseVel[0];
  1470         pBasePos[1] += dt * pBaseVel[1];
  1471         pBasePos[2] += dt * pBaseVel[2];
  1501                                 axis   = angvel*( 
btScalar(0.5)*dt-(dt*dt*dt)*(
btScalar(0.020833333333))*fAngle*fAngle );
  1525         static btQuaternion baseQuat; baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
  1526         static btVector3 baseOmega; baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
  1527         pQuatUpdateFun(baseOmega, baseQuat, 
true, dt);
  1528         pBaseQuat[0] = baseQuat.
x();
  1529         pBaseQuat[1] = baseQuat.
y();
  1530         pBaseQuat[2] = baseQuat.
z();
  1531         pBaseQuat[3] = baseQuat.
w();
  1544     for (
int i = 0; i < num_links; ++i) 
  1549                 switch(
m_links[i].m_jointType)
  1555                                 pJointPos[0] += dt * jointVel;
  1560                                 static btVector3 jointVel; jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
  1561                                 static btQuaternion jointOri; jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
  1562                                 pQuatUpdateFun(jointVel, jointOri, 
false, dt);
  1563                                 pJointPos[0] = jointOri.
x(); pJointPos[1] = jointOri.
y(); pJointPos[2] = jointOri.
z(); pJointPos[3] = jointOri.
w();
  1572                                 pJointPos[1] += 
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
  1573                                 pJointPos[2] += 
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
  1583                 m_links[i].updateCacheMultiDof(pq);
  1586                         pq += 
m_links[i].m_posVarCount;
  1604     scratch_v.
resize(3*num_links + 3);                  
  1605     scratch_m.
resize(num_links + 1);
  1608     btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
  1609     btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
  1610         btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
  1613     scratch_r.
resize(m_dofCount);
  1614     btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
  1619         const btVector3 &normal_lin_world = normal_lin;                                                 
  1620         const btVector3 &normal_ang_world = normal_ang;
  1626     omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
  1627         jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
  1628         jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
  1629         jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
  1631     jac[3] = normal_lin_world[0];
  1632     jac[4] = normal_lin_world[1];
  1633     jac[5] = normal_lin_world[2];
  1636         p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
  1637     n_local_lin[0] = rot_from_world[0] * normal_lin_world;
  1638         n_local_ang[0] = rot_from_world[0] * normal_ang_world;
  1647     if (num_links > 0 && link > -1) {
  1654         for (
int i = 0; i < num_links; ++i) {
  1657             const int parent = 
m_links[i].m_parent;
  1659             rot_from_world[i+1] = mtx * rot_from_world[parent+1];
  1661             n_local_lin[i+1] = mtx * n_local_lin[parent+1];
  1662                         n_local_ang[i+1] = mtx * n_local_ang[parent+1];
  1663             p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - 
m_links[i].m_cachedRVector;
  1666                         switch(
m_links[i].m_jointType)
  1670                                         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));
  1671                                         results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
  1676                                         results[
m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
  1681                                         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));
  1682                                         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));
  1683                                         results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
  1685                                         results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
  1686                                         results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
  1687                                         results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
  1693                                         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]));
  1694                                         results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
  1695                                         results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
  1710                         for(
int dof = 0; dof < 
m_links[link].m_dofCount; ++dof)
  1712                                 jac[6 + 
m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
  1716                         link = 
m_links[link].m_parent;
  1752     if (motion < SLEEP_EPSILON) {
  1775         for (
int i = 0; i < num_links; ++i) 
  1782         world_to_local.
resize(nLinks+1);
  1783         local_origin.
resize(nLinks+1);
  1800                 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
  1823                 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
  1853                         btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
  1880                         if (mbd->m_baseName)
  1886                 if (mbd->m_numLinks)
  1889                         int numElem = mbd->m_numLinks;
  1892                         for (
int i=0;i<numElem;i++,memPtr++)
  1916                                 for (
int posvar = 0; posvar < numPosVar;posvar++)
  1925                                         if (memPtr->m_linkName)
  1933                                         if (memPtr->m_jointName)
 btMatrix3x3 inverse() const 
Return the inverse of the matrix. 
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
btQuaternion m_zeroRotParentToThis
void clearConstraintForces()
void setupRevolute(int linkIndex, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
const btMultibodyLink & getLink(int index) const 
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
const btVector3 & getAxisBottom(int dof) const 
const btVector3 getBaseVel() const 
void serialize(struct btQuaternionData &dataOut) const 
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone. 
#define btMultiBodyLinkDataName
btScalar btSin(btScalar x)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
eFeatherstoneJointType m_jointType
const btScalar & z() const 
Return the z value. 
class btMultiBodyLinkCollider * m_collider
btTransform getBaseWorldTransform() const 
bool gDisableDeactivation
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
const btVector3 & getLinkForce(int i) const 
virtual int calculateSerializeBufferSize() const 
virtual void * getUniquePointer(void *oldPtr)=0
btScalar getBaseMass() const 
bool gJointFeedbackInWorldSpace
todo: determine if we need these options. If so, make a proper API, otherwise delete those globals ...
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btVector3 localDirToWorld(int i, const btVector3 &vec) const 
void addLinkConstraintForce(int i, const btVector3 &f)
btMatrix3x3 m_cachedInertiaLowerRight
btScalar * getJointVelMultiDof(int i)
void addLinkForce(int i, const btVector3 &f)
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const 
void addLinear(const btVector3 &linear)
btScalar getJointPos(int i) const 
bool gJointFeedbackInJointFrame
const btScalar & y() const 
Return the y value. 
const btVector3 & getAngular() const 
void setJointPosMultiDof(int i, btScalar *q)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
btScalar dot(const btSpatialForceVector &b) const 
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btVector3 m_baseConstraintTorque
#define btCollisionObjectData
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion. 
const btScalar & w() const 
Return the w value. 
btAlignedObjectArray< btScalar > m_deltaV
btScalar dot(const btVector3 &v) const 
Return the dot product. 
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
void clearForcesAndTorques()
const btScalar & x() const 
Return the x value. 
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
btVector3 getAngularMomentum() const 
void addAngular(const btVector3 &angular)
void addLinkConstraintTorque(int i, const btVector3 &t)
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btMatrix3x3 m_cachedInertiaTopLeft
const btVector3 & getBaseInertia() const 
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
btScalar m_jointTorque[6]
int size() const 
return the number of elements in the array 
void setVector(const btVector3 &angular, const btVector3 &linear)
btMatrix3x3 m_cachedInertiaLowerLeft
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1. 
const btVector3 & getLinkTorque(int i) const 
void addLinkTorque(int i, const btVector3 &t)
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const 
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const 
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w. 
void setLinear(const btVector3 &linear)
btVector3 cross(const btVector3 &v) const 
Return the cross product between this and another vector. 
void serialize(struct btVector3Data &dataOut) const 
void addJointTorque(int i, btScalar Q)
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major) 
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, float result[6]) const 
const btVector3 & getRVector(int i) const 
btScalar length() const 
Return the length of the vector. 
void setJointVel(int i, btScalar qdot)
void setAngular(const btVector3 &angular)
void setJointPos(int i, btScalar q)
void cross(const SpatialVectorType &b, SpatialVectorType &out) const 
void setWorldTransform(const btTransform &worldTrans)
const btScalar & y() const 
Return the y value. 
void checkMotionAndSleepIfRequired(btScalar timestep)
btVector3 m_baseConstraintForce
btVector3 can be used to represent 3D points and vectors. 
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool deprecatedMultiDof=true)
btScalar getLinkMass(int i) const 
btAlignedObjectArray< btScalar > m_realBuf
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const 
void updateLinksDofOffsets()
#define btMultiBodyDataName
btVector3 getBaseOmega() const 
btVector3 normalized() const 
Return a normalized version of this vector. 
#define btMultiBodyData
serialization data, don't change them if you are not familiar with the details of the serialization m...
bool m_useGlobalVelocities
virtual void serializeName(const char *ptr)=0
void resize(int newsize, const T &fillData=T())
const btVector3 & getLinear() const 
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
btMatrix3x3 transpose() const 
Return the transpose of the matrix. 
btVector3 worldPosToLocal(int i, const btVector3 &vec) const 
const btQuaternion & getParentToLocalRot(int i) const 
const btVector3 & getAngular() const 
btMatrix3x3 m_bottomLeftMat
void setVector(const btVector3 &angular, const btVector3 &linear)
btAlignedObjectArray< btVector3 > m_vectorBuf
btTransform m_cachedWorldTransform
const btScalar & x() const 
Return the x value. 
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
void setJointVelMultiDof(int i, btScalar *qdot)
const btMultiBodyLinkCollider * getBaseCollider() const 
btVector3 worldDirToLocal(int i, const btVector3 &vec) const 
const btVector3 & getLinkInertia(int i) const 
virtual const char * findNameForPointer(const void *ptr) const =0
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void addVector(const btVector3 &angular, const btVector3 &linear)
btScalar getJointTorque(int i) const 
const btQuaternion & getWorldToBaseRot() const 
int getParent(int link_num) const 
const btVector3 & getBasePos() const 
const btVector3 & getAxisTop(int dof) const 
btVector3 localPosToWorld(int i, const btVector3 &vec) const 
btAlignedObjectArray< btMultibodyLink > m_links
btScalar * getJointTorqueMultiDof(int i)
#define btMultiBodyLinkData
btMatrix3x3 m_cachedInertiaTopRight
btScalar * getJointPosMultiDof(int i)
virtual btChunk * allocate(size_t size, int numElements)=0
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const 
fills the dataBuffer and returns the struct name (and 0 on failure) 
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar m_angularDamping
btScalar btCos(btScalar x)
btMatrix3x3 m_topRightMat
btScalar getJointVel(int i) const 
btScalar getKineticEnergy() const 
const btScalar & z() const 
Return the z value. 
const btVector3 & getLinear() const