Bullet Collision Detection & Physics Library
btSpatialAlgebra.h
Go to the documentation of this file.
1/*
2Copyright (c) 2003-2015 Erwin Coumans, Jakub Stepien
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
17
18#ifndef BT_SPATIAL_ALGEBRA_H
19#define BT_SPATIAL_ALGEBRA_H
20
21
22#include "btMatrix3x3.h"
23
25{
27 //
29 btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
30 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 //
35 void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
36 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 //
41 void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
42 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 //
48 const btVector3 & getLinear() const { return m_topVec; }
49 const btVector3 & getAngular() const { return m_bottomVec; }
50 //
51 void setLinear(const btVector3 &linear) { m_topVec = linear; }
52 void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
53 //
54 void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
55 void addLinear(const btVector3 &linear) { m_topVec += linear; }
56 //
58 //
65 //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
66};
67
69{
71 //
73 btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
74 //
75 void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
76 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 //
81 void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
82 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 //
88 const btVector3 & getAngular() const { return m_topVec; }
89 const btVector3 & getLinear() const { return m_bottomVec; }
90 //
91 void setAngular(const btVector3 &angular) { m_topVec = angular; }
92 void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
93 //
94 void addAngular(const btVector3 &angular) { m_topVec += angular; }
95 void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
96 //
98 //
100 {
102 }
103 //
104 template<typename SpatialVectorType>
105 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>
111 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 //
121 btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
126};
127
129{
131 //
133 btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
134 //
135 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 //
142 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 //
150 //
152 {
156 return *this;
157 }
158 //
160 {
162 }
163};
164
166{
167 btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
169 //
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
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;
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;
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;
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
306template<typename SpatialVectorType>
307void 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
317template<typename SpatialVectorType>
318btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
319{
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
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:958
void setIdentity()
Set the matrix to the identity.
Definition: btMatrix3x3.h:317
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
void setZero()
Definition: btVector3.h:683
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
btSpatialForceVector & operator+=(const btSpatialForceVector &vec)
void addLinear(const btVector3 &linear)
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
btSpatialForceVector & operator-=(const btSpatialForceVector &vec)
void setAngular(const btVector3 &angular)
void addVector(const btVector3 &angular, const btVector3 &linear)
btSpatialForceVector(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
btSpatialForceVector operator*(const btScalar &s) const
const btVector3 & getAngular() const
btSpatialForceVector operator-() const
const btVector3 & getLinear() const
void addAngular(const btVector3 &angular)
btSpatialForceVector operator+(const btSpatialForceVector &vec) const
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear)
void setLinear(const btVector3 &linear)
void setVector(const btVector3 &angular, const btVector3 &linear)
SpatialVectorType cross(const SpatialVectorType &b) const
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
void setLinear(const btVector3 &linear)
const btVector3 & getLinear() const
btSpatialMotionVector & operator-=(const btSpatialMotionVector &vec)
btSpatialMotionVector operator-() const
void addVector(const btVector3 &angular, const btVector3 &linear)
btSpatialMotionVector & operator+=(const btSpatialMotionVector &vec)
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
const btVector3 & getAngular() const
btScalar dot(const btSpatialForceVector &b) const
btSpatialMotionVector operator+(const btSpatialMotionVector &vec) const
void addAngular(const btVector3 &angular)
btSpatialMotionVector operator*(const btScalar &s) const
void setAngular(const btVector3 &angular)
btSpatialMotionVector & operator*=(const btScalar &s)
void addLinear(const btVector3 &linear)
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear)
void setVector(const btVector3 &angular, const btVector3 &linear)
void transformInverse(const btSymmetricSpatialDyad &inMat, btSymmetricSpatialDyad &outMat, eOutputOperation outOp=None)
void transformInverse(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
void transformRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
SpatialVectorType operator*(const SpatialVectorType &vec)
void transform(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
void transformInverseRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
btSpatialForceVector operator*(const btSpatialMotionVector &vec)
btSymmetricSpatialDyad & operator-=(const btSymmetricSpatialDyad &mat)
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)