47 top_out = rotation_matrix * top_in;
48 bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
56 void InverseSpatialTransform(
const btMatrix3x3 &rotation_matrix,
63 top_out = rotation_matrix.transpose() * top_in;
64 bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
72 return a_bottom.dot(b_top) + a_top.dot(b_bottom);
75 void SpatialCrossProduct(
const btVector3 &a_top,
82 top_out = a_top.cross(b_top);
83 bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
102 m_baseQuat(0, 0, 0, 1),
103 m_basePos_interpolate(0, 0, 0),
104 m_baseQuat_interpolate(0, 0, 0, 1),
106 m_baseInertia(inertia),
108 m_fixedBase(fixedBase),
110 m_canSleep(canSleep),
121 m_maxCoordinateVelocity(100.f),
122 m_hasSelfCollision(true),
127 m_useGlobalVelocities(false),
128 m_internalNeedsJointFeedback(false)
130 m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
131 m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
132 m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
133 m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
134 m_cachedInertiaValid =
false;
136 m_links.resize(n_links);
137 m_matrixBuf.
resize(n_links + 1);
139 m_baseForce.setValue(0, 0, 0);
140 m_baseTorque.setValue(0, 0, 0);
155 const btVector3 &parentComToThisPivotOffset,
156 const btVector3 &thisPivotToThisComOffset,
bool )
158 m_links[i].m_mass = mass;
159 m_links[i].m_inertiaLocal = inertia;
160 m_links[i].m_parent = parent;
161 m_links[i].setAxisTop(0, 0., 0., 0.);
162 m_links[i].setAxisBottom(0,
btVector3(0, 0, 0));
163 m_links[i].m_zeroRotParentToThis = rotParentToThis;
164 m_links[i].m_dVector = thisPivotToThisComOffset;
165 m_links[i].m_eVector = parentComToThisPivotOffset;
168 m_links[i].m_dofCount = 0;
169 m_links[i].m_posVarCount = 0;
173 m_links[i].updateCacheMultiDof();
175 updateLinksDofOffsets();
184 const btVector3 &parentComToThisPivotOffset,
185 const btVector3 &thisPivotToThisComOffset,
186 bool disableParentCollision)
191 m_links[i].m_mass = mass;
192 m_links[i].m_inertiaLocal = inertia;
193 m_links[i].m_parent = parent;
194 m_links[i].m_zeroRotParentToThis = rotParentToThis;
195 m_links[i].setAxisTop(0, 0., 0., 0.);
196 m_links[i].setAxisBottom(0, jointAxis);
197 m_links[i].m_eVector = parentComToThisPivotOffset;
198 m_links[i].m_dVector = thisPivotToThisComOffset;
199 m_links[i].m_cachedRotParentToThis = rotParentToThis;
202 m_links[i].m_dofCount = 1;
203 m_links[i].m_posVarCount = 1;
204 m_links[i].m_jointPos[0] = 0.f;
205 m_links[i].m_jointTorque[0] = 0.f;
207 if (disableParentCollision)
211 m_links[i].updateCacheMultiDof();
213 updateLinksDofOffsets();
222 const btVector3 &parentComToThisPivotOffset,
223 const btVector3 &thisPivotToThisComOffset,
224 bool disableParentCollision)
229 m_links[i].m_mass = mass;
230 m_links[i].m_inertiaLocal = inertia;
231 m_links[i].m_parent = parent;
232 m_links[i].m_zeroRotParentToThis = rotParentToThis;
233 m_links[i].setAxisTop(0, jointAxis);
234 m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
235 m_links[i].m_dVector = thisPivotToThisComOffset;
236 m_links[i].m_eVector = parentComToThisPivotOffset;
239 m_links[i].m_dofCount = 1;
240 m_links[i].m_posVarCount = 1;
241 m_links[i].m_jointPos[0] = 0.f;
242 m_links[i].m_jointTorque[0] = 0.f;
244 if (disableParentCollision)
247 m_links[i].updateCacheMultiDof();
249 updateLinksDofOffsets();
257 const btVector3 &parentComToThisPivotOffset,
258 const btVector3 &thisPivotToThisComOffset,
259 bool disableParentCollision)
264 m_links[i].m_mass = mass;
265 m_links[i].m_inertiaLocal = inertia;
266 m_links[i].m_parent = parent;
267 m_links[i].m_zeroRotParentToThis = rotParentToThis;
268 m_links[i].m_dVector = thisPivotToThisComOffset;
269 m_links[i].m_eVector = parentComToThisPivotOffset;
272 m_links[i].m_dofCount = 3;
273 m_links[i].m_posVarCount = 4;
274 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
275 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
276 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
277 m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).
cross(thisPivotToThisComOffset));
278 m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).
cross(thisPivotToThisComOffset));
279 m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).
cross(thisPivotToThisComOffset));
280 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
281 m_links[i].m_jointPos[3] = 1.f;
282 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
284 if (disableParentCollision)
287 m_links[i].updateCacheMultiDof();
289 updateLinksDofOffsets();
298 const btVector3 &parentComToThisComOffset,
299 bool disableParentCollision)
304 m_links[i].m_mass = mass;
305 m_links[i].m_inertiaLocal = inertia;
306 m_links[i].m_parent = parent;
307 m_links[i].m_zeroRotParentToThis = rotParentToThis;
308 m_links[i].m_dVector.setZero();
309 m_links[i].m_eVector = parentComToThisComOffset;
312 btVector3 vecNonParallelToRotAxis(1, 0, 0);
313 if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
314 vecNonParallelToRotAxis.setValue(0, 1, 0);
318 m_links[i].m_dofCount = 3;
319 m_links[i].m_posVarCount = 3;
321 m_links[i].setAxisTop(0, n[0], n[1], n[2]);
322 m_links[i].setAxisTop(1, 0, 0, 0);
323 m_links[i].setAxisTop(2, 0, 0, 0);
324 m_links[i].setAxisBottom(0, 0, 0, 0);
325 btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
326 m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
327 cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
328 m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
329 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
330 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
332 if (disableParentCollision)
335 m_links[i].updateCacheMultiDof();
337 updateLinksDofOffsets();
339 m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).
normalized());
340 m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).
normalized());
346 m_deltaV.
resize(6 + m_dofCount);
348 m_splitV.
resize(6 + m_dofCount);
349 m_realBuf.
resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount);
350 m_vectorBuf.
resize(2 * m_dofCount);
351 m_matrixBuf.
resize(m_links.size() + 1);
352 for (
int i = 0; i < m_vectorBuf.
size(); i++)
354 m_vectorBuf[i].setValue(0, 0, 0);
356 updateLinksDofOffsets();
361 return m_links[link_num].m_parent;
366 return m_links[i].m_mass;
371 return m_links[i].m_inertiaLocal;
376 return m_links[i].m_jointPos[0];
381 return m_realBuf[6 + m_links[i].m_dofOffset];
386 return &m_links[i].m_jointPos[0];
391 return &m_realBuf[6 + m_links[i].m_dofOffset];
396 return &m_links[i].m_jointPos[0];
401 return &m_realBuf[6 + m_links[i].m_dofOffset];
406 m_links[i].m_jointPos[0] = q;
407 m_links[i].updateCacheMultiDof();
413 for (
int pos = 0;
pos < m_links[i].m_posVarCount; ++
pos)
416 m_links[i].updateCacheMultiDof();
421 for (
int pos = 0;
pos < m_links[i].m_posVarCount; ++
pos)
424 m_links[i].updateCacheMultiDof();
431 m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
436 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
437 m_realBuf[6 + m_links[i].m_dofOffset + dof] = (
btScalar)qdot[dof];
442 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
443 m_realBuf[6 + m_links[i].m_dofOffset + dof] = (
btScalar)qdot[dof];
448 return m_links[i].m_cachedRVector;
453 return m_links[i].m_cachedRotParentToThis;
458 return m_links[i].m_cachedRVector_interpolate;
463 return m_links[i].m_cachedRotParentToThis_interpolate;
470 if ((i < -1) || (i >= m_links.size()))
495 if ((i < -1) || (i >= m_links.size()))
516 if ((i < -1) || (i >= m_links.size()))
535 if ((i < -1) || (i >= m_links.size()))
556 result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
568 for (
int i = 0; i < num_links; ++i)
575 omega[parent + 1], vel[parent + 1],
576 omega[i + 1], vel[i + 1]);
580 for (
int dof = 0; dof < link.
m_dofCount; ++dof)
582 omega[i + 1] += jointVel[dof] * link.
getAxisTop(dof);
591 m_baseConstraintForce.setValue(0, 0, 0);
592 m_baseConstraintTorque.setValue(0, 0, 0);
596 m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
597 m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
602 m_baseForce.setValue(0, 0, 0);
603 m_baseTorque.setValue(0, 0, 0);
607 m_links[i].m_appliedForce.setValue(0, 0, 0);
608 m_links[i].m_appliedTorque.setValue(0, 0, 0);
609 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
622 m_links[i].m_appliedForce += f;
627 m_links[i].m_appliedTorque +=
t;
632 m_links[i].m_appliedConstraintForce += f;
637 m_links[i].m_appliedConstraintTorque +=
t;
642 m_links[i].m_jointTorque[0] += Q;
647 m_links[i].m_jointTorque[dof] += Q;
652 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
653 m_links[i].m_jointTorque[dof] = Q[dof];
658 return m_links[i].m_appliedForce;
663 return m_links[i].m_appliedTorque;
668 return m_links[i].m_jointTorque[0];
673 return &m_links[i].m_jointTorque[0];
692 row1[0], row1[1], row1[2],
693 row2[0], row2[1], row2[2]);
697 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
704 bool isConstraintPass,
705 bool jointFeedbackInWorldSpace,
706 bool jointFeedbackInJointFrame)
722 m_internalNeedsJointFeedback =
false;
738 scratch_r.
resize(2 * m_dofCount + 7);
739 scratch_v.
resize(8 * num_links + 6);
740 scratch_m.
resize(4 * num_links + 4);
748 v_ptr += num_links * 2 + 2;
752 v_ptr += num_links * 2 + 2;
756 v_ptr += num_links * 2;
769 v_ptr += num_links * 2 + 2;
772 btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
799 spatVel[0].
setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
807 const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
808 const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
810 zeroAccSpatFrc[0].
setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
813 const btScalar linDampMult = 1., angDampMult = 1.;
814 zeroAccSpatFrc[0].
addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().
safeNorm()),
815 linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().
safeNorm()));
820 zeroAccSpatFrc[0].
addAngular(spatVel[0].getAngular().
cross(m_baseInertia * spatVel[0].getAngular()));
822 zeroAccSpatFrc[0].
addLinear(m_baseMass * spatVel[0].getAngular().
cross(spatVel[0].getLinear()));
833 0, m_baseInertia[1], 0,
834 0, 0, m_baseInertia[2]));
836 rot_from_world[0] = rot_from_parent[0];
839 for (
int i = 0; i < num_links; ++i)
841 const int parent = m_links[i].m_parent;
842 rot_from_parent[i + 1] =
btMatrix3x3(m_links[i].m_cachedRotParentToThis);
843 rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
845 fromParent.
m_rotMat = rot_from_parent[i + 1];
846 fromParent.
m_trnVec = m_links[i].m_cachedRVector;
847 fromWorld.
m_rotMat = rot_from_world[i + 1];
848 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
852 if (!m_useGlobalVelocities)
856 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
860 spatVel[i + 1] += spatJointVel;
873 spatVel[i + 1].
cross(spatJointVel, spatCoriolisAcc[i]);
878 btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
879 btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
881 zeroAccSpatFrc[i + 1].
setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
886 b3Printf(
"stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
888 zeroAccSpatFrc[i+1].m_topVec[0],
889 zeroAccSpatFrc[i+1].m_topVec[1],
890 zeroAccSpatFrc[i+1].m_topVec[2],
892 zeroAccSpatFrc[i+1].m_bottomVec[0],
893 zeroAccSpatFrc[i+1].m_bottomVec[1],
894 zeroAccSpatFrc[i+1].m_bottomVec[2]);
899 btScalar linDampMult = 1., angDampMult = 1.;
900 zeroAccSpatFrc[i + 1].
addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().
safeNorm()),
901 linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().
safeNorm()));
908 0, m_links[i].m_mass, 0,
909 0, 0, m_links[i].m_mass),
912 0, m_links[i].m_inertiaLocal[1], 0,
913 0, 0, m_links[i].m_inertiaLocal[2]));
917 zeroAccSpatFrc[i + 1].
addAngular(spatVel[i + 1].getAngular().
cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
919 zeroAccSpatFrc[i + 1].
addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().
cross(spatVel[i + 1].getLinear()));
938 for (
int i = num_links - 1; i >= 0; --i)
940 const int parent = m_links[i].m_parent;
941 fromParent.
m_rotMat = rot_from_parent[i + 1];
942 fromParent.
m_trnVec = m_links[i].m_cachedRVector;
944 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
948 hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
950 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].
dot(hDof);
952 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
954 btScalar *D_row = &
D[dof * m_links[i].m_dofCount];
955 for (
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
958 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
962 btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
963 switch (m_links[i].m_jointType)
970 invDi[0] = 1.0f /
D[0];
985 for (
int row = 0; row < 3; ++row)
989 invDi[row * 3 +
col] = invD3x3[row][
col];
1001 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1003 spatForceVecTemps[dof].
setZero();
1005 for (
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1009 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
1013 dyadTemp = spatInertia[i + 1];
1016 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1025 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1027 invD_times_Y[dof] = 0.f;
1029 for (
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1031 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] *
Y[m_links[i].m_dofOffset + dof2];
1035 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1037 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1041 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1046 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1060 m_cachedInertiaValid =
true;
1064 m_cachedInertiaLowerRight = spatInertia[0].
m_topLeftMat.transpose();
1067 solveImatrix(zeroAccSpatFrc[0],
result);
1072 for (
int i = 0; i < num_links; ++i)
1080 const int parent = m_links[i].m_parent;
1081 fromParent.
m_rotMat = rot_from_parent[i + 1];
1082 fromParent.
m_trnVec = m_links[i].m_cachedRVector;
1084 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1086 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1090 Y_minus_hT_a[dof] =
Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1093 btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1095 mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1097 spatAcc[i + 1] += spatCoriolisAcc[i];
1099 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1100 spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1104 m_internalNeedsJointFeedback =
true;
1106 btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1107 btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1109 if (jointFeedbackInJointFrame)
1114 angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
1117 if (jointFeedbackInWorldSpace)
1119 if (isConstraintPass)
1121 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1122 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1126 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1127 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1132 if (isConstraintPass)
1134 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1135 m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1139 m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1140 m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1148 output[0] = omegadot_out[0];
1149 output[1] = omegadot_out[1];
1150 output[2] = omegadot_out[2];
1152 const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].
getLinear() + spatVel[0].
getAngular().cross(spatVel[0].getLinear()));
1177 if (!isConstraintPass)
1210 if (m_useGlobalVelocities)
1212 for (
int i = 0; i < num_links; ++i)
1214 const int parent = m_links[i].m_parent;
1218 fromParent.
m_rotMat = rot_from_parent[i + 1];
1219 fromParent.
m_trnVec = m_links[i].m_cachedRVector;
1220 fromWorld.
m_rotMat = rot_from_world[i + 1];
1223 fromParent.
transform(spatVel[parent + 1], spatVel[i + 1]);
1231 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1235 spatVel[i + 1] += spatJointVel;
1253 result[0] = rhs_bot[0] / m_baseInertia[0];
1254 result[1] = rhs_bot[1] / m_baseInertia[1];
1255 result[2] = rhs_bot[2] / m_baseInertia[2];
1265 result[3] = rhs_top[0] / m_baseMass;
1266 result[4] = rhs_top[1] / m_baseMass;
1267 result[5] = rhs_top[2] / m_baseMass;
1278 if (!m_cachedInertiaValid)
1280 for (
int i = 0; i < 6; i++)
1288 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
1289 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1290 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).
inverse();
1291 tmp = invIupper_right * m_cachedInertiaLowerRight;
1294 tmp = m_cachedInertiaTopLeft * invI_upper_left;
1302 btVector3 vtop = invI_upper_left * rhs_top;
1304 tmp = invIupper_right * rhs_bot;
1306 btVector3 vbot = invI_lower_left * rhs_top;
1307 tmp = invI_lower_right * rhs_bot;
1346 if (!m_cachedInertiaValid)
1353 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse() * -1.f;
1354 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1355 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).
inverse();
1356 tmp = invIupper_right * m_cachedInertiaLowerRight;
1359 tmp = m_cachedInertiaTopLeft * invI_upper_left;
1374 result.setVector(vtop, vbot);
1379 void btMultiBody::mulMatrix(
btScalar *pA,
btScalar *pB,
int rowsA,
int colsA,
int rowsB,
int colsB,
btScalar *pC)
const
1381 for (
int row = 0; row < rowsA; row++)
1385 pC[row * colsB +
col] = 0.f;
1386 for (
int inner = 0; inner < rowsB; inner++)
1388 pC[row * colsB +
col] += pA[row * colsA + inner] * pB[
col + inner * colsB];
1401 scratch_r.
resize(m_dofCount);
1402 scratch_v.
resize(4 * num_links + 4);
1404 btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;
1409 v_ptr += num_links * 2 + 2;
1412 const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
1418 v_ptr += num_links * 2 + 2;
1421 const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1444 fromParent.
m_rotMat = rot_from_parent[0];
1447 for (
int i = 0; i < num_links; ++i)
1449 zeroAccSpatFrc[i + 1].
setZero();
1454 for (
int i = num_links - 1; i >= 0; --i)
1456 const int parent = m_links[i].m_parent;
1457 fromParent.
m_rotMat = rot_from_parent[i + 1];
1458 fromParent.
m_trnVec = m_links[i].m_cachedRVector;
1460 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1462 Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]);
1465 btVector3 in_top, in_bottom, out_top, out_bottom;
1466 const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1468 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1470 invD_times_Y[dof] = 0.f;
1472 for (
int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1474 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] *
Y[m_links[i].m_dofOffset + dof2];
1479 spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1481 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1485 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1490 zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1505 solveImatrix(zeroAccSpatFrc[0],
result);
1510 for (
int i = 0; i < num_links; ++i)
1512 const int parent = m_links[i].m_parent;
1513 fromParent.
m_rotMat = rot_from_parent[i + 1];
1514 fromParent.
m_trnVec = m_links[i].m_cachedRVector;
1516 fromParent.
transform(spatAcc[parent + 1], spatAcc[i + 1]);
1518 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1522 Y_minus_hT_a[dof] =
Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].
dot(hDof);
1525 const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1526 mulMatrix(
const_cast<btScalar *
>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1528 for (
int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1529 spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1534 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].
getAngular();
1535 output[0] = omegadot_out[0];
1536 output[1] = omegadot_out[1];
1537 output[2] = omegadot_out[2];
1540 vdot_out = rot_from_parent[0].transpose() * spatAcc[0].
getLinear();
1560 btScalar *pBaseVel = &m_realBuf[3];
1563 for (
int i = 0; i < 3; ++i)
1565 m_basePos_interpolate[i] = m_basePos[i];
1567 pBasePos = m_basePos_interpolate;
1569 pBasePos[0] += dt * pBaseVel[0];
1570 pBasePos[1] += dt * pBaseVel[1];
1571 pBasePos[2] += dt * pBaseVel[2];
1625 for (
int i = 0; i < 4; ++i)
1627 m_baseQuat_interpolate[i] = m_baseQuat[i];
1629 pBaseQuat = m_baseQuat_interpolate;
1631 btScalar *pBaseOmega = &m_realBuf[0];
1634 baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1636 baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1637 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1638 pBaseQuat[0] = baseQuat.x();
1639 pBaseQuat[1] = baseQuat.y();
1640 pBaseQuat[2] = baseQuat.z();
1641 pBaseQuat[3] = baseQuat.w();
1644 for (
int i = 0; i < num_links; ++i)
1647 pJointPos = &m_links[i].m_jointPos_interpolate[0];
1651 switch (m_links[i].m_jointType)
1657 pJointPos[0] = m_links[i].m_jointPos[0];
1659 pJointPos[0] += dt * jointVel;
1666 for (
int j = 0; j < 4; ++j)
1668 pJointPos[j] = m_links[i].m_jointPos[j];
1672 jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1674 jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1675 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1676 pJointPos[0] = jointOri.x();
1677 pJointPos[1] = jointOri.y();
1678 pJointPos[2] = jointOri.z();
1679 pJointPos[3] = jointOri.w();
1684 for (
int j = 0; j < 3; ++j)
1686 pJointPos[j] = m_links[i].m_jointPos[j];
1692 pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1693 pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1701 m_links[i].updateInterpolationCacheMultiDof();
1712 btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
1713 btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]);
1715 pBasePos[0] += dt * pBaseVel[0];
1716 pBasePos[1] += dt * pBaseVel[1];
1717 pBasePos[2] += dt * pBaseVel[2];
1768 btScalar *pBaseQuat = pq ? pq : m_baseQuat;
1769 btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0];
1772 baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1774 baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1775 pQuatUpdateFun(baseOmega, baseQuat,
true, dt);
1776 pBaseQuat[0] = baseQuat.x();
1777 pBaseQuat[1] = baseQuat.y();
1778 pBaseQuat[2] = baseQuat.z();
1779 pBaseQuat[3] = baseQuat.w();
1791 for (
int i = 0; i < num_links; ++i)
1794 pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
1798 switch (m_links[i].m_jointType)
1805 pJointPos[0] += dt * jointVel;
1812 jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1814 jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1815 pQuatUpdateFun(jointVel, jointOri,
false, dt);
1816 pJointPos[0] = jointOri.x();
1817 pJointPos[1] = jointOri.y();
1818 pJointPos[2] = jointOri.z();
1819 pJointPos[3] = jointOri.w();
1828 pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1829 pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1838 m_links[i].updateCacheMultiDof(pq);
1841 pq += m_links[i].m_posVarCount;
1843 pqd += m_links[i].m_dofCount;
1859 scratch_v.
resize(3 * num_links + 3);
1860 scratch_m.
resize(num_links + 1);
1864 v_ptr += num_links + 1;
1866 v_ptr += num_links + 1;
1868 v_ptr += num_links + 1;
1874 scratch_r1.
resize(m_dofCount+num_links);
1875 btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
1876 btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
1877 int numLinksChildToRoot=0;
1881 links[numLinksChildToRoot++]=
l;
1882 l = m_links[
l].m_parent;
1887 const btVector3 p_minus_com_world = contact_point - m_basePos;
1888 const btVector3 &normal_lin_world = normal_lin;
1889 const btVector3 &normal_ang_world = normal_ang;
1895 omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
1896 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1897 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1898 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1900 jac[3] = normal_lin_world[0];
1901 jac[4] = normal_lin_world[1];
1902 jac[5] = normal_lin_world[2];
1905 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1906 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1907 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1910 for (
int i = 6; i < 6 + m_dofCount; ++i)
1916 if (num_links > 0 && link > -1)
1922 for (
int a = 0;
a < numLinksChildToRoot;
a++)
1924 int i = links[numLinksChildToRoot-1-
a];
1926 const int parent = m_links[i].m_parent;
1927 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
1928 rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
1930 n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
1931 n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
1932 p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;
1935 switch (m_links[i].m_jointType)
1939 results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).
cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
1940 results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
1945 results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
1950 results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).
cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
1951 results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).
cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
1952 results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).
cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));
1954 results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
1955 results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
1956 results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
1962 results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).
cross(p_minus_com_local[i + 1]));
1963 results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
1964 results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));
1978 for (
int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1980 jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
1984 link = m_links[link].m_parent;
2016 for (
int i = 0; i < 6 + m_dofCount; ++i)
2017 motion += m_realBuf[i] * m_realBuf[i];
2020 if (motion < SLEEP_EPSILON)
2022 m_sleepTimer += timestep;
2023 if (m_sleepTimer > SLEEP_TIMEOUT)
2048 for (
int i = 0; i < num_links; ++i)
2050 rot_from_parent[i + 1] =
btMatrix3x3(m_links[i].m_cachedRotParentToThis);
2055 world_to_local.
resize(nLinks + 1);
2056 local_origin.
resize(nLinks + 1);
2070 int index = link + 1;
2073 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2077 tr.setRotation(
btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2094 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2098 tr.setRotation(
btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2116 int link =
col->m_link;
2119 int index = link + 1;
2123 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2127 tr.setRotation(
btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2129 col->setWorldTransform(tr);
2130 col->setInterpolationWorldTransform(tr);
2147 btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2151 tr.setRotation(
btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2168 int link =
col->m_link;
2171 int index = link + 1;
2175 btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2179 tr.setRotation(
btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2181 col->setInterpolationWorldTransform(tr);
2196 getBasePos().serialize(mbd->m_baseWorldPosition);
2198 getBaseVel().serialize(mbd->m_baseLinearVelocity);
2206 if (mbd->m_baseName)
2212 if (mbd->m_numLinks)
2215 int numElem = mbd->m_numLinks;
2218 for (
int i = 0; i < numElem; i++, memPtr++)
2253 for (
int posvar = 0; posvar < numPosVar; posvar++)
2261 if (memPtr->m_linkName)
2269 if (memPtr->m_jointName)
2281 #ifdef BT_USE_DOUBLE_PRECISION
2282 memset(mbd->m_padding, 0,
sizeof(mbd->m_padding));
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble v1
in reality light always falls off quadratically Particle Retrieve the data of the particle that spawned the object for example to give variation to multiple instances of an object Point Retrieve information about points in a point cloud Retrieve the edges of an object as it appears to Cycles topology will always appear triangulated Convert a blackbody temperature to an RGB value Normal Generate a perturbed normal from an RGB normal map image Typically used for faking highly detailed surfaces Generate an OSL shader from a file or text data block Image Sample an image file as a texture Sky Generate a procedural sky texture Noise Generate fractal Perlin noise Wave Generate procedural bands or rings with noise Voronoi Generate Worley noise based on the distance to random points Typically used to generate textures such as or biological cells Brick Generate a procedural texture producing bricks Texture Retrieve multiple types of texture coordinates nTypically used as inputs for texture nodes Vector Convert a or normal between and object coordinate space Combine Create a color from its and value channels Color Retrieve a color or the default fallback if none is specified Separate Split a vector into its Y
ATTR_WARN_UNUSED_RESULT const BMLoop * l
#define btCollisionObjectData
void * m_userObjectPointer
users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPoin...
btScalar m_angularDamping
btMatrix3x3 inverse() const
Return the inverse of the matrix.
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btScalar m_maxAppliedImpulse
@ BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
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)
#define btMultiBodyDataName
void setJointVelMultiDof(int i, const double *qdot)
const btVector3 & getLinkTorque(int i) const
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
btScalar * getJointPosMultiDof(int i)
const btVector3 & getInterpolateBasePos() const
btScalar getBaseMass() const
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
const btQuaternion & getInterpolateParentToLocalRot(int i) const
const btVector3 & getBaseInertia() const
btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const
btVector3 getBaseOmega() const
btScalar getJointPos(int i) const
const btVector3 & getLinkInertia(int i) const
void predictPositionsMultiDof(btScalar dt)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
const btVector3 getBaseVel() const
void addLinkConstraintForce(int i, const btVector3 &f)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
const btVector3 & getRVector(int i) const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const
void addLinkTorque(int i, const btVector3 &t)
void checkMotionAndSleepIfRequired(btScalar timestep)
btScalar * getJointTorqueMultiDof(int i)
const btVector3 & getInterpolateRVector(int i) const
const btQuaternion & getInterpolateWorldToBaseRot() const
btScalar getLinkMass(int i) const
#define btMultiBodyData
serialization data, don't change them if you are not familiar with the details of the serialization m...
void addLinkForce(int i, const btVector3 &f)
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void addJointTorque(int i, btScalar Q)
const btMultiBodyLinkCollider * getBaseCollider() const
const btMultibodyLink & getLink(int index) const
const btVector3 & getLinkForce(int i) const
int getParent(int link_num) const
void setJointPosMultiDof(int i, const double *q)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void setJointVel(int i, btScalar qdot)
#define btMultiBodyLinkData
btScalar getJointTorque(int i) const
void clearConstraintForces()
void addLinkConstraintTorque(int i, const btVector3 &t)
void setupSpherical(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
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)
#define btMultiBodyLinkDataName
static void spatialTransform(const btMatrix3x3 &rotation_matrix, const btVector3 &displacement, const btVector3 &top_in, const btVector3 &bottom_in, btVector3 &top_out, btVector3 &bottom_out)
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
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
btScalar * getJointVelMultiDof(int i)
void setupRevolute(int i, 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
void setupFixed(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
const btQuaternion & getWorldToBaseRot() const
const btQuaternion & getParentToLocalRot(int i) const
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
btVector3 localDirToWorld(int i, const btVector3 &local_dir) const
btScalar getJointVel(int i) const
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
void clearForcesAndTorques()
unsigned calculateSerializeBufferSize() const
virtual bool serialize(void *o_alignedDataBuffer, unsigned i_dataBufferSize, bool i_swapEndian) const
Data buffer MUST be 16 byte aligned.
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
bool gDisableDeactivation
SIMD_FORCE_INLINE btScalar btCos(btScalar x)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btSin(btScalar x)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
btJointFeedback * m_jointFeedback
SIMD_FORCE_INLINE btScalar safeNorm() const
Return the norm (length) of the vector.
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
SIMD_FORCE_INLINE btVector3 normalized() const
Return a normalized version of this vector.
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
SIMD_FORCE_INLINE void serialize(struct btQuaternionData &dataOut) const
btQuaternion inverse() const
Return the inverse of this quaternion.
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 const char * findNameForPointer(const void *ptr) const =0
virtual void serializeName(const char *ptr)=0
virtual void * getUniquePointer(void *oldPtr)=0
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
ccl_global KernelShaderEvalInput ccl_global float * output
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
btQuaternion m_zeroRotParentToThis
class btMultiBodyLinkCollider * m_collider
btQuaternion m_cachedRotParentToThis
btScalar m_jointUpperLimit
eFeatherstoneJointType m_jointType
btScalar m_jointLowerLimit
btSpatialMotionVector m_absFrameTotVelocity
const btVector3 & getAxisBottom(int dof) const
btTransform m_cachedWorldTransform
const btVector3 & getAxisTop(int dof) const
btScalar m_jointMaxVelocity
btScalar m_jointTorque[6]
btSpatialMotionVector m_absFrameLocVelocity
btVector3 m_cachedRVector
void addLinear(const btVector3 &linear)
const btVector3 & getAngular() const
void addVector(const btVector3 &angular, const btVector3 &linear)
void addAngular(const btVector3 &angular)
const btVector3 & getLinear() const
void setVector(const btVector3 &angular, const btVector3 &linear)
const btVector3 & getAngular() const
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
btScalar dot(const btSpatialForceVector &b) const
const btVector3 & getLinear() const
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)
BLI_INLINE float D(const float *data, const int res[3], int x, int y, int z)