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());
361 return m_links[link_num].m_parent;
371 return m_links[i].m_inertiaLocal;
376 return m_links[i].m_jointPos[0];
386 return &
m_links[i].m_jointPos[0];
396 return &
m_links[i].m_jointPos[0];
407 m_links[i].updateCacheMultiDof();
413 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
416 m_links[i].updateCacheMultiDof();
421 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
424 m_links[i].updateCacheMultiDof();
436 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
442 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
448 return m_links[i].m_cachedRVector;
453 return m_links[i].m_cachedRotParentToThis;
458 return m_links[i].m_cachedRVector_interpolate;
463 return m_links[i].m_cachedRotParentToThis_interpolate;
556 result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
568 for (
int i = 0; i < num_links; ++i)
575 omega[parent + 1], vel[parent + 1],
576 omega[i + 1], vel[i + 1]);
580 for (
int dof = 0; dof < link.
m_dofCount; ++dof)
582 omega[i + 1] += jointVel[dof] * link.
getAxisTop(dof);
596 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
597 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
607 m_links[i].m_appliedForce.setValue(0, 0, 0);
608 m_links[i].m_appliedTorque.setValue(0, 0, 0);
622 m_links[i].m_appliedForce += f;
627 m_links[i].m_appliedTorque += t;
632 m_links[i].m_appliedConstraintForce += f;
637 m_links[i].m_appliedConstraintTorque += t;
642 m_links[i].m_jointTorque[0] += Q;
647 m_links[i].m_jointTorque[dof] += Q;
652 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
653 m_links[i].m_jointTorque[dof] = Q[dof];
658 return m_links[i].m_appliedForce;
663 return m_links[i].m_appliedTorque;
668 return m_links[i].m_jointTorque[0];
673 return &
m_links[i].m_jointTorque[0];
716 row1[0], row1[1], row1[2],
717 row2[0], row2[1], row2[2]);
721 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
728 bool isConstraintPass,
729 bool jointFeedbackInWorldSpace,
730 bool jointFeedbackInJointFrame)
763 scratch_v.
resize(8 * num_links + 6);
764 scratch_m.
resize(4 * num_links + 4);
772 v_ptr += num_links * 2 + 2;
776 v_ptr += num_links * 2 + 2;
780 v_ptr += num_links * 2;
793 v_ptr += num_links * 2 + 2;
823 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
834 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
837 const btScalar linDampMult = 1., angDampMult = 1.;
838 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
839 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
846 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
860 rot_from_world[0] = rot_from_parent[0];
863 for (
int i = 0; i < num_links; ++i)
865 const int parent =
m_links[i].m_parent;
867 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
869 fromParent.
m_rotMat = rot_from_parent[i + 1];
871 fromWorld.
m_rotMat = rot_from_world[i + 1];
872 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
880 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
884 spatVel[i + 1] += spatJointVel;
897 spatVel[i + 1].
cross(spatJointVel, spatCoriolisAcc[i]);
907 btVector3 linkAppliedForce = isConstraintPass ?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
908 btVector3 linkAppliedTorque = isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
910 zeroAccSpatFrc[i + 1].
setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
915 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
917 zeroAccSpatFrc[i+1].m_topVec[0],
918 zeroAccSpatFrc[i+1].m_topVec[1],
919 zeroAccSpatFrc[i+1].m_topVec[2],
921 zeroAccSpatFrc[i+1].m_bottomVec[0],
922 zeroAccSpatFrc[i+1].m_bottomVec[1],
923 zeroAccSpatFrc[i+1].m_bottomVec[2]);
928 btScalar linDampMult = 1., angDampMult = 1.;
929 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()),
930 linDampMult *
m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
933 zeroAccSpatFrc[i + 1].
addAngular(spatVel[i + 1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
935 zeroAccSpatFrc[i + 1].
addLinear(
m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
958 0,
m_links[i].m_inertiaLocal[1], 0,
959 0, 0,
m_links[i].m_inertiaLocal[2]));
968 for (
int i = num_links - 1; i >= 0; --i)
972 const int parent =
m_links[i].m_parent;
973 fromParent.
m_rotMat = rot_from_parent[i + 1];
976 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
980 hDof = spatInertia[i + 1] *
m_links[i].m_axes[dof];
982 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);
984 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
987 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
990 D_row[dof2] =
m_links[i].m_axes[dof].dot(hDof2);
995 switch (
m_links[i].m_jointType)
1002 invDi[0] = 1.0f / D[0];
1013 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1017 for (
int row = 0; row < 3; ++row)
1019 for (
int col = 0; col < 3; ++col)
1021 invDi[row * 3 + col] = invD3x3[row][col];
1033 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1035 spatForceVecTemps[dof].
setZero();
1037 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1041 spatForceVecTemps[dof] += hDof2 * invDi[dof2 *
m_links[i].m_dofCount + dof];
1045 dyadTemp = spatInertia[i + 1];
1048 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1057 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1059 invD_times_Y[dof] = 0.f;
1061 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1063 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1067 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1069 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1073 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1078 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1100 spatAcc[0] = -result;
1104 for (
int i = 0; i < num_links; ++i)
1112 const int parent =
m_links[i].m_parent;
1113 fromParent.
m_rotMat = rot_from_parent[i + 1];
1116 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1120 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1124 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1130 spatAcc[i + 1] += spatCoriolisAcc[i];
1132 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1133 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1136 if (
m_links[i].m_jointFeedback)
1140 btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1141 btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1143 if (jointFeedbackInJointFrame)
1148 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1151 if (jointFeedbackInWorldSpace)
1153 if (isConstraintPass)
1155 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1156 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1160 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1161 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1166 if (isConstraintPass)
1168 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1169 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1173 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1174 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1182 output[0] = omegadot_out[0];
1183 output[1] = omegadot_out[1];
1184 output[2] = omegadot_out[2];
1211 if (!isConstraintPass)
1246 for (
int i = 0; i < num_links; ++i)
1248 const int parent =
m_links[i].m_parent;
1252 fromParent.
m_rotMat = rot_from_parent[i + 1];
1254 fromWorld.
m_rotMat = rot_from_world[i + 1];
1257 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
1265 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1269 spatVel[i + 1] += spatJointVel;
1314 for (
int i = 0; i < 6; i++)
1327 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1336 btVector3 vtop = invI_upper_left * rhs_top;
1338 tmp = invIupper_right * rhs_bot;
1340 btVector3 vbot = invI_lower_left * rhs_top;
1341 tmp = invI_lower_right * rhs_bot;
1343 result[0] = vtop[0];
1344 result[1] = vtop[1];
1345 result[2] = vtop[2];
1346 result[3] = vbot[0];
1347 result[4] = vbot[1];
1348 result[5] = vbot[2];
1392 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1415 for (
int row = 0; row < rowsA; row++)
1417 for (
int col = 0; col < colsB; col++)
1419 pC[row * colsB + col] = 0.f;
1420 for (
int inner = 0; inner < rowsB; inner++)
1422 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1436 scratch_v.
resize(4 * num_links + 4);
1443 v_ptr += num_links * 2 + 2;
1452 v_ptr += num_links * 2 + 2;
1478 fromParent.
m_rotMat = rot_from_parent[0];
1481 for (
int i = 0; i < num_links; ++i)
1483 zeroAccSpatFrc[i + 1].
setZero();
1488 for (
int i = num_links - 1; i >= 0; --i)
1492 const int parent =
m_links[i].m_parent;
1493 fromParent.
m_rotMat = rot_from_parent[i + 1];
1496 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1498 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]);
1501 btVector3 in_top, in_bottom, out_top, out_bottom;
1504 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1506 invD_times_Y[dof] = 0.f;
1508 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1510 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1515 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1517 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1521 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1526 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1542 spatAcc[0] = -result;
1546 for (
int i = 0; i < num_links; ++i)
1550 const int parent =
m_links[i].m_parent;
1551 fromParent.
m_rotMat = rot_from_parent[i + 1];
1554 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1556 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1560 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1566 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1567 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1573 output[0] = omegadot_out[0];
1574 output[1] = omegadot_out[1];
1575 output[2] = omegadot_out[2];
1603 for (
int i = 0; i < 3; ++i)
1609 pBasePos[0] += dt * pBaseVel[0];
1610 pBasePos[1] += dt * pBaseVel[1];
1611 pBasePos[2] += dt * pBaseVel[2];
1642 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1647 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1668 for (
int i = 0; i < 4; ++i)
1677 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1679 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1680 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1681 pBaseQuat[0] = baseQuat.
x();
1682 pBaseQuat[1] = baseQuat.
y();
1683 pBaseQuat[2] = baseQuat.
z();
1684 pBaseQuat[3] = baseQuat.
w();
1688 for (
int i = 0; i < num_links; ++i)
1691 pJointPos = &
m_links[i].m_jointPos_interpolate[0];
1693 if (
m_links[i].m_collider &&
m_links[i].m_collider->isStaticOrKinematic())
1695 switch (
m_links[i].m_jointType)
1700 pJointPos[0] =
m_links[i].m_jointPos[0];
1705 for (
int j = 0; j < 4; ++j)
1707 pJointPos[j] =
m_links[i].m_jointPos[j];
1713 for (
int j = 0; j < 3; ++j)
1715 pJointPos[j] =
m_links[i].m_jointPos[j];
1727 switch (
m_links[i].m_jointType)
1733 pJointPos[0] =
m_links[i].m_jointPos[0];
1735 pJointPos[0] += dt * jointVel;
1742 for (
int j = 0; j < 4; ++j)
1744 pJointPos[j] =
m_links[i].m_jointPos[j];
1748 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1750 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1751 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1752 pJointPos[0] = jointOri.
x();
1753 pJointPos[1] = jointOri.
y();
1754 pJointPos[2] = jointOri.
z();
1755 pJointPos[3] = jointOri.
w();
1760 for (
int j = 0; j < 3; ++j)
1762 pJointPos[j] =
m_links[i].m_jointPos[j];
1768 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1769 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1778 m_links[i].updateInterpolationCacheMultiDof();
1794 pBasePos[0] += dt * pBaseVel[0];
1795 pBasePos[1] += dt * pBaseVel[1];
1796 pBasePos[2] += dt * pBaseVel[2];
1827 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1832 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1854 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1856 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1857 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1858 pBaseQuat[0] = baseQuat.
x();
1859 pBaseQuat[1] = baseQuat.
y();
1860 pBaseQuat[2] = baseQuat.
z();
1861 pBaseQuat[3] = baseQuat.
w();
1874 for (
int i = 0; i < num_links; ++i)
1876 if (!(
m_links[i].m_collider &&
m_links[i].m_collider->isStaticOrKinematic()))
1879 pJointPos= (pq ? pq : &
m_links[i].m_jointPos[0]);
1883 switch (
m_links[i].m_jointType)
1890 pJointPos[0] += dt * jointVel;
1897 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1899 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1900 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1901 pJointPos[0] = jointOri.
x();
1902 pJointPos[1] = jointOri.
y();
1903 pJointPos[2] = jointOri.
z();
1904 pJointPos[3] = jointOri.
w();
1913 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1914 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1924 m_links[i].updateCacheMultiDof(pq);
1927 pq +=
m_links[i].m_posVarCount;
1945 scratch_v.
resize(3 * num_links + 3);
1946 scratch_m.
resize(num_links + 1);
1950 v_ptr += num_links + 1;
1952 v_ptr += num_links + 1;
1954 v_ptr += num_links + 1;
1963 int numLinksChildToRoot=0;
1967 links[numLinksChildToRoot++]=l;
1974 const btVector3 &normal_lin_world = normal_lin;
1975 const btVector3 &normal_ang_world = normal_ang;
1981 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1982 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1983 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1984 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1986 jac[3] = normal_lin_world[0];
1987 jac[4] = normal_lin_world[1];
1988 jac[5] = normal_lin_world[2];
1991 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1992 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1993 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2002 if (num_links > 0 && link > -1)
2008 for (
int a = 0; a < numLinksChildToRoot; a++)
2010 int i = links[numLinksChildToRoot-1-a];
2012 const int parent =
m_links[i].m_parent;
2014 rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
2016 n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
2017 n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
2018 p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] -
m_links[i].m_cachedRVector;
2021 switch (
m_links[i].m_jointType)
2025 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));
2026 results[
m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
2031 results[
m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(0));
2036 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));
2037 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));
2038 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));
2040 results[
m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
2041 results[
m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(1));
2042 results[
m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(2));
2048 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]));
2049 results[
m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(1));
2050 results[
m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(2));
2064 for (
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
2066 jac[6 +
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
2070 link =
m_links[link].m_parent;
2106 if (motion < SLEEP_EPSILON)
2134 for (
int i = 0; i < num_links; ++i)
2141 world_to_local.
resize(nLinks + 1);
2142 local_origin.
resize(nLinks + 1);
2156 int index = link + 1;
2159 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2180 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2205 int index = link + 1;
2209 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2240 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2264 int index = link + 1;
2268 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2299 if (mbd->m_baseName)
2305 if (mbd->m_numLinks)
2308 int numElem = mbd->m_numLinks;
2311 for (
int i = 0; i < numElem; i++, memPtr++)
2346 for (
int posvar = 0; posvar < numPosVar; posvar++)
2354 if (memPtr->m_linkName)
2362 if (memPtr->m_jointName)
2374 #ifdef BT_USE_DOUBLE_PRECISION
2375 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));
2386 btVector3 linearVelocity, angularVelocity;
2404 m_links[i].m_collider->setDynamicType(type);
2418 return m_links[i].m_collider->isStaticOrKinematic();
2432 return m_links[i].m_collider->isKinematic();
2440 while (link != -1) {
2443 link =
m_links[link].m_parent;
2451 while (link != -1) {
2454 link =
m_links[link].m_parent;