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