Bullet Collision Detection & Physics Library
btMultiBodyLink.h
Go to the documentation of this file.
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 
20 #include "LinearMath/btVector3.h"
22 
24 {
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 
40 
41 #endif
42 //}
43 
44 //
45 // Link struct
46 //
47 
49 {
50 
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.
66 
67 #ifdef TEST_SPATIAL_ALGEBRA_LAYER
69 #endif
70 
72  {
73  eRevolute = 0,
76 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
77  ePlanar = 3,
78 #endif
79  eFixed = 4,
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];
101  void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; }
102  void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; }
103  void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); }
104  void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); }
105  const btVector3 & getAxisTop(int dof) const { return m_axesTop[dof]; }
106  const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; }
107 #else
109  void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
110  void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
111  void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
112  void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
113  const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
114  const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
115 #endif
116 
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 
127 
129  int m_flags;
130 
131 
132  int m_dofCount, m_posVarCount; //redundant but handy
134 
135  // ctor: set some sensible defaults
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
162  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 
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
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
#define btAssert(x)
Definition: btScalar.h:113
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278