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