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;