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 
23 //#define BT_DEBUG_OSTREAM
24 #ifdef BT_DEBUG_OSTREAM
25 #include <iostream>
26 #include <iomanip> // std::setw
27 #endif //BT_DEBUG_OSTREAM
28 
30 {
31  public:
32  bool operator() ( const int& a, const int& b ) const
33  {
34  return a < b;
35  }
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  }
114  void setZero()
115  {
116  if (m_storage.size())
117  {
118  // for (int i=0;i<m_storage.size();i++)
119  // m_storage[i]=0;
120  //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
121  btSetZero(&m_storage[0],m_storage.size());
122  }
123  }
124  const T& operator[] (int index) const
125  {
126  return m_storage[index];
127  }
128 
129  T& operator[] (int index)
130  {
131  return m_storage[index];
132  }
133 
135  {
136  return m_storage.size() ? &m_storage[0] : 0;
137  }
138 
139  const T* getBufferPointer() const
140  {
141  return m_storage.size() ? &m_storage[0] : 0;
142  }
143 
144 };
145 /*
146  template <typename T>
147  void setElem(btMatrixX<T>& mat, int row, int col, T val)
148  {
149  mat.setElem(row,col,val);
150  }
151  */
152 
153 
154 template <typename T>
155 struct btMatrixX
156 {
157  int m_rows;
158  int m_cols;
162 
165 
167  {
168  return m_storage.size() ? &m_storage[0] : 0;
169  }
170 
171  const T* getBufferPointer() const
172  {
173  return m_storage.size() ? &m_storage[0] : 0;
174  }
176  :m_rows(0),
177  m_cols(0),
178  m_operations(0),
179  m_resizeOperations(0),
180  m_setElemOperations(0)
181  {
182  }
183  btMatrixX(int rows,int cols)
184  :m_rows(rows),
185  m_cols(cols),
186  m_operations(0),
187  m_resizeOperations(0),
188  m_setElemOperations(0)
189  {
190  resize(rows,cols);
191  }
192  void resize(int rows, int cols)
193  {
194  m_resizeOperations++;
195  m_rows = rows;
196  m_cols = cols;
197  {
198  BT_PROFILE("m_storage.resize");
199  m_storage.resize(rows*cols);
200  }
201  }
202  int cols() const
203  {
204  return m_cols;
205  }
206  int rows() const
207  {
208  return m_rows;
209  }
211  /*T& operator() (int row,int col)
212  {
213  return m_storage[col*m_rows+row];
214  }
215  */
216 
217  void addElem(int row,int col, T val)
218  {
219  if (val)
220  {
221  if (m_storage[col+row*m_cols]==0.f)
222  {
223  setElem(row,col,val);
224  } else
225  {
226  m_storage[row*m_cols+col] += val;
227  }
228  }
229  }
230 
231 
232  void setElem(int row,int col, T val)
233  {
234  m_setElemOperations++;
235  m_storage[row*m_cols+col] = val;
236  }
237 
238  void mulElem(int row,int col, T val)
239  {
240  m_setElemOperations++;
241  //mul doesn't change sparsity info
242 
243  m_storage[row*m_cols+col] *= val;
244  }
245 
246 
247 
248 
250  {
251  int count=0;
252  for (int row=0;row<rows();row++)
253  {
254  for (int col=0;col<row;col++)
255  {
256  setElem(col,row, (*this)(row,col));
257  count++;
258 
259  }
260  }
261  //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
262  }
263 
264  const T& operator() (int row,int col) const
265  {
266  return m_storage[col+row*m_cols];
267  }
268 
269 
270  void setZero()
271  {
272  {
273  BT_PROFILE("storage=0");
274  btSetZero(&m_storage[0],m_storage.size());
275  //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
276  //for (int i=0;i<m_storage.size();i++)
277  // m_storage[i]=0;
278  }
279  }
280 
281  void setIdentity()
282  {
283  btAssert(rows() == cols());
284 
285  setZero();
286  for (int row=0;row<rows();row++)
287  {
288  setElem(row,row,1);
289  }
290  }
291 
292 
293 
294  void printMatrix(const char* msg)
295  {
296  printf("%s ---------------------\n",msg);
297  for (int i=0;i<rows();i++)
298  {
299  printf("\n");
300  for (int j=0;j<cols();j++)
301  {
302  printf("%2.1f\t",(*this)(i,j));
303  }
304  }
305  printf("\n---------------------\n");
306 
307  }
308 
309 
311  {
312  m_rowNonZeroElements1.resize(rows());
313  for (int i=0;i<rows();i++)
314  {
315  m_rowNonZeroElements1[i].resize(0);
316  for (int j=0;j<cols();j++)
317  {
318  if ((*this)(i,j)!=0.f)
319  {
320  m_rowNonZeroElements1[i].push_back(j);
321  }
322  }
323  }
324  }
326  {
327  //transpose is optimized for sparse matrices
328  btMatrixX tr(m_cols,m_rows);
329  tr.setZero();
330  for (int i=0;i<m_cols;i++)
331  for (int j=0;j<m_rows;j++)
332  {
333  T v = (*this)(j,i);
334  if (v)
335  {
336  tr.setElem(i,j,v);
337  }
338  }
339  return tr;
340  }
341 
342 
344  {
345  //btMatrixX*btMatrixX implementation, brute force
346  btAssert(cols() == other.rows());
347 
348  btMatrixX res(rows(),other.cols());
349  res.setZero();
350 // BT_PROFILE("btMatrixX mul");
351  for (int j=0; j < res.cols(); ++j)
352  {
353  {
354  for (int i=0; i < res.rows(); ++i)
355  {
356  T dotProd=0;
357 // T dotProd2=0;
358  //int waste=0,waste2=0;
359 
360  {
361 // bool useOtherCol = true;
362  {
363  for (int v=0;v<rows();v++)
364  {
365  T w = (*this)(i,v);
366  if (other(v,j)!=0.f)
367  {
368  dotProd+=w*other(v,j);
369  }
370 
371  }
372  }
373  }
374  if (dotProd)
375  res.setElem(i,j,dotProd);
376  }
377  }
378  }
379  return res;
380  }
381 
382  // this assumes the 4th and 8th rows of B and C are zero.
383  void multiplyAdd2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther ,int row, int col)
384  {
385  const btScalar *bb = B;
386  for ( int i = 0;i<numRows;i++)
387  {
388  const btScalar *cc = C;
389  for ( int j = 0;j<numRowsOther;j++)
390  {
391  btScalar sum;
392  sum = bb[0]*cc[0];
393  sum += bb[1]*cc[1];
394  sum += bb[2]*cc[2];
395  sum += bb[4]*cc[4];
396  sum += bb[5]*cc[5];
397  sum += bb[6]*cc[6];
398  addElem(row+i,col+j,sum);
399  cc += 8;
400  }
401  bb += 8;
402  }
403  }
404 
405  void multiply2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
406  {
407  btAssert (numRows>0 && numRowsOther>0 && B && C);
408  const btScalar *bb = B;
409  for ( int i = 0;i<numRows;i++)
410  {
411  const btScalar *cc = C;
412  for ( int j = 0;j<numRowsOther;j++)
413  {
414  btScalar sum;
415  sum = bb[0]*cc[0];
416  sum += bb[1]*cc[1];
417  sum += bb[2]*cc[2];
418  sum += bb[4]*cc[4];
419  sum += bb[5]*cc[5];
420  sum += bb[6]*cc[6];
421  setElem(row+i,col+j,sum);
422  cc += 8;
423  }
424  bb += 8;
425  }
426  }
427 
428  void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
429  {
430  int numRows = rowend+1-rowstart;
431  int numCols = colend+1-colstart;
432 
433  for (int row=0;row<numRows;row++)
434  {
435  for (int col=0;col<numCols;col++)
436  {
437  setElem(rowstart+row,colstart+col,value);
438  }
439  }
440  }
441 
442  void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
443  {
444  btAssert(rowend+1-rowstart == block.rows());
445  btAssert(colend+1-colstart == block.cols());
446  for (int row=0;row<block.rows();row++)
447  {
448  for (int col=0;col<block.cols();col++)
449  {
450  setElem(rowstart+row,colstart+col,block(row,col));
451  }
452  }
453  }
454  void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
455  {
456  btAssert(rowend+1-rowstart == block.rows());
457  btAssert(colend+1-colstart == block.cols());
458  for (int row=0;row<block.rows();row++)
459  {
460  for (int col=0;col<block.cols();col++)
461  {
462  setElem(rowstart+row,colstart+col,block[row]);
463  }
464  }
465  }
466 
467 
469  {
470  btMatrixX neg(rows(),cols());
471  for (int i=0;i<rows();i++)
472  for (int j=0;j<cols();j++)
473  {
474  T v = (*this)(i,j);
475  neg.setElem(i,j,-v);
476  }
477  return neg;
478  }
479 
480 };
481 
482 
483 
486 
489 
490 
491 #ifdef BT_DEBUG_OSTREAM
492 template <typename T>
493 std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
494  {
495 
496  os << " [";
497  //printf("%s ---------------------\n",msg);
498  for (int i=0;i<mat.rows();i++)
499  {
500  for (int j=0;j<mat.cols();j++)
501  {
502  os << std::setw(12) << mat(i,j);
503  }
504  if (i!=mat.rows()-1)
505  os << std::endl << " ";
506  }
507  os << " ]";
508  //printf("\n---------------------\n");
509 
510  return os;
511  }
512 template <typename T>
513 std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
514  {
515 
516  os << " [";
517  //printf("%s ---------------------\n",msg);
518  for (int i=0;i<mat.rows();i++)
519  {
520  os << std::setw(12) << mat[i];
521  if (i!=mat.rows()-1)
522  os << std::endl << " ";
523  }
524  os << " ]";
525  //printf("\n---------------------\n");
526 
527  return os;
528  }
529 
530 #endif //BT_DEBUG_OSTREAM
531 
532 
533 inline void setElem(btMatrixXd& mat, int row, int col, double val)
534 {
535  mat.setElem(row,col,val);
536 }
537 
538 inline void setElem(btMatrixXf& mat, int row, int col, float val)
539 {
540  mat.setElem(row,col,val);
541 }
542 
543 #ifdef BT_USE_DOUBLE_PRECISION
544  #define btVectorXu btVectorXd
545  #define btMatrixXu btMatrixXd
546 #else
547  #define btVectorXu btVectorXf
548  #define btMatrixXu btMatrixXf
549 #endif //BT_USE_DOUBLE_PRECISION
550 
551 
552 
553 #endif//BT_MATRIX_H_H
void setElem(int row, int col, T val)
Definition: btMatrixX.h:232
static T sum(const btAlignedObjectArray< T > &items)
void resize(int rows)
Definition: btMatrixX.h:52
btVectorX< double > btVectorXd
Definition: btMatrixX.h:488
void push_back(const T &_Val)
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:42
btVectorX()
Definition: btMatrixX.h:44
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:163
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
bool operator()(const int &a, const int &b) const
Definition: btMatrixX.h:32
btVectorX< float > btVectorXf
Definition: btMatrixX.h:485
#define BT_ONE
Definition: btScalar.h:496
void setZero()
Definition: btMatrixX.h:114
#define btAssert(x)
Definition: btScalar.h:113
void mulElem(int row, int col, T val)
Definition: btMatrixX.h:238
original version written by Erwin Coumans, October 2013
Definition: btMatrixX.h:29
int m_resizeOperations
Definition: btMatrixX.h:160
void printMatrix(const char *msg)
Definition: btMatrixX.h:294
T * getBufferPointerWritable()
Definition: btMatrixX.h:134
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:442
void addElem(int row, int col, T val)
we don&#39;t want this read/write operator(), because we cannot keep track of non-zero elements...
Definition: btMatrixX.h:217
void setIdentity()
Definition: btMatrixX.h:281
int size() const
Definition: btMatrixX.h:64
int m_setElemOperations
Definition: btMatrixX.h:161
btVectorX(int numRows)
Definition: btMatrixX.h:47
const T * getBufferPointer() const
Definition: btMatrixX.h:139
int m_operations
Definition: btMatrixX.h:159
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition: btMatrixX.h:454
T * getBufferPointerWritable()
Definition: btMatrixX.h:166
void btSetZero(T *a, int n)
Definition: btScalar.h:684
int cols() const
Definition: btMatrixX.h:202
int rows() const
Definition: btMatrixX.h:206
int rows() const
Definition: btMatrixX.h:60
void setZero()
Definition: btMatrixX.h:270
int size() const
return the number of elements in the array
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:487
btMatrixX transpose() const
Definition: btMatrixX.h:325
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:405
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:428
void rowComputeNonZeroElements() const
Definition: btMatrixX.h:310
const T * getBufferPointer() const
Definition: btMatrixX.h:171
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:484
void resize(int newsize, const T &fillData=T())
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:533
int m_rows
Definition: btMatrixX.h:157
void multiplyAdd2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:383
int m_cols
Definition: btMatrixX.h:158
btMatrixX negative()
Definition: btMatrixX.h:468
void copyLowerToUpperTriangle()
Definition: btMatrixX.h:249
void resize(int rows, int cols)
Definition: btMatrixX.h:192
int cols() const
Definition: btMatrixX.h:56
btMatrixX operator*(const btMatrixX &other)
Definition: btMatrixX.h:343
T nrm2() const
Definition: btMatrixX.h:69
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:164
btMatrixX(int rows, int cols)
Definition: btMatrixX.h:183
btScalar btFabs(btScalar x)
Definition: btScalar.h:449