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),
104 m_baseInertia(inertia),
106 m_fixedBase(fixedBase),
108 m_canSleep(canSleep),
110 m_userObjectPointer(0),
114 m_linearDamping(0.04f),
115 m_angularDamping(0.04f),
117 m_maxAppliedImpulse(1000.f),
118 m_maxCoordinateVelocity(100.f),
119 m_hasSelfCollision(true),
124 m_useGlobalVelocities(false),
125 m_internalNeedsJointFeedback(false)
152 const btVector3 &parentComToThisPivotOffset,
153 const btVector3 &thisPivotToThisComOffset,
bool )
156 m_links[i].m_inertiaLocal = inertia;
158 m_links[i].setAxisTop(0, 0., 0., 0.);
160 m_links[i].m_zeroRotParentToThis = rotParentToThis;
161 m_links[i].m_dVector = thisPivotToThisComOffset;
162 m_links[i].m_eVector = parentComToThisPivotOffset;
170 m_links[i].updateCacheMultiDof();
181 const btVector3 &parentComToThisPivotOffset,
182 const btVector3 &thisPivotToThisComOffset,
183 bool disableParentCollision)
189 m_links[i].m_inertiaLocal = inertia;
191 m_links[i].m_zeroRotParentToThis = rotParentToThis;
192 m_links[i].setAxisTop(0, 0., 0., 0.);
193 m_links[i].setAxisBottom(0, jointAxis);
194 m_links[i].m_eVector = parentComToThisPivotOffset;
195 m_links[i].m_dVector = thisPivotToThisComOffset;
196 m_links[i].m_cachedRotParentToThis = rotParentToThis;
201 m_links[i].m_jointPos[0] = 0.f;
202 m_links[i].m_jointTorque[0] = 0.f;
204 if (disableParentCollision)
208 m_links[i].updateCacheMultiDof();
219 const btVector3 &parentComToThisPivotOffset,
220 const btVector3 &thisPivotToThisComOffset,
221 bool disableParentCollision)
227 m_links[i].m_inertiaLocal = inertia;
229 m_links[i].m_zeroRotParentToThis = rotParentToThis;
230 m_links[i].setAxisTop(0, jointAxis);
231 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
232 m_links[i].m_dVector = thisPivotToThisComOffset;
233 m_links[i].m_eVector = parentComToThisPivotOffset;
238 m_links[i].m_jointPos[0] = 0.f;
239 m_links[i].m_jointTorque[0] = 0.f;
241 if (disableParentCollision)
244 m_links[i].updateCacheMultiDof();
254 const btVector3 &parentComToThisPivotOffset,
255 const btVector3 &thisPivotToThisComOffset,
256 bool disableParentCollision)
262 m_links[i].m_inertiaLocal = inertia;
264 m_links[i].m_zeroRotParentToThis = rotParentToThis;
265 m_links[i].m_dVector = thisPivotToThisComOffset;
266 m_links[i].m_eVector = parentComToThisPivotOffset;
271 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
272 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
273 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
274 m_links[i].setAxisBottom(0,
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
275 m_links[i].setAxisBottom(1,
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
276 m_links[i].setAxisBottom(2,
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
278 m_links[i].m_jointPos[3] = 1.f;
281 if (disableParentCollision)
284 m_links[i].updateCacheMultiDof();
295 const btVector3 &parentComToThisComOffset,
296 bool disableParentCollision)
302 m_links[i].m_inertiaLocal = inertia;
304 m_links[i].m_zeroRotParentToThis = rotParentToThis;
305 m_links[i].m_dVector.setZero();
306 m_links[i].m_eVector = parentComToThisComOffset;
309 btVector3 vecNonParallelToRotAxis(1, 0, 0);
310 if (rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
311 vecNonParallelToRotAxis.
setValue(0, 1, 0);
318 m_links[i].setAxisTop(0, n[0], n[1], n[2]);
319 m_links[i].setAxisTop(1, 0, 0, 0);
320 m_links[i].setAxisTop(2, 0, 0, 0);
321 m_links[i].setAxisBottom(0, 0, 0, 0);
323 m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
325 m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
329 if (disableParentCollision)
332 m_links[i].updateCacheMultiDof();
336 m_links[i].setAxisBottom(1,
m_links[i].getAxisBottom(1).normalized());
337 m_links[i].setAxisBottom(2,
m_links[i].getAxisBottom(2).normalized());
365 return m_links[i].m_inertiaLocal;
370 return m_links[i].m_jointPos[0];
380 return &
m_links[i].m_jointPos[0];
390 return &
m_links[i].m_jointPos[0];
401 m_links[i].updateCacheMultiDof();
407 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
410 m_links[i].updateCacheMultiDof();
415 for (
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
418 m_links[i].updateCacheMultiDof();
430 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
436 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
442 return m_links[i].m_cachedRVector;
447 return m_links[i].m_cachedRotParentToThis;
540 result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
552 for (
int i = 0; i < num_links; ++i)
559 omega[parent + 1], vel[parent + 1],
560 omega[i + 1], vel[i + 1]);
564 for (
int dof = 0; dof < link.
m_dofCount; ++dof)
566 omega[i + 1] += jointVel[dof] * link.
getAxisTop(dof);
577 omega.
resize(num_links + 1);
579 vel.
resize(num_links + 1);
586 for (
int i = 0; i < num_links; ++i)
588 result +=
m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
589 result += omega[i + 1].dot(
m_links[i].m_inertiaLocal * omega[i + 1]);
592 return 0.5f * result;
600 omega.
resize(num_links + 1);
602 vel.
resize(num_links + 1);
604 rot_from_world.
resize(num_links + 1);
610 for (
int i = 0; i < num_links; ++i)
612 rot_from_world[i + 1] =
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent + 1];
626 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
627 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
637 m_links[i].m_appliedForce.setValue(0, 0, 0);
638 m_links[i].m_appliedTorque.setValue(0, 0, 0);
652 m_links[i].m_appliedForce += f;
657 m_links[i].m_appliedTorque += t;
662 m_links[i].m_appliedConstraintForce += f;
667 m_links[i].m_appliedConstraintTorque += t;
672 m_links[i].m_jointTorque[0] += Q;
677 m_links[i].m_jointTorque[dof] += Q;
682 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
683 m_links[i].m_jointTorque[dof] = Q[dof];
688 return m_links[i].m_appliedForce;
693 return m_links[i].m_appliedTorque;
698 return m_links[i].m_jointTorque[0];
703 return &
m_links[i].m_jointTorque[0];
722 row1[0], row1[1], row1[2],
723 row2[0], row2[1], row2[2]);
727 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
734 bool isConstraintPass,
735 bool jointFeedbackInWorldSpace,
736 bool jointFeedbackInJointFrame)
769 scratch_v.
resize(8 * num_links + 6);
770 scratch_m.
resize(4 * num_links + 4);
778 v_ptr += num_links * 2 + 2;
782 v_ptr += num_links * 2 + 2;
786 v_ptr += num_links * 2;
799 v_ptr += num_links * 2 + 2;
829 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
840 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
843 const btScalar linDampMult = 1., angDampMult = 1.;
844 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
845 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
852 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
866 rot_from_world[0] = rot_from_parent[0];
869 for (
int i = 0; i < num_links; ++i)
871 const int parent =
m_links[i].m_parent;
873 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
875 fromParent.
m_rotMat = rot_from_parent[i + 1];
877 fromWorld.
m_rotMat = rot_from_world[i + 1];
878 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
886 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
890 spatVel[i + 1] += spatJointVel;
903 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()));
942 0,
m_links[i].m_inertiaLocal[1], 0,
943 0, 0,
m_links[i].m_inertiaLocal[2]));
947 zeroAccSpatFrc[i + 1].
addAngular(spatVel[i + 1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
949 zeroAccSpatFrc[i + 1].
addLinear(
m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
968 for (
int i = num_links - 1; i >= 0; --i)
970 const int parent =
m_links[i].m_parent;
971 fromParent.
m_rotMat = rot_from_parent[i + 1];
974 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
978 hDof = spatInertia[i + 1] *
m_links[i].m_axes[dof];
980 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);
982 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
985 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
988 D_row[dof2] =
m_links[i].m_axes[dof].dot(hDof2);
993 switch (
m_links[i].m_jointType)
1000 invDi[0] = 1.0f / D[0];
1011 const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1015 for (
int row = 0; row < 3; ++row)
1017 for (
int col = 0; col < 3; ++col)
1019 invDi[row * 3 + col] = invD3x3[row][col];
1031 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1033 spatForceVecTemps[dof].
setZero();
1035 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1039 spatForceVecTemps[dof] += hDof2 * invDi[dof2 *
m_links[i].m_dofCount + dof];
1043 dyadTemp = spatInertia[i + 1];
1046 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1055 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1057 invD_times_Y[dof] = 0.f;
1059 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1061 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1065 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1067 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1071 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1076 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1098 spatAcc[0] = -result;
1102 for (
int i = 0; i < num_links; ++i)
1110 const int parent =
m_links[i].m_parent;
1111 fromParent.
m_rotMat = rot_from_parent[i + 1];
1114 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1116 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1120 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1127 spatAcc[i + 1] += spatCoriolisAcc[i];
1129 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1130 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1132 if (
m_links[i].m_jointFeedback)
1136 btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1137 btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1139 if (jointFeedbackInJointFrame)
1144 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1147 if (jointFeedbackInWorldSpace)
1149 if (isConstraintPass)
1151 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1152 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
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;
1162 if (isConstraintPass)
1164 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1165 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1169 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1170 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1178 output[0] = omegadot_out[0];
1179 output[1] = omegadot_out[1];
1180 output[2] = omegadot_out[2];
1207 if (!isConstraintPass)
1242 for (
int i = 0; i < num_links; ++i)
1244 const int parent =
m_links[i].m_parent;
1248 fromParent.
m_rotMat = rot_from_parent[i + 1];
1250 fromWorld.
m_rotMat = rot_from_world[i + 1];
1253 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
1261 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1265 spatVel[i + 1] += spatJointVel;
1310 for (
int i = 0; i < 6; i++)
1323 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1332 btVector3 vtop = invI_upper_left * rhs_top;
1334 tmp = invIupper_right * rhs_bot;
1336 btVector3 vbot = invI_lower_left * rhs_top;
1337 tmp = invI_lower_right * rhs_bot;
1339 result[0] = vtop[0];
1340 result[1] = vtop[1];
1341 result[2] = vtop[2];
1342 result[3] = vbot[0];
1343 result[4] = vbot[1];
1344 result[5] = vbot[2];
1388 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1411 for (
int row = 0; row < rowsA; row++)
1413 for (
int col = 0; col < colsB; col++)
1415 pC[row * colsB + col] = 0.f;
1416 for (
int inner = 0; inner < rowsB; inner++)
1418 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1432 scratch_v.
resize(4 * num_links + 4);
1439 v_ptr += num_links * 2 + 2;
1448 v_ptr += num_links * 2 + 2;
1474 fromParent.
m_rotMat = rot_from_parent[0];
1477 for (
int i = 0; i < num_links; ++i)
1479 zeroAccSpatFrc[i + 1].
setZero();
1484 for (
int i = num_links - 1; i >= 0; --i)
1486 const int parent =
m_links[i].m_parent;
1487 fromParent.
m_rotMat = rot_from_parent[i + 1];
1490 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1492 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]);
1495 btVector3 in_top, in_bottom, out_top, out_bottom;
1498 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1500 invD_times_Y[dof] = 0.f;
1502 for (
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1504 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1509 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1511 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1515 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1520 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1536 spatAcc[0] = -result;
1540 for (
int i = 0; i < num_links; ++i)
1542 const int parent =
m_links[i].m_parent;
1543 fromParent.
m_rotMat = rot_from_parent[i + 1];
1546 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1548 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1552 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1558 for (
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1559 spatAcc[i + 1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1565 output[0] = omegadot_out[0];
1566 output[1] = omegadot_out[1];
1567 output[2] = omegadot_out[2];
1593 pBasePos[0] += dt * pBaseVel[0];
1594 pBasePos[1] += dt * pBaseVel[1];
1595 pBasePos[2] += dt * pBaseVel[2];
1625 axis = angvel * (
btScalar(0.5) * dt - (dt * dt * dt) * (
btScalar(0.020833333333)) * fAngle * fAngle);
1630 axis = angvel * (
btSin(
btScalar(0.5) * fAngle * dt) / fAngle);
1650 baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1652 baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1653 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1654 pBaseQuat[0] = baseQuat.
x();
1655 pBaseQuat[1] = baseQuat.
y();
1656 pBaseQuat[2] = baseQuat.
z();
1657 pBaseQuat[3] = baseQuat.
w();
1669 for (
int i = 0; i < num_links; ++i)
1674 switch (
m_links[i].m_jointType)
1680 pJointPos[0] += dt * jointVel;
1686 jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1688 jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1689 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1690 pJointPos[0] = jointOri.
x();
1691 pJointPos[1] = jointOri.
y();
1692 pJointPos[2] = jointOri.
z();
1693 pJointPos[3] = jointOri.
w();
1702 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1703 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1712 m_links[i].updateCacheMultiDof(pq);
1715 pq +=
m_links[i].m_posVarCount;
1733 scratch_v.
resize(3 * num_links + 3);
1734 scratch_m.
resize(num_links + 1);
1738 v_ptr += num_links + 1;
1740 v_ptr += num_links + 1;
1742 v_ptr += num_links + 1;
1751 int numLinksChildToRoot=0;
1755 links[numLinksChildToRoot++]=l;
1762 const btVector3 &normal_lin_world = normal_lin;
1763 const btVector3 &normal_ang_world = normal_ang;
1769 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1770 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1771 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1772 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1774 jac[3] = normal_lin_world[0];
1775 jac[4] = normal_lin_world[1];
1776 jac[5] = normal_lin_world[2];
1779 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1780 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1781 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1790 if (num_links > 0 && link > -1)
1796 for (
int a = 0; a < numLinksChildToRoot; a++)
1798 int i = links[numLinksChildToRoot-1-a];
1800 const int parent =
m_links[i].m_parent;
1802 rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
1804 n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
1805 n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
1806 p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] -
m_links[i].m_cachedRVector;
1809 switch (
m_links[i].m_jointType)
1813 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));
1814 results[
m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
1819 results[
m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(0));
1824 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));
1825 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));
1826 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));
1828 results[
m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(0));
1829 results[
m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(1));
1830 results[
m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(
m_links[i].getAxisTop(2));
1836 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]));
1837 results[
m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(1));
1838 results[
m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(
m_links[i].getAxisBottom(2));
1852 for (
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
1854 jac[6 +
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
1858 link =
m_links[link].m_parent;
1892 if (motion < SLEEP_EPSILON)
1917 for (
int i = 0; i < num_links; ++i)
1924 world_to_local.
resize(nLinks + 1);
1925 local_origin.
resize(nLinks + 1);
1939 int index = link + 1;
1942 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
1963 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
1987 int index = link + 1;
1991 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2022 if (mbd->m_baseName)
2028 if (mbd->m_numLinks)
2031 int numElem = mbd->m_numLinks;
2034 for (
int i = 0; i < numElem; i++, memPtr++)
2069 for (
int posvar = 0; posvar < numPosVar; posvar++)
2077 if (memPtr->m_linkName)
2085 if (memPtr->m_jointName)
2097 #ifdef BT_USE_DOUBLE_PRECISION
2098 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));