Bullet Collision Detection & Physics Library
btMatrixX.h
Go to the documentation of this file.
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 */
16 
17 #ifndef BT_MATRIX_X_H
18 #define BT_MATRIX_X_H
19 
20 #include "LinearMath/btQuickprof.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 
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 {
43 
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 
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;
159 
162 
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  }
173  : m_rows(0),
174  m_cols(0),
175  m_operations(0),
178  {
179  }
180  btMatrixX(int rows, int cols)
181  : m_rows(rows),
182  m_cols(cols),
183  m_operations(0),
186  {
187  resize(rows, cols);
188  }
189  void resize(int rows, int cols)
190  {
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  }
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  {
232  m_storage[row * m_cols + col] = val;
233  }
234 
235  void mulElem(int row, int col, T val)
236  {
238  //mul doesn't change sparsity info
239 
240  m_storage[row * m_cols + col] *= val;
241  }
242 
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  btSetZero(&m_storage[0], m_storage.size());
267  //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
268  //for (int i=0;i<m_storage.size();i++)
269  // m_storage[i]=0;
270  }
271  }
272 
273  void setIdentity()
274  {
275  btAssert(rows() == cols());
276 
277  setZero();
278  for (int row = 0; row < rows(); row++)
279  {
280  setElem(row, row, 1);
281  }
282  }
283 
284  void printMatrix(const char* msg)
285  {
286  printf("%s ---------------------\n", msg);
287  for (int i = 0; i < rows(); i++)
288  {
289  printf("\n");
290  for (int j = 0; j < cols(); j++)
291  {
292  printf("%2.1f\t", (*this)(i, j));
293  }
294  }
295  printf("\n---------------------\n");
296  }
297 
299  {
301  for (int i = 0; i < rows(); i++)
302  {
304  for (int j = 0; j < cols(); j++)
305  {
306  if ((*this)(i, j) != 0.f)
307  {
309  }
310  }
311  }
312  }
314  {
315  //transpose is optimized for sparse matrices
316  btMatrixX tr(m_cols, m_rows);
317  tr.setZero();
318  for (int i = 0; i < m_cols; i++)
319  for (int j = 0; j < m_rows; j++)
320  {
321  T v = (*this)(j, i);
322  if (v)
323  {
324  tr.setElem(i, j, v);
325  }
326  }
327  return tr;
328  }
329 
331  {
332  //btMatrixX*btMatrixX implementation, brute force
333  btAssert(cols() == other.rows());
334 
335  btMatrixX res(rows(), other.cols());
336  res.setZero();
337  // BT_PROFILE("btMatrixX mul");
338  for (int j = 0; j < res.cols(); ++j)
339  {
340  {
341  for (int i = 0; i < res.rows(); ++i)
342  {
343  T dotProd = 0;
344  // T dotProd2=0;
345  //int waste=0,waste2=0;
346 
347  {
348  // bool useOtherCol = true;
349  {
350  for (int v = 0; v < rows(); v++)
351  {
352  T w = (*this)(i, v);
353  if (other(v, j) != 0.f)
354  {
355  dotProd += w * other(v, j);
356  }
357  }
358  }
359  }
360  if (dotProd)
361  res.setElem(i, j, dotProd);
362  }
363  }
364  }
365  return res;
366  }
367 
368  // this assumes the 4th and 8th rows of B and C are zero.
369  void multiplyAdd2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
370  {
371  const btScalar* bb = B;
372  for (int i = 0; i < numRows; i++)
373  {
374  const btScalar* cc = C;
375  for (int j = 0; j < numRowsOther; j++)
376  {
377  btScalar sum;
378  sum = bb[0] * cc[0];
379  sum += bb[1] * cc[1];
380  sum += bb[2] * cc[2];
381  sum += bb[4] * cc[4];
382  sum += bb[5] * cc[5];
383  sum += bb[6] * cc[6];
384  addElem(row + i, col + j, sum);
385  cc += 8;
386  }
387  bb += 8;
388  }
389  }
390 
391  void multiply2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
392  {
393  btAssert(numRows > 0 && numRowsOther > 0 && B && C);
394  const btScalar* bb = B;
395  for (int i = 0; i < numRows; i++)
396  {
397  const btScalar* cc = C;
398  for (int j = 0; j < numRowsOther; j++)
399  {
400  btScalar sum;
401  sum = bb[0] * cc[0];
402  sum += bb[1] * cc[1];
403  sum += bb[2] * cc[2];
404  sum += bb[4] * cc[4];
405  sum += bb[5] * cc[5];
406  sum += bb[6] * cc[6];
407  setElem(row + i, col + j, sum);
408  cc += 8;
409  }
410  bb += 8;
411  }
412  }
413 
414  void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
415  {
416  int numRows = rowend + 1 - rowstart;
417  int numCols = colend + 1 - colstart;
418 
419  for (int row = 0; row < numRows; row++)
420  {
421  for (int col = 0; col < numCols; col++)
422  {
423  setElem(rowstart + row, colstart + col, value);
424  }
425  }
426  }
427 
428  void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX& block)
429  {
430  btAssert(rowend + 1 - rowstart == block.rows());
431  btAssert(colend + 1 - colstart == block.cols());
432  for (int row = 0; row < block.rows(); row++)
433  {
434  for (int col = 0; col < block.cols(); col++)
435  {
436  setElem(rowstart + row, colstart + col, block(row, col));
437  }
438  }
439  }
440  void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX<T>& block)
441  {
442  btAssert(rowend + 1 - rowstart == block.rows());
443  btAssert(colend + 1 - colstart == block.cols());
444  for (int row = 0; row < block.rows(); row++)
445  {
446  for (int col = 0; col < block.cols(); col++)
447  {
448  setElem(rowstart + row, colstart + col, block[row]);
449  }
450  }
451  }
452 
454  {
455  btMatrixX neg(rows(), cols());
456  for (int i = 0; i < rows(); i++)
457  for (int j = 0; j < cols(); j++)
458  {
459  T v = (*this)(i, j);
460  neg.setElem(i, j, -v);
461  }
462  return neg;
463  }
464 };
465 
468 
471 
472 #ifdef BT_DEBUG_OSTREAM
473 template <typename T>
474 std::ostream& operator<<(std::ostream& os, const btMatrixX<T>& mat)
475 {
476  os << " [";
477  //printf("%s ---------------------\n",msg);
478  for (int i = 0; i < mat.rows(); i++)
479  {
480  for (int j = 0; j < mat.cols(); j++)
481  {
482  os << std::setw(12) << mat(i, j);
483  }
484  if (i != mat.rows() - 1)
485  os << std::endl
486  << " ";
487  }
488  os << " ]";
489  //printf("\n---------------------\n");
490 
491  return os;
492 }
493 template <typename T>
494 std::ostream& operator<<(std::ostream& os, const btVectorX<T>& mat)
495 {
496  os << " [";
497  //printf("%s ---------------------\n",msg);
498  for (int i = 0; i < mat.rows(); i++)
499  {
500  os << std::setw(12) << mat[i];
501  if (i != mat.rows() - 1)
502  os << std::endl
503  << " ";
504  }
505  os << " ]";
506  //printf("\n---------------------\n");
507 
508  return os;
509 }
510 
511 #endif //BT_DEBUG_OSTREAM
512 
513 inline void setElem(btMatrixXd& mat, int row, int col, double val)
514 {
515  mat.setElem(row, col, val);
516 }
517 
518 inline void setElem(btMatrixXf& mat, int row, int col, float val)
519 {
520  mat.setElem(row, col, val);
521 }
522 
523 #ifdef BT_USE_DOUBLE_PRECISION
524 #define btVectorXu btVectorXd
525 #define btMatrixXu btMatrixXd
526 #else
527 #define btVectorXu btVectorXf
528 #define btMatrixXu btMatrixXf
529 #endif //BT_USE_DOUBLE_PRECISION
530 
531 #endif //BT_MATRIX_H_H
btMatrixX::multiplyAdd2_p8r
void multiplyAdd2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:369
btMatrixX::m_storage
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:160
btVectorXd
btVectorX< double > btVectorXd
Definition: btMatrixX.h:470
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:414
btVectorX::nrm2
T nrm2() const
Definition: btMatrixX.h:69
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMatrixX::mulElem
void mulElem(int row, int col, T val)
Definition: btMatrixX.h:235
btVectorX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:133
btVectorX::size
int size() const
Definition: btMatrixX.h:64
btMatrixX::operator*
btMatrixX operator*(const btMatrixX &other)
Definition: btMatrixX.h:330
btMatrixX::m_rows
int m_rows
Definition: btMatrixX.h:154
btVectorX::btVectorX
btVectorX()
Definition: btMatrixX.h:44
btMatrixX::setElem
void setElem(int row, int col, T val)
Definition: btMatrixX.h:229
btMatrixX::m_setElemOperations
int m_setElemOperations
Definition: btMatrixX.h:158
btMatrixX::rowComputeNonZeroElements
void rowComputeNonZeroElements() const
Definition: btMatrixX.h:298
btVectorX::btVectorX
btVectorX(int numRows)
Definition: btMatrixX.h:47
btVectorX
Definition: btMatrixX.h:40
btMatrixX::btMatrixX
btMatrixX()
Definition: btMatrixX.h:172
btMatrixX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:163
BT_ONE
#define BT_ONE
Definition: btScalar.h:525
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMatrixX::m_rowNonZeroElements1
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:161
btVectorXf
btVectorX< float > btVectorXf
Definition: btMatrixX.h:467
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:477
btVectorX::rows
int rows() const
Definition: btMatrixX.h:60
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btMatrixX::negative
btMatrixX negative()
Definition: btMatrixX.h:453
btVectorX::m_storage
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:42
btMatrixXd
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:469
btIntSortPredicate
original version written by Erwin Coumans, October 2013
Definition: btMatrixX.h:30
btMatrixX::printMatrix
void printMatrix(const char *msg)
Definition: btMatrixX.h:284
btMatrixX::m_operations
int m_operations
Definition: btMatrixX.h:156
btVectorX::resize
void resize(int rows)
Definition: btMatrixX.h:52
btMatrixX::cols
int cols() const
Definition: btMatrixX.h:199
btMatrixX::copyLowerToUpperTriangle
void copyLowerToUpperTriangle()
Definition: btMatrixX.h:243
btMatrixX::getBufferPointer
const T * getBufferPointer() const
Definition: btMatrixX.h:168
btMatrixX::resize
void resize(int rows, int cols)
Definition: btMatrixX.h:189
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:52
btMatrixX::setIdentity
void setIdentity()
Definition: btMatrixX.h:273
btMatrixXf
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:466
btMatrixX::multiply2_p8r
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:391
btVectorX::cols
int cols() const
Definition: btMatrixX.h:56
btMatrixX::btMatrixX
btMatrixX(int rows, int cols)
Definition: btMatrixX.h:180
btSetZero
void btSetZero(T *a, int n)
Definition: btScalar.h:719
btMatrixX::m_resizeOperations
int m_resizeOperations
Definition: btMatrixX.h:157
btMatrixX::addElem
void addElem(int row, int col, T val)
we don't want this read/write operator(), because we cannot keep track of non-zero elements,...
Definition: btMatrixX.h:214
btQuickprof.h
btMatrixX::setZero
void setZero()
Definition: btMatrixX.h:262
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition: btMatrixX.h:440
btMatrixX::rows
int rows() const
Definition: btMatrixX.h:203
btVectorX::setZero
void setZero()
Definition: btMatrixX.h:113
btMatrixX
Definition: btMatrixX.h:152
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:428
btAlignedObjectArray.h
btMatrixX::transpose
btMatrixX transpose() const
Definition: btMatrixX.h:313
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:513
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:264
btVectorX::operator[]
T & operator[](int index)
Definition: btMatrixX.h:128
btMatrixX::operator()
const T & operator()(int row, int col) const
Definition: btMatrixX.h:257
btVectorX::operator[]
const T & operator[](int index) const
Definition: btMatrixX.h:123
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:197
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:89
btVectorX::getBufferPointer
const T * getBufferPointer() const
Definition: btMatrixX.h:138
btIntSortPredicate::operator()
bool operator()(const int &a, const int &b) const
Definition: btMatrixX.h:33
btMatrixX::m_cols
int m_cols
Definition: btMatrixX.h:155