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