Bullet Collision Detection & Physics Library: btMultiBodyLink.h Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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
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];
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
108 btSpatialMotionVector m_axes[6];
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
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
136 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
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
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
btMultibodyLink::m_zeroRotParentToThis
btQuaternion m_zeroRotParentToThis
Definition: btMultiBodyLink.h:58
btMultiBodyLinkFlags
btMultiBodyLinkFlags
Definition: btMultiBodyLink.h:23
btMultibodyLink::getAxisBottom
const btVector3 & getAxisBottom(int dof) const
Definition: btMultiBodyLink.h:114
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3.h
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
Definition: btMultiBodyLink.h:25
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:73
btMultibodyLink::m_cachedRVector
btVector3 m_cachedRVector
Definition: btMultiBodyLink.h:120
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:133
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:128
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:176
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMultibodyLink::setAxisTop
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
Definition: btMultiBodyLink.h:111
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition: btMultiBodyLink.h:132
btMultibodyLink::m_absFrameLocVelocity
btSpatialMotionVector m_absFrameLocVelocity
Definition: btMultiBodyLink.h:68
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:117
btMultibodyLink
Definition: btMultiBodyLink.h:48
btMultibodyLink::btMultibodyLink
btMultibodyLink()
Definition: btMultiBodyLink.h:136
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition: btMultiBodyLink.h:60
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
btMultibodyLink::eFixed
Definition: btMultiBodyLink.h:79
btSpatialAlgebra.h
btMultibodyLink::m_jointTorque
btScalar m_jointTorque[6]
Definition: btMultiBodyLink.h:126
btQuaternion.h
btMultibodyLink::m_dofCount
int m_dofCount
Definition: btMultiBodyLink.h:132
btMultibodyLink::m_cachedRotParentToThis
btQuaternion m_cachedRotParentToThis
Definition: btMultiBodyLink.h:119
btMultibodyLink::eFeatherstoneJointType
eFeatherstoneJointType
Definition: btMultiBodyLink.h:71
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:108
btMultibodyLink::m_mass
btScalar m_mass
Definition: btMultiBodyLink.h:53
btMultibodyLink::setAxisBottom
void setAxisBottom(int dof, const btVector3 &axis)
Definition: btMultiBodyLink.h:110
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:70
btSpatialMotionVector
Definition: btSpatialAlgebra.h:68
btMultibodyLink::m_eVector
btVector3 m_eVector
Definition: btMultiBodyLink.h:65
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition: btMultiBodyLink.h:125
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:117
btMultibodyLink::m_appliedTorque
btVector3 m_appliedTorque
Definition: btMultiBodyLink.h:123
btMultibodyLink::eSpherical
Definition: btMultiBodyLink.h:75
btMultibodyLink::eInvalid
Definition: btMultiBodyLink.h:80
btMultibodyLink::m_flags
int m_flags
Definition: btMultiBodyLink.h:129
btMultibodyLink::m_absFrameTotVelocity
btSpatialMotionVector m_absFrameTotVelocity
Definition: btMultiBodyLink.h:68
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:70
btMultibodyLink::BT_DECLARE_ALIGNED_ALLOCATOR
BT_DECLARE_ALIGNED_ALLOCATOR()
btMultibodyLink::updateCache
void updateCache()
Definition: btMultiBodyLink.h:162
btCollisionObject.h
btMultibodyLink::setAxisTop
void setAxisTop(int dof, const btVector3 &axis)
Definition: btMultiBodyLink.h:109
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
btMultibodyLink::m_inertiaLocal
btVector3 m_inertiaLocal
Definition: btMultiBodyLink.h:54
btMultibodyLink::m_appliedForce
btVector3 m_appliedForce
Definition: btMultiBodyLink.h:122
btMultibodyLink::getAxisTop
const btVector3 & getAxisTop(int dof) const
Definition: btMultiBodyLink.h:113
btMultibodyLink::setAxisBottom
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
Definition: btMultiBodyLink.h:112
btMultibodyLink::ePlanar
Definition: btMultiBodyLink.h:77
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:74
btMultibodyLink::m_parent
int m_parent
Definition: btMultiBodyLink.h:56
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278