Blender  V3.3
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 
119  btMultibodyLink &getLink(int index)
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  m_basePos_interpolate = pos;
214  }
215 
217  {
218  setBasePos(tr.getOrigin());
219  setWorldToBaseRot(tr.getRotation().inverse());
220  }
221 
223  {
224  btTransform tr;
225  tr.setOrigin(getBasePos());
226  tr.setRotation(getWorldToBaseRot().inverse());
227  return tr;
228  }
229 
230  void setBaseVel(const btVector3 &vel)
231  {
232  m_realBuf[3] = vel[0];
233  m_realBuf[4] = vel[1];
234  m_realBuf[5] = vel[2];
235  }
237  {
238  m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
239  m_baseQuat_interpolate = rot;
240  }
241  void setBaseOmega(const btVector3 &omega)
242  {
243  m_realBuf[0] = omega[0];
244  m_realBuf[1] = omega[1];
245  m_realBuf[2] = omega[2];
246  }
247 
248  //
249  // get/set pos/vel for child m_links (i = 0 to num_links-1)
250  //
251 
252  btScalar getJointPos(int i) const;
253  btScalar getJointVel(int i) const;
254 
257 
258  const btScalar *getJointVelMultiDof(int i) const;
259  const btScalar *getJointPosMultiDof(int i) const;
260 
261  void setJointPos(int i, btScalar q);
262  void setJointVel(int i, btScalar qdot);
263  void setJointPosMultiDof(int i, const double *q);
264  void setJointVelMultiDof(int i, const double *qdot);
265  void setJointPosMultiDof(int i, const float *q);
266  void setJointVelMultiDof(int i, const float *qdot);
267 
268  //
269  // direct access to velocities as a vector of 6 + num_links elements.
270  // (omega first, then v, then joint velocities.)
271  //
273  {
274  return &m_realBuf[0];
275  }
276 
278  {
279  return &m_deltaV[0];
280  }
281 
283  {
284  return &m_splitV[0];
285  }
286  /* btScalar * getVelocityVector()
287  {
288  return &real_buf[0];
289  }
290  */
291 
292  //
293  // get the frames of reference (positions and orientations) of the child m_links
294  // (i = 0 to num_links-1)
295  //
296 
297  const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
298  const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
299  const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
300  const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
301 
302  //
303  // transform vectors in local frame of link i to world frame (or vice versa)
304  //
305  btVector3 localPosToWorld(int i, const btVector3 &local_pos) const;
306  btVector3 localDirToWorld(int i, const btVector3 &local_dir) const;
307  btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const;
308  btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const;
309 
310  //
311  // transform a frame in local coordinate to a frame in world coordinate
312  //
313  btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
314 
315 
316  //
317  // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
318  //
319 
320  void clearForcesAndTorques();
321  void clearConstraintForces();
322 
323  void clearVelocities();
324 
325  void addBaseForce(const btVector3 &f)
326  {
327  m_baseForce += f;
328  }
329  void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
330  void addLinkForce(int i, const btVector3 &f);
331  void addLinkTorque(int i, const btVector3 &t);
332 
334  {
335  m_baseConstraintForce += f;
336  }
337  void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; }
338  void addLinkConstraintForce(int i, const btVector3 &f);
339  void addLinkConstraintTorque(int i, const btVector3 &t);
340 
341  void addJointTorque(int i, btScalar Q);
342  void addJointTorqueMultiDof(int i, int dof, btScalar Q);
343  void addJointTorqueMultiDof(int i, const btScalar *Q);
344 
345  const btVector3 &getBaseForce() const { return m_baseForce; }
346  const btVector3 &getBaseTorque() const { return m_baseTorque; }
347  const btVector3 &getLinkForce(int i) const;
348  const btVector3 &getLinkTorque(int i) const;
349  btScalar getJointTorque(int i) const;
351 
352  //
353  // dynamics routines.
354  //
355 
356  // timestep the velocities (given the external forces/torques set using addBaseForce etc).
357  // also sets up caches for calcAccelerationDeltas.
358  //
359  // Note: the caller must provide three vectors which are used as
360  // temporary scratch space. The idea here is to reduce dynamic
361  // memory allocation: the same scratch vectors can be re-used
362  // again and again for different Multibodies, instead of each
363  // btMultiBody allocating (and then deallocating) their own
364  // individual scratch buffers. This gives a considerable speed
365  // improvement, at least on Windows (where dynamic memory
366  // allocation appears to be fairly slow).
367  //
368 
370  btAlignedObjectArray<btScalar> & scratch_r,
373  bool isConstraintPass,
374  bool jointFeedbackInWorldSpace,
375  bool jointFeedbackInJointFrame
376  );
377 
379  //void stepVelocitiesMultiDof(btScalar dt,
380  // btAlignedObjectArray<btScalar> & scratch_r,
381  // btAlignedObjectArray<btVector3> & scratch_v,
382  // btAlignedObjectArray<btMatrix3x3> & scratch_m,
383  // bool isConstraintPass = false)
384  //{
385  // computeAccelerationsArticulatedBodyAlgorithmMultiDof(dt, scratch_r, scratch_v, scratch_m, isConstraintPass, false, false);
386  //}
387 
388  // calcAccelerationDeltasMultiDof
389  // input: force vector (in same format as jacobian, i.e.:
390  // 3 torque values, 3 force values, num_links joint torque values)
391  // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
392  // (existing contents of output array are replaced)
393  // calcAccelerationDeltasMultiDof must have been called first.
396  btAlignedObjectArray<btVector3> &scratch_v) const;
397 
398  void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
399  {
400  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
401  {
402  m_deltaV[dof] += delta_vee[dof] * multiplier;
403  }
404  }
405  void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
406  {
407  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
408  {
409  m_splitV[dof] += delta_vee[dof] * multiplier;
410  }
411  }
412  void addSplitV()
413  {
414  applyDeltaVeeMultiDof(&m_splitV[0], 1);
415  }
417  {
418  applyDeltaVeeMultiDof(&m_splitV[0], -1);
419 
420  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
421  {
422  m_splitV[dof] = 0.f;
423  }
424  }
426  {
427  applyDeltaVeeMultiDof(&m_deltaV[0], 1);
428 
429  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
430  {
431  m_deltaV[dof] = 0.f;
432  }
433  }
434 
435  void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
436  {
437  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
438  // printf("%.4f ", delta_vee[dof]*multiplier);
439  //printf("\n");
440 
441  //btScalar sum = 0;
442  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
443  //{
444  // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
445  //}
446  //btScalar l = btSqrt(sum);
447 
448  //if (l>m_maxAppliedImpulse)
449  //{
450  // multiplier *= m_maxAppliedImpulse/l;
451  //}
452 
453  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
454  {
455  m_realBuf[dof] += delta_vee[dof] * multiplier;
456  btClamp(m_realBuf[dof], -m_maxCoordinateVelocity, m_maxCoordinateVelocity);
457  }
458  }
459 
460  // timestep the positions (given current velocities).
461  void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
462 
463  // predict the positions
465 
466  //
467  // contacts
468  //
469 
470  // This routine fills out a contact constraint jacobian for this body.
471  // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
472  // 'normal' & 'contact_point' are both given in world coordinates.
473 
475  const btVector3 &contact_point,
476  const btVector3 &normal,
477  btScalar *jac,
480  btAlignedObjectArray<btMatrix3x3> &scratch_m) const { fillConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
481 
482  //a more general version of fillContactJacobianMultiDof which does not assume..
483  //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
484  void fillConstraintJacobianMultiDof(int link,
485  const btVector3 &contact_point,
486  const btVector3 &normal_ang,
487  const btVector3 &normal_lin,
488  btScalar *jac,
491  btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
492 
493  //
494  // sleeping
495  //
496  void setCanSleep(bool canSleep)
497  {
498  if (m_canWakeup)
499  {
500  m_canSleep = canSleep;
501  }
502  }
503 
504  bool getCanSleep() const
505  {
506  return m_canSleep;
507  }
508 
509  bool getCanWakeup() const
510  {
511  return m_canWakeup;
512  }
513 
514  void setCanWakeup(bool canWakeup)
515  {
516  m_canWakeup = canWakeup;
517  }
518  bool isAwake() const { return m_awake; }
519  void wakeUp();
520  void goToSleep();
522 
523  bool hasFixedBase() const
524  {
525  return m_fixedBase;
526  }
527 
528  void setFixedBase(bool fixedBase)
529  {
530  m_fixedBase = fixedBase;
531  }
532 
533  int getCompanionId() const
534  {
535  return m_companionId;
536  }
537  void setCompanionId(int id)
538  {
539  //printf("for %p setCompanionId(%d)\n",this, id);
540  m_companionId = id;
541  }
542 
543  void setNumLinks(int numLinks) //careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
544  {
545  m_links.resize(numLinks);
546  }
547 
549  {
550  return m_linearDamping;
551  }
553  {
554  m_linearDamping = damp;
555  }
557  {
558  return m_angularDamping;
559  }
561  {
562  m_angularDamping = damp;
563  }
564 
565  bool getUseGyroTerm() const
566  {
567  return m_useGyroTerm;
568  }
569  void setUseGyroTerm(bool useGyro)
570  {
571  m_useGyroTerm = useGyro;
572  }
574  {
575  return m_maxCoordinateVelocity;
576  }
578  {
579  m_maxCoordinateVelocity = maxVel;
580  }
581 
583  {
584  return m_maxAppliedImpulse;
585  }
587  {
588  m_maxAppliedImpulse = maxImp;
589  }
591  {
592  m_hasSelfCollision = hasSelfCollision;
593  }
594  bool hasSelfCollision() const
595  {
596  return m_hasSelfCollision;
597  }
598 
600 
601  void useRK4Integration(bool use) { m_useRK4 = use; }
602  bool isUsingRK4Integration() const { return m_useRK4; }
603  void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
604  bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; }
605 
606  bool isPosUpdated() const
607  {
608  return __posUpdated;
609  }
610  void setPosUpdated(bool updated)
611  {
612  __posUpdated = updated;
613  }
614 
615  //internalNeedsJointFeedback is for internal use only
617  {
618  return m_internalNeedsJointFeedback;
619  }
621 
622  void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
623 
626 
627  virtual int calculateSerializeBufferSize() const;
628 
630  virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const;
631 
632  const char *getBaseName() const
633  {
634  return m_baseName;
635  }
637  void setBaseName(const char *name)
638  {
639  m_baseName = name;
640  }
641 
643  void *getUserPointer() const
644  {
645  return m_userObjectPointer;
646  }
647 
648  int getUserIndex() const
649  {
650  return m_userIndex;
651  }
652 
653  int getUserIndex2() const
654  {
655  return m_userIndex2;
656  }
658  void setUserPointer(void *userPointer)
659  {
660  m_userObjectPointer = userPointer;
661  }
662 
664  void setUserIndex(int index)
665  {
666  m_userIndex = index;
667  }
668 
669  void setUserIndex2(int index)
670  {
671  m_userIndex2 = index;
672  }
673 
674  static void spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
675  const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
676  const btVector3 &top_in, // top part of input vector
677  const btVector3 &bottom_in, // bottom part of input vector
678  btVector3 &top_out, // top part of output vector
679  btVector3 &bottom_out); // bottom part of output vector
680 
681 
682 
683 private:
684  btMultiBody(const btMultiBody &); // not implemented
685  void operator=(const btMultiBody &); // not implemented
686 
687  void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, btScalar result[6]) const;
688  void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
689 
690  void updateLinksDofOffsets()
691  {
692  int dofOffset = 0, cfgOffset = 0;
693  for (int bidx = 0; bidx < m_links.size(); ++bidx)
694  {
695  m_links[bidx].m_dofOffset = dofOffset;
696  m_links[bidx].m_cfgOffset = cfgOffset;
697  dofOffset += m_links[bidx].m_dofCount;
698  cfgOffset += m_links[bidx].m_posVarCount;
699  }
700  }
701 
702  void mulMatrix(btScalar * pA, btScalar * pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
703 
704 private:
705  btMultiBodyLinkCollider *m_baseCollider; //can be NULL
706  const char *m_baseName; //memory needs to be manager by user!
707 
708  btVector3 m_basePos; // position of COM of base (world frame)
709  btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame)
710  btQuaternion m_baseQuat; // rotates world points into base frame
711  btQuaternion m_baseQuat_interpolate;
712 
713  btScalar m_baseMass; // mass of the base
714  btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
715 
716  btVector3 m_baseForce; // external force applied to base. World frame.
717  btVector3 m_baseTorque; // external torque applied to base. World frame.
718 
719  btVector3 m_baseConstraintForce; // external force applied to base. World frame.
720  btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
721 
722  btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
723 
724  //
725  // realBuf:
726  // offset size array
727  // 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]
728  // 6+num_links num_links D
729  //
730  // vectorBuf:
731  // offset size array
732  // 0 num_links h_top
733  // num_links num_links h_bottom
734  //
735  // matrixBuf:
736  // offset size array
737  // 0 num_links+1 rot_from_parent
738  //
744 
745  btMatrix3x3 m_cachedInertiaTopLeft;
746  btMatrix3x3 m_cachedInertiaTopRight;
747  btMatrix3x3 m_cachedInertiaLowerLeft;
748  btMatrix3x3 m_cachedInertiaLowerRight;
749  bool m_cachedInertiaValid;
750 
751  bool m_fixedBase;
752 
753  // Sleep parameters.
754  bool m_awake;
755  bool m_canSleep;
756  bool m_canWakeup;
757  btScalar m_sleepTimer;
758 
759  void *m_userObjectPointer;
760  int m_userIndex2;
761  int m_userIndex;
762 
763  int m_companionId;
766  bool m_useGyroTerm;
768  btScalar m_maxCoordinateVelocity;
769  bool m_hasSelfCollision;
770 
771  bool __posUpdated;
772  int m_dofCount, m_posVarCnt;
773 
774  bool m_useRK4, m_useGlobalVelocities;
775  //for global velocities, see 8.3.2B Proposed resolution in Jakub Stepien PhD Thesis
776  //https://drive.google.com/file/d/0Bz3vEa19XOYGNWdZWGpMdUdqVmZ5ZVBOaEh4ZnpNaUxxZFNV/view?usp=sharing
777 
779  bool m_internalNeedsJointFeedback;
780 };
781 
782 struct btMultiBodyLinkDoubleData
783 {
784  btQuaternionDoubleData m_zeroRotParentToThis;
785  btVector3DoubleData m_parentComToThisPivotOffset;
786  btVector3DoubleData m_thisPivotToThisComOffset;
787  btVector3DoubleData m_jointAxisTop[6];
788  btVector3DoubleData m_jointAxisBottom[6];
789 
790  btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
791  btVector3DoubleData m_absFrameTotVelocityTop;
792  btVector3DoubleData m_absFrameTotVelocityBottom;
793  btVector3DoubleData m_absFrameLocVelocityTop;
794  btVector3DoubleData m_absFrameLocVelocityBottom;
795 
796  double m_linkMass;
797  int m_parentIndex;
798  int m_jointType;
799 
800  int m_dofCount;
801  int m_posVarCount;
802  double m_jointPos[7];
803  double m_jointVel[6];
804  double m_jointTorque[6];
805 
806  double m_jointDamping;
807  double m_jointFriction;
808  double m_jointLowerLimit;
809  double m_jointUpperLimit;
810  double m_jointMaxForce;
811  double m_jointMaxVelocity;
812 
813  char *m_linkName;
814  char *m_jointName;
815  btCollisionObjectDoubleData *m_linkCollider;
816  char *m_paddingPtr;
817 };
818 
819 struct btMultiBodyLinkFloatData
820 {
821  btQuaternionFloatData m_zeroRotParentToThis;
822  btVector3FloatData m_parentComToThisPivotOffset;
823  btVector3FloatData m_thisPivotToThisComOffset;
824  btVector3FloatData m_jointAxisTop[6];
825  btVector3FloatData m_jointAxisBottom[6];
826  btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
827  btVector3FloatData m_absFrameTotVelocityTop;
828  btVector3FloatData m_absFrameTotVelocityBottom;
829  btVector3FloatData m_absFrameLocVelocityTop;
830  btVector3FloatData m_absFrameLocVelocityBottom;
831 
832  int m_dofCount;
833  float m_linkMass;
834  int m_parentIndex;
835  int m_jointType;
836 
837  float m_jointPos[7];
838  float m_jointVel[6];
839  float m_jointTorque[6];
840  int m_posVarCount;
841  float m_jointDamping;
842  float m_jointFriction;
843  float m_jointLowerLimit;
844  float m_jointUpperLimit;
845  float m_jointMaxForce;
846  float m_jointMaxVelocity;
847 
848  char *m_linkName;
849  char *m_jointName;
850  btCollisionObjectFloatData *m_linkCollider;
851  char *m_paddingPtr;
852 };
853 
855 struct btMultiBodyDoubleData
856 {
857  btVector3DoubleData m_baseWorldPosition;
858  btQuaternionDoubleData m_baseWorldOrientation;
859  btVector3DoubleData m_baseLinearVelocity;
860  btVector3DoubleData m_baseAngularVelocity;
861  btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
862  double m_baseMass;
863  int m_numLinks;
864  char m_padding[4];
865 
866  char *m_baseName;
867  btMultiBodyLinkDoubleData *m_links;
868  btCollisionObjectDoubleData *m_baseCollider;
869 };
870 
872 struct btMultiBodyFloatData
873 {
874  btVector3FloatData m_baseWorldPosition;
875  btQuaternionFloatData m_baseWorldOrientation;
876  btVector3FloatData m_baseLinearVelocity;
877  btVector3FloatData m_baseAngularVelocity;
878 
879  btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
880  float m_baseMass;
881  int m_numLinks;
882 
883  char *m_baseName;
884  btMultiBodyLinkFloatData *m_links;
885  btCollisionObjectFloatData *m_baseCollider;
886 };
887 
888 #endif
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble y2 _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat y2 _GL_VOID_RET _GL_VOID GLint GLint GLint y2 _GL_VOID_RET _GL_VOID GLshort GLshort GLshort y2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLuint *buffer _GL_VOID_RET _GL_VOID GLdouble t _GL_VOID_RET _GL_VOID GLfloat t _GL_VOID_RET _GL_VOID GLint t _GL_VOID_RET _GL_VOID GLshort t _GL_VOID_RET _GL_VOID GLdouble t
void * m_userObjectPointer
users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPoin...
int m_userIndex
int m_companionId
int m_userIndex2
btGeneric6DofConstraint & operator=(btGeneric6DofConstraint &other)
btScalar m_angularDamping
btScalar m_linearDamping
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btTransform.h:182
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
SIMD_FORCE_INLINE void btClamp(T &a, const T &lb, const T &ub)
Definition: btMinMax.h:57
btScalar m_maxAppliedImpulse
btScalar getMaxAppliedImpulse() const
Definition: btMultiBody.h:582
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)
bool getCanWakeup() const
Definition: btMultiBody.h:509
void setUserPointer(void *userPointer)
users can point to their objects, userPointer is not used by Bullet
Definition: btMultiBody.h:658
void setJointVelMultiDof(int i, const double *qdot)
const btVector3 & getLinkTorque(int i) const
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
const btScalar * getDeltaVelocityVector() const
Definition: btMultiBody.h:277
bool isUsingGlobalVelocities() const
Definition: btMultiBody.h:604
btScalar * getJointPosMultiDof(int i)
const btVector3 & getInterpolateBasePos() const
Definition: btMultiBody.h:198
btScalar getBaseMass() const
Definition: btMultiBody.h:169
bool getUseGyroTerm() const
Definition: btMultiBody.h:565
void substractSplitV()
Definition: btMultiBody.h:416
int getCompanionId() const
Definition: btMultiBody.h:533
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:616
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const
void setBasePos(const btVector3 &pos)
Definition: btMultiBody.h:210
void applyDeltaSplitVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:405
void setMaxAppliedImpulse(btScalar maxImp)
Definition: btMultiBody.h:586
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:425
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void setBaseCollider(btMultiBodyLinkCollider *collider)
Definition: btMultiBody.h:124
void finalizeMultiDof()
void wakeUp()
bool isPosUpdated() const
Definition: btMultiBody.h:606
virtual int calculateSerializeBufferSize() const
const btQuaternion & getInterpolateParentToLocalRot(int i) const
btScalar getAngularDamping() const
Definition: btMultiBody.h:556
const btScalar * getSplitVelocityVector() const
Definition: btMultiBody.h:282
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:170
void useGlobalVelocities(bool use)
Definition: btMultiBody.h:603
btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const
int getNumDofs() const
Definition: btMultiBody.h:167
btVector3 getBaseOmega() const
Definition: btMultiBody.h:208
btScalar getJointPos(int i) const
const btVector3 & getLinkInertia(int i) const
void predictPositionsMultiDof(btScalar dt)
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:325
void clearVelocities()
bool hasFixedBase() const
Definition: btMultiBody.h:523
void setWorldToBaseRot(const btQuaternion &rot)
Definition: btMultiBody.h:236
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:435
void computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass, bool jointFeedbackInWorldSpace, bool jointFeedbackInJointFrame)
const btVector3 getBaseVel() const
Definition: btMultiBody.h:189
int getNumLinks() const
Definition: btMultiBody.h:166
void addLinkConstraintForce(int i, const btVector3 &f)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
bool isUsingRK4Integration() const
Definition: btMultiBody.h:602
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:333
const btVector3 & getRVector(int i) const
void setLinearDamping(btScalar damp)
Definition: btMultiBody.h:552
void setCanSleep(bool canSleep)
Definition: btMultiBody.h:496
bool getCanSleep() const
Definition: btMultiBody.h:504
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const
void setFixedBase(bool fixedBase)
Definition: btMultiBody.h:528
void addLinkTorque(int i, const btVector3 &t)
void checkMotionAndSleepIfRequired(btScalar timestep)
int getUserIndex() const
Definition: btMultiBody.h:648
btScalar * getJointTorqueMultiDof(int i)
void setAngularDamping(btScalar damp)
Definition: btMultiBody.h:560
void setUserIndex(int index)
users can point to their objects, userPointer is not used by Bullet
Definition: btMultiBody.h:664
const btVector3 & getInterpolateRVector(int i) const
const btQuaternion & getInterpolateWorldToBaseRot() const
Definition: btMultiBody.h:202
void setBaseInertia(const btVector3 &inertia)
Definition: btMultiBody.h:179
btScalar getLinkMass(int i) const
void setBaseVel(const btVector3 &vel)
Definition: btMultiBody.h:230
const btMultiBodyLinkCollider * getLinkCollider(int index) const
Definition: btMultiBody.h:137
void addLinkForce(int i, const btVector3 &f)
void forwardKinematics(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void addJointTorque(int i, btScalar Q)
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:337
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:128
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btMultiBody
Definition: btMultiBody.h:51
const btVector3 & getLinkForce(int i) const
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:222
int getParent(int link_num) const
void addBaseTorque(const btVector3 &t)
Definition: btMultiBody.h:329
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:398
void setJointPosMultiDof(int i, const double *q)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &world_to_local, btAlignedObjectArray< btVector3 > &local_origin)
void setJointVel(int i, btScalar qdot)
bool isAwake() const
Definition: btMultiBody.h:518
int getNumPosVars() const
Definition: btMultiBody.h:168
void * getUserPointer() const
users can point to their objects, userPointer is not used by Bullet
Definition: btMultiBody.h:643
btScalar getJointTorque(int i) const
const btVector3 & getBaseForce() const
Definition: btMultiBody.h:345
void setUserIndex2(int index)
Definition: btMultiBody.h:669
void setCompanionId(int id)
Definition: btMultiBody.h:537
void clearConstraintForces()
bool hasSelfCollision() const
Definition: btMultiBody.h:594
void addLinkConstraintTorque(int i, const btVector3 &t)
void setBaseMass(btScalar mass)
Definition: btMultiBody.h:178
void setupSpherical(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
virtual ~btMultiBody()
void setJointPos(int i, btScalar q)
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
void setNumLinks(int numLinks)
Definition: btMultiBody.h:543
static void spatialTransform(const btMatrix3x3 &rotation_matrix, const btVector3 &displacement, const btVector3 &top_in, const btVector3 &bottom_in, btVector3 &top_out, btVector3 &bottom_out)
Definition: btMultiBody.cpp:40
void setBaseName(const char *name)
memory of setBaseName needs to be manager by user
Definition: btMultiBody.h:637
void setCanWakeup(bool canWakeup)
Definition: btMultiBody.h:514
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
btVector3 localPosToWorld(int i, const btVector3 &local_pos) const
btScalar * getJointVelMultiDof(int i)
void setupRevolute(int i, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
void useRK4Integration(bool use)
Definition: btMultiBody.h:601
void setHasSelfCollision(bool hasSelfCollision)
Definition: btMultiBody.h:590
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
void setBaseWorldTransform(const btTransform &tr)
Definition: btMultiBody.h:216
void setupFixed(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool deprecatedDisableParentCollision=true)
void goToSleep()
void setPosUpdated(bool updated)
Definition: btMultiBody.h:610
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:193
const btQuaternion & getParentToLocalRot(int i) const
void addSplitV()
Definition: btMultiBody.h:412
const char * getBaseName() const
Definition: btMultiBody.h:632
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
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:474
btVector3 localDirToWorld(int i, const btVector3 &local_dir) const
void setBaseOmega(const btVector3 &omega)
Definition: btMultiBody.h:241
btScalar getJointVel(int i) const
const btVector3 & getBaseTorque() const
Definition: btMultiBody.h:346
btScalar getMaxCoordinateVelocity() const
Definition: btMultiBody.h:573
void setUseGyroTerm(bool useGyro)
Definition: btMultiBody.h:569
int getUserIndex2() const
Definition: btMultiBody.h:653
void setMaxCoordinateVelocity(btScalar maxVel)
Definition: btMultiBody.h:577
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
btScalar getLinearDamping() const
Definition: btMultiBody.h:548
void clearForcesAndTorques()
char m_padding[20]
#define BT_DECLARE_ALIGNED_ALLOCATOR()
Definition: btScalar.h:425
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:285
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
#define rot(x, k)
uint pos
IconTextureDrawCall normal
ccl_global KernelShaderEvalInput ccl_global float * output
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64