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