37 void SpatialTransform(
const btMatrix3x3 &rotation_matrix,
44 top_out = rotation_matrix * top_in;
45 bottom_out = -displacement.
cross(top_out) + rotation_matrix * bottom_in;
48 void InverseSpatialTransform(
const btMatrix3x3 &rotation_matrix,
55 top_out = rotation_matrix.
transpose() * top_in;
56 bottom_out = rotation_matrix.
transpose() * (bottom_in + displacement.
cross(top_in));
64 return a_bottom.
dot(b_top) + a_top.
dot(b_bottom);
67 void SpatialCrossProduct(
const btVector3 &a_top,
74 top_out = a_top.
cross(b_top);
75 bottom_out = a_bottom.
cross(b_top) + a_top.
cross(b_bottom);
93 m_baseQuat(0, 0, 0, 1),
95 m_baseInertia(inertia),
97 m_fixedBase(fixedBase),
102 m_linearDamping(0.04f),
103 m_angularDamping(0.04f),
105 m_maxAppliedImpulse(1000.f),
106 m_maxCoordinateVelocity(100.f),
107 m_hasSelfCollision(true),
108 m_isMultiDof(multiDof),
113 m_useGlobalVelocities(false)
140 const btVector3 &parentComToThisPivotOffset,
141 const btVector3 &thisPivotToThisComOffset,
142 bool disableParentCollision)
147 m_links[i].m_inertiaLocal = inertia;
149 m_links[i].m_zeroRotParentToThis = rotParentToThis;
150 m_links[i].m_dVector = thisPivotToThisComOffset;
151 m_links[i].m_eVector = parentComToThisPivotOffset;
157 if (disableParentCollision)
160 m_links[i].updateCacheMultiDof();
177 const btVector3 &parentComToThisComOffset,
178 const btVector3 &thisPivotToThisComOffset,
179 bool disableParentCollision)
188 m_links[i].m_inertiaLocal = inertia;
190 m_links[i].m_zeroRotParentToThis = rotParentToThis;
191 m_links[i].setAxisTop(0, 0., 0., 0.);
192 m_links[i].setAxisBottom(0, jointAxis);
193 m_links[i].m_eVector = parentComToThisComOffset;
194 m_links[i].m_dVector = thisPivotToThisComOffset;
195 m_links[i].m_cachedRotParentToThis = rotParentToThis;
200 m_links[i].m_jointPos[0] = 0.f;
201 m_links[i].m_jointTorque[0] = 0.f;
203 if (disableParentCollision)
207 m_links[i].updateCacheMultiDof();
221 const btVector3 &parentComToThisPivotOffset,
222 const btVector3 &thisPivotToThisComOffset,
223 bool disableParentCollision)
232 m_links[i].m_inertiaLocal = inertia;
234 m_links[i].m_zeroRotParentToThis = rotParentToThis;
235 m_links[i].setAxisTop(0, jointAxis);
236 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
237 m_links[i].m_dVector = thisPivotToThisComOffset;
238 m_links[i].m_eVector = parentComToThisPivotOffset;
243 m_links[i].m_jointPos[0] = 0.f;
244 m_links[i].m_jointTorque[0] = 0.f;
246 if (disableParentCollision)
250 m_links[i].updateCacheMultiDof();
265 const btVector3 &parentComToThisPivotOffset,
266 const btVector3 &thisPivotToThisComOffset,
267 bool disableParentCollision)
274 m_links[i].m_inertiaLocal = inertia;
276 m_links[i].m_zeroRotParentToThis = rotParentToThis;
277 m_links[i].m_dVector = thisPivotToThisComOffset;
278 m_links[i].m_eVector = parentComToThisPivotOffset;
283 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
284 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
285 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
286 m_links[i].setAxisBottom(0,
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
287 m_links[i].setAxisBottom(1,
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
288 m_links[i].setAxisBottom(2,
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
293 if (disableParentCollision)
296 m_links[i].updateCacheMultiDof();
301 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 308 const btVector3 &parentComToThisComOffset,
309 bool disableParentCollision)
316 m_links[i].m_inertiaLocal = inertia;
318 m_links[i].m_zeroRotParentToThis = rotParentToThis;
319 m_links[i].m_dVector.setZero();
320 m_links[i].m_eVector = parentComToThisComOffset;
323 static btVector3 vecNonParallelToRotAxis(1, 0, 0);
324 if(rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
325 vecNonParallelToRotAxis.
setValue(0, 1, 0);
332 m_links[i].setAxisTop(0, n[0],n[1],n[2]);
333 m_links[i].setAxisTop(1,0,0,0);
334 m_links[i].setAxisTop(2,0,0,0);
335 m_links[i].setAxisBottom(0,0,0,0);
337 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
339 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
343 if (disableParentCollision)
346 m_links[i].updateCacheMultiDof();
374 return m_links[i].m_inertiaLocal;
379 return m_links[i].m_jointPos[0];
389 return &
m_links[i].m_jointPos[0];
405 for(
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
406 m_links[i].m_jointPos[pos] = q[pos];
408 m_links[i].updateCacheMultiDof();
418 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
424 return m_links[i].m_cachedRVector;
429 return m_links[i].m_cachedRotParentToThis;
487 for (
int i = 0; i < num_links; ++i) {
488 const int parent =
m_links[i].m_parent;
492 omega[parent+1], vel[parent+1],
493 omega[i+1], vel[i+1]);
513 for (
int i = 0; i < num_links; ++i) {
514 result +=
m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
515 result += omega[i+1].dot(
m_links[i].m_inertiaLocal * omega[i+1]);
518 return 0.5f * result;
533 for (
int i = 0; i < num_links; ++i) {
534 rot_from_world[i+1] =
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent+1];
535 result += (
quatRotate(rot_from_world[i+1].
inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
549 m_links[i].m_appliedForce.setValue(0, 0, 0);
550 m_links[i].m_appliedTorque.setValue(0, 0, 0);
564 m_links[i].m_appliedForce += f;
569 m_links[i].m_appliedTorque += t;
574 m_links[i].m_jointTorque[0] += Q;
579 m_links[i].m_jointTorque[dof] += Q;
584 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
585 m_links[i].m_jointTorque[dof] = Q[dof];
590 return m_links[i].m_appliedForce;
595 return m_links[i].m_appliedTorque;
600 return m_links[i].m_jointTorque[0];
605 return &
m_links[i].m_jointTorque[0];
624 row1[0],row1[1],row1[2],
625 row2[0],row2[1],row2[2]);
629 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed) 632 #ifndef TEST_SPATIAL_ALGEBRA_LAYER 663 scratch_v.
resize(8*num_links + 6);
664 scratch_m.
resize(4*num_links + 4);
671 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
672 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
675 btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
676 btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
679 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
680 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
685 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
686 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
687 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
697 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
698 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
706 btScalar * joint_accel = output + 6;
715 vel_top_angular[0] = rot_from_parent[0] * base_omega;
716 vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
720 zeroAccForce[0] = zeroAccTorque[0] =
btVector3(0,0,0);
724 zeroAccForce[0] = - (rot_from_parent[0] * (
m_baseForce 725 -
m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
733 zeroAccTorque[0] +=
m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
738 inertia_top_left[0] =
btMatrix3x3(0,0,0,0,0,0,0,0,0);
748 rot_from_world[0] = rot_from_parent[0];
750 for (
int i = 0; i < num_links; ++i) {
751 const int parent =
m_links[i].m_parent;
755 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
758 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
759 vel_top_angular[parent+1], vel_bottom_linear[parent+1],
760 vel_top_angular[i+1], vel_bottom_linear[i+1]);
765 btVector3 joint_vel_spat_top, joint_vel_spat_bottom;
767 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
773 vel_top_angular[i+1] += joint_vel_spat_top;
774 vel_bottom_linear[i+1] += joint_vel_spat_bottom;
778 SpatialCrossProduct(vel_top_angular[i+1], vel_bottom_linear[i+1], joint_vel_spat_top, joint_vel_spat_bottom, coriolis_top_angular[i], coriolis_bottom_linear[i]);
783 zeroAccForce[i+1] = -(rot_from_world[i+1] * (
m_links[i].m_appliedForce));
784 zeroAccTorque[i+1] = -(rot_from_world[i+1] *
m_links[i].m_appliedTorque);
787 zeroAccForce[i+1] +=
m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].
norm()) * vel_bottom_linear[i+1];
788 zeroAccTorque[i+1] +=
m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].
norm());
791 inertia_top_left[i+1] =
btMatrix3x3(0,0,0,0,0,0,0,0,0);
797 0, 0,
m_links[i].m_inertia[2]);
803 zeroAccTorque[i+1] += vel_top_angular[i+1].
cross(
m_links[i].m_inertia * vel_top_angular[i+1] );
805 coriolis_bottom_linear[i] += vel_top_angular[i+1].
cross(vel_bottom_linear[i+1]) - (rot_from_parent[i+1] * (vel_top_angular[parent+1].
cross(vel_bottom_linear[parent+1])));
816 for (
int i = num_links - 1; i >= 0; --i)
818 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
821 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
825 h_t = inertia_top_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_right[i+1] * m_links[i].getAxisBottom(dof);
826 h_b = inertia_bottom_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_left[i+1].
transpose() * m_links[i].getAxisBottom(dof);
829 btScalar *D_row = &D[dof * m_links[i].m_dofCount];
830 for(
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
832 D_row[dof2] = SpatialDotProduct(m_links[i].getAxisTop(dof2), m_links[i].getAxisBottom(dof2), h_t, h_b);
835 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
836 - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
837 - SpatialDotProduct(h_t, h_b, coriolis_top_angular[i], coriolis_bottom_linear[i])
841 const int parent =
m_links[i].m_parent;
849 invDi[0] = 1.0f / D[0];
853 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 857 static btMatrix3x3 D3x3; D3x3.
setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
861 for(
int row = 0; row < 3; ++row)
862 for(
int col = 0; col < 3; ++col)
863 invDi[row * 3 + col] = invD3x3[row][col];
903 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
908 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
911 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
913 tmp_top[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_t;
914 tmp_bottom[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_b;
918 btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
920 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
923 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
935 -
m_links[i].m_cachedRVector[1],
m_links[i].m_cachedRVector[0], 0);
937 inertia_top_left[parent+1] += rot_from_parent[i+1].
transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
938 inertia_top_right[parent+1] += rot_from_parent[i+1].
transpose() * TR * rot_from_parent[i+1];
939 inertia_bottom_left[parent+1] += rot_from_parent[i+1].
transpose() *
940 (r_cross * (TL - TR * r_cross) + BL - TL.
transpose() * r_cross) * rot_from_parent[i+1];
943 btVector3 in_top, in_bottom, out_top, out_bottom;
946 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
948 invD_times_Y[dof] = 0.f;
950 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
952 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
956 in_top = zeroAccForce[i+1]
957 + inertia_top_left[i+1] * coriolis_top_angular[i]
958 + inertia_top_right[i+1] * coriolis_bottom_linear[i];
960 in_bottom = zeroAccTorque[i+1]
961 + inertia_bottom_left[i+1] * coriolis_top_angular[i]
962 + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i];
965 for(
int row = 0; row < 3; ++row)
967 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
970 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
972 in_top[row] += h_t[row] * invD_times_Y[dof];
973 in_bottom[row] += h_b[row] * invD_times_Y[dof];
977 InverseSpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
978 in_top, in_bottom, out_top, out_bottom);
980 zeroAccForce[parent+1] += out_top;
981 zeroAccTorque[parent+1] += out_bottom;
990 accel_top[0] = accel_bottom[0] =
btVector3(0,0,0);
1002 btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
1003 btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
1007 for (
int i = 0; i < 3; ++i) {
1008 accel_top[0][i] = -result[i];
1009 accel_bottom[0][i] = -result[i+3];
1016 for (
int i = 0; i < num_links; ++i) {
1017 const int parent =
m_links[i].m_parent;
1019 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1020 accel_top[parent+1], accel_bottom[parent+1],
1021 accel_top[i+1], accel_bottom[i+1]);
1023 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1026 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
1028 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
1034 accel_top[i+1] += coriolis_top_angular[i];
1035 accel_bottom[i+1] += coriolis_bottom_linear[i];
1037 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1039 accel_top[i+1] += joint_accel[
m_links[i].m_dofOffset + dof] *
m_links[i].getAxisTop(dof);
1040 accel_bottom[i+1] += joint_accel[
m_links[i].m_dofOffset + dof] *
m_links[i].getAxisBottom(dof);
1046 output[0] = omegadot_out[0];
1047 output[1] = omegadot_out[1];
1048 output[2] = omegadot_out[2];
1051 output[3] = vdot_out[0];
1052 output[4] = vdot_out[1];
1053 output[5] = vdot_out[2];
1077 #else //i.e. TEST_SPATIAL_ALGEBRA_LAYER 1108 scratch_v.
resize(8*num_links + 6);
1109 scratch_m.
resize(4*num_links + 4);
1117 v_ptr += num_links * 2 + 2;
1121 v_ptr += num_links * 2 + 2;
1125 v_ptr += num_links * 2;
1138 v_ptr += num_links * 2 + 2;
1158 btScalar * joint_accel = output + 6;
1168 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
1180 btScalar linDampMult = 1., angDampMult = 1.;
1181 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
1182 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
1189 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1205 rot_from_world[0] = rot_from_parent[0];
1208 for (
int i = 0; i < num_links; ++i) {
1209 const int parent =
m_links[i].m_parent;
1211 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1214 fromWorld.
m_rotMat = rot_from_world[i+1];
1215 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
1223 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1227 spatVel[i+1] += spatJointVel;
1241 spatVel[i+1].
cross(spatJointVel, spatCoriolisAcc[i]);
1246 zeroAccSpatFrc[i+1].
setVector(-(rot_from_world[i+1] *
m_links[i].m_appliedTorque), -(rot_from_world[i+1] *
m_links[i].m_appliedForce));
1249 btScalar linDampMult = 1., angDampMult = 1.;
1250 zeroAccSpatFrc[i+1].
addVector(angDampMult *
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
1251 linDampMult *
m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
1262 0,
m_links[i].m_inertiaLocal[1], 0,
1263 0, 0,
m_links[i].m_inertiaLocal[2])
1268 zeroAccSpatFrc[i+1].
addAngular(spatVel[i+1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
1270 zeroAccSpatFrc[i+1].
addLinear(
m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
1291 for (
int i = num_links - 1; i >= 0; --i)
1293 const int parent =
m_links[i].m_parent;
1296 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1300 hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
1302 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
1303 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1304 - spatCoriolisAcc[i].
dot(hDof)
1308 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1311 for(
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1314 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
1319 switch(
m_links[i].m_jointType)
1324 invDi[0] = 1.0f / D[0];
1328 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 1332 static btMatrix3x3 D3x3; D3x3.
setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1336 for(
int row = 0; row < 3; ++row)
1338 for(
int col = 0; col < 3; ++col)
1340 invDi[row * 3 + col] = invD3x3[row][col];
1353 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1355 spatForceVecTemps[dof].
setZero();
1357 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1361 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
1365 dyadTemp = spatInertia[i+1];
1368 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1377 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1379 invD_times_Y[dof] = 0.f;
1381 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1383 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1387 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1389 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1393 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1398 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1421 spatAcc[0] = -result;
1425 for (
int i = 0; i < num_links; ++i)
1433 const int parent =
m_links[i].m_parent;
1436 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1438 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1442 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1449 spatAcc[i+1] += spatCoriolisAcc[i];
1451 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1452 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1457 output[0] = omegadot_out[0];
1458 output[1] = omegadot_out[1];
1459 output[2] = omegadot_out[2];
1462 output[3] = vdot_out[0];
1463 output[4] = vdot_out[1];
1464 output[5] = vdot_out[2];
1518 for (
int i = 0; i < num_links; ++i)
1520 const int parent =
m_links[i].m_parent;
1525 fromWorld.
m_rotMat = rot_from_world[i+1];
1528 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
1536 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1540 spatVel[i+1] += spatJointVel;
1581 scratch_r.
resize(2*num_links + 6);
1582 scratch_v.
resize(8*num_links + 6);
1583 scratch_m.
resize(4*num_links + 4);
1590 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
1591 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
1594 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1595 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1598 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
1599 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
1604 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
1605 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
1606 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
1616 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1617 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1620 btScalar * Y = r_ptr; r_ptr += num_links;
1624 btScalar * joint_accel = output + 6;
1634 vel_top_angular[0] = rot_from_parent[0] * base_omega;
1635 vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
1638 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] =
btVector3(0,0,0);
1640 zero_acc_top_angular[0] = - (rot_from_parent[0] * (
m_baseForce 1641 -
m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
1643 zero_acc_bottom_linear[0] =
1647 zero_acc_bottom_linear[0]+=vel_top_angular[0].
cross(
m_baseInertia * vel_top_angular[0] );
1649 zero_acc_bottom_linear[0] +=
m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
1655 inertia_top_left[0] =
btMatrix3x3(0,0,0,0,0,0,0,0,0);
1665 rot_from_world[0] = rot_from_parent[0];
1667 for (
int i = 0; i < num_links; ++i) {
1668 const int parent =
m_links[i].m_parent;
1672 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1675 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1676 vel_top_angular[parent+1], vel_bottom_linear[parent+1],
1677 vel_top_angular[i+1], vel_bottom_linear[i+1]);
1681 coriolis_bottom_linear[i] = vel_top_angular[i+1].
cross(vel_top_angular[i+1].cross(
m_links[i].m_cachedRVector))
1687 coriolis_top_angular[i] =
btVector3(0,0,0);
1696 zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (
m_links[i].m_appliedForce));
1697 zero_acc_top_angular[i+1] +=
m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].
norm()) * vel_bottom_linear[i+1];
1699 zero_acc_bottom_linear[i+1] =
1700 - (rot_from_world[i+1] *
m_links[i].m_appliedTorque);
1703 zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].
cross(
m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
1706 zero_acc_bottom_linear[i+1] +=
m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].
norm());
1709 inertia_top_left[i+1] =
btMatrix3x3(0,0,0,0,0,0,0,0,0);
1714 0,
m_links[i].m_inertiaLocal[1], 0,
1715 0, 0,
m_links[i].m_inertiaLocal[2]);
1721 for (
int i = num_links - 1; i >= 0; --i) {
1723 h_top[i] = inertia_top_left[i+1] *
m_links[i].getAxisTop(0) + inertia_top_right[i+1] *
m_links[i].getAxisBottom(0);
1724 h_bottom[i] = inertia_bottom_left[i+1] *
m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() *
m_links[i].getAxisBottom(0);
1725 btScalar val = SpatialDotProduct(
m_links[i].getAxisTop(0),
m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
1728 Y[i] =
m_links[i].m_jointTorque[0]
1729 - SpatialDotProduct(
m_links[i].getAxisTop(0),
m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
1730 - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
1732 const int parent =
m_links[i].m_parent;
1736 const btScalar one_over_di = 1.0f / D[i];
1748 0, -
m_links[i].m_cachedRVector[2],
m_links[i].m_cachedRVector[1],
1749 m_links[i].m_cachedRVector[2], 0, -
m_links[i].m_cachedRVector[0],
1750 -
m_links[i].m_cachedRVector[1],
m_links[i].m_cachedRVector[0], 0);
1752 inertia_top_left[parent+1] += rot_from_parent[i+1].
transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
1753 inertia_top_right[parent+1] += rot_from_parent[i+1].
transpose() * TR * rot_from_parent[i+1];
1754 inertia_bottom_left[parent+1] += rot_from_parent[i+1].
transpose() *
1755 (r_cross * (TL - TR * r_cross) + BL - TL.
transpose() * r_cross) * rot_from_parent[i+1];
1759 btVector3 in_top, in_bottom, out_top, out_bottom;
1760 const btScalar Y_over_D = Y[i] * one_over_di;
1761 in_top = zero_acc_top_angular[i+1]
1762 + inertia_top_left[i+1] * coriolis_top_angular[i]
1763 + inertia_top_right[i+1] * coriolis_bottom_linear[i]
1764 + Y_over_D * h_top[i];
1765 in_bottom = zero_acc_bottom_linear[i+1]
1766 + inertia_bottom_left[i+1] * coriolis_top_angular[i]
1767 + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
1768 + Y_over_D * h_bottom[i];
1769 InverseSpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1770 in_top, in_bottom, out_top, out_bottom);
1771 zero_acc_top_angular[parent+1] += out_top;
1772 zero_acc_bottom_linear[parent+1] += out_bottom;
1781 accel_top[0] = accel_bottom[0] =
btVector3(0,0,0);
1800 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
1801 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
1806 for (
int i = 0; i < 3; ++i) {
1807 accel_top[0][i] = -result[i];
1808 accel_bottom[0][i] = -result[i+3];
1814 for (
int i = 0; i < num_links; ++i) {
1815 const int parent =
m_links[i].m_parent;
1816 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1817 accel_top[parent+1], accel_bottom[parent+1],
1818 accel_top[i+1], accel_bottom[i+1]);
1819 joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
1820 accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] *
m_links[i].getAxisTop(0);
1821 accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] *
m_links[i].getAxisBottom(0);
1826 output[0] = omegadot_out[0];
1827 output[1] = omegadot_out[1];
1828 output[2] = omegadot_out[2];
1831 output[3] = vdot_out[0];
1832 output[4] = vdot_out[1];
1833 output[5] = vdot_out[2];
1881 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1890 btVector3 vtop = invI_upper_left*rhs_top;
1892 tmp = invIupper_right * rhs_bot;
1894 btVector3 vbot = invI_lower_left*rhs_top;
1895 tmp = invI_lower_right * rhs_bot;
1897 result[0] = vtop[0];
1898 result[1] = vtop[1];
1899 result[2] = vtop[2];
1900 result[3] = vbot[0];
1901 result[4] = vbot[1];
1902 result[5] = vbot[2];
1907 #ifdef TEST_SPATIAL_ALGEBRA_LAYER 1926 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1951 for (
int row = 0; row < rowsA; row++)
1953 for (
int col = 0; col < colsB; col++)
1955 pC[row * colsB + col] = 0.f;
1956 for (
int inner = 0; inner < rowsB; inner++)
1958 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1964 #ifndef TEST_SPATIAL_ALGEBRA_LAYER 1973 scratch_r.
resize(m_dofCount);
1974 scratch_v.
resize(4*num_links + 4);
1976 btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
1980 btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
1981 btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
1990 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1991 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
2001 btVector3 input_force(force[3],force[4],force[5]);
2002 btVector3 input_torque(force[0],force[1],force[2]);
2008 zeroAccForce[0] = zeroAccTorque[0] =
btVector3(0,0,0);
2011 zeroAccForce[0] = - (rot_from_parent[0] * input_force);
2012 zeroAccTorque[0] = - (rot_from_parent[0] * input_torque);
2014 for (
int i = 0; i < num_links; ++i)
2016 zeroAccForce[i+1] = zeroAccTorque[i+1] =
btVector3(0,0,0);
2021 for (
int i = num_links - 1; i >= 0; --i)
2023 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2027 Y[
m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
2028 - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
2033 const int parent =
m_links[i].m_parent;
2035 btVector3 in_top, in_bottom, out_top, out_bottom;
2039 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2041 invD_times_Y[dof] = 0.f;
2043 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
2045 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
2050 in_top = zeroAccForce[i+1];
2051 in_bottom = zeroAccTorque[i+1];
2054 for(
int row = 0; row < 3; ++row)
2056 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2059 const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
2061 in_top[row] += h_t[row] * invD_times_Y[dof];
2062 in_bottom[row] += h_b[row] * invD_times_Y[dof];
2066 InverseSpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
2067 in_top, in_bottom, out_top, out_bottom);
2068 zeroAccForce[parent+1] += out_top;
2069 zeroAccTorque[parent+1] += out_bottom;
2073 btScalar * joint_accel = output + 6;
2081 accel_top[0] = accel_bottom[0] =
btVector3(0,0,0);
2085 btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
2086 btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
2090 for (
int i = 0; i < 3; ++i) {
2091 accel_top[0][i] = -result[i];
2092 accel_bottom[0][i] = -result[i+3];
2099 for (
int i = 0; i < num_links; ++i) {
2100 const int parent =
m_links[i].m_parent;
2102 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
2103 accel_top[parent+1], accel_bottom[parent+1],
2104 accel_top[i+1], accel_bottom[i+1]);
2106 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2109 const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
2111 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
2117 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2119 accel_top[i+1] += joint_accel[
m_links[i].m_dofOffset + dof] *
m_links[i].getAxisTop(dof);
2120 accel_bottom[i+1] += joint_accel[
m_links[i].m_dofOffset + dof] *
m_links[i].getAxisBottom(dof);
2126 omegadot_out = rot_from_parent[0].
transpose() * accel_top[0];
2127 output[0] = omegadot_out[0];
2128 output[1] = omegadot_out[1];
2129 output[2] = omegadot_out[2];
2132 vdot_out = rot_from_parent[0].
transpose() * accel_bottom[0];
2134 output[3] = vdot_out[0];
2135 output[4] = vdot_out[1];
2136 output[5] = vdot_out[2];
2145 #else //i.e. TEST_SPATIAL_ALGEBRA_LAYER 2154 scratch_v.
resize(4*num_links + 4);
2161 v_ptr += num_links * 2 + 2;
2170 v_ptr += num_links * 2 + 2;
2195 fromParent.
m_rotMat = rot_from_parent[0];
2198 for (
int i = 0; i < num_links; ++i)
2200 zeroAccSpatFrc[i+1].
setZero();
2205 for (
int i = num_links - 1; i >= 0; --i)
2207 const int parent =
m_links[i].m_parent;
2210 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2212 Y[
m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
2213 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
2217 btVector3 in_top, in_bottom, out_top, out_bottom;
2220 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2222 invD_times_Y[dof] = 0.f;
2224 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
2226 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
2231 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
2233 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2237 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
2243 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
2247 btScalar * joint_accel = output + 6;
2260 spatAcc[0] = -result;
2265 for (
int i = 0; i < num_links; ++i)
2267 const int parent =
m_links[i].m_parent;
2270 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
2272 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2276 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
2282 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
2283 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
2289 output[0] = omegadot_out[0];
2290 output[1] = omegadot_out[1];
2291 output[2] = omegadot_out[2];
2295 output[3] = vdot_out[0];
2296 output[4] = vdot_out[1];
2297 output[5] = vdot_out[2];
2315 scratch_r.
resize(num_links);
2316 scratch_v.
resize(4*num_links + 4);
2318 btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
2322 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
2323 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
2331 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
2332 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
2335 btScalar * Y = r_ptr; r_ptr += num_links;
2338 btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.
size());
2346 btVector3 input_force(force[3],force[4],force[5]);
2347 btVector3 input_torque(force[0],force[1],force[2]);
2353 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] =
btVector3(0,0,0);
2356 zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
2357 zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
2359 for (
int i = 0; i < num_links; ++i)
2361 zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] =
btVector3(0,0,0);
2365 for (
int i = num_links - 1; i >= 0; --i)
2368 Y[i] = - SpatialDotProduct(
m_links[i].getAxisTop(0),
m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
2369 Y[i] += force[6 + i];
2371 const int parent =
m_links[i].m_parent;
2374 btVector3 in_top, in_bottom, out_top, out_bottom;
2375 const btScalar Y_over_D = Y[i] / D[i];
2376 in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
2377 in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
2378 InverseSpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
2379 in_top, in_bottom, out_top, out_bottom);
2380 zero_acc_top_angular[parent+1] += out_top;
2381 zero_acc_bottom_linear[parent+1] += out_bottom;
2385 btScalar * joint_accel = output + 6;
2390 accel_top[0] = accel_bottom[0] =
btVector3(0,0,0);
2393 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
2394 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
2400 for (
int i = 0; i < 3; ++i) {
2401 accel_top[0][i] = -result[i];
2402 accel_bottom[0][i] = -result[i+3];
2408 for (
int i = 0; i < num_links; ++i) {
2409 const int parent =
m_links[i].m_parent;
2410 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
2411 accel_top[parent+1], accel_bottom[parent+1],
2412 accel_top[i+1], accel_bottom[i+1]);
2413 btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
2414 joint_accel[i] = Y_minus_hT_a / D[i];
2415 accel_top[i+1] += joint_accel[i] *
m_links[i].getAxisTop(0);
2416 accel_bottom[i+1] += joint_accel[i] *
m_links[i].getAxisBottom(0);
2421 omegadot_out = rot_from_parent[0].
transpose() * accel_top[0];
2422 output[0] = omegadot_out[0];
2423 output[1] = omegadot_out[1];
2424 output[2] = omegadot_out[2];
2427 vdot_out = rot_from_parent[0].
transpose() * accel_bottom[0];
2429 output[3] = vdot_out[0];
2430 output[4] = vdot_out[1];
2431 output[5] = vdot_out[2];
2465 const btScalar omega_times_dt = omega_norm * dt;
2466 const btScalar SMALL_ROTATION_ANGLE = 0.02f;
2467 if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
2469 const btScalar xsq = omega_times_dt * omega_times_dt;
2470 const btScalar sin_term = dt * (xsq / 48.0f - 0.5f);
2471 const btScalar cos_term = 1.0f - xsq / 8.0f;
2483 for (
int i = 0; i < num_links; ++i)
2486 m_links[i].m_jointPos[0] += dt * jointVel;
2501 pBasePos[0] += dt * pBaseVel[0];
2502 pBasePos[1] += dt * pBaseVel[1];
2503 pBasePos[2] += dt * pBaseVel[2];
2533 axis = angvel*(
btScalar(0.5)*dt-(dt*dt*dt)*(
btScalar(0.020833333333))*fAngle*fAngle );
2557 static btQuaternion baseQuat; baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
2558 static btVector3 baseOmega; baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
2559 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
2560 pBaseQuat[0] = baseQuat.
x();
2561 pBaseQuat[1] = baseQuat.
y();
2562 pBaseQuat[2] = baseQuat.
z();
2563 pBaseQuat[3] = baseQuat.
w();
2576 for (
int i = 0; i < num_links; ++i)
2581 switch(
m_links[i].m_jointType)
2587 pJointPos[0] += dt * jointVel;
2592 static btVector3 jointVel; jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
2593 static btQuaternion jointOri; jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
2594 pQuatUpdateFun(jointVel, jointOri,
false, dt);
2595 pJointPos[0] = jointOri.
x(); pJointPos[1] = jointOri.
y(); pJointPos[2] = jointOri.
z(); pJointPos[3] = jointOri.
w();
2598 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 2605 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
2606 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
2617 m_links[i].updateCacheMultiDof(pq);
2620 pq +=
m_links[i].m_posVarCount;
2638 scratch_v.
resize(3*num_links + 3);
2639 scratch_m.
resize(num_links + 1);
2642 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
2643 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
2644 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
2647 scratch_r.
resize(m_dofCount);
2648 btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
2653 const btVector3 &normal_lin_world = normal_lin;
2654 const btVector3 &normal_ang_world = normal_ang;
2660 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
2661 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
2662 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
2663 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
2665 jac[3] = normal_lin_world[0];
2666 jac[4] = normal_lin_world[1];
2667 jac[5] = normal_lin_world[2];
2670 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
2671 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
2672 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2681 if (num_links > 0 && link > -1) {
2688 for (
int i = 0; i < num_links; ++i) {
2691 const int parent =
m_links[i].m_parent;
2693 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2695 n_local_lin[i+1] = mtx * n_local_lin[parent+1];
2696 n_local_ang[i+1] = mtx * n_local_ang[parent+1];
2697 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] -
m_links[i].m_cachedRVector;
2700 switch(
m_links[i].m_jointType)
2704 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));
2705 results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2710 results[
m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
2715 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));
2716 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));
2717 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));
2719 results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2720 results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
2721 results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
2725 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 2728 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]));
2729 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
2730 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
2746 for(
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
2748 jac[6 +
m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
2752 link =
m_links[link].m_parent;
2768 scratch_v.
resize(2*num_links + 2);
2769 scratch_m.
resize(num_links + 1);
2772 btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
2773 btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
2776 scratch_r.
resize(num_links);
2777 btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
2785 p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
2786 n_local[0] = rot_from_world[0] * normal;
2791 for (
int i=0;i<6;i++)
2799 omega_coeffs = p_minus_com_world.
cross(normal);
2800 jac[0] = omega_coeffs[0];
2801 jac[1] = omega_coeffs[1];
2802 jac[2] = omega_coeffs[2];
2809 for (
int i = 6; i < 6 + num_links; ++i) {
2814 if (num_links > 0 && link > -1) {
2821 for (
int i = 0; i < num_links; ++i) {
2824 const int parent =
m_links[i].m_parent;
2826 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2828 n_local[i+1] = mtx * n_local[parent+1];
2829 p_minus_com[i+1] = mtx * p_minus_com[parent+1] -
m_links[i].m_cachedRVector;
2833 results[i] = n_local[i+1].dot(
m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) +
m_links[i].getAxisBottom(0) );
2835 results[i] = n_local[i+1].dot(
m_links[i].getAxisBottom(0) );
2843 jac[6 + link] = results[link];
2845 link =
m_links[link].m_parent;
2881 for (
int i = 0; i < 6 + num_links; ++i)
2886 if (motion < SLEEP_EPSILON) {
void filConstraintJacobianMultiDof(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
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
const btScalar & x() const
Return the x value.
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, float result[6]) const
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
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 & getBasePos() const
btVector3 localPosToWorld(int i, const btVector3 &vec) const
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
btScalar norm() const
Return the norm (length) of the vector.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
const btVector3 & getLinear() const
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
btScalar btSin(btScalar x)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
#define vecMulVecTranspose(v0, v1Transposed)
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
bool gDisableDeactivation
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
const btQuaternion & getWorldToBaseRot() const
btMatrix3x3 m_cachedInertiaLowerRight
btScalar * getJointVelMultiDof(int i)
void addLinkForce(int i, const btVector3 &f)
void addLinear(const btVector3 &linear)
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisComOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void setJointPosMultiDof(int i, btScalar *q)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
const btVector3 & getLinkTorque(int i) const
btVector3 getBaseOmega() const
void applyDeltaVee(const btScalar *delta_vee)
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
void clearForcesAndTorques()
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
btVector3 normalized() const
Return a normalized version of this vector.
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool multiDof=false)
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void addAngular(const btVector3 &angular)
btMatrix3x3 m_cachedInertiaTopLeft
const btQuaternion & getParentToLocalRot(int i) const
const btVector3 & getLinear() const
const btScalar & x() const
Return the x value.
void setVector(const btVector3 &angular, const btVector3 &linear)
btMatrix3x3 m_cachedInertiaLowerLeft
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
btScalar getJointTorque(int i) const
void addLinkTorque(int i, const btVector3 &t)
const btVector3 & getAngular() const
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)
Set x,y,z and zero w.
void setLinear(const btVector3 &linear)
int getParent(int link_num) const
const btScalar & y() const
Return the y value.
const btScalar & z() const
Return the z value.
void addJointTorque(int i, btScalar Q)
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 setJointVel(int i, btScalar qdot)
const btScalar & z() const
Return the z value.
void setAngular(const btVector3 &angular)
void setJointPos(int i, btScalar q)
void checkMotionAndSleepIfRequired(btScalar timestep)
btVector3 can be used to represent 3D points and vectors.
int size() const
return the number of elements in the array
btAlignedObjectArray< btScalar > m_realBuf
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
btScalar dot(const btSpatialForceVector &b) const
void updateLinksDofOffsets()
const btVector3 & getAngular() const
bool m_useGlobalVelocities
btVector3 localDirToWorld(int i, const btVector3 &vec) const
void resize(int newsize, const T &fillData=T())
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
btScalar getJointPos(int i) const
btVector3 getAngularMomentum() const
const btVector3 & getLinkForce(int i) const
btMatrix3x3 m_bottomLeftMat
void setVector(const btVector3 &angular, const btVector3 &linear)
btAlignedObjectArray< btVector3 > m_vectorBuf
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
const btScalar & y() const
Return the y value.
void setJointVelMultiDof(int i, btScalar *qdot)
btScalar getLinkMass(int i) const
btScalar getKineticEnergy() const
void fillContactJacobian(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void addVector(const btVector3 &angular, const btVector3 &linear)
btScalar getJointVel(int i) const
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
void stepPositions(btScalar dt)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
btAlignedObjectArray< btMultibodyLink > m_links
btScalar * getJointTorqueMultiDof(int i)
const btScalar & w() const
Return the w value.
btMatrix3x3 m_cachedInertiaTopRight
btScalar * getJointPosMultiDof(int i)
const btVector3 & getLinkInertia(int i) const
const btVector3 & getRVector(int i) const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar m_angularDamping
btScalar btCos(btScalar x)
btMatrix3x3 m_topRightMat
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar length() const
Return the length of the vector.
const btVector3 getBaseVel() const