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: 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 #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 
33 
34 #include "btMultiBodyLink.h"
36 
38 {
39 public:
40 
41 
43 
44  //
45  // initialization
46  //
47 
48  btMultiBody(int n_links, // NOT including the base
49  btScalar mass, // mass of base
50  const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
51  bool fixedBase, // whether the base is fixed (true) or can move (false)
52  bool canSleep,
53  bool multiDof = false
54  );
55 
56 
57  ~btMultiBody();
58 
59  void setupFixed(int linkIndex,
60  btScalar mass,
61  const btVector3 &inertia,
62  int parent,
63  const btQuaternion &rotParentToThis,
64  const btVector3 &parentComToThisPivotOffset,
65  const btVector3 &thisPivotToThisComOffset,
66  bool disableParentCollision);
67 
68 
69  void setupPrismatic(int i,
70  btScalar mass,
71  const btVector3 &inertia,
72  int parent,
73  const btQuaternion &rotParentToThis,
74  const btVector3 &jointAxis,
75  const btVector3 &parentComToThisComOffset,
76  const btVector3 &thisPivotToThisComOffset,
77  bool disableParentCollision);
78 
79  void setupRevolute(int linkIndex, // 0 to num_links-1
80  btScalar mass,
81  const btVector3 &inertia,
82  int parentIndex,
83  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
84  const btVector3 &jointAxis, // in my frame
85  const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
86  const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
87  bool disableParentCollision=false);
88 
89  void setupSpherical(int linkIndex, // 0 to num_links-1
90  btScalar mass,
91  const btVector3 &inertia,
92  int parent,
93  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
94  const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
95  const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
96  bool disableParentCollision=false);
97 
98 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
99  void setupPlanar(int i, // 0 to num_links-1
100  btScalar mass,
101  const btVector3 &inertia,
102  int parent,
103  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
104  const btVector3 &rotationAxis,
105  const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
106  bool disableParentCollision=false);
107 #endif
108 
109  const btMultibodyLink& getLink(int index) const
110  {
111  return m_links[index];
112  }
113 
115  {
116  return m_links[index];
117  }
118 
119 
120  void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
121  {
122  m_baseCollider = collider;
123  }
125  {
126  return m_baseCollider;
127  }
129  {
130  return m_baseCollider;
131  }
132 
133  //
134  // get parent
135  // input: link num from 0 to num_links-1
136  // output: link num from 0 to num_links-1, OR -1 to mean the base.
137  //
138  int getParent(int link_num) const;
139 
140 
141  //
142  // get number of m_links, masses, moments of inertia
143  //
144 
145  int getNumLinks() const { return m_links.size(); }
146  int getNumDofs() const { return m_dofCount; }
147  int getNumPosVars() const { return m_posVarCnt; }
148  btScalar getBaseMass() const { return m_baseMass; }
149  const btVector3 & getBaseInertia() const { return m_baseInertia; }
150  btScalar getLinkMass(int i) const;
151  const btVector3 & getLinkInertia(int i) const;
152 
153 
154  //
155  // change mass (incomplete: can only change base mass and inertia at present)
156  //
157 
158  void setBaseMass(btScalar mass) { m_baseMass = mass; }
159  void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
160 
161 
162  //
163  // get/set pos/vel/rot/omega for the base link
164  //
165 
166  const btVector3 & getBasePos() const { return m_basePos; } // in world frame
167  const btVector3 getBaseVel() const
168  {
169  return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
170  } // in world frame
172  {
173  return m_baseQuat;
174  } // rotates world vectors into base frame
175  btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame
176 
177  void setBasePos(const btVector3 &pos)
178  {
179  m_basePos = pos;
180  }
181 
183  {
184  setBasePos(tr.getOrigin());
186 
187  }
188  void setBaseVel(const btVector3 &vel)
189  {
190 
191  m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
192  }
193  void setWorldToBaseRot(const btQuaternion &rot)
194  {
195  m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
196  }
197  void setBaseOmega(const btVector3 &omega)
198  {
199  m_realBuf[0]=omega[0];
200  m_realBuf[1]=omega[1];
201  m_realBuf[2]=omega[2];
202  }
203 
204 
205  //
206  // get/set pos/vel for child m_links (i = 0 to num_links-1)
207  //
208 
209  btScalar getJointPos(int i) const;
210  btScalar getJointVel(int i) const;
211 
212  btScalar * getJointVelMultiDof(int i);
213  btScalar * getJointPosMultiDof(int i);
214 
215  void setJointPos(int i, btScalar q);
216  void setJointVel(int i, btScalar qdot);
217  void setJointPosMultiDof(int i, btScalar *q);
218  void setJointVelMultiDof(int i, btScalar *qdot);
219 
220  //
221  // direct access to velocities as a vector of 6 + num_links elements.
222  // (omega first, then v, then joint velocities.)
223  //
224  const btScalar * getVelocityVector() const
225  {
226  return &m_realBuf[0];
227  }
228 /* btScalar * getVelocityVector()
229  {
230  return &real_buf[0];
231  }
232  */
233 
234  //
235  // get the frames of reference (positions and orientations) of the child m_links
236  // (i = 0 to num_links-1)
237  //
238 
239  const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
240  const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
241 
242 
243  //
244  // transform vectors in local frame of link i to world frame (or vice versa)
245  //
246  btVector3 localPosToWorld(int i, const btVector3 &vec) const;
247  btVector3 localDirToWorld(int i, const btVector3 &vec) const;
248  btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
249  btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
250 
251 
252  //
253  // calculate kinetic energy and angular momentum
254  // useful for debugging.
255  //
256 
257  btScalar getKineticEnergy() const;
259 
260 
261  //
262  // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
263  //
264 
265  void clearForcesAndTorques();
266  void clearVelocities();
267 
268  void addBaseForce(const btVector3 &f)
269  {
270  m_baseForce += f;
271  }
272  void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
273  void addLinkForce(int i, const btVector3 &f);
274  void addLinkTorque(int i, const btVector3 &t);
275  void addJointTorque(int i, btScalar Q);
276  void addJointTorqueMultiDof(int i, int dof, btScalar Q);
277  void addJointTorqueMultiDof(int i, const btScalar *Q);
278 
279  const btVector3 & getBaseForce() const { return m_baseForce; }
280  const btVector3 & getBaseTorque() const { return m_baseTorque; }
281  const btVector3 & getLinkForce(int i) const;
282  const btVector3 & getLinkTorque(int i) const;
283  btScalar getJointTorque(int i) const;
285 
286 
287  //
288  // dynamics routines.
289  //
290 
291  // timestep the velocities (given the external forces/torques set using addBaseForce etc).
292  // also sets up caches for calcAccelerationDeltas.
293  //
294  // Note: the caller must provide three vectors which are used as
295  // temporary scratch space. The idea here is to reduce dynamic
296  // memory allocation: the same scratch vectors can be re-used
297  // again and again for different Multibodies, instead of each
298  // btMultiBody allocating (and then deallocating) their own
299  // individual scratch buffers. This gives a considerable speed
300  // improvement, at least on Windows (where dynamic memory
301  // allocation appears to be fairly slow).
302  //
303  void stepVelocities(btScalar dt,
307 
312 
313  // calcAccelerationDeltas
314  // input: force vector (in same format as jacobian, i.e.:
315  // 3 torque values, 3 force values, num_links joint torque values)
316  // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
317  // (existing contents of output array are replaced)
318  // stepVelocities must have been called first.
319  void calcAccelerationDeltas(const btScalar *force, btScalar *output,
321  btAlignedObjectArray<btVector3> &scratch_v) const;
322 
323  void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
325  btAlignedObjectArray<btVector3> &scratch_v) const;
326 
327  // apply a delta-vee directly. used in sequential impulses code.
328  void applyDeltaVee(const btScalar * delta_vee)
329  {
330 
331  for (int i = 0; i < 6 + getNumLinks(); ++i)
332  {
333  m_realBuf[i] += delta_vee[i];
334  }
335 
336  }
337  void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
338  {
339  btScalar sum = 0;
340  for (int i = 0; i < 6 + getNumLinks(); ++i)
341  {
342  sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
343  }
344  btScalar l = btSqrt(sum);
345  /*
346  static btScalar maxl = -1e30f;
347  if (l>maxl)
348  {
349  maxl=l;
350  // printf("maxl=%f\n",maxl);
351  }
352  */
353  if (l>m_maxAppliedImpulse)
354  {
355 // printf("exceeds 100: l=%f\n",maxl);
356  multiplier *= m_maxAppliedImpulse/l;
357  }
358 
359  for (int i = 0; i < 6 + getNumLinks(); ++i)
360  {
361  sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
362  m_realBuf[i] += delta_vee[i] * multiplier;
364  }
365  }
366 
367  void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
368  {
369  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
370  // printf("%.4f ", delta_vee[dof]*multiplier);
371  //printf("\n");
372 
373  //btScalar sum = 0;
374  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
375  //{
376  // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
377  //}
378  //btScalar l = btSqrt(sum);
379 
380  //if (l>m_maxAppliedImpulse)
381  //{
382  // multiplier *= m_maxAppliedImpulse/l;
383  //}
384 
385  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
386  {
387  m_realBuf[dof] += delta_vee[dof] * multiplier;
389  }
390  }
391 
392  // timestep the positions (given current velocities).
393  void stepPositions(btScalar dt);
394  void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
395 
396 
397  //
398  // contacts
399  //
400 
401  // This routine fills out a contact constraint jacobian for this body.
402  // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
403  // 'normal' & 'contact_point' are both given in world coordinates.
404  void fillContactJacobian(int link,
405  const btVector3 &contact_point,
406  const btVector3 &normal,
407  btScalar *jac,
410  btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
411 
412  //multidof version of fillContactJacobian
414  const btVector3 &contact_point,
415  const btVector3 &normal,
416  btScalar *jac,
419  btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
420 
421  //a more general version of fillContactJacobianMultiDof which does not assume..
422  //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
423  void filConstraintJacobianMultiDof(int link,
424  const btVector3 &contact_point,
425  const btVector3 &normal_ang,
426  const btVector3 &normal_lin,
427  btScalar *jac,
430  btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
431 
432 
433  //
434  // sleeping
435  //
436  void setCanSleep(bool canSleep)
437  {
438  m_canSleep = canSleep;
439  }
440 
441  bool getCanSleep()const
442  {
443  return m_canSleep;
444  }
445 
446  bool isAwake() const { return m_awake; }
447  void wakeUp();
448  void goToSleep();
450 
451  bool hasFixedBase() const
452  {
453  return m_fixedBase;
454  }
455 
456  int getCompanionId() const
457  {
458  return m_companionId;
459  }
460  void setCompanionId(int id)
461  {
462  //printf("for %p setCompanionId(%d)\n",this, id);
463  m_companionId = id;
464  }
465 
466  void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
467  {
468  m_links.resize(numLinks);
469  }
470 
472  {
473  return m_linearDamping;
474  }
476  {
477  m_linearDamping = damp;
478  }
480  {
481  return m_angularDamping;
482  }
484  {
485  m_angularDamping = damp;
486  }
487 
488  bool getUseGyroTerm() const
489  {
490  return m_useGyroTerm;
491  }
492  void setUseGyroTerm(bool useGyro)
493  {
494  m_useGyroTerm = useGyro;
495  }
497  {
498  return m_maxCoordinateVelocity ;
499  }
501  {
502  m_maxCoordinateVelocity = maxVel;
503  }
504 
506  {
507  return m_maxAppliedImpulse;
508  }
510  {
511  m_maxAppliedImpulse = maxImp;
512  }
514  {
516  }
517  bool hasSelfCollision() const
518  {
519  return m_hasSelfCollision;
520  }
521 
522  bool isMultiDof() { return m_isMultiDof; }
523  void finalizeMultiDof();
524 
525  void useRK4Integration(bool use) { m_useRK4 = use; }
526  bool isUsingRK4Integration() const { return m_useRK4; }
527  void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
529 
530  bool isPosUpdated() const
531  {
532  return __posUpdated;
533  }
534  void setPosUpdated(bool updated)
535  {
536  __posUpdated = updated;
537  }
538 
539 private:
540  btMultiBody(const btMultiBody &); // not implemented
541  void operator=(const btMultiBody &); // not implemented
542 
543  void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
544 
545  void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
546 #ifdef TEST_SPATIAL_ALGEBRA_LAYER
547  void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
548 #endif
549 
551  {
552  int dofOffset = 0, cfgOffset = 0;
553  for(int bidx = 0; bidx < m_links.size(); ++bidx)
554  {
555  m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
556  dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
557  }
558  }
559 
560  void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
561 
562 
563 private:
564 
566 
567  btVector3 m_basePos; // position of COM of base (world frame)
568  btQuaternion m_baseQuat; // rotates world points into base frame
569 
570  btScalar m_baseMass; // mass of the base
571  btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
572 
573  btVector3 m_baseForce; // external force applied to base. World frame.
574  btVector3 m_baseTorque; // external torque applied to base. World frame.
575 
576  btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
578 
579 
580  //
581  // realBuf:
582  // offset size array
583  // 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]
584  // 6+num_links num_links D
585  //
586  // vectorBuf:
587  // offset size array
588  // 0 num_links h_top
589  // num_links num_links h_bottom
590  //
591  // matrixBuf:
592  // offset size array
593  // 0 num_links+1 rot_from_parent
594  //
595 
599 
600  //std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;
601 
606 
608 
609  // Sleep parameters.
610  bool m_awake;
613 
625 };
626 
627 #endif
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
static T sum(const btAlignedObjectArray< T > &items)
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
void setCanSleep(bool canSleep)
Definition: btMultiBody.h:436
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
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< btMultiBodyLinkCollider * > m_colliders
Definition: btMultiBody.h:577
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:598
bool m_useGyroTerm
Definition: btMultiBody.h:617
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:109
BT_DECLARE_ALIGNED_ALLOCATOR()
btVector3 m_baseForce
Definition: btMultiBody.h:573
bool isUsingGlobalVelocities() const
Definition: btMultiBody.h:528
const btVector3 getBaseVel() const
Definition: btMultiBody.h:167
btMultiBodyLinkCollider * getBaseCollider()
Definition: btMultiBody.h:128
void setNumLinks(int numLinks)
Definition: btMultiBody.h:466
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btMultiBodyLinkCollider * m_baseCollider
Definition: btMultiBody.h:565
btScalar m_baseMass
Definition: btMultiBody.h:570
void finalizeMultiDof()
bool m_fixedBase
Definition: btMultiBody.h:607
bool isAwake() const
Definition: btMultiBody.h:446
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
int getNumLinks() const
Definition: btMultiBody.h:145
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
btScalar m_maxCoordinateVelocity
Definition: btMultiBody.h:619
void addBaseTorque(const btVector3 &t)
Definition: btMultiBody.h:272
const btVector3 & getLinkForce(int i) const
btScalar getBaseMass() const
Definition: btMultiBody.h:148
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btVector3 localDirToWorld(int i, const btVector3 &vec) const
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:605
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 setBaseCollider(btMultiBodyLinkCollider *collider)
Definition: btMultiBody.h:120
btScalar getJointPos(int i) const
void btClamp(T &a, const T &lb, const T &ub)
Definition: btMinMax.h:59
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisComOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
const btVector3 & getBaseTorque() const
Definition: btMultiBody.h:280
bool isPosUpdated() const
Definition: btMultiBody.h:530
void setJointPosMultiDof(int i, btScalar *q)
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:367
void setWorldToBaseRot(const btQuaternion &rot)
Definition: btMultiBody.h:193
btScalar m_maxAppliedImpulse
Definition: btMultiBody.h:618
btScalar getMaxAppliedImpulse() const
Definition: btMultiBody.h:505
void applyDeltaVee(const btScalar *delta_vee)
Definition: btMultiBody.h:328
void clearForcesAndTorques()
btVector3 getAngularMomentum() const
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool multiDof=false)
Definition: btMultiBody.cpp:84
void setBaseVel(const btVector3 &vel)
Definition: btMultiBody.h:188
btVector3 m_baseTorque
Definition: btMultiBody.h:574
bool m_isMultiDof
Definition: btMultiBody.h:621
int getNumPosVars() const
Definition: btMultiBody.h:147
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:602
int m_companionId
Definition: btMultiBody.h:614
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:149
void setHasSelfCollision(bool hasSelfCollision)
Definition: btMultiBody.h:513
void setMaxCoordinateVelocity(btScalar maxVel)
Definition: btMultiBody.h:500
int size() const
return the number of elements in the array
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
void setUseGyroTerm(bool useGyro)
Definition: btMultiBody.h:492
btQuaternion m_baseQuat
Definition: btMultiBody.h:568
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:604
const btVector3 & getLinkTorque(int i) const
void addLinkTorque(int i, const btVector3 &t)
void setCompanionId(int id)
Definition: btMultiBody.h:460
void setBaseOmega(const btVector3 &omega)
Definition: btMultiBody.h:197
bool m_hasSelfCollision
Definition: btMultiBody.h:620
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
bool hasFixedBase() const
Definition: btMultiBody.h:451
btMultibodyLink & getLink(int index)
Definition: btMultiBody.h:114
#define output
void addJointTorque(int i, btScalar Q)
btScalar getLinearDamping() const
Definition: btMultiBody.h:471
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, float result[6]) const
const btVector3 & getRVector(int i) const
bool hasSelfCollision() const
Definition: btMultiBody.h:517
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:446
void setJointVel(int i, btScalar qdot)
void operator=(const btMultiBody &)
btScalar m_sleepTimer
Definition: btMultiBody.h:612
void setJointPos(int i, btScalar q)
void clearVelocities()
btVector3 m_basePos
Definition: btMultiBody.h:567
void setAngularDamping(btScalar damp)
Definition: btMultiBody.h:483
void checkMotionAndSleepIfRequired(btScalar timestep)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btVector3 m_baseInertia
Definition: btMultiBody.h:571
void setLinearDamping(btScalar damp)
Definition: btMultiBody.h:475
btScalar getLinkMass(int i) const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:268
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:596
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void updateLinksDofOffsets()
Definition: btMultiBody.h:550
void setMaxAppliedImpulse(btScalar maxImp)
Definition: btMultiBody.h:509
btVector3 getBaseOmega() const
Definition: btMultiBody.h:175
int getCompanionId() const
Definition: btMultiBody.h:456
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:413
bool m_useGlobalVelocities
Definition: btMultiBody.h:624
btScalar m_linearDamping
Definition: btMultiBody.h:615
btScalar getAngularDamping() const
Definition: btMultiBody.h:479
void resize(int newsize, const T &fillData=T())
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
void goToSleep()
void applyDeltaVee(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:337
void setBaseInertia(const btVector3 &inertia)
Definition: btMultiBody.h:159
bool getCanSleep() const
Definition: btMultiBody.h:441
void setPosUpdated(bool updated)
Definition: btMultiBody.h:534
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
const btQuaternion & getParentToLocalRot(int i) const
bool isUsingRK4Integration() const
Definition: btMultiBody.h:526
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:597
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:124
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
bool isMultiDof()
Definition: btMultiBody.h:522
const btVector3 & getLinkInertia(int i) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar getJointTorque(int i) const
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:171
void setBaseMass(btScalar mass)
Definition: btMultiBody.h:158
void stepPositions(btScalar dt)
int getParent(int link_num) const
bool __posUpdated
Definition: btMultiBody.h:622
const btVector3 & getBasePos() const
Definition: btMultiBody.h:166
btVector3 localPosToWorld(int i, const btVector3 &vec) const
const btVector3 & getBaseForce() const
Definition: btMultiBody.h:279
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:576
btScalar * getJointTorqueMultiDof(int i)
void setBasePos(const btVector3 &pos)
Definition: btMultiBody.h:177
void useGlobalVelocities(bool use)
Definition: btMultiBody.h:527
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:603
int getNumDofs() const
Definition: btMultiBody.h:146
btScalar * getJointPosMultiDof(int i)
void useRK4Integration(bool use)
Definition: btMultiBody.h:525
void setBaseWorldTransform(const btTransform &tr)
Definition: btMultiBody.h:182
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:224
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:616
bool getUseGyroTerm() const
Definition: btMultiBody.h:488
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar getMaxCoordinateVelocity() const
Definition: btMultiBody.h:496
btScalar getJointVel(int i) const
bool m_canSleep
Definition: btMultiBody.h:611
btScalar getKineticEnergy() const