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 #include "btMatrix3x3.h"
22
23 struct btSpatialForceVector
24 {
25 btVector3 m_topVec, m_bottomVec;
26 //
btSpatialForceVectorbtSpatialForceVector27 btSpatialForceVector() { setZero(); }
btSpatialForceVectorbtSpatialForceVector28 btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
btSpatialForceVectorbtSpatialForceVector29 btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
30 {
31 setValue(ax, ay, az, lx, ly, lz);
32 }
33 //
setVectorbtSpatialForceVector34 void setVector(const btVector3 &angular, const btVector3 &linear)
35 {
36 m_topVec = linear;
37 m_bottomVec = angular;
38 }
setValuebtSpatialForceVector39 void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
40 {
41 m_bottomVec.setValue(ax, ay, az);
42 m_topVec.setValue(lx, ly, lz);
43 }
44 //
addVectorbtSpatialForceVector45 void addVector(const btVector3 &angular, const btVector3 &linear)
46 {
47 m_topVec += linear;
48 m_bottomVec += angular;
49 }
addValuebtSpatialForceVector50 void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
51 {
52 m_bottomVec[0] += ax;
53 m_bottomVec[1] += ay;
54 m_bottomVec[2] += az;
55 m_topVec[0] += lx;
56 m_topVec[1] += ly;
57 m_topVec[2] += lz;
58 }
59 //
getLinearbtSpatialForceVector60 const btVector3 &getLinear() const { return m_topVec; }
getAngularbtSpatialForceVector61 const btVector3 &getAngular() const { return m_bottomVec; }
62 //
setLinearbtSpatialForceVector63 void setLinear(const btVector3 &linear) { m_topVec = linear; }
setAngularbtSpatialForceVector64 void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
65 //
addAngularbtSpatialForceVector66 void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
addLinearbtSpatialForceVector67 void addLinear(const btVector3 &linear) { m_topVec += linear; }
68 //
setZerobtSpatialForceVector69 void setZero()
70 {
71 m_topVec.setZero();
72 m_bottomVec.setZero();
73 }
74 //
75 btSpatialForceVector &operator+=(const btSpatialForceVector &vec)
76 {
77 m_topVec += vec.m_topVec;
78 m_bottomVec += vec.m_bottomVec;
79 return *this;
80 }
81 btSpatialForceVector &operator-=(const btSpatialForceVector &vec)
82 {
83 m_topVec -= vec.m_topVec;
84 m_bottomVec -= vec.m_bottomVec;
85 return *this;
86 }
87 btSpatialForceVector operator-(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
88 btSpatialForceVector operator+(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
89 btSpatialForceVector operator-() const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
90 btSpatialForceVector operator*(const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
91 //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
92 };
93
94 struct btSpatialMotionVector
95 {
96 btVector3 m_topVec, m_bottomVec;
97 //
btSpatialMotionVectorbtSpatialMotionVector98 btSpatialMotionVector() { setZero(); }
btSpatialMotionVectorbtSpatialMotionVector99 btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
100 //
setVectorbtSpatialMotionVector101 void setVector(const btVector3 &angular, const btVector3 &linear)
102 {
103 m_topVec = angular;
104 m_bottomVec = linear;
105 }
setValuebtSpatialMotionVector106 void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
107 {
108 m_topVec.setValue(ax, ay, az);
109 m_bottomVec.setValue(lx, ly, lz);
110 }
111 //
addVectorbtSpatialMotionVector112 void addVector(const btVector3 &angular, const btVector3 &linear)
113 {
114 m_topVec += linear;
115 m_bottomVec += angular;
116 }
addValuebtSpatialMotionVector117 void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
118 {
119 m_topVec[0] += ax;
120 m_topVec[1] += ay;
121 m_topVec[2] += az;
122 m_bottomVec[0] += lx;
123 m_bottomVec[1] += ly;
124 m_bottomVec[2] += lz;
125 }
126 //
getAngularbtSpatialMotionVector127 const btVector3 &getAngular() const { return m_topVec; }
getLinearbtSpatialMotionVector128 const btVector3 &getLinear() const { return m_bottomVec; }
129 //
setAngularbtSpatialMotionVector130 void setAngular(const btVector3 &angular) { m_topVec = angular; }
setLinearbtSpatialMotionVector131 void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
132 //
addAngularbtSpatialMotionVector133 void addAngular(const btVector3 &angular) { m_topVec += angular; }
addLinearbtSpatialMotionVector134 void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
135 //
setZerobtSpatialMotionVector136 void setZero()
137 {
138 m_topVec.setZero();
139 m_bottomVec.setZero();
140 }
141 //
dotbtSpatialMotionVector142 btScalar dot(const btSpatialForceVector &b) const
143 {
144 return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
145 }
146 //
147 template <typename SpatialVectorType>
crossbtSpatialMotionVector148 void cross(const SpatialVectorType &b, SpatialVectorType &out) const
149 {
150 out.m_topVec = m_topVec.cross(b.m_topVec);
151 out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
152 }
153 template <typename SpatialVectorType>
crossbtSpatialMotionVector154 SpatialVectorType cross(const SpatialVectorType &b) const
155 {
156 SpatialVectorType out;
157 out.m_topVec = m_topVec.cross(b.m_topVec);
158 out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
159 return out;
160 }
161 //
162 btSpatialMotionVector &operator+=(const btSpatialMotionVector &vec)
163 {
164 m_topVec += vec.m_topVec;
165 m_bottomVec += vec.m_bottomVec;
166 return *this;
167 }
168 btSpatialMotionVector &operator-=(const btSpatialMotionVector &vec)
169 {
170 m_topVec -= vec.m_topVec;
171 m_bottomVec -= vec.m_bottomVec;
172 return *this;
173 }
174 btSpatialMotionVector &operator*=(const btScalar &s)
175 {
176 m_topVec *= s;
177 m_bottomVec *= s;
178 return *this;
179 }
180 btSpatialMotionVector operator-(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
181 btSpatialMotionVector operator+(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
182 btSpatialMotionVector operator-() const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
183 btSpatialMotionVector operator*(const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
184 };
185
186 struct btSymmetricSpatialDyad
187 {
188 btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
189 //
btSymmetricSpatialDyadbtSymmetricSpatialDyad190 btSymmetricSpatialDyad() { setIdentity(); }
btSymmetricSpatialDyadbtSymmetricSpatialDyad191 btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
192 //
setMatrixbtSymmetricSpatialDyad193 void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
194 {
195 m_topLeftMat = topLeftMat;
196 m_topRightMat = topRightMat;
197 m_bottomLeftMat = bottomLeftMat;
198 }
199 //
addMatrixbtSymmetricSpatialDyad200 void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
201 {
202 m_topLeftMat += topLeftMat;
203 m_topRightMat += topRightMat;
204 m_bottomLeftMat += bottomLeftMat;
205 }
206 //
setIdentitybtSymmetricSpatialDyad207 void setIdentity()
208 {
209 m_topLeftMat.setIdentity();
210 m_topRightMat.setIdentity();
211 m_bottomLeftMat.setIdentity();
212 }
213 //
214 btSymmetricSpatialDyad &operator-=(const btSymmetricSpatialDyad &mat)
215 {
216 m_topLeftMat -= mat.m_topLeftMat;
217 m_topRightMat -= mat.m_topRightMat;
218 m_bottomLeftMat -= mat.m_bottomLeftMat;
219 return *this;
220 }
221 //
222 btSpatialForceVector operator*(const btSpatialMotionVector &vec)
223 {
224 return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
225 }
226 };
227
228 struct btSpatialTransformationMatrix
229 {
230 btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
231 btVector3 m_trnVec;
232 //
233 enum eOutputOperation
234 {
235 None = 0,
236 Add = 1,
237 Subtract = 2
238 };
239 //
240 template <typename SpatialVectorType>
241 void transform(const SpatialVectorType &inVec,
242 SpatialVectorType &outVec,
243 eOutputOperation outOp = None)
244 {
245 if (outOp == None)
246 {
247 outVec.m_topVec = m_rotMat * inVec.m_topVec;
248 outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
249 }
250 else if (outOp == Add)
251 {
252 outVec.m_topVec += m_rotMat * inVec.m_topVec;
253 outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
254 }
255 else if (outOp == Subtract)
256 {
257 outVec.m_topVec -= m_rotMat * inVec.m_topVec;
258 outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
259 }
260 }
261
262 template <typename SpatialVectorType>
263 void transformRotationOnly(const SpatialVectorType &inVec,
264 SpatialVectorType &outVec,
265 eOutputOperation outOp = None)
266 {
267 if (outOp == None)
268 {
269 outVec.m_topVec = m_rotMat * inVec.m_topVec;
270 outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
271 }
272 else if (outOp == Add)
273 {
274 outVec.m_topVec += m_rotMat * inVec.m_topVec;
275 outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
276 }
277 else if (outOp == Subtract)
278 {
279 outVec.m_topVec -= m_rotMat * inVec.m_topVec;
280 outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
281 }
282 }
283
284 template <typename SpatialVectorType>
285 void transformInverse(const SpatialVectorType &inVec,
286 SpatialVectorType &outVec,
287 eOutputOperation outOp = None)
288 {
289 if (outOp == None)
290 {
291 outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
292 outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
293 }
294 else if (outOp == Add)
295 {
296 outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
297 outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
298 }
299 else if (outOp == Subtract)
300 {
301 outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
302 outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
303 }
304 }
305
306 template <typename SpatialVectorType>
307 void transformInverseRotationOnly(const SpatialVectorType &inVec,
308 SpatialVectorType &outVec,
309 eOutputOperation outOp = None)
310 {
311 if (outOp == None)
312 {
313 outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
314 outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
315 }
316 else if (outOp == Add)
317 {
318 outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
319 outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
320 }
321 else if (outOp == Subtract)
322 {
323 outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
324 outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
325 }
326 }
327
328 void transformInverse(const btSymmetricSpatialDyad &inMat,
329 btSymmetricSpatialDyad &outMat,
330 eOutputOperation outOp = None)
331 {
332 const btMatrix3x3 r_cross(0, -m_trnVec[2], m_trnVec[1],
333 m_trnVec[2], 0, -m_trnVec[0],
334 -m_trnVec[1], m_trnVec[0], 0);
335
336 if (outOp == None)
337 {
338 outMat.m_topLeftMat = m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat;
339 outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
340 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;
341 }
342 else if (outOp == Add)
343 {
344 outMat.m_topLeftMat += m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat;
345 outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
346 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;
347 }
348 else if (outOp == Subtract)
349 {
350 outMat.m_topLeftMat -= m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat;
351 outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
352 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;
353 }
354 }
355
356 template <typename SpatialVectorType>
357 SpatialVectorType operator*(const SpatialVectorType &vec)
358 {
359 SpatialVectorType out;
360 transform(vec, out);
361 return out;
362 }
363 };
364
365 template <typename SpatialVectorType>
symmetricSpatialOuterProduct(const SpatialVectorType & a,const SpatialVectorType & b,btSymmetricSpatialDyad & out)366 void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
367 {
368 //output op maybe?
369
370 out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
371 out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
372 out.m_topLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
373 //maybe simple a*spatTranspose(a) would be nicer?
374 }
375
376 template <typename SpatialVectorType>
symmetricSpatialOuterProduct(const SpatialVectorType & a,const SpatialVectorType & b)377 btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
378 {
379 btSymmetricSpatialDyad out;
380
381 out.m_topLeftMat = outerProduct(a.m_topVec, b.m_bottomVec);
382 out.m_topRightMat = outerProduct(a.m_topVec, b.m_topVec);
383 out.m_bottomLeftMat = outerProduct(a.m_bottomVec, b.m_bottomVec);
384
385 return out;
386 //maybe simple a*spatTranspose(a) would be nicer?
387 }
388
389 #endif //BT_SPATIAL_ALGEBRA_H
390