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;
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);
104 m_baseQuat(0, 0, 0, 1),
106 m_baseInertia(inertia),
108 m_fixedBase(fixedBase),
110 m_canSleep(canSleep),
112 m_userObjectPointer(0),
116 m_linearDamping(0.04f),
117 m_angularDamping(0.04f),
119 m_maxAppliedImpulse(1000.f),
120 m_maxCoordinateVelocity(100.f),
121 m_hasSelfCollision(true),
126 m_useGlobalVelocities(false),
127 m_internalNeedsJointFeedback(false)
154 const btVector3 &parentComToThisPivotOffset,
155 const btVector3 &thisPivotToThisComOffset,
bool )
159 m_links[i].m_inertiaLocal = inertia;
161 m_links[i].setAxisTop(0, 0., 0., 0.);
163 m_links[i].m_zeroRotParentToThis = rotParentToThis;
164 m_links[i].m_dVector = thisPivotToThisComOffset;
165 m_links[i].m_eVector = parentComToThisPivotOffset;
173 m_links[i].updateCacheMultiDof();
186 const btVector3 &parentComToThisPivotOffset,
187 const btVector3 &thisPivotToThisComOffset,
188 bool disableParentCollision)
194 m_links[i].m_inertiaLocal = inertia;
196 m_links[i].m_zeroRotParentToThis = rotParentToThis;
197 m_links[i].setAxisTop(0, 0., 0., 0.);
198 m_links[i].setAxisBottom(0, jointAxis);
199 m_links[i].m_eVector = parentComToThisPivotOffset;
200 m_links[i].m_dVector = thisPivotToThisComOffset;
201 m_links[i].m_cachedRotParentToThis = rotParentToThis;
206 m_links[i].m_jointPos[0] = 0.f;
207 m_links[i].m_jointTorque[0] = 0.f;
209 if (disableParentCollision)
213 m_links[i].updateCacheMultiDof();
224 const btVector3 &parentComToThisPivotOffset,
225 const btVector3 &thisPivotToThisComOffset,
226 bool disableParentCollision)
232 m_links[i].m_inertiaLocal = inertia;
234 m_links[i].m_zeroRotParentToThis = rotParentToThis;
235 m_links[i].setAxisTop(0, jointAxis);
236 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
237 m_links[i].m_dVector = thisPivotToThisComOffset;
238 m_links[i].m_eVector = parentComToThisPivotOffset;
243 m_links[i].m_jointPos[0] = 0.f;
244 m_links[i].m_jointTorque[0] = 0.f;
246 if (disableParentCollision)
249 m_links[i].updateCacheMultiDof();
261 const btVector3 &parentComToThisPivotOffset,
262 const btVector3 &thisPivotToThisComOffset,
263 bool disableParentCollision)
270 m_links[i].m_inertiaLocal = inertia;
272 m_links[i].m_zeroRotParentToThis = rotParentToThis;
273 m_links[i].m_dVector = thisPivotToThisComOffset;
274 m_links[i].m_eVector = parentComToThisPivotOffset;
279 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
280 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
281 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
282 m_links[i].setAxisBottom(0,
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
283 m_links[i].setAxisBottom(1,
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
284 m_links[i].setAxisBottom(2,
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
289 if (disableParentCollision)
292 m_links[i].updateCacheMultiDof();
303 const btVector3 &parentComToThisComOffset,
304 bool disableParentCollision)
311 m_links[i].m_inertiaLocal = inertia;
313 m_links[i].m_zeroRotParentToThis = rotParentToThis;
314 m_links[i].m_dVector.setZero();
315 m_links[i].m_eVector = parentComToThisComOffset;
318 btVector3 vecNonParallelToRotAxis(1, 0, 0);
319 if(rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
320 vecNonParallelToRotAxis.
setValue(0, 1, 0);
327 m_links[i].setAxisTop(0, n[0],n[1],n[2]);
328 m_links[i].setAxisTop(1,0,0,0);
329 m_links[i].setAxisTop(2,0,0,0);
330 m_links[i].setAxisBottom(0,0,0,0);
332 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
334 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
338 if (disableParentCollision)
341 m_links[i].updateCacheMultiDof();
371 return m_links[i].m_inertiaLocal;
376 return m_links[i].m_jointPos[0];
386 return &
m_links[i].m_jointPos[0];
396 return &
m_links[i].m_jointPos[0];
408 m_links[i].updateCacheMultiDof();
413 for(
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
414 m_links[i].m_jointPos[pos] = q[pos];
416 m_links[i].updateCacheMultiDof();
426 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
432 return m_links[i].m_cachedRVector;
437 return m_links[i].m_cachedRotParentToThis;
523 result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
534 for (
int i = 0; i < num_links; ++i)
536 const int parent =
m_links[i].m_parent;
540 omega[parent+1], vel[parent+1],
541 omega[i+1], vel[i+1]);
553 omega[i+1] += jointVel * axisTop;
554 vel[i+1] += jointVel * axisBottom;
576 for (
int i = 0; i < num_links; ++i) {
577 result +=
m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
578 result += omega[i+1].dot(
m_links[i].m_inertiaLocal * omega[i+1]);
581 return 0.5f * result;
596 for (
int i = 0; i < num_links; ++i) {
597 rot_from_world[i+1] =
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent+1];
598 result += (
quatRotate(rot_from_world[i+1].
inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
611 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
612 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
622 m_links[i].m_appliedForce.setValue(0, 0, 0);
623 m_links[i].m_appliedTorque.setValue(0, 0, 0);
637 m_links[i].m_appliedForce += f;
642 m_links[i].m_appliedTorque += t;
647 m_links[i].m_appliedConstraintForce += f;
652 m_links[i].m_appliedConstraintTorque += t;
659 m_links[i].m_jointTorque[0] += Q;
664 m_links[i].m_jointTorque[dof] += Q;
669 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
670 m_links[i].m_jointTorque[dof] = Q[dof];
675 return m_links[i].m_appliedForce;
680 return m_links[i].m_appliedTorque;
685 return m_links[i].m_jointTorque[0];
690 return &
m_links[i].m_jointTorque[0];
709 row1[0],row1[1],row1[2],
710 row2[0],row2[1],row2[2]);
714 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) 721 bool isConstraintPass)
754 scratch_v.
resize(8*num_links + 6);
755 scratch_m.
resize(4*num_links + 4);
763 v_ptr += num_links * 2 + 2;
767 v_ptr += num_links * 2 + 2;
771 v_ptr += num_links * 2;
784 v_ptr += num_links * 2 + 2;
804 btScalar * joint_accel = output + 6;
814 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
825 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
828 const btScalar linDampMult = 1., angDampMult = 1.;
829 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
830 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
837 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
853 rot_from_world[0] = rot_from_parent[0];
856 for (
int i = 0; i < num_links; ++i) {
857 const int parent =
m_links[i].m_parent;
859 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
862 fromWorld.
m_rotMat = rot_from_world[i+1];
863 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
871 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
875 spatVel[i+1] += spatJointVel;
889 spatVel[i+1].
cross(spatJointVel, spatCoriolisAcc[i]);
894 btVector3 linkAppliedForce = isConstraintPass?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
895 btVector3 linkAppliedTorque =isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
897 zeroAccSpatFrc[i+1].
setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
902 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
904 zeroAccSpatFrc[i+1].m_topVec[0],
905 zeroAccSpatFrc[i+1].m_topVec[1],
906 zeroAccSpatFrc[i+1].m_topVec[2],
908 zeroAccSpatFrc[i+1].m_bottomVec[0],
909 zeroAccSpatFrc[i+1].m_bottomVec[1],
910 zeroAccSpatFrc[i+1].m_bottomVec[2]);
915 btScalar linDampMult = 1., angDampMult = 1.;
916 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()),
917 linDampMult *
m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().safeNorm()));
928 0,
m_links[i].m_inertiaLocal[1], 0,
929 0, 0,
m_links[i].m_inertiaLocal[2])
934 zeroAccSpatFrc[i+1].
addAngular(spatVel[i+1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
936 zeroAccSpatFrc[i+1].
addLinear(
m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
957 for (
int i = num_links - 1; i >= 0; --i)
959 const int parent =
m_links[i].m_parent;
962 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
966 hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
968 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
969 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
970 - spatCoriolisAcc[i].
dot(hDof);
973 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
976 for(
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
979 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
991 invDi[0] = 1.0f / D[0];
1001 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1005 for(
int row = 0; row < 3; ++row)
1007 for(
int col = 0; col < 3; ++col)
1009 invDi[row * 3 + col] = invD3x3[row][col];
1022 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1024 spatForceVecTemps[dof].
setZero();
1026 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1030 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
1034 dyadTemp = spatInertia[i+1];
1037 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1046 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1048 invD_times_Y[dof] = 0.f;
1050 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1052 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1056 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1058 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1062 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1067 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1091 spatAcc[0] = -result;
1096 for (
int i = 0; i < num_links; ++i)
1104 const int parent =
m_links[i].m_parent;
1107 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1109 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1113 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1120 spatAcc[i+1] += spatCoriolisAcc[i];
1122 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1123 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1125 if (
m_links[i].m_jointFeedback)
1129 btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
1130 btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
1137 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1143 if (isConstraintPass)
1145 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1146 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1149 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1150 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1154 if (isConstraintPass)
1156 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1157 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1162 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1163 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1172 output[0] = omegadot_out[0];
1173 output[1] = omegadot_out[1];
1174 output[2] = omegadot_out[2];
1177 output[3] = vdot_out[0];
1178 output[4] = vdot_out[1];
1179 output[5] = vdot_out[2];
1201 if (!isConstraintPass)
1237 for (
int i = 0; i < num_links; ++i)
1239 const int parent =
m_links[i].m_parent;
1244 fromWorld.
m_rotMat = rot_from_world[i+1];
1247 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
1255 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1259 spatVel[i+1] += spatJointVel;
1305 for (
int i=0;i<6;i++)
1318 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1327 btVector3 vtop = invI_upper_left*rhs_top;
1329 tmp = invIupper_right * rhs_bot;
1331 btVector3 vbot = invI_lower_left*rhs_top;
1332 tmp = invI_lower_right * rhs_bot;
1334 result[0] = vtop[0];
1335 result[1] = vtop[1];
1336 result[2] = vtop[2];
1337 result[3] = vbot[0];
1338 result[4] = vbot[1];
1339 result[5] = vbot[2];
1381 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1405 for (
int row = 0; row < rowsA; row++)
1407 for (
int col = 0; col < colsB; col++)
1409 pC[row * colsB + col] = 0.f;
1410 for (
int inner = 0; inner < rowsB; inner++)
1412 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1427 scratch_v.
resize(4*num_links + 4);
1434 v_ptr += num_links * 2 + 2;
1443 v_ptr += num_links * 2 + 2;
1468 fromParent.
m_rotMat = rot_from_parent[0];
1471 for (
int i = 0; i < num_links; ++i)
1473 zeroAccSpatFrc[i+1].
setZero();
1478 for (
int i = num_links - 1; i >= 0; --i)
1480 const int parent =
m_links[i].m_parent;
1483 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1485 Y[
m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
1486 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1490 btVector3 in_top, in_bottom, out_top, out_bottom;
1493 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1495 invD_times_Y[dof] = 0.f;
1497 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1499 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1504 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
1506 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1510 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1516 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1520 btScalar * joint_accel = output + 6;
1533 spatAcc[0] = -result;
1538 for (
int i = 0; i < num_links; ++i)
1540 const int parent =
m_links[i].m_parent;
1543 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1545 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1549 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1555 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1556 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1562 output[0] = omegadot_out[0];
1563 output[1] = omegadot_out[1];
1564 output[2] = omegadot_out[2];
1568 output[3] = vdot_out[0];
1569 output[4] = vdot_out[1];
1570 output[5] = vdot_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 );
1649 btQuaternion baseQuat; baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1650 btVector3 baseOmega; baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1651 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1652 pBaseQuat[0] = baseQuat.
x();
1653 pBaseQuat[1] = baseQuat.
y();
1654 pBaseQuat[2] = baseQuat.
z();
1655 pBaseQuat[3] = baseQuat.
w();
1668 for (
int i = 0; i < num_links; ++i)
1673 switch(
m_links[i].m_jointType)
1679 pJointPos[0] += dt * jointVel;
1684 btVector3 jointVel; jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1685 btQuaternion jointOri; jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1686 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1687 pJointPos[0] = jointOri.
x(); pJointPos[1] = jointOri.
y(); pJointPos[2] = jointOri.
z(); pJointPos[3] = jointOri.
w();
1696 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1697 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1707 m_links[i].updateCacheMultiDof(pq);
1710 pq +=
m_links[i].m_posVarCount;
1728 scratch_v.
resize(3*num_links + 3);
1729 scratch_m.
resize(num_links + 1);
1732 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
1733 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
1734 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
1737 scratch_r.
resize(m_dofCount);
1738 btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
1743 const btVector3 &normal_lin_world = normal_lin;
1744 const btVector3 &normal_ang_world = normal_ang;
1750 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1751 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1752 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1753 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1755 jac[3] = normal_lin_world[0];
1756 jac[4] = normal_lin_world[1];
1757 jac[5] = normal_lin_world[2];
1760 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1761 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1762 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1771 if (num_links > 0 && link > -1) {
1778 for (
int i = 0; i < num_links; ++i) {
1781 const int parent =
m_links[i].m_parent;
1783 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
1785 n_local_lin[i+1] = mtx * n_local_lin[parent+1];
1786 n_local_ang[i+1] = mtx * n_local_ang[parent+1];
1787 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] -
m_links[i].m_cachedRVector;
1790 switch(
m_links[i].m_jointType)
1794 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));
1795 results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
1800 results[
m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
1805 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));
1806 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));
1807 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));
1809 results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
1810 results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
1811 results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
1817 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]));
1818 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
1819 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
1834 for(
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
1836 jac[6 +
m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
1840 link =
m_links[link].m_parent;
1876 if (motion < SLEEP_EPSILON) {
1899 for (
int i = 0; i < num_links; ++i)
1906 world_to_local.
resize(nLinks+1);
1907 local_origin.
resize(nLinks+1);
1924 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
1947 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
1977 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2008 if (mbd->m_baseName)
2014 if (mbd->m_numLinks)
2017 int numElem = mbd->m_numLinks;
2020 for (
int i=0;i<numElem;i++,memPtr++)
2057 for (
int posvar = 0; posvar < numPosVar;posvar++)
2066 if (memPtr->m_linkName)
2074 if (memPtr->m_jointName)
2087 #ifdef BT_USE_DOUBLE_PRECISION 2088 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));
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)
btScalar m_jointLowerLimit
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
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
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
btScalar getBaseMass() const
bool gJointFeedbackInWorldSpace
todo: determine if we need these options. If so, make a proper API, otherwise delete those globals ...
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
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
btSpatialMotionVector m_absFrameLocVelocity
bool gJointFeedbackInJointFrame
const btScalar & y() const
Return the y value.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
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
btScalar m_jointUpperLimit
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
bool m_cachedInertiaValid
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)
const btVector3 & getRVector(int i) const
btQuaternion inverse() const
Return the inverse of this quaternion.
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
btSpatialMotionVector m_absFrameTotVelocity
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 m_jointMaxVelocity
btScalar getJointVel(int i) const
btScalar getKineticEnergy() const
const btScalar & z() const
Return the z value.
const btVector3 & getLinear() const