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