Bullet
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: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10 
11  This software is provided 'as-is', without any express or implied warranty.
12  In no event will the authors be held liable for any damages arising from the use of this software.
13  Permission is granted to anyone to use this software for any purpose,
14  including commercial applications, and to alter it and redistribute it freely,
15  subject to the following restrictions:
16 
17  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.
18  2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
19  3. This notice may not be removed or altered from any source distribution.
20 
21  */
22 
23 
24 #include "btMultiBody.h"
25 #include "btMultiBodyLink.h"
30 #include "Bullet3Common/b3Logging.h"
31 // #define INCLUDE_GYRO_TERM
32 
36 
37 namespace {
38  const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
39  const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
40 }
41 
42 namespace {
43  void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
44  const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
45  const btVector3 &top_in, // top part of input vector
46  const btVector3 &bottom_in, // bottom part of input vector
47  btVector3 &top_out, // top part of output vector
48  btVector3 &bottom_out) // bottom part of output vector
49  {
50  top_out = rotation_matrix * top_in;
51  bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
52  }
53 
54  void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
55  const btVector3 &displacement,
56  const btVector3 &top_in,
57  const btVector3 &bottom_in,
58  btVector3 &top_out,
59  btVector3 &bottom_out)
60  {
61  top_out = rotation_matrix.transpose() * top_in;
62  bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
63  }
64 
65  btScalar SpatialDotProduct(const btVector3 &a_top,
66  const btVector3 &a_bottom,
67  const btVector3 &b_top,
68  const btVector3 &b_bottom)
69  {
70  return a_bottom.dot(b_top) + a_top.dot(b_bottom);
71  }
72 
73  void SpatialCrossProduct(const btVector3 &a_top,
74  const btVector3 &a_bottom,
75  const btVector3 &b_top,
76  const btVector3 &b_bottom,
77  btVector3 &top_out,
78  btVector3 &bottom_out)
79  {
80  top_out = a_top.cross(b_top);
81  bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
82  }
83 }
84 
85 
86 //
87 // Implementation of class btMultiBody
88 //
89 
91  btScalar mass,
92  const btVector3 &inertia,
93  bool fixedBase,
94  bool canSleep,
95  bool multiDof)
96  :
97  m_baseCollider(0),
98  m_baseName(0),
99  m_basePos(0,0,0),
100  m_baseQuat(0, 0, 0, 1),
101  m_baseMass(mass),
102  m_baseInertia(inertia),
103 
104  m_fixedBase(fixedBase),
105  m_awake(true),
106  m_canSleep(canSleep),
107  m_sleepTimer(0),
108 
109  m_linearDamping(0.04f),
110  m_angularDamping(0.04f),
111  m_useGyroTerm(true),
112  m_maxAppliedImpulse(1000.f),
113  m_maxCoordinateVelocity(100.f),
114  m_hasSelfCollision(true),
115  m_isMultiDof(multiDof),
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 
124  if(!m_isMultiDof)
125  {
126  m_vectorBuf.resize(2*n_links);
127  m_realBuf.resize(6 + 2*n_links);
128  m_posVarCnt = n_links;
129  }
130 
131  m_links.resize(n_links);
132  m_matrixBuf.resize(n_links + 1);
133 
134 
135  m_baseForce.setValue(0, 0, 0);
136  m_baseTorque.setValue(0, 0, 0);
137 }
138 
140 {
141 }
142 
144  btScalar mass,
145  const btVector3 &inertia,
146  int parent,
147  const btQuaternion &rotParentToThis,
148  const btVector3 &parentComToThisPivotOffset,
149  const btVector3 &thisPivotToThisComOffset,
150  bool disableParentCollision)
151 {
153 
154  m_links[i].m_mass = mass;
155  m_links[i].m_inertiaLocal = inertia;
156  m_links[i].m_parent = parent;
157  m_links[i].m_zeroRotParentToThis = rotParentToThis;
158  m_links[i].m_dVector = thisPivotToThisComOffset;
159  m_links[i].m_eVector = parentComToThisPivotOffset;
160 
161  m_links[i].m_jointType = btMultibodyLink::eFixed;
162  m_links[i].m_dofCount = 0;
163  m_links[i].m_posVarCount = 0;
164 
165  if (disableParentCollision)
167  //
168  m_links[i].updateCacheMultiDof();
169 
170  //
171  //if(m_isMultiDof)
172  // resizeInternalMultiDofBuffers();
173  //
175 
176 }
177 
178 
180  btScalar mass,
181  const btVector3 &inertia,
182  int parent,
183  const btQuaternion &rotParentToThis,
184  const btVector3 &jointAxis,
185  const btVector3 &parentComToThisPivotOffset,
186  const btVector3 &thisPivotToThisComOffset,
187  bool disableParentCollision)
188 {
189  if(m_isMultiDof)
190  {
191  m_dofCount += 1;
192  m_posVarCnt += 1;
193  }
194 
195  m_links[i].m_mass = mass;
196  m_links[i].m_inertiaLocal = inertia;
197  m_links[i].m_parent = parent;
198  m_links[i].m_zeroRotParentToThis = rotParentToThis;
199  m_links[i].setAxisTop(0, 0., 0., 0.);
200  m_links[i].setAxisBottom(0, jointAxis);
201  m_links[i].m_eVector = parentComToThisPivotOffset;
202  m_links[i].m_dVector = thisPivotToThisComOffset;
203  m_links[i].m_cachedRotParentToThis = rotParentToThis;
204 
205  m_links[i].m_jointType = btMultibodyLink::ePrismatic;
206  m_links[i].m_dofCount = 1;
207  m_links[i].m_posVarCount = 1;
208  m_links[i].m_jointPos[0] = 0.f;
209  m_links[i].m_jointTorque[0] = 0.f;
210 
211  if (disableParentCollision)
213  //
214  if(m_isMultiDof)
215  m_links[i].updateCacheMultiDof();
216  else
217  m_links[i].updateCache();
218  //
219  if(m_isMultiDof)
221 }
222 
224  btScalar mass,
225  const btVector3 &inertia,
226  int parent,
227  const btQuaternion &rotParentToThis,
228  const btVector3 &jointAxis,
229  const btVector3 &parentComToThisPivotOffset,
230  const btVector3 &thisPivotToThisComOffset,
231  bool disableParentCollision)
232 {
233  if(m_isMultiDof)
234  {
235  m_dofCount += 1;
236  m_posVarCnt += 1;
237  }
238 
239  m_links[i].m_mass = mass;
240  m_links[i].m_inertiaLocal = inertia;
241  m_links[i].m_parent = parent;
242  m_links[i].m_zeroRotParentToThis = rotParentToThis;
243  m_links[i].setAxisTop(0, jointAxis);
244  m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
245  m_links[i].m_dVector = thisPivotToThisComOffset;
246  m_links[i].m_eVector = parentComToThisPivotOffset;
247 
248  m_links[i].m_jointType = btMultibodyLink::eRevolute;
249  m_links[i].m_dofCount = 1;
250  m_links[i].m_posVarCount = 1;
251  m_links[i].m_jointPos[0] = 0.f;
252  m_links[i].m_jointTorque[0] = 0.f;
253 
254  if (disableParentCollision)
256  //
257  if(m_isMultiDof)
258  m_links[i].updateCacheMultiDof();
259  else
260  m_links[i].updateCache();
261  //
262  if(m_isMultiDof)
264 }
265 
266 
267 
269  btScalar mass,
270  const btVector3 &inertia,
271  int parent,
272  const btQuaternion &rotParentToThis,
273  const btVector3 &parentComToThisPivotOffset,
274  const btVector3 &thisPivotToThisComOffset,
275  bool disableParentCollision)
276 {
278  m_dofCount += 3;
279  m_posVarCnt += 4;
280 
281  m_links[i].m_mass = mass;
282  m_links[i].m_inertiaLocal = inertia;
283  m_links[i].m_parent = parent;
284  m_links[i].m_zeroRotParentToThis = rotParentToThis;
285  m_links[i].m_dVector = thisPivotToThisComOffset;
286  m_links[i].m_eVector = parentComToThisPivotOffset;
287 
288  m_links[i].m_jointType = btMultibodyLink::eSpherical;
289  m_links[i].m_dofCount = 3;
290  m_links[i].m_posVarCount = 4;
291  m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
292  m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
293  m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
294  m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
295  m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
296  m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
297  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;
298  m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
299 
300 
301  if (disableParentCollision)
303  //
304  m_links[i].updateCacheMultiDof();
305  //
307 }
308 
310  btScalar mass,
311  const btVector3 &inertia,
312  int parent,
313  const btQuaternion &rotParentToThis,
314  const btVector3 &rotationAxis,
315  const btVector3 &parentComToThisComOffset,
316  bool disableParentCollision)
317 {
319  m_dofCount += 3;
320  m_posVarCnt += 3;
321 
322  m_links[i].m_mass = mass;
323  m_links[i].m_inertiaLocal = inertia;
324  m_links[i].m_parent = parent;
325  m_links[i].m_zeroRotParentToThis = rotParentToThis;
326  m_links[i].m_dVector.setZero();
327  m_links[i].m_eVector = parentComToThisComOffset;
328 
329  //
330  static btVector3 vecNonParallelToRotAxis(1, 0, 0);
331  if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
332  vecNonParallelToRotAxis.setValue(0, 1, 0);
333  //
334 
335  m_links[i].m_jointType = btMultibodyLink::ePlanar;
336  m_links[i].m_dofCount = 3;
337  m_links[i].m_posVarCount = 3;
338  btVector3 n=rotationAxis.normalized();
339  m_links[i].setAxisTop(0, n[0],n[1],n[2]);
340  m_links[i].setAxisTop(1,0,0,0);
341  m_links[i].setAxisTop(2,0,0,0);
342  m_links[i].setAxisBottom(0,0,0,0);
343  btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
344  m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
345  cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
346  m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
347  m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
348  m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
349 
350  if (disableParentCollision)
352  //
353  m_links[i].updateCacheMultiDof();
354  //
356 }
357 
359 {
361  m_deltaV.resize(0);
363  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")
364  m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
365 
367 }
368 
369 int btMultiBody::getParent(int i) const
370 {
371  return m_links[i].m_parent;
372 }
373 
375 {
376  return m_links[i].m_mass;
377 }
378 
380 {
381  return m_links[i].m_inertiaLocal;
382 }
383 
385 {
386  return m_links[i].m_jointPos[0];
387 }
388 
390 {
391  return m_realBuf[6 + i];
392 }
393 
395 {
396  return &m_links[i].m_jointPos[0];
397 }
398 
400 {
401  return &m_realBuf[6 + m_links[i].m_dofOffset];
402 }
403 
405 {
406  return &m_links[i].m_jointPos[0];
407 }
408 
410 {
411  return &m_realBuf[6 + m_links[i].m_dofOffset];
412 }
413 
414 
416 {
417  m_links[i].m_jointPos[0] = q;
418  m_links[i].updateCache();
419 }
420 
422 {
423  for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
424  m_links[i].m_jointPos[pos] = q[pos];
425 
426  m_links[i].updateCacheMultiDof();
427 }
428 
430 {
431  m_realBuf[6 + i] = qdot;
432 }
433 
435 {
436  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
437  m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
438 }
439 
440 const btVector3 & btMultiBody::getRVector(int i) const
441 {
442  return m_links[i].m_cachedRVector;
443 }
444 
446 {
447  return m_links[i].m_cachedRotParentToThis;
448 }
449 
450 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
451 {
452  btVector3 result = local_pos;
453  while (i != -1) {
454  // 'result' is in frame i. transform it to frame parent(i)
455  result += getRVector(i);
456  result = quatRotate(getParentToLocalRot(i).inverse(),result);
457  i = getParent(i);
458  }
459 
460  // 'result' is now in the base frame. transform it to world frame
461  result = quatRotate(getWorldToBaseRot().inverse() ,result);
462  result += getBasePos();
463 
464  return result;
465 }
466 
467 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
468 {
469  if (i == -1) {
470  // world to base
471  return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
472  } else {
473  // find position in parent frame, then transform to current frame
474  return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
475  }
476 }
477 
478 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
479 {
480  btVector3 result = local_dir;
481  while (i != -1) {
482  result = quatRotate(getParentToLocalRot(i).inverse() , result);
483  i = getParent(i);
484  }
485  result = quatRotate(getWorldToBaseRot().inverse() , result);
486  return result;
487 }
488 
489 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
490 {
491  if (i == -1) {
492  return quatRotate(getWorldToBaseRot(), world_dir);
493  } else {
494  return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
495  }
496 }
497 
499 {
500  int num_links = getNumLinks();
501  // Calculates the velocities of each link (and the base) in its local frame
502  omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
503  vel[0] = quatRotate(m_baseQuat ,getBaseVel());
504 
505  for (int i = 0; i < num_links; ++i) {
506  const int parent = m_links[i].m_parent;
507 
508  // transform parent vel into this frame, store in omega[i+1], vel[i+1]
509  SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
510  omega[parent+1], vel[parent+1],
511  omega[i+1], vel[i+1]);
512 
513  // now add qidot * shat_i
514  omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
515  vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
516  }
517 }
518 
520 {
521  int num_links = getNumLinks();
522  // TODO: would be better not to allocate memory here
523  btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
524  btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
525  compTreeLinkVelocities(&omega[0], &vel[0]);
526 
527  // we will do the factor of 0.5 at the end
528  btScalar result = m_baseMass * vel[0].dot(vel[0]);
529  result += omega[0].dot(m_baseInertia * omega[0]);
530 
531  for (int i = 0; i < num_links; ++i) {
532  result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
533  result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
534  }
535 
536  return 0.5f * result;
537 }
538 
540 {
541  int num_links = getNumLinks();
542  // TODO: would be better not to allocate memory here
543  btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
544  btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
545  btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
546  compTreeLinkVelocities(&omega[0], &vel[0]);
547 
548  rot_from_world[0] = m_baseQuat;
549  btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
550 
551  for (int i = 0; i < num_links; ++i) {
552  rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
553  result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
554  }
555 
556  return result;
557 }
558 
560 {
563 
564 
565  for (int i = 0; i < getNumLinks(); ++i) {
566  m_links[i].m_appliedConstraintForce.setValue(0, 0, 0);
567  m_links[i].m_appliedConstraintTorque.setValue(0, 0, 0);
568  }
569 }
571 {
572  m_baseForce.setValue(0, 0, 0);
573  m_baseTorque.setValue(0, 0, 0);
574 
575 
576  for (int i = 0; i < getNumLinks(); ++i) {
577  m_links[i].m_appliedForce.setValue(0, 0, 0);
578  m_links[i].m_appliedTorque.setValue(0, 0, 0);
579  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;
580  }
581 }
582 
584 {
585  for (int i = 0; i < 6 + getNumLinks(); ++i)
586  {
587  m_realBuf[i] = 0.f;
588  }
589 }
591 {
592  m_links[i].m_appliedForce += f;
593 }
594 
596 {
597  m_links[i].m_appliedTorque += t;
598 }
599 
601 {
602  m_links[i].m_appliedConstraintForce += f;
603 }
604 
606 {
607  m_links[i].m_appliedConstraintTorque += t;
608 }
609 
610 
611 
613 {
614  m_links[i].m_jointTorque[0] += Q;
615 }
616 
618 {
619  m_links[i].m_jointTorque[dof] += Q;
620 }
621 
623 {
624  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
625  m_links[i].m_jointTorque[dof] = Q[dof];
626 }
627 
629 {
630  return m_links[i].m_appliedForce;
631 }
632 
634 {
635  return m_links[i].m_appliedTorque;
636 }
637 
639 {
640  return m_links[i].m_jointTorque[0];
641 }
642 
644 {
645  return &m_links[i].m_jointTorque[0];
646 }
647 
648 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?
649 {
650  btVector3 row0 = btVector3(
651  v0.x() * v1.x(),
652  v0.x() * v1.y(),
653  v0.x() * v1.z());
654  btVector3 row1 = btVector3(
655  v0.y() * v1.x(),
656  v0.y() * v1.y(),
657  v0.y() * v1.z());
658  btVector3 row2 = btVector3(
659  v0.z() * v1.x(),
660  v0.z() * v1.y(),
661  v0.z() * v1.z());
662 
663  btMatrix3x3 m(row0[0],row0[1],row0[2],
664  row1[0],row1[1],row1[2],
665  row2[0],row2[1],row2[2]);
666  return m;
667 }
668 
669 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
670 //
671 
676  bool isConstraintPass)
677 {
678  // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
679  // and the base linear & angular accelerations.
680 
681  // We apply damping forces in this routine as well as any external forces specified by the
682  // caller (via addBaseForce etc).
683 
684  // output should point to an array of 6 + num_links reals.
685  // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
686  // num_links joint acceleration values.
687 
689 
690  int num_links = getNumLinks();
691 
692  const btScalar DAMPING_K1_LINEAR = m_linearDamping;
693  const btScalar DAMPING_K2_LINEAR = m_linearDamping;
694 
695  const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
696  const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
697 
698  btVector3 base_vel = getBaseVel();
699  btVector3 base_omega = getBaseOmega();
700 
701  // Temporary matrices/vectors -- use scratch space from caller
702  // so that we don't have to keep reallocating every frame
703 
704  scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
705  scratch_v.resize(8*num_links + 6);
706  scratch_m.resize(4*num_links + 4);
707 
708  //btScalar * r_ptr = &scratch_r[0];
709  btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
710  btVector3 * v_ptr = &scratch_v[0];
711 
712  // vhat_i (top = angular, bottom = linear part)
713  btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
714  v_ptr += num_links * 2 + 2;
715  //
716  // zhat_i^A
717  btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
718  v_ptr += num_links * 2 + 2;
719  //
720  // chat_i (note NOT defined for the base)
721  btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
722  v_ptr += num_links * 2;
723  //
724  // Ihat_i^A.
725  btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
726 
727  // Cached 3x3 rotation matrices from parent frame to this frame.
728  btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
729  btMatrix3x3 * rot_from_world = &scratch_m[0];
730 
731  // hhat_i, ahat_i
732  // hhat is NOT stored for the base (but ahat is)
734  btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
735  v_ptr += num_links * 2 + 2;
736  //
737  // Y_i, invD_i
738  btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
739  btScalar * Y = &scratch_r[0];
740  //
741  //aux variables
742  static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
743  static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
744  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
745  static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
746  static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
747  static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
748  static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
749  static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
750  static btSpatialTransformationMatrix fromWorld;
751  fromWorld.m_trnVec.setZero();
753 
754  // ptr to the joint accel part of the output
755  btScalar * joint_accel = output + 6;
756 
757  // Start of the algorithm proper.
758 
759  // First 'upward' loop.
760  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
761 
762  rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
763 
764  //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
765  spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
766 
767  if (m_fixedBase)
768  {
769  zeroAccSpatFrc[0].setZero();
770  }
771  else
772  {
773  btVector3 baseForce = isConstraintPass? m_baseConstraintForce : m_baseForce;
774  btVector3 baseTorque = isConstraintPass? m_baseConstraintTorque : m_baseTorque;
775  //external forces
776  zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * baseTorque), -(rot_from_parent[0] * baseForce));
777 
778  //adding damping terms (only)
779  btScalar linDampMult = 1., angDampMult = 1.;
780  zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
781  linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
782 
783  //
784  //p += vhat x Ihat vhat - done in a simpler way
785  if (m_useGyroTerm)
786  zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
787  //
788  zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
789  }
790 
791 
792  //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
793  spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
794  //
795  btMatrix3x3(m_baseMass, 0, 0,
796  0, m_baseMass, 0,
797  0, 0, m_baseMass),
798  //
799  btMatrix3x3(m_baseInertia[0], 0, 0,
800  0, m_baseInertia[1], 0,
801  0, 0, m_baseInertia[2])
802  );
803 
804  rot_from_world[0] = rot_from_parent[0];
805 
806  //
807  for (int i = 0; i < num_links; ++i) {
808  const int parent = m_links[i].m_parent;
809  rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
810  rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
811 
812  fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
813  fromWorld.m_rotMat = rot_from_world[i+1];
814  fromParent.transform(spatVel[parent+1], spatVel[i+1]);
815 
816  // now set vhat_i to its true value by doing
817  // vhat_i += qidot * shat_i
819  {
820  spatJointVel.setZero();
821 
822  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
823  spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
824 
825  // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
826  spatVel[i+1] += spatJointVel;
827 
828  //
829  // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
830  //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
831 
832  }
833  else
834  {
835  fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
836  fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
837  }
838 
839  // we can now calculate chat_i
840  spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);
841 
842  // calculate zhat_i^A
843  //
844  //external forces
845  btVector3 linkAppliedForce = isConstraintPass? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
846  btVector3 linkAppliedTorque =isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
847 
848  zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * linkAppliedTorque), -(rot_from_world[i+1] * linkAppliedForce ));
849 
850 #if 0
851  {
852 
853  b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
854  i+1,
855  zeroAccSpatFrc[i+1].m_topVec[0],
856  zeroAccSpatFrc[i+1].m_topVec[1],
857  zeroAccSpatFrc[i+1].m_topVec[2],
858 
859  zeroAccSpatFrc[i+1].m_bottomVec[0],
860  zeroAccSpatFrc[i+1].m_bottomVec[1],
861  zeroAccSpatFrc[i+1].m_bottomVec[2]);
862  }
863 #endif
864  //
865  //adding damping terms (only)
866  btScalar linDampMult = 1., angDampMult = 1.;
867  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()),
868  linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
869 
870  // calculate Ihat_i^A
871  //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
872  spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
873  //
874  btMatrix3x3(m_links[i].m_mass, 0, 0,
875  0, m_links[i].m_mass, 0,
876  0, 0, m_links[i].m_mass),
877  //
878  btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
879  0, m_links[i].m_inertiaLocal[1], 0,
880  0, 0, m_links[i].m_inertiaLocal[2])
881  );
882  //
883  //p += vhat x Ihat vhat - done in a simpler way
884  if(m_useGyroTerm)
885  zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
886  //
887  zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
888  //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
890  //btScalar parOmegaMod = temp.length();
891  //btScalar parOmegaModMax = 1000;
892  //if(parOmegaMod > parOmegaModMax)
893  // temp *= parOmegaModMax / parOmegaMod;
894  //zeroAccSpatFrc[i+1].addLinear(temp);
895  //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
896  //temp = spatCoriolisAcc[i].getLinear();
897  //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
898 
899 
900 
901  //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());
902  //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());
903  //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
904  }
905 
906  // 'Downward' loop.
907  // (part of TreeForwardDynamics in Mirtich.)
908  for (int i = num_links - 1; i >= 0; --i)
909  {
910  const int parent = m_links[i].m_parent;
911  fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
912 
913  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
914  {
915  btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
916  //
917  hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
918  //
919  Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
920  - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
921  - spatCoriolisAcc[i].dot(hDof)
922  ;
923  }
924 
925  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
926  {
927  btScalar *D_row = &D[dof * m_links[i].m_dofCount];
928  for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
929  {
930  btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
931  D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
932  }
933  }
934 
935  btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
936  switch(m_links[i].m_jointType)
937  {
940  {
941  invDi[0] = 1.0f / D[0];
942  break;
943  }
946  {
947  static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
948  static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
949 
950  //unroll the loop?
951  for(int row = 0; row < 3; ++row)
952  {
953  for(int col = 0; col < 3; ++col)
954  {
955  invDi[row * 3 + col] = invD3x3[row][col];
956  }
957  }
958 
959  break;
960  }
961  default:
962  {
963 
964  }
965  }
966 
967  //determine h*D^{-1}
968  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
969  {
970  spatForceVecTemps[dof].setZero();
971 
972  for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
973  {
974  btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
975  //
976  spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
977  }
978  }
979 
980  dyadTemp = spatInertia[i+1];
981 
982  //determine (h*D^{-1}) * h^{T}
983  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
984  {
985  btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
986  //
987  dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
988  }
989 
990  fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
991 
992  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
993  {
994  invD_times_Y[dof] = 0.f;
995 
996  for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
997  {
998  invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
999  }
1000  }
1001 
1002  spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1003 
1004  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1005  {
1006  btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1007  //
1008  spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1009  }
1010 
1011  fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1012 
1013  zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1014  }
1015 
1016 
1017  // Second 'upward' loop
1018  // (part of TreeForwardDynamics in Mirtich)
1019 
1020  if (m_fixedBase)
1021  {
1022  spatAcc[0].setZero();
1023  }
1024  else
1025  {
1026  if (num_links > 0)
1027  {
1028  m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
1029  m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
1030  m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
1032 
1033  }
1034 
1035  solveImatrix(zeroAccSpatFrc[0], result);
1036  spatAcc[0] = -result;
1037  }
1038 
1039 
1040  // now do the loop over the m_links
1041  for (int i = 0; i < num_links; ++i)
1042  {
1043  // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
1044  // a = apar + cor + Sqdd
1045  //or
1046  // qdd = D^{-1} * (Y - h^{T}*(apar+cor))
1047  // a = apar + Sqdd
1048 
1049  const int parent = m_links[i].m_parent;
1050  fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1051 
1052  fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1053 
1054  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1055  {
1056  btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1057  //
1058  Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
1059  }
1060 
1061  btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1062  //D^{-1} * (Y - h^{T}*apar)
1063  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]);
1064 
1065  spatAcc[i+1] += spatCoriolisAcc[i];
1066 
1067  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1068  spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1069 
1070  if (m_links[i].m_jointFeedback)
1071  {
1073 
1074  btVector3 angularBotVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
1075  btVector3 linearTopVec = (spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
1076 
1078  {
1079  //shift the reaction forces to the joint frame
1080  //linear (force) component is the same
1081  //shift the angular (torque, moment) component using the relative position, m_links[i].m_dVector
1082  angularBotVec = angularBotVec - linearTopVec.cross(m_links[i].m_dVector);
1083  }
1084 
1085 
1087  {
1088  if (isConstraintPass)
1089  {
1090  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1091  m_links[i].m_jointFeedback->m_reactionForces.m_topVec += m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1092  } else
1093  {
1094  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = m_links[i].m_cachedWorldTransform.getBasis()*angularBotVec;
1095  m_links[i].m_jointFeedback->m_reactionForces.m_topVec = m_links[i].m_cachedWorldTransform.getBasis()*linearTopVec;
1096  }
1097  } else
1098  {
1099  if (isConstraintPass)
1100  {
1101  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec += angularBotVec;
1102  m_links[i].m_jointFeedback->m_reactionForces.m_topVec += linearTopVec;
1103 
1104  }
1105  else
1106  {
1107  m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = angularBotVec;
1108  m_links[i].m_jointFeedback->m_reactionForces.m_topVec = linearTopVec;
1109  }
1110  }
1111  }
1112 
1113  }
1114 
1115  // transform base accelerations back to the world frame.
1116  btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1117  output[0] = omegadot_out[0];
1118  output[1] = omegadot_out[1];
1119  output[2] = omegadot_out[2];
1120 
1121  btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1122  output[3] = vdot_out[0];
1123  output[4] = vdot_out[1];
1124  output[5] = vdot_out[2];
1125 
1127  //printf("q = [");
1128  //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());
1129  //for(int link = 0; link < getNumLinks(); ++link)
1130  // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1131  // printf("%.6f ", m_links[link].m_jointPos[dof]);
1132  //printf("]\n");
1134  //printf("qd = [");
1135  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1136  // printf("%.6f ", m_realBuf[dof]);
1137  //printf("]\n");
1138  //printf("qdd = [");
1139  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1140  // printf("%.6f ", output[dof]);
1141  //printf("]\n");
1143 
1144  // Final step: add the accelerations (times dt) to the velocities.
1145 
1146  if (!isConstraintPass)
1147  {
1148  if(dt > 0.)
1149  applyDeltaVeeMultiDof(output, dt);
1150 
1151  }
1153  //btScalar angularThres = 1;
1154  //btScalar maxAngVel = 0.;
1155  //bool scaleDown = 1.;
1156  //for(int link = 0; link < m_links.size(); ++link)
1157  //{
1158  // if(spatVel[link+1].getAngular().length() > maxAngVel)
1159  // {
1160  // maxAngVel = spatVel[link+1].getAngular().length();
1161  // scaleDown = angularThres / spatVel[link+1].getAngular().length();
1162  // break;
1163  // }
1164  //}
1165 
1166  //if(scaleDown != 1.)
1167  //{
1168  // for(int link = 0; link < m_links.size(); ++link)
1169  // {
1170  // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
1171  // {
1172  // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1173  // getJointVelMultiDof(link)[dof] *= scaleDown;
1174  // }
1175  // }
1176  //}
1178 
1181  {
1182  for (int i = 0; i < num_links; ++i)
1183  {
1184  const int parent = m_links[i].m_parent;
1185  //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
1186  //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
1187 
1188  fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1189  fromWorld.m_rotMat = rot_from_world[i+1];
1190 
1191  // vhat_i = i_xhat_p(i) * vhat_p(i)
1192  fromParent.transform(spatVel[parent+1], spatVel[i+1]);
1193  //nice alternative below (using operator *) but it generates temps
1195 
1196  // now set vhat_i to its true value by doing
1197  // vhat_i += qidot * shat_i
1198  spatJointVel.setZero();
1199 
1200  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1201  spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1202 
1203  // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
1204  spatVel[i+1] += spatJointVel;
1205 
1206 
1207  fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
1208  fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
1209  }
1210  }
1211 
1212 }
1213 
1214 
1216  btAlignedObjectArray<btScalar> &scratch_r,
1219 {
1220  // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
1221  // and the base linear & angular accelerations.
1222 
1223  // We apply damping forces in this routine as well as any external forces specified by the
1224  // caller (via addBaseForce etc).
1225 
1226  // output should point to an array of 6 + num_links reals.
1227  // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
1228  // num_links joint acceleration values.
1229 
1230  int num_links = getNumLinks();
1231 
1232  const btScalar DAMPING_K1_LINEAR = m_linearDamping;
1233  const btScalar DAMPING_K2_LINEAR = m_linearDamping;
1234 
1235  const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
1236  const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
1237 
1238  btVector3 base_vel = getBaseVel();
1239  btVector3 base_omega = getBaseOmega();
1240 
1241  // Temporary matrices/vectors -- use scratch space from caller
1242  // so that we don't have to keep reallocating every frame
1243 
1244  scratch_r.resize(2*num_links + 6);
1245  scratch_v.resize(8*num_links + 6);
1246  scratch_m.resize(4*num_links + 4);
1247 
1248  btScalar * r_ptr = &scratch_r[0];
1249  btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
1250  btVector3 * v_ptr = &scratch_v[0];
1251 
1252  // vhat_i (top = angular, bottom = linear part)
1253  btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
1254  btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
1255 
1256  // zhat_i^A
1257  btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1258  btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1259 
1260  // chat_i (note NOT defined for the base)
1261  btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
1262  btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
1263 
1264  // top left, top right and bottom left blocks of Ihat_i^A.
1265  // bottom right block = transpose of top left block and is not stored.
1266  // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
1267  btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
1268  btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
1269  btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
1270 
1271  // Cached 3x3 rotation matrices from parent frame to this frame.
1272  btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1273  btMatrix3x3 * rot_from_world = &scratch_m[0];
1274 
1275  // hhat_i, ahat_i
1276  // hhat is NOT stored for the base (but ahat is)
1277  btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
1278  btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
1279  btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1280  btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1281 
1282  // Y_i, D_i
1283  btScalar * Y1 = r_ptr; r_ptr += num_links;
1284  btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
1285 
1286  // ptr to the joint accel part of the output
1287  btScalar * joint_accel = output + 6;
1288 
1289 
1290  // Start of the algorithm proper.
1291 
1292  // First 'upward' loop.
1293  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1294 
1295  rot_from_parent[0] = btMatrix3x3(m_baseQuat);
1296 
1297  vel_top_angular[0] = rot_from_parent[0] * base_omega;
1298  vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
1299 
1300  if (m_fixedBase) {
1301  f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
1302  } else {
1303  f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
1304  - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
1305 
1306  f_zero_acc_bottom_linear[0] =
1307  - (rot_from_parent[0] * m_baseTorque);
1308 
1309  if (m_useGyroTerm)
1310  f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
1311 
1312  f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
1313 
1314  }
1315 
1316 
1317 
1318  inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
1319 
1320 
1321  inertia_top_right[0].setValue(m_baseMass, 0, 0,
1322  0, m_baseMass, 0,
1323  0, 0, m_baseMass);
1324  inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
1325  0, m_baseInertia[1], 0,
1326  0, 0, m_baseInertia[2]);
1327 
1328  rot_from_world[0] = rot_from_parent[0];
1329 
1330  for (int i = 0; i < num_links; ++i) {
1331  const int parent = m_links[i].m_parent;
1332  rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
1333 
1334 
1335  rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1336 
1337  // vhat_i = i_xhat_p(i) * vhat_p(i)
1338  SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1339  vel_top_angular[parent+1], vel_bottom_linear[parent+1],
1340  vel_top_angular[i+1], vel_bottom_linear[i+1]);
1341 
1342  // we can now calculate chat_i
1343  // remember vhat_i is really vhat_p(i) (but in current frame) at this point
1344  coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector))
1345  + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
1346  if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
1347  coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
1348  coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
1349  } else {
1350  coriolis_top_angular[i] = btVector3(0,0,0);
1351  }
1352 
1353  // now set vhat_i to its true value by doing
1354  // vhat_i += qidot * shat_i
1355  vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
1356  vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
1357 
1358  // calculate zhat_i^A
1359  f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
1360  f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
1361 
1362  f_zero_acc_bottom_linear[i+1] =
1363  - (rot_from_world[i+1] * m_links[i].m_appliedTorque);
1364  if (m_useGyroTerm)
1365  {
1366  f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
1367  }
1368 
1369  f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
1370 
1371  // calculate Ihat_i^A
1372  inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
1373  inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
1374  0, m_links[i].m_mass, 0,
1375  0, 0, m_links[i].m_mass);
1376  inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
1377  0, m_links[i].m_inertiaLocal[1], 0,
1378  0, 0, m_links[i].m_inertiaLocal[2]);
1379  }
1380 
1381 
1382  // 'Downward' loop.
1383  // (part of TreeForwardDynamics in Mirtich.)
1384  for (int i = num_links - 1; i >= 0; --i) {
1385 
1386  h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
1387  h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
1388  btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
1389  D[i] = val;
1390  //Y1 = joint torque - zero acceleration force - coriolis force
1391  Y1[i] = m_links[i].m_jointTorque[0]
1392  - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_zero_acc_bottom_linear[i+1])
1393  - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
1394 
1395  const int parent = m_links[i].m_parent;
1396 
1397  btAssert(D[i]!=0.f);
1398  // Ip += pXi * (Ii - hi hi' / Di) * iXp
1399  const btScalar one_over_di = 1.0f / D[i];
1400 
1401 
1402 
1403 
1404  const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
1405  const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
1406  const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
1407 
1408 
1409  btMatrix3x3 r_cross;
1410  r_cross.setValue(
1411  0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
1412  m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
1413  -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
1414 
1415  inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
1416  inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
1417  inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
1418  (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
1419 
1420 
1421  // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
1422  btVector3 in_top, in_bottom, out_top, out_bottom;
1423  const btScalar Y_over_D = Y1[i] * one_over_di;
1424  in_top = f_zero_acc_top_angular[i+1]
1425  + inertia_top_left[i+1] * coriolis_top_angular[i]
1426  + inertia_top_right[i+1] * coriolis_bottom_linear[i]
1427  + Y_over_D * h_top[i];
1428  in_bottom = f_zero_acc_bottom_linear[i+1]
1429  + inertia_bottom_left[i+1] * coriolis_top_angular[i]
1430  + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
1431  + Y_over_D * h_bottom[i];
1432  InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1433  in_top, in_bottom, out_top, out_bottom);
1434  f_zero_acc_top_angular[parent+1] += out_top;
1435  f_zero_acc_bottom_linear[parent+1] += out_bottom;
1436  }
1437 
1438 
1439  // Second 'upward' loop
1440  // (part of TreeForwardDynamics in Mirtich)
1441 
1442  if (m_fixedBase)
1443  {
1444  accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
1445  }
1446  else
1447  {
1448  if (num_links > 0)
1449  {
1450  //Matrix<btScalar, 6, 6> Imatrix;
1451  //Imatrix.block<3,3>(0,0) = inertia_top_left[0];
1452  //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
1453  //Imatrix.block<3,3>(0,3) = inertia_top_right[0];
1454  //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
1455  //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
1456 
1457  m_cachedInertiaTopLeft = inertia_top_left[0];
1458  m_cachedInertiaTopRight = inertia_top_right[0];
1459  m_cachedInertiaLowerLeft = inertia_bottom_left[0];
1460  m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
1461 
1462  }
1463  btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
1464  btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
1465  float result[6];
1466 
1467  solveImatrix(rhs_top, rhs_bot, result);
1468 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
1469  for (int i = 0; i < 3; ++i) {
1470  accel_top[0][i] = -result[i];
1471  accel_bottom[0][i] = -result[i+3];
1472  }
1473 
1474  }
1475 
1476  // now do the loop over the m_links
1477  for (int i = 0; i < num_links; ++i)
1478  {
1479  //acceleration of the child link = acceleration of the parent transformed into child frame +
1480  // + coriolis acceleration
1481  // + joint acceleration
1482  const int parent = m_links[i].m_parent;
1483  SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1484  accel_top[parent+1], accel_bottom[parent+1],
1485  accel_top[i+1], accel_bottom[i+1]);
1486 
1487 
1488  joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
1489  accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
1490  accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
1491  }
1492 
1493  // transform base accelerations back to the world frame.
1494  btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
1495  output[0] = omegadot_out[0];
1496  output[1] = omegadot_out[1];
1497  output[2] = omegadot_out[2];
1498 
1499  btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
1500  output[3] = vdot_out[0];
1501  output[4] = vdot_out[1];
1502  output[5] = vdot_out[2];
1503 
1505  //printf("q = [");
1506  //printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1507  //for(int link = 0; link < getNumLinks(); ++link)
1508  // printf("%.2f ", m_links[link].m_jointPos[0]);
1509  //printf("]\n");
1511  //printf("qd = [");
1512  //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1513  // printf("%.2f ", m_realBuf[dof]);
1514  //printf("]\n");
1516  //printf("qdd = [");
1517  //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1518  // printf("%.2f ", output[dof]);
1519  //printf("]\n");
1521 
1522  // Final step: add the accelerations (times dt) to the velocities.
1523  //todo: do we already need to update the velocities, or can we move this into the constraint solver?
1524  //the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
1525  applyDeltaVee(output, dt);
1526 
1527 
1528 }
1529 
1530 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
1531 {
1532  int num_links = getNumLinks();
1534  if (num_links == 0)
1535  {
1536  // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1537  result[0] = rhs_bot[0] / m_baseInertia[0];
1538  result[1] = rhs_bot[1] / m_baseInertia[1];
1539  result[2] = rhs_bot[2] / m_baseInertia[2];
1540  result[3] = rhs_top[0] / m_baseMass;
1541  result[4] = rhs_top[1] / m_baseMass;
1542  result[5] = rhs_top[2] / m_baseMass;
1543  } else
1544  {
1550  tmp = invIupper_right * m_cachedInertiaLowerRight;
1551  btMatrix3x3 invI_upper_left = (tmp * Binv);
1552  btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1553  tmp = m_cachedInertiaTopLeft * invI_upper_left;
1554  tmp[0][0]-= 1.0;
1555  tmp[1][1]-= 1.0;
1556  tmp[2][2]-= 1.0;
1557  btMatrix3x3 invI_lower_left = (Binv * tmp);
1558 
1559  //multiply result = invI * rhs
1560  {
1561  btVector3 vtop = invI_upper_left*rhs_top;
1562  btVector3 tmp;
1563  tmp = invIupper_right * rhs_bot;
1564  vtop += tmp;
1565  btVector3 vbot = invI_lower_left*rhs_top;
1566  tmp = invI_lower_right * rhs_bot;
1567  vbot += tmp;
1568  result[0] = vtop[0];
1569  result[1] = vtop[1];
1570  result[2] = vtop[2];
1571  result[3] = vbot[0];
1572  result[4] = vbot[1];
1573  result[5] = vbot[2];
1574  }
1575 
1576  }
1577 }
1579 {
1580  int num_links = getNumLinks();
1582  if (num_links == 0)
1583  {
1584  // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1585  result.setAngular(rhs.getAngular() / m_baseInertia);
1586  result.setLinear(rhs.getLinear() / m_baseMass);
1587  } else
1588  {
1594  tmp = invIupper_right * m_cachedInertiaLowerRight;
1595  btMatrix3x3 invI_upper_left = (tmp * Binv);
1596  btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1597  tmp = m_cachedInertiaTopLeft * invI_upper_left;
1598  tmp[0][0]-= 1.0;
1599  tmp[1][1]-= 1.0;
1600  tmp[2][2]-= 1.0;
1601  btMatrix3x3 invI_lower_left = (Binv * tmp);
1602 
1603  //multiply result = invI * rhs
1604  {
1605  btVector3 vtop = invI_upper_left*rhs.getLinear();
1606  btVector3 tmp;
1607  tmp = invIupper_right * rhs.getAngular();
1608  vtop += tmp;
1609  btVector3 vbot = invI_lower_left*rhs.getLinear();
1610  tmp = invI_lower_right * rhs.getAngular();
1611  vbot += tmp;
1612  result.setVector(vtop, vbot);
1613  }
1614 
1615  }
1616 }
1617 
1618 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
1619 {
1620  for (int row = 0; row < rowsA; row++)
1621  {
1622  for (int col = 0; col < colsB; col++)
1623  {
1624  pC[row * colsB + col] = 0.f;
1625  for (int inner = 0; inner < rowsB; inner++)
1626  {
1627  pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1628  }
1629  }
1630  }
1631 }
1632 
1635 {
1636  // Temporary matrices/vectors -- use scratch space from caller
1637  // so that we don't have to keep reallocating every frame
1638 
1639 
1640  int num_links = getNumLinks();
1641  scratch_r.resize(m_dofCount);
1642  scratch_v.resize(4*num_links + 4);
1643 
1644  btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
1645  btVector3 * v_ptr = &scratch_v[0];
1646 
1647  // zhat_i^A (scratch space)
1648  btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
1649  v_ptr += num_links * 2 + 2;
1650 
1651  // rot_from_parent (cached from calcAccelerations)
1652  const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1653 
1654  // hhat (cached), accel (scratch)
1655  // hhat is NOT stored for the base (but ahat is)
1656  const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
1657  btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
1658  v_ptr += num_links * 2 + 2;
1659 
1660  // Y_i (scratch), invD_i (cached)
1661  const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1662  btScalar * Y = r_ptr;
1664  //aux variables
1665  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
1666  static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
1667  static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
1668  static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
1669  static btSpatialTransformationMatrix fromParent;
1671 
1672  // First 'upward' loop.
1673  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1674 
1675  // Fill in zero_acc
1676  // -- set to force/torque on the base, zero otherwise
1677  if (m_fixedBase)
1678  {
1679  zeroAccSpatFrc[0].setZero();
1680  } else
1681  {
1682  //test forces
1683  fromParent.m_rotMat = rot_from_parent[0];
1684  fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
1685  }
1686  for (int i = 0; i < num_links; ++i)
1687  {
1688  zeroAccSpatFrc[i+1].setZero();
1689  }
1690 
1691  // 'Downward' loop.
1692  // (part of TreeForwardDynamics in Mirtich.)
1693  for (int i = num_links - 1; i >= 0; --i)
1694  {
1695  const int parent = m_links[i].m_parent;
1696  fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1697 
1698  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1699  {
1700  Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
1701  - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1702  ;
1703  }
1704 
1705  btVector3 in_top, in_bottom, out_top, out_bottom;
1706  const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1707 
1708  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1709  {
1710  invD_times_Y[dof] = 0.f;
1711 
1712  for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1713  {
1714  invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1715  }
1716  }
1717 
1718  // Zp += pXi * (Zi + hi*Yi/Di)
1719  spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
1720 
1721  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1722  {
1723  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1724  //
1725  spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1726  }
1727 
1728 
1729  fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1730 
1731  zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1732  }
1733 
1734  // ptr to the joint accel part of the output
1735  btScalar * joint_accel = output + 6;
1736 
1737 
1738  // Second 'upward' loop
1739  // (part of TreeForwardDynamics in Mirtich)
1740 
1741  if (m_fixedBase)
1742  {
1743  spatAcc[0].setZero();
1744  }
1745  else
1746  {
1747  solveImatrix(zeroAccSpatFrc[0], result);
1748  spatAcc[0] = -result;
1749 
1750  }
1751 
1752  // now do the loop over the m_links
1753  for (int i = 0; i < num_links; ++i)
1754  {
1755  const int parent = m_links[i].m_parent;
1756  fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1757 
1758  fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1759 
1760  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1761  {
1762  const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1763  //
1764  Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
1765  }
1766 
1767  const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1768  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]);
1769 
1770  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1771  spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1772  }
1773 
1774  // transform base accelerations back to the world frame.
1775  btVector3 omegadot_out;
1776  omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1777  output[0] = omegadot_out[0];
1778  output[1] = omegadot_out[1];
1779  output[2] = omegadot_out[2];
1780 
1781  btVector3 vdot_out;
1782  vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
1783  output[3] = vdot_out[0];
1784  output[4] = vdot_out[1];
1785  output[5] = vdot_out[2];
1786 
1788  //printf("delta = [");
1789  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1790  // printf("%.2f ", output[dof]);
1791  //printf("]\n");
1793 }
1794 
1795 
1798 {
1799  // Temporary matrices/vectors -- use scratch space from caller
1800  // so that we don't have to keep reallocating every frame
1801  int num_links = getNumLinks();
1802  scratch_r.resize(num_links);
1803  scratch_v.resize(4*num_links + 4);
1804 
1805  btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
1806  btVector3 * v_ptr = &scratch_v[0];
1807 
1808  // zhat_i^A (scratch space)
1809  btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1810  btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1811 
1812  // rot_from_parent (cached from calcAccelerations)
1813  const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1814 
1815  // hhat (cached), accel (scratch)
1816  const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
1817  const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
1818  btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1819  btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1820 
1821  // Y_i (scratch), D_i (cached)
1822  btScalar * Y = r_ptr; r_ptr += num_links;
1823  const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
1824 
1825  btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
1826  btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
1827 
1828 
1829 
1830  // First 'upward' loop.
1831  // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1832 
1833  btVector3 input_force(force[3],force[4],force[5]);
1834  btVector3 input_torque(force[0],force[1],force[2]);
1835 
1836  // Fill in zero_acc
1837  // -- set to force/torque on the base, zero otherwise
1838  if (m_fixedBase)
1839  {
1840  zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
1841  } else
1842  {
1843  zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
1844  zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
1845  }
1846  for (int i = 0; i < num_links; ++i)
1847  {
1848  zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
1849  }
1850 
1851  // 'Downward' loop.
1852  for (int i = num_links - 1; i >= 0; --i)
1853  {
1854 // btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
1855  Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
1856  Y[i] += force[6 + i]; // add joint torque
1857 
1858  const int parent = m_links[i].m_parent;
1859 
1860  // Zp += pXi * (Zi + hi*Yi/Di)
1861  btVector3 in_top, in_bottom, out_top, out_bottom;
1862  const btScalar Y_over_D = Y[i] / D[i];
1863  in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
1864  in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
1865  InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1866  in_top, in_bottom, out_top, out_bottom);
1867  zero_acc_top_angular[parent+1] += out_top;
1868  zero_acc_bottom_linear[parent+1] += out_bottom;
1869  }
1870 
1871  // ptr to the joint accel part of the output
1872  btScalar * joint_accel = output + 6;
1873 
1874  // Second 'upward' loop
1875  if (m_fixedBase)
1876  {
1877  accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
1878  } else
1879  {
1880  btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
1881  btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
1882 
1883  float result[6];
1884  solveImatrix(rhs_top,rhs_bot, result);
1885  // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
1886 
1887  for (int i = 0; i < 3; ++i) {
1888  accel_top[0][i] = -result[i];
1889  accel_bottom[0][i] = -result[i+3];
1890  }
1891 
1892  }
1893 
1894  // now do the loop over the m_links
1895  for (int i = 0; i < num_links; ++i) {
1896  const int parent = m_links[i].m_parent;
1897  SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1898  accel_top[parent+1], accel_bottom[parent+1],
1899  accel_top[i+1], accel_bottom[i+1]);
1900  btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
1901  joint_accel[i] = Y_minus_hT_a / D[i];
1902  accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
1903  accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
1904  }
1905 
1906  // transform base accelerations back to the world frame.
1907  btVector3 omegadot_out;
1908  omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
1909  output[0] = omegadot_out[0];
1910  output[1] = omegadot_out[1];
1911  output[2] = omegadot_out[2];
1912 
1913  btVector3 vdot_out;
1914  vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
1915 
1916  output[3] = vdot_out[0];
1917  output[4] = vdot_out[1];
1918  output[5] = vdot_out[2];
1919 
1920 
1922  /*
1923  int ndof = getNumLinks() + 6;
1924  printf("test force(impulse) (%d) = [\n",ndof);
1925 
1926  for (int i=0;i<ndof;i++)
1927  {
1928  printf("%.2f ", force[i]);
1929  printf("]\n");
1930  }
1931 
1932  printf("delta(%d) = [",ndof);
1933  for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1934  printf("%.2f ", output[dof]);
1935  printf("]\n");
1937 */
1938 
1939  //int dummy = 0;
1940 }
1941 
1943 {
1944  int num_links = getNumLinks();
1945  // step position by adding dt * velocity
1946  btVector3 v = getBaseVel();
1947  m_basePos += dt * v;
1948 
1949  // "exponential map" method for the rotation
1950  btVector3 base_omega = getBaseOmega();
1951  const btScalar omega_norm = base_omega.norm();
1952  const btScalar omega_times_dt = omega_norm * dt;
1953  const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
1954  if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
1955  {
1956  const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2
1957  const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega|
1958  const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|)
1959  m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
1960  } else
1961  {
1962  m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
1963  }
1964 
1965  // Make sure the quaternion represents a valid rotation.
1966  // (Not strictly necessary, but helps prevent any round-off errors from building up.)
1968 
1969  // Finally we can update m_jointPos for each of the m_links
1970  for (int i = 0; i < num_links; ++i)
1971  {
1972  float jointVel = getJointVel(i);
1973  m_links[i].m_jointPos[0] += dt * jointVel;
1974  m_links[i].updateCache();
1975  }
1976 }
1977 
1979 {
1980  int num_links = getNumLinks();
1981  // step position by adding dt * velocity
1982  //btVector3 v = getBaseVel();
1983  //m_basePos += dt * v;
1984  //
1985  btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
1986  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)
1987  //
1988  pBasePos[0] += dt * pBaseVel[0];
1989  pBasePos[1] += dt * pBaseVel[1];
1990  pBasePos[2] += dt * pBaseVel[2];
1991 
1993  //local functor for quaternion integration (to avoid error prone redundancy)
1994  struct
1995  {
1996  //"exponential map" based on btTransformUtil::integrateTransform(..)
1997  void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
1998  {
1999  //baseBody => quat is alias and omega is global coor
2001 
2002  btVector3 axis;
2003  btVector3 angvel;
2004 
2005  if(!baseBody)
2006  angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
2007  else
2008  angvel = omega;
2009 
2010  btScalar fAngle = angvel.length();
2011  //limit the angular motion
2012  if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
2013  {
2014  fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
2015  }
2016 
2017  if ( fAngle < btScalar(0.001) )
2018  {
2019  // use Taylor's expansions of sync function
2020  axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
2021  }
2022  else
2023  {
2024  // sync(fAngle) = sin(c*fAngle)/t
2025  axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
2026  }
2027 
2028  if(!baseBody)
2029  quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;
2030  else
2031  quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
2032  //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
2033 
2034  quat.normalize();
2035  }
2036  } pQuatUpdateFun;
2038 
2039  //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
2040  //
2041  btScalar *pBaseQuat = pq ? pq : m_baseQuat;
2042  btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
2043  //
2044  static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
2045  static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
2046  pQuatUpdateFun(baseOmega, baseQuat, true, dt);
2047  pBaseQuat[0] = baseQuat.x();
2048  pBaseQuat[1] = baseQuat.y();
2049  pBaseQuat[2] = baseQuat.z();
2050  pBaseQuat[3] = baseQuat.w();
2051 
2052 
2053  //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
2054  //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
2055  //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
2056 
2057  if(pq)
2058  pq += 7;
2059  if(pqd)
2060  pqd += 6;
2061 
2062  // Finally we can update m_jointPos for each of the m_links
2063  for (int i = 0; i < num_links; ++i)
2064  {
2065  btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
2066  btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
2067 
2068  switch(m_links[i].m_jointType)
2069  {
2072  {
2073  btScalar jointVel = pJointVel[0];
2074  pJointPos[0] += dt * jointVel;
2075  break;
2076  }
2078  {
2079  static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
2080  static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
2081  pQuatUpdateFun(jointVel, jointOri, false, dt);
2082  pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
2083  break;
2084  }
2086  {
2087  pJointPos[0] += dt * getJointVelMultiDof(i)[0];
2088 
2089  btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
2090  btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
2091  pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
2092  pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
2093 
2094  break;
2095  }
2096  default:
2097  {
2098  }
2099 
2100  }
2101 
2102  m_links[i].updateCacheMultiDof(pq);
2103 
2104  if(pq)
2105  pq += m_links[i].m_posVarCount;
2106  if(pqd)
2107  pqd += m_links[i].m_dofCount;
2108  }
2109 }
2110 
2112  const btVector3 &contact_point,
2113  const btVector3 &normal_ang,
2114  const btVector3 &normal_lin,
2115  btScalar *jac,
2116  btAlignedObjectArray<btScalar> &scratch_r,
2118  btAlignedObjectArray<btMatrix3x3> &scratch_m) const
2119 {
2120  // temporary space
2121  int num_links = getNumLinks();
2122  int m_dofCount = getNumDofs();
2123  scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
2124  scratch_m.resize(num_links + 1);
2125 
2126  btVector3 * v_ptr = &scratch_v[0];
2127  btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
2128  btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
2129  btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
2130  btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2131 
2132  scratch_r.resize(m_dofCount);
2133  btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
2134 
2135  btMatrix3x3 * rot_from_world = &scratch_m[0];
2136 
2137  const btVector3 p_minus_com_world = contact_point - m_basePos;
2138  const btVector3 &normal_lin_world = normal_lin; //convenience
2139  const btVector3 &normal_ang_world = normal_ang;
2140 
2141  rot_from_world[0] = btMatrix3x3(m_baseQuat);
2142 
2143  // omega coeffients first.
2144  btVector3 omega_coeffs_world;
2145  omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
2146  jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
2147  jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
2148  jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
2149  // then v coefficients
2150  jac[3] = normal_lin_world[0];
2151  jac[4] = normal_lin_world[1];
2152  jac[5] = normal_lin_world[2];
2153 
2154  //create link-local versions of p_minus_com and normal
2155  p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
2156  n_local_lin[0] = rot_from_world[0] * normal_lin_world;
2157  n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2158 
2159  // Set remaining jac values to zero for now.
2160  for (int i = 6; i < 6 + m_dofCount; ++i)
2161  {
2162  jac[i] = 0;
2163  }
2164 
2165  // Qdot coefficients, if necessary.
2166  if (num_links > 0 && link > -1) {
2167 
2168  // TODO: speed this up -- don't calculate for m_links we don't need.
2169  // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2170  // which is resulting in repeated work being done...)
2171 
2172  // calculate required normals & positions in the local frames.
2173  for (int i = 0; i < num_links; ++i) {
2174 
2175  // transform to local frame
2176  const int parent = m_links[i].m_parent;
2177  const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2178  rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2179 
2180  n_local_lin[i+1] = mtx * n_local_lin[parent+1];
2181  n_local_ang[i+1] = mtx * n_local_ang[parent+1];
2182  p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
2183 
2184  // calculate the jacobian entry
2185  switch(m_links[i].m_jointType)
2186  {
2188  {
2189  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));
2190  results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2191  break;
2192  }
2194  {
2195  results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
2196  break;
2197  }
2199  {
2200  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));
2201  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));
2202  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));
2203 
2204  results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2205  results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
2206  results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
2207 
2208  break;
2209  }
2211  {
2212  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));
2213  results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
2214  results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
2215 
2216  break;
2217  }
2218  default:
2219  {
2220  }
2221  }
2222 
2223  }
2224 
2225  // Now copy through to output.
2226  //printf("jac[%d] = ", link);
2227  while (link != -1)
2228  {
2229  for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
2230  {
2231  jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
2232  //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
2233  }
2234 
2235  link = m_links[link].m_parent;
2236  }
2237  //printf("]\n");
2238  }
2239 }
2240 
2242  const btVector3 &contact_point,
2243  const btVector3 &normal,
2244  btScalar *jac,
2245  btAlignedObjectArray<btScalar> &scratch_r,
2247  btAlignedObjectArray<btMatrix3x3> &scratch_m) const
2248 {
2249  // temporary space
2250  int num_links = getNumLinks();
2251  scratch_v.resize(2*num_links + 2);
2252  scratch_m.resize(num_links + 1);
2253 
2254  btVector3 * v_ptr = &scratch_v[0];
2255  btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
2256  btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
2257  btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2258 
2259  scratch_r.resize(num_links);
2260  btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
2261 
2262  btMatrix3x3 * rot_from_world = &scratch_m[0];
2263 
2264  const btVector3 p_minus_com_world = contact_point - m_basePos;
2265 
2266  rot_from_world[0] = btMatrix3x3(m_baseQuat);
2267 
2268  p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
2269  n_local[0] = rot_from_world[0] * normal;
2270 
2271  // omega coeffients first.
2272  if (this->m_fixedBase)
2273  {
2274  for (int i=0;i<6;i++)
2275  {
2276  jac[i]=0;
2277  }
2278  } else
2279  {
2280  btVector3 omega_coeffs;
2281 
2282  omega_coeffs = p_minus_com_world.cross(normal);
2283  jac[0] = omega_coeffs[0];
2284  jac[1] = omega_coeffs[1];
2285  jac[2] = omega_coeffs[2];
2286  // then v coefficients
2287  jac[3] = normal[0];
2288  jac[4] = normal[1];
2289  jac[5] = normal[2];
2290  }
2291  // Set remaining jac values to zero for now.
2292  for (int i = 6; i < 6 + num_links; ++i) {
2293  jac[i] = 0;
2294  }
2295 
2296  // Qdot coefficients, if necessary.
2297  if (num_links > 0 && link > -1) {
2298 
2299  // TODO: speed this up -- don't calculate for m_links we don't need.
2300  // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2301  // which is resulting in repeated work being done...)
2302 
2303  // calculate required normals & positions in the local frames.
2304  for (int i = 0; i < num_links; ++i) {
2305 
2306  // transform to local frame
2307  const int parent = m_links[i].m_parent;
2308  const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2309  rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2310 
2311  n_local[i+1] = mtx * n_local[parent+1];
2312  p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
2313 
2314  // calculate the jacobian entry
2315  if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
2316  results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
2317  } else {
2318  results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
2319  }
2320  }
2321 
2322  // Now copy through to output.
2323  //printf("jac[%d] = ", link);
2324  while (link != -1)
2325  {
2326  jac[6 + link] = results[link];
2327  //printf("%.2f\t", jac[6 + link]);
2328  link = m_links[link].m_parent;
2329  }
2330  //printf("]\n");
2331  }
2332 }
2333 
2335 {
2336  m_awake = true;
2337 }
2338 
2340 {
2341  m_awake = false;
2342 }
2343 
2345 {
2346  int num_links = getNumLinks();
2347  extern bool gDisableDeactivation;
2348  if (!m_canSleep || gDisableDeactivation)
2349  {
2350  m_awake = true;
2351  m_sleepTimer = 0;
2352  return;
2353  }
2354 
2355  // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
2356  btScalar motion = 0;
2357  if(m_isMultiDof)
2358  {
2359  for (int i = 0; i < 6 + m_dofCount; ++i)
2360  motion += m_realBuf[i] * m_realBuf[i];
2361  }
2362  else
2363  {
2364  for (int i = 0; i < 6 + num_links; ++i)
2365  motion += m_realBuf[i] * m_realBuf[i];
2366  }
2367 
2368 
2369  if (motion < SLEEP_EPSILON) {
2370  m_sleepTimer += timestep;
2371  if (m_sleepTimer > SLEEP_TIMEOUT) {
2372  goToSleep();
2373  }
2374  } else {
2375  m_sleepTimer = 0;
2376  if (!m_awake)
2377  wakeUp();
2378  }
2379 }
2380 
2381 
2383 {
2384 
2385  int num_links = getNumLinks();
2386 
2387  // Cached 3x3 rotation matrices from parent frame to this frame.
2388  btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
2389 
2390  rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
2391 
2392  for (int i = 0; i < num_links; ++i)
2393  {
2394  rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
2395  }
2396 
2397  int nLinks = getNumLinks();
2399  world_to_local.resize(nLinks+1);
2400  local_origin.resize(nLinks+1);
2401 
2402  world_to_local[0] = getWorldToBaseRot();
2403  local_origin[0] = getBasePos();
2404 
2405  for (int k=0;k<getNumLinks();k++)
2406  {
2407  const int parent = getParent(k);
2408  world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
2409  local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
2410  }
2411 
2412  for (int link=0;link<getNumLinks();link++)
2413  {
2414  int index = link+1;
2415 
2416  btVector3 posr = local_origin[index];
2417  btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2418  btTransform tr;
2419  tr.setIdentity();
2420  tr.setOrigin(posr);
2421  tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
2422  getLink(link).m_cachedWorldTransform = tr;
2423 
2424  }
2425 
2426 }
2427 
2429 {
2430  world_to_local.resize(getNumLinks()+1);
2431  local_origin.resize(getNumLinks()+1);
2432 
2433  world_to_local[0] = getWorldToBaseRot();
2434  local_origin[0] = getBasePos();
2435 
2436  if (getBaseCollider())
2437  {
2438  btVector3 posr = local_origin[0];
2439  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2440  btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
2441  btTransform tr;
2442  tr.setIdentity();
2443  tr.setOrigin(posr);
2444  tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
2445 
2447 
2448  }
2449 
2450  for (int k=0;k<getNumLinks();k++)
2451  {
2452  const int parent = getParent(k);
2453  world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
2454  local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
2455  }
2456 
2457 
2458  for (int m=0;m<getNumLinks();m++)
2459  {
2461  if (col)
2462  {
2463  int link = col->m_link;
2464  btAssert(link == m);
2465 
2466  int index = link+1;
2467 
2468  btVector3 posr = local_origin[index];
2469  // float pos[4]={posr.x(),posr.y(),posr.z(),1};
2470  btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
2471  btTransform tr;
2472  tr.setIdentity();
2473  tr.setOrigin(posr);
2474  tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
2475 
2476  col->setWorldTransform(tr);
2477  }
2478  }
2479 }
2480 
2482 {
2483  int sz = sizeof(btMultiBodyData);
2484  return sz;
2485 }
2486 
2488 const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* serializer) const
2489 {
2490  btMultiBodyData* mbd = (btMultiBodyData*) dataBuffer;
2491  getBaseWorldTransform().serialize(mbd->m_baseWorldTransform);
2492  mbd->m_baseMass = this->getBaseMass();
2493  getBaseInertia().serialize(mbd->m_baseInertia);
2494  {
2495  char* name = (char*) serializer->findNameForPointer(m_baseName);
2496  mbd->m_baseName = (char*)serializer->getUniquePointer(name);
2497  if (mbd->m_baseName)
2498  {
2499  serializer->serializeName(name);
2500  }
2501  }
2502  mbd->m_numLinks = this->getNumLinks();
2503  if (mbd->m_numLinks)
2504  {
2505  int sz = sizeof(btMultiBodyLinkData);
2506  int numElem = mbd->m_numLinks;
2507  btChunk* chunk = serializer->allocate(sz,numElem);
2509  for (int i=0;i<numElem;i++,memPtr++)
2510  {
2511 
2512  memPtr->m_jointType = getLink(i).m_jointType;
2513  memPtr->m_dofCount = getLink(i).m_dofCount;
2514  memPtr->m_posVarCount = getLink(i).m_posVarCount;
2515 
2516  getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
2517  memPtr->m_linkMass = getLink(i).m_mass;
2518  memPtr->m_parentIndex = getLink(i).m_parent;
2519  getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
2520  getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
2521  getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
2522  btAssert(memPtr->m_dofCount<=3);
2523  for (int dof = 0;dof<getLink(i).m_dofCount;dof++)
2524  {
2525  getLink(i).getAxisBottom(dof).serialize(memPtr->m_jointAxisBottom[dof]);
2526  getLink(i).getAxisTop(dof).serialize(memPtr->m_jointAxisTop[dof]);
2527 
2528  memPtr->m_jointTorque[dof] = getLink(i).m_jointTorque[dof];
2529  memPtr->m_jointVel[dof] = getJointVelMultiDof(i)[dof];
2530 
2531  }
2532  int numPosVar = getLink(i).m_posVarCount;
2533  for (int posvar = 0; posvar < numPosVar;posvar++)
2534  {
2535  memPtr->m_jointPos[posvar] = getLink(i).m_jointPos[posvar];
2536  }
2537 
2538 
2539  {
2540  char* name = (char*) serializer->findNameForPointer(m_links[i].m_linkName);
2541  memPtr->m_linkName = (char*)serializer->getUniquePointer(name);
2542  if (memPtr->m_linkName)
2543  {
2544  serializer->serializeName(name);
2545  }
2546  }
2547  {
2548  char* name = (char*) serializer->findNameForPointer(m_links[i].m_jointName);
2549  memPtr->m_jointName = (char*)serializer->getUniquePointer(name);
2550  if (memPtr->m_jointName)
2551  {
2552  serializer->serializeName(name);
2553  }
2554  }
2555  memPtr->m_linkCollider = (btCollisionObjectData*)serializer->getUniquePointer(getLink(i).m_collider);
2556 
2557  }
2558  serializer->finalizeChunk(chunk,btMultiBodyLinkDataName,BT_ARRAY_CODE,(void*) &m_links[0]);
2559  }
2560  mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0;
2561 
2562  return btMultiBodyDataName;
2563 }
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 calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
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:681
bool m_useGyroTerm
Definition: btMultiBody.h:699
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
virtual ~btMultiBody()
btVector3 m_baseForce
Definition: btMultiBody.h:653
const btVector3 getBaseVel() const
Definition: btMultiBody.h:177
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:650
void finalizeMultiDof()
bool m_fixedBase
Definition: btMultiBody.h:689
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
#define btMultiBodyLinkDataName
Definition: btMultiBody.h:42
btScalar btSin(btScalar x)
Definition: btScalar.h:451
int getNumLinks() const
Definition: btMultiBody.h:154
void transformInverseRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
#define vecMulVecTranspose(v0, v1Transposed)
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:199
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
const btVector3 & getLinkForce(int i) const
virtual int calculateSerializeBufferSize() const
virtual void * getUniquePointer(void *oldPtr)=0
btScalar getBaseMass() const
Definition: btMultiBody.h:157
bool gJointFeedbackInWorldSpace
todo: determine if we need these options. If so, make a proper API, otherwise delete those globals ...
Definition: btMultiBody.cpp:34
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:687
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:35
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:657
#define btCollisionObjectData
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:422
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:678
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void applyDeltaVee(const btScalar *delta_vee)
Definition: btMultiBody.h:366
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
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool multiDof=false)
Definition: btMultiBody.cpp:90
btVector3 m_baseTorque
Definition: btMultiBody.h:654
#define ANGULAR_MOTION_THRESHOLD
bool m_isMultiDof
Definition: btMultiBody.h:703
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:684
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:158
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:648
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:686
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 filConstraintJacobianMultiDof(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:694
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()
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3 m_basePos
Definition: btMultiBody.h:647
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:656
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:709
virtual const char * findNameForPointer(const void *ptr) const =0
btVector3 m_baseInertia
Definition: btMultiBody.h:651
const char * m_baseName
Definition: btMultiBody.h:645
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:679
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:629
#define btMultiBodyDataName
Definition: btMultiBody.h:40
btVector3 getBaseOmega() const
Definition: btMultiBody.h:185
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
#define btMultiBodyData
Definition: btMultiBody.h:39
bool m_useGlobalVelocities
Definition: btMultiBody.h:706
virtual void serializeName(const char *ptr)=0
btScalar m_linearDamping
Definition: btMultiBody.h:697
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)
void fillContactJacobian(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
btAlignedObjectArray< btVector3 > m_vectorBuf
Definition: btMultiBody.h:680
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:133
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
const btVector3 & getLinkInertia(int i) const
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:181
void serialize(struct btTransformData &dataOut) const
Definition: btTransform.h:267
void stepPositions(btScalar dt)
int getParent(int link_num) const
const btVector3 & getBasePos() const
Definition: btMultiBody.h:176
btVector3 localPosToWorld(int i, const btVector3 &vec) const
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:659
btScalar * getJointTorqueMultiDof(int i)
#define btMultiBodyLinkData
Definition: btMultiBody.h:41
void * m_oldPtr
Definition: btSerializer.h:56
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:685
int getNumDofs() const
Definition: btMultiBody.h:155
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:698
btScalar btCos(btScalar x)
Definition: btScalar.h:450
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar getJointVel(int i) const
bool m_canSleep
Definition: btMultiBody.h:693
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