dali-physics/third-party/bullet3/src/LinearMath/btSpatialAlgebra.h

   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         //
  27         btSpatialForceVector() { setZero(); }
  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         //
  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         //
  98         btSpatialMotionVector() { setZero(); }
  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         //
 142         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>
 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         //
 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         //
 190         btSymmetricSpatialDyad() { setIdentity(); }
 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         //
 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>
 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 {
 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