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];
692 row1[0], row1[1], row1[2],
693 row2[0], row2[1], row2[2]);
697 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
704 bool isConstraintPass,
705 bool jointFeedbackInWorldSpace,
706 bool jointFeedbackInJointFrame)
739 scratch_v.
resize(8 * num_links + 6);
740 scratch_m.
resize(4 * num_links + 4);
748 v_ptr += num_links * 2 + 2;
752 v_ptr += num_links * 2 + 2;
756 v_ptr += num_links * 2;
769 v_ptr += num_links * 2 + 2;
799 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
810 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
813 const btScalar linDampMult = 1., angDampMult = 1.;
814 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
815 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
822 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
836 rot_from_world[0] = rot_from_parent[0];
839 for (
int i = 0; i < num_links; ++i)
841 const int parent =
m_links[i].m_parent;
843 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
845 fromParent.
m_rotMat = rot_from_parent[i + 1];
847 fromWorld.
m_rotMat = rot_from_world[i + 1];
848 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
856 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
860 spatVel[i + 1] += spatJointVel;
873 spatVel[i + 1].
cross(spatJointVel, spatCoriolisAcc[i]);
878 btVector3 linkAppliedForce = isConstraintPass ?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
879 btVector3 linkAppliedTorque = isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
881 zeroAccSpatFrc[i + 1].
setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
886 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
888 zeroAccSpatFrc[i+1].m_topVec[0],
889 zeroAccSpatFrc[i+1].m_topVec[1],
890 zeroAccSpatFrc[i+1].m_topVec[2],
892 zeroAccSpatFrc[i+1].m_bottomVec[0],
893 zeroAccSpatFrc[i+1].m_bottomVec[1],
894 zeroAccSpatFrc[i+1].m_bottomVec[2]);
899 btScalar linDampMult = 1., angDampMult = 1.;
900 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()),
901 linDampMult *
m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
912 0,
m_links[i].m_inertiaLocal[1], 0,
913 0, 0,
m_links[i].m_inertiaLocal[2]));
917 zeroAccSpatFrc[i + 1].
addAngular(spatVel[i + 1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
919 zeroAccSpatFrc[i + 1].
addLinear(
m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
938 for (
int i = num_links - 1; i >= 0; --i)
940 const int parent =
m_links[i].m_parent;
941 fromParent.
m_rotMat = rot_from_parent[i + 1];
944 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
948 hDof = spatInertia[i + 1] *
m_links[i].m_axes[dof];
950 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);
952 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
955 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
958 D_row[dof2] =
m_links[i].m_axes[dof].dot(hDof2);
963 switch (
m_links[i].m_jointType)
970 invDi[0] = 1.0f / D[0];
981 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
985 for (
int row = 0; row < 3; ++row)
987 for (
int col = 0; col < 3; ++col)
989 invDi[row * 3 + col] = invD3x3[row][col];
1001 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1003 spatForceVecTemps[dof].
setZero();
1005 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1009 spatForceVecTemps[dof] += hDof2 * invDi[dof2 *
m_links[i].m_dofCount + dof];
1013 dyadTemp = spatInertia[i + 1];
1016 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1025 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1027 invD_times_Y[dof] = 0.f;
1029 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1031 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1035 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1037 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1041 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1046 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1068 spatAcc[0] = -result;
1072 for (
int i = 0; i < num_links; ++i)
1080 const int parent =
m_links[i].m_parent;
1081 fromParent.
m_rotMat = rot_from_parent[i + 1];
1084 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1086 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1090 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1097 spatAcc[i + 1] += spatCoriolisAcc[i];
1099 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1100 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1102 if (
m_links[i].m_jointFeedback)
1106 btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1107 btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1109 if (jointFeedbackInJointFrame)
1114 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1117 if (jointFeedbackInWorldSpace)
1119 if (isConstraintPass)
1121 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1122 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1126 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1127 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1132 if (isConstraintPass)
1134 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1135 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1139 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1140 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1148 output[0] = omegadot_out[0];
1149 output[1] = omegadot_out[1];
1150 output[2] = omegadot_out[2];
1177 if (!isConstraintPass)
1212 for (
int i = 0; i < num_links; ++i)
1214 const int parent =
m_links[i].m_parent;
1218 fromParent.
m_rotMat = rot_from_parent[i + 1];
1220 fromWorld.
m_rotMat = rot_from_world[i + 1];
1223 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
1231 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1235 spatVel[i + 1] += spatJointVel;
1280 for (
int i = 0; i < 6; i++)
1293 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1302 btVector3 vtop = invI_upper_left * rhs_top;
1304 tmp = invIupper_right * rhs_bot;
1306 btVector3 vbot = invI_lower_left * rhs_top;
1307 tmp = invI_lower_right * rhs_bot;
1309 result[0] = vtop[0];
1310 result[1] = vtop[1];
1311 result[2] = vtop[2];
1312 result[3] = vbot[0];
1313 result[4] = vbot[1];
1314 result[5] = vbot[2];
1358 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1381 for (
int row = 0; row < rowsA; row++)
1383 for (
int col = 0; col < colsB; col++)
1385 pC[row * colsB + col] = 0.f;
1386 for (
int inner = 0; inner < rowsB; inner++)
1388 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1402 scratch_v.
resize(4 * num_links + 4);
1409 v_ptr += num_links * 2 + 2;
1418 v_ptr += num_links * 2 + 2;
1444 fromParent.
m_rotMat = rot_from_parent[0];
1447 for (
int i = 0; i < num_links; ++i)
1449 zeroAccSpatFrc[i + 1].
setZero();
1454 for (
int i = num_links - 1; i >= 0; --i)
1456 const int parent =
m_links[i].m_parent;
1457 fromParent.
m_rotMat = rot_from_parent[i + 1];
1460 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1462 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]);
1465 btVector3 in_top, in_bottom, out_top, out_bottom;
1468 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1470 invD_times_Y[dof] = 0.f;
1472 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1474 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1479 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1481 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1485 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1490 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1506 spatAcc[0] = -result;
1510 for (
int i = 0; i < num_links; ++i)
1512 const int parent =
m_links[i].m_parent;
1513 fromParent.
m_rotMat = rot_from_parent[i + 1];
1516 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1518 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1522 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1528 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1529 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1535 output[0] = omegadot_out[0];
1536 output[1] = omegadot_out[1];
1537 output[2] = omegadot_out[2];
1563 for (
int i = 0; i < 3; ++i)
1569 pBasePos[0] += dt * pBaseVel[0];
1570 pBasePos[1] += dt * pBaseVel[1];
1571 pBasePos[2] += dt * pBaseVel[2];
1601 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1606 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1625 for (
int i = 0; i < 4; ++i)
1634 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1636 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1637 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1638 pBaseQuat[0] = baseQuat.
x();
1639 pBaseQuat[1] = baseQuat.
y();
1640 pBaseQuat[2] = baseQuat.
z();
1641 pBaseQuat[3] = baseQuat.
w();
1644 for (
int i = 0; i < num_links; ++i)
1647 pJointPos = &
m_links[i].m_jointPos_interpolate[0];
1651 switch (
m_links[i].m_jointType)
1657 pJointPos[0] =
m_links[i].m_jointPos[0];
1659 pJointPos[0] += dt * jointVel;
1666 for (
int j = 0; j < 4; ++j)
1668 pJointPos[j] =
m_links[i].m_jointPos[j];
1672 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1674 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1675 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1676 pJointPos[0] = jointOri.
x();
1677 pJointPos[1] = jointOri.
y();
1678 pJointPos[2] = jointOri.
z();
1679 pJointPos[3] = jointOri.
w();
1684 for (
int j = 0; j < 3; ++j)
1686 pJointPos[j] =
m_links[i].m_jointPos[j];
1692 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1693 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1701 m_links[i].updateInterpolationCacheMultiDof();
1715 pBasePos[0] += dt * pBaseVel[0];
1716 pBasePos[1] += dt * pBaseVel[1];
1717 pBasePos[2] += dt * pBaseVel[2];
1747 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1752 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1772 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1774 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1775 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1776 pBaseQuat[0] = baseQuat.
x();
1777 pBaseQuat[1] = baseQuat.
y();
1778 pBaseQuat[2] = baseQuat.
z();
1779 pBaseQuat[3] = baseQuat.
w();
1791 for (
int i = 0; i < num_links; ++i)
1794 pJointPos= (pq ? pq : &
m_links[i].m_jointPos[0]);
1798 switch (
m_links[i].m_jointType)
1805 pJointPos[0] += dt * jointVel;
1812 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1814 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1815 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1816 pJointPos[0] = jointOri.
x();
1817 pJointPos[1] = jointOri.
y();
1818 pJointPos[2] = jointOri.
z();
1819 pJointPos[3] = jointOri.
w();
1828 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1829 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1838 m_links[i].updateCacheMultiDof(pq);
1841 pq +=
m_links[i].m_posVarCount;
1859 scratch_v.
resize(3 * num_links + 3);
1860 scratch_m.
resize(num_links + 1);
1864 v_ptr += num_links + 1;
1866 v_ptr += num_links + 1;
1868 v_ptr += num_links + 1;
1877 int numLinksChildToRoot=0;
1881 links[numLinksChildToRoot++]=l;
1888 const btVector3 &normal_lin_world = normal_lin;
1889 const btVector3 &normal_ang_world = normal_ang;
1895 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1896 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1897 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1898 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1900 jac[3] = normal_lin_world[0];
1901 jac[4] = normal_lin_world[1];
1902 jac[5] = normal_lin_world[2];
1905 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1906 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1907 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1916 if (num_links > 0 && link > -1)
1922 for (
int a = 0; a < numLinksChildToRoot; a++)
1924 int i = links[numLinksChildToRoot-1-a];
1926 const int parent =
m_links[i].m_parent;
1928 rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
1930 n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
1931 n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
1932 p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] -
m_links[i].m_cachedRVector;
1935 switch (
m_links[i].m_jointType)
1939 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));
1940 results[
m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
1945 results[
m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(0));
1950 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));
1951 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));
1952 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));
1954 results[
m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
1955 results[
m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(1));
1956 results[
m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(2));
1962 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]));
1963 results[
m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(1));
1964 results[
m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(2));
1978 for (
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
1980 jac[6 +
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
1984 link =
m_links[link].m_parent;
2020 if (motion < SLEEP_EPSILON)
2048 for (
int i = 0; i < num_links; ++i)
2055 world_to_local.
resize(nLinks + 1);
2056 local_origin.
resize(nLinks + 1);
2070 int index = link + 1;
2073 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2094 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2119 int index = link + 1;
2123 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2147 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2171 int index = link + 1;
2175 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2206 if (mbd->m_baseName)
2212 if (mbd->m_numLinks)
2215 int numElem = mbd->m_numLinks;
2218 for (
int i = 0; i < numElem; i++, memPtr++)
2253 for (
int posvar = 0; posvar < numPosVar; posvar++)
2261 if (memPtr->m_linkName)
2269 if (memPtr->m_jointName)
2281 #ifdef BT_USE_DOUBLE_PRECISION
2282 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));