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