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