30 #include "Bullet3Common/b3Logging.h"
43 void SpatialTransform(
const btMatrix3x3 &rotation_matrix,
50 top_out = rotation_matrix * top_in;
51 bottom_out = -displacement.
cross(top_out) + rotation_matrix * bottom_in;
54 void InverseSpatialTransform(
const btMatrix3x3 &rotation_matrix,
61 top_out = rotation_matrix.
transpose() * top_in;
62 bottom_out = rotation_matrix.
transpose() * (bottom_in + displacement.
cross(top_in));
70 return a_bottom.
dot(b_top) + a_top.
dot(b_bottom);
73 void SpatialCrossProduct(
const btVector3 &a_top,
80 top_out = a_top.
cross(b_top);
81 bottom_out = a_bottom.
cross(b_top) + a_top.
cross(b_bottom);
100 m_baseQuat(0, 0, 0, 1),
102 m_baseInertia(inertia),
104 m_fixedBase(fixedBase),
106 m_canSleep(canSleep),
109 m_linearDamping(0.04f),
110 m_angularDamping(0.04f),
112 m_maxAppliedImpulse(1000.f),
113 m_maxCoordinateVelocity(100.f),
114 m_hasSelfCollision(true),
115 m_isMultiDof(multiDof),
120 m_useGlobalVelocities(false),
121 m_internalNeedsJointFeedback(false)
148 const btVector3 &parentComToThisPivotOffset,
149 const btVector3 &thisPivotToThisComOffset,
150 bool disableParentCollision)
155 m_links[i].m_inertiaLocal = inertia;
157 m_links[i].m_zeroRotParentToThis = rotParentToThis;
158 m_links[i].m_dVector = thisPivotToThisComOffset;
159 m_links[i].m_eVector = parentComToThisPivotOffset;
165 if (disableParentCollision)
168 m_links[i].updateCacheMultiDof();
185 const btVector3 &parentComToThisPivotOffset,
186 const btVector3 &thisPivotToThisComOffset,
187 bool disableParentCollision)
196 m_links[i].m_inertiaLocal = inertia;
198 m_links[i].m_zeroRotParentToThis = rotParentToThis;
199 m_links[i].setAxisTop(0, 0., 0., 0.);
200 m_links[i].setAxisBottom(0, jointAxis);
201 m_links[i].m_eVector = parentComToThisPivotOffset;
202 m_links[i].m_dVector = thisPivotToThisComOffset;
203 m_links[i].m_cachedRotParentToThis = rotParentToThis;
208 m_links[i].m_jointPos[0] = 0.f;
209 m_links[i].m_jointTorque[0] = 0.f;
211 if (disableParentCollision)
215 m_links[i].updateCacheMultiDof();
229 const btVector3 &parentComToThisPivotOffset,
230 const btVector3 &thisPivotToThisComOffset,
231 bool disableParentCollision)
240 m_links[i].m_inertiaLocal = inertia;
242 m_links[i].m_zeroRotParentToThis = rotParentToThis;
243 m_links[i].setAxisTop(0, jointAxis);
244 m_links[i].setAxisBottom(0, jointAxis.
cross(thisPivotToThisComOffset));
245 m_links[i].m_dVector = thisPivotToThisComOffset;
246 m_links[i].m_eVector = parentComToThisPivotOffset;
251 m_links[i].m_jointPos[0] = 0.f;
252 m_links[i].m_jointTorque[0] = 0.f;
254 if (disableParentCollision)
258 m_links[i].updateCacheMultiDof();
273 const btVector3 &parentComToThisPivotOffset,
274 const btVector3 &thisPivotToThisComOffset,
275 bool disableParentCollision)
282 m_links[i].m_inertiaLocal = inertia;
284 m_links[i].m_zeroRotParentToThis = rotParentToThis;
285 m_links[i].m_dVector = thisPivotToThisComOffset;
286 m_links[i].m_eVector = parentComToThisPivotOffset;
291 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
292 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
293 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
294 m_links[i].setAxisBottom(0,
m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
295 m_links[i].setAxisBottom(1,
m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
296 m_links[i].setAxisBottom(2,
m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
301 if (disableParentCollision)
304 m_links[i].updateCacheMultiDof();
315 const btVector3 &parentComToThisComOffset,
316 bool disableParentCollision)
323 m_links[i].m_inertiaLocal = inertia;
325 m_links[i].m_zeroRotParentToThis = rotParentToThis;
326 m_links[i].m_dVector.setZero();
327 m_links[i].m_eVector = parentComToThisComOffset;
330 static btVector3 vecNonParallelToRotAxis(1, 0, 0);
331 if(rotationAxis.
normalized().
dot(vecNonParallelToRotAxis) > 0.999)
332 vecNonParallelToRotAxis.
setValue(0, 1, 0);
339 m_links[i].setAxisTop(0, n[0],n[1],n[2]);
340 m_links[i].setAxisTop(1,0,0,0);
341 m_links[i].setAxisTop(2,0,0,0);
342 m_links[i].setAxisBottom(0,0,0,0);
344 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
346 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
350 if (disableParentCollision)
353 m_links[i].updateCacheMultiDof();
381 return m_links[i].m_inertiaLocal;
386 return m_links[i].m_jointPos[0];
396 return &
m_links[i].m_jointPos[0];
406 return &
m_links[i].m_jointPos[0];
423 for(
int pos = 0; pos <
m_links[i].m_posVarCount; ++pos)
424 m_links[i].m_jointPos[pos] = q[pos];
426 m_links[i].updateCacheMultiDof();
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;
505 for (
int i = 0; i < num_links; ++i) {
506 const int parent =
m_links[i].m_parent;
510 omega[parent+1], vel[parent+1],
511 omega[i+1], vel[i+1]);
531 for (
int i = 0; i < num_links; ++i) {
532 result +=
m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
533 result += omega[i+1].dot(
m_links[i].m_inertiaLocal * omega[i+1]);
536 return 0.5f * result;
551 for (
int i = 0; i < num_links; ++i) {
552 rot_from_world[i+1] =
m_links[i].m_cachedRotParentToThis * rot_from_world[
m_links[i].m_parent+1];
553 result += (
quatRotate(rot_from_world[i+1].
inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
566 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
567 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
577 m_links[i].m_appliedForce.setValue(0, 0, 0);
578 m_links[i].m_appliedTorque.setValue(0, 0, 0);
592 m_links[i].m_appliedForce += f;
597 m_links[i].m_appliedTorque += t;
602 m_links[i].m_appliedConstraintForce += f;
607 m_links[i].m_appliedConstraintTorque += t;
614 m_links[i].m_jointTorque[0] += Q;
619 m_links[i].m_jointTorque[dof] += Q;
624 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
625 m_links[i].m_jointTorque[dof] = Q[dof];
630 return m_links[i].m_appliedForce;
635 return m_links[i].m_appliedTorque;
640 return m_links[i].m_jointTorque[0];
645 return &
m_links[i].m_jointTorque[0];
664 row1[0],row1[1],row1[2],
665 row2[0],row2[1],row2[2]);
669 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
676 bool isConstraintPass)
705 scratch_v.
resize(8*num_links + 6);
706 scratch_m.
resize(4*num_links + 4);
714 v_ptr += num_links * 2 + 2;
718 v_ptr += num_links * 2 + 2;
722 v_ptr += num_links * 2;
735 v_ptr += num_links * 2 + 2;
755 btScalar * joint_accel = output + 6;
765 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
776 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
779 btScalar linDampMult = 1., angDampMult = 1.;
780 zeroAccSpatFrc[0].
addVector(angDampMult *
m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
781 linDampMult *
m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
788 zeroAccSpatFrc[0].
addLinear(
m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
804 rot_from_world[0] = rot_from_parent[0];
807 for (
int i = 0; i < num_links; ++i) {
808 const int parent =
m_links[i].m_parent;
810 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
813 fromWorld.
m_rotMat = rot_from_world[i+1];
814 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
822 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
826 spatVel[i+1] += spatJointVel;
840 spatVel[i+1].
cross(spatJointVel, spatCoriolisAcc[i]);
845 btVector3 linkAppliedForce = isConstraintPass?
m_links[i].m_appliedConstraintForce :
m_links[i].m_appliedForce;
846 btVector3 linkAppliedTorque =isConstraintPass ?
m_links[i].m_appliedConstraintTorque :
m_links[i].m_appliedTorque;
848 zeroAccSpatFrc[i+1].
setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
853 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
855 zeroAccSpatFrc[i+1].m_topVec[0],
856 zeroAccSpatFrc[i+1].m_topVec[1],
857 zeroAccSpatFrc[i+1].m_topVec[2],
859 zeroAccSpatFrc[i+1].m_bottomVec[0],
860 zeroAccSpatFrc[i+1].m_bottomVec[1],
861 zeroAccSpatFrc[i+1].m_bottomVec[2]);
866 btScalar linDampMult = 1., angDampMult = 1.;
867 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()),
868 linDampMult *
m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
879 0,
m_links[i].m_inertiaLocal[1], 0,
880 0, 0,
m_links[i].m_inertiaLocal[2])
885 zeroAccSpatFrc[i+1].
addAngular(spatVel[i+1].getAngular().cross(
m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
887 zeroAccSpatFrc[i+1].
addLinear(
m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
908 for (
int i = num_links - 1; i >= 0; --i)
910 const int parent =
m_links[i].m_parent;
913 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
917 hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
919 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
920 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
921 - spatCoriolisAcc[i].
dot(hDof)
925 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
928 for(
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
931 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
941 invDi[0] = 1.0f / D[0];
947 static btMatrix3x3 D3x3; D3x3.
setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
951 for(
int row = 0; row < 3; ++row)
953 for(
int col = 0; col < 3; ++col)
955 invDi[row * 3 + col] = invD3x3[row][col];
968 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
970 spatForceVecTemps[dof].
setZero();
972 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
976 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
980 dyadTemp = spatInertia[i+1];
983 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
992 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
994 invD_times_Y[dof] = 0.f;
996 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
998 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1002 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1004 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1008 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1013 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1036 spatAcc[0] = -result;
1041 for (
int i = 0; i < num_links; ++i)
1049 const int parent =
m_links[i].m_parent;
1052 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1054 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1058 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1065 spatAcc[i+1] += spatCoriolisAcc[i];
1067 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1068 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1070 if (
m_links[i].m_jointFeedback)
1074 btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
1075 btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
1082 angularBotVec = angularBotVec - linearTopVec.
cross(
m_links[i].m_dVector);
1088 if (isConstraintPass)
1090 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec +=
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1091 m_links[i].m_jointFeedback->m_reactionForces.m_topVec +=
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1094 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec =
m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1095 m_links[i].m_jointFeedback->m_reactionForces.m_topVec =
m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1099 if (isConstraintPass)
1101 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1102 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1107 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1108 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1117 output[0] = omegadot_out[0];
1118 output[1] = omegadot_out[1];
1119 output[2] = omegadot_out[2];
1122 output[3] = vdot_out[0];
1123 output[4] = vdot_out[1];
1124 output[5] = vdot_out[2];
1146 if (!isConstraintPass)
1182 for (
int i = 0; i < num_links; ++i)
1184 const int parent =
m_links[i].m_parent;
1189 fromWorld.
m_rotMat = rot_from_world[i+1];
1192 fromParent.
transform(spatVel[parent+1], spatVel[i+1]);
1200 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1204 spatVel[i+1] += spatJointVel;
1244 scratch_r.
resize(2*num_links + 6);
1245 scratch_v.
resize(8*num_links + 6);
1246 scratch_m.
resize(4*num_links + 4);
1253 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
1254 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
1257 btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1258 btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1261 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
1262 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
1267 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
1268 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
1269 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
1279 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1280 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1283 btScalar * Y1 = r_ptr; r_ptr += num_links;
1287 btScalar * joint_accel = output + 6;
1297 vel_top_angular[0] = rot_from_parent[0] * base_omega;
1298 vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
1301 f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] =
btVector3(0,0,0);
1303 f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (
m_baseForce
1304 -
m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
1306 f_zero_acc_bottom_linear[0] =
1310 f_zero_acc_bottom_linear[0]+=vel_top_angular[0].
cross(
m_baseInertia * vel_top_angular[0] );
1312 f_zero_acc_bottom_linear[0] +=
m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
1318 inertia_top_left[0] =
btMatrix3x3(0,0,0,0,0,0,0,0,0);
1328 rot_from_world[0] = rot_from_parent[0];
1330 for (
int i = 0; i < num_links; ++i) {
1331 const int parent =
m_links[i].m_parent;
1335 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1338 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1339 vel_top_angular[parent+1], vel_bottom_linear[parent+1],
1340 vel_top_angular[i+1], vel_bottom_linear[i+1]);
1344 coriolis_bottom_linear[i] = vel_top_angular[i+1].
cross(vel_top_angular[i+1].cross(
m_links[i].m_cachedRVector))
1350 coriolis_top_angular[i] =
btVector3(0,0,0);
1359 f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (
m_links[i].m_appliedForce));
1360 f_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];
1362 f_zero_acc_bottom_linear[i+1] =
1363 - (rot_from_world[i+1] *
m_links[i].m_appliedTorque);
1366 f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].
cross(
m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
1369 f_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());
1372 inertia_top_left[i+1] =
btMatrix3x3(0,0,0,0,0,0,0,0,0);
1377 0,
m_links[i].m_inertiaLocal[1], 0,
1378 0, 0,
m_links[i].m_inertiaLocal[2]);
1384 for (
int i = num_links - 1; i >= 0; --i) {
1386 h_top[i] = inertia_top_left[i+1] *
m_links[i].getAxisTop(0) + inertia_top_right[i+1] *
m_links[i].getAxisBottom(0);
1387 h_bottom[i] = inertia_bottom_left[i+1] *
m_links[i].getAxisTop(0) + inertia_top_left[i+1].
transpose() *
m_links[i].getAxisBottom(0);
1388 btScalar val = SpatialDotProduct(
m_links[i].getAxisTop(0),
m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
1391 Y1[i] =
m_links[i].m_jointTorque[0]
1392 - SpatialDotProduct(
m_links[i].getAxisTop(0),
m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
1393 - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
1395 const int parent =
m_links[i].m_parent;
1399 const btScalar one_over_di = 1.0f / D[i];
1411 0, -
m_links[i].m_cachedRVector[2],
m_links[i].m_cachedRVector[1],
1412 m_links[i].m_cachedRVector[2], 0, -
m_links[i].m_cachedRVector[0],
1413 -
m_links[i].m_cachedRVector[1],
m_links[i].m_cachedRVector[0], 0);
1415 inertia_top_left[parent+1] += rot_from_parent[i+1].
transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
1416 inertia_top_right[parent+1] += rot_from_parent[i+1].
transpose() * TR * rot_from_parent[i+1];
1417 inertia_bottom_left[parent+1] += rot_from_parent[i+1].
transpose() *
1418 (r_cross * (TL - TR * r_cross) + BL - TL.
transpose() * r_cross) * rot_from_parent[i+1];
1422 btVector3 in_top, in_bottom, out_top, out_bottom;
1423 const btScalar Y_over_D = Y1[i] * one_over_di;
1424 in_top = f_zero_acc_top_angular[i+1]
1425 + inertia_top_left[i+1] * coriolis_top_angular[i]
1426 + inertia_top_right[i+1] * coriolis_bottom_linear[i]
1427 + Y_over_D * h_top[i];
1428 in_bottom = f_zero_acc_bottom_linear[i+1]
1429 + inertia_bottom_left[i+1] * coriolis_top_angular[i]
1430 + inertia_top_left[i+1].
transpose() * coriolis_bottom_linear[i]
1431 + Y_over_D * h_bottom[i];
1432 InverseSpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1433 in_top, in_bottom, out_top, out_bottom);
1434 f_zero_acc_top_angular[parent+1] += out_top;
1435 f_zero_acc_bottom_linear[parent+1] += out_bottom;
1444 accel_top[0] = accel_bottom[0] =
btVector3(0,0,0);
1463 btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
1464 btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
1469 for (
int i = 0; i < 3; ++i) {
1470 accel_top[0][i] = -result[i];
1471 accel_bottom[0][i] = -result[i+3];
1477 for (
int i = 0; i < num_links; ++i)
1482 const int parent =
m_links[i].m_parent;
1483 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1484 accel_top[parent+1], accel_bottom[parent+1],
1485 accel_top[i+1], accel_bottom[i+1]);
1488 joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
1489 accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] *
m_links[i].getAxisTop(0);
1490 accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] *
m_links[i].getAxisBottom(0);
1495 output[0] = omegadot_out[0];
1496 output[1] = omegadot_out[1];
1497 output[2] = omegadot_out[2];
1500 output[3] = vdot_out[0];
1501 output[4] = vdot_out[1];
1502 output[5] = vdot_out[2];
1552 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1561 btVector3 vtop = invI_upper_left*rhs_top;
1563 tmp = invIupper_right * rhs_bot;
1565 btVector3 vbot = invI_lower_left*rhs_top;
1566 tmp = invI_lower_right * rhs_bot;
1568 result[0] = vtop[0];
1569 result[1] = vtop[1];
1570 result[2] = vtop[2];
1571 result[3] = vbot[0];
1572 result[4] = vbot[1];
1573 result[5] = vbot[2];
1596 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1620 for (
int row = 0; row < rowsA; row++)
1622 for (
int col = 0; col < colsB; col++)
1624 pC[row * colsB + col] = 0.f;
1625 for (
int inner = 0; inner < rowsB; inner++)
1627 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1642 scratch_v.
resize(4*num_links + 4);
1649 v_ptr += num_links * 2 + 2;
1658 v_ptr += num_links * 2 + 2;
1683 fromParent.
m_rotMat = rot_from_parent[0];
1686 for (
int i = 0; i < num_links; ++i)
1688 zeroAccSpatFrc[i+1].
setZero();
1693 for (
int i = num_links - 1; i >= 0; --i)
1695 const int parent =
m_links[i].m_parent;
1698 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1700 Y[
m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
1701 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1705 btVector3 in_top, in_bottom, out_top, out_bottom;
1708 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1710 invD_times_Y[dof] = 0.f;
1712 for(
int dof2 = 0; dof2 <
m_links[i].m_dofCount; ++dof2)
1714 invD_times_Y[dof] += invDi[dof *
m_links[i].m_dofCount + dof2] * Y[
m_links[i].m_dofOffset + dof2];
1719 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
1721 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1725 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1731 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1735 btScalar * joint_accel = output + 6;
1748 spatAcc[0] = -result;
1753 for (
int i = 0; i < num_links; ++i)
1755 const int parent =
m_links[i].m_parent;
1758 fromParent.
transform(spatAcc[parent+1], spatAcc[i+1]);
1760 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1764 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].
dot(hDof);
1770 for(
int dof = 0; dof <
m_links[i].m_dofCount; ++dof)
1771 spatAcc[i+1] +=
m_links[i].m_axes[dof] * joint_accel[
m_links[i].m_dofOffset + dof];
1777 output[0] = omegadot_out[0];
1778 output[1] = omegadot_out[1];
1779 output[2] = omegadot_out[2];
1783 output[3] = vdot_out[0];
1784 output[4] = vdot_out[1];
1785 output[5] = vdot_out[2];
1802 scratch_r.
resize(num_links);
1803 scratch_v.
resize(4*num_links + 4);
1805 btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
1809 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1810 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1818 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1819 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1822 btScalar * Y = r_ptr; r_ptr += num_links;
1825 btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.
size());
1833 btVector3 input_force(force[3],force[4],force[5]);
1834 btVector3 input_torque(force[0],force[1],force[2]);
1840 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] =
btVector3(0,0,0);
1843 zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
1844 zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
1846 for (
int i = 0; i < num_links; ++i)
1848 zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] =
btVector3(0,0,0);
1852 for (
int i = num_links - 1; i >= 0; --i)
1855 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]);
1856 Y[i] += force[6 + i];
1858 const int parent =
m_links[i].m_parent;
1861 btVector3 in_top, in_bottom, out_top, out_bottom;
1862 const btScalar Y_over_D = Y[i] / D[i];
1863 in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
1864 in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
1865 InverseSpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1866 in_top, in_bottom, out_top, out_bottom);
1867 zero_acc_top_angular[parent+1] += out_top;
1868 zero_acc_bottom_linear[parent+1] += out_bottom;
1872 btScalar * joint_accel = output + 6;
1877 accel_top[0] = accel_bottom[0] =
btVector3(0,0,0);
1880 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
1881 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
1887 for (
int i = 0; i < 3; ++i) {
1888 accel_top[0][i] = -result[i];
1889 accel_bottom[0][i] = -result[i+3];
1895 for (
int i = 0; i < num_links; ++i) {
1896 const int parent =
m_links[i].m_parent;
1897 SpatialTransform(rot_from_parent[i+1],
m_links[i].m_cachedRVector,
1898 accel_top[parent+1], accel_bottom[parent+1],
1899 accel_top[i+1], accel_bottom[i+1]);
1900 btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
1901 joint_accel[i] = Y_minus_hT_a / D[i];
1902 accel_top[i+1] += joint_accel[i] *
m_links[i].getAxisTop(0);
1903 accel_bottom[i+1] += joint_accel[i] *
m_links[i].getAxisBottom(0);
1908 omegadot_out = rot_from_parent[0].
transpose() * accel_top[0];
1909 output[0] = omegadot_out[0];
1910 output[1] = omegadot_out[1];
1911 output[2] = omegadot_out[2];
1914 vdot_out = rot_from_parent[0].
transpose() * accel_bottom[0];
1916 output[3] = vdot_out[0];
1917 output[4] = vdot_out[1];
1918 output[5] = vdot_out[2];
1952 const btScalar omega_times_dt = omega_norm * dt;
1953 const btScalar SMALL_ROTATION_ANGLE = 0.02f;
1954 if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
1956 const btScalar xsq = omega_times_dt * omega_times_dt;
1957 const btScalar sin_term = dt * (xsq / 48.0f - 0.5f);
1958 const btScalar cos_term = 1.0f - xsq / 8.0f;
1970 for (
int i = 0; i < num_links; ++i)
1973 m_links[i].m_jointPos[0] += dt * jointVel;
1988 pBasePos[0] += dt * pBaseVel[0];
1989 pBasePos[1] += dt * pBaseVel[1];
1990 pBasePos[2] += dt * pBaseVel[2];
2020 axis = angvel*(
btScalar(0.5)*dt-(dt*dt*dt)*(
btScalar(0.020833333333))*fAngle*fAngle );
2044 static btQuaternion baseQuat; baseQuat.
setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
2045 static btVector3 baseOmega; baseOmega.
setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
2046 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
2047 pBaseQuat[0] = baseQuat.
x();
2048 pBaseQuat[1] = baseQuat.
y();
2049 pBaseQuat[2] = baseQuat.
z();
2050 pBaseQuat[3] = baseQuat.
w();
2063 for (
int i = 0; i < num_links; ++i)
2068 switch(
m_links[i].m_jointType)
2074 pJointPos[0] += dt * jointVel;
2079 static btVector3 jointVel; jointVel.
setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
2080 static btQuaternion jointOri; jointOri.
setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
2081 pQuatUpdateFun(jointVel, jointOri,
false, dt);
2082 pJointPos[0] = jointOri.
x(); pJointPos[1] = jointOri.
y(); pJointPos[2] = jointOri.
z(); pJointPos[3] = jointOri.
w();
2091 pJointPos[1] +=
m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
2092 pJointPos[2] +=
m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
2102 m_links[i].updateCacheMultiDof(pq);
2105 pq +=
m_links[i].m_posVarCount;
2123 scratch_v.
resize(3*num_links + 3);
2124 scratch_m.
resize(num_links + 1);
2127 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
2128 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
2129 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
2132 scratch_r.
resize(m_dofCount);
2133 btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
2138 const btVector3 &normal_lin_world = normal_lin;
2139 const btVector3 &normal_ang_world = normal_ang;
2145 omega_coeffs_world = p_minus_com_world.
cross(normal_lin_world);
2146 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
2147 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
2148 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
2150 jac[3] = normal_lin_world[0];
2151 jac[4] = normal_lin_world[1];
2152 jac[5] = normal_lin_world[2];
2155 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
2156 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
2157 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2166 if (num_links > 0 && link > -1) {
2173 for (
int i = 0; i < num_links; ++i) {
2176 const int parent =
m_links[i].m_parent;
2178 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2180 n_local_lin[i+1] = mtx * n_local_lin[parent+1];
2181 n_local_ang[i+1] = mtx * n_local_ang[parent+1];
2182 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] -
m_links[i].m_cachedRVector;
2185 switch(
m_links[i].m_jointType)
2189 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));
2190 results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2195 results[
m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
2200 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));
2201 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));
2202 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));
2204 results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2205 results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
2206 results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
2212 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]));
2213 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
2214 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
2229 for(
int dof = 0; dof <
m_links[link].m_dofCount; ++dof)
2231 jac[6 +
m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
2235 link =
m_links[link].m_parent;
2251 scratch_v.
resize(2*num_links + 2);
2252 scratch_m.
resize(num_links + 1);
2255 btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
2256 btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
2259 scratch_r.
resize(num_links);
2260 btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
2268 p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
2269 n_local[0] = rot_from_world[0] * normal;
2274 for (
int i=0;i<6;i++)
2282 omega_coeffs = p_minus_com_world.
cross(normal);
2283 jac[0] = omega_coeffs[0];
2284 jac[1] = omega_coeffs[1];
2285 jac[2] = omega_coeffs[2];
2292 for (
int i = 6; i < 6 + num_links; ++i) {
2297 if (num_links > 0 && link > -1) {
2304 for (
int i = 0; i < num_links; ++i) {
2307 const int parent =
m_links[i].m_parent;
2309 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2311 n_local[i+1] = mtx * n_local[parent+1];
2312 p_minus_com[i+1] = mtx * p_minus_com[parent+1] -
m_links[i].m_cachedRVector;
2316 results[i] = n_local[i+1].dot(
m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) +
m_links[i].getAxisBottom(0) );
2318 results[i] = n_local[i+1].dot(
m_links[i].getAxisBottom(0) );
2326 jac[6 + link] = results[link];
2328 link =
m_links[link].m_parent;
2364 for (
int i = 0; i < 6 + num_links; ++i)
2369 if (motion < SLEEP_EPSILON) {
2392 for (
int i = 0; i < num_links; ++i)
2399 world_to_local.
resize(nLinks+1);
2400 local_origin.
resize(nLinks+1);
2417 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2440 btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
2470 btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2497 if (mbd->m_baseName)
2503 if (mbd->m_numLinks)
2506 int numElem = mbd->m_numLinks;
2509 for (
int i=0;i<numElem;i++,memPtr++)
2533 for (
int posvar = 0; posvar < numPosVar;posvar++)
2542 if (memPtr->m_linkName)
2550 if (memPtr->m_jointName)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
btQuaternion m_zeroRotParentToThis
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void clearConstraintForces()
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)
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
const btMultibodyLink & getLink(int index) const
const btVector3 & getAxisBottom(int dof) const
const btVector3 getBaseVel() const
void serialize(struct btQuaternionData &dataOut) const
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
#define btMultiBodyLinkDataName
btScalar btSin(btScalar x)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
#define vecMulVecTranspose(v0, v1Transposed)
eFeatherstoneJointType m_jointType
const btScalar & z() const
Return the z value.
class btMultiBodyLinkCollider * m_collider
btTransform getBaseWorldTransform() const
bool gDisableDeactivation
const btVector3 & getLinkForce(int i) const
virtual int calculateSerializeBufferSize() const
virtual void * getUniquePointer(void *oldPtr)=0
btScalar getBaseMass() const
bool gJointFeedbackInWorldSpace
todo: determine if we need these options. If so, make a proper API, otherwise delete those globals ...
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btVector3 localDirToWorld(int i, const btVector3 &vec) const
void addLinkConstraintForce(int i, const btVector3 &f)
btMatrix3x3 m_cachedInertiaLowerRight
btScalar * getJointVelMultiDof(int i)
void addLinkForce(int i, const btVector3 &f)
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
void addLinear(const btVector3 &linear)
btScalar getJointPos(int i) const
bool gJointFeedbackInJointFrame
const btScalar & y() const
Return the y value.
const btVector3 & getAngular() const
void setJointPosMultiDof(int i, btScalar *q)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
btScalar dot(const btSpatialForceVector &b) const
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btVector3 m_baseConstraintTorque
#define btCollisionObjectData
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
const btScalar & w() const
Return the w value.
btAlignedObjectArray< btScalar > m_deltaV
btScalar dot(const btVector3 &v) const
Return the dot product.
void applyDeltaVee(const btScalar *delta_vee)
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
void clearForcesAndTorques()
const btScalar & x() const
Return the x value.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
btVector3 getAngularMomentum() const
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool multiDof=false)
void addAngular(const btVector3 &angular)
void addLinkConstraintTorque(int i, const btVector3 &t)
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btMatrix3x3 m_cachedInertiaTopLeft
const btVector3 & getBaseInertia() const
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)
btScalar m_jointTorque[6]
int size() const
return the number of elements in the array
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.
const btVector3 & getLinkTorque(int i) const
void addLinkTorque(int i, const btVector3 &t)
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 compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
void setLinear(const btVector3 &linear)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
void serialize(struct btVector3Data &dataOut) const
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 solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, float result[6]) const
const btVector3 & getRVector(int i) const
btScalar length() const
Return the length of the vector.
void setJointVel(int i, btScalar qdot)
void setAngular(const btVector3 &angular)
void setJointPos(int i, btScalar q)
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
void setWorldTransform(const btTransform &worldTrans)
btScalar norm() const
Return the norm (length) of the vector.
const btScalar & y() const
Return the y value.
void checkMotionAndSleepIfRequired(btScalar timestep)
btVector3 m_baseConstraintForce
btVector3 can be used to represent 3D points and vectors.
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
virtual const char * findNameForPointer(const void *ptr) const =0
btScalar getLinkMass(int i) const
btAlignedObjectArray< btScalar > m_realBuf
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void updateLinksDofOffsets()
#define btMultiBodyDataName
btVector3 getBaseOmega() const
btVector3 normalized() const
Return a normalized version of this vector.
bool m_useGlobalVelocities
virtual void serializeName(const char *ptr)=0
void resize(int newsize, const T &fillData=T())
const btVector3 & getLinear() const
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
const btQuaternion & getParentToLocalRot(int i) const
const btVector3 & getAngular() const
btMatrix3x3 m_bottomLeftMat
void setVector(const btVector3 &angular, const btVector3 &linear)
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
btAlignedObjectArray< btVector3 > m_vectorBuf
btTransform m_cachedWorldTransform
const btScalar & x() const
Return the x value.
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
void setJointVelMultiDof(int i, btScalar *qdot)
const btMultiBodyLinkCollider * getBaseCollider() const
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
const btVector3 & getLinkInertia(int i) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
void addVector(const btVector3 &angular, const btVector3 &linear)
btScalar getJointTorque(int i) const
const btQuaternion & getWorldToBaseRot() const
void stepPositions(btScalar dt)
int getParent(int link_num) const
const btVector3 & getBasePos() const
const btVector3 & getAxisTop(int dof) const
btVector3 localPosToWorld(int i, const btVector3 &vec) const
btAlignedObjectArray< btMultibodyLink > m_links
btScalar * getJointTorqueMultiDof(int i)
#define btMultiBodyLinkData
btMatrix3x3 m_cachedInertiaTopRight
btScalar * getJointPosMultiDof(int i)
virtual btChunk * allocate(size_t size, int numElements)=0
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
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 getJointVel(int i) const
btScalar getKineticEnergy() const
const btScalar & z() const
Return the z value.
const btVector3 & getLinear() const