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

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///original version written by Erwin Coumans, October 2013
  16
  17 #ifndef BT_MATRIX_X_H
  18 #define BT_MATRIX_X_H
  19
  20 #include "LinearMath/btQuickprof.h"
  21 #include "LinearMath/btAlignedObjectArray.h"
  22 #include <stdio.h>
  23
  24 //#define BT_DEBUG_OSTREAM
  25 #ifdef BT_DEBUG_OSTREAM
  26 #include <iostream>
  27 #include <iomanip>  // std::setw
  28 #endif              //BT_DEBUG_OSTREAM
  29
  30 class btIntSortPredicate
  31 {
  32 public:
  33         bool operator()(const int& a, const int& b) const
  34         {
  35                 return a < b;
  36         }
  37 };
  38
  39 template <typename T>
  40 struct btVectorX
  41 {
  42         btAlignedObjectArray<T> m_storage;
  43
  44         btVectorX()
  45         {
  46         }
  47         btVectorX(int numRows)
  48         {
  49                 m_storage.resize(numRows);
  50         }
  51
  52         void resize(int rows)
  53         {
  54                 m_storage.resize(rows);
  55         }
  56         int cols() const
  57         {
  58                 return 1;
  59         }
  60         int rows() const
  61         {
  62                 return m_storage.size();
  63         }
  64         int size() const
  65         {
  66                 return rows();
  67         }
  68
  69         T nrm2() const
  70         {
  71                 T norm = T(0);
  72
  73                 int nn = rows();
  74
  75                 {
  76                         if (nn == 1)
  77                         {
  78                                 norm = btFabs((*this)[0]);
  79                         }
  80                         else
  81                         {
  82                                 T scale = 0.0;
  83                                 T ssq = 1.0;
  84
  85                                 /* The following loop is equivalent to this call to the LAPACK
  86                                  auxiliary routine:   CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
  87
  88                                 for (int ix = 0; ix < nn; ix++)
  89                                 {
  90                                         if ((*this)[ix] != 0.0)
  91                                         {
  92                                                 T absxi = btFabs((*this)[ix]);
  93                                                 if (scale < absxi)
  94                                                 {
  95                                                         T temp;
  96                                                         temp = scale / absxi;
  97                                                         ssq = ssq * (temp * temp) + BT_ONE;
  98                                                         scale = absxi;
  99                                                 }
 100                                                 else
 101                                                 {
 102                                                         T temp;
 103                                                         temp = absxi / scale;
 104                                                         ssq += temp * temp;
 105                                                 }
 106                                         }
 107                                 }
 108                                 norm = scale * sqrt(ssq);
 109                         }
 110                 }
 111                 return norm;
 112         }
 113         void setZero()
 114         {
 115                 if (m_storage.size())
 116                 {
 117                         //      for (int i=0;i<m_storage.size();i++)
 118                         //              m_storage[i]=0;
 119                         //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
 120                         btSetZero(&m_storage[0], m_storage.size());
 121                 }
 122         }
 123         const T& operator[](int index) const
 124         {
 125                 return m_storage[index];
 126         }
 127
 128         T& operator[](int index)
 129         {
 130                 return m_storage[index];
 131         }
 132
 133         T* getBufferPointerWritable()
 134         {
 135                 return m_storage.size() ? &m_storage[0] : 0;
 136         }
 137
 138         const T* getBufferPointer() const
 139         {
 140                 return m_storage.size() ? &m_storage[0] : 0;
 141         }
 142 };
 143 /*
 144  template <typename T>
 145  void setElem(btMatrixX<T>& mat, int row, int col, T val)
 146  {
 147  mat.setElem(row,col,val);
 148  }
 149  */
 150
 151 template <typename T>
 152 struct btMatrixX
 153 {
 154         int m_rows;
 155         int m_cols;
 156         int m_operations;
 157         int m_resizeOperations;
 158         int m_setElemOperations;
 159
 160         btAlignedObjectArray<T> m_storage;
 161         mutable btAlignedObjectArray<btAlignedObjectArray<int> > m_rowNonZeroElements1;
 162
 163         T* getBufferPointerWritable()
 164         {
 165                 return m_storage.size() ? &m_storage[0] : 0;
 166         }
 167
 168         const T* getBufferPointer() const
 169         {
 170                 return m_storage.size() ? &m_storage[0] : 0;
 171         }
 172         btMatrixX()
 173                 : m_rows(0),
 174                   m_cols(0),
 175                   m_operations(0),
 176                   m_resizeOperations(0),
 177                   m_setElemOperations(0)
 178         {
 179         }
 180         btMatrixX(int rows, int cols)
 181                 : m_rows(rows),
 182                   m_cols(cols),
 183                   m_operations(0),
 184                   m_resizeOperations(0),
 185                   m_setElemOperations(0)
 186         {
 187                 resize(rows, cols);
 188         }
 189         void resize(int rows, int cols)
 190         {
 191                 m_resizeOperations++;
 192                 m_rows = rows;
 193                 m_cols = cols;
 194                 {
 195                         BT_PROFILE("m_storage.resize");
 196                         m_storage.resize(rows * cols);
 197                 }
 198         }
 199         int cols() const
 200         {
 201                 return m_cols;
 202         }
 203         int rows() const
 204         {
 205                 return m_rows;
 206         }
 207         ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead
 208         /*T& operator() (int row,int col)
 209         {
 210                 return m_storage[col*m_rows+row];
 211         }
 212         */
 213
 214         void addElem(int row, int col, T val)
 215         {
 216                 if (val)
 217                 {
 218                         if (m_storage[col + row * m_cols] == 0.f)
 219                         {
 220                                 setElem(row, col, val);
 221                         }
 222                         else
 223                         {
 224                                 m_storage[row * m_cols + col] += val;
 225                         }
 226                 }
 227         }
 228
 229         void setElem(int row, int col, T val)
 230         {
 231                 m_setElemOperations++;
 232                 m_storage[row * m_cols + col] = val;
 233         }
 234
 235         void mulElem(int row, int col, T val)
 236         {
 237                 m_setElemOperations++;
 238                 //mul doesn't change sparsity info
 239
 240                 m_storage[row * m_cols + col] *= val;
 241         }
 242
 243         void copyLowerToUpperTriangle()
 244         {
 245                 int count = 0;
 246                 for (int row = 0; row < rows(); row++)
 247                 {
 248                         for (int col = 0; col < row; col++)
 249                         {
 250                                 setElem(col, row, (*this)(row, col));
 251                                 count++;
 252                         }
 253                 }
 254                 //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
 255         }
 256
 257         const T& operator()(int row, int col) const
 258         {
 259                 return m_storage[col + row * m_cols];
 260         }
 261
 262         void setZero()
 263         {
 264                 {
 265                         BT_PROFILE("storage=0");
 266                         if (m_storage.size())
 267                         {
 268                                 btSetZero(&m_storage[0], m_storage.size());
 269                         }
 270                         //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
 271                         //for (int i=0;i<m_storage.size();i++)
 272                         //                      m_storage[i]=0;
 273                 }
 274         }
 275
 276         void setIdentity()
 277         {
 278                 btAssert(rows() == cols());
 279
 280                 setZero();
 281                 for (int row = 0; row < rows(); row++)
 282                 {
 283                         setElem(row, row, 1);
 284                 }
 285         }
 286
 287         void printMatrix(const char* msg) const
 288         {
 289                 printf("%s ---------------------\n", msg);
 290                 for (int i = 0; i < rows(); i++)
 291                 {
 292                         printf("\n");
 293                         for (int j = 0; j < cols(); j++)
 294                         {
 295                                 printf("%2.1f\t", (*this)(i, j));
 296                         }
 297                 }
 298                 printf("\n---------------------\n");
 299         }
 300
 301         void rowComputeNonZeroElements() const
 302         {
 303                 m_rowNonZeroElements1.resize(rows());
 304                 for (int i = 0; i < rows(); i++)
 305                 {
 306                         m_rowNonZeroElements1[i].resize(0);
 307                         for (int j = 0; j < cols(); j++)
 308                         {
 309                                 if ((*this)(i, j) != 0.f)
 310                                 {
 311                                         m_rowNonZeroElements1[i].push_back(j);
 312                                 }
 313                         }
 314                 }
 315         }
 316         btMatrixX transpose() const
 317         {
 318                 //transpose is optimized for sparse matrices
 319                 btMatrixX tr(m_cols, m_rows);
 320                 tr.setZero();
 321                 for (int i = 0; i < m_cols; i++)
 322                         for (int j = 0; j < m_rows; j++)
 323                         {
 324                                 T v = (*this)(j, i);
 325                                 if (v)
 326                                 {
 327                                         tr.setElem(i, j, v);
 328                                 }
 329                         }
 330                 return tr;
 331         }
 332
 333         btMatrixX operator*(const btMatrixX& other)
 334         {
 335                 //btMatrixX*btMatrixX implementation, brute force
 336                 btAssert(cols() == other.rows());
 337
 338                 btMatrixX res(rows(), other.cols());
 339                 res.setZero();
 340                 //              BT_PROFILE("btMatrixX mul");
 341                 for (int i = 0; i < rows(); ++i)
 342                 {
 343                         {
 344                                 for (int j = 0; j < other.cols(); ++j)
 345                                 {
 346                                         T dotProd = 0;
 347                                         {
 348                                                 {
 349                                                         int c = cols();
 350
 351                                                         for (int k = 0; k < c; k++)
 352                                                         {
 353                                                                 T w = (*this)(i, k);
 354                                                                 if (other(k, j) != 0.f)
 355                                                                 {
 356                                                                         dotProd += w * other(k, j);
 357                                                                 }
 358                                                         }
 359                                                 }
 360                                         }
 361                                         if (dotProd)
 362                                                 res.setElem(i, j, dotProd);
 363                                 }
 364                         }
 365                 }
 366                 return res;
 367         }
 368
 369         // this assumes the 4th and 8th rows of B and C are zero.
 370         void multiplyAdd2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
 371         {
 372                 const btScalar* bb = B;
 373                 for (int i = 0; i < numRows; i++)
 374                 {
 375                         const btScalar* cc = C;
 376                         for (int j = 0; j < numRowsOther; j++)
 377                         {
 378                                 btScalar sum;
 379                                 sum = bb[0] * cc[0];
 380                                 sum += bb[1] * cc[1];
 381                                 sum += bb[2] * cc[2];
 382                                 sum += bb[4] * cc[4];
 383                                 sum += bb[5] * cc[5];
 384                                 sum += bb[6] * cc[6];
 385                                 addElem(row + i, col + j, sum);
 386                                 cc += 8;
 387                         }
 388                         bb += 8;
 389                 }
 390         }
 391
 392         void multiply2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
 393         {
 394                 btAssert(numRows > 0 && numRowsOther > 0 && B && C);
 395                 const btScalar* bb = B;
 396                 for (int i = 0; i < numRows; i++)
 397                 {
 398                         const btScalar* cc = C;
 399                         for (int j = 0; j < numRowsOther; j++)
 400                         {
 401                                 btScalar sum;
 402                                 sum = bb[0] * cc[0];
 403                                 sum += bb[1] * cc[1];
 404                                 sum += bb[2] * cc[2];
 405                                 sum += bb[4] * cc[4];
 406                                 sum += bb[5] * cc[5];
 407                                 sum += bb[6] * cc[6];
 408                                 setElem(row + i, col + j, sum);
 409                                 cc += 8;
 410                         }
 411                         bb += 8;
 412                 }
 413         }
 414
 415         void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
 416         {
 417                 int numRows = rowend + 1 - rowstart;
 418                 int numCols = colend + 1 - colstart;
 419
 420                 for (int row = 0; row < numRows; row++)
 421                 {
 422                         for (int col = 0; col < numCols; col++)
 423                         {
 424                                 setElem(rowstart + row, colstart + col, value);
 425                         }
 426                 }
 427         }
 428
 429         void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX& block)
 430         {
 431                 btAssert(rowend + 1 - rowstart == block.rows());
 432                 btAssert(colend + 1 - colstart == block.cols());
 433                 for (int row = 0; row < block.rows(); row++)
 434                 {
 435                         for (int col = 0; col < block.cols(); col++)
 436                         {
 437                                 setElem(rowstart + row, colstart + col, block(row, col));
 438                         }
 439                 }
 440         }
 441         void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX<T>& block)
 442         {
 443                 btAssert(rowend + 1 - rowstart == block.rows());
 444                 btAssert(colend + 1 - colstart == block.cols());
 445                 for (int row = 0; row < block.rows(); row++)
 446                 {
 447                         for (int col = 0; col < block.cols(); col++)
 448                         {
 449                                 setElem(rowstart + row, colstart + col, block[row]);
 450                         }
 451                 }
 452         }
 453
 454         btMatrixX negative()
 455         {
 456                 btMatrixX neg(rows(), cols());
 457                 for (int i = 0; i < rows(); i++)
 458                         for (int j = 0; j < cols(); j++)
 459                         {
 460                                 T v = (*this)(i, j);
 461                                 neg.setElem(i, j, -v);
 462                         }
 463                 return neg;
 464         }
 465 };
 466
 467 typedef btMatrixX<float> btMatrixXf;
 468 typedef btVectorX<float> btVectorXf;
 469
 470 typedef btMatrixX<double> btMatrixXd;
 471 typedef btVectorX<double> btVectorXd;
 472
 473 #ifdef BT_DEBUG_OSTREAM
 474 template <typename T>
 475 std::ostream& operator<<(std::ostream& os, const btMatrixX<T>& mat)
 476 {
 477         os << " [";
 478         //printf("%s ---------------------\n",msg);
 479         for (int i = 0; i < mat.rows(); i++)
 480         {
 481                 for (int j = 0; j < mat.cols(); j++)
 482                 {
 483                         os << std::setw(12) << mat(i, j);
 484                 }
 485                 if (i != mat.rows() - 1)
 486                         os << std::endl
 487                            << "  ";
 488         }
 489         os << " ]";
 490         //printf("\n---------------------\n");
 491
 492         return os;
 493 }
 494 template <typename T>
 495 std::ostream& operator<<(std::ostream& os, const btVectorX<T>& mat)
 496 {
 497         os << " [";
 498         //printf("%s ---------------------\n",msg);
 499         for (int i = 0; i < mat.rows(); i++)
 500         {
 501                 os << std::setw(12) << mat[i];
 502                 if (i != mat.rows() - 1)
 503                         os << std::endl
 504                            << "  ";
 505         }
 506         os << " ]";
 507         //printf("\n---------------------\n");
 508
 509         return os;
 510 }
 511
 512 #endif  //BT_DEBUG_OSTREAM
 513
 514 inline void setElem(btMatrixXd& mat, int row, int col, double val)
 515 {
 516         mat.setElem(row, col, val);
 517 }
 518
 519 inline void setElem(btMatrixXf& mat, int row, int col, float val)
 520 {
 521         mat.setElem(row, col, val);
 522 }
 523
 524 #ifdef BT_USE_DOUBLE_PRECISION
 525 #define btVectorXu btVectorXd
 526 #define btMatrixXu btMatrixXd
 527 #else
 528 #define btVectorXu btVectorXf
 529 #define btMatrixXu btMatrixXf
 530 #endif  //BT_USE_DOUBLE_PRECISION
 531
 532 #endif  //BT_MATRIX_H_H