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),
103 m_basePos_interpolate(0, 0, 0),
104 m_baseQuat_interpolate(0, 0, 0, 1),
106 m_baseInertia(inertia),
108 m_fixedBase(fixedBase),
110 m_canSleep(canSleep),
113 m_userObjectPointer(0),
117 m_linearDamping(0.04f),
118 m_angularDamping(0.04f),
120 m_maxAppliedImpulse(1000.f),
121 m_maxCoordinateVelocity(100.f),
122 m_hasSelfCollision(true),
127 m_useGlobalVelocities(false),
128 m_internalNeedsJointFeedback(false)
155 const btVector3 &parentComToThisPivotOffset,
156 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();
184 const btVector3 &parentComToThisPivotOffset,
185 const btVector3 &thisPivotToThisComOffset,
186 bool disableParentCollision)
192 m_links[i].m_inertiaLocal = inertia;
194 m_links[i].m_zeroRotParentToThis = rotParentToThis;
195 m_links[i].setAxisTop(0, 0., 0., 0.);
196 m_links[i].setAxisBottom(0, jointAxis);
197 m_links[i].m_eVector = parentComToThisPivotOffset;
198 m_links[i].m_dVector = thisPivotToThisComOffset;
199 m_links[i].m_cachedRotParentToThis = rotParentToThis;
204 m_links[i].m_jointPos[0] = 0.f;
205 m_links[i].m_jointTorque[0] = 0.f;
207 if (disableParentCollision)
211 m_links[i].updateCacheMultiDof();
222 const btVector3 &parentComToThisPivotOffset,
223 const btVector3 &thisPivotToThisComOffset,
224 bool disableParentCollision)
230 m_links[i].m_inertiaLocal = inertia;
232 m_links[i].m_zeroRotParentToThis = rotParentToThis;
233 m_links[i].setAxisTop(0, jointAxis);
234 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
235 m_links[i].m_dVector = thisPivotToThisComOffset;
236 m_links[i].m_eVector = parentComToThisPivotOffset;
241 m_links[i].m_jointPos[0] = 0.f;
242 m_links[i].m_jointTorque[0] = 0.f;
244 if (disableParentCollision)
247 m_links[i].updateCacheMultiDof();
257 const btVector3 &parentComToThisPivotOffset,
258 const btVector3 &thisPivotToThisComOffset,
259 bool disableParentCollision)
265 m_links[i].m_inertiaLocal = inertia;
267 m_links[i].m_zeroRotParentToThis = rotParentToThis;
268 m_links[i].m_dVector = thisPivotToThisComOffset;
269 m_links[i].m_eVector = parentComToThisPivotOffset;
274 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
275 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
276 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
277 m_links[i].setAxisBottom(0,
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
278 m_links[i].setAxisBottom(1,
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
279 m_links[i].setAxisBottom(2,
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
281 m_links[i].m_jointPos[3] = 1.f;
284 if (disableParentCollision)
287 m_links[i].updateCacheMultiDof();
298 const btVector3 &parentComToThisComOffset,
299 bool disableParentCollision)
305 m_links[i].m_inertiaLocal = inertia;
307 m_links[i].m_zeroRotParentToThis = rotParentToThis;
308 m_links[i].m_dVector.setZero();
309 m_links[i].m_eVector = parentComToThisComOffset;
312 btVector3 vecNonParallelToRotAxis(1, 0, 0);
313 if (rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
314 vecNonParallelToRotAxis.
setValue(0, 1, 0);
321 m_links[i].setAxisTop(0, n[0], n[1], n[2]);
322 m_links[i].setAxisTop(1, 0, 0, 0);
323 m_links[i].setAxisTop(2, 0, 0, 0);
324 m_links[i].setAxisBottom(0, 0, 0, 0);
326 m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
328 m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
332 if (disableParentCollision)
335 m_links[i].updateCacheMultiDof();
339 m_links[i].setAxisBottom(1,
m_links[i].getAxisBottom(1).normalized());
340 m_links[i].setAxisBottom(2,
m_links[i].getAxisBottom(2).normalized());
359 return m_links[link_num].m_parent;
369 return m_links[i].m_inertiaLocal;
374 return m_links[i].m_jointPos[0];
384 return &
m_links[i].m_jointPos[0];
394 return &
m_links[i].m_jointPos[0];
405 m_links[i].updateCacheMultiDof();
411 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
414 m_links[i].updateCacheMultiDof();
419 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
422 m_links[i].updateCacheMultiDof();
434 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
440 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
446 return m_links[i].m_cachedRVector;
451 return m_links[i].m_cachedRotParentToThis;
456 return m_links[i].m_cachedRVector_interpolate;
461 return m_links[i].m_cachedRotParentToThis_interpolate;
554 result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
566 for (
int i = 0; i < num_links; ++i)
573 omega[parent + 1], vel[parent + 1],
574 omega[i + 1], vel[i + 1]);
578 for (
int dof = 0; dof < link.
m_dofCount; ++dof)
580 omega[i + 1] += jointVel[dof] * link.
getAxisTop(dof);
591 omega.
resize(num_links + 1);
593 vel.
resize(num_links + 1);
600 for (
int i = 0; i < num_links; ++i)
602 result +=
m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
603 result += omega[i + 1].dot(
m_links[i].m_inertiaLocal * omega[i + 1]);
606 return 0.5f * result;
614 omega.
resize(num_links + 1);
616 vel.
resize(num_links + 1);
618 rot_from_world.
resize(num_links + 1);
624 for (
int i = 0; i < num_links; ++i)
626 rot_from_world[i + 1] =
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent + 1];
640 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
641 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
651 m_links[i].m_appliedForce.setValue(0, 0, 0);
652 m_links[i].m_appliedTorque.setValue(0, 0, 0);
666 m_links[i].m_appliedForce += f;
671 m_links[i].m_appliedTorque += t;
676 m_links[i].m_appliedConstraintForce += f;
681 m_links[i].m_appliedConstraintTorque += t;
686 m_links[i].m_jointTorque[0] += Q;
691 m_links[i].m_jointTorque[dof] += Q;
696 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
697 m_links[i].m_jointTorque[dof] = Q[dof];
702 return m_links[i].m_appliedForce;
707 return m_links[i].m_appliedTorque;
712 return m_links[i].m_jointTorque[0];
717 return &
m_links[i].m_jointTorque[0];
736 row1[0], row1[1], row1[2],
737 row2[0], row2[1], row2[2]);
741 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
748 bool isConstraintPass,
749 bool jointFeedbackInWorldSpace,
750 bool jointFeedbackInJointFrame)
783 scratch_v.
resize(8 * num_links + 6);
784 scratch_m.
resize(4 * num_links + 4);
792 v_ptr += num_links * 2 + 2;
796 v_ptr += num_links * 2 + 2;
800 v_ptr += num_links * 2;
813 v_ptr += num_links * 2 + 2;
843 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
854 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
857 const btScalar linDampMult = 1., angDampMult = 1.;
858 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
859 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
866 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
880 rot_from_world[0] = rot_from_parent[0];
883 for (
int i = 0; i < num_links; ++i)
885 const int parent =
m_links[i].m_parent;
887 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
889 fromParent.
m_rotMat = rot_from_parent[i + 1];
891 fromWorld.
m_rotMat = rot_from_world[i + 1];
892 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
900 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
904 spatVel[i + 1] += spatJointVel;
917 spatVel[i + 1].
cross(spatJointVel, spatCoriolisAcc[i]);
922 btVector3 linkAppliedForce = isConstraintPass ?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
923 btVector3 linkAppliedTorque = isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
925 zeroAccSpatFrc[i + 1].
setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
930 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
932 zeroAccSpatFrc[i+1].m_topVec[0],
933 zeroAccSpatFrc[i+1].m_topVec[1],
934 zeroAccSpatFrc[i+1].m_topVec[2],
936 zeroAccSpatFrc[i+1].m_bottomVec[0],
937 zeroAccSpatFrc[i+1].m_bottomVec[1],
938 zeroAccSpatFrc[i+1].m_bottomVec[2]);
943 btScalar linDampMult = 1., angDampMult = 1.;
944 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()),
945 linDampMult *
m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
956 0,
m_links[i].m_inertiaLocal[1], 0,
957 0, 0,
m_links[i].m_inertiaLocal[2]));
961 zeroAccSpatFrc[i + 1].
addAngular(spatVel[i + 1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
963 zeroAccSpatFrc[i + 1].
addLinear(
m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
982 for (
int i = num_links - 1; i >= 0; --i)
984 const int parent =
m_links[i].m_parent;
985 fromParent.
m_rotMat = rot_from_parent[i + 1];
988 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
992 hDof = spatInertia[i + 1] *
m_links[i].m_axes[dof];
994 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);
996 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
999 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1002 D_row[dof2] =
m_links[i].m_axes[dof].dot(hDof2);
1007 switch (
m_links[i].m_jointType)
1014 invDi[0] = 1.0f / D[0];
1025 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1029 for (
int row = 0; row < 3; ++row)
1031 for (
int col = 0; col < 3; ++col)
1033 invDi[row * 3 + col] = invD3x3[row][col];
1045 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1047 spatForceVecTemps[dof].
setZero();
1049 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1053 spatForceVecTemps[dof] += hDof2 * invDi[dof2 *
m_links[i].m_dofCount + dof];
1057 dyadTemp = spatInertia[i + 1];
1060 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1069 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1071 invD_times_Y[dof] = 0.f;
1073 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1075 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1079 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1081 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1085 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1090 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1112 spatAcc[0] = -result;
1116 for (
int i = 0; i < num_links; ++i)
1124 const int parent =
m_links[i].m_parent;
1125 fromParent.
m_rotMat = rot_from_parent[i + 1];
1128 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1130 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1134 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1141 spatAcc[i + 1] += spatCoriolisAcc[i];
1143 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1144 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1146 if (
m_links[i].m_jointFeedback)
1150 btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1151 btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1153 if (jointFeedbackInJointFrame)
1158 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1161 if (jointFeedbackInWorldSpace)
1163 if (isConstraintPass)
1165 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1166 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1170 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1171 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1176 if (isConstraintPass)
1178 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1179 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1183 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1184 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1192 output[0] = omegadot_out[0];
1193 output[1] = omegadot_out[1];
1194 output[2] = omegadot_out[2];
1221 if (!isConstraintPass)
1256 for (
int i = 0; i < num_links; ++i)
1258 const int parent =
m_links[i].m_parent;
1262 fromParent.
m_rotMat = rot_from_parent[i + 1];
1264 fromWorld.
m_rotMat = rot_from_world[i + 1];
1267 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
1275 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1279 spatVel[i + 1] += spatJointVel;
1324 for (
int i = 0; i < 6; i++)
1337 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1346 btVector3 vtop = invI_upper_left * rhs_top;
1348 tmp = invIupper_right * rhs_bot;
1350 btVector3 vbot = invI_lower_left * rhs_top;
1351 tmp = invI_lower_right * rhs_bot;
1353 result[0] = vtop[0];
1354 result[1] = vtop[1];
1355 result[2] = vtop[2];
1356 result[3] = vbot[0];
1357 result[4] = vbot[1];
1358 result[5] = vbot[2];
1402 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1425 for (
int row = 0; row < rowsA; row++)
1427 for (
int col = 0; col < colsB; col++)
1429 pC[row * colsB + col] = 0.f;
1430 for (
int inner = 0; inner < rowsB; inner++)
1432 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1446 scratch_v.
resize(4 * num_links + 4);
1453 v_ptr += num_links * 2 + 2;
1462 v_ptr += num_links * 2 + 2;
1488 fromParent.
m_rotMat = rot_from_parent[0];
1491 for (
int i = 0; i < num_links; ++i)
1493 zeroAccSpatFrc[i + 1].
setZero();
1498 for (
int i = num_links - 1; i >= 0; --i)
1500 const int parent =
m_links[i].m_parent;
1501 fromParent.
m_rotMat = rot_from_parent[i + 1];
1504 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1506 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]);
1509 btVector3 in_top, in_bottom, out_top, out_bottom;
1512 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1514 invD_times_Y[dof] = 0.f;
1516 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1518 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1523 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1525 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1529 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1534 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1550 spatAcc[0] = -result;
1554 for (
int i = 0; i < num_links; ++i)
1556 const int parent =
m_links[i].m_parent;
1557 fromParent.
m_rotMat = rot_from_parent[i + 1];
1560 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1562 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1566 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1572 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1573 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1579 output[0] = omegadot_out[0];
1580 output[1] = omegadot_out[1];
1581 output[2] = omegadot_out[2];
1607 for (
int i = 0; i < 3; ++i)
1613 pBasePos[0] += dt * pBaseVel[0];
1614 pBasePos[1] += dt * pBaseVel[1];
1615 pBasePos[2] += dt * pBaseVel[2];
1645 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1650 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1669 for (
int i = 0; i < 4; ++i)
1678 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1680 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1681 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1682 pBaseQuat[0] = baseQuat.
x();
1683 pBaseQuat[1] = baseQuat.
y();
1684 pBaseQuat[2] = baseQuat.
z();
1685 pBaseQuat[3] = baseQuat.
w();
1688 for (
int i = 0; i < num_links; ++i)
1691 pJointPos = &
m_links[i].m_jointPos_interpolate[0];
1695 switch (
m_links[i].m_jointType)
1701 pJointPos[0] =
m_links[i].m_jointPos[0];
1703 pJointPos[0] += dt * jointVel;
1710 for (
int j = 0; j < 4; ++j)
1712 pJointPos[j] =
m_links[i].m_jointPos[j];
1716 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1718 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1719 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1720 pJointPos[0] = jointOri.
x();
1721 pJointPos[1] = jointOri.
y();
1722 pJointPos[2] = jointOri.
z();
1723 pJointPos[3] = jointOri.
w();
1728 for (
int j = 0; j < 3; ++j)
1730 pJointPos[j] =
m_links[i].m_jointPos[j];
1736 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1737 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1745 m_links[i].updateInterpolationCacheMultiDof();
1759 pBasePos[0] += dt * pBaseVel[0];
1760 pBasePos[1] += dt * pBaseVel[1];
1761 pBasePos[2] += dt * pBaseVel[2];
1791 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1796 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1816 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1818 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1819 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1820 pBaseQuat[0] = baseQuat.
x();
1821 pBaseQuat[1] = baseQuat.
y();
1822 pBaseQuat[2] = baseQuat.
z();
1823 pBaseQuat[3] = baseQuat.
w();
1835 for (
int i = 0; i < num_links; ++i)
1838 pJointPos= (pq ? pq : &
m_links[i].m_jointPos[0]);
1842 switch (
m_links[i].m_jointType)
1849 pJointPos[0] += dt * jointVel;
1856 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1858 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1859 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1860 pJointPos[0] = jointOri.
x();
1861 pJointPos[1] = jointOri.
y();
1862 pJointPos[2] = jointOri.
z();
1863 pJointPos[3] = jointOri.
w();
1872 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1873 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1882 m_links[i].updateCacheMultiDof(pq);
1885 pq +=
m_links[i].m_posVarCount;
1903 scratch_v.
resize(3 * num_links + 3);
1904 scratch_m.
resize(num_links + 1);
1908 v_ptr += num_links + 1;
1910 v_ptr += num_links + 1;
1912 v_ptr += num_links + 1;
1921 int numLinksChildToRoot=0;
1925 links[numLinksChildToRoot++]=l;
1932 const btVector3 &normal_lin_world = normal_lin;
1933 const btVector3 &normal_ang_world = normal_ang;
1939 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1940 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1941 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1942 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1944 jac[3] = normal_lin_world[0];
1945 jac[4] = normal_lin_world[1];
1946 jac[5] = normal_lin_world[2];
1949 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1950 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1951 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1960 if (num_links > 0 && link > -1)
1966 for (
int a = 0; a < numLinksChildToRoot; a++)
1968 int i = links[numLinksChildToRoot-1-a];
1970 const int parent =
m_links[i].m_parent;
1972 rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
1974 n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
1975 n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
1976 p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] -
m_links[i].m_cachedRVector;
1979 switch (
m_links[i].m_jointType)
1983 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));
1984 results[
m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
1989 results[
m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(0));
1994 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));
1995 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));
1996 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));
1998 results[
m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
1999 results[
m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(1));
2000 results[
m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(2));
2006 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]));
2007 results[
m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(1));
2008 results[
m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(2));
2022 for (
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
2024 jac[6 +
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
2028 link =
m_links[link].m_parent;
2064 if (motion < SLEEP_EPSILON)
2092 for (
int i = 0; i < num_links; ++i)
2099 world_to_local.
resize(nLinks + 1);
2100 local_origin.
resize(nLinks + 1);
2114 int index = link + 1;
2117 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2138 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2163 int index = link + 1;
2167 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2191 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2215 int index = link + 1;
2219 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2250 if (mbd->m_baseName)
2256 if (mbd->m_numLinks)
2259 int numElem = mbd->m_numLinks;
2262 for (
int i = 0; i < numElem; i++, memPtr++)
2297 for (
int posvar = 0; posvar < numPosVar; posvar++)
2305 if (memPtr->m_linkName)
2313 if (memPtr->m_jointName)
2325 #ifdef BT_USE_DOUBLE_PRECISION
2326 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));
#define btCollisionObjectData
@ BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
#define btMultiBodyDataName
#define btMultiBodyData
serialization data, don't change them if you are not familiar with the details of the serialization m...
#define btMultiBodyLinkData
#define btMultiBodyLinkDataName
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
bool gDisableDeactivation
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSin(btScalar x)
btScalar btCos(btScalar x)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void setWorldTransform(const btTransform &worldTrans)
void setInterpolationWorldTransform(const btTransform &trans)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
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 setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void setJointVelMultiDof(int i, const double *qdot)
const btVector3 & getLinkTorque(int i) const
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
btAlignedObjectArray< btMultibodyLink > m_links
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
const btQuaternion & getInterpolateWorldToBaseRot() const
btVector3 m_basePos_interpolate
btAlignedObjectArray< btVector3 > m_vectorBuf
const btQuaternion & getInterpolateParentToLocalRot(int i) const
btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const
btScalar getJointPos(int i) const
const btVector3 & getLinkInertia(int i) const
btScalar * getJointPosMultiDof(int i)
btScalar m_angularDamping
btScalar getKineticEnergy() const
void predictPositionsMultiDof(btScalar dt)
btMatrix3x3 m_cachedInertiaLowerRight
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
void addLinkConstraintForce(int i, const btVector3 &f)
const btQuaternion & getWorldToBaseRot() const
btAlignedObjectArray< btScalar > m_realBuf
btVector3 m_baseConstraintTorque
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
const btVector3 & getRVector(int i) const
const btVector3 & getBasePos() const
btVector3 getBaseOmega() const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const
void addLinkTorque(int i, const btVector3 &t)
void checkMotionAndSleepIfRequired(btScalar timestep)
btScalar * getJointTorqueMultiDof(int i)
btQuaternion m_baseQuat_interpolate
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
const btVector3 & getInterpolateRVector(int i) const
btScalar getLinkMass(int i) const
bool m_useGlobalVelocities
void addLinkForce(int i, const btVector3 &f)
void addJointTorque(int i, btScalar Q)
btMatrix3x3 m_cachedInertiaLowerLeft
const btVector3 & getLinkForce(int i) const
int getParent(int link_num) const
void setJointPosMultiDof(int i, const double *q)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void setJointVel(int i, btScalar qdot)
const btMultibodyLink & getLink(int index) const
btScalar getJointTorque(int i) const
void clearConstraintForces()
btMatrix3x3 m_cachedInertiaTopLeft
void addLinkConstraintTorque(int i, const btVector3 &t)
bool m_cachedInertiaValid
void setupSpherical(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
void setJointPos(int i, btScalar q)
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
const btVector3 & getBaseInertia() const
static void spatialTransform(const btMatrix3x3 &rotation_matrix, const btVector3 &displacement, const btVector3 &top_in, const btVector3 &bottom_in, btVector3 &top_out, btVector3 &bottom_out)
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
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
void setupRevolute(int i, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
const btMultiBodyLinkCollider * getBaseCollider() const
btMatrix3x3 m_cachedInertiaTopRight
btScalar getBaseMass() const
btAlignedObjectArray< btScalar > m_deltaV
btVector3 getAngularMomentum() const
void updateLinksDofOffsets()
const btVector3 & getInterpolateBasePos() const
virtual int calculateSerializeBufferSize() const
void setupFixed(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
const btQuaternion & getParentToLocalRot(int i) const
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
btVector3 m_baseConstraintForce
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
btVector3 localDirToWorld(int i, const btVector3 &local_dir) const
btScalar getJointVel(int i) const
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
const btVector3 getBaseVel() const
btScalar * getJointVelMultiDof(int i)
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool deprecatedMultiDof=true)
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
void clearForcesAndTorques()
const btScalar & x() const
Return the x value.
const btScalar & z() const
Return the z value.
const btScalar & w() const
Return the w value.
const btScalar & y() const
Return the y value.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void serialize(struct btQuaternionData &dataOut) const
btQuaternion inverse() const
Return the inverse of this quaternion.
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
virtual btChunk * allocate(size_t size, int numElements)=0
virtual const char * findNameForPointer(const void *ptr) const =0
virtual void serializeName(const char *ptr)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btVector3 can be used to represent 3D points and vectors.
const btScalar & y() const
Return the y value.
btScalar length() const
Return the length of the vector.
const btScalar & z() const
Return the z value.
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
btVector3 normalized() const
Return a normalized version of this vector.
const btScalar & x() const
Return the x value.
void serialize(struct btVector3Data &dataOut) const
btQuaternion m_zeroRotParentToThis
class btMultiBodyLinkCollider * m_collider
btQuaternion m_cachedRotParentToThis
btScalar m_jointUpperLimit
eFeatherstoneJointType m_jointType
btScalar m_jointLowerLimit
btSpatialMotionVector m_absFrameTotVelocity
const btVector3 & getAxisBottom(int dof) const
btTransform m_cachedWorldTransform
const btVector3 & getAxisTop(int dof) const
btScalar m_jointMaxVelocity
btScalar m_jointTorque[6]
btSpatialMotionVector m_absFrameLocVelocity
btVector3 m_cachedRVector
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
void addLinear(const btVector3 &linear)
const btVector3 & getAngular() const
void addVector(const btVector3 &angular, const btVector3 &linear)
void addAngular(const btVector3 &angular)
const btVector3 & getLinear() const
void setVector(const btVector3 &angular, const btVector3 &linear)
void setLinear(const btVector3 &linear)
const btVector3 & getAngular() const
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
btScalar dot(const btSpatialForceVector &b) const
void setAngular(const btVector3 &angular)
const btVector3 & getLinear() const
void setVector(const btVector3 &angular, const btVector3 &linear)
btMatrix3x3 m_topRightMat
btMatrix3x3 m_bottomLeftMat
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)