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_basePos_interpolate(0, 0, 0),
104  m_baseQuat_interpolate(0, 0, 0, 1),
105  m_baseMass(mass),
106  m_baseInertia(inertia),
107 
108  m_fixedBase(fixedBase),
109  m_awake(true),
110  m_canSleep(canSleep),
111  m_canWakeup(true),
112  m_sleepTimer(0),
113  m_userObjectPointer(0),
114  m_userIndex2(-1),
115  m_userIndex(-1),
116  m_companionId(-1),
117  m_linearDamping(0.04f),
118  m_angularDamping(0.04f),
119  m_useGyroTerm(true),
120  m_maxAppliedImpulse(1000.f),
121  m_maxCoordinateVelocity(100.f),
122  m_hasSelfCollision(true),
123  __posUpdated(false),
124  m_dofCount(0),
125  m_posVarCnt(0),
126  m_useRK4(false),
127  m_useGlobalVelocities(false),
128  m_internalNeedsJointFeedback(false),
129  m_kinematic_calculate_velocity(false)
130 {
131  m_cachedInertiaTopLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
132  m_cachedInertiaTopRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
133  m_cachedInertiaLowerLeft.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
134  m_cachedInertiaLowerRight.setValue(0, 0, 0, 0, 0, 0, 0, 0, 0);
135  m_cachedInertiaValid = false;
136 
137  m_links.resize(n_links);
138  m_matrixBuf.resize(n_links + 1);
139 
140  m_baseForce.setValue(0, 0, 0);
141  m_baseTorque.setValue(0, 0, 0);
142 
145 }
146 
148 {
149 }
150 
152  btScalar mass,
153  const btVector3 &inertia,
154  int parent,
155  const btQuaternion &rotParentToThis,
156  const btVector3 &parentComToThisPivotOffset,
157  const btVector3 &thisPivotToThisComOffset, bool /*deprecatedDisableParentCollision*/)
158 {
159  m_links[i].m_mass = mass;
160  m_links[i].m_inertiaLocal = inertia;
161  m_links[i].m_parent = parent;
162  m_links[i].setAxisTop(0, 0., 0., 0.);
163  m_links[i].setAxisBottom(0, btVector3(0, 0, 0));
164  m_links[i].m_zeroRotParentToThis = rotParentToThis;
165  m_links[i].m_dVector = thisPivotToThisComOffset;
166  m_links[i].m_eVector = parentComToThisPivotOffset;
167 
168  m_links[i].m_jointType = btMultibodyLink::eFixed;
169  m_links[i].m_dofCount = 0;
170  m_links[i].m_posVarCount = 0;
171 
173 
174  m_links[i].updateCacheMultiDof();
175 
177 }
178 
180  btScalar mass,
181  const btVector3 &inertia,
182  int parent,
183  const btQuaternion &rotParentToThis,
184  const btVector3 &jointAxis,
185  const btVector3 &parentComToThisPivotOffset,
186  const btVector3 &thisPivotToThisComOffset,
187  bool disableParentCollision)
188 {
189  m_dofCount += 1;
190  m_posVarCnt += 1;
191 
192  m_links[i].m_mass = mass;
193  m_links[i].m_inertiaLocal = inertia;
194  m_links[i].m_parent = parent;
195  m_links[i].m_zeroRotParentToThis = rotParentToThis;
196  m_links[i].setAxisTop(0, 0., 0., 0.);
197  m_links[i].setAxisBottom(0, jointAxis);
198  m_links[i].m_eVector = parentComToThisPivotOffset;
199  m_links[i].m_dVector = thisPivotToThisComOffset;
200  m_links[i].m_cachedRotParentToThis = rotParentToThis;
201 
202  m_links[i].m_jointType = btMultibodyLink::ePrismatic;
203  m_links[i].m_dofCount = 1;
204  m_links[i].m_posVarCount = 1;
205  m_links[i].m_jointPos[0] = 0.f;
206  m_links[i].m_jointTorque[0] = 0.f;
207 
208  if (disableParentCollision)
210  //
211 
212  m_links[i].updateCacheMultiDof();
213 
215 }
216 
218  btScalar mass,
219  const btVector3 &inertia,
220  int parent,
221  const btQuaternion &rotParentToThis,
222  const btVector3 &jointAxis,
223  const btVector3 &parentComToThisPivotOffset,
224  const btVector3 &thisPivotToThisComOffset,
225  bool disableParentCollision)
226 {
227  m_dofCount += 1;
228  m_posVarCnt += 1;
229 
230  m_links[i].m_mass = mass;
231  m_links[i].m_inertiaLocal = inertia;
232  m_links[i].m_parent = parent;
233  m_links[i].m_zeroRotParentToThis = rotParentToThis;
234  m_links[i].setAxisTop(0, jointAxis);
235  m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
236  m_links[i].m_dVector = thisPivotToThisComOffset;
237  m_links[i].m_eVector = parentComToThisPivotOffset;
238 
239  m_links[i].m_jointType = btMultibodyLink::eRevolute;
240  m_links[i].m_dofCount = 1;
241  m_links[i].m_posVarCount = 1;
242  m_links[i].m_jointPos[0] = 0.f;
243  m_links[i].m_jointTorque[0] = 0.f;
244 
245  if (disableParentCollision)
247  //
248  m_links[i].updateCacheMultiDof();
249  //
251 }
252 
254  btScalar mass,
255  const btVector3 &inertia,
256  int parent,
257  const btQuaternion &rotParentToThis,
258  const btVector3 &parentComToThisPivotOffset,
259  const btVector3 &thisPivotToThisComOffset,
260  bool disableParentCollision)
261 {
262  m_dofCount += 3;
263  m_posVarCnt += 4;
264 
265  m_links[i].m_mass = mass;
266  m_links[i].m_inertiaLocal = inertia;
267  m_links[i].m_parent = parent;
268  m_links[i].m_zeroRotParentToThis = rotParentToThis;
269  m_links[i].m_dVector = thisPivotToThisComOffset;
270  m_links[i].m_eVector = parentComToThisPivotOffset;
271 
272  m_links[i].m_jointType = btMultibodyLink::eSpherical;
273  m_links[i].m_dofCount = 3;
274  m_links[i].m_posVarCount = 4;
275  m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
276  m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
277  m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
278  m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
279  m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
280  m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
281  m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
282  m_links[i].m_jointPos[3] = 1.f;
283  m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
284 
285  if (disableParentCollision)
287  //
288  m_links[i].updateCacheMultiDof();
289  //
291 }
292 
294  btScalar mass,
295  const btVector3 &inertia,
296  int parent,
297  const btQuaternion &rotParentToThis,
298  const btVector3 &rotationAxis,
299  const btVector3 &parentComToThisComOffset,
300  bool disableParentCollision)
301 {
302  m_dofCount += 3;
303  m_posVarCnt += 3;
304 
305  m_links[i].m_mass = mass;
306  m_links[i].m_inertiaLocal = inertia;
307  m_links[i].m_parent = parent;
308  m_links[i].m_zeroRotParentToThis = rotParentToThis;
309  m_links[i].m_dVector.setZero();
310  m_links[i].m_eVector = parentComToThisComOffset;
311 
312  //
313  btVector3 vecNonParallelToRotAxis(1, 0, 0);
314  if (rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
315  vecNonParallelToRotAxis.setValue(0, 1, 0);
316  //
317 
318  m_links[i].m_jointType = btMultibodyLink::ePlanar;
319  m_links[i].m_dofCount = 3;
320  m_links[i].m_posVarCount = 3;
321  btVector3 n = rotationAxis.normalized();
322  m_links[i].setAxisTop(0, n[0], n[1], n[2]);
323  m_links[i].setAxisTop(1, 0, 0, 0);
324  m_links[i].setAxisTop(2, 0, 0, 0);
325  m_links[i].setAxisBottom(0, 0, 0, 0);
326  btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
327  m_links[i].setAxisBottom(1, cr[0], cr[1], cr[2]);
328  cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
329  m_links[i].setAxisBottom(2, cr[0], cr[1], cr[2]);
330  m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
331  m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
332 
333  if (disableParentCollision)
335  //
336  m_links[i].updateCacheMultiDof();
337  //
339 
340  m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized());
341  m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized());
342 }
343 
345 {
346  m_deltaV.resize(0);
348  m_splitV.resize(0);
350  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")
351  m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
353  for (int i = 0; i < m_vectorBuf.size(); i++)
354  {
355  m_vectorBuf[i].setValue(0, 0, 0);
356  }
358 }
359 
360 int btMultiBody::getParent(int link_num) const
361 {
362  return m_links[link_num].m_parent;
363 }
364 
366 {
367  return m_links[i].m_mass;
368 }
369 
371 {
372  return m_links[i].m_inertiaLocal;
373 }
374 
376 {
377  return m_links[i].m_jointPos[0];
378 }
379 
381 {
382  return m_realBuf[6 + m_links[i].m_dofOffset];
383 }
384 
386 {
387  return &m_links[i].m_jointPos[0];
388 }
389 
391 {
392  return &m_realBuf[6 + m_links[i].m_dofOffset];
393 }
394 
396 {
397  return &m_links[i].m_jointPos[0];
398 }
399 
401 {
402  return &m_realBuf[6 + m_links[i].m_dofOffset];
403 }
404 
406 {
407  m_links[i].m_jointPos[0] = q;
408  m_links[i].updateCacheMultiDof();
409 }
410 
411 
412 void btMultiBody::setJointPosMultiDof(int i, const double *q)
413 {
414  for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
415  m_links[i].m_jointPos[pos] = (btScalar)q[pos];
416 
417  m_links[i].updateCacheMultiDof();
418 }
419 
420 void btMultiBody::setJointPosMultiDof(int i, const float *q)
421 {
422  for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
423  m_links[i].m_jointPos[pos] = (btScalar)q[pos];
424 
425  m_links[i].updateCacheMultiDof();
426 }
427 
428 
429 
431 {
432  m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
433 }
434 
435 void btMultiBody::setJointVelMultiDof(int i, const double *qdot)
436 {
437  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
438  m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
439 }
440 
441 void btMultiBody::setJointVelMultiDof(int i, const float* qdot)
442 {
443  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
444  m_realBuf[6 + m_links[i].m_dofOffset + dof] = (btScalar)qdot[dof];
445 }
446 
448 {
449  return m_links[i].m_cachedRVector;
450 }
451 
453 {
454  return m_links[i].m_cachedRotParentToThis;
455 }
456 
458 {
459  return m_links[i].m_cachedRVector_interpolate;
460 }
461 
463 {
464  return m_links[i].m_cachedRotParentToThis_interpolate;
465 }
466 
467 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
468 {
469  btAssert(i >= -1);
470  btAssert(i < m_links.size());
471  if ((i < -1) || (i >= m_links.size()))
472  {
474  }
475 
476  btVector3 result = local_pos;
477  while (i != -1)
478  {
479  // 'result' is in frame i. transform it to frame parent(i)
480  result += getRVector(i);
481  result = quatRotate(getParentToLocalRot(i).inverse(), result);
482  i = getParent(i);
483  }
484 
485  // 'result' is now in the base frame. transform it to world frame
486  result = quatRotate(getWorldToBaseRot().inverse(), result);
487  result += getBasePos();
488 
489  return result;
490 }
491 
492 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
493 {
494  btAssert(i >= -1);
495  btAssert(i < m_links.size());
496  if ((i < -1) || (i >= m_links.size()))
497  {
499  }
500 
501  if (i == -1)
502  {
503  // world to base
504  return quatRotate(getWorldToBaseRot(), (world_pos - getBasePos()));
505  }
506  else
507  {
508  // find position in parent frame, then transform to current frame
509  return quatRotate(getParentToLocalRot(i), worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
510  }
511 }
512 
513 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
514 {
515  btAssert(i >= -1);
516  btAssert(i < m_links.size());
517  if ((i < -1) || (i >= m_links.size()))
518  {
520  }
521 
522  btVector3 result = local_dir;
523  while (i != -1)
524  {
525  result = quatRotate(getParentToLocalRot(i).inverse(), result);
526  i = getParent(i);
527  }
528  result = quatRotate(getWorldToBaseRot().inverse(), result);
529  return result;
530 }
531 
532 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
533 {
534  btAssert(i >= -1);
535  btAssert(i < m_links.size());
536  if ((i < -1) || (i >= m_links.size()))
537  {
539  }
540 
541  if (i == -1)
542  {
543  return quatRotate(getWorldToBaseRot(), world_dir);
544  }
545  else
546  {
547  return quatRotate(getParentToLocalRot(i), worldDirToLocal(getParent(i), world_dir));
548  }
549 }
550 
552 {
553  btMatrix3x3 result = local_frame;
554  btVector3 frameInWorld0 = localDirToWorld(i, local_frame.getColumn(0));
555  btVector3 frameInWorld1 = localDirToWorld(i, local_frame.getColumn(1));
556  btVector3 frameInWorld2 = localDirToWorld(i, local_frame.getColumn(2));
557  result.setValue(frameInWorld0[0], frameInWorld1[0], frameInWorld2[0], frameInWorld0[1], frameInWorld1[1], frameInWorld2[1], frameInWorld0[2], frameInWorld1[2], frameInWorld2[2]);
558  return result;
559 }
560 
562 {
563  int num_links = getNumLinks();
564  // Calculates the velocities of each link (and the base) in its local frame
565  const btQuaternion& base_rot = getWorldToBaseRot();
566  omega[0] = quatRotate(base_rot, getBaseOmega());
567  vel[0] = quatRotate(base_rot, getBaseVel());
568 
569  for (int i = 0; i < num_links; ++i)
570  {
571  const btMultibodyLink& link = getLink(i);
572  const int parent = link.m_parent;
573 
574  // transform parent vel into this frame, store in omega[i+1], vel[i+1]
576  omega[parent + 1], vel[parent + 1],
577  omega[i + 1], vel[i + 1]);
578 
579  // now add qidot * shat_i
580  const btScalar* jointVel = getJointVelMultiDof(i);
581  for (int dof = 0; dof < link.m_dofCount; ++dof)
582  {
583  omega[i + 1] += jointVel[dof] * link.getAxisTop(dof);
584  vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof);
585  }
586  }
587 }
588 
589 
591 {
594 
595  for (int i = 0; i < getNumLinks(); ++i)
596  {
597  m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
598  m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
599  }
600 }
602 {
603  m_baseForce.setValue(0, 0, 0);
604  m_baseTorque.setValue(0, 0, 0);
605 
606  for (int i = 0; i < getNumLinks(); ++i)
607  {
608  m_links[i].m_appliedForce.setValue(0, 0, 0);
609  m_links[i].m_appliedTorque.setValue(0, 0, 0);
610  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;
611  }
612 }
613 
615 {
616  for (int i = 0; i < 6 + getNumDofs(); ++i)
617  {
618  m_realBuf[i] = 0.f;
619  }
620 }
622 {
623  m_links[i].m_appliedForce += f;
624 }
625 
627 {
628  m_links[i].m_appliedTorque += t;
629 }
630 
632 {
633  m_links[i].m_appliedConstraintForce += f;
634 }
635 
637 {
638  m_links[i].m_appliedConstraintTorque += t;
639 }
640 
642 {
643  m_links[i].m_jointTorque[0] += Q;
644 }
645 
647 {
648  m_links[i].m_jointTorque[dof] += Q;
649 }
650 
652 {
653  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
654  m_links[i].m_jointTorque[dof] = Q[dof];
655 }
656 
658 {
659  return m_links[i].m_appliedForce;
660 }
661 
663 {
664  return m_links[i].m_appliedTorque;
665 }
666 
668 {
669  return m_links[i].m_jointTorque[0];
670 }
671 
673 {
674  return &m_links[i].m_jointTorque[0];
675 }
676 
678 {
680 }
681 
683 {
685 }
686 
688 {
690 }
691 
692 void btMultiBody::setBaseDynamicType(int dynamicType)
693 {
694  if(getBaseCollider()) {
695  int oldFlags = getBaseCollider()->getCollisionFlags();
697  getBaseCollider()->setCollisionFlags(oldFlags | dynamicType);
698  }
699 }
700 
701 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?
702 {
703  btVector3 row0 = btVector3(
704  v0.x() * v1.x(),
705  v0.x() * v1.y(),
706  v0.x() * v1.z());
707  btVector3 row1 = btVector3(
708  v0.y() * v1.x(),
709  v0.y() * v1.y(),
710  v0.y() * v1.z());
711  btVector3 row2 = btVector3(
712  v0.z() * v1.x(),
713  v0.z() * v1.y(),
714  v0.z() * v1.z());
715 
716  btMatrix3x3 m(row0[0], row0[1], row0[2],
717  row1[0], row1[1], row1[2],
718  row2[0], row2[1], row2[2]);
719  return m;
720 }
721 
722 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
723 //
724 
729  bool isConstraintPass,
730  bool jointFeedbackInWorldSpace,
731  bool jointFeedbackInJointFrame)
732 {
733  // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
734  // and the base linear & angular accelerations.
735 
736  // We apply damping forces in this routine as well as any external forces specified by the
737  // caller (via addBaseForce etc).
738 
739  // output should point to an array of 6 + num_links reals.
740  // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
741  // num_links joint acceleration values.
742 
743  // We added support for multi degree of freedom (multi dof) joints.
744  // In addition we also can compute the joint reaction forces. This is performed in a second pass,
745  // so that we can include the effect of the constraint solver forces (computed in the PGS LCP solver)
746 
748 
749  int num_links = getNumLinks();
750 
751  const btScalar DAMPING_K1_LINEAR = m_linearDamping;
752  const btScalar DAMPING_K2_LINEAR = m_linearDamping;
753 
754  const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
755  const btScalar DAMPING_K2_ANGULAR = m_angularDamping;
756 
757  const btVector3 base_vel = getBaseVel();
758  const btVector3 base_omega = getBaseOmega();
759 
760  // Temporary matrices/vectors -- use scratch space from caller
761  // so that we don't have to keep reallocating every frame
762 
763  scratch_r.resize(2 * m_dofCount + 7); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
764  scratch_v.resize(8 * num_links + 6);
765  scratch_m.resize(4 * num_links + 4);
766 
767  //btScalar * r_ptr = &scratch_r[0];
768  btScalar *output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
769  btVector3 *v_ptr = &scratch_v[0];
770 
771  // vhat_i (top = angular, bottom = linear part)
772  btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
773  v_ptr += num_links * 2 + 2;
774  //
775  // zhat_i^A
776  btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
777  v_ptr += num_links * 2 + 2;
778  //
779  // chat_i (note NOT defined for the base)
780  btSpatialMotionVector *spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
781  v_ptr += num_links * 2;
782  //
783  // Ihat_i^A.
784  btSymmetricSpatialDyad *spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
785 
786  // Cached 3x3 rotation matrices from parent frame to this frame.
787  btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
788  btMatrix3x3 *rot_from_world = &scratch_m[0];
789 
790  // hhat_i, ahat_i
791  // hhat is NOT stored for the base (but ahat is)
793  btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
794  v_ptr += num_links * 2 + 2;
795  //
796  // Y_i, invD_i
797  btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
798  btScalar *Y = &scratch_r[0];
799  //
800  //aux variables
801  btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
802  btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
803  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
804  btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
805  btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
806  btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
807  btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
808  btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
810  fromWorld.m_trnVec.setZero();
812 
813  // ptr to the joint accel part of the output
814  btScalar *joint_accel = output + 6;
815 
816  // Start of the algorithm proper.
817 
818  // First 'upward' loop.
819  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
820 
821  rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
822 
823  //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
824  spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
825 
827  {
828  zeroAccSpatFrc[0].setZero();
829  }
830  else
831  {
832  const btVector3 &baseForce = isConstraintPass ? m_baseConstraintForce : m_baseForce;
833  const btVector3 &baseTorque = isConstraintPass ? m_baseConstraintTorque : m_baseTorque;
834  //external forces
835  zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
836 
837  //adding damping terms (only)
838  const btScalar linDampMult = 1., angDampMult = 1.;
839  zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()),
840  linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm()));
841 
842  //
843  //p += vhat x Ihat vhat - done in a simpler way
844  if (m_useGyroTerm)
845  zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
846  //
847  zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
848  }
849 
850  //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
851  spatInertia[0].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
852  //
853  btMatrix3x3(m_baseMass, 0, 0,
854  0, m_baseMass, 0,
855  0, 0, m_baseMass),
856  //
857  btMatrix3x3(m_baseInertia[0], 0, 0,
858  0, m_baseInertia[1], 0,
859  0, 0, m_baseInertia[2]));
860 
861  rot_from_world[0] = rot_from_parent[0];
862 
863  //
864  for (int i = 0; i < num_links; ++i)
865  {
866  const int parent = m_links[i].m_parent;
867  rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
868  rot_from_world[i + 1] = rot_from_parent[i + 1] * rot_from_world[parent + 1];
869 
870  fromParent.m_rotMat = rot_from_parent[i + 1];
871  fromParent.m_trnVec = m_links[i].m_cachedRVector;
872  fromWorld.m_rotMat = rot_from_world[i + 1];
873  fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
874 
875  // now set vhat_i to its true value by doing
876  // vhat_i += qidot * shat_i
878  {
879  spatJointVel.setZero();
880 
881  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
882  spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
883 
884  // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
885  spatVel[i + 1] += spatJointVel;
886 
887  //
888  // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
889  //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
890  }
891  else
892  {
893  fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i + 1]);
894  fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
895  }
896 
897  // we can now calculate chat_i
898  spatVel[i + 1].cross(spatJointVel, spatCoriolisAcc[i]);
899 
900  // calculate zhat_i^A
901  //
903  {
904  zeroAccSpatFrc[i].setZero();
905  }
906  else{
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  //p += vhat x Ihat vhat - done in a simpler way
933  if (m_useGyroTerm)
934  zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
935  //
936  zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
937  //
938  //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
940  //btScalar parOmegaMod = temp.length();
941  //btScalar parOmegaModMax = 1000;
942  //if(parOmegaMod > parOmegaModMax)
943  // temp *= parOmegaModMax / parOmegaMod;
944  //zeroAccSpatFrc[i+1].addLinear(temp);
945  //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
946  //temp = spatCoriolisAcc[i].getLinear();
947  //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
948  }
949 
950  // calculate Ihat_i^A
951  //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
952  spatInertia[i + 1].setMatrix(btMatrix3x3(0, 0, 0, 0, 0, 0, 0, 0, 0),
953  //
954  btMatrix3x3(m_links[i].m_mass, 0, 0,
955  0, m_links[i].m_mass, 0,
956  0, 0, m_links[i].m_mass),
957  //
958  btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
959  0, m_links[i].m_inertiaLocal[1], 0,
960  0, 0, m_links[i].m_inertiaLocal[2]));
961 
962  //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());
963  //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());
964  //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
965  }
966 
967  // 'Downward' loop.
968  // (part of TreeForwardDynamics in Mirtich.)
969  for (int i = num_links - 1; i >= 0; --i)
970  {
972  continue;
973  const int parent = m_links[i].m_parent;
974  fromParent.m_rotMat = rot_from_parent[i + 1];
975  fromParent.m_trnVec = m_links[i].m_cachedRVector;
976 
977  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
978  {
979  btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
980  //
981  hDof = spatInertia[i + 1] * m_links[i].m_axes[dof];
982  //
983  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);
984  }
985  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
986  {
987  btScalar *D_row = &D[dof * m_links[i].m_dofCount];
988  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
989  {
990  const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
991  D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
992  }
993  }
994 
995  btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
996  switch (m_links[i].m_jointType)
997  {
1000  {
1001  if (D[0] >= SIMD_EPSILON)
1002  {
1003  invDi[0] = 1.0f / D[0];
1004  }
1005  else
1006  {
1007  invDi[0] = 0;
1008  }
1009  break;
1010  }
1013  {
1014  const btMatrix3x3 D3x3(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1015  const btMatrix3x3 invD3x3(D3x3.inverse());
1016 
1017  //unroll the loop?
1018  for (int row = 0; row < 3; ++row)
1019  {
1020  for (int col = 0; col < 3; ++col)
1021  {
1022  invDi[row * 3 + col] = invD3x3[row][col];
1023  }
1024  }
1025 
1026  break;
1027  }
1028  default:
1029  {
1030  }
1031  }
1032 
1033  //determine h*D^{-1}
1034  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1035  {
1036  spatForceVecTemps[dof].setZero();
1037 
1038  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1039  {
1040  const btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
1041  //
1042  spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
1043  }
1044  }
1045 
1046  dyadTemp = spatInertia[i + 1];
1047 
1048  //determine (h*D^{-1}) * h^{T}
1049  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1050  {
1051  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1052  //
1053  dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
1054  }
1055 
1056  fromParent.transformInverse(dyadTemp, spatInertia[parent + 1], btSpatialTransformationMatrix::Add);
1057 
1058  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1059  {
1060  invD_times_Y[dof] = 0.f;
1061 
1062  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1063  {
1064  invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1065  }
1066  }
1067 
1068  spatForceVecTemps[0] = zeroAccSpatFrc[i + 1] + spatInertia[i + 1] * spatCoriolisAcc[i];
1069 
1070  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1071  {
1072  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1073  //
1074  spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1075  }
1076 
1077  fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1078 
1079  zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1080  }
1081 
1082  // Second 'upward' loop
1083  // (part of TreeForwardDynamics in Mirtich)
1084 
1086  {
1087  spatAcc[0].setZero();
1088  }
1089  else
1090  {
1091  if (num_links > 0)
1092  {
1093  m_cachedInertiaValid = true;
1094  m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
1095  m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
1096  m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
1098  }
1099 
1100  solveImatrix(zeroAccSpatFrc[0], result);
1101  spatAcc[0] = -result;
1102  }
1103 
1104  // now do the loop over the m_links
1105  for (int i = 0; i < num_links; ++i)
1106  {
1107  // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
1108  // a = apar + cor + Sqdd
1109  //or
1110  // qdd = D^{-1} * (Y - h^{T}*(apar+cor))
1111  // a = apar + Sqdd
1112 
1113  const int parent = m_links[i].m_parent;
1114  fromParent.m_rotMat = rot_from_parent[i + 1];
1115  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1116 
1117  fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
1118 
1120  {
1121  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1122  {
1123  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1124  //
1125  Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
1126  }
1127  btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1128  //D^{-1} * (Y - h^{T}*apar)
1129  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]);
1130 
1131  spatAcc[i + 1] += spatCoriolisAcc[i];
1132 
1133  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1134  spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1135  }
1136 
1137  if (m_links[i].m_jointFeedback)
1138  {
1140 
1141  btVector3 angularBotVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_bottomVec;
1142  btVector3 linearTopVec = (spatInertia[i + 1] * spatAcc[i + 1] + zeroAccSpatFrc[i + 1]).m_topVec;
1143 
1144  if (jointFeedbackInJointFrame)
1145  {
1146  //shift the reaction forces to the joint frame
1147  //linear (force) component is the same
1148  //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
1149  angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
1150  }
1151 
1152  if (jointFeedbackInWorldSpace)
1153  {
1154  if (isConstraintPass)
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  else
1160  {
1161  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis() * angularBotVec;
1162  m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis() * linearTopVec;
1163  }
1164  }
1165  else
1166  {
1167  if (isConstraintPass)
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  else
1173  {
1174  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1175  m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1176  }
1177  }
1178  }
1179  }
1180 
1181  // transform base accelerations back to the world frame.
1182  const btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1183  output[0] = omegadot_out[0];
1184  output[1] = omegadot_out[1];
1185  output[2] = omegadot_out[2];
1186 
1187  const btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1188  output[3] = vdot_out[0];
1189  output[4] = vdot_out[1];
1190  output[5] = vdot_out[2];
1191 
1193  //printf("q = [");
1194  //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());
1195  //for(int link = 0; link < getNumLinks(); ++link)
1196  // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1197  // printf("%.6f ", m_links[link].m_jointPos[dof]);
1198  //printf("]\n");
1200  //printf("qd = [");
1201  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1202  // printf("%.6f ", m_realBuf[dof]);
1203  //printf("]\n");
1204  //printf("qdd = [");
1205  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1206  // printf("%.6f ", output[dof]);
1207  //printf("]\n");
1209 
1210  // Final step: add the accelerations (times dt) to the velocities.
1211 
1212  if (!isConstraintPass)
1213  {
1214  if (dt > 0.)
1216  }
1218  //btScalar angularThres = 1;
1219  //btScalar maxAngVel = 0.;
1220  //bool scaleDown = 1.;
1221  //for(int link = 0; link < m_links.size(); ++link)
1222  //{
1223  // if(spatVel[link+1].getAngular().length() > maxAngVel)
1224  // {
1225  // maxAngVel = spatVel[link+1].getAngular().length();
1226  // scaleDown = angularThres / spatVel[link+1].getAngular().length();
1227  // break;
1228  // }
1229  //}
1230 
1231  //if(scaleDown != 1.)
1232  //{
1233  // for(int link = 0; link < m_links.size(); ++link)
1234  // {
1235  // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
1236  // {
1237  // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1238  // getJointVelMultiDof(link)[dof] *= scaleDown;
1239  // }
1240  // }
1241  //}
1243 
1246  {
1247  for (int i = 0; i < num_links; ++i)
1248  {
1249  const int parent = m_links[i].m_parent;
1250  //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
1251  //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
1252 
1253  fromParent.m_rotMat = rot_from_parent[i + 1];
1254  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1255  fromWorld.m_rotMat = rot_from_world[i + 1];
1256 
1257  // vhat_i = i_xhat_p(i) * vhat_p(i)
1258  fromParent.transform(spatVel[parent + 1], spatVel[i + 1]);
1259  //nice alternative below (using operator *) but it generates temps
1261 
1262  // now set vhat_i to its true value by doing
1263  // vhat_i += qidot * shat_i
1264  spatJointVel.setZero();
1265 
1266  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1267  spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1268 
1269  // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
1270  spatVel[i + 1] += spatJointVel;
1271 
1272  fromWorld.transformInverseRotationOnly(spatVel[i + 1], m_links[i].m_absFrameTotVelocity);
1273  fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
1274  }
1275  }
1276 }
1277 
1278 void btMultiBody::solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
1279 {
1280  int num_links = getNumLinks();
1282  if (num_links == 0)
1283  {
1284  // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1285 
1287  {
1288  result[0] = rhs_bot[0] / m_baseInertia[0];
1289  result[1] = rhs_bot[1] / m_baseInertia[1];
1290  result[2] = rhs_bot[2] / m_baseInertia[2];
1291  }
1292  else
1293  {
1294  result[0] = 0;
1295  result[1] = 0;
1296  result[2] = 0;
1297  }
1298  if (m_baseMass >= SIMD_EPSILON)
1299  {
1300  result[3] = rhs_top[0] / m_baseMass;
1301  result[4] = rhs_top[1] / m_baseMass;
1302  result[5] = rhs_top[2] / m_baseMass;
1303  }
1304  else
1305  {
1306  result[3] = 0;
1307  result[4] = 0;
1308  result[5] = 0;
1309  }
1310  }
1311  else
1312  {
1313  if (!m_cachedInertiaValid)
1314  {
1315  for (int i = 0; i < 6; i++)
1316  {
1317  result[i] = 0.f;
1318  }
1319  return;
1320  }
1326  tmp = invIupper_right * m_cachedInertiaLowerRight;
1327  btMatrix3x3 invI_upper_left = (tmp * Binv);
1328  btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1329  tmp = m_cachedInertiaTopLeft * invI_upper_left;
1330  tmp[0][0] -= 1.0;
1331  tmp[1][1] -= 1.0;
1332  tmp[2][2] -= 1.0;
1333  btMatrix3x3 invI_lower_left = (Binv * tmp);
1334 
1335  //multiply result = invI * rhs
1336  {
1337  btVector3 vtop = invI_upper_left * rhs_top;
1338  btVector3 tmp;
1339  tmp = invIupper_right * rhs_bot;
1340  vtop += tmp;
1341  btVector3 vbot = invI_lower_left * rhs_top;
1342  tmp = invI_lower_right * rhs_bot;
1343  vbot += tmp;
1344  result[0] = vtop[0];
1345  result[1] = vtop[1];
1346  result[2] = vtop[2];
1347  result[3] = vbot[0];
1348  result[4] = vbot[1];
1349  result[5] = vbot[2];
1350  }
1351  }
1352 }
1354 {
1355  int num_links = getNumLinks();
1357  if (num_links == 0)
1358  {
1359  // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1361  {
1362  result.setAngular(rhs.getAngular() / m_baseInertia);
1363  }
1364  else
1365  {
1366  result.setAngular(btVector3(0, 0, 0));
1367  }
1368  if (m_baseMass >= SIMD_EPSILON)
1369  {
1370  result.setLinear(rhs.getLinear() / m_baseMass);
1371  }
1372  else
1373  {
1374  result.setLinear(btVector3(0, 0, 0));
1375  }
1376  }
1377  else
1378  {
1381  if (!m_cachedInertiaValid)
1382  {
1383  result.setLinear(btVector3(0, 0, 0));
1384  result.setAngular(btVector3(0, 0, 0));
1385  result.setVector(btVector3(0, 0, 0), btVector3(0, 0, 0));
1386  return;
1387  }
1391  tmp = invIupper_right * m_cachedInertiaLowerRight;
1392  btMatrix3x3 invI_upper_left = (tmp * Binv);
1393  btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1394  tmp = m_cachedInertiaTopLeft * invI_upper_left;
1395  tmp[0][0] -= 1.0;
1396  tmp[1][1] -= 1.0;
1397  tmp[2][2] -= 1.0;
1398  btMatrix3x3 invI_lower_left = (Binv * tmp);
1399 
1400  //multiply result = invI * rhs
1401  {
1402  btVector3 vtop = invI_upper_left * rhs.getLinear();
1403  btVector3 tmp;
1404  tmp = invIupper_right * rhs.getAngular();
1405  vtop += tmp;
1406  btVector3 vbot = invI_lower_left * rhs.getLinear();
1407  tmp = invI_lower_right * rhs.getAngular();
1408  vbot += tmp;
1409  result.setVector(vtop, vbot);
1410  }
1411  }
1412 }
1413 
1414 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
1415 {
1416  for (int row = 0; row < rowsA; row++)
1417  {
1418  for (int col = 0; col < colsB; col++)
1419  {
1420  pC[row * colsB + col] = 0.f;
1421  for (int inner = 0; inner < rowsB; inner++)
1422  {
1423  pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1424  }
1425  }
1426  }
1427 }
1428 
1431 {
1432  // Temporary matrices/vectors -- use scratch space from caller
1433  // so that we don't have to keep reallocating every frame
1434 
1435  int num_links = getNumLinks();
1436  scratch_r.resize(m_dofCount);
1437  scratch_v.resize(4 * num_links + 4);
1438 
1439  btScalar *r_ptr = m_dofCount ? &scratch_r[0] : 0;
1440  btVector3 *v_ptr = &scratch_v[0];
1441 
1442  // zhat_i^A (scratch space)
1443  btSpatialForceVector *zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
1444  v_ptr += num_links * 2 + 2;
1445 
1446  // rot_from_parent (cached from calcAccelerations)
1447  const btMatrix3x3 *rot_from_parent = &m_matrixBuf[0];
1448 
1449  // hhat (cached), accel (scratch)
1450  // hhat is NOT stored for the base (but ahat is)
1451  const btSpatialForceVector *h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
1452  btSpatialMotionVector *spatAcc = (btSpatialMotionVector *)v_ptr;
1453  v_ptr += num_links * 2 + 2;
1454 
1455  // Y_i (scratch), invD_i (cached)
1456  const btScalar *invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1457  btScalar *Y = r_ptr;
1459  //aux variables
1460  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
1461  btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
1462  btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
1463  btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
1464  btSpatialTransformationMatrix fromParent;
1466 
1467  // First 'upward' loop.
1468  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1469 
1470  // Fill in zero_acc
1471  // -- set to force/torque on the base, zero otherwise
1473  {
1474  zeroAccSpatFrc[0].setZero();
1475  }
1476  else
1477  {
1478  //test forces
1479  fromParent.m_rotMat = rot_from_parent[0];
1480  fromParent.transformRotationOnly(btSpatialForceVector(-force[0], -force[1], -force[2], -force[3], -force[4], -force[5]), zeroAccSpatFrc[0]);
1481  }
1482  for (int i = 0; i < num_links; ++i)
1483  {
1484  zeroAccSpatFrc[i + 1].setZero();
1485  }
1486 
1487  // 'Downward' loop.
1488  // (part of TreeForwardDynamics in Mirtich.)
1489  for (int i = num_links - 1; i >= 0; --i)
1490  {
1492  continue;
1493  const int parent = m_links[i].m_parent;
1494  fromParent.m_rotMat = rot_from_parent[i + 1];
1495  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1496 
1497  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1498  {
1499  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]);
1500  }
1501 
1502  btVector3 in_top, in_bottom, out_top, out_bottom;
1503  const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1504 
1505  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1506  {
1507  invD_times_Y[dof] = 0.f;
1508 
1509  for (int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1510  {
1511  invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1512  }
1513  }
1514 
1515  // Zp += pXi * (Zi + hi*Yi/Di)
1516  spatForceVecTemps[0] = zeroAccSpatFrc[i + 1];
1517 
1518  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1519  {
1520  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1521  //
1522  spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1523  }
1524 
1525  fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1526 
1527  zeroAccSpatFrc[parent + 1] += spatForceVecTemps[1];
1528  }
1529 
1530  // ptr to the joint accel part of the output
1531  btScalar *joint_accel = output + 6;
1532 
1533  // Second 'upward' loop
1534  // (part of TreeForwardDynamics in Mirtich)
1535 
1537  {
1538  spatAcc[0].setZero();
1539  }
1540  else
1541  {
1542  solveImatrix(zeroAccSpatFrc[0], result);
1543  spatAcc[0] = -result;
1544  }
1545 
1546  // now do the loop over the m_links
1547  for (int i = 0; i < num_links; ++i)
1548  {
1550  continue;
1551  const int parent = m_links[i].m_parent;
1552  fromParent.m_rotMat = rot_from_parent[i + 1];
1553  fromParent.m_trnVec = m_links[i].m_cachedRVector;
1554 
1555  fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
1556 
1557  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1558  {
1559  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1560  //
1561  Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
1562  }
1563 
1564  const btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
1565  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]);
1566 
1567  for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1568  spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1569  }
1570 
1571  // transform base accelerations back to the world frame.
1572  btVector3 omegadot_out;
1573  omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1574  output[0] = omegadot_out[0];
1575  output[1] = omegadot_out[1];
1576  output[2] = omegadot_out[2];
1577 
1578  btVector3 vdot_out;
1579  vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
1580  output[3] = vdot_out[0];
1581  output[4] = vdot_out[1];
1582  output[5] = vdot_out[2];
1583 
1585  //printf("delta = [");
1586  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1587  // printf("%.2f ", output[dof]);
1588  //printf("]\n");
1590 }
1592 {
1593  int num_links = getNumLinks();
1594  if(!isBaseKinematic())
1595  {
1596  // step position by adding dt * velocity
1597  //btVector3 v = getBaseVel();
1598  //m_basePos += dt * v;
1599  //
1600  btScalar *pBasePos;
1601  btScalar *pBaseVel = &m_realBuf[3]; //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
1602 
1603  // reset to current position
1604  for (int i = 0; i < 3; ++i)
1605  {
1607  }
1608  pBasePos = m_basePos_interpolate;
1609 
1610  pBasePos[0] += dt * pBaseVel[0];
1611  pBasePos[1] += dt * pBaseVel[1];
1612  pBasePos[2] += dt * pBaseVel[2];
1613  }
1614 
1616  //local functor for quaternion integration (to avoid error prone redundancy)
1617  struct
1618  {
1619  //"exponential map" based on btTransformUtil::integrateTransform(..)
1620  void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
1621  {
1622  //baseBody => quat is alias and omega is global coor
1624 
1625  btVector3 axis;
1626  btVector3 angvel;
1627 
1628  if (!baseBody)
1629  angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
1630  else
1631  angvel = omega;
1632 
1633  btScalar fAngle = angvel.length();
1634  //limit the angular motion
1635  if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
1636  {
1637  fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
1638  }
1639 
1640  if (fAngle < btScalar(0.001))
1641  {
1642  // use Taylor's expansions of sync function
1643  axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
1644  }
1645  else
1646  {
1647  // sync(fAngle) = sin(c*fAngle)/t
1648  axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
1649  }
1650 
1651  if (!baseBody)
1652  quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
1653  else
1654  quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
1655  //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
1656 
1657  quat.normalize();
1658  }
1659  } pQuatUpdateFun;
1661 
1662  //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
1663  //
1664  if(!isBaseKinematic())
1665  {
1666  btScalar *pBaseQuat;
1667 
1668  // reset to current orientation
1669  for (int i = 0; i < 4; ++i)
1670  {
1672  }
1673  pBaseQuat = m_baseQuat_interpolate;
1674 
1675  btScalar *pBaseOmega = &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
1676  //
1677  btQuaternion baseQuat;
1678  baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1679  btVector3 baseOmega;
1680  baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1681  pQuatUpdateFun(baseOmega, baseQuat, true, dt);
1682  pBaseQuat[0] = baseQuat.x();
1683  pBaseQuat[1] = baseQuat.y();
1684  pBaseQuat[2] = baseQuat.z();
1685  pBaseQuat[3] = baseQuat.w();
1686  }
1687 
1688  // Finally we can update m_jointPos for each of the m_links
1689  for (int i = 0; i < num_links; ++i)
1690  {
1691  btScalar *pJointPos;
1692  pJointPos = &m_links[i].m_jointPos_interpolate[0];
1693 
1694  if (m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic())
1695  {
1696  switch (m_links[i].m_jointType)
1697  {
1700  {
1701  pJointPos[0] = m_links[i].m_jointPos[0];
1702  break;
1703  }
1705  {
1706  for (int j = 0; j < 4; ++j)
1707  {
1708  pJointPos[j] = m_links[i].m_jointPos[j];
1709  }
1710  break;
1711  }
1713  {
1714  for (int j = 0; j < 3; ++j)
1715  {
1716  pJointPos[j] = m_links[i].m_jointPos[j];
1717  }
1718  break;
1719  }
1720  default:
1721  break;
1722  }
1723  }
1724  else
1725  {
1726  btScalar *pJointVel = getJointVelMultiDof(i);
1727 
1728  switch (m_links[i].m_jointType)
1729  {
1732  {
1733  //reset to current pos
1734  pJointPos[0] = m_links[i].m_jointPos[0];
1735  btScalar jointVel = pJointVel[0];
1736  pJointPos[0] += dt * jointVel;
1737  break;
1738  }
1740  {
1741  //reset to current pos
1742 
1743  for (int j = 0; j < 4; ++j)
1744  {
1745  pJointPos[j] = m_links[i].m_jointPos[j];
1746  }
1747 
1748  btVector3 jointVel;
1749  jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1750  btQuaternion jointOri;
1751  jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1752  pQuatUpdateFun(jointVel, jointOri, false, dt);
1753  pJointPos[0] = jointOri.x();
1754  pJointPos[1] = jointOri.y();
1755  pJointPos[2] = jointOri.z();
1756  pJointPos[3] = jointOri.w();
1757  break;
1758  }
1760  {
1761  for (int j = 0; j < 3; ++j)
1762  {
1763  pJointPos[j] = m_links[i].m_jointPos[j];
1764  }
1765  pJointPos[0] += dt * getJointVelMultiDof(i)[0];
1766 
1767  btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
1768  btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
1769  pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1770  pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1771  break;
1772  }
1773  default:
1774  {
1775  }
1776  }
1777  }
1778 
1779  m_links[i].updateInterpolationCacheMultiDof();
1780  }
1781 }
1782 
1784 {
1785  int num_links = getNumLinks();
1786  if(!isBaseKinematic())
1787  {
1788  // step position by adding dt * velocity
1789  //btVector3 v = getBaseVel();
1790  //m_basePos += dt * v;
1791  //
1792  btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
1793  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)
1794 
1795  pBasePos[0] += dt * pBaseVel[0];
1796  pBasePos[1] += dt * pBaseVel[1];
1797  pBasePos[2] += dt * pBaseVel[2];
1798  }
1799 
1801  //local functor for quaternion integration (to avoid error prone redundancy)
1802  struct
1803  {
1804  //"exponential map" based on btTransformUtil::integrateTransform(..)
1805  void operator()(const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
1806  {
1807  //baseBody => quat is alias and omega is global coor
1809 
1810  btVector3 axis;
1811  btVector3 angvel;
1812 
1813  if (!baseBody)
1814  angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
1815  else
1816  angvel = omega;
1817 
1818  btScalar fAngle = angvel.length();
1819  //limit the angular motion
1820  if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
1821  {
1822  fAngle = btScalar(0.5) * SIMD_HALF_PI / dt;
1823  }
1824 
1825  if (fAngle < btScalar(0.001))
1826  {
1827  // use Taylor's expansions of sync function
1828  axis = angvel * (btScalar(0.5) * dt - (dt * dt * dt) * (btScalar(0.020833333333)) * fAngle * fAngle);
1829  }
1830  else
1831  {
1832  // sync(fAngle) = sin(c*fAngle)/t
1833  axis = angvel * (btSin(btScalar(0.5) * fAngle * dt) / fAngle);
1834  }
1835 
1836  if (!baseBody)
1837  quat = btQuaternion(axis.x(), axis.y(), axis.z(), btCos(fAngle * dt * btScalar(0.5))) * quat;
1838  else
1839  quat = quat * btQuaternion(-axis.x(), -axis.y(), -axis.z(), btCos(fAngle * dt * btScalar(0.5)));
1840  //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
1841 
1842  quat.normalize();
1843  }
1844  } pQuatUpdateFun;
1846 
1847  //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
1848  //
1849  if(!isBaseKinematic())
1850  {
1851  btScalar *pBaseQuat = pq ? pq : m_baseQuat;
1852  btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
1853  //
1854  btQuaternion baseQuat;
1855  baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
1856  btVector3 baseOmega;
1857  baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
1858  pQuatUpdateFun(baseOmega, baseQuat, true, dt);
1859  pBaseQuat[0] = baseQuat.x();
1860  pBaseQuat[1] = baseQuat.y();
1861  pBaseQuat[2] = baseQuat.z();
1862  pBaseQuat[3] = baseQuat.w();
1863 
1864  //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
1865  //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
1866  //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
1867  }
1868 
1869  if (pq)
1870  pq += 7;
1871  if (pqd)
1872  pqd += 6;
1873 
1874  // Finally we can update m_jointPos for each of the m_links
1875  for (int i = 0; i < num_links; ++i)
1876  {
1877  if (!(m_links[i].m_collider && m_links[i].m_collider->isStaticOrKinematic()))
1878  {
1879  btScalar *pJointPos;
1880  pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
1881 
1882  btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
1883 
1884  switch (m_links[i].m_jointType)
1885  {
1888  {
1889  //reset to current pos
1890  btScalar jointVel = pJointVel[0];
1891  pJointPos[0] += dt * jointVel;
1892  break;
1893  }
1895  {
1896  //reset to current pos
1897  btVector3 jointVel;
1898  jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
1899  btQuaternion jointOri;
1900  jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
1901  pQuatUpdateFun(jointVel, jointOri, false, dt);
1902  pJointPos[0] = jointOri.x();
1903  pJointPos[1] = jointOri.y();
1904  pJointPos[2] = jointOri.z();
1905  pJointPos[3] = jointOri.w();
1906  break;
1907  }
1909  {
1910  pJointPos[0] += dt * getJointVelMultiDof(i)[0];
1911 
1912  btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
1913  btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
1914  pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
1915  pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
1916 
1917  break;
1918  }
1919  default:
1920  {
1921  }
1922  }
1923  }
1924 
1925  m_links[i].updateCacheMultiDof(pq);
1926 
1927  if (pq)
1928  pq += m_links[i].m_posVarCount;
1929  if (pqd)
1930  pqd += m_links[i].m_dofCount;
1931  }
1932 }
1933 
1935  const btVector3 &contact_point,
1936  const btVector3 &normal_ang,
1937  const btVector3 &normal_lin,
1938  btScalar *jac,
1939  btAlignedObjectArray<btScalar> &scratch_r1,
1941  btAlignedObjectArray<btMatrix3x3> &scratch_m) const
1942 {
1943  // temporary space
1944  int num_links = getNumLinks();
1945  int m_dofCount = getNumDofs();
1946  scratch_v.resize(3 * num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
1947  scratch_m.resize(num_links + 1);
1948 
1949  btVector3 *v_ptr = &scratch_v[0];
1950  btVector3 *p_minus_com_local = v_ptr;
1951  v_ptr += num_links + 1;
1952  btVector3 *n_local_lin = v_ptr;
1953  v_ptr += num_links + 1;
1954  btVector3 *n_local_ang = v_ptr;
1955  v_ptr += num_links + 1;
1956  btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
1957 
1958  //scratch_r.resize(m_dofCount);
1959  //btScalar *results = m_dofCount > 0 ? &scratch_r[0] : 0;
1960 
1961  scratch_r1.resize(m_dofCount+num_links);
1962  btScalar * results = m_dofCount > 0 ? &scratch_r1[0] : 0;
1963  btScalar* links = num_links? &scratch_r1[m_dofCount] : 0;
1964  int numLinksChildToRoot=0;
1965  int l = link;
1966  while (l != -1)
1967  {
1968  links[numLinksChildToRoot++]=l;
1969  l = m_links[l].m_parent;
1970  }
1971 
1972  btMatrix3x3 *rot_from_world = &scratch_m[0];
1973 
1974  const btVector3 p_minus_com_world = contact_point - m_basePos;
1975  const btVector3 &normal_lin_world = normal_lin; //convenience
1976  const btVector3 &normal_ang_world = normal_ang;
1977 
1978  rot_from_world[0] = btMatrix3x3(m_baseQuat);
1979 
1980  // omega coeffients first.
1981  btVector3 omega_coeffs_world;
1982  omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
1983  jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
1984  jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
1985  jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
1986  // then v coefficients
1987  jac[3] = normal_lin_world[0];
1988  jac[4] = normal_lin_world[1];
1989  jac[5] = normal_lin_world[2];
1990 
1991  //create link-local versions of p_minus_com and normal
1992  p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
1993  n_local_lin[0] = rot_from_world[0] * normal_lin_world;
1994  n_local_ang[0] = rot_from_world[0] * normal_ang_world;
1995 
1996  // Set remaining jac values to zero for now.
1997  for (int i = 6; i < 6 + m_dofCount; ++i)
1998  {
1999  jac[i] = 0;
2000  }
2001 
2002  // Qdot coefficients, if necessary.
2003  if (num_links > 0 && link > -1)
2004  {
2005  // TODO: (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2006  // which is resulting in repeated work being done...)
2007 
2008  // calculate required normals & positions in the local frames.
2009  for (int a = 0; a < numLinksChildToRoot; a++)
2010  {
2011  int i = links[numLinksChildToRoot-1-a];
2012  // transform to local frame
2013  const int parent = m_links[i].m_parent;
2014  const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2015  rot_from_world[i + 1] = mtx * rot_from_world[parent + 1];
2016 
2017  n_local_lin[i + 1] = mtx * n_local_lin[parent + 1];
2018  n_local_ang[i + 1] = mtx * n_local_ang[parent + 1];
2019  p_minus_com_local[i + 1] = mtx * p_minus_com_local[parent + 1] - m_links[i].m_cachedRVector;
2020 
2021  // calculate the jacobian entry
2022  switch (m_links[i].m_jointType)
2023  {
2025  {
2026  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));
2027  results[m_links[i].m_dofOffset] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
2028  break;
2029  }
2031  {
2032  results[m_links[i].m_dofOffset] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(0));
2033  break;
2034  }
2036  {
2037  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));
2038  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));
2039  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));
2040 
2041  results[m_links[i].m_dofOffset + 0] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(0));
2042  results[m_links[i].m_dofOffset + 1] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(1));
2043  results[m_links[i].m_dofOffset + 2] += n_local_ang[i + 1].dot(m_links[i].getAxisTop(2));
2044 
2045  break;
2046  }
2048  {
2049  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));
2050  results[m_links[i].m_dofOffset + 1] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(1));
2051  results[m_links[i].m_dofOffset + 2] = n_local_lin[i + 1].dot(m_links[i].getAxisBottom(2));
2052 
2053  break;
2054  }
2055  default:
2056  {
2057  }
2058  }
2059  }
2060 
2061  // Now copy through to output.
2062  //printf("jac[%d] = ", link);
2063  while (link != -1)
2064  {
2065  for (int dof = 0; dof < m_links[link].m_dofCount; ++dof)
2066  {
2067  jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
2068  //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
2069  }
2070 
2071  link = m_links[link].m_parent;
2072  }
2073  //printf("]\n");
2074  }
2075 }
2076 
2078 {
2079  m_sleepTimer = 0;
2080  m_awake = true;
2081 }
2082 
2084 {
2085  m_awake = false;
2086 }
2087 
2089 {
2090  extern bool gDisableDeactivation;
2092  {
2093  m_awake = true;
2094  m_sleepTimer = 0;
2095  return;
2096  }
2097 
2098 
2099 
2100  // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
2101  btScalar motion = 0;
2102  {
2103  for (int i = 0; i < 6 + m_dofCount; ++i)
2104  motion += m_realBuf[i] * m_realBuf[i];
2105  }
2106 
2107  if (motion < SLEEP_EPSILON)
2108  {
2109  m_sleepTimer += timestep;
2110  if (m_sleepTimer > SLEEP_TIMEOUT)
2111  {
2112  goToSleep();
2113  }
2114  }
2115  else
2116  {
2117  m_sleepTimer = 0;
2118  if (m_canWakeup)
2119  {
2120  if (!m_awake)
2121  wakeUp();
2122  }
2123  }
2124 }
2125 
2127 {
2128  int num_links = getNumLinks();
2129 
2130  // Cached 3x3 rotation matrices from parent frame to this frame.
2131  btMatrix3x3 *rot_from_parent = (btMatrix3x3 *)&m_matrixBuf[0];
2132 
2133  rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
2134 
2135  for (int i = 0; i < num_links; ++i)
2136  {
2137  rot_from_parent[i + 1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
2138  }
2139 
2140  int nLinks = getNumLinks();
2142  world_to_local.resize(nLinks + 1);
2143  local_origin.resize(nLinks + 1);
2144 
2145  world_to_local[0] = getWorldToBaseRot();
2146  local_origin[0] = getBasePos();
2147 
2148  for (int k = 0; k < getNumLinks(); k++)
2149  {
2150  const int parent = getParent(k);
2151  world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
2152  local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
2153  }
2154 
2155  for (int link = 0; link < getNumLinks(); link++)
2156  {
2157  int index = link + 1;
2158 
2159  btVector3 posr = local_origin[index];
2160  btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2161  btTransform tr;
2162  tr.setIdentity();
2163  tr.setOrigin(posr);
2164  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2165  getLink(link).m_cachedWorldTransform = tr;
2166  }
2167 }
2168 
2170 {
2171  world_to_local.resize(getNumLinks() + 1);
2172  local_origin.resize(getNumLinks() + 1);
2173 
2174  world_to_local[0] = getWorldToBaseRot();
2175  local_origin[0] = getBasePos();
2176 
2177  if (getBaseCollider())
2178  {
2179  btVector3 posr = local_origin[0];
2180  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2181  btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2182  btTransform tr;
2183  tr.setIdentity();
2184  tr.setOrigin(posr);
2185  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2186 
2189  }
2190 
2191  for (int k = 0; k < getNumLinks(); k++)
2192  {
2193  const int parent = getParent(k);
2194  world_to_local[k + 1] = getParentToLocalRot(k) * world_to_local[parent + 1];
2195  local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getRVector(k)));
2196  }
2197 
2198  for (int m = 0; m < getNumLinks(); m++)
2199  {
2201  if (col)
2202  {
2203  int link = col->m_link;
2204  btAssert(link == m);
2205 
2206  int index = link + 1;
2207 
2208  btVector3 posr = local_origin[index];
2209  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2210  btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2211  btTransform tr;
2212  tr.setIdentity();
2213  tr.setOrigin(posr);
2214  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2215 
2216  col->setWorldTransform(tr);
2218  }
2219  }
2220 }
2221 
2223 {
2224  world_to_local.resize(getNumLinks() + 1);
2225  local_origin.resize(getNumLinks() + 1);
2226 
2227  if(isBaseKinematic()){
2228  world_to_local[0] = getWorldToBaseRot();
2229  local_origin[0] = getBasePos();
2230  }
2231  else
2232  {
2233  world_to_local[0] = getInterpolateWorldToBaseRot();
2234  local_origin[0] = getInterpolateBasePos();
2235  }
2236 
2237  if (getBaseCollider())
2238  {
2239  btVector3 posr = local_origin[0];
2240  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2241  btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
2242  btTransform tr;
2243  tr.setIdentity();
2244  tr.setOrigin(posr);
2245  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2246 
2248  }
2249 
2250  for (int k = 0; k < getNumLinks(); k++)
2251  {
2252  const int parent = getParent(k);
2253  world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
2254  local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
2255  }
2256 
2257  for (int m = 0; m < getNumLinks(); m++)
2258  {
2260  if (col)
2261  {
2262  int link = col->m_link;
2263  btAssert(link == m);
2264 
2265  int index = link + 1;
2266 
2267  btVector3 posr = local_origin[index];
2268  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2269  btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
2270  btTransform tr;
2271  tr.setIdentity();
2272  tr.setOrigin(posr);
2273  tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
2274 
2276  }
2277  }
2278 }
2279 
2281 {
2282  int sz = sizeof(btMultiBodyData);
2283  return sz;
2284 }
2285 
2287 const char *btMultiBody::serialize(void *dataBuffer, class btSerializer *serializer) const
2288 {
2289  btMultiBodyData *mbd = (btMultiBodyData *)dataBuffer;
2290  getBasePos().serialize(mbd->m_baseWorldPosition);
2291  getWorldToBaseRot().inverse().serialize(mbd->m_baseWorldOrientation);
2292  getBaseVel().serialize(mbd->m_baseLinearVelocity);
2293  getBaseOmega().serialize(mbd->m_baseAngularVelocity);
2294 
2295  mbd->m_baseMass = this->getBaseMass();
2296  getBaseInertia().serialize(mbd->m_baseInertia);
2297  {
2298  char *name = (char *)serializer->findNameForPointer(m_baseName);
2299  mbd->m_baseName = (char *)serializer->getUniquePointer(name);
2300  if (mbd->m_baseName)
2301  {
2302  serializer->serializeName(name);
2303  }
2304  }
2305  mbd->m_numLinks = this->getNumLinks();
2306  if (mbd->m_numLinks)
2307  {
2308  int sz = sizeof(btMultiBodyLinkData);
2309  int numElem = mbd->m_numLinks;
2310  btChunk *chunk = serializer->allocate(sz, numElem);
2311  btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
2312  for (int i = 0; i < numElem; i++, memPtr++)
2313  {
2314  memPtr->m_jointType = getLink(i).m_jointType;
2315  memPtr->m_dofCount = getLink(i).m_dofCount;
2316  memPtr->m_posVarCount = getLink(i).m_posVarCount;
2317 
2318  getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
2319 
2320  getLink(i).m_absFrameTotVelocity.m_topVec.serialize(memPtr->m_absFrameTotVelocityTop);
2321  getLink(i).m_absFrameTotVelocity.m_bottomVec.serialize(memPtr->m_absFrameTotVelocityBottom);
2322  getLink(i).m_absFrameLocVelocity.m_topVec.serialize(memPtr->m_absFrameLocVelocityTop);
2323  getLink(i).m_absFrameLocVelocity.m_bottomVec.serialize(memPtr->m_absFrameLocVelocityBottom);
2324 
2325  memPtr->m_linkMass = getLink(i).m_mass;
2326  memPtr->m_parentIndex = getLink(i).m_parent;
2327  memPtr->m_jointDamping = getLink(i).m_jointDamping;
2328  memPtr->m_jointFriction = getLink(i).m_jointFriction;
2329  memPtr->m_jointLowerLimit = getLink(i).m_jointLowerLimit;
2330  memPtr->m_jointUpperLimit = getLink(i).m_jointUpperLimit;
2331  memPtr->m_jointMaxForce = getLink(i).m_jointMaxForce;
2332  memPtr->m_jointMaxVelocity = getLink(i).m_jointMaxVelocity;
2333 
2334  getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
2335  getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
2336  getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
2337  btAssert(memPtr->m_dofCount <= 3);
2338  for (int dof = 0; dof < getLink(i).m_dofCount; dof++)
2339  {
2340  getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
2341  getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
2342 
2343  memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
2344  memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
2345  }
2346  int numPosVar = getLink(i).m_posVarCount;
2347  for (int posvar = 0; posvar < numPosVar; posvar++)
2348  {
2349  memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
2350  }
2351 
2352  {
2353  char *name = (char *)serializer->findNameForPointer(m_links[i].m_linkName);
2354  memPtr->m_linkName = (char *)serializer->getUniquePointer(name);
2355  if (memPtr->m_linkName)
2356  {
2357  serializer->serializeName(name);
2358  }
2359  }
2360  {
2361  char *name = (char *)serializer->findNameForPointer(m_links[i].m_jointName);
2362  memPtr->m_jointName = (char *)serializer->getUniquePointer(name);
2363  if (memPtr->m_jointName)
2364  {
2365  serializer->serializeName(name);
2366  }
2367  }
2368  memPtr->m_linkCollider = (btCollisionObjectData *)serializer->getUniquePointer(getLink(i).m_collider);
2369  }
2370  serializer->finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)&m_links[0]);
2371  }
2372  mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)serializer->getUniquePointer((void *)&m_links[0]) : 0;
2373 
2374  // Fill padding with zeros to appease msan.
2375 #ifdef BT_USE_DOUBLE_PRECISION
2376  memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
2377 #endif
2378 
2379  return btMultiBodyDataName;
2380 }
2381 
2383 {
2384  //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
2385  if (m_kinematic_calculate_velocity && timeStep != btScalar(0.))
2386  {
2387  btVector3 linearVelocity, angularVelocity;
2388  btTransformUtil::calculateVelocity(getInterpolateBaseWorldTransform(), getBaseWorldTransform(), timeStep, linearVelocity, angularVelocity);
2389  setBaseVel(linearVelocity);
2390  setBaseOmega(angularVelocity);
2392  }
2393 }
2394 
2395 void btMultiBody::setLinkDynamicType(const int i, int type)
2396 {
2397  if (i == -1)
2398  {
2399  setBaseDynamicType(type);
2400  }
2401  else if (i >= 0 && i < getNumLinks())
2402  {
2403  if (m_links[i].m_collider)
2404  {
2405  m_links[i].m_collider->setDynamicType(type);
2406  }
2407  }
2408 }
2409 
2411 {
2412  if (i == -1)
2413  {
2414  return isBaseStaticOrKinematic();
2415  }
2416  else
2417  {
2418  if (m_links[i].m_collider)
2419  return m_links[i].m_collider->isStaticOrKinematic();
2420  }
2421  return false;
2422 }
2423 
2424 bool btMultiBody::isLinkKinematic(const int i) const
2425 {
2426  if (i == -1)
2427  {
2428  return isBaseKinematic();
2429  }
2430  else
2431  {
2432  if (m_links[i].m_collider)
2433  return m_links[i].m_collider->isKinematic();
2434  }
2435  return false;
2436 }
2437 
2439 {
2440  int link = i;
2441  while (link != -1) {
2442  if (!isLinkStaticOrKinematic(link))
2443  return false;
2444  link = m_links[link].m_parent;
2445  }
2446  return isBaseStaticOrKinematic();
2447 }
2448 
2450 {
2451  int link = i;
2452  while (link != -1) {
2453  if (!isLinkKinematic(link))
2454  return false;
2455  link = m_links[link].m_parent;
2456  }
2457  return isBaseKinematic();
2458 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btMultiBody::m_awake
bool m_awake
Definition: btMultiBody.h:800
btSpatialTransformationMatrix::transformInverseRotationOnly
void transformInverseRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:307
btMultiBody::getBaseVel
const btVector3 getBaseVel() const
Definition: btMultiBody.h:189
btVector3::serialize
void serialize(struct btVector3Data &dataOut) const
Definition: btVector3.h:1317
btMultiBody::setupRevolute
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)
Definition: btMultiBody.cpp:217
btMultiBody::getInterpolateParentToLocalRot
const btQuaternion & getInterpolateParentToLocalRot(int i) const
Definition: btMultiBody.cpp:462
btMultiBody::predictPositionsMultiDof
void predictPositionsMultiDof(btScalar dt)
Definition: btMultiBody.cpp:1591
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:115
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:631
btMultiBody::getJointPosMultiDof
btScalar * getJointPosMultiDof(int i)
Definition: btMultiBody.cpp:385
btMultiBody::setJointPosMultiDof
void setJointPosMultiDof(int i, const double *q)
Definition: btMultiBody.cpp:412
btMultiBody::getRVector
const btVector3 & getRVector(int i) const
Definition: btMultiBody.cpp:447
btSpatialForceVector
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
Definition: btSpatialAlgebra.h:24
btMultiBody::updateCollisionObjectInterpolationWorldTransforms
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2222
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition: btScalar.h:528
btMultiBody::m_useGyroTerm
bool m_useGyroTerm
Definition: btMultiBody.h:812
btMultiBody::worldDirToLocal
btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const
Definition: btMultiBody.cpp:532
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btMultiBody::~btMultiBody
virtual ~btMultiBody()
Definition: btMultiBody.cpp:147
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:166
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btMultiBodyDataName
#define btMultiBodyDataName
Definition: btMultiBody.h:41
btMultiBody::m_baseForce
btVector3 m_baseForce
Definition: btMultiBody.h:762
gDisableDeactivation
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
btMultiBody::m_canSleep
bool m_canSleep
Definition: btMultiBody.h:801
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1093
btSymmetricSpatialDyad::m_topLeftMat
btMatrix3x3 m_topLeftMat
Definition: btSpatialAlgebra.h:188
btMultiBody::m_baseConstraintTorque
btVector3 m_baseConstraintTorque
Definition: btMultiBody.h:766
btMultiBody::isLinkAndAllAncestorsStaticOrKinematic
bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const
Definition: btMultiBody.cpp:2438
btMultiBody::isLinkAndAllAncestorsKinematic
bool isLinkAndAllAncestorsKinematic(const int i) const
Definition: btMultiBody.cpp:2449
btSpatialTransformationMatrix::transformRotationOnly
void transformRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:263
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btMultiBody::getWorldToBaseRot
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:193
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btSerializer::getUniquePointer
virtual void * getUniquePointer(void *oldPtr)=0
btMultiBody::m_fixedBase
bool m_fixedBase
Definition: btMultiBody.h:797
btMultiBody::setJointVelMultiDof
void setJointVelMultiDof(int i, const double *qdot)
Definition: btMultiBody.cpp:435
btChunk
Definition: btSerializer.h:48
btMultiBody::clearConstraintForces
void clearConstraintForces()
Definition: btMultiBody.cpp:590
btMultiBody::getJointVelMultiDof
btScalar * getJointVelMultiDof(int i)
Definition: btMultiBody.cpp:390
btSpatialMotionVector::getLinear
const btVector3 & getLinear() const
Definition: btSpatialAlgebra.h:128
btMultiBody::isLinkStaticOrKinematic
bool isLinkStaticOrKinematic(const int i) const
Definition: btMultiBody.cpp:2410
btSpatialMotionVector::setVector
void setVector(const btVector3 &angular, const btVector3 &linear)
Definition: btSpatialAlgebra.h:101
btMultiBody::solveImatrix
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const
Definition: btMultiBody.cpp:1278
btMultiBody::m_dofCount
int m_dofCount
Definition: btMultiBody.h:818
btMultiBody::m_matrixBuf
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:789
btSpatialMotionVector::setZero
void setZero()
Definition: btSpatialAlgebra.h:136
btCollisionObjectData
#define btCollisionObjectData
Definition: btCollisionObject.h:41
btSpatialTransformationMatrix::m_trnVec
btVector3 m_trnVec
Definition: btSpatialAlgebra.h:231
btMultiBody::compTreeLinkVelocities
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
Definition: btMultiBody.cpp:561
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btMultiBody::updateCollisionObjectWorldTransforms
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2169
btMultiBodyData
#define btMultiBodyData
serialization data, don't change them if you are not familiar with the details of the serialization m...
Definition: btMultiBody.h:40
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:636
btQuadWord::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:149
btMultiBody::calculateSerializeBufferSize
virtual int calculateSerializeBufferSize() const
Definition: btMultiBody.cpp:2280
btSpatialForceVector::getLinear
const btVector3 & getLinear() const
Definition: btSpatialAlgebra.h:60
btMultiBody::localDirToWorld
btVector3 localDirToWorld(int i, const btVector3 &local_dir) const
Definition: btMultiBody.cpp:513
btQuadWord::w
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
btSerializer::findNameForPointer
virtual const char * findNameForPointer(const void *ptr) const =0
btMultiBody::m_deltaV
btAlignedObjectArray< btScalar > m_deltaV
Definition: btMultiBody.h:786
btVector3::setZero
void setZero()
Definition: btVector3.h:671
inverse
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
btMultiBody::m_canWakeup
bool m_canWakeup
Definition: btMultiBody.h:802
btMultiBody::getLinkTorque
const btVector3 & getLinkTorque(int i) const
Definition: btMultiBody.cpp:662
btMultiBody::m_cachedInertiaLowerRight
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:794
btMultiBody::getBaseWorldTransform
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:228
btCollisionObject::isKinematicObject
bool isKinematicObject() const
Definition: btCollisionObject.h:201
btTransformUtil.h
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
btMultiBody::wakeUp
void wakeUp()
Definition: btMultiBody.cpp:2077
btMultiBody::addLinkTorque
void addLinkTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:626
btMultiBody::setBaseVel
void setBaseVel(const btVector3 &vel)
Definition: btMultiBody.h:250
btMultiBody::stepPositionsMultiDof
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
Definition: btMultiBody.cpp:1783
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btMultiBodyLinkData
#define btMultiBodyLinkData
Definition: btMultiBody.h:42
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btCollisionObject::isStaticOrKinematicObject
bool isStaticOrKinematicObject() const
Definition: btCollisionObject.h:206
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:465
btMultiBodyJointFeedback.h
btMultiBody::setupPrismatic
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)
Definition: btMultiBody.cpp:179
btMultiBody::m_cachedInertiaValid
bool m_cachedInertiaValid
Definition: btMultiBody.h:795
btSpatialTransformationMatrix::transform
void transform(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:241
btSpatialTransformationMatrix::Add
@ Add
Definition: btSpatialAlgebra.h:236
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:499
outerProduct
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
Definition: btMultiBody.cpp:701
btMultiBody::setBaseDynamicType
void setBaseDynamicType(int dynamicType)
Definition: btMultiBody.cpp:692
btMultiBody::clearForcesAndTorques
void clearForcesAndTorques()
Definition: btMultiBody.cpp:601
btMultiBody::addLinkForce
void addLinkForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:621
btMultiBody::m_sleepTimer
btScalar m_sleepTimer
Definition: btMultiBody.h:803
btSpatialTransformationMatrix::transformInverse
void transformInverse(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
Definition: btSpatialAlgebra.h:285
btMultiBody::checkMotionAndSleepIfRequired
void checkMotionAndSleepIfRequired(btScalar timestep)
Definition: btMultiBody.cpp:2088
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1429
btMultiBody::m_baseConstraintForce
btVector3 m_baseConstraintForce
Definition: btMultiBody.h:765
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btMultiBody::isBaseKinematic
bool isBaseKinematic() const
Definition: btMultiBody.cpp:687
btMultiBodyLinkCollider.h
btTransform::setRotation
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:160
btMultiBody::m_internalNeedsJointFeedback
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
Definition: btMultiBody.h:825
btMultiBody::btMultiBody
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool deprecatedMultiDof=true)
Definition: btMultiBody.cpp:93
btMultiBody::getBaseInertia
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:170
btMultiBody::getBaseOmega
btVector3 getBaseOmega() const
Definition: btMultiBody.h:208
btMultiBody::hasFixedBase
bool hasFixedBase() const
Definition: btMultiBody.cpp:677
btMultiBody::saveKinematicState
void saveKinematicState(btScalar timeStep)
Definition: btMultiBody.cpp:2382
btMultiBody::getInterpolateRVector
const btVector3 & getInterpolateRVector(int i) const
Definition: btMultiBody.cpp:457
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:498
btMultiBody::setJointPos
void setJointPos(int i, btScalar q)
Definition: btMultiBody.cpp:405
btMultiBody::m_cachedInertiaLowerLeft
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:793
btCollisionObject::setWorldTransform
void setWorldTransform(const btTransform &worldTrans)
Definition: btCollisionObject.h:388
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMultiBody::getBaseMass
btScalar getBaseMass() const
Definition: btMultiBody.h:169
btMultiBody::m_baseTorque
btVector3 m_baseTorque
Definition: btMultiBody.h:763
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btMultiBody::setBaseOmega
void setBaseOmega(const btVector3 &omega)
Definition: btMultiBody.h:269
btSerializer.h
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btCollisionObject::getCollisionFlags
int getCollisionFlags() const
Definition: btCollisionObject.h:488
btSerializer::finalizeChunk
virtual void finalizeChunk(btChunk *chunk, const char *structType, int chunkCode, void *oldPtr)=0
btCollisionObject::isStaticObject
bool isStaticObject() const
Definition: btCollisionObject.h:196
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:142
btMultiBody::getInterpolateBaseWorldTransform
btTransform getInterpolateBaseWorldTransform() const
Definition: btMultiBody.h:242
btCollisionObject::setInterpolationWorldTransform
void setInterpolationWorldTransform(const btTransform &trans)
Definition: btCollisionObject.h:419
btMultiBody::m_cachedInertiaTopLeft
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:791
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btMultiBody::m_basePos_interpolate
btVector3 m_basePos_interpolate
Definition: btMultiBody.h:755
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btMultiBody::m_baseQuat_interpolate
btQuaternion m_baseQuat_interpolate
Definition: btMultiBody.h:757
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btMultiBody::m_splitV
btAlignedObjectArray< btScalar > m_splitV
Definition: btMultiBody.h:785
btSpatialForceVector::addVector
void addVector(const btVector3 &angular, const btVector3 &linear)
Definition: btSpatialAlgebra.h:45
btMultiBody::getInterpolateWorldToBaseRot
const btQuaternion & getInterpolateWorldToBaseRot() const
Definition: btMultiBody.h:202
btMultiBody::setupSpherical
void setupSpherical(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
Definition: btMultiBody.cpp:253
btSerializer::serializeName
virtual void serializeName(const char *ptr)=0
btMultiBody::getParentToLocalRot
const btQuaternion & getParentToLocalRot(int i) const
Definition: btMultiBody.cpp:452
btMultiBody::m_baseName
const char * m_baseName
Definition: btMultiBody.h:752
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btMultiBody::setJointVel
void setJointVel(int i, btScalar qdot)
Definition: btMultiBody.cpp:430
btChunk::m_oldPtr
void * m_oldPtr
Definition: btSerializer.h:52
btCollisionObject::CF_STATIC_OBJECT
@ CF_STATIC_OBJECT
Definition: btCollisionObject.h:131
btMultiBody::m_posVarCnt
int m_posVarCnt
Definition: btMultiBody.h:818
btMultiBody::m_baseQuat
btQuaternion m_baseQuat
Definition: btMultiBody.h:756
btMultiBody::getJointTorque
btScalar getJointTorque(int i) const
Definition: btMultiBody.cpp:667
btMultiBody::updateLinksDofOffsets
void updateLinksDofOffsets()
Definition: btMultiBody.h:736
btMultiBody::m_basePos
btVector3 m_basePos
Definition: btMultiBody.h:754
btMultiBody::spatialTransform
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
btSymmetricSpatialDyad::m_bottomLeftMat
btMatrix3x3 m_bottomLeftMat
Definition: btSpatialAlgebra.h:188
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:113
output
#define output
btSymmetricSpatialDyad::m_topRightMat
btMatrix3x3 m_topRightMat
Definition: btSpatialAlgebra.h:188
btAlignedObjectArray< btScalar >
symmetricSpatialOuterProduct
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
Definition: btSpatialAlgebra.h:366
btMultiBody::getParent
int getParent(int link_num) const
Definition: btMultiBody.cpp:360
btMultiBody::addJointTorqueMultiDof
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
Definition: btMultiBody.cpp:646
btSpatialMotionVector::cross
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
Definition: btSpatialAlgebra.h:148
btMultiBody::setInterpolateBaseWorldTransform
void setInterpolateBaseWorldTransform(const btTransform &tr)
Definition: btMultiBody.h:236
btMultiBody::getLinkMass
btScalar getLinkMass(int i) const
Definition: btMultiBody.cpp:365
btMultiBody::addJointTorque
void addJointTorque(int i, btScalar Q)
Definition: btMultiBody.cpp:641
btSpatialMotionVector::setAngular
void setAngular(const btVector3 &angular)
Definition: btSpatialAlgebra.h:130
btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
Definition: btMultiBody.cpp:725
btMatrix3x3::setValue
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:204
btMultiBody::serialize
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
Definition: btMultiBody.cpp:2287
btSymmetricSpatialDyad
Definition: btSpatialAlgebra.h:187
btSpatialTransformationMatrix
Definition: btSpatialAlgebra.h:229
btSerializer
Definition: btSerializer.h:66
btMultiBody::getJointPos
btScalar getJointPos(int i) const
Definition: btMultiBody.cpp:375
btMultiBody.h
btMultiBody::setLinkDynamicType
void setLinkDynamicType(const int i, int type)
Definition: btMultiBody.cpp:2395
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btQuaternion::serialize
void serialize(struct btQuaternionData &dataOut) const
Definition: btQuaternion.h:1002
btSpatialTransformationMatrix::m_rotMat
btMatrix3x3 m_rotMat
Definition: btSpatialAlgebra.h:230
btMultiBody::getInterpolateBasePos
const btVector3 & getInterpolateBasePos() const
Definition: btMultiBody.h:198
btCollisionObject::CF_KINEMATIC_OBJECT
@ CF_KINEMATIC_OBJECT
Definition: btCollisionObject.h:132
btSpatialForceVector::addLinear
void addLinear(const btVector3 &linear)
Definition: btSpatialAlgebra.h:67
BT_ARRAY_CODE
#define BT_ARRAY_CODE
Definition: btSerializer.h:118
btSpatialMotionVector
Definition: btSpatialAlgebra.h:95
btMultiBody::clearVelocities
void clearVelocities()
Definition: btMultiBody.cpp:614
btCollisionObject::setCollisionFlags
void setCollisionFlags(int flags)
Definition: btCollisionObject.h:493
btMultiBody::forwardKinematics
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
Definition: btMultiBody.cpp:2126
btMultiBody::m_vectorBuf
btAlignedObjectArray< btVector3 > m_vectorBuf
Definition: btMultiBody.h:788
btSpatialForceVector::setZero
void setZero()
Definition: btSpatialAlgebra.h:69
btSpatialForceVector::addAngular
void addAngular(const btVector3 &angular)
Definition: btSpatialAlgebra.h:66
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:37
btMultiBody::isLinkKinematic
bool isLinkKinematic(const int i) const
Definition: btMultiBody.cpp:2424
btMultiBody::m_cachedInertiaTopRight
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:792
btMultiBody::m_baseInertia
btVector3 m_baseInertia
Definition: btMultiBody.h:760
btMultiBody::getLinkInertia
const btVector3 & getLinkInertia(int i) const
Definition: btMultiBody.cpp:370
btMultiBody::mulMatrix
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
Definition: btMultiBody.cpp:1414
btMultiBody::getJointVel
btScalar getJointVel(int i) const
Definition: btMultiBody.cpp:380
btMultiBody::m_realBuf
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:787
btMultiBody::m_angularDamping
btScalar m_angularDamping
Definition: btMultiBody.h:811
btSpatialMotionVector::getAngular
const btVector3 & getAngular() const
Definition: btSpatialAlgebra.h:127
btMultiBody::getJointTorqueMultiDof
btScalar * getJointTorqueMultiDof(int i)
Definition: btMultiBody.cpp:672
btMultiBody::m_kinematic_calculate_velocity
bool m_kinematic_calculate_velocity
Definition: btMultiBody.h:828
btMultiBody::m_useGlobalVelocities
bool m_useGlobalVelocities
Definition: btMultiBody.h:820
ANGULAR_MOTION_THRESHOLD
#define ANGULAR_MOTION_THRESHOLD
Definition: btTransformUtil.h:19
btSpatialForceVector::setVector
void setVector(const btVector3 &angular, const btVector3 &linear)
Definition: btSpatialAlgebra.h:34
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btMultiBody::m_linearDamping
btScalar m_linearDamping
Definition: btMultiBody.h:810
btSpatialMotionVector::setLinear
void setLinear(const btVector3 &linear)
Definition: btSpatialAlgebra.h:131
btMultiBody::localPosToWorld
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
Definition: btMultiBody.cpp:467
btMultiBody::goToSleep
void goToSleep()
Definition: btMultiBody.cpp:2083
btSymmetricSpatialDyad::setMatrix
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
Definition: btSpatialAlgebra.h:193
btMultiBodyLinkDataName
#define btMultiBodyLinkDataName
Definition: btMultiBody.h:43
btMultiBody::localFrameToWorld
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
Definition: btMultiBody.cpp:551
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:117
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btMultiBody::isBaseStaticOrKinematic
bool isBaseStaticOrKinematic() const
Definition: btMultiBody.cpp:682
btMultiBody::getLinkForce
const btVector3 & getLinkForce(int i) const
Definition: btMultiBody.cpp:657
btSpatialMotionVector::dot
btScalar dot(const btSpatialForceVector &b) const
Definition: btSpatialAlgebra.h:142
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:115
btMultiBody::getBaseCollider
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
btMultiBody::worldPosToLocal
btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const
Definition: btMultiBody.cpp:492
btMultiBody::m_links
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:768
btSpatialForceVector::getAngular
const btVector3 & getAngular() const
Definition: btSpatialAlgebra.h:61
btSerializer::allocate
virtual btChunk * allocate(size_t size, int numElements)=0
btMultiBody::m_baseMass
btScalar m_baseMass
Definition: btMultiBody.h:759
btMultiBody::setupPlanar
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
Definition: btMultiBody.cpp:293
btMultiBody::finalizeMultiDof
void finalizeMultiDof()
Definition: btMultiBody.cpp:344
btMultiBody::fillConstraintJacobianMultiDof
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
Definition: btMultiBody.cpp:1934
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btMultiBody::setupFixed
void setupFixed(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
Definition: btMultiBody.cpp:151
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926