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 }; 27 28 //both defines are now permanently enabled 29 #define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS 30 #define TEST_SPATIAL_ALGEBRA_LAYER 31 32 // 33 // Various spatial helper functions 34 // 35 36 //namespace { 37 38 39 #include "LinearMath/btSpatialAlgebra.h" 40 41 //} 42 43 // 44 // Link struct 45 // 46 47 struct btMultibodyLink 48 { 49 50 BT_DECLARE_ALIGNED_ALLOCATOR(); 51 52 btScalar m_mass; // mass of link 53 btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) 54 55 int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link. 56 57 btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant. 58 59 btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant. 60 //this is set to zero for planar joint (see also m_eVector comment) 61 62 // m_eVector is constant, but depends on the joint type: 63 // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame. 64 // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.) 65 // todo: fix the planar so it is consistent with the other joints 66 67 btVector3 m_eVector; 68 69 btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; 70 71 enum eFeatherstoneJointType 72 { 73 eRevolute = 0, 74 ePrismatic = 1, 75 eSpherical = 2, 76 ePlanar = 3, 77 eFixed = 4, 78 eInvalid 79 }; 80 81 82 83 // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant. 84 // for prismatic: m_axesTop[0] = zero; 85 // m_axesBottom[0] = unit vector along the joint axis. 86 // for revolute: m_axesTop[0] = unit vector along the rotation axis (u); 87 // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint) 88 // 89 // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes) 90 // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint) 91 // 92 // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion 93 // m_axesTop[1][2] = zero 94 // m_axesBottom[0] = zero 95 // m_axesBottom[1][2] = unit vectors along the translational axes on that plane 96 btSpatialMotionVector m_axes[6]; setAxisTopbtMultibodyLink97 void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; } setAxisBottombtMultibodyLink98 void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; } setAxisTopbtMultibodyLink99 void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); } setAxisBottombtMultibodyLink100 void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); } getAxisTopbtMultibodyLink101 const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; } getAxisBottombtMultibodyLink102 const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; } 103 104 int m_dofOffset, m_cfgOffset; 105 106 btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame 107 btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame. 108 109 btVector3 m_appliedForce; // In WORLD frame 110 btVector3 m_appliedTorque; // In WORLD frame 111 112 btVector3 m_appliedConstraintForce; // In WORLD frame 113 btVector3 m_appliedConstraintTorque; // In WORLD frame 114 115 btScalar m_jointPos[7]; 116 117 //m_jointTorque is the joint torque applied by the user using 'addJointTorque'. 118 //It gets set to zero after each internal stepSimulation call 119 btScalar m_jointTorque[6]; 120 121 class btMultiBodyLinkCollider* m_collider; 122 int m_flags; 123 124 125 int m_dofCount, m_posVarCount; //redundant but handy 126 127 eFeatherstoneJointType m_jointType; 128 129 struct btMultiBodyJointFeedback* m_jointFeedback; 130 131 btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics 132 133 const char* m_linkName;//m_linkName memory needs to be managed by the developer/user! 134 const char* m_jointName;//m_jointName memory needs to be managed by the developer/user! 135 136 // ctor: set some sensible defaults btMultibodyLinkbtMultibodyLink137 btMultibodyLink() 138 : m_mass(1), 139 m_parent(-1), 140 m_zeroRotParentToThis(0, 0, 0, 1), 141 m_cachedRotParentToThis(0, 0, 0, 1), 142 m_collider(0), 143 m_flags(0), 144 m_dofCount(0), 145 m_posVarCount(0), 146 m_jointType(btMultibodyLink::eInvalid), 147 m_jointFeedback(0), 148 m_linkName(0), 149 m_jointName(0) 150 { 151 152 m_inertiaLocal.setValue(1, 1, 1); 153 setAxisTop(0, 0., 0., 0.); 154 setAxisBottom(0, 1., 0., 0.); 155 m_dVector.setValue(0, 0, 0); 156 m_eVector.setValue(0, 0, 0); 157 m_cachedRVector.setValue(0, 0, 0); 158 m_appliedForce.setValue( 0, 0, 0); 159 m_appliedTorque.setValue(0, 0, 0); 160 // 161 m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f; 162 m_jointPos[3] = 1.f; //"quat.w" 163 m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f; 164 m_cachedWorldTransform.setIdentity(); 165 } 166 167 // routine to update m_cachedRotParentToThis and m_cachedRVector updateCachebtMultibodyLink168 void updateCache() 169 { 170 //multidof 171 if (m_jointType == eRevolute) 172 { 173 m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis; 174 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); 175 } else 176 { 177 // m_cachedRotParentToThis never changes, so no need to update 178 m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0); 179 } 180 } 181 182 void updateCacheMultiDof(btScalar *pq = 0) 183 { 184 btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); 185 186 switch(m_jointType) 187 { 188 case eRevolute: 189 { 190 m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; 191 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); 192 193 break; 194 } 195 case ePrismatic: 196 { 197 // m_cachedRotParentToThis never changes, so no need to update 198 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0); 199 200 break; 201 } 202 case eSpherical: 203 { 204 m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; 205 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); 206 207 break; 208 } 209 case ePlanar: 210 { 211 m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; 212 m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector); 213 214 break; 215 } 216 case eFixed: 217 { 218 m_cachedRotParentToThis = m_zeroRotParentToThis; 219 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); 220 221 break; 222 } 223 default: 224 { 225 //invalid type 226 btAssert(0); 227 } 228 } 229 } 230 }; 231 232 233 #endif //BT_MULTIBODY_LINK_H 234