Blender  V3.3
btSpatialAlgebra.h
Go to the documentation of this file.
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 
17 
18 #ifndef BT_SPATIAL_ALGEBRA_H
19 #define BT_SPATIAL_ALGEBRA_H
20 
21 #include "btMatrix3x3.h"
22 
24 {
26  //
28  btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
29  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  //
34  void setVector(const btVector3 &angular, const btVector3 &linear)
35  {
36  m_topVec = linear;
37  m_bottomVec = angular;
38  }
39  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  //
45  void addVector(const btVector3 &angular, const btVector3 &linear)
46  {
47  m_topVec += linear;
48  m_bottomVec += angular;
49  }
50  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  //
60  const btVector3 &getLinear() const { return m_topVec; }
61  const btVector3 &getAngular() const { return m_bottomVec; }
62  //
63  void setLinear(const btVector3 &linear) { m_topVec = linear; }
64  void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
65  //
66  void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
67  void addLinear(const btVector3 &linear) { m_topVec += linear; }
68  //
69  void setZero()
70  {
71  m_topVec.setZero();
72  m_bottomVec.setZero();
73  }
74  //
76  {
77  m_topVec += vec.m_topVec;
78  m_bottomVec += vec.m_bottomVec;
79  return *this;
80  }
82  {
83  m_topVec -= vec.m_topVec;
84  m_bottomVec -= vec.m_bottomVec;
85  return *this;
86  }
91  //btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
92 };
93 
95 {
97  //
99  btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
100  //
101  void setVector(const btVector3 &angular, const btVector3 &linear)
102  {
103  m_topVec = angular;
104  m_bottomVec = linear;
105  }
106  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  //
112  void addVector(const btVector3 &angular, const btVector3 &linear)
113  {
114  m_topVec += linear;
115  m_bottomVec += angular;
116  }
117  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  //
127  const btVector3 &getAngular() const { return m_topVec; }
128  const btVector3 &getLinear() const { return m_bottomVec; }
129  //
130  void setAngular(const btVector3 &angular) { m_topVec = angular; }
131  void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
132  //
133  void addAngular(const btVector3 &angular) { m_topVec += angular; }
134  void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
135  //
136  void setZero()
137  {
138  m_topVec.setZero();
139  m_bottomVec.setZero();
140  }
141  //
143  {
144  return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
145  }
146  //
147  template <typename SpatialVectorType>
148  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>
154  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  //
163  {
164  m_topVec += vec.m_topVec;
165  m_bottomVec += vec.m_bottomVec;
166  return *this;
167  }
169  {
170  m_topVec -= vec.m_topVec;
171  m_bottomVec -= vec.m_bottomVec;
172  return *this;
173  }
175  {
176  m_topVec *= s;
177  m_bottomVec *= s;
178  return *this;
179  }
184 };
185 
187 {
189  //
191  btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
192  //
193  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  //
200  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  //
207  void setIdentity()
208  {
209  m_topLeftMat.setIdentity();
210  m_topRightMat.setIdentity();
211  m_bottomLeftMat.setIdentity();
212  }
213  //
215  {
216  m_topLeftMat -= mat.m_topLeftMat;
219  return *this;
220  }
221  //
223  {
225  }
226 };
227 
229 {
230  btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
232  //
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 
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>
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>
377 btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
378 {
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
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
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:314
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
static unsigned a[3]
Definition: RandGen.cpp:78
static const pxr::TfToken out("out", pxr::TfToken::Immortal)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
void addLinear(const btVector3 &linear)
const btVector3 & getAngular() const
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
void setAngular(const btVector3 &angular)
void addVector(const btVector3 &angular, const btVector3 &linear)
btSpatialForceVector & operator+=(const btSpatialForceVector &vec)
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 btSpatialForceVector &vec)
btSpatialForceVector operator*(const btScalar &s) const
btSpatialForceVector operator-() const
btSpatialForceVector operator-(const btSpatialForceVector &vec) const
void addAngular(const btVector3 &angular)
btSpatialForceVector operator+(const btSpatialForceVector &vec) const
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear)
const btVector3 & getLinear() const
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)
btSpatialMotionVector & operator*=(const btScalar &s)
btSpatialMotionVector operator-() const
const btVector3 & getAngular() 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)
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 btSpatialMotionVector &vec)
const btVector3 & getLinear() const
void addLinear(const btVector3 &linear)
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear)
btSpatialMotionVector operator-(const btSpatialMotionVector &vec) const
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(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
btSymmetricSpatialDyad & operator-=(const btSymmetricSpatialDyad &mat)
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
void addMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)