dali-physics/third-party/bullet3/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp

   1 #ifndef IDLINEARMATHINTERFACE_HPP_
   2 #define IDLINEARMATHINTERFACE_HPP_
   3
   4 #include <cstdlib>
   5
   6 #include "../IDConfig.hpp"
   7
   8 #include "../../LinearMath/btMatrix3x3.h"
   9 #include "../../LinearMath/btVector3.h"
  10 #include "../../LinearMath/btMatrixX.h"
  11 #define BT_ID_HAVE_MAT3X
  12
  13 namespace btInverseDynamics
  14 {
  15 class vec3;
  16 class vecx;
  17 class mat33;
  18 typedef btMatrixX<idScalar> matxx;
  19
  20 class vec3 : public btVector3
  21 {
  22 public:
  23         vec3() : btVector3() {}
  24         vec3(const btVector3& btv) { *this = btv; }
  25         idScalar& operator()(int i) { return (*this)[i]; }
  26         const idScalar& operator()(int i) const { return (*this)[i]; }
  27         int size() const { return 3; }
  28         const vec3& operator=(const btVector3& rhs)
  29         {
  30                 *static_cast<btVector3*>(this) = rhs;
  31                 return *this;
  32         }
  33 };
  34
  35 class mat33 : public btMatrix3x3
  36 {
  37 public:
  38         mat33() : btMatrix3x3() {}
  39         mat33(const btMatrix3x3& btm) { *this = btm; }
  40         idScalar& operator()(int i, int j) { return (*this)[i][j]; }
  41         const idScalar& operator()(int i, int j) const { return (*this)[i][j]; }
  42         const mat33& operator=(const btMatrix3x3& rhs)
  43         {
  44                 *static_cast<btMatrix3x3*>(this) = rhs;
  45                 return *this;
  46         }
  47         friend mat33 operator*(const idScalar& s, const mat33& a);
  48         friend mat33 operator/(const mat33& a, const idScalar& s);
  49 };
  50
  51 inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s); }
  52
  53 inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
  54
  55 class vecx : public btVectorX<idScalar>
  56 {
  57 public:
  58         vecx(int size) : btVectorX<idScalar>(size) {}
  59         const vecx& operator=(const btVectorX<idScalar>& rhs)
  60         {
  61                 *static_cast<btVectorX<idScalar>*>(this) = rhs;
  62                 return *this;
  63         }
  64
  65         idScalar& operator()(int i) { return (*this)[i]; }
  66         const idScalar& operator()(int i) const { return (*this)[i]; }
  67
  68         friend vecx operator*(const vecx& a, const idScalar& s);
  69         friend vecx operator*(const idScalar& s, const vecx& a);
  70
  71         friend vecx operator+(const vecx& a, const vecx& b);
  72         friend vecx operator-(const vecx& a, const vecx& b);
  73         friend vecx operator/(const vecx& a, const idScalar& s);
  74 };
  75
  76 inline vecx operator*(const vecx& a, const idScalar& s)
  77 {
  78         vecx result(a.size());
  79         for (int i = 0; i < result.size(); i++)
  80         {
  81                 result(i) = a(i) * s;
  82         }
  83         return result;
  84 }
  85 inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
  86 inline vecx operator+(const vecx& a, const vecx& b)
  87 {
  88         vecx result(a.size());
  89         // TODO: error handling for a.size() != b.size()??
  90         if (a.size() != b.size())
  91         {
  92                 bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
  93                 abort();
  94         }
  95         for (int i = 0; i < a.size(); i++)
  96         {
  97                 result(i) = a(i) + b(i);
  98         }
  99
 100         return result;
 101 }
 102
 103 inline vecx operator-(const vecx& a, const vecx& b)
 104 {
 105         vecx result(a.size());
 106         // TODO: error handling for a.size() != b.size()??
 107         if (a.size() != b.size())
 108         {
 109                 bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 110                 abort();
 111         }
 112         for (int i = 0; i < a.size(); i++)
 113         {
 114                 result(i) = a(i) - b(i);
 115         }
 116         return result;
 117 }
 118 inline vecx operator/(const vecx& a, const idScalar& s)
 119 {
 120         vecx result(a.size());
 121         for (int i = 0; i < result.size(); i++)
 122         {
 123                 result(i) = a(i) / s;
 124         }
 125
 126         return result;
 127 }
 128
 129 // use btMatrixX to implement 3xX matrix
 130 class mat3x : public matxx
 131 {
 132 public:
 133         mat3x() {}
 134         mat3x(const mat3x& rhs)
 135         {
 136                 matxx::resize(rhs.rows(), rhs.cols());
 137                 *this = rhs;
 138         }
 139         mat3x(int rows, int cols) : matxx(3, cols)
 140         {
 141         }
 142         void operator=(const mat3x& rhs)
 143         {
 144                 if (m_cols != rhs.m_cols)
 145                 {
 146                         bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
 147                         abort();
 148                 }
 149                 for (int i = 0; i < rows(); i++)
 150                 {
 151                         for (int k = 0; k < cols(); k++)
 152                         {
 153                                 setElem(i, k, rhs(i, k));
 154                         }
 155                 }
 156         }
 157         void setZero()
 158         {
 159                 matxx::setZero();
 160         }
 161 };
 162
 163 inline vec3 operator*(const mat3x& a, const vecx& b)
 164 {
 165         vec3 result;
 166         if (a.cols() != b.size())
 167         {
 168                 bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
 169                 abort();
 170         }
 171         result(0) = 0.0;
 172         result(1) = 0.0;
 173         result(2) = 0.0;
 174         for (int i = 0; i < b.size(); i++)
 175         {
 176                 for (int k = 0; k < 3; k++)
 177                 {
 178                         result(k) += a(k, i) * b(i);
 179                 }
 180         }
 181         return result;
 182 }
 183
 184 inline void resize(mat3x& m, idArrayIdx size)
 185 {
 186         m.resize(3, size);
 187         m.setZero();
 188 }
 189
 190 inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
 191 {
 192         m->setElem(row, col, val);
 193 }
 194
 195 inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
 196 {
 197         m->setElem(row, col, val);
 198 }
 199
 200 }  // namespace btInverseDynamics
 201
 202 #endif  // IDLINEARMATHINTERFACE_HPP_