1 /*
2 Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9
10 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.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15 ///These spatial algebra classes are used for btMultiBody,
16 ///see BulletDynamics/Featherstone
17
18 #ifndef BT_SPATIAL_ALGEBRA_H
19 #define BT_SPATIAL_ALGEBRA_H
20
21
22 #include "btMatrix3x3.h"
23
24 struct btSpatialForceVector
25 {
26 btVector3 m_topVec, m_bottomVec;
27 //
btSpatialForceVectorbtSpatialForceVector28 btSpatialForceVector() { setZero(); }
btSpatialForceVectorbtSpatialForceVector29 btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
btSpatialForceVectorbtSpatialForceVector30 btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
31 {
32 setValue(ax, ay, az, lx, ly, lz);
33 }
34 //
setVectorbtSpatialForceVector35 void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
setValuebtSpatialForceVector36 void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
37 {
38 m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
39 }
40 //
addVectorbtSpatialForceVector41 void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
addValuebtSpatialForceVector42 void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
43 {
44 m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
45 m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;
46 }
47 //
getLinearbtSpatialForceVector48 const btVector3 & getLinear() const { return m_topVec; }
getAngularbtSpatialForceVector49 const btVector3 & getAngular() const { return m_bottomVec; }
50 //
setLinearbtSpatialForceVector51 void setLinear(const btVector3 &linear) { m_topVec = linear; }
setAngularbtSpatialForceVector52 void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
53 //
addAngularbtSpatialForceVector54 void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
addLinearbtSpatialForceVector55 void addLinear(const btVector3 &linear) { m_topVec += linear; }
56 //
setZerobtSpatialForceVector57 void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
58 //
59 btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
60 btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
61 btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
62 btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
63 btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
64 btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
65 //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
66 };
67
68 struct btSpatialMotionVector
69 {
70 btVector3 m_topVec, m_bottomVec;
71 //
btSpatialMotionVectorbtSpatialMotionVector72 btSpatialMotionVector() { setZero(); }
btSpatialMotionVectorbtSpatialMotionVector73 btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
74 //
setVectorbtSpatialMotionVector75 void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
setValuebtSpatialMotionVector76 void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
77 {
78 m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
79 }
80 //
addVectorbtSpatialMotionVector81 void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
addValuebtSpatialMotionVector82 void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
83 {
84 m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
85 m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;
86 }
87 //
getAngularbtSpatialMotionVector88 const btVector3 & getAngular() const { return m_topVec; }
getLinearbtSpatialMotionVector89 const btVector3 & getLinear() const { return m_bottomVec; }
90 //
setAngularbtSpatialMotionVector91 void setAngular(const btVector3 &angular) { m_topVec = angular; }
setLinearbtSpatialMotionVector92 void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
93 //
addAngularbtSpatialMotionVector94 void addAngular(const btVector3 &angular) { m_topVec += angular; }
addLinearbtSpatialMotionVector95 void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
96 //
setZerobtSpatialMotionVector97 void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
98 //
dotbtSpatialMotionVector99 btScalar dot(const btSpatialForceVector &b) const
100 {
101 return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
102 }
103 //
104 template<typename SpatialVectorType>
crossbtSpatialMotionVector105 void cross(const SpatialVectorType &b, SpatialVectorType &out) const
106 {
107 out.m_topVec = m_topVec.cross(b.m_topVec);
108 out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
109 }
110 template<typename SpatialVectorType>
crossbtSpatialMotionVector111 SpatialVectorType cross(const SpatialVectorType &b) const
112 {
113 SpatialVectorType out;
114 out.m_topVec = m_topVec.cross(b.m_topVec);
115 out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
116 return out;
117 }
118 //
119 btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
120 btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
121 btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
122 btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
123 btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
124 btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
125 btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
126 };
127
128 struct btSymmetricSpatialDyad
129 {
130 btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
131 //
btSymmetricSpatialDyadbtSymmetricSpatialDyad132 btSymmetricSpatialDyad() { setIdentity(); }
btSymmetricSpatialDyadbtSymmetricSpatialDyad133 btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
134 //
setMatrixbtSymmetricSpatialDyad135 void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
136 {
137 m_topLeftMat = topLeftMat;
138 m_topRightMat = topRightMat;
139 m_bottomLeftMat = bottomLeftMat;
140 }
141 //
addMatrixbtSymmetricSpatialDyad142 void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
143 {
144 m_topLeftMat += topLeftMat;
145 m_topRightMat += topRightMat;
146 m_bottomLeftMat += bottomLeftMat;
147 }
148 //
setIdentitybtSymmetricSpatialDyad149 void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); }
150 //
151 btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
152 {
153 m_topLeftMat -= mat.m_topLeftMat;
154 m_topRightMat -= mat.m_topRightMat;
155 m_bottomLeftMat -= mat.m_bottomLeftMat;
156 return *this;
157 }
158 //
159 btSpatialForceVector operator * (const btSpatialMotionVector &vec)
160 {
161 return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
162 }
163 };
164
165 struct btSpatialTransformationMatrix
166 {
167 btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
168 btVector3 m_trnVec;
169 //
170 enum eOutputOperation
171 {
172 None = 0,
173 Add = 1,
174 Subtract = 2
175 };
176 //
177 template<typename SpatialVectorType>
178 void transform( const SpatialVectorType &inVec,
179 SpatialVectorType &outVec,
180 eOutputOperation outOp = None)
181 {
182 if(outOp == None)
183 {
184 outVec.m_topVec = m_rotMat * inVec.m_topVec;
185 outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
186 }
187 else if(outOp == Add)
188 {
189 outVec.m_topVec += m_rotMat * inVec.m_topVec;
190 outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
191 }
192 else if(outOp == Subtract)
193 {
194 outVec.m_topVec -= m_rotMat * inVec.m_topVec;
195 outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
196 }
197
198 }
199
200 template<typename SpatialVectorType>
201 void transformRotationOnly( const SpatialVectorType &inVec,
202 SpatialVectorType &outVec,
203 eOutputOperation outOp = None)
204 {
205 if(outOp == None)
206 {
207 outVec.m_topVec = m_rotMat * inVec.m_topVec;
208 outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
209 }
210 else if(outOp == Add)
211 {
212 outVec.m_topVec += m_rotMat * inVec.m_topVec;
213 outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
214 }
215 else if(outOp == Subtract)
216 {
217 outVec.m_topVec -= m_rotMat * inVec.m_topVec;
218 outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
219 }
220
221 }
222
223 template<typename SpatialVectorType>
224 void transformInverse( const SpatialVectorType &inVec,
225 SpatialVectorType &outVec,
226 eOutputOperation outOp = None)
227 {
228 if(outOp == None)
229 {
230 outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
231 outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
232 }
233 else if(outOp == Add)
234 {
235 outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
236 outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
237 }
238 else if(outOp == Subtract)
239 {
240 outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
241 outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
242 }
243 }
244
245 template<typename SpatialVectorType>
246 void transformInverseRotationOnly( const SpatialVectorType &inVec,
247 SpatialVectorType &outVec,
248 eOutputOperation outOp = None)
249 {
250 if(outOp == None)
251 {
252 outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
253 outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
254 }
255 else if(outOp == Add)
256 {
257 outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
258 outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
259 }
260 else if(outOp == Subtract)
261 {
262 outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
263 outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
264 }
265
266 }
267
268 void transformInverse( const btSymmetricSpatialDyad &inMat,
269 btSymmetricSpatialDyad &outMat,
270 eOutputOperation outOp = None)
271 {
272 const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1],
273 m_trnVec[2], 0, -m_trnVec[0],
274 -m_trnVec[1], m_trnVec[0], 0);
275
276
277 if(outOp == None)
278 {
279 outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
280 outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
281 outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
282 }
283 else if(outOp == Add)
284 {
285 outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
286 outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
287 outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
288 }
289 else if(outOp == Subtract)
290 {
291 outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
292 outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
293 outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
294 }
295 }
296
297 template<typename SpatialVectorType>
298 SpatialVectorType operator * (const SpatialVectorType &vec)
299 {
300 SpatialVectorType out;
301 transform(vec, out);
302 return out;
303 }
304 };
305
306 template<typename SpatialVectorType>
symmetricSpatialOuterProduct(const SpatialVectorType & a,const SpatialVectorType & b,btSymmetricSpatialDyad & out)307 void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
308 {
309 //output op maybe?
310
311 out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
312 out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
313 out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
314 //maybe simple a*spatTranspose(a) would be nicer?
315 }
316
317 template<typename SpatialVectorType>
symmetricSpatialOuterProduct(const SpatialVectorType & a,const SpatialVectorType & b)318 btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
319 {
320 btSymmetricSpatialDyad out;
321
322 out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
323 out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
324 out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
325
326 return out;
327 //maybe simple a*spatTranspose(a) would be nicer?
328 }
329
330 #endif //BT_SPATIAL_ALGEBRA_H
331
332