Engine/lib/bullet/src/BulletInverseDynamics/details/IDMatVec.hpp

   1 /// @file Built-In Matrix-Vector functions
   2 #ifndef IDMATVEC_HPP_
   3 #define IDMATVEC_HPP_
   4
   5 #include <cstdlib>
   6
   7 #include "../IDConfig.hpp"
   8 #define BT_ID_HAVE_MAT3X
   9
  10 namespace btInverseDynamics {
  11 class vec3;
  12 class vecx;
  13 class mat33;
  14 class matxx;
  15 class mat3x;
  16
  17 /// This is a very basic implementation to enable stand-alone use of the library.
  18 /// The implementation is not really optimized and misses many features that you would
  19 /// want from a "fully featured" linear math library.
  20 class vec3 {
  21 public:
  22         idScalar& operator()(int i) { return m_data[i]; }
  23         const idScalar& operator()(int i) const { return m_data[i]; }
  24         const int size() const { return 3; }
  25         const vec3& operator=(const vec3& rhs);
  26         const vec3& operator+=(const vec3& b);
  27         const vec3& operator-=(const vec3& b);
  28         vec3 cross(const vec3& b) const;
  29         idScalar dot(const vec3& b) const;
  30
  31         friend vec3 operator*(const mat33& a, const vec3& b);
  32         friend vec3 operator*(const vec3& a, const idScalar& s);
  33         friend vec3 operator*(const idScalar& s, const vec3& a);
  34
  35         friend vec3 operator+(const vec3& a, const vec3& b);
  36         friend vec3 operator-(const vec3& a, const vec3& b);
  37         friend vec3 operator/(const vec3& a, const idScalar& s);
  38
  39 private:
  40         idScalar m_data[3];
  41 };
  42
  43 class mat33 {
  44 public:
  45         idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
  46         const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
  47         const mat33& operator=(const mat33& rhs);
  48         mat33 transpose() const;
  49         const mat33& operator+=(const mat33& b);
  50         const mat33& operator-=(const mat33& b);
  51
  52         friend mat33 operator*(const mat33& a, const mat33& b);
  53         friend vec3 operator*(const mat33& a, const vec3& b);
  54         friend mat33 operator*(const mat33& a, const idScalar& s);
  55         friend mat33 operator*(const idScalar& s, const mat33& a);
  56         friend mat33 operator+(const mat33& a, const mat33& b);
  57         friend mat33 operator-(const mat33& a, const mat33& b);
  58         friend mat33 operator/(const mat33& a, const idScalar& s);
  59
  60 private:
  61         // layout is [0,1,2;3,4,5;6,7,8]
  62         idScalar m_data[9];
  63 };
  64
  65 class vecx {
  66 public:
  67         vecx(int size) : m_size(size) {
  68                 m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
  69         }
  70         ~vecx() { idFree(m_data); }
  71         const vecx& operator=(const vecx& rhs);
  72         idScalar& operator()(int i) { return m_data[i]; }
  73         const idScalar& operator()(int i) const { return m_data[i]; }
  74         const int& size() const { return m_size; }
  75
  76         friend vecx operator*(const vecx& a, const idScalar& s);
  77         friend vecx operator*(const idScalar& s, const vecx& a);
  78
  79         friend vecx operator+(const vecx& a, const vecx& b);
  80         friend vecx operator-(const vecx& a, const vecx& b);
  81         friend vecx operator/(const vecx& a, const idScalar& s);
  82
  83 private:
  84         int m_size;
  85         idScalar* m_data;
  86 };
  87
  88 class matxx {
  89 public:
  90     matxx() {
  91         m_data = 0x0;
  92         m_cols=0;
  93         m_rows=0;
  94     }
  95         matxx(int rows, int cols) : m_rows(rows), m_cols(cols) {
  96                 m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
  97         }
  98         ~matxx() { idFree(m_data); }
  99         idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
 100         const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
 101         const int& rows() const { return m_rows; }
 102         const int& cols() const { return m_cols; }
 103
 104 private:
 105         int m_rows;
 106         int m_cols;
 107         idScalar* m_data;
 108 };
 109
 110 class mat3x {
 111 public:
 112     mat3x() {
 113         m_data = 0x0;
 114         m_cols=0;
 115     }
 116     mat3x(const mat3x&rhs) {
 117         m_cols=rhs.m_cols;
 118         allocate();
 119         *this = rhs;
 120     }
 121     mat3x(int rows, int cols): m_cols(cols) {
 122         allocate();
 123     };
 124     void operator=(const mat3x& rhs) {
 125         if (m_cols != rhs.m_cols) {
 126             error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
 127             abort();
 128         }
 129         for(int i=0;i<3*m_cols;i++) {
 130             m_data[i] = rhs.m_data[i];
 131         }
 132     }
 133
 134     ~mat3x() {
 135         free();
 136     }
 137     idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
 138     const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
 139     int rows() const { return m_rows; }
 140     const int& cols() const { return m_cols; }
 141     void resize(int rows, int cols) {
 142         m_cols=cols;
 143         free();
 144         allocate();
 145     }
 146     void setZero() {
 147         memset(m_data,0x0,sizeof(idScalar)*m_rows*m_cols);
 148     }
 149     // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
 150 private:
 151     void allocate(){m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols));}
 152     void free() { idFree(m_data);}
 153     enum {m_rows=3};
 154     int m_cols;
 155     idScalar* m_data;
 156 };
 157
 158 inline void resize(mat3x &m, idArrayIdx size) {
 159     m.resize(3, size);
 160     m.setZero();
 161 }
 162
 163 //////////////////////////////////////////////////
 164 // Implementations
 165 inline const vec3& vec3::operator=(const vec3& rhs) {
 166         if (&rhs != this) {
 167                 memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
 168         }
 169         return *this;
 170 }
 171
 172 inline vec3 vec3::cross(const vec3& b) const {
 173         vec3 result;
 174         result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
 175         result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2];
 176         result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0];
 177
 178         return result;
 179 }
 180
 181 inline idScalar vec3::dot(const vec3& b) const {
 182         return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
 183 }
 184
 185 inline const mat33& mat33::operator=(const mat33& rhs) {
 186         if (&rhs != this) {
 187                 memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
 188         }
 189         return *this;
 190 }
 191 inline mat33 mat33::transpose() const {
 192         mat33 result;
 193         result.m_data[0] = m_data[0];
 194         result.m_data[1] = m_data[3];
 195         result.m_data[2] = m_data[6];
 196         result.m_data[3] = m_data[1];
 197         result.m_data[4] = m_data[4];
 198         result.m_data[5] = m_data[7];
 199         result.m_data[6] = m_data[2];
 200         result.m_data[7] = m_data[5];
 201         result.m_data[8] = m_data[8];
 202
 203         return result;
 204 }
 205
 206 inline mat33 operator*(const mat33& a, const mat33& b) {
 207         mat33 result;
 208         result.m_data[0] =
 209                 a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6];
 210         result.m_data[1] =
 211                 a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7];
 212         result.m_data[2] =
 213                 a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8];
 214         result.m_data[3] =
 215                 a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6];
 216         result.m_data[4] =
 217                 a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7];
 218         result.m_data[5] =
 219                 a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8];
 220         result.m_data[6] =
 221                 a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6];
 222         result.m_data[7] =
 223                 a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7];
 224         result.m_data[8] =
 225                 a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8];
 226
 227         return result;
 228 }
 229
 230 inline const mat33& mat33::operator+=(const mat33& b) {
 231         for (int i = 0; i < 9; i++) {
 232                 m_data[i] += b.m_data[i];
 233         }
 234
 235         return *this;
 236 }
 237
 238 inline const mat33& mat33::operator-=(const mat33& b) {
 239         for (int i = 0; i < 9; i++) {
 240                 m_data[i] -= b.m_data[i];
 241         }
 242         return *this;
 243 }
 244
 245 inline vec3 operator*(const mat33& a, const vec3& b) {
 246         vec3 result;
 247
 248         result.m_data[0] =
 249                 a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2];
 250         result.m_data[1] =
 251                 a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2];
 252         result.m_data[2] =
 253                 a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2];
 254
 255         return result;
 256 }
 257
 258 inline const vec3& vec3::operator+=(const vec3& b) {
 259         for (int i = 0; i < 3; i++) {
 260                 m_data[i] += b.m_data[i];
 261         }
 262         return *this;
 263 }
 264
 265 inline const vec3& vec3::operator-=(const vec3& b) {
 266         for (int i = 0; i < 3; i++) {
 267                 m_data[i] -= b.m_data[i];
 268         }
 269         return *this;
 270 }
 271
 272 inline mat33 operator*(const mat33& a, const idScalar& s) {
 273         mat33 result;
 274         for (int i = 0; i < 9; i++) {
 275                 result.m_data[i] = a.m_data[i] * s;
 276         }
 277         return result;
 278 }
 279
 280 inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
 281
 282 inline vec3 operator*(const vec3& a, const idScalar& s) {
 283         vec3 result;
 284         for (int i = 0; i < 3; i++) {
 285                 result.m_data[i] = a.m_data[i] * s;
 286         }
 287         return result;
 288 }
 289 inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
 290
 291 inline mat33 operator+(const mat33& a, const mat33& b) {
 292         mat33 result;
 293         for (int i = 0; i < 9; i++) {
 294                 result.m_data[i] = a.m_data[i] + b.m_data[i];
 295         }
 296         return result;
 297 }
 298 inline vec3 operator+(const vec3& a, const vec3& b) {
 299         vec3 result;
 300         for (int i = 0; i < 3; i++) {
 301                 result.m_data[i] = a.m_data[i] + b.m_data[i];
 302         }
 303         return result;
 304 }
 305
 306 inline mat33 operator-(const mat33& a, const mat33& b) {
 307         mat33 result;
 308         for (int i = 0; i < 9; i++) {
 309                 result.m_data[i] = a.m_data[i] - b.m_data[i];
 310         }
 311         return result;
 312 }
 313 inline vec3 operator-(const vec3& a, const vec3& b) {
 314         vec3 result;
 315         for (int i = 0; i < 3; i++) {
 316                 result.m_data[i] = a.m_data[i] - b.m_data[i];
 317         }
 318         return result;
 319 }
 320
 321 inline mat33 operator/(const mat33& a, const idScalar& s) {
 322         mat33 result;
 323         for (int i = 0; i < 9; i++) {
 324                 result.m_data[i] = a.m_data[i] / s;
 325         }
 326         return result;
 327 }
 328
 329 inline vec3 operator/(const vec3& a, const idScalar& s) {
 330         vec3 result;
 331         for (int i = 0; i < 3; i++) {
 332                 result.m_data[i] = a.m_data[i] / s;
 333         }
 334         return result;
 335 }
 336
 337 inline const vecx& vecx::operator=(const vecx& rhs) {
 338         if (size() != rhs.size()) {
 339                 error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
 340                 abort();
 341         }
 342         if (&rhs != this) {
 343                 memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
 344         }
 345         return *this;
 346 }
 347 inline vecx operator*(const vecx& a, const idScalar& s) {
 348         vecx result(a.size());
 349         for (int i = 0; i < result.size(); i++) {
 350                 result.m_data[i] = a.m_data[i] * s;
 351         }
 352         return result;
 353 }
 354 inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
 355 inline vecx operator+(const vecx& a, const vecx& b) {
 356         vecx result(a.size());
 357         // TODO: error handling for a.size() != b.size()??
 358         if (a.size() != b.size()) {
 359                 error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 360                 abort();
 361         }
 362         for (int i = 0; i < a.size(); i++) {
 363                 result.m_data[i] = a.m_data[i] + b.m_data[i];
 364         }
 365
 366         return result;
 367 }
 368 inline vecx operator-(const vecx& a, const vecx& b) {
 369         vecx result(a.size());
 370         // TODO: error handling for a.size() != b.size()??
 371         if (a.size() != b.size()) {
 372                 error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
 373                 abort();
 374         }
 375         for (int i = 0; i < a.size(); i++) {
 376                 result.m_data[i] = a.m_data[i] - b.m_data[i];
 377         }
 378         return result;
 379 }
 380 inline vecx operator/(const vecx& a, const idScalar& s) {
 381         vecx result(a.size());
 382         for (int i = 0; i < result.size(); i++) {
 383                 result.m_data[i] = a.m_data[i] / s;
 384         }
 385
 386         return result;
 387 }
 388
 389 inline vec3 operator*(const mat3x& a, const vecx& b) {
 390     vec3 result;
 391     if (a.cols() != b.size()) {
 392         error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
 393         abort();
 394     }
 395     result(0)=0.0;
 396     result(1)=0.0;
 397     result(2)=0.0;
 398     for(int i=0;i<b.size();i++) {
 399         for(int k=0;k<3;k++) {
 400             result(k)+=a(k,i)*b(i);
 401         }
 402     }
 403     return result;
 404 }
 405
 406 inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx*m){
 407     (*m)(row, col) = val;
 408 }
 409
 410 inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x*m){
 411     (*m)(row, col) = val;
 412 }
 413
 414 } // namespace btInverseDynamcis
 415 #endif