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),
129 m_kinematic_calculate_velocity(false)
156 const btVector3 &parentComToThisPivotOffset,
157 const btVector3 &thisPivotToThisComOffset,
bool )
160 m_links[i].m_inertiaLocal = inertia;
162 m_links[i].setAxisTop(0, 0., 0., 0.);
164 m_links[i].m_zeroRotParentToThis = rotParentToThis;
165 m_links[i].m_dVector = thisPivotToThisComOffset;
166 m_links[i].m_eVector = parentComToThisPivotOffset;
174 m_links[i].updateCacheMultiDof();
185 const btVector3 &parentComToThisPivotOffset,
186 const btVector3 &thisPivotToThisComOffset,
187 bool disableParentCollision)
193 m_links[i].m_inertiaLocal = inertia;
195 m_links[i].m_zeroRotParentToThis = rotParentToThis;
196 m_links[i].setAxisTop(0, 0., 0., 0.);
197 m_links[i].setAxisBottom(0, jointAxis);
198 m_links[i].m_eVector = parentComToThisPivotOffset;
199 m_links[i].m_dVector = thisPivotToThisComOffset;
200 m_links[i].m_cachedRotParentToThis = rotParentToThis;
205 m_links[i].m_jointPos[0] = 0.f;
206 m_links[i].m_jointTorque[0] = 0.f;
208 if (disableParentCollision)
212 m_links[i].updateCacheMultiDof();
223 const btVector3 &parentComToThisPivotOffset,
224 const btVector3 &thisPivotToThisComOffset,
225 bool disableParentCollision)
231 m_links[i].m_inertiaLocal = inertia;
233 m_links[i].m_zeroRotParentToThis = rotParentToThis;
234 m_links[i].setAxisTop(0, jointAxis);
235 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
236 m_links[i].m_dVector = thisPivotToThisComOffset;
237 m_links[i].m_eVector = parentComToThisPivotOffset;
242 m_links[i].m_jointPos[0] = 0.f;
243 m_links[i].m_jointTorque[0] = 0.f;
245 if (disableParentCollision)
248 m_links[i].updateCacheMultiDof();
258 const btVector3 &parentComToThisPivotOffset,
259 const btVector3 &thisPivotToThisComOffset,
260 bool disableParentCollision)
266 m_links[i].m_inertiaLocal = inertia;
268 m_links[i].m_zeroRotParentToThis = rotParentToThis;
269 m_links[i].m_dVector = thisPivotToThisComOffset;
270 m_links[i].m_eVector = parentComToThisPivotOffset;
275 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
276 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
277 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
278 m_links[i].setAxisBottom(0,
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
279 m_links[i].setAxisBottom(1,
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
280 m_links[i].setAxisBottom(2,
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
282 m_links[i].m_jointPos[3] = 1.f;
285 if (disableParentCollision)
288 m_links[i].updateCacheMultiDof();
299 const btVector3 &parentComToThisComOffset,
300 bool disableParentCollision)
306 m_links[i].m_inertiaLocal = inertia;
308 m_links[i].m_zeroRotParentToThis = rotParentToThis;
309 m_links[i].m_dVector.setZero();
310 m_links[i].m_eVector = parentComToThisComOffset;
313 btVector3 vecNonParallelToRotAxis(1, 0, 0);
314 if (rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
315 vecNonParallelToRotAxis.
setValue(0, 1, 0);
322 m_links[i].setAxisTop(0, n[0], n[1], n[2]);
323 m_links[i].setAxisTop(1, 0, 0, 0);
324 m_links[i].setAxisTop(2, 0, 0, 0);
325 m_links[i].setAxisBottom(0, 0, 0, 0);
327 m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
329 m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
333 if (disableParentCollision)
336 m_links[i].updateCacheMultiDof();
340 m_links[i].setAxisBottom(1,
m_links[i].getAxisBottom(1).normalized());
341 m_links[i].setAxisBottom(2,
m_links[i].getAxisBottom(2).normalized());
362 return m_links[link_num].m_parent;
372 return m_links[i].m_inertiaLocal;
377 return m_links[i].m_jointPos[0];
387 return &
m_links[i].m_jointPos[0];
397 return &
m_links[i].m_jointPos[0];
408 m_links[i].updateCacheMultiDof();
414 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
417 m_links[i].updateCacheMultiDof();
422 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
425 m_links[i].updateCacheMultiDof();
437 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
443 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
449 return m_links[i].m_cachedRVector;
454 return m_links[i].m_cachedRotParentToThis;
459 return m_links[i].m_cachedRVector_interpolate;
464 return m_links[i].m_cachedRotParentToThis_interpolate;
557 result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
569 for (
int i = 0; i < num_links; ++i)
576 omega[parent + 1], vel[parent + 1],
577 omega[i + 1], vel[i + 1]);
581 for (
int dof = 0; dof < link.
m_dofCount; ++dof)
583 omega[i + 1] += jointVel[dof] * link.
getAxisTop(dof);
597 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
598 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
608 m_links[i].m_appliedForce.setValue(0, 0, 0);
609 m_links[i].m_appliedTorque.setValue(0, 0, 0);
623 m_links[i].m_appliedForce += f;
628 m_links[i].m_appliedTorque += t;
633 m_links[i].m_appliedConstraintForce += f;
638 m_links[i].m_appliedConstraintTorque += t;
643 m_links[i].m_jointTorque[0] += Q;
648 m_links[i].m_jointTorque[dof] += Q;
653 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
654 m_links[i].m_jointTorque[dof] = Q[dof];
659 return m_links[i].m_appliedForce;
664 return m_links[i].m_appliedTorque;
669 return m_links[i].m_jointTorque[0];
674 return &
m_links[i].m_jointTorque[0];
717 row1[0], row1[1], row1[2],
718 row2[0], row2[1], row2[2]);
722 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
729 bool isConstraintPass,
730 bool jointFeedbackInWorldSpace,
731 bool jointFeedbackInJointFrame)
764 scratch_v.
resize(8 * num_links + 6);
765 scratch_m.
resize(4 * num_links + 4);
773 v_ptr += num_links * 2 + 2;
777 v_ptr += num_links * 2 + 2;
781 v_ptr += num_links * 2;
794 v_ptr += num_links * 2 + 2;
824 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
835 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
838 const btScalar linDampMult = 1., angDampMult = 1.;
839 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
840 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
847 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
861 rot_from_world[0] = rot_from_parent[0];
864 for (
int i = 0; i < num_links; ++i)
866 const int parent =
m_links[i].m_parent;
868 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
870 fromParent.
m_rotMat = rot_from_parent[i + 1];
872 fromWorld.
m_rotMat = rot_from_world[i + 1];
873 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
881 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
885 spatVel[i + 1] += spatJointVel;
898 spatVel[i + 1].
cross(spatJointVel, spatCoriolisAcc[i]);
908 btVector3 linkAppliedForce = isConstraintPass ?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
909 btVector3 linkAppliedTorque = isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
911 zeroAccSpatFrc[i + 1].
setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
916 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
918 zeroAccSpatFrc[i+1].m_topVec[0],
919 zeroAccSpatFrc[i+1].m_topVec[1],
920 zeroAccSpatFrc[i+1].m_topVec[2],
922 zeroAccSpatFrc[i+1].m_bottomVec[0],
923 zeroAccSpatFrc[i+1].m_bottomVec[1],
924 zeroAccSpatFrc[i+1].m_bottomVec[2]);
929 btScalar linDampMult = 1., angDampMult = 1.;
930 zeroAccSpatFrc[i + 1].
addVector(angDampMult *
m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
931 linDampMult *
m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
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()));
959 0,
m_links[i].m_inertiaLocal[1], 0,
960 0, 0,
m_links[i].m_inertiaLocal[2]));
969 for (
int i = num_links - 1; i >= 0; --i)
973 const int parent =
m_links[i].m_parent;
974 fromParent.
m_rotMat = rot_from_parent[i + 1];
977 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
981 hDof = spatInertia[i + 1] *
m_links[i].m_axes[dof];
983 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);
985 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
988 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
991 D_row[dof2] =
m_links[i].m_axes[dof].dot(hDof2);
996 switch (
m_links[i].m_jointType)
1003 invDi[0] = 1.0f / D[0];
1014 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1018 for (
int row = 0; row < 3; ++row)
1020 for (
int col = 0; col < 3; ++col)
1022 invDi[row * 3 + col] = invD3x3[row][col];
1034 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1036 spatForceVecTemps[dof].
setZero();
1038 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1042 spatForceVecTemps[dof] += hDof2 * invDi[dof2 *
m_links[i].m_dofCount + dof];
1046 dyadTemp = spatInertia[i + 1];
1049 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1058 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1060 invD_times_Y[dof] = 0.f;
1062 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1064 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1068 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1070 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1074 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1079 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1101 spatAcc[0] = -result;
1105 for (
int i = 0; i < num_links; ++i)
1113 const int parent =
m_links[i].m_parent;
1114 fromParent.
m_rotMat = rot_from_parent[i + 1];
1117 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1121 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1125 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1131 spatAcc[i + 1] += spatCoriolisAcc[i];
1133 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1134 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1137 if (
m_links[i].m_jointFeedback)
1141 btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1142 btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1144 if (jointFeedbackInJointFrame)
1149 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1152 if (jointFeedbackInWorldSpace)
1154 if (isConstraintPass)
1156 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1157 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1161 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1162 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1167 if (isConstraintPass)
1169 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1170 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1174 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1175 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1183 output[0] = omegadot_out[0];
1184 output[1] = omegadot_out[1];
1185 output[2] = omegadot_out[2];
1212 if (!isConstraintPass)
1247 for (
int i = 0; i < num_links; ++i)
1249 const int parent =
m_links[i].m_parent;
1253 fromParent.
m_rotMat = rot_from_parent[i + 1];
1255 fromWorld.
m_rotMat = rot_from_world[i + 1];
1258 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
1266 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1270 spatVel[i + 1] += spatJointVel;
1315 for (
int i = 0; i < 6; i++)
1328 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1337 btVector3 vtop = invI_upper_left * rhs_top;
1339 tmp = invIupper_right * rhs_bot;
1341 btVector3 vbot = invI_lower_left * rhs_top;
1342 tmp = invI_lower_right * rhs_bot;
1344 result[0] = vtop[0];
1345 result[1] = vtop[1];
1346 result[2] = vtop[2];
1347 result[3] = vbot[0];
1348 result[4] = vbot[1];
1349 result[5] = vbot[2];
1393 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1416 for (
int row = 0; row < rowsA; row++)
1418 for (
int col = 0; col < colsB; col++)
1420 pC[row * colsB + col] = 0.f;
1421 for (
int inner = 0; inner < rowsB; inner++)
1423 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1437 scratch_v.
resize(4 * num_links + 4);
1444 v_ptr += num_links * 2 + 2;
1453 v_ptr += num_links * 2 + 2;
1479 fromParent.
m_rotMat = rot_from_parent[0];
1482 for (
int i = 0; i < num_links; ++i)
1484 zeroAccSpatFrc[i + 1].
setZero();
1489 for (
int i = num_links - 1; i >= 0; --i)
1493 const int parent =
m_links[i].m_parent;
1494 fromParent.
m_rotMat = rot_from_parent[i + 1];
1497 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1499 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]);
1502 btVector3 in_top, in_bottom, out_top, out_bottom;
1505 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1507 invD_times_Y[dof] = 0.f;
1509 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1511 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1516 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1518 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1522 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1527 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1543 spatAcc[0] = -result;
1547 for (
int i = 0; i < num_links; ++i)
1551 const int parent =
m_links[i].m_parent;
1552 fromParent.
m_rotMat = rot_from_parent[i + 1];
1555 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1557 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1561 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1567 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1568 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1574 output[0] = omegadot_out[0];
1575 output[1] = omegadot_out[1];
1576 output[2] = omegadot_out[2];
1604 for (
int i = 0; i < 3; ++i)
1610 pBasePos[0] += dt * pBaseVel[0];
1611 pBasePos[1] += dt * pBaseVel[1];
1612 pBasePos[2] += dt * pBaseVel[2];
1643 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1648 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();
1689 for (
int i = 0; i < num_links; ++i)
1692 pJointPos = &
m_links[i].m_jointPos_interpolate[0];
1694 if (
m_links[i].m_collider &&
m_links[i].m_collider->isStaticOrKinematic())
1696 switch (
m_links[i].m_jointType)
1701 pJointPos[0] =
m_links[i].m_jointPos[0];
1706 for (
int j = 0; j < 4; ++j)
1708 pJointPos[j] =
m_links[i].m_jointPos[j];
1714 for (
int j = 0; j < 3; ++j)
1716 pJointPos[j] =
m_links[i].m_jointPos[j];
1728 switch (
m_links[i].m_jointType)
1734 pJointPos[0] =
m_links[i].m_jointPos[0];
1736 pJointPos[0] += dt * jointVel;
1743 for (
int j = 0; j < 4; ++j)
1745 pJointPos[j] =
m_links[i].m_jointPos[j];
1749 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1751 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1752 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1753 pJointPos[0] = jointOri.
x();
1754 pJointPos[1] = jointOri.
y();
1755 pJointPos[2] = jointOri.
z();
1756 pJointPos[3] = jointOri.
w();
1761 for (
int j = 0; j < 3; ++j)
1763 pJointPos[j] =
m_links[i].m_jointPos[j];
1769 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1770 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1779 m_links[i].updateInterpolationCacheMultiDof();
1795 pBasePos[0] += dt * pBaseVel[0];
1796 pBasePos[1] += dt * pBaseVel[1];
1797 pBasePos[2] += dt * pBaseVel[2];
1828 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1833 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1855 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1857 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1858 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1859 pBaseQuat[0] = baseQuat.
x();
1860 pBaseQuat[1] = baseQuat.
y();
1861 pBaseQuat[2] = baseQuat.
z();
1862 pBaseQuat[3] = baseQuat.
w();
1875 for (
int i = 0; i < num_links; ++i)
1877 if (!(
m_links[i].m_collider &&
m_links[i].m_collider->isStaticOrKinematic()))
1880 pJointPos= (pq ? pq : &
m_links[i].m_jointPos[0]);
1884 switch (
m_links[i].m_jointType)
1891 pJointPos[0] += dt * jointVel;
1898 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1900 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1901 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1902 pJointPos[0] = jointOri.
x();
1903 pJointPos[1] = jointOri.
y();
1904 pJointPos[2] = jointOri.
z();
1905 pJointPos[3] = jointOri.
w();
1914 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1915 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1925 m_links[i].updateCacheMultiDof(pq);
1928 pq +=
m_links[i].m_posVarCount;
1946 scratch_v.
resize(3 * num_links + 3);
1947 scratch_m.
resize(num_links + 1);
1951 v_ptr += num_links + 1;
1953 v_ptr += num_links + 1;
1955 v_ptr += num_links + 1;
1964 int numLinksChildToRoot=0;
1968 links[numLinksChildToRoot++]=l;
1975 const btVector3 &normal_lin_world = normal_lin;
1976 const btVector3 &normal_ang_world = normal_ang;
1982 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1983 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1984 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1985 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1987 jac[3] = normal_lin_world[0];
1988 jac[4] = normal_lin_world[1];
1989 jac[5] = normal_lin_world[2];
1992 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1993 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1994 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2003 if (num_links > 0 && link > -1)
2009 for (
int a = 0; a < numLinksChildToRoot; a++)
2011 int i = links[numLinksChildToRoot-1-a];
2013 const int parent =
m_links[i].m_parent;
2015 rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
2017 n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
2018 n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
2019 p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] -
m_links[i].m_cachedRVector;
2022 switch (
m_links[i].m_jointType)
2026 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));
2027 results[
m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
2032 results[
m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(0));
2037 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));
2038 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));
2039 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));
2041 results[
m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
2042 results[
m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(1));
2043 results[
m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(2));
2049 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]));
2050 results[
m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(1));
2051 results[
m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(2));
2065 for (
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
2067 jac[6 +
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
2071 link =
m_links[link].m_parent;
2107 if (motion < SLEEP_EPSILON)
2135 for (
int i = 0; i < num_links; ++i)
2142 world_to_local.
resize(nLinks + 1);
2143 local_origin.
resize(nLinks + 1);
2157 int index = link + 1;
2160 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2181 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2206 int index = link + 1;
2210 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2241 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2265 int index = link + 1;
2269 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2300 if (mbd->m_baseName)
2306 if (mbd->m_numLinks)
2309 int numElem = mbd->m_numLinks;
2312 for (
int i = 0; i < numElem; i++, memPtr++)
2347 for (
int posvar = 0; posvar < numPosVar; posvar++)
2355 if (memPtr->m_linkName)
2363 if (memPtr->m_jointName)
2375 #ifdef BT_USE_DOUBLE_PRECISION
2376 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));
2387 btVector3 linearVelocity, angularVelocity;
2405 m_links[i].m_collider->setDynamicType(type);
2419 return m_links[i].m_collider->isStaticOrKinematic();
2433 return m_links[i].m_collider->isKinematic();
2441 while (link != -1) {
2444 link =
m_links[link].m_parent;
2452 while (link != -1) {
2455 link =
m_links[link].m_parent;
#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())
bool isStaticOrKinematicObject() const
void setCollisionFlags(int flags)
bool isStaticObject() const
void setWorldTransform(const btTransform &worldTrans)
bool isKinematicObject() const
int getCollisionFlags() const
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)
btTransform getInterpolateBaseWorldTransform() 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)
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)
bool isLinkAndAllAncestorsKinematic(const int i) const
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
void setBaseDynamicType(int dynamicType)
btScalar * getJointPosMultiDof(int i)
btScalar m_angularDamping
void predictPositionsMultiDof(btScalar dt)
void setBaseVel(const btVector3 &vel)
btMatrix3x3 m_cachedInertiaLowerRight
void setBaseOmega(const btVector3 &omega)
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
bool isBaseKinematic() 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
void setInterpolateBaseWorldTransform(const btTransform &tr)
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
bool hasFixedBase() const
bool isLinkAndAllAncestorsStaticOrKinematic(const 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)
void setLinkDynamicType(const int i, int type)
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
btTransform getBaseWorldTransform() const
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
bool isBaseStaticOrKinematic() 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
bool m_kinematic_calculate_velocity
btMatrix3x3 m_cachedInertiaTopRight
btScalar getBaseMass() const
btAlignedObjectArray< btScalar > m_deltaV
void updateLinksDofOffsets()
const btVector3 & getInterpolateBasePos() const
virtual int calculateSerializeBufferSize() const
bool isLinkKinematic(const int i) 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
btAlignedObjectArray< btScalar > m_splitV
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
bool isLinkStaticOrKinematic(const 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 saveKinematicState(btScalar timeStep)
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)