Bullet Collision Detection & Physics Library
btMultiBody.h
Go to the documentation of this file.
1 /*
2  * PURPOSE:
3  * Class representing an articulated rigid body. Stores the body's
4  * current state, allows forces and torques to be set, handles
5  * timestepping and implements Featherstone's algorithm.
6  *
7  * COPYRIGHT:
8  * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
9  * Portions written By Erwin Coumans: connection to LCP solver, various multibody constraints, replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10  * Portions written By Jakub Stepien: support for multi-DOF constraints, introduction of spatial algebra and several other improvements
11 
12  This software is provided 'as-is', without any express or implied warranty.
13  In no event will the authors be held liable for any damages arising from the use of this software.
14  Permission is granted to anyone to use this software for any purpose,
15  including commercial applications, and to alter it and redistribute it freely,
16  subject to the following restrictions:
17 
18  1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
19  2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
20  3. This notice may not be removed or altered from any source distribution.
21 
22  */
23 
24 #ifndef BT_MULTIBODY_H
25 #define BT_MULTIBODY_H
26 
27 #include "LinearMath/btScalar.h"
28 #include "LinearMath/btVector3.h"
30 #include "LinearMath/btMatrix3x3.h"
32 
34 #ifdef BT_USE_DOUBLE_PRECISION
35 #define btMultiBodyData btMultiBodyDoubleData
36 #define btMultiBodyDataName "btMultiBodyDoubleData"
37 #define btMultiBodyLinkData btMultiBodyLinkDoubleData
38 #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
39 #else
40 #define btMultiBodyData btMultiBodyFloatData
41 #define btMultiBodyDataName "btMultiBodyFloatData"
42 #define btMultiBodyLinkData btMultiBodyLinkFloatData
43 #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
44 #endif //BT_USE_DOUBLE_PRECISION
45 
46 #include "btMultiBodyLink.h"
48 
51 {
52 public:
54 
55  //
56  // initialization
57  //
58 
59  btMultiBody(int n_links, // NOT including the base
60  btScalar mass, // mass of base
61  const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
62  bool fixedBase, // whether the base is fixed (true) or can move (false)
63  bool canSleep, bool deprecatedMultiDof = true);
64 
65  virtual ~btMultiBody();
66 
67  //note: fixed link collision with parent is always disabled
68  void setupFixed(int i, //linkIndex
69  btScalar mass,
70  const btVector3 &inertia,
71  int parent,
72  const btQuaternion &rotParentToThis,
73  const btVector3 &parentComToThisPivotOffset,
74  const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision = true);
75 
76  void setupPrismatic(int i,
77  btScalar mass,
78  const btVector3 &inertia,
79  int parent,
80  const btQuaternion &rotParentToThis,
81  const btVector3 &jointAxis,
82  const btVector3 &parentComToThisPivotOffset,
83  const btVector3 &thisPivotToThisComOffset,
84  bool disableParentCollision);
85 
86  void setupRevolute(int i, // 0 to num_links-1
87  btScalar mass,
88  const btVector3 &inertia,
89  int parentIndex,
90  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
91  const btVector3 &jointAxis, // in my frame
92  const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
93  const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
94  bool disableParentCollision = false);
95 
96  void setupSpherical(int i, // linkIndex, 0 to num_links-1
97  btScalar mass,
98  const btVector3 &inertia,
99  int parent,
100  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
101  const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
102  const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
103  bool disableParentCollision = false);
104 
105  void setupPlanar(int i, // 0 to num_links-1
106  btScalar mass,
107  const btVector3 &inertia,
108  int parent,
109  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
110  const btVector3 &rotationAxis,
111  const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
112  bool disableParentCollision = false);
113 
114  const btMultibodyLink &getLink(int index) const
115  {
116  return m_links[index];
117  }
118 
120  {
121  return m_links[index];
122  }
123 
124  void setBaseCollider(btMultiBodyLinkCollider * collider) //collider can be NULL to disable collision for the base
125  {
126  m_baseCollider = collider;
127  }
129  {
130  return m_baseCollider;
131  }
133  {
134  return m_baseCollider;
135  }
136 
137  const btMultiBodyLinkCollider *getLinkCollider(int index) const
138  {
139  if (index >= 0 && index < getNumLinks())
140  {
141  return getLink(index).m_collider;
142  }
143  return 0;
144  }
145 
147  {
148  if (index >= 0 && index < getNumLinks())
149  {
150  return getLink(index).m_collider;
151  }
152  return 0;
153  }
154 
155  //
156  // get parent
157  // input: link num from 0 to num_links-1
158  // output: link num from 0 to num_links-1, OR -1 to mean the base.
159  //
160  int getParent(int link_num) const;
161 
162  //
163  // get number of m_links, masses, moments of inertia
164  //
165 
166  int getNumLinks() const { return m_links.size(); }
167  int getNumDofs() const { return m_dofCount; }
168  int getNumPosVars() const { return m_posVarCnt; }
169  btScalar getBaseMass() const { return m_baseMass; }
170  const btVector3 &getBaseInertia() const { return m_baseInertia; }
171  btScalar getLinkMass(int i) const;
172  const btVector3 &getLinkInertia(int i) const;
173 
174  //
175  // change mass (incomplete: can only change base mass and inertia at present)
176  //
177 
178  void setBaseMass(btScalar mass) { m_baseMass = mass; }
179  void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
180 
181  //
182  // get/set pos/vel/rot/omega for the base link
183  //
184 
185  const btVector3 &getBasePos() const
186  {
187  return m_basePos;
188  } // in world frame
189  const btVector3 getBaseVel() const
190  {
191  return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);
192  } // in world frame
194  {
195  return m_baseQuat;
196  }
197 
199  {
200  return m_basePos_interpolate;
201  } // in world frame
203  {
204  return m_baseQuat_interpolate;
205  }
206 
207  // rotates world vectors into base frame
208  btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame
209 
210  void setBasePos(const btVector3 &pos)
211  {
212  m_basePos = pos;
213  if(!isBaseKinematic())
214  m_basePos_interpolate = pos;
215  }
216 
218  {
219  m_basePos_interpolate = pos;
220  }
221 
223  {
224  setBasePos(tr.getOrigin());
225  setWorldToBaseRot(tr.getRotation().inverse());
226  }
227 
229  {
230  btTransform tr;
231  tr.setOrigin(getBasePos());
232  tr.setRotation(getWorldToBaseRot().inverse());
233  return tr;
234  }
235 
237  {
238  setInterpolateBasePos(tr.getOrigin());
239  setInterpolateWorldToBaseRot(tr.getRotation().inverse());
240  }
241 
243  {
244  btTransform tr;
245  tr.setOrigin(getInterpolateBasePos());
246  tr.setRotation(getInterpolateWorldToBaseRot().inverse());
247  return tr;
248  }
249 
250  void setBaseVel(const btVector3 &vel)
251  {
252  m_realBuf[3] = vel[0];
253  m_realBuf[4] = vel[1];
254  m_realBuf[5] = vel[2];
255  }
256 
258  {
259  m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
260  if(!isBaseKinematic())
261  m_baseQuat_interpolate = rot;
262  }
263 
265  {
266  m_baseQuat_interpolate = rot;
267  }
268 
269  void setBaseOmega(const btVector3 &omega)
270  {
271  m_realBuf[0] = omega[0];
272  m_realBuf[1] = omega[1];
273  m_realBuf[2] = omega[2];
274  }
275 
276  void saveKinematicState(btScalar timeStep);
277 
278  //
279  // get/set pos/vel for child m_links (i = 0 to num_links-1)
280  //
281 
282  btScalar getJointPos(int i) const;
283  btScalar getJointVel(int i) const;
284 
285  btScalar *getJointVelMultiDof(int i);
286  btScalar *getJointPosMultiDof(int i);
287 
288  const btScalar *getJointVelMultiDof(int i) const;
289  const btScalar *getJointPosMultiDof(int i) const;
290 
291  void setJointPos(int i, btScalar q);
292  void setJointVel(int i, btScalar qdot);
293  void setJointPosMultiDof(int i, const double *q);
294  void setJointVelMultiDof(int i, const double *qdot);
295  void setJointPosMultiDof(int i, const float *q);
296  void setJointVelMultiDof(int i, const float *qdot);
297 
298  //
299  // direct access to velocities as a vector of 6 + num_links elements.
300  // (omega first, then v, then joint velocities.)
301  //
303  {
304  return &m_realBuf[0];
305  }
306 
308  {
309  return &m_deltaV[0];
310  }
311 
313  {
314  return &m_splitV[0];
315  }
316  /* btScalar * getVelocityVector()
317  {
318  return &real_buf[0];
319  }
320  */
321 
322  //
323  // get the frames of reference (positions and orientations) of the child m_links
324  // (i = 0 to num_links-1)
325  //
326 
327  const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
328  const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
329  const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
330  const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
331 
332  //
333  // transform vectors in local frame of link i to world frame (or vice versa)
334  //
335  btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
336  btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
337  btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
338  btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
339 
340  //
341  // transform a frame in local coordinate to a frame in world coordinate
342  //
343  btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
344 
345 
346  //
347  // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
348  //
349 
350  void clearForcesAndTorques();
351  void clearConstraintForces();
352 
353  void clearVelocities();
354 
355  void addBaseForce(const btVector3 &f)
356  {
357  m_baseForce += f;
358  }
359  void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
360  void addLinkForce(int i, const btVector3 &f);
361  void addLinkTorque(int i, const btVector3 &t);
362 
364  {
365  m_baseConstraintForce += f;
366  }
367  void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
368  void addLinkConstraintForce(int i, const btVector3 &f);
369  void addLinkConstraintTorque(int i, const btVector3 &t);
370 
371  void addJointTorque(int i, btScalar Q);
372  void addJointTorqueMultiDof(int i, int dof, btScalar Q);
373  void addJointTorqueMultiDof(int i, const btScalar *Q);
374 
375  const btVector3 &getBaseForce() const { return m_baseForce; }
376  const btVector3 &getBaseTorque() const { return m_baseTorque; }
377  const btVector3 &getLinkForce(int i) const;
378  const btVector3 &getLinkTorque(int i) const;
379  btScalar getJointTorque(int i) const;
380  btScalar *getJointTorqueMultiDof(int i);
381 
382  //
383  // dynamics routines.
384  //
385 
386  // timestep the velocities (given the external forces/torques set using addBaseForce etc).
387  // also sets up caches for calcAccelerationDeltas.
388  //
389  // Note: the caller must provide three vectors which are used as
390  // temporary scratch space. The idea here is to reduce dynamic
391  // memory allocation: the same scratch vectors can be re-used
392  // again and again for different Multibodies, instead of each
393  // btMultiBody allocating (and then deallocating) their own
394  // individual scratch buffers. This gives a considerable speed
395  // improvement, at least on Windows (where dynamic memory
396  // allocation appears to be fairly slow).
397  //
398 
399  void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt,
400  btAlignedObjectArray<btScalar> & scratch_r,
403  bool isConstraintPass,
404  bool jointFeedbackInWorldSpace,
405  bool jointFeedbackInJointFrame
406  );
407 
409  //void stepVelocitiesMultiDof(btScalar dt,
410  // btAlignedObjectArray<btScalar> & scratch_r,
411  // btAlignedObjectArray<btVector3> & scratch_v,
412  // btAlignedObjectArray<btMatrix3x3> & scratch_m,
413  // bool isConstraintPass = false)
414  //{
415  // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
416  //}
417 
418  // calcAccelerationDeltasMultiDof
419  // input: force vector (in same format as jacobian, i.e.:
420  // 3 torque values, 3 force values, num_links joint torque values)
421  // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
422  // (existing contents of output array are replaced)
423  // calcAccelerationDeltasMultiDof must have been called first.
424  void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
426  btAlignedObjectArray<btVector3> &scratch_v) const;
427 
428  void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
429  {
430  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
431  {
432  m_deltaV[dof] += delta_vee[dof] * multiplier;
433  }
434  }
435  void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
436  {
437  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
438  {
439  m_splitV[dof] += delta_vee[dof] * multiplier;
440  }
441  }
442  void addSplitV()
443  {
444  applyDeltaVeeMultiDof(&m_splitV[0], 1);
445  }
447  {
448  applyDeltaVeeMultiDof(&m_splitV[0], -1);
449 
450  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
451  {
452  m_splitV[dof] = 0.f;
453  }
454  }
456  {
457  applyDeltaVeeMultiDof(&m_deltaV[0], 1);
458 
459  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
460  {
461  m_deltaV[dof] = 0.f;
462  }
463  }
464 
465  void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
466  {
467  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
468  // printf("%.4f ", delta_vee[dof]*multiplier);
469  //printf("\n");
470 
471  //btScalar sum = 0;
472  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
473  //{
474  // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
475  //}
476  //btScalar l = btSqrt(sum);
477 
478  //if (l>m_maxAppliedImpulse)
479  //{
480  // multiplier *= m_maxAppliedImpulse/l;
481  //}
482 
483  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
484  {
485  m_realBuf[dof] += delta_vee[dof] * multiplier;
486  btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity);
487  }
488  }
489 
490  // timestep the positions (given current velocities).
491  void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
492 
493  // predict the positions
494  void predictPositionsMultiDof(btScalar dt);
495 
496  //
497  // contacts
498  //
499 
500  // This routine fills out a contact constraint jacobian for this body.
501  // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
502  // 'normal' & 'contact_point' are both given in world coordinates.
503 
505  const btVector3 &contact_point,
506  const btVector3 &normal,
507  btScalar *jac,
510  btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
511 
512  //a more general version of fillContactJacobianMultiDof which does not assume..
513  //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
514  void fillConstraintJacobianMultiDof(int link,
515  const btVector3 &contact_point,
516  const btVector3 &normal_ang,
517  const btVector3 &normal_lin,
518  btScalar *jac,
521  btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
522 
523  //
524  // sleeping
525  //
526  void setCanSleep(bool canSleep)
527  {
528  if (m_canWakeup)
529  {
530  m_canSleep = canSleep;
531  }
532  }
533 
534  bool getCanSleep() const
535  {
536  return m_canSleep;
537  }
538 
539  bool getCanWakeup() const
540  {
541  return m_canWakeup;
542  }
543 
544  void setCanWakeup(bool canWakeup)
545  {
546  m_canWakeup = canWakeup;
547  }
548  bool isAwake() const
549  {
550  return m_awake;
551  }
552  void wakeUp();
553  void goToSleep();
554  void checkMotionAndSleepIfRequired(btScalar timestep);
555 
556  bool hasFixedBase() const;
557 
558  bool isBaseKinematic() const;
559 
560  bool isBaseStaticOrKinematic() const;
561 
562  // set the dynamic type in the base's collision flags.
563  void setBaseDynamicType(int dynamicType);
564 
565  void setFixedBase(bool fixedBase)
566  {
567  m_fixedBase = fixedBase;
568  if(m_fixedBase)
569  setBaseDynamicType(btCollisionObject::CF_STATIC_OBJECT);
570  else
571  setBaseDynamicType(btCollisionObject::CF_DYNAMIC_OBJECT);
572  }
573 
574  int getCompanionId() const
575  {
576  return m_companionId;
577  }
578  void setCompanionId(int id)
579  {
580  //printf("for %p setCompanionId(%d)\n",this, id);
581  m_companionId = id;
582  }
583 
584  void setNumLinks(int numLinks) //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
585  {
586  m_links.resize(numLinks);
587  }
588 
590  {
591  return m_linearDamping;
592  }
594  {
595  m_linearDamping = damp;
596  }
598  {
599  return m_angularDamping;
600  }
602  {
603  m_angularDamping = damp;
604  }
605 
606  bool getUseGyroTerm() const
607  {
608  return m_useGyroTerm;
609  }
610  void setUseGyroTerm(bool useGyro)
611  {
612  m_useGyroTerm = useGyro;
613  }
615  {
616  return m_maxCoordinateVelocity;
617  }
619  {
620  m_maxCoordinateVelocity = maxVel;
621  }
622 
624  {
625  return m_maxAppliedImpulse;
626  }
628  {
629  m_maxAppliedImpulse = maxImp;
630  }
631  void setHasSelfCollision(bool hasSelfCollision)
632  {
633  m_hasSelfCollision = hasSelfCollision;
634  }
635  bool hasSelfCollision() const
636  {
637  return m_hasSelfCollision;
638  }
639 
640  void finalizeMultiDof();
641 
642  void useRK4Integration(bool use) { m_useRK4 = use; }
643  bool isUsingRK4Integration() const { return m_useRK4; }
644  void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
645  bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
646 
647  bool isPosUpdated() const
648  {
649  return __posUpdated;
650  }
651  void setPosUpdated(bool updated)
652  {
653  __posUpdated = updated;
654  }
655 
656  //internalNeedsJointFeedback is for internal use only
658  {
659  return m_internalNeedsJointFeedback;
660  }
661  void forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local, btAlignedObjectArray<btVector3> & local_origin);
662 
663  void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
664 
665  void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
666  void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
667 
668  virtual int calculateSerializeBufferSize() const;
669 
671  virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const;
672 
673  const char *getBaseName() const
674  {
675  return m_baseName;
676  }
678  void setBaseName(const char *name)
679  {
680  m_baseName = name;
681  }
682 
684  void *getUserPointer() const
685  {
686  return m_userObjectPointer;
687  }
688 
689  int getUserIndex() const
690  {
691  return m_userIndex;
692  }
693 
694  int getUserIndex2() const
695  {
696  return m_userIndex2;
697  }
699  void setUserPointer(void *userPointer)
700  {
701  m_userObjectPointer = userPointer;
702  }
703 
705  void setUserIndex(int index)
706  {
707  m_userIndex = index;
708  }
709 
710  void setUserIndex2(int index)
711  {
712  m_userIndex2 = index;
713  }
714 
715  static void spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
716  const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
717  const btVector3 &top_in, // top part of input vector
718  const btVector3 &bottom_in, // bottom part of input vector
719  btVector3 &top_out, // top part of output vector
720  btVector3 &bottom_out); // bottom part of output vector
721 
722  void setLinkDynamicType(const int i, int type);
723 
724  bool isLinkStaticOrKinematic(const int i) const;
725 
726  bool isLinkKinematic(const int i) const;
727 
728  bool isLinkAndAllAncestorsStaticOrKinematic(const int i) const;
729 
730  bool isLinkAndAllAncestorsKinematic(const int i) const;
731 
732  void setSleepThreshold(btScalar sleepThreshold)
733  {
734  m_sleepEpsilon = sleepThreshold;
735  }
736 
737  void setSleepTimeout(btScalar sleepTimeout)
738  {
739  this->m_sleepTimeout = sleepTimeout;
740  }
741 
742 
743 private:
744  btMultiBody(const btMultiBody &); // not implemented
745  void operator=(const btMultiBody &); // not implemented
746 
747  void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const;
748  void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
749 
751  {
752  int dofOffset = 0, cfgOffset = 0;
753  for (int bidx = 0; bidx < m_links.size(); ++bidx)
754  {
755  m_links[bidx].m_dofOffset = dofOffset;
756  m_links[bidx].m_cfgOffset = cfgOffset;
757  dofOffset += m_links[bidx].m_dofCount;
758  cfgOffset += m_links[bidx].m_posVarCount;
759  }
760  }
761 
762  void mulMatrix(const btScalar *pA, const btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
763 
764 private:
766  const char *m_baseName; //memory needs to be manager by user!
767 
768  btVector3 m_basePos; // position of COM of base (world frame)
769  btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame)
770  btQuaternion m_baseQuat; // rotates world points into base frame
772 
773  btScalar m_baseMass; // mass of the base
774  btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
775 
776  btVector3 m_baseForce; // external force applied to base. World frame.
777  btVector3 m_baseTorque; // external torque applied to base. World frame.
778 
779  btVector3 m_baseConstraintForce; // external force applied to base. World frame.
780  btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
781 
782  btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
783 
784  //
785  // realBuf:
786  // offset size array
787  // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
788  // 6+num_links num_links D
789  //
790  // vectorBuf:
791  // offset size array
792  // 0 num_links h_top
793  // num_links num_links h_bottom
794  //
795  // matrixBuf:
796  // offset size array
797  // 0 num_links+1 rot_from_parent
798  //
804 
810 
812 
813  // Sleep parameters.
814  bool m_awake;
820 
824 
832 
834  int m_dofCount, m_posVarCnt;
835 
836  bool m_useRK4, m_useGlobalVelocities;
837  //for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
838  //https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
839 
842 
843  //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base.
845 };
846 
848 {
854 
855  btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
860 
861  double m_linkMass;
864 
867  double m_jointPos[7];
868  double m_jointVel[6];
869  double m_jointTorque[6];
870 
877 
878  char *m_linkName;
879  char *m_jointName;
882 };
883 
885 {
891  btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
896 
898  float m_linkMass;
901 
902  float m_jointPos[7];
903  float m_jointVel[6];
904  float m_jointTorque[6];
912 
913  char *m_linkName;
914  char *m_jointName;
917 };
918 
921 {
926  btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
927  double m_baseMass;
929  char m_padding[4];
930 
931  char *m_baseName;
934 };
935 
938 {
943 
944  btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
945  float m_baseMass;
947 
948  char *m_baseName;
951 };
952 
953 #endif
void btClamp(T &a, const T &lb, const T &ub)
Definition: btMinMax.h:57
#define output
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:909
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define ATTRIBUTE_ALIGNED16(a)
Definition: btScalar.h:99
void * m_userObjectPointer
users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPoin...
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
virtual int calculateSerializeBufferSize() const
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
btScalar m_maxCoordinateVelocity
Definition: btMultiBody.h:830
bool __posUpdated
Definition: btMultiBody.h:833
void useRK4Integration(bool use)
Definition: btMultiBody.h:642
const btScalar * getDeltaVelocityVector() const
Definition: btMultiBody.h:307
void setMaxCoordinateVelocity(btScalar maxVel)
Definition: btMultiBody.h:618
btTransform getInterpolateBaseWorldTransform() const
Definition: btMultiBody.h:242
void * getUserPointer() const
users can point to their objects, userPointer is not used by Bullet
Definition: btMultiBody.h:684
int m_userIndex2
Definition: btMultiBody.h:822
btMultiBodyLinkCollider * getLinkCollider(int index)
Definition: btMultiBody.h:146
bool m_useGyroTerm
Definition: btMultiBody.h:828
void setMaxAppliedImpulse(btScalar maxImp)
Definition: btMultiBody.h:627
void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:435
bool isAwake() const
Definition: btMultiBody.h:548
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:782
int m_companionId
Definition: btMultiBody.h:825
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:803
btMultiBodyLinkCollider * m_baseCollider
Definition: btMultiBody.h:765
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:504
bool getUseGyroTerm() const
Definition: btMultiBody.h:606
bool getCanWakeup() const
Definition: btMultiBody.h:539
void setCanSleep(bool canSleep)
Definition: btMultiBody.h:526
void setUseGyroTerm(bool useGyro)
Definition: btMultiBody.h:610
int getCompanionId() const
Definition: btMultiBody.h:574
btScalar getAngularDamping() const
Definition: btMultiBody.h:597
const btQuaternion & getInterpolateWorldToBaseRot() const
Definition: btMultiBody.h:202
int getUserIndex2() const
Definition: btMultiBody.h:694
btVector3 m_basePos_interpolate
Definition: btMultiBody.h:769
btVector3 m_baseInertia
Definition: btMultiBody.h:774
btAlignedObjectArray< btVector3 > m_vectorBuf
Definition: btMultiBody.h:802
btVector3 m_baseForce
Definition: btMultiBody.h:776
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:363
btScalar m_maxAppliedImpulse
Definition: btMultiBody.h:829
void setBaseMass(btScalar mass)
Definition: btMultiBody.h:178
btScalar m_sleepTimeout
Definition: btMultiBody.h:819
void setBaseInertia(const btVector3 &inertia)
Definition: btMultiBody.h:179
btScalar m_angularDamping
Definition: btMultiBody.h:827
btQuaternion m_baseQuat
Definition: btMultiBody.h:770
void setHasSelfCollision(bool hasSelfCollision)
Definition: btMultiBody.h:631
btVector3 m_basePos
Definition: btMultiBody.h:768
void * m_userObjectPointer
Definition: btMultiBody.h:821
void setLinearDamping(btScalar damp)
Definition: btMultiBody.h:593
const char * m_baseName
Definition: btMultiBody.h:766
bool hasSelfCollision() const
Definition: btMultiBody.h:635
void setUserIndex2(int index)
Definition: btMultiBody.h:710
void setBaseVel(const btVector3 &vel)
Definition: btMultiBody.h:250
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:808
void setBaseOmega(const btVector3 &omega)
Definition: btMultiBody.h:269
void addBaseTorque(const btVector3 &t)
Definition: btMultiBody.h:359
void useGlobalVelocities(bool use)
Definition: btMultiBody.h:644
int getNumLinks() const
Definition: btMultiBody.h:166
btScalar m_baseMass
Definition: btMultiBody.h:773
bool m_hasSelfCollision
Definition: btMultiBody.h:831
BT_DECLARE_ALIGNED_ALLOCATOR()
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:193
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:801
btVector3 m_baseConstraintTorque
Definition: btMultiBody.h:780
void operator=(const btMultiBody &)
void setBaseName(const char *name)
memory of setBaseName needs to be manager by user
Definition: btMultiBody.h:678
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btVector3 getBaseOmega() const
Definition: btMultiBody.h:208
const btScalar * getSplitVelocityVector() const
Definition: btMultiBody.h:312
btQuaternion m_baseQuat_interpolate
Definition: btMultiBody.h:771
void setSleepTimeout(btScalar sleepTimeout)
Definition: btMultiBody.h:737
bool getCanSleep() const
Definition: btMultiBody.h:534
btScalar m_sleepEpsilon
Definition: btMultiBody.h:818
btMultibodyLink & getLink(int index)
Definition: btMultiBody.h:119
int getNumDofs() const
Definition: btMultiBody.h:167
void setBasePos(const btVector3 &pos)
Definition: btMultiBody.h:210
bool m_useGlobalVelocities
Definition: btMultiBody.h:836
bool m_canWakeup
Definition: btMultiBody.h:816
bool isUsingGlobalVelocities() const
Definition: btMultiBody.h:645
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:807
void addSplitV()
Definition: btMultiBody.h:442
void setInterpolateBaseWorldTransform(const btTransform &tr)
Definition: btMultiBody.h:236
btMultiBodyLinkCollider * getBaseCollider()
Definition: btMultiBody.h:132
bool m_canSleep
Definition: btMultiBody.h:815
void setInterpolateBasePos(const btVector3 &pos)
Definition: btMultiBody.h:217
btVector3 m_baseTorque
Definition: btMultiBody.h:777
void setAngularDamping(btScalar damp)
Definition: btMultiBody.h:601
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
const char * getBaseName() const
Definition: btMultiBody.h:673
btScalar m_linearDamping
Definition: btMultiBody.h:826
int getUserIndex() const
Definition: btMultiBody.h:689
void setUserPointer(void *userPointer)
users can point to their objects, userPointer is not used by Bullet
Definition: btMultiBody.h:699
const btVector3 & getBaseForce() const
Definition: btMultiBody.h:375
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:455
void setPosUpdated(bool updated)
Definition: btMultiBody.h:651
btScalar getMaxAppliedImpulse() const
Definition: btMultiBody.h:623
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:805
bool isUsingRK4Integration() const
Definition: btMultiBody.h:643
bool m_cachedInertiaValid
Definition: btMultiBody.h:809
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:657
void setNumLinks(int numLinks)
Definition: btMultiBody.h:584
void setSleepThreshold(btScalar sleepThreshold)
Definition: btMultiBody.h:732
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:170
const btMultiBodyLinkCollider * getLinkCollider(int index) const
Definition: btMultiBody.h:137
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:302
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:228
btScalar getLinearDamping() const
Definition: btMultiBody.h:589
void setCanWakeup(bool canWakeup)
Definition: btMultiBody.h:544
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
bool m_kinematic_calculate_velocity
Definition: btMultiBody.h:844
btScalar m_sleepTimer
Definition: btMultiBody.h:817
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:806
bool m_fixedBase
Definition: btMultiBody.h:811
btScalar getBaseMass() const
Definition: btMultiBody.h:169
void setFixedBase(bool fixedBase)
Definition: btMultiBody.h:565
const btVector3 & getBaseTorque() const
Definition: btMultiBody.h:376
btScalar getMaxCoordinateVelocity() const
Definition: btMultiBody.h:614
btAlignedObjectArray< btScalar > m_deltaV
Definition: btMultiBody.h:800
int getNumPosVars() const
Definition: btMultiBody.h:168
void updateLinksDofOffsets()
Definition: btMultiBody.h:750
const btVector3 & getInterpolateBasePos() const
Definition: btMultiBody.h:198
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:367
void substractSplitV()
Definition: btMultiBody.h:446
bool isPosUpdated() const
Definition: btMultiBody.h:647
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
Definition: btMultiBody.h:841
btAlignedObjectArray< btScalar > m_splitV
Definition: btMultiBody.h:799
btVector3 m_baseConstraintForce
Definition: btMultiBody.h:779
void setInterpolateWorldToBaseRot(const btQuaternion &rot)
Definition: btMultiBody.h:264
void setWorldToBaseRot(const btQuaternion &rot)
Definition: btMultiBody.h:257
void setCompanionId(int id)
Definition: btMultiBody.h:578
btMultiBody(const btMultiBody &)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:465
const btVector3 getBaseVel() const
Definition: btMultiBody.h:189
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:355
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:428
void setUserIndex(int index)
users can point to their objects, userPointer is not used by Bullet
Definition: btMultiBody.h:705
void setBaseCollider(btMultiBodyLinkCollider *collider)
Definition: btMultiBody.h:124
void setBaseWorldTransform(const btTransform &tr)
Definition: btMultiBody.h:222
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:160
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:118
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:146
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btMultiBody.h:921
btVector3DoubleData m_baseWorldPosition
Definition: btMultiBody.h:922
btVector3DoubleData m_baseInertia
Definition: btMultiBody.h:926
btVector3DoubleData m_baseLinearVelocity
Definition: btMultiBody.h:924
btCollisionObjectDoubleData * m_baseCollider
Definition: btMultiBody.h:933
btVector3DoubleData m_baseAngularVelocity
Definition: btMultiBody.h:925
btQuaternionDoubleData m_baseWorldOrientation
Definition: btMultiBody.h:923
btMultiBodyLinkDoubleData * m_links
Definition: btMultiBody.h:932
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
Definition: btMultiBody.h:938
btCollisionObjectFloatData * m_baseCollider
Definition: btMultiBody.h:950
btQuaternionFloatData m_baseWorldOrientation
Definition: btMultiBody.h:940
btVector3FloatData m_baseInertia
Definition: btMultiBody.h:944
btVector3FloatData m_baseAngularVelocity
Definition: btMultiBody.h:942
btVector3FloatData m_baseLinearVelocity
Definition: btMultiBody.h:941
btMultiBodyLinkFloatData * m_links
Definition: btMultiBody.h:949
btVector3FloatData m_baseWorldPosition
Definition: btMultiBody.h:939
btQuaternionDoubleData m_zeroRotParentToThis
Definition: btMultiBody.h:849
btVector3DoubleData m_absFrameLocVelocityBottom
Definition: btMultiBody.h:859
btCollisionObjectDoubleData * m_linkCollider
Definition: btMultiBody.h:880
btVector3DoubleData m_jointAxisBottom[6]
Definition: btMultiBody.h:853
btVector3DoubleData m_thisPivotToThisComOffset
Definition: btMultiBody.h:851
btVector3DoubleData m_absFrameTotVelocityTop
Definition: btMultiBody.h:856
btVector3DoubleData m_parentComToThisPivotOffset
Definition: btMultiBody.h:850
btVector3DoubleData m_jointAxisTop[6]
Definition: btMultiBody.h:852
btVector3DoubleData m_absFrameTotVelocityBottom
Definition: btMultiBody.h:857
btVector3DoubleData m_absFrameLocVelocityTop
Definition: btMultiBody.h:858
btVector3DoubleData m_linkInertia
Definition: btMultiBody.h:855
btCollisionObjectFloatData * m_linkCollider
Definition: btMultiBody.h:915
btVector3FloatData m_absFrameLocVelocityTop
Definition: btMultiBody.h:894
btQuaternionFloatData m_zeroRotParentToThis
Definition: btMultiBody.h:886
btVector3FloatData m_absFrameTotVelocityBottom
Definition: btMultiBody.h:893
btVector3FloatData m_jointAxisBottom[6]
Definition: btMultiBody.h:890
btVector3FloatData m_parentComToThisPivotOffset
Definition: btMultiBody.h:887
btVector3FloatData m_absFrameLocVelocityBottom
Definition: btMultiBody.h:895
btVector3FloatData m_linkInertia
Definition: btMultiBody.h:891
btVector3FloatData m_thisPivotToThisComOffset
Definition: btMultiBody.h:888
btVector3FloatData m_absFrameTotVelocityTop
Definition: btMultiBody.h:892
btVector3FloatData m_jointAxisTop[6]
Definition: btMultiBody.h:889
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.