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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2013 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #ifndef BT_MULTIBODY_LINK_H
  17 #define BT_MULTIBODY_LINK_H
  18
  19 #include "LinearMath/btQuaternion.h"
  20 #include "LinearMath/btVector3.h"
  21 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  22
  23 enum btMultiBodyLinkFlags
  24 {
  25         BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
  26         BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
  27 };
  28
  29 //both defines are now permanently enabled
  30 #define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
  31 #define TEST_SPATIAL_ALGEBRA_LAYER
  32
  33 //
  34 // Various spatial helper functions
  35 //
  36
  37 //namespace {
  38
  39 #include "LinearMath/btSpatialAlgebra.h"
  40
  41 //}
  42
  43 //
  44 // Link struct
  45 //
  46
  47 struct btMultibodyLink
  48 {
  49         BT_DECLARE_ALIGNED_ALLOCATOR();
  50
  51         btScalar m_mass;           // mass of link
  52         btVector3 m_inertiaLocal;  // inertia of link (local frame; diagonal)
  53
  54         int m_parent;  // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
  55
  56         btQuaternion m_zeroRotParentToThis;  // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
  57
  58         btVector3 m_dVector;  // vector from the inboard joint pos to this link's COM. (local frame.) constant.
  59                                                   //this is set to zero for planar joint (see also m_eVector comment)
  60
  61         // m_eVector is constant, but depends on the joint type:
  62         // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
  63         // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
  64         // todo: fix the planar so it is consistent with the other joints
  65
  66         btVector3 m_eVector;
  67
  68         btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
  69
  70         enum eFeatherstoneJointType
  71         {
  72                 eRevolute = 0,
  73                 ePrismatic = 1,
  74                 eSpherical = 2,
  75                 ePlanar = 3,
  76                 eFixed = 4,
  77                 eInvalid
  78         };
  79
  80         // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
  81         // for prismatic: m_axesTop[0] = zero;
  82         //                m_axesBottom[0] = unit vector along the joint axis.
  83         // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
  84         //               m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
  85         //
  86         // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
  87         //                                m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
  88         //
  89         // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
  90         //                         m_axesTop[1][2] = zero
  91         //                         m_axesBottom[0] = zero
  92         //                         m_axesBottom[1][2] = unit vectors along the translational axes on that plane
  93         btSpatialMotionVector m_axes[6];
  94         void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
  95         void setAxisBottom(int dof, const btVector3 &axis)
  96         {
  97                 m_axes[dof].m_bottomVec = axis;
  98         }
  99         void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
 100         {
 101                 m_axes[dof].m_topVec.setValue(x, y, z);
 102         }
 103         void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
 104         {
 105                 m_axes[dof].m_bottomVec.setValue(x, y, z);
 106         }
 107         const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
 108         const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
 109
 110         int m_dofOffset, m_cfgOffset;
 111
 112         btQuaternion m_cachedRotParentToThis;  // rotates vectors in parent frame to vectors in local frame
 113         btVector3 m_cachedRVector;             // vector from COM of parent to COM of this link, in local frame.
 114
 115     // predicted verstion
 116     btQuaternion m_cachedRotParentToThis_interpolate;  // rotates vectors in parent frame to vectors in local frame
 117     btVector3 m_cachedRVector_interpolate;             // vector from COM of parent to COM of this link, in local frame.
 118
 119         btVector3 m_appliedForce;   // In WORLD frame
 120         btVector3 m_appliedTorque;  // In WORLD frame
 121
 122         btVector3 m_appliedConstraintForce;   // In WORLD frame
 123         btVector3 m_appliedConstraintTorque;  // In WORLD frame
 124
 125         btScalar m_jointPos[7];
 126     btScalar m_jointPos_interpolate[7];
 127
 128         //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
 129         //It gets set to zero after each internal stepSimulation call
 130         btScalar m_jointTorque[6];
 131
 132         class btMultiBodyLinkCollider *m_collider;
 133         int m_flags;
 134
 135         int m_dofCount, m_posVarCount;  //redundant but handy
 136
 137         eFeatherstoneJointType m_jointType;
 138
 139         struct btMultiBodyJointFeedback *m_jointFeedback;
 140
 141         btTransform m_cachedWorldTransform;  //this cache is updated when calling btMultiBody::forwardKinematics
 142
 143         const char *m_linkName;   //m_linkName memory needs to be managed by the developer/user!
 144         const char *m_jointName;  //m_jointName memory needs to be managed by the developer/user!
 145         const void *m_userPtr;    //m_userPtr ptr needs to be managed by the developer/user!
 146
 147         btScalar m_jointDamping;      //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
 148         btScalar m_jointFriction;     //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
 149         btScalar m_jointLowerLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
 150         btScalar m_jointUpperLimit;   //todo: implement this internally. It is unused for now, it is set by a URDF loader.
 151         btScalar m_jointMaxForce;     //todo: implement this internally. It is unused for now, it is set by a URDF loader.
 152         btScalar m_jointMaxVelocity;  //todo: implement this internally. It is unused for now, it is set by a URDF loader.
 153
 154         // ctor: set some sensible defaults
 155         btMultibodyLink()
 156                 : m_mass(1),
 157                   m_parent(-1),
 158                   m_zeroRotParentToThis(0, 0, 0, 1),
 159                   m_cachedRotParentToThis(0, 0, 0, 1),
 160           m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
 161                   m_collider(0),
 162                   m_flags(0),
 163                   m_dofCount(0),
 164                   m_posVarCount(0),
 165                   m_jointType(btMultibodyLink::eInvalid),
 166                   m_jointFeedback(0),
 167                   m_linkName(0),
 168                   m_jointName(0),
 169                   m_userPtr(0),
 170                   m_jointDamping(0),
 171                   m_jointFriction(0),
 172                   m_jointLowerLimit(0),
 173                   m_jointUpperLimit(0),
 174                   m_jointMaxForce(0),
 175                   m_jointMaxVelocity(0)
 176         {
 177                 m_inertiaLocal.setValue(1, 1, 1);
 178                 setAxisTop(0, 0., 0., 0.);
 179                 setAxisBottom(0, 1., 0., 0.);
 180                 m_dVector.setValue(0, 0, 0);
 181                 m_eVector.setValue(0, 0, 0);
 182                 m_cachedRVector.setValue(0, 0, 0);
 183         m_cachedRVector_interpolate.setValue(0, 0, 0);
 184                 m_appliedForce.setValue(0, 0, 0);
 185                 m_appliedTorque.setValue(0, 0, 0);
 186                 m_appliedConstraintForce.setValue(0, 0, 0);
 187                 m_appliedConstraintTorque.setValue(0, 0, 0);
 188                 //
 189                 m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
 190                 m_jointPos[3] = 1.f;  //"quat.w"
 191                 m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
 192                 m_cachedWorldTransform.setIdentity();
 193         }
 194
 195         // routine to update m_cachedRotParentToThis and m_cachedRVector
 196         void updateCacheMultiDof(btScalar *pq = 0)
 197         {
 198         btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
 199         btQuaternion& cachedRot = m_cachedRotParentToThis;
 200         btVector3& cachedVector = m_cachedRVector;
 201                 switch (m_jointType)
 202                 {
 203                         case eRevolute:
 204                         {
 205                                 cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
 206                                 cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 207
 208                                 break;
 209                         }
 210                         case ePrismatic:
 211                         {
 212                                 // m_cachedRotParentToThis never changes, so no need to update
 213                                 cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
 214
 215                                 break;
 216                         }
 217                         case eSpherical:
 218                         {
 219                                 cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
 220                                 cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 221
 222                                 break;
 223                         }
 224                         case ePlanar:
 225                         {
 226                                 cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
 227                                 cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 228
 229                                 break;
 230                         }
 231                         case eFixed:
 232                         {
 233                                 cachedRot = m_zeroRotParentToThis;
 234                                 cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 235
 236                                 break;
 237                         }
 238                         default:
 239                         {
 240                                 //invalid type
 241                                 btAssert(0);
 242                         }
 243                 }
 244         m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
 245         m_cachedRVector_interpolate = m_cachedRVector;
 246         }
 247
 248     void updateInterpolationCacheMultiDof()
 249     {
 250         btScalar *pJointPos = &m_jointPos_interpolate[0];
 251
 252         btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
 253         btVector3& cachedVector = m_cachedRVector_interpolate;
 254         switch (m_jointType)
 255         {
 256             case eRevolute:
 257             {
 258                 cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
 259                 cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
 260
 261                 break;
 262             }
 263             case ePrismatic:
 264             {
 265                 // m_cachedRotParentToThis never changes, so no need to update
 266                 cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
 267
 268                 break;
 269             }
 270             case eSpherical:
 271             {
 272                 cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
 273                 cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 274
 275                 break;
 276             }
 277             case ePlanar:
 278             {
 279                 cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
 280                 cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
 281
 282                 break;
 283             }
 284             case eFixed:
 285             {
 286                 cachedRot = m_zeroRotParentToThis;
 287                 cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
 288
 289                 break;
 290             }
 291             default:
 292             {
 293                 //invalid type
 294                 btAssert(0);
 295             }
 296         }
 297     }
 298
 299
 300
 301 };
 302
 303 #endif  //BT_MULTIBODY_LINK_H