Bullet Collision Detection & Physics Library
btMultiBody.cpp
Go to the documentation of this file.
1 /*
2  * PURPOSE:
3  * Class representing an articulated rigid body. Stores the body's
4  * current state, allows forces and torques to be set, handles
5  * timestepping and implements Featherstone's algorithm.
6  *
7  * COPYRIGHT:
8  * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
9  * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10  * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
11 
12  This software is provided 'as-is', without any express or implied warranty.
13  In no event will the authors be held liable for any damages arising from the use of this software.
14  Permission is granted to anyone to use this software for any purpose,
15  including commercial applications, and to alter it and redistribute it freely,
16  subject to the following restrictions:
17 
18  1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
19  2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
20  3. This notice may not be removed or altered from any source distribution.
21 
22  */
23 
24 #include "btMultiBody.h"
25 #include "btMultiBodyLink.h"
30 //#include "Bullet3Common/b3Logging.h"
31 // #define INCLUDE_GYRO_TERM
32 
33 
34 namespace
35 {
36 const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
37 const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
38 } // namespace
39 
40 void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
41  const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
42  const btVector3 &top_in, // top part of input vector
43  const btVector3 &bottom_in, // bottom part of input vector
44  btVector3 &top_out, // top part of output vector
45  btVector3 &bottom_out) // bottom part of output vector
46 {
47  top_out = rotation_matrix * top_in;
48  bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
49 }
50 
51 namespace
52 {
53 
54 
55 #if 0
56  void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
57  const btVector3 &displacement,
58  const btVector3 &top_in,
59  const btVector3 &bottom_in,
60  btVector3 &top_out,
61  btVector3 &bottom_out)
62  {
63  top_out = rotation_matrix.transpose() * top_in;
64  bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
65  }
66 
67  btScalar SpatialDotProduct(const btVector3 &a_top,
68  const btVector3 &a_bottom,
69  const btVector3 &b_top,
70  const btVector3 &b_bottom)
71  {
72  return a_bottom.dot(b_top) + a_top.dot(b_bottom);
73  }
74 
75  void SpatialCrossProduct(const btVector3 &a_top,
76  const btVector3 &a_bottom,
77  const btVector3 &b_top,
78  const btVector3 &b_bottom,
79  btVector3 &top_out,
80  btVector3 &bottom_out)
81  {
82  top_out = a_top.cross(b_top);
83  bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
84  }
85 #endif
86 
87 } // namespace
88 
89 //
90 // Implementation of class btMultiBody
91 //
92 
94  btScalar mass,
95  const btVector3 &inertia,
96  bool fixedBase,
97  bool canSleep,
98  bool /*deprecatedUseMultiDof*/)
99  : m_baseCollider(0),
100  m_baseName(0),
101  m_basePos(0, 0, 0),
102  m_baseQuat(0, 0, 0, 1),
103  m_baseMass(mass),
104  m_baseInertia(inertia),
105 
106  m_fixedBase(fixedBase),
107  m_awake(true),
108  m_canSleep(canSleep),
109  m_sleepTimer(0),
110  m_userObjectPointer(0),
111  m_userIndex2(-1),
112  m_userIndex(-1),
113  m_companionId(-1),
114  m_linearDamping(0.04f),
115  m_angularDamping(0.04f),
116  m_useGyroTerm(true),
117  m_maxAppliedImpulse(1000.f),
118  m_maxCoordinateVelocity(100.f),
119  m_hasSelfCollision(true),
120  __posUpdated(false),
121  m_dofCount(0),
122  m_posVarCnt(0),
123  m_useRK4(false),
124  m_useGlobalVelocities(false),
125  m_internalNeedsJointFeedback(false)
126 {
127  m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
128  m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
129  m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
130  m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
131  m_cachedInertiaValid = false;
132 
133  m_links.resize(n_links);
134  m_matrixBuf.resize(n_links + 1);
135 
136  m_baseForce.setValue(0, 0, 0);
137  m_baseTorque.setValue(0, 0, 0);
138 
141 }
142 
144 {
145 }
146 
148  btScalar mass,
149  const btVector3 &inertia,
150  int parent,
151  const btQuaternion &rotParentToThis,
152  const btVector3 &parentComToThisPivotOffset,
153  const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
154 {
155  m_links[i].m_mass = mass;
156  m_links[i].m_inertiaLocal = inertia;
157  m_links[i].m_parent = parent;
158  m_links[i].setAxisTop(0, 0., 0., 0.);
159  m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
160  m_links[i].m_zeroRotParentToThis = rotParentToThis;
161  m_links[i].m_dVector = thisPivotToThisComOffset;
162  m_links[i].m_eVector = parentComToThisPivotOffset;
163 
164  m_links[i].m_jointType = btMultibodyLink::eFixed;
165  m_links[i].m_dofCount = 0;
166  m_links[i].m_posVarCount = 0;
167 
169 
170  m_links[i].updateCacheMultiDof();
171 
173 }
174 
176  btScalar mass,
177  const btVector3 &inertia,
178  int parent,
179  const btQuaternion &rotParentToThis,
180  const btVector3 &jointAxis,
181  const btVector3 &parentComToThisPivotOffset,
182  const btVector3 &thisPivotToThisComOffset,
183  bool disableParentCollision)
184 {
185  m_dofCount += 1;
186  m_posVarCnt += 1;
187 
188  m_links[i].m_mass = mass;
189  m_links[i].m_inertiaLocal = inertia;
190  m_links[i].m_parent = parent;
191  m_links[i].m_zeroRotParentToThis = rotParentToThis;
192  m_links[i].setAxisTop(0, 0., 0., 0.);
193  m_links[i].setAxisBottom(0, jointAxis);
194  m_links[i].m_eVector = parentComToThisPivotOffset;
195  m_links[i].m_dVector = thisPivotToThisComOffset;
196  m_links[i].m_cachedRotParentToThis = rotParentToThis;
197 
198  m_links[i].m_jointType = btMultibodyLink::ePrismatic;
199  m_links[i].m_dofCount = 1;
200  m_links[i].m_posVarCount = 1;
201  m_links[i].m_jointPos[0] = 0.f;
202  m_links[i].m_jointTorque[0] = 0.f;
203 
204  if (disableParentCollision)
206  //
207 
208  m_links[i].updateCacheMultiDof();
209 
211 }
212 
214  btScalar mass,
215  const btVector3 &inertia,
216  int parent,
217  const btQuaternion &rotParentToThis,
218  const btVector3 &jointAxis,
219  const btVector3 &parentComToThisPivotOffset,
220  const btVector3 &thisPivotToThisComOffset,
221  bool disableParentCollision)
222 {
223  m_dofCount += 1;
224  m_posVarCnt += 1;
225 
226  m_links[i].m_mass = mass;
227  m_links[i].m_inertiaLocal = inertia;
228  m_links[i].m_parent = parent;
229  m_links[i].m_zeroRotParentToThis = rotParentToThis;
230  m_links[i].setAxisTop(0, jointAxis);
231  m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
232  m_links[i].m_dVector = thisPivotToThisComOffset;
233  m_links[i].m_eVector = parentComToThisPivotOffset;
234 
235  m_links[i].m_jointType = btMultibodyLink::eRevolute;
236  m_links[i].m_dofCount = 1;
237  m_links[i].m_posVarCount = 1;
238  m_links[i].m_jointPos[0] = 0.f;
239  m_links[i].m_jointTorque[0] = 0.f;
240 
241  if (disableParentCollision)
243  //
244  m_links[i].updateCacheMultiDof();
245  //
247 }
248 
250  btScalar mass,
251  const btVector3 &inertia,
252  int parent,
253  const btQuaternion &rotParentToThis,
254  const btVector3 &parentComToThisPivotOffset,
255  const btVector3 &thisPivotToThisComOffset,
256  bool disableParentCollision)
257 {
258  m_dofCount += 3;
259  m_posVarCnt += 4;
260 
261  m_links[i].m_mass = mass;
262  m_links[i].m_inertiaLocal = inertia;
263  m_links[i].m_parent = parent;
264  m_links[i].m_zeroRotParentToThis = rotParentToThis;
265  m_links[i].m_dVector = thisPivotToThisComOffset;
266  m_links[i].m_eVector = parentComToThisPivotOffset;
267 
268  m_links[i].m_jointType = btMultibodyLink::eSpherical;
269  m_links[i].m_dofCount = 3;
270  m_links[i].m_posVarCount = 4;
271  m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
272  m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
273  m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
274  m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
275  m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
276  m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
277  m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
278  m_links[i].m_jointPos[3] = 1.f;
279  m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
280 
281  if (disableParentCollision)
283  //
284  m_links[i].updateCacheMultiDof();
285  //
287 }
288 
290  btScalar mass,
291  const btVector3 &inertia,
292  int parent,
293  const btQuaternion &rotParentToThis,
294  const btVector3 &rotationAxis,
295  const btVector3 &parentComToThisComOffset,
296  bool disableParentCollision)
297 {
298  m_dofCount += 3;
299  m_posVarCnt += 3;
300 
301  m_links[i].m_mass = mass;
302  m_links[i].m_inertiaLocal = inertia;
303  m_links[i].m_parent = parent;
304  m_links[i].m_zeroRotParentToThis = rotParentToThis;
305  m_links[i].m_dVector.setZero();
306  m_links[i].m_eVector = parentComToThisComOffset;
307 
308  //
309  btVector3 vecNonParallelToRotAxis(1, 0, 0);
310  if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
311  vecNonParallelToRotAxis.setValue(0, 1, 0);
312  //
313 
314  m_links[i].m_jointType = btMultibodyLink::ePlanar;
315  m_links[i].m_dofCount = 3;
316  m_links[i].m_posVarCount = 3;
317  btVector3 n = rotationAxis.normalized();
318  m_links[i].setAxisTop(0, n[0], n[1], n[2]);
319  m_links[i].setAxisTop(1, 0, 0, 0);
320  m_links[i].setAxisTop(2, 0, 0, 0);
321  m_links[i].setAxisBottom(0, 0, 0, 0);
322  btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
323  m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
324  cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
325  m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
326  m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
327  m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
328 
329  if (disableParentCollision)
331  //
332  m_links[i].updateCacheMultiDof();
333  //
335 
336  m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
337  m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
338 }
339 
341 {
342  m_deltaV.resize(0);
344  m_realBuf.resize(6 + m_dofCount + m_dofCount * m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
345  m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
346  for (int i = 0; i < m_vectorBuf.size(); i++)
347  {
348  m_vectorBuf[i].setValue(0, 0, 0);
349  }
351 }
352 
353 int btMultiBody::getParent(int i) const
354 {
355  return m_links[i].m_parent;
356 }
357 
359 {
360  return m_links[i].m_mass;
361 }
362 
364 {
365  return m_links[i].m_inertiaLocal;
366 }
367 
369 {
370  return m_links[i].m_jointPos[0];
371 }
372 
374 {
375  return m_realBuf[6 + m_links[i].m_dofOffset];
376 }
377 
379 {
380  return &m_links[i].m_jointPos[0];
381 }
382 
384 {
385  return &m_realBuf[6 + m_links[i].m_dofOffset];
386 }
387 
389 {
390  return &m_links[i].m_jointPos[0];
391 }
392 
394 {
395  return &m_realBuf[6 + m_links[i].m_dofOffset];
396 }
397 
399 {
400  m_links[i].m_jointPos[0] = q;
401  m_links[i].updateCacheMultiDof();
402 }
403 
404 
405 void btMultiBody::setJointPosMultiDof(int i, const double *q)
406 {
407  for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
408  m_links[i].m_jointPos[pos] = (btScalar)q[pos];
409 
410  m_links[i].updateCacheMultiDof();
411 }
412 
413 void btMultiBody::setJointPosMultiDof(int i, const float *q)
414 {
415  for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
416  m_links[i].m_jointPos[pos] = (btScalar)q[pos];
417 
418  m_links[i].updateCacheMultiDof();
419 }
420 
421 
422 
424 {
425  m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
426 }
427 
428 void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
429 {
430  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
431  m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
432 }
433 
434 void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
435 {
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];
438 }
439 
441 {
442  return m_links[i].m_cachedRVector;
443 }
444 
446 {
447  return m_links[i].m_cachedRotParentToThis;
448 }
449 
450 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
451 {
452  btAssert(i >= -1);
453  btAssert(i < m_links.size());
454  if ((i < -1) || (i >= m_links.size()))
455  {
457  }
458 
459  btVector3 result = local_pos;
460  while (i != -1)
461  {
462  // 'result' is in frame i. transform it to frame parent(i)
463  result += getRVector(i);
464  result = quatRotate(getParentToLocalRot(i).inverse(), result);
465  i = getParent(i);
466  }
467 
468  // 'result' is now in the base frame. transform it to world frame
469  result = quatRotate(getWorldToBaseRot().inverse(), result);
470  result += getBasePos();
471 
472  return result;
473 }
474 
475 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
476 {
477  btAssert(i >= -1);
478  btAssert(i < m_links.size());
479  if ((i < -1) || (i >= m_links.size()))
480  {
482  }
483 
484  if (i == -1)
485  {
486  // world to base
487  return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
488  }
489  else
490  {
491  // find position in parent frame, then transform to current frame
492  return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
493  }
494 }
495 
496 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
497 {
498  btAssert(i >= -1);
499  btAssert(i < m_links.size());
500  if ((i < -1) || (i >= m_links.size()))
501  {
503  }
504 
505  btVector3 result = local_dir;
506  while (i != -1)
507  {
508  result = quatRotate(getParentToLocalRot(i).inverse(), result);
509  i = getParent(i);
510  }
511  result = quatRotate(getWorldToBaseRot().inverse(), result);
512  return result;
513 }
514 
515 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
516 {
517  btAssert(i >= -1);
518  btAssert(i < m_links.size());
519  if ((i < -1) || (i >= m_links.size()))
520  {
522  }
523 
524  if (i == -1)
525  {
526  return quatRotate(getWorldToBaseRot(), world_dir);
527  }
528  else
529  {
530  return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
531  }
532 }
533 
535 {
536  btMatrix3x3 result = local_frame;
537  btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
538  btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
539  btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
540  result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
541  return result;
542 }
543 
545 {
546  int num_links = getNumLinks();
547  // Calculates the velocities of each link (and the base) in its local frame
548  const btQuaternion& base_rot = getWorldToBaseRot();
549  omega[0] = quatRotate(base_rot, getBaseOmega());
550  vel[0] = quatRotate(base_rot, getBaseVel());
551 
552  for (int i = 0; i < num_links; ++i)
553  {
554  const btMultibodyLink& link = getLink(i);
555  const int parent = link.m_parent;
556 
557  // transform parent vel into this frame, store in omega[i+1], vel[i+1]
559  omega[parent + 1], vel[parent + 1],
560  omega[i + 1], vel[i + 1]);
561 
562  // now add qidot * shat_i
563  const btScalar* jointVel = getJointVelMultiDof(i);
564  for (int dof = 0; dof < link.m_dofCount; ++dof)
565  {
566  omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
567  vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
568  }
569  }
570 }
571 
573 {
574  int num_links = getNumLinks();
575  // TODO: would be better not to allocate memory here
577  omega.resize(num_links + 1);
579  vel.resize(num_links + 1);
580  compTreeLinkVelocities(&omega[0], &vel[0]);
581 
582  // we will do the factor of 0.5 at the end
583  btScalar result = m_baseMass * vel[0].dot(vel[0]);
584  result += omega[0].dot(m_baseInertia * omega[0]);
585 
586  for (int i = 0; i < num_links; ++i)
587  {
588  result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
589  result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
590  }
591 
592  return 0.5f * result;
593 }
594 
596 {
597  int num_links = getNumLinks();
598  // TODO: would be better not to allocate memory here
600  omega.resize(num_links + 1);
602  vel.resize(num_links + 1);
603  btAlignedObjectArray<btQuaternion> rot_from_world;
604  rot_from_world.resize(num_links + 1);
605  compTreeLinkVelocities(&omega[0], &vel[0]);
606 
607  rot_from_world[0] = m_baseQuat;
608  btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
609 
610  for (int i = 0; i < num_links; ++i)
611  {
612  rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
613  result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
614  }
615 
616  return result;
617 }
618 
620 {
623 
624  for (int i = 0; i < getNumLinks(); ++i)
625  {
626  m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
627  m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
628  }
629 }
631 {
632  m_baseForce.setValue(0, 0, 0);
633  m_baseTorque.setValue(0, 0, 0);
634 
635  for (int i = 0; i < getNumLinks(); ++i)
636  {
637  m_links[i].m_appliedForce.setValue(0, 0, 0);
638  m_links[i].m_appliedTorque.setValue(0, 0, 0);
639  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;
640  }
641 }
642 
644 {
645  for (int i = 0; i < 6 + getNumDofs(); ++i)
646  {
647  m_realBuf[i] = 0.f;
648  }
649 }
651 {
652  m_links[i].m_appliedForce += f;
653 }
654 
656 {
657  m_links[i].m_appliedTorque += t;
658 }
659 
661 {
662  m_links[i].m_appliedConstraintForce += f;
663 }
664 
666 {
667  m_links[i].m_appliedConstraintTorque += t;
668 }
669 
671 {
672  m_links[i].m_jointTorque[0] += Q;
673 }
674 
676 {
677  m_links[i].m_jointTorque[dof] += Q;
678 }
679 
681 {
682  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
683  m_links[i].m_jointTorque[dof] = Q[dof];
684 }
685 
687 {
688  return m_links[i].m_appliedForce;
689 }
690 
692 {
693  return m_links[i].m_appliedTorque;
694 }
695 
697 {
698  return m_links[i].m_jointTorque[0];
699 }
700 
702 {
703  return &m_links[i].m_jointTorque[0];
704 }
705 
706 inline btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
707 {
708  btVector3 row0 = btVector3(
709  v0.x() * v1.x(),
710  v0.x() * v1.y(),
711  v0.x() * v1.z());
712  btVector3 row1 = btVector3(
713  v0.y() * v1.x(),
714  v0.y() * v1.y(),
715  v0.y() * v1.z());
716  btVector3 row2 = btVector3(
717  v0.z() * v1.x(),
718  v0.z() * v1.y(),
719  v0.z() * v1.z());
720 
721  btMatrix3x3 m(row0[0], row0[1], row0[2],
722  row1[0], row1[1], row1[2],
723  row2[0], row2[1], row2[2]);
724  return m;
725 }
726 
727 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
728 //
729 
734  bool isConstraintPass,
735  bool jointFeedbackInWorldSpace,
736  bool jointFeedbackInJointFrame)
737 {
738  // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
739  // and the base linear & angular accelerations.
740 
741  // We apply damping forces in this routine as well as any external forces specified by the
742  // caller (via addBaseForce etc).
743 
744  // output should point to an array of 6 + num_links reals.
745  // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
746  // num_links joint acceleration values.
747 
748  // We added support for multi degree of freedom (multi dof) joints.
749  // In addition we also can compute the joint reaction forces. This is performed in a second pass,
750  // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
751 
753 
754  int num_links = getNumLinks();
755 
756  const btScalar DAMPING_K1_LINEAR = m_linearDamping;
757  const btScalar DAMPING_K2_LINEAR = m_linearDamping;
758 
759  const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
760  const btScalar DAMPING_K2_ANGULAR = m_angularDamping;
761 
762  const btVector3 base_vel = getBaseVel();
763  const btVector3 base_omega = getBaseOmega();
764 
765  // Temporary matrices/vectors -- use scratch space from caller
766  // so that we don't have to keep reallocating every frame
767 
768  scratch_r.resize(2 * m_dofCount + 7); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
769  scratch_v.resize(8 * num_links + 6);
770  scratch_m.resize(4 * num_links + 4);
771 
772  //btScalar * r_ptr = &scratch_r[0];
773  btScalar *output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
774  btVector3 *v_ptr = &scratch_v[0];
775 
776  // vhat_i (top = angular, bottom = linear part)
777  btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
778  v_ptr += num_links * 2 + 2;
779  //
780  // zhat_i^A
781  btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
782  v_ptr += num_links * 2 + 2;
783  //
784  // chat_i (note NOT defined for the base)
785  btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
786  v_ptr += num_links * 2;
787  //
788  // Ihat_i^A.
789  btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
790 
791  // Cached 3x3 rotation matrices from parent frame to this frame.
792  btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
793  btMatrix3x3 *rot_from_world = &scratch_m[0];
794 
795  // hhat_i, ahat_i
796  // hhat is NOT stored for the base (but ahat is)
798  btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
799  v_ptr += num_links * 2 + 2;
800  //
801  // Y_i, invD_i
802  btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
803  btScalar *Y = &scratch_r[0];
804  //
805  //aux variables
806  btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
807  btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
808  btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
809  btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
810  btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
811  btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
812  btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
813  btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
815  fromWorld.m_trnVec.setZero();
817 
818  // ptr to the joint accel part of the output
819  btScalar *joint_accel = output + 6;
820 
821  // Start of the algorithm proper.
822 
823  // First 'upward' loop.
824  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
825 
826  rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
827 
828  //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
829  spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
830 
831  if (m_fixedBase)
832  {
833  zeroAccSpatFrc[0].setZero();
834  }
835  else
836  {
837  const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
838  const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
839  //external forces
840  zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
841 
842  //adding damping terms (only)
843  const btScalar linDampMult = 1., angDampMult = 1.;
844  zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
845  linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
846 
847  //
848  //p += vhat x Ihat vhat - done in a simpler way
849  if (m_useGyroTerm)
850  zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
851  //
852  zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
853  }
854 
855  //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
856  spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
857  //
858  btMatrix3x3(m_baseMass, 0, 0,
859  0, m_baseMass, 0,
860  0, 0, m_baseMass),
861  //
862  btMatrix3x3(m_baseInertia[0], 0, 0,
863  0, m_baseInertia[1], 0,
864  0, 0, m_baseInertia[2]));
865 
866  rot_from_world[0] = rot_from_parent[0];
867 
868  //
869  for (int i = 0; i < num_links; ++i)
870  {
871  const int parent = m_links[i].m_parent;
872  rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
873  rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
874 
875  fromParent.m_rotMat = rot_from_parent[i + 1];
876  fromParent.m_trnVec = m_links[i].m_cachedRVector;
877  fromWorld.m_rotMat = rot_from_world[i + 1];
878  fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
879 
880  // now set vhat_i to its true value by doing
881  // vhat_i += qidot * shat_i
883  {
884  spatJointVel.setZero();
885 
886  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
887  spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
888 
889  // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
890  spatVel[i + 1] += spatJointVel;
891 
892  //
893  // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
894  //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
895  }
896  else
897  {
898  fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);
899  fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
900  }
901 
902  // we can now calculate chat_i
903  spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);
904 
905  // calculate zhat_i^A
906  //
907  //external forces
908  btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
909  btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
910 
911  zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
912 
913 #if 0
914  {
915 
916  b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
917  i+1,
918  zeroAccSpatFrc[i+1].m_topVec[0],
919  zeroAccSpatFrc[i+1].m_topVec[1],
920  zeroAccSpatFrc[i+1].m_topVec[2],
921 
922  zeroAccSpatFrc[i+1].m_bottomVec[0],
923  zeroAccSpatFrc[i+1].m_bottomVec[1],
924  zeroAccSpatFrc[i+1].m_bottomVec[2]);
925  }
926 #endif
927  //
928  //adding damping terms (only)
929  btScalar linDampMult = 1., angDampMult = 1.;
930  zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
931  linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
932 
933  // calculate Ihat_i^A
934  //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
935  spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
936  //
937  btMatrix3x3(m_links[i].m_mass, 0, 0,
938  0, m_links[i].m_mass, 0,
939  0, 0, m_links[i].m_mass),
940  //
941  btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
942  0, m_links[i].m_inertiaLocal[1], 0,
943  0, 0, m_links[i].m_inertiaLocal[2]));
944  //
945  //p += vhat x Ihat vhat - done in a simpler way
946  if (m_useGyroTerm)
947  zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
948  //
949  zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
950  //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
952  //btScalar parOmegaMod = temp.length();
953  //btScalar parOmegaModMax = 1000;
954  //if(parOmegaMod > parOmegaModMax)
955  // temp *= parOmegaModMax / parOmegaMod;
956  //zeroAccSpatFrc[i+1].addLinear(temp);
957  //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
958  //temp = spatCoriolisAcc[i].getLinear();
959  //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
960 
961  //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
962  //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
963  //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
964  }
965 
966  // 'Downward' loop.
967  // (part of TreeForwardDynamics in Mirtich.)
968  for (int i = num_links - 1; i >= 0; --i)
969  {
970  const int parent = m_links[i].m_parent;
971  fromParent.m_rotMat = rot_from_parent[i + 1];
972  fromParent.m_trnVec = m_links[i].m_cachedRVector;
973 
974  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
975  {
976  btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
977  //
978  hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
979  //
980  Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]) - spatCoriolisAcc[i].dot(hDof);
981  }
982  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
983  {
984  btScalar *D_row = &D[dof * m_links[i].m_dofCount];
985  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
986  {
987  const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
988  D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
989  }
990  }
991 
992  btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
993  switch (m_links[i].m_jointType)
994  {
997  {
998  if (D[0] >= SIMD_EPSILON)
999  {
1000  invDi[0] = 1.0f / D[0];
1001  }
1002  else
1003  {
1004  invDi[0] = 0;
1005  }
1006  break;
1007  }
1010  {
1011  const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1012  const btMatrix3x3 invD3x3(D3x3.inverse());
1013 
1014  //unroll the loop?
1015  for (int row = 0; row < 3; ++row)
1016  {
1017  for (int col = 0; col < 3; ++col)
1018  {
1019  invDi[row * 3 + col] = invD3x3[row][col];
1020  }
1021  }
1022 
1023  break;
1024  }
1025  default:
1026  {
1027  }
1028  }
1029 
1030  //determine h*D^{-1}
1031  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1032  {
1033  spatForceVecTemps[dof].setZero();
1034 
1035  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1036  {
1037  const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
1038  //
1039  spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
1040  }
1041  }
1042 
1043  dyadTemp = spatInertia[i + 1];
1044 
1045  //determine (h*D^{-1}) * h^{T}
1046  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1047  {
1048  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1049  //
1050  dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
1051  }
1052 
1053  fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);
1054 
1055  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1056  {
1057  invD_times_Y[dof] = 0.f;
1058 
1059  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1060  {
1061  invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1062  }
1063  }
1064 
1065  spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1066 
1067  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1068  {
1069  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1070  //
1071  spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1072  }
1073 
1074  fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1075 
1076  zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1077  }
1078 
1079  // Second 'upward' loop
1080  // (part of TreeForwardDynamics in Mirtich)
1081 
1082  if (m_fixedBase)
1083  {
1084  spatAcc[0].setZero();
1085  }
1086  else
1087  {
1088  if (num_links > 0)
1089  {
1090  m_cachedInertiaValid = true;
1091  m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
1092  m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
1093  m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
1095  }
1096 
1097  solveImatrix(zeroAccSpatFrc[0], result);
1098  spatAcc[0] = -result;
1099  }
1100 
1101  // now do the loop over the m_links
1102  for (int i = 0; i < num_links; ++i)
1103  {
1104  // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
1105  // a = apar + cor + Sqdd
1106  //or
1107  // qdd = D^{-1} * (Y - h^{T}*(apar+cor))
1108  // a = apar + Sqdd
1109 
1110  const int parent = m_links[i].m_parent;
1111  fromParent.m_rotMat = rot_from_parent[i + 1];
1112  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1113 
1114  fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
1115 
1116  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1117  {
1118  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1119  //
1120  Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
1121  }
1122 
1123  btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1124  //D^{-1} * (Y - h^{T}*apar)
1125  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]);
1126 
1127  spatAcc[i + 1] += spatCoriolisAcc[i];
1128 
1129  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1130  spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1131 
1132  if (m_links[i].m_jointFeedback)
1133  {
1135 
1136  btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1137  btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1138 
1139  if (jointFeedbackInJointFrame)
1140  {
1141  //shift the reaction forces to the joint frame
1142  //linear (force) component is the same
1143  //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
1144  angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
1145  }
1146 
1147  if (jointFeedbackInWorldSpace)
1148  {
1149  if (isConstraintPass)
1150  {
1151  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1152  m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1153  }
1154  else
1155  {
1156  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1157  m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1158  }
1159  }
1160  else
1161  {
1162  if (isConstraintPass)
1163  {
1164  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1165  m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1166  }
1167  else
1168  {
1169  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1170  m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1171  }
1172  }
1173  }
1174  }
1175 
1176  // transform base accelerations back to the world frame.
1177  const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1178  output[0] = omegadot_out[0];
1179  output[1] = omegadot_out[1];
1180  output[2] = omegadot_out[2];
1181 
1182  const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1183  output[3] = vdot_out[0];
1184  output[4] = vdot_out[1];
1185  output[5] = vdot_out[2];
1186 
1188  //printf("q = [");
1189  //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1190  //for(int link = 0; link < getNumLinks(); ++link)
1191  // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1192  // printf("%.6f ", m_links[link].m_jointPos[dof]);
1193  //printf("]\n");
1195  //printf("qd = [");
1196  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1197  // printf("%.6f ", m_realBuf[dof]);
1198  //printf("]\n");
1199  //printf("qdd = [");
1200  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1201  // printf("%.6f ", output[dof]);
1202  //printf("]\n");
1204 
1205  // Final step: add the accelerations (times dt) to the velocities.
1206 
1207  if (!isConstraintPass)
1208  {
1209  if (dt > 0.)
1211  }
1213  //btScalar angularThres = 1;
1214  //btScalar maxAngVel = 0.;
1215  //bool scaleDown = 1.;
1216  //for(int link = 0; link < m_links.size(); ++link)
1217  //{
1218  // if(spatVel[link+1].getAngular().length() > maxAngVel)
1219  // {
1220  // maxAngVel = spatVel[link+1].getAngular().length();
1221  // scaleDown = angularThres / spatVel[link+1].getAngular().length();
1222  // break;
1223  // }
1224  //}
1225 
1226  //if(scaleDown != 1.)
1227  //{
1228  // for(int link = 0; link < m_links.size(); ++link)
1229  // {
1230  // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
1231  // {
1232  // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1233  // getJointVelMultiDof(link)[dof] *= scaleDown;
1234  // }
1235  // }
1236  //}
1238 
1241  {
1242  for (int i = 0; i < num_links; ++i)
1243  {
1244  const int parent = m_links[i].m_parent;
1245  //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
1246  //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
1247 
1248  fromParent.m_rotMat = rot_from_parent[i + 1];
1249  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1250  fromWorld.m_rotMat = rot_from_world[i + 1];
1251 
1252  // vhat_i = i_xhat_p(i) * vhat_p(i)
1253  fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
1254  //nice alternative below (using operator *) but it generates temps
1256 
1257  // now set vhat_i to its true value by doing
1258  // vhat_i += qidot * shat_i
1259  spatJointVel.setZero();
1260 
1261  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1262  spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1263 
1264  // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
1265  spatVel[i + 1] += spatJointVel;
1266 
1267  fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
1268  fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
1269  }
1270  }
1271 }
1272 
1273 void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
1274 {
1275  int num_links = getNumLinks();
1277  if (num_links == 0)
1278  {
1279  // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1280 
1282  {
1283  result[0] = rhs_bot[0] / m_baseInertia[0];
1284  result[1] = rhs_bot[1] / m_baseInertia[1];
1285  result[2] = rhs_bot[2] / m_baseInertia[2];
1286  }
1287  else
1288  {
1289  result[0] = 0;
1290  result[1] = 0;
1291  result[2] = 0;
1292  }
1293  if (m_baseMass >= SIMD_EPSILON)
1294  {
1295  result[3] = rhs_top[0] / m_baseMass;
1296  result[4] = rhs_top[1] / m_baseMass;
1297  result[5] = rhs_top[2] / m_baseMass;
1298  }
1299  else
1300  {
1301  result[3] = 0;
1302  result[4] = 0;
1303  result[5] = 0;
1304  }
1305  }
1306  else
1307  {
1308  if (!m_cachedInertiaValid)
1309  {
1310  for (int i = 0; i < 6; i++)
1311  {
1312  result[i] = 0.f;
1313  }
1314  return;
1315  }
1321  tmp = invIupper_right * m_cachedInertiaLowerRight;
1322  btMatrix3x3 invI_upper_left = (tmp * Binv);
1323  btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1324  tmp = m_cachedInertiaTopLeft * invI_upper_left;
1325  tmp[0][0] -= 1.0;
1326  tmp[1][1] -= 1.0;
1327  tmp[2][2] -= 1.0;
1328  btMatrix3x3 invI_lower_left = (Binv * tmp);
1329 
1330  //multiply result = invI * rhs
1331  {
1332  btVector3 vtop = invI_upper_left * rhs_top;
1333  btVector3 tmp;
1334  tmp = invIupper_right * rhs_bot;
1335  vtop += tmp;
1336  btVector3 vbot = invI_lower_left * rhs_top;
1337  tmp = invI_lower_right * rhs_bot;
1338  vbot += tmp;
1339  result[0] = vtop[0];
1340  result[1] = vtop[1];
1341  result[2] = vtop[2];
1342  result[3] = vbot[0];
1343  result[4] = vbot[1];
1344  result[5] = vbot[2];
1345  }
1346  }
1347 }
1349 {
1350  int num_links = getNumLinks();
1352  if (num_links == 0)
1353  {
1354  // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1356  {
1357  result.setAngular(rhs.getAngular() / m_baseInertia);
1358  }
1359  else
1360  {
1361  result.setAngular(btVector3(0, 0, 0));
1362  }
1363  if (m_baseMass >= SIMD_EPSILON)
1364  {
1365  result.setLinear(rhs.getLinear() / m_baseMass);
1366  }
1367  else
1368  {
1369  result.setLinear(btVector3(0, 0, 0));
1370  }
1371  }
1372  else
1373  {
1376  if (!m_cachedInertiaValid)
1377  {
1378  result.setLinear(btVector3(0, 0, 0));
1379  result.setAngular(btVector3(0, 0, 0));
1380  result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
1381  return;
1382  }
1386  tmp = invIupper_right * m_cachedInertiaLowerRight;
1387  btMatrix3x3 invI_upper_left = (tmp * Binv);
1388  btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1389  tmp = m_cachedInertiaTopLeft * invI_upper_left;
1390  tmp[0][0] -= 1.0;
1391  tmp[1][1] -= 1.0;
1392  tmp[2][2] -= 1.0;
1393  btMatrix3x3 invI_lower_left = (Binv * tmp);
1394 
1395  //multiply result = invI * rhs
1396  {
1397  btVector3 vtop = invI_upper_left * rhs.getLinear();
1398  btVector3 tmp;
1399  tmp = invIupper_right * rhs.getAngular();
1400  vtop += tmp;
1401  btVector3 vbot = invI_lower_left * rhs.getLinear();
1402  tmp = invI_lower_right * rhs.getAngular();
1403  vbot += tmp;
1404  result.setVector(vtop, vbot);
1405  }
1406  }
1407 }
1408 
1409 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
1410 {
1411  for (int row = 0; row < rowsA; row++)
1412  {
1413  for (int col = 0; col < colsB; col++)
1414  {
1415  pC[row * colsB + col] = 0.f;
1416  for (int inner = 0; inner < rowsB; inner++)
1417  {
1418  pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1419  }
1420  }
1421  }
1422 }
1423 
1426 {
1427  // Temporary matrices/vectors -- use scratch space from caller
1428  // so that we don't have to keep reallocating every frame
1429 
1430  int num_links = getNumLinks();
1431  scratch_r.resize(m_dofCount);
1432  scratch_v.resize(4 * num_links + 4);
1433 
1434  btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;
1435  btVector3 *v_ptr = &scratch_v[0];
1436 
1437  // zhat_i^A (scratch space)
1438  btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
1439  v_ptr += num_links * 2 + 2;
1440 
1441  // rot_from_parent (cached from calcAccelerations)
1442  const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
1443 
1444  // hhat (cached), accel (scratch)
1445  // hhat is NOT stored for the base (but ahat is)
1446  const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
1447  btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
1448  v_ptr += num_links * 2 + 2;
1449 
1450  // Y_i (scratch), invD_i (cached)
1451  const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1452  btScalar *Y = r_ptr;
1454  //aux variables
1455  btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
1456  btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
1457  btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
1458  btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
1459  btSpatialTransformationMatrix fromParent;
1461 
1462  // First 'upward' loop.
1463  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1464 
1465  // Fill in zero_acc
1466  // -- set to force/torque on the base, zero otherwise
1467  if (m_fixedBase)
1468  {
1469  zeroAccSpatFrc[0].setZero();
1470  }
1471  else
1472  {
1473  //test forces
1474  fromParent.m_rotMat = rot_from_parent[0];
1475  fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]);
1476  }
1477  for (int i = 0; i < num_links; ++i)
1478  {
1479  zeroAccSpatFrc[i + 1].setZero();
1480  }
1481 
1482  // 'Downward' loop.
1483  // (part of TreeForwardDynamics in Mirtich.)
1484  for (int i = num_links - 1; i >= 0; --i)
1485  {
1486  const int parent = m_links[i].m_parent;
1487  fromParent.m_rotMat = rot_from_parent[i + 1];
1488  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1489 
1490  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1491  {
1492  Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i + 1]);
1493  }
1494 
1495  btVector3 in_top, in_bottom, out_top, out_bottom;
1496  const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1497 
1498  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1499  {
1500  invD_times_Y[dof] = 0.f;
1501 
1502  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1503  {
1504  invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1505  }
1506  }
1507 
1508  // Zp += pXi * (Zi + hi*Yi/Di)
1509  spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1510 
1511  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1512  {
1513  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1514  //
1515  spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1516  }
1517 
1518  fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1519 
1520  zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1521  }
1522 
1523  // ptr to the joint accel part of the output
1524  btScalar *joint_accel = output + 6;
1525 
1526  // Second 'upward' loop
1527  // (part of TreeForwardDynamics in Mirtich)
1528 
1529  if (m_fixedBase)
1530  {
1531  spatAcc[0].setZero();
1532  }
1533  else
1534  {
1535  solveImatrix(zeroAccSpatFrc[0], result);
1536  spatAcc[0] = -result;
1537  }
1538 
1539  // now do the loop over the m_links
1540  for (int i = 0; i < num_links; ++i)
1541  {
1542  const int parent = m_links[i].m_parent;
1543  fromParent.m_rotMat = rot_from_parent[i + 1];
1544  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1545 
1546  fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
1547 
1548  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1549  {
1550  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1551  //
1552  Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
1553  }
1554 
1555  const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1556  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]);
1557 
1558  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1559  spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1560  }
1561 
1562  // transform base accelerations back to the world frame.
1563  btVector3 omegadot_out;
1564  omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1565  output[0] = omegadot_out[0];
1566  output[1] = omegadot_out[1];
1567  output[2] = omegadot_out[2];
1568 
1569  btVector3 vdot_out;
1570  vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
1571  output[3] = vdot_out[0];
1572  output[4] = vdot_out[1];
1573  output[5] = vdot_out[2];
1574 
1576  //printf("delta = [");
1577  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1578  // printf("%.2f ", output[dof]);
1579  //printf("]\n");
1581 }
1582 
1584 {
1585  int num_links = getNumLinks();
1586  // step position by adding dt * velocity
1587  //btVector3 v = getBaseVel();
1588  //m_basePos += dt * v;
1589  //
1590  btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
1591  btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
1592  //
1593  pBasePos[0] += dt * pBaseVel[0];
1594  pBasePos[1] += dt * pBaseVel[1];
1595  pBasePos[2] += dt * pBaseVel[2];
1596 
1598  //local functor for quaternion integration (to avoid error prone redundancy)
1599  struct
1600  {
1601  //"exponential map" based on btTransformUtil::integrateTransform(..)
1602  void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
1603  {
1604  //baseBody => quat is alias and omega is global coor
1606 
1607  btVector3 axis;
1608  btVector3 angvel;
1609 
1610  if (!baseBody)
1611  angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
1612  else
1613  angvel = omega;
1614 
1615  btScalar fAngle = angvel.length();
1616  //limit the angular motion
1617  if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
1618  {
1619  fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
1620  }
1621 
1622  if (fAngle < btScalar(0.001))
1623  {
1624  // use Taylor's expansions of sync function
1625  axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
1626  }
1627  else
1628  {
1629  // sync(fAngle) = sin(c*fAngle)/t
1630  axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
1631  }
1632 
1633  if (!baseBody)
1634  quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
1635  else
1636  quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
1637  //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
1638 
1639  quat.normalize();
1640  }
1641  } pQuatUpdateFun;
1643 
1644  //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
1645  //
1646  btScalar *pBaseQuat = pq ? pq : m_baseQuat;
1647  btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
1648  //
1649  btQuaternion baseQuat;
1650  baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1651  btVector3 baseOmega;
1652  baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1653  pQuatUpdateFun(baseOmega, baseQuat, true, dt);
1654  pBaseQuat[0] = baseQuat.x();
1655  pBaseQuat[1] = baseQuat.y();
1656  pBaseQuat[2] = baseQuat.z();
1657  pBaseQuat[3] = baseQuat.w();
1658 
1659  //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
1660  //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
1661  //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
1662 
1663  if (pq)
1664  pq += 7;
1665  if (pqd)
1666  pqd += 6;
1667 
1668  // Finally we can update m_jointPos for each of the m_links
1669  for (int i = 0; i < num_links; ++i)
1670  {
1671  btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
1672  btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
1673 
1674  switch (m_links[i].m_jointType)
1675  {
1678  {
1679  btScalar jointVel = pJointVel[0];
1680  pJointPos[0] += dt * jointVel;
1681  break;
1682  }
1684  {
1685  btVector3 jointVel;
1686  jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1687  btQuaternion jointOri;
1688  jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1689  pQuatUpdateFun(jointVel, jointOri, false, dt);
1690  pJointPos[0] = jointOri.x();
1691  pJointPos[1] = jointOri.y();
1692  pJointPos[2] = jointOri.z();
1693  pJointPos[3] = jointOri.w();
1694  break;
1695  }
1697  {
1698  pJointPos[0] += dt * getJointVelMultiDof(i)[0];
1699 
1700  btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
1701  btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
1702  pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1703  pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1704 
1705  break;
1706  }
1707  default:
1708  {
1709  }
1710  }
1711 
1712  m_links[i].updateCacheMultiDof(pq);
1713 
1714  if (pq)
1715  pq += m_links[i].m_posVarCount;
1716  if (pqd)
1717  pqd += m_links[i].m_dofCount;
1718  }
1719 }
1720 
1722  const btVector3 &contact_point,
1723  const btVector3 &normal_ang,
1724  const btVector3 &normal_lin,
1725  btScalar *jac,
1726  btAlignedObjectArray<btScalar> &scratch_r1,
1728  btAlignedObjectArray<btMatrix3x3> &scratch_m) const
1729 {
1730  // temporary space
1731  int num_links = getNumLinks();
1732  int m_dofCount = getNumDofs();
1733  scratch_v.resize(3 * num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
1734  scratch_m.resize(num_links + 1);
1735 
1736  btVector3 *v_ptr = &scratch_v[0];
1737  btVector3 *p_minus_com_local = v_ptr;
1738  v_ptr += num_links + 1;
1739  btVector3 *n_local_lin = v_ptr;
1740  v_ptr += num_links + 1;
1741  btVector3 *n_local_ang = v_ptr;
1742  v_ptr += num_links + 1;
1743  btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
1744 
1745  //scratch_r.resize(m_dofCount);
1746  //btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
1747 
1748  scratch_r1.resize(m_dofCount+num_links);
1749  btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
1750  btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
1751  int numLinksChildToRoot=0;
1752  int l = link;
1753  while (l != -1)
1754  {
1755  links[numLinksChildToRoot++]=l;
1756  l = m_links[l].m_parent;
1757  }
1758 
1759  btMatrix3x3 *rot_from_world = &scratch_m[0];
1760 
1761  const btVector3 p_minus_com_world = contact_point - m_basePos;
1762  const btVector3 &normal_lin_world = normal_lin; //convenience
1763  const btVector3 &normal_ang_world = normal_ang;
1764 
1765  rot_from_world[0] = btMatrix3x3(m_baseQuat);
1766 
1767  // omega coeffients first.
1768  btVector3 omega_coeffs_world;
1769  omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
1770  jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1771  jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1772  jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1773  // then v coefficients
1774  jac[3] = normal_lin_world[0];
1775  jac[4] = normal_lin_world[1];
1776  jac[5] = normal_lin_world[2];
1777 
1778  //create link-local versions of p_minus_com and normal
1779  p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1780  n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1781  n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1782 
1783  // Set remaining jac values to zero for now.
1784  for (int i = 6; i < 6 + m_dofCount; ++i)
1785  {
1786  jac[i] = 0;
1787  }
1788 
1789  // Qdot coefficients, if necessary.
1790  if (num_links > 0 && link > -1)
1791  {
1792  // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
1793  // which is resulting in repeated work being done...)
1794 
1795  // calculate required normals & positions in the local frames.
1796  for (int a = 0; a < numLinksChildToRoot; a++)
1797  {
1798  int i = links[numLinksChildToRoot-1-a];
1799  // transform to local frame
1800  const int parent = m_links[i].m_parent;
1801  const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
1802  rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
1803 
1804  n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
1805  n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
1806  p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;
1807 
1808  // calculate the jacobian entry
1809  switch (m_links[i].m_jointType)
1810  {
1812  {
1813  results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
1814  results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
1815  break;
1816  }
1818  {
1819  results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
1820  break;
1821  }
1823  {
1824  results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(0));
1825  results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(1));
1826  results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i + 1]) + m_links[i].getAxisBottom(2));
1827 
1828  results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
1829  results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
1830  results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
1831 
1832  break;
1833  }
1835  {
1836  results[m_links[i].m_dofOffset + 0] = n_local_lin[i + 1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i + 1])); // + m_links[i].getAxisBottom(0));
1837  results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
1838  results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));
1839 
1840  break;
1841  }
1842  default:
1843  {
1844  }
1845  }
1846  }
1847 
1848  // Now copy through to output.
1849  //printf("jac[%d] = ", link);
1850  while (link != -1)
1851  {
1852  for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1853  {
1854  jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
1855  //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
1856  }
1857 
1858  link = m_links[link].m_parent;
1859  }
1860  //printf("]\n");
1861  }
1862 }
1863 
1865 {
1866  m_sleepTimer = 0;
1867  m_awake = true;
1868 }
1869 
1871 {
1872  m_awake = false;
1873 }
1874 
1876 {
1877  extern bool gDisableDeactivation;
1879  {
1880  m_awake = true;
1881  m_sleepTimer = 0;
1882  return;
1883  }
1884 
1885  // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
1886  btScalar motion = 0;
1887  {
1888  for (int i = 0; i < 6 + m_dofCount; ++i)
1889  motion += m_realBuf[i] * m_realBuf[i];
1890  }
1891 
1892  if (motion < SLEEP_EPSILON)
1893  {
1894  m_sleepTimer += timestep;
1895  if (m_sleepTimer > SLEEP_TIMEOUT)
1896  {
1897  goToSleep();
1898  }
1899  }
1900  else
1901  {
1902  m_sleepTimer = 0;
1903  if (!m_awake)
1904  wakeUp();
1905  }
1906 }
1907 
1909 {
1910  int num_links = getNumLinks();
1911 
1912  // Cached 3x3 rotation matrices from parent frame to this frame.
1913  btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];
1914 
1915  rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
1916 
1917  for (int i = 0; i < num_links; ++i)
1918  {
1919  rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
1920  }
1921 
1922  int nLinks = getNumLinks();
1924  world_to_local.resize(nLinks + 1);
1925  local_origin.resize(nLinks + 1);
1926 
1927  world_to_local[0] = getWorldToBaseRot();
1928  local_origin[0] = getBasePos();
1929 
1930  for (int k = 0; k < getNumLinks(); k++)
1931  {
1932  const int parent = getParent(k);
1933  world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
1934  local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
1935  }
1936 
1937  for (int link = 0; link < getNumLinks(); link++)
1938  {
1939  int index = link + 1;
1940 
1941  btVector3 posr = local_origin[index];
1942  btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
1943  btTransform tr;
1944  tr.setIdentity();
1945  tr.setOrigin(posr);
1946  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
1947  getLink(link).m_cachedWorldTransform = tr;
1948  }
1949 }
1950 
1952 {
1953  world_to_local.resize(getNumLinks() + 1);
1954  local_origin.resize(getNumLinks() + 1);
1955 
1956  world_to_local[0] = getWorldToBaseRot();
1957  local_origin[0] = getBasePos();
1958 
1959  if (getBaseCollider())
1960  {
1961  btVector3 posr = local_origin[0];
1962  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
1963  btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
1964  btTransform tr;
1965  tr.setIdentity();
1966  tr.setOrigin(posr);
1967  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
1968 
1970  }
1971 
1972  for (int k = 0; k < getNumLinks(); k++)
1973  {
1974  const int parent = getParent(k);
1975  world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
1976  local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
1977  }
1978 
1979  for (int m = 0; m < getNumLinks(); m++)
1980  {
1982  if (col)
1983  {
1984  int link = col->m_link;
1985  btAssert(link == m);
1986 
1987  int index = link + 1;
1988 
1989  btVector3 posr = local_origin[index];
1990  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
1991  btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
1992  btTransform tr;
1993  tr.setIdentity();
1994  tr.setOrigin(posr);
1995  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
1996 
1997  col->setWorldTransform(tr);
1998  }
1999  }
2000 }
2001 
2003 {
2004  int sz = sizeof(btMultiBodyData);
2005  return sz;
2006 }
2007 
2009 const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
2010 {
2011  btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
2012  getBasePos().serialize(mbd->m_baseWorldPosition);
2013  getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
2014  getBaseVel().serialize(mbd->m_baseLinearVelocity);
2015  getBaseOmega().serialize(mbd->m_baseAngularVelocity);
2016 
2017  mbd->m_baseMass = this->getBaseMass();
2018  getBaseInertia().serialize(mbd->m_baseInertia);
2019  {
2020  char *name = (char *)serializer->findNameForPointer(m_baseName);
2021  mbd->m_baseName = (char *)serializer->getUniquePointer(name);
2022  if (mbd->m_baseName)
2023  {
2024  serializer->serializeName(name);
2025  }
2026  }
2027  mbd->m_numLinks = this->getNumLinks();
2028  if (mbd->m_numLinks)
2029  {
2030  int sz = sizeof(btMultiBodyLinkData);
2031  int numElem = mbd->m_numLinks;
2032  btChunk *chunk = serializer->allocate(sz, numElem);
2033  btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
2034  for (int i = 0; i < numElem; i++, memPtr++)
2035  {
2036  memPtr->m_jointType = getLink(i).m_jointType;
2037  memPtr->m_dofCount = getLink(i).m_dofCount;
2038  memPtr->m_posVarCount = getLink(i).m_posVarCount;
2039 
2040  getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
2041 
2042  getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
2043  getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
2044  getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
2045  getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
2046 
2047  memPtr->m_linkMass = getLink(i).m_mass;
2048  memPtr->m_parentIndex = getLink(i).m_parent;
2049  memPtr->m_jointDamping = getLink(i).m_jointDamping;
2050  memPtr->m_jointFriction = getLink(i).m_jointFriction;
2051  memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
2052  memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
2053  memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
2054  memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
2055 
2056  getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
2057  getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
2058  getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
2059  btAssert(memPtr->m_dofCount <= 3);
2060  for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
2061  {
2062  getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
2063  getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
2064 
2065  memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
2066  memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
2067  }
2068  int numPosVar = getLink(i).m_posVarCount;
2069  for (int posvar = 0; posvar < numPosVar; posvar++)
2070  {
2071  memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
2072  }
2073 
2074  {
2075  char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
2076  memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
2077  if (memPtr->m_linkName)
2078  {
2079  serializer->serializeName(name);
2080  }
2081  }
2082  {
2083  char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
2084  memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
2085  if (memPtr->m_jointName)
2086  {
2087  serializer->serializeName(name);
2088  }
2089  }
2090  memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
2091  }
2092  serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
2093  }
2094  mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;
2095 
2096  // Fill padding with zeros to appease msan.
2097 #ifdef BT_USE_DOUBLE_PRECISION
2098  memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
2099 #endif
2100 
2101  return btMultiBodyDataName;
2102 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
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.
Definition: btQuadWord.h:113
#define SIMD_EPSILON
Definition: btScalar.h:523
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)
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btVector3 localPosToWorld(int i, const btVector3 &vec) const
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:678
bool m_useGyroTerm
Definition: btMultiBody.h:700
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
int getNumLinks() const
Definition: btMultiBody.h:166
virtual ~btMultiBody()
btVector3 m_baseForce
Definition: btMultiBody.h:652
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
const btVector3 & getLinear() const
btScalar m_baseMass
Definition: btMultiBody.h:649
void finalizeMultiDof()
void setJointVelMultiDof(int i, const double *qdot)
bool m_fixedBase
Definition: btMultiBody.h:686
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
#define btMultiBodyLinkDataName
Definition: btMultiBody.h:43
btScalar btSin(btScalar x)
Definition: btScalar.h:479
void transformInverseRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
virtual void * getUniquePointer(void *oldPtr)=0
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
#define btAssert(x)
Definition: btScalar.h:133
void addLinkConstraintForce(int i, const btVector3 &f)
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:190
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:683
void setJointPosMultiDof(int i, const double *q)
btScalar * getJointVelMultiDof(int i)
void addLinkForce(int i, const btVector3 &f)
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
void addLinear(const btVector3 &linear)
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btVector3 m_baseConstraintTorque
Definition: btMultiBody.h:656
#define btCollisionObjectData
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:394
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
#define SIMD_HALF_PI
Definition: btScalar.h:508
const btVector3 & getLinkTorque(int i) const
btAlignedObjectArray< btScalar > m_deltaV
Definition: btMultiBody.h:675
btVector3 getBaseOmega() const
Definition: btMultiBody.h:194
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
void clearForcesAndTorques()
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3 m_baseTorque
Definition: btMultiBody.h:653
#define ANGULAR_MOTION_THRESHOLD
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1026
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:133
void addAngular(const btVector3 &angular)
void addLinkConstraintTorque(int i, const btVector3 &t)
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
virtual int calculateSerializeBufferSize() const
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:680
#define SIMD_INFINITY
Definition: btScalar.h:524
void transformRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
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)
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:170
const btQuaternion & getParentToLocalRot(int i) const
const btVector3 & getLinear() const
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
void setVector(const btVector3 &angular, const btVector3 &linear)
btQuaternion m_baseQuat
Definition: btMultiBody.h:647
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:682
btScalar getBaseMass() const
Definition: btMultiBody.h:169
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
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.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
#define output
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:149
int getNumDofs() const
Definition: btMultiBody.h:167
void setLinear(const btVector3 &linear)
int getParent(int link_num) const
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:160
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
bool m_cachedInertiaValid
Definition: btMultiBody.h:684
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)
Definition: btMatrix3x3.h:195
void setZero()
Definition: btVector3.h:671
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void setJointVel(int i, btScalar qdot)
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btScalar m_sleepTimer
Definition: btMultiBody.h:691
void setAngular(const btVector3 &angular)
void setJointPos(int i, btScalar q)
void setWorldTransform(const btTransform &worldTrans)
void clearVelocities()
btVector3 m_basePos
Definition: btMultiBody.h:646
void checkMotionAndSleepIfRequired(btScalar timestep)
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
btVector3 m_baseConstraintForce
Definition: btMultiBody.h:655
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
Definition: btMultiBody.h:713
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool deprecatedMultiDof=true)
Definition: btMultiBody.cpp:93
int size() const
return the number of elements in the array
btVector3 m_baseInertia
Definition: btMultiBody.h:650
const char * m_baseName
Definition: btMultiBody.h:644
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1317
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:676
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
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
void updateLinksDofOffsets()
Definition: btMultiBody.h:628
const btVector3 & getAngular() const
#define btMultiBodyDataName
Definition: btMultiBody.h:41
#define btMultiBodyData
serialization data, don&#39;t change them if you are not familiar with the details of the serialization m...
Definition: btMultiBody.h:40
static void spatialTransform(const btMatrix3x3 &rotation_matrix, const btVector3 &displacement, const btVector3 &top_in, const btVector3 &bottom_in, btVector3 &top_out, btVector3 &bottom_out)
Definition: btMultiBody.cpp:40
bool m_useGlobalVelocities
Definition: btMultiBody.h:708
btVector3 localDirToWorld(int i, const btVector3 &vec) const
virtual void serializeName(const char *ptr)=0
btScalar m_linearDamping
Definition: btMultiBody.h:698
void resize(int newsize, const T &fillData=T())
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
void goToSleep()
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
void setVector(const btVector3 &angular, const btVector3 &linear)
void transformInverse(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
btAlignedObjectArray< btVector3 > m_vectorBuf
Definition: btMultiBody.h:677
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
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
btScalar getLinkMass(int i) const
btScalar getKineticEnergy() const
virtual const char * findNameForPointer(const void *ptr) const =0
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
void addVector(const btVector3 &angular, const btVector3 &linear)
btScalar getJointVel(int i) const
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1070
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:658
btScalar * getJointTorqueMultiDof(int i)
#define btMultiBodyLinkData
Definition: btMultiBody.h:42
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
void * m_oldPtr
Definition: btSerializer.h:52
void serialize(struct btQuaternionData &dataOut) const
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:681
btScalar * getJointPosMultiDof(int i)
virtual btChunk * allocate(size_t size, int numElements)=0
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...
Definition: btScalar.h:294
btScalar m_angularDamping
Definition: btMultiBody.h:699
btScalar btCos(btScalar x)
Definition: btScalar.h:478
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
bool m_canSleep
Definition: btMultiBody.h:690
void transform(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
const btVector3 getBaseVel() const
Definition: btMultiBody.h:186