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));