44 void SpatialTransform(
const btMatrix3x3 &rotation_matrix,
51 top_out = rotation_matrix * top_in;
52 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);
104 m_baseQuat(0, 0, 0, 1),
106 m_baseInertia(inertia),
108 m_fixedBase(fixedBase),
110 m_canSleep(canSleep),
112 m_userObjectPointer(0),
115 m_linearDamping(0.04f),
116 m_angularDamping(0.04f),
118 m_maxAppliedImpulse(1000.f),
119 m_maxCoordinateVelocity(100.f),
120 m_hasSelfCollision(true),
125 m_useGlobalVelocities(false),
126 m_internalNeedsJointFeedback(false)
150 const btVector3 &parentComToThisPivotOffset,
151 const btVector3 &thisPivotToThisComOffset,
bool )
155 m_links[i].m_inertiaLocal = inertia;
157 m_links[i].setAxisTop(0, 0., 0., 0.);
159 m_links[i].m_zeroRotParentToThis = rotParentToThis;
160 m_links[i].m_dVector = thisPivotToThisComOffset;
161 m_links[i].m_eVector = parentComToThisPivotOffset;
169 m_links[i].updateCacheMultiDof();
182 const btVector3 &parentComToThisPivotOffset,
183 const btVector3 &thisPivotToThisComOffset,
184 bool disableParentCollision)
190 m_links[i].m_inertiaLocal = inertia;
192 m_links[i].m_zeroRotParentToThis = rotParentToThis;
193 m_links[i].setAxisTop(0, 0., 0., 0.);
194 m_links[i].setAxisBottom(0, jointAxis);
195 m_links[i].m_eVector = parentComToThisPivotOffset;
196 m_links[i].m_dVector = thisPivotToThisComOffset;
197 m_links[i].m_cachedRotParentToThis = rotParentToThis;
202 m_links[i].m_jointPos[0] = 0.f;
203 m_links[i].m_jointTorque[0] = 0.f;
205 if (disableParentCollision)
209 m_links[i].updateCacheMultiDof();
220 const btVector3 &parentComToThisPivotOffset,
221 const btVector3 &thisPivotToThisComOffset,
222 bool disableParentCollision)
228 m_links[i].m_inertiaLocal = inertia;
230 m_links[i].m_zeroRotParentToThis = rotParentToThis;
231 m_links[i].setAxisTop(0, jointAxis);
232 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
233 m_links[i].m_dVector = thisPivotToThisComOffset;
234 m_links[i].m_eVector = parentComToThisPivotOffset;
239 m_links[i].m_jointPos[0] = 0.f;
240 m_links[i].m_jointTorque[0] = 0.f;
242 if (disableParentCollision)
245 m_links[i].updateCacheMultiDof();
257 const btVector3 &parentComToThisPivotOffset,
258 const btVector3 &thisPivotToThisComOffset,
259 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));
285 if (disableParentCollision)
288 m_links[i].updateCacheMultiDof();
299 const btVector3 &parentComToThisComOffset,
300 bool disableParentCollision)
307 m_links[i].m_inertiaLocal = inertia;
309 m_links[i].m_zeroRotParentToThis = rotParentToThis;
310 m_links[i].m_dVector.setZero();
311 m_links[i].m_eVector = parentComToThisComOffset;
314 btVector3 vecNonParallelToRotAxis(1, 0, 0);
315 if(rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
316 vecNonParallelToRotAxis.
setValue(0, 1, 0);
323 m_links[i].setAxisTop(0, n[0],n[1],n[2]);
324 m_links[i].setAxisTop(1,0,0,0);
325 m_links[i].setAxisTop(2,0,0,0);
326 m_links[i].setAxisBottom(0,0,0,0);
328 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
330 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
334 if (disableParentCollision)
337 m_links[i].updateCacheMultiDof();
364 return m_links[i].m_inertiaLocal;
369 return m_links[i].m_jointPos[0];
379 return &
m_links[i].m_jointPos[0];
389 return &
m_links[i].m_jointPos[0];
401 m_links[i].updateCacheMultiDof();
406 for(
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
407 m_links[i].m_jointPos[pos] = q[pos];
409 m_links[i].updateCacheMultiDof();
419 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
425 return m_links[i].m_cachedRVector;
430 return m_links[i].m_cachedRotParentToThis;
437 if ((i<-1) || (i>=
m_links.size()))
461 if ((i<-1) || (i>=
m_links.size()))
479 if ((i<-1) || (i>=
m_links.size()))
498 if ((i<-1) || (i>=
m_links.size()))
516 result.
setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
527 for (
int i = 0; i < num_links; ++i)
529 const int parent =
m_links[i].m_parent;
533 omega[parent+1], vel[parent+1],
534 omega[i+1], vel[i+1]);
546 omega[i+1] += jointVel * axisTop;
547 vel[i+1] += jointVel * axisBottom;
569 for (
int i = 0; i < num_links; ++i) {
570 result +=
m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
571 result += omega[i+1].dot(
m_links[i].m_inertiaLocal * omega[i+1]);
574 return 0.5f * result;
589 for (
int i = 0; i < num_links; ++i) {
590 rot_from_world[i+1] =
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent+1];
604 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
605 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
615 m_links[i].m_appliedForce.setValue(0, 0, 0);
616 m_links[i].m_appliedTorque.setValue(0, 0, 0);
630 m_links[i].m_appliedForce += f;
635 m_links[i].m_appliedTorque += t;
640 m_links[i].m_appliedConstraintForce += f;
645 m_links[i].m_appliedConstraintTorque += t;
652 m_links[i].m_jointTorque[0] += Q;
657 m_links[i].m_jointTorque[dof] += Q;
662 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
663 m_links[i].m_jointTorque[dof] = Q[dof];
668 return m_links[i].m_appliedForce;
673 return m_links[i].m_appliedTorque;
678 return m_links[i].m_jointTorque[0];
683 return &
m_links[i].m_jointTorque[0];
702 row1[0],row1[1],row1[2],
703 row2[0],row2[1],row2[2]);
707#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
714 bool isConstraintPass)
747 scratch_v.
resize(8*num_links + 6);
748 scratch_m.
resize(4*num_links + 4);
756 v_ptr += num_links * 2 + 2;
760 v_ptr += num_links * 2 + 2;
764 v_ptr += num_links * 2;
777 v_ptr += num_links * 2 + 2;
807 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
818 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
821 btScalar linDampMult = 1., angDampMult = 1.;
822 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
823 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
830 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
846 rot_from_world[0] = rot_from_parent[0];
849 for (
int i = 0; i < num_links; ++i) {
850 const int parent =
m_links[i].m_parent;
852 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
855 fromWorld.
m_rotMat = rot_from_world[i+1];
856 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
864 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
868 spatVel[i+1] += spatJointVel;
882 spatVel[i+1].
cross(spatJointVel, spatCoriolisAcc[i]);
887 btVector3 linkAppliedForce = isConstraintPass?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
888 btVector3 linkAppliedTorque =isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
890 zeroAccSpatFrc[i+1].
setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
895 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
897 zeroAccSpatFrc[i+1].m_topVec[0],
898 zeroAccSpatFrc[i+1].m_topVec[1],
899 zeroAccSpatFrc[i+1].m_topVec[2],
901 zeroAccSpatFrc[i+1].m_bottomVec[0],
902 zeroAccSpatFrc[i+1].m_bottomVec[1],
903 zeroAccSpatFrc[i+1].m_bottomVec[2]);
908 btScalar linDampMult = 1., angDampMult = 1.;
909 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()),
910 linDampMult *
m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().safeNorm()));
921 0,
m_links[i].m_inertiaLocal[1], 0,
922 0, 0,
m_links[i].m_inertiaLocal[2])
927 zeroAccSpatFrc[i+1].
addAngular(spatVel[i+1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
929 zeroAccSpatFrc[i+1].
addLinear(
m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
950 for (
int i = num_links - 1; i >= 0; --i)
952 const int parent =
m_links[i].m_parent;
955 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
959 hDof = spatInertia[i+1] *
m_links[i].m_axes[dof];
962 -
m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
963 - spatCoriolisAcc[i].
dot(hDof)
967 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
970 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
973 D_row[dof2] =
m_links[i].m_axes[dof].dot(hDof2);
983 invDi[0] = 1.0f / D[0];
993 for(
int row = 0; row < 3; ++row)
995 for(
int col = 0; col < 3; ++col)
997 invDi[row * 3 + col] = invD3x3[row][col];
1010 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1012 spatForceVecTemps[dof].
setZero();
1014 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1018 spatForceVecTemps[dof] += hDof2 * invDi[dof2 *
m_links[i].m_dofCount + dof];
1022 dyadTemp = spatInertia[i+1];
1025 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1034 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1036 invD_times_Y[dof] = 0.f;
1038 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1040 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1044 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1046 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1050 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1055 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1079 spatAcc[0] = -result;
1084 for (
int i = 0; i < num_links; ++i)
1092 const int parent =
m_links[i].m_parent;
1095 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1097 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1101 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1108 spatAcc[i+1] += spatCoriolisAcc[i];
1110 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1111 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1113 if (
m_links[i].m_jointFeedback)
1117 btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
1118 btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
1125 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1131 if (isConstraintPass)
1133 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1134 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1137 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1138 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1142 if (isConstraintPass)
1144 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1145 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1150 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1151 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1160 output[0] = omegadot_out[0];
1161 output[1] = omegadot_out[1];
1162 output[2] = omegadot_out[2];
1189 if (!isConstraintPass)
1225 for (
int i = 0; i < num_links; ++i)
1227 const int parent =
m_links[i].m_parent;
1232 fromWorld.
m_rotMat = rot_from_world[i+1];
1235 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
1243 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1247 spatVel[i+1] += spatJointVel;
1276 for (
int i=0;i<6;i++)
1289 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1298 btVector3 vtop = invI_upper_left*rhs_top;
1300 tmp = invIupper_right * rhs_bot;
1302 btVector3 vbot = invI_lower_left*rhs_top;
1303 tmp = invI_lower_right * rhs_bot;
1305 result[0] = vtop[0];
1306 result[1] = vtop[1];
1307 result[2] = vtop[2];
1308 result[3] = vbot[0];
1309 result[4] = vbot[1];
1310 result[5] = vbot[2];
1340 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1364 for (
int row = 0; row < rowsA; row++)
1366 for (
int col = 0; col < colsB; col++)
1368 pC[row * colsB + col] = 0.f;
1369 for (
int inner = 0; inner < rowsB; inner++)
1371 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1386 scratch_v.
resize(4*num_links + 4);
1393 v_ptr += num_links * 2 + 2;
1402 v_ptr += num_links * 2 + 2;
1427 fromParent.
m_rotMat = rot_from_parent[0];
1430 for (
int i = 0; i < num_links; ++i)
1432 zeroAccSpatFrc[i+1].
setZero();
1437 for (
int i = num_links - 1; i >= 0; --i)
1439 const int parent =
m_links[i].m_parent;
1442 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1444 Y[
m_links[i].m_dofOffset + dof] = force[6 +
m_links[i].m_dofOffset + dof]
1445 -
m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1449 btVector3 in_top, in_bottom, out_top, out_bottom;
1452 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1454 invD_times_Y[dof] = 0.f;
1456 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1458 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1463 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
1465 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1469 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1475 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1492 spatAcc[0] = -result;
1497 for (
int i = 0; i < num_links; ++i)
1499 const int parent =
m_links[i].m_parent;
1502 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1504 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1508 Y_minus_hT_a[dof] = Y[
m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1514 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1515 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1521 output[0] = omegadot_out[0];
1522 output[1] = omegadot_out[1];
1523 output[2] = omegadot_out[2];
1552 pBasePos[0] += dt * pBaseVel[0];
1553 pBasePos[1] += dt * pBaseVel[1];
1554 pBasePos[2] += dt * pBaseVel[2];
1584 axis = angvel*(
btScalar(0.5)*dt-(dt*dt*dt)*(
btScalar(0.020833333333))*fAngle*fAngle );
1608 btQuaternion baseQuat; baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1609 btVector3 baseOmega; baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1610 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1611 pBaseQuat[0] = baseQuat.
x();
1612 pBaseQuat[1] = baseQuat.
y();
1613 pBaseQuat[2] = baseQuat.
z();
1614 pBaseQuat[3] = baseQuat.
w();
1627 for (
int i = 0; i < num_links; ++i)
1632 switch(
m_links[i].m_jointType)
1638 pJointPos[0] += dt * jointVel;
1643 btVector3 jointVel; jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1644 btQuaternion jointOri; jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1645 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1646 pJointPos[0] = jointOri.
x(); pJointPos[1] = jointOri.
y(); pJointPos[2] = jointOri.
z(); pJointPos[3] = jointOri.
w();
1655 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1656 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1666 m_links[i].updateCacheMultiDof(pq);
1669 pq +=
m_links[i].m_posVarCount;
1687 scratch_v.
resize(3*num_links + 3);
1688 scratch_m.
resize(num_links + 1);
1691 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
1692 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
1693 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
1702 const btVector3 &normal_lin_world = normal_lin;
1703 const btVector3 &normal_ang_world = normal_ang;
1709 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
1710 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1711 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1712 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1714 jac[3] = normal_lin_world[0];
1715 jac[4] = normal_lin_world[1];
1716 jac[5] = normal_lin_world[2];
1719 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1720 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1721 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1730 if (num_links > 0 && link > -1) {
1737 for (
int i = 0; i < num_links; ++i) {
1740 const int parent =
m_links[i].m_parent;
1742 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
1744 n_local_lin[i+1] = mtx * n_local_lin[parent+1];
1745 n_local_ang[i+1] = mtx * n_local_ang[parent+1];
1746 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] -
m_links[i].m_cachedRVector;
1749 switch(
m_links[i].m_jointType)
1753 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));
1754 results[
m_links[i].m_dofOffset] += n_local_ang[i+1].dot(
m_links[i].getAxisTop(0));
1759 results[
m_links[i].m_dofOffset] = n_local_lin[i+1].dot(
m_links[i].getAxisBottom(0));
1764 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));
1765 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));
1766 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));
1768 results[
m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(
m_links[i].getAxisTop(0));
1769 results[
m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(
m_links[i].getAxisTop(1));
1770 results[
m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(
m_links[i].getAxisTop(2));
1776 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]));
1777 results[
m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(
m_links[i].getAxisBottom(1));
1778 results[
m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(
m_links[i].getAxisBottom(2));
1793 for(
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
1795 jac[6 +
m_links[link].m_dofOffset + dof] = results[
m_links[link].m_dofOffset + dof];
1799 link =
m_links[link].m_parent;
1834 if (motion < SLEEP_EPSILON) {
1857 for (
int i = 0; i < num_links; ++i)
1864 world_to_local.
resize(nLinks+1);
1865 local_origin.
resize(nLinks+1);
1882 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
1905 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
1935 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
1962 if (mbd->m_baseName)
1968 if (mbd->m_numLinks)
1971 int numElem = mbd->m_numLinks;
1974 for (
int i=0;i<numElem;i++,memPtr++)
2005 for (
int posvar = 0; posvar < numPosVar;posvar++)
2014 if (memPtr->m_linkName)
2022 if (memPtr->m_jointName)
2035#ifdef BT_USE_DOUBLE_PRECISION
2036 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));
#define btCollisionObjectData
@ BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
bool gJointFeedbackInWorldSpace
todo: determine if we need these options. If so, make a proper API, otherwise delete those globals
bool gJointFeedbackInJointFrame
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
#define btMultiBodyDataName
#define btMultiBodyData
serialization data, don't change them if you are not familiar with the details of the serialization m...
#define btMultiBodyLinkData
#define btMultiBodyLinkDataName
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
bool gDisableDeactivation
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btSin(btScalar x)
btScalar btCos(btScalar x)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void setWorldTransform(const btTransform &worldTrans)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
const btVector3 & getLinkTorque(int i) const
btAlignedObjectArray< btMultibodyLink > m_links
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
const btVector3 & getBasePos() const
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btAlignedObjectArray< btVector3 > m_vectorBuf
btVector3 localPosToWorld(int i, const btVector3 &vec) const
void setJointVelMultiDof(int i, btScalar *qdot)
btScalar getJointPos(int i) const
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
const btVector3 & getLinkInertia(int i) const
btScalar * getJointPosMultiDof(int i)
btScalar m_angularDamping
btScalar getKineticEnergy() const
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btMatrix3x3 m_cachedInertiaLowerRight
void addLinkConstraintForce(int i, const btVector3 &f)
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
btAlignedObjectArray< btScalar > m_realBuf
btVector3 m_baseConstraintTorque
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
const btMultibodyLink & getLink(int index) const
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
const btVector3 & getRVector(int i) const
btVector3 getBaseOmega() const
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void addLinkTorque(int i, const btVector3 &t)
void checkMotionAndSleepIfRequired(btScalar timestep)
btScalar * getJointTorqueMultiDof(int i)
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
btScalar getLinkMass(int i) const
bool m_useGlobalVelocities
void addLinkForce(int i, const btVector3 &f)
void addJointTorque(int i, btScalar Q)
btMatrix3x3 m_cachedInertiaLowerLeft
void setupRevolute(int linkIndex, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
const btVector3 & getLinkForce(int i) const
int getParent(int link_num) const
void setJointVel(int i, btScalar qdot)
btScalar getJointTorque(int i) const
void clearConstraintForces()
btMatrix3x3 m_cachedInertiaTopLeft
void addLinkConstraintTorque(int i, const btVector3 &t)
bool m_cachedInertiaValid
const btMultiBodyLinkCollider * getBaseCollider() const
void setJointPos(int i, btScalar q)
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
btVector3 localDirToWorld(int i, const btVector3 &vec) const
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
btTransform getBaseWorldTransform() const
btMatrix3x3 m_cachedInertiaTopRight
btScalar getBaseMass() const
btAlignedObjectArray< btScalar > m_deltaV
btVector3 getAngularMomentum() const
void updateLinksDofOffsets()
virtual int calculateSerializeBufferSize() const
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
void setJointPosMultiDof(int i, btScalar *q)
const btQuaternion & getParentToLocalRot(int i) const
const btVector3 & getBaseInertia() const
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
const btQuaternion & getWorldToBaseRot() const
btVector3 m_baseConstraintForce
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
btScalar getJointVel(int i) const
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
const btVector3 getBaseVel() const
btScalar * getJointVelMultiDof(int i)
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool deprecatedMultiDof=true)
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
void clearForcesAndTorques()
const btScalar & w() const
Return the w value.
const btScalar & z() const
Return the z value.
const btScalar & y() const
Return the y value.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
const btScalar & x() const
Return the x value.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void serialize(struct btQuaternionData &dataOut) const
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
virtual btChunk * allocate(size_t size, int numElements)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void serializeName(const char *ptr)=0
virtual const char * findNameForPointer(const void *ptr) const =0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btVector3 can be used to represent 3D points and vectors.
const btScalar & z() const
Return the z value.
btScalar length() const
Return the length of the vector.
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
btVector3 normalized() const
Return a normalized version of this vector.
const btScalar & x() const
Return the x value.
void serialize(struct btVector3Data &dataOut) const
const btScalar & y() const
Return the y value.
btQuaternion m_zeroRotParentToThis
class btMultiBodyLinkCollider * m_collider
const btVector3 & getAxisTop(int dof) const
btScalar m_jointUpperLimit
const btVector3 & getAxisBottom(int dof) const
eFeatherstoneJointType m_jointType
btScalar m_jointLowerLimit
btTransform m_cachedWorldTransform
btScalar m_jointMaxVelocity
btScalar m_jointTorque[6]
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
void addLinear(const btVector3 &linear)
void addVector(const btVector3 &angular, const btVector3 &linear)
const btVector3 & getAngular() const
const btVector3 & getLinear() const
void addAngular(const btVector3 &angular)
void setVector(const btVector3 &angular, const btVector3 &linear)
void setLinear(const btVector3 &linear)
const btVector3 & getLinear() const
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
const btVector3 & getAngular() const
btScalar dot(const btSpatialForceVector &b) const
void setAngular(const btVector3 &angular)
void setVector(const btVector3 &angular, const btVector3 &linear)
btMatrix3x3 m_topRightMat
btMatrix3x3 m_bottomLeftMat
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)