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