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