dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBody.h

   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"
  29 #include "LinearMath/btQuaternion.h"
  30 #include "LinearMath/btMatrix3x3.h"
  31 #include "LinearMath/btAlignedObjectArray.h"
  32
  33 ///serialization data, don't change them if you are not familiar with the details of the serialization mechanisms
  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"
  47 class btMultiBodyLinkCollider;
  48
  49 ATTRIBUTE_ALIGNED16(class)
  50 btMultiBody
  51 {
  52 public:
  53         BT_DECLARE_ALIGNED_ALLOCATOR();
  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         }
 128         const btMultiBodyLinkCollider *getBaseCollider() const
 129         {
 130                 return m_baseCollider;
 131         }
 132         btMultiBodyLinkCollider *getBaseCollider()
 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
 146         btMultiBodyLinkCollider *getLinkCollider(int index)
 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
 193         const btQuaternion &getWorldToBaseRot() const
 194         {
 195                 return m_baseQuat;
 196         }
 197
 198     const btVector3 &getInterpolateBasePos() const
 199     {
 200         return m_basePos_interpolate;
 201     }  // in world frame
 202     const btQuaternion &getInterpolateWorldToBaseRot() const
 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
 217         void setInterpolateBasePos(const btVector3 &pos)
 218         {
 219                 m_basePos_interpolate = pos;
 220         }
 221
 222         void setBaseWorldTransform(const btTransform &tr)
 223         {
 224                 setBasePos(tr.getOrigin());
 225                 setWorldToBaseRot(tr.getRotation().inverse());
 226         }
 227
 228         btTransform getBaseWorldTransform() const
 229         {
 230                 btTransform tr;
 231                 tr.setOrigin(getBasePos());
 232                 tr.setRotation(getWorldToBaseRot().inverse());
 233                 return tr;
 234         }
 235
 236         void setInterpolateBaseWorldTransform(const btTransform &tr)
 237         {
 238                 setInterpolateBasePos(tr.getOrigin());
 239                 setInterpolateWorldToBaseRot(tr.getRotation().inverse());
 240         }
 241
 242         btTransform getInterpolateBaseWorldTransform() const
 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
 257         void setWorldToBaseRot(const btQuaternion &rot)
 258         {
 259                 m_baseQuat = rot;  //m_baseQuat asumed to ba alias!?
 260                 if(!isBaseKinematic())
 261                         m_baseQuat_interpolate = rot;
 262         }
 263
 264         void setInterpolateWorldToBaseRot(const btQuaternion &rot)
 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         //
 302         const btScalar *getVelocityVector() const
 303         {
 304                 return &m_realBuf[0];
 305         }
 306
 307     const btScalar *getDeltaVelocityVector() const
 308     {
 309         return &m_deltaV[0];
 310     }
 311
 312     const btScalar *getSplitVelocityVector() const
 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
 363         void addBaseConstraintForce(const btVector3 &f)
 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,
 401                                                                                                                           btAlignedObjectArray<btVector3> & scratch_v,
 402                                                                                                                           btAlignedObjectArray<btMatrix3x3> & scratch_m,
 403                                                                                                                           bool isConstraintPass,
 404                                                               bool jointFeedbackInWorldSpace,
 405                                                               bool jointFeedbackInJointFrame
 406                                                               );
 407
 408         ///stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instead
 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,
 425                                                                                 btAlignedObjectArray<btScalar> &scratch_r,
 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     }
 446     void substractSplitV()
 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     }
 455         void processDeltaVeeMultiDof2()
 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
 504         void fillContactJacobianMultiDof(int link,
 505                                                                          const btVector3 &contact_point,
 506                                                                          const btVector3 &normal,
 507                                                                          btScalar *jac,
 508                                                                          btAlignedObjectArray<btScalar> &scratch_r,
 509                                                                          btAlignedObjectArray<btVector3> &scratch_v,
 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,
 519                                                                                 btAlignedObjectArray<btScalar> &scratch_r,
 520                                                                                 btAlignedObjectArray<btVector3> &scratch_v,
 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
 589         btScalar getLinearDamping() const
 590         {
 591                 return m_linearDamping;
 592         }
 593         void setLinearDamping(btScalar damp)
 594         {
 595                 m_linearDamping = damp;
 596         }
 597         btScalar getAngularDamping() const
 598         {
 599                 return m_angularDamping;
 600         }
 601         void setAngularDamping(btScalar damp)
 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         }
 614         btScalar getMaxCoordinateVelocity() const
 615         {
 616                 return m_maxCoordinateVelocity;
 617         }
 618         void setMaxCoordinateVelocity(btScalar maxVel)
 619         {
 620                 m_maxCoordinateVelocity = maxVel;
 621         }
 622
 623         btScalar getMaxAppliedImpulse() const
 624         {
 625                 return m_maxAppliedImpulse;
 626         }
 627         void setMaxAppliedImpulse(btScalar maxImp)
 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
 657         bool internalNeedsJointFeedback() const
 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
 670         ///fills the dataBuffer and returns the struct name (and 0 on failure)
 671         virtual const char *serialize(void *dataBuffer, class btSerializer *serializer) const;
 672
 673         const char *getBaseName() const
 674         {
 675                 return m_baseName;
 676         }
 677         ///memory of setBaseName needs to be manager by user
 678         void setBaseName(const char *name)
 679         {
 680                 m_baseName = name;
 681         }
 682
 683         ///users can point to their objects, userPointer is not used by Bullet
 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         }
 698         ///users can point to their objects, userPointer is not used by Bullet
 699         void setUserPointer(void *userPointer)
 700         {
 701                 m_userObjectPointer = userPointer;
 702         }
 703
 704         ///users can point to their objects, userPointer is not used by Bullet
 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
 750         void updateLinksDofOffsets()
 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:
 765         btMultiBodyLinkCollider *m_baseCollider;  //can be NULL
 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
 771     btQuaternion m_baseQuat_interpolate;
 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         //
 799     btAlignedObjectArray<btScalar> m_splitV;
 800         btAlignedObjectArray<btScalar> m_deltaV;
 801         btAlignedObjectArray<btScalar> m_realBuf;
 802         btAlignedObjectArray<btVector3> m_vectorBuf;
 803         btAlignedObjectArray<btMatrix3x3> m_matrixBuf;
 804
 805         btMatrix3x3 m_cachedInertiaTopLeft;
 806         btMatrix3x3 m_cachedInertiaTopRight;
 807         btMatrix3x3 m_cachedInertiaLowerLeft;
 808         btMatrix3x3 m_cachedInertiaLowerRight;
 809         bool m_cachedInertiaValid;
 810
 811         bool m_fixedBase;
 812
 813         // Sleep parameters.
 814         bool m_awake;
 815         bool m_canSleep;
 816         bool m_canWakeup;
 817         btScalar m_sleepTimer;
 818         btScalar m_sleepEpsilon;
 819         btScalar m_sleepTimeout;
 820
 821         void *m_userObjectPointer;
 822         int m_userIndex2;
 823         int m_userIndex;
 824
 825         int m_companionId;
 826         btScalar m_linearDamping;
 827         btScalar m_angularDamping;
 828         bool m_useGyroTerm;
 829         btScalar m_maxAppliedImpulse;
 830         btScalar m_maxCoordinateVelocity;
 831         bool m_hasSelfCollision;
 832
 833         bool __posUpdated;
 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
 840         ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
 841         bool m_internalNeedsJointFeedback;
 842
 843   //If enabled, calculate the velocity based on kinematic transform changes. Currently only implemented for the base.
 844         bool m_kinematic_calculate_velocity;
 845 };
 846
 847 struct btMultiBodyLinkDoubleData
 848 {
 849         btQuaternionDoubleData m_zeroRotParentToThis;
 850         btVector3DoubleData m_parentComToThisPivotOffset;
 851         btVector3DoubleData m_thisPivotToThisComOffset;
 852         btVector3DoubleData m_jointAxisTop[6];
 853         btVector3DoubleData m_jointAxisBottom[6];
 854
 855         btVector3DoubleData m_linkInertia;  // inertia of the base (in local frame; diagonal)
 856         btVector3DoubleData m_absFrameTotVelocityTop;
 857         btVector3DoubleData m_absFrameTotVelocityBottom;
 858         btVector3DoubleData m_absFrameLocVelocityTop;
 859         btVector3DoubleData m_absFrameLocVelocityBottom;
 860
 861         double m_linkMass;
 862         int m_parentIndex;
 863         int m_jointType;
 864
 865         int m_dofCount;
 866         int m_posVarCount;
 867         double m_jointPos[7];
 868         double m_jointVel[6];
 869         double m_jointTorque[6];
 870
 871         double m_jointDamping;
 872         double m_jointFriction;
 873         double m_jointLowerLimit;
 874         double m_jointUpperLimit;
 875         double m_jointMaxForce;
 876         double m_jointMaxVelocity;
 877
 878         char *m_linkName;
 879         char *m_jointName;
 880         btCollisionObjectDoubleData *m_linkCollider;
 881         char *m_paddingPtr;
 882 };
 883
 884 struct btMultiBodyLinkFloatData
 885 {
 886         btQuaternionFloatData m_zeroRotParentToThis;
 887         btVector3FloatData m_parentComToThisPivotOffset;
 888         btVector3FloatData m_thisPivotToThisComOffset;
 889         btVector3FloatData m_jointAxisTop[6];
 890         btVector3FloatData m_jointAxisBottom[6];
 891         btVector3FloatData m_linkInertia;  // inertia of the base (in local frame; diagonal)
 892         btVector3FloatData m_absFrameTotVelocityTop;
 893         btVector3FloatData m_absFrameTotVelocityBottom;
 894         btVector3FloatData m_absFrameLocVelocityTop;
 895         btVector3FloatData m_absFrameLocVelocityBottom;
 896
 897         int m_dofCount;
 898         float m_linkMass;
 899         int m_parentIndex;
 900         int m_jointType;
 901
 902         float m_jointPos[7];
 903         float m_jointVel[6];
 904         float m_jointTorque[6];
 905         int m_posVarCount;
 906         float m_jointDamping;
 907         float m_jointFriction;
 908         float m_jointLowerLimit;
 909         float m_jointUpperLimit;
 910         float m_jointMaxForce;
 911         float m_jointMaxVelocity;
 912
 913         char *m_linkName;
 914         char *m_jointName;
 915         btCollisionObjectFloatData *m_linkCollider;
 916         char *m_paddingPtr;
 917 };
 918
 919 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 920 struct btMultiBodyDoubleData
 921 {
 922         btVector3DoubleData m_baseWorldPosition;
 923         btQuaternionDoubleData m_baseWorldOrientation;
 924         btVector3DoubleData m_baseLinearVelocity;
 925         btVector3DoubleData m_baseAngularVelocity;
 926         btVector3DoubleData m_baseInertia;  // inertia of the base (in local frame; diagonal)
 927         double m_baseMass;
 928         int m_numLinks;
 929         char m_padding[4];
 930
 931         char *m_baseName;
 932         btMultiBodyLinkDoubleData *m_links;
 933         btCollisionObjectDoubleData *m_baseCollider;
 934 };
 935
 936 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
 937 struct btMultiBodyFloatData
 938 {
 939         btVector3FloatData m_baseWorldPosition;
 940         btQuaternionFloatData m_baseWorldOrientation;
 941         btVector3FloatData m_baseLinearVelocity;
 942         btVector3FloatData m_baseAngularVelocity;
 943
 944         btVector3FloatData m_baseInertia;  // inertia of the base (in local frame; diagonal)
 945         float m_baseMass;
 946         int m_numLinks;
 947
 948         char *m_baseName;
 949         btMultiBodyLinkFloatData *m_links;
 950         btCollisionObjectFloatData *m_baseCollider;
 951 };
 952
 953 #endif