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"
21 #include "LinearMath/btAlignedObjectArray.h"
22 
23 class btIntSortPredicate
24 {
25  public:
26  bool operator() ( const int& a, const int& b ) const
27  {
28  return a < b;
29  }
30 };
31 
32 
33 template <typename T>
34 struct btMatrixX
35 {
36  int m_rows;
37  int m_cols;
38  int m_operations;
39  int m_resizeOperations;
40  int m_setElemOperations;
41 
42  btAlignedObjectArray<T> m_storage;
43  btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
44  btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
45 
46  T* getBufferPointerWritable()
47  {
48  return m_storage.size() ? &m_storage[0] : 0;
49  }
50 
51  const T* getBufferPointer() const
52  {
53  return m_storage.size() ? &m_storage[0] : 0;
54  }
55  btMatrixX()
56  :m_rows(0),
57  m_cols(0),
58  m_operations(0),
59  m_resizeOperations(0),
60  m_setElemOperations(0)
61  {
62  }
63  btMatrixX(int rows,int cols)
64  :m_rows(rows),
65  m_cols(cols),
66  m_operations(0),
67  m_resizeOperations(0),
68  m_setElemOperations(0)
69  {
70  resize(rows,cols);
71  }
72  void resize(int rows, int cols)
73  {
74  m_resizeOperations++;
75  m_rows = rows;
76  m_cols = cols;
77  {
78  BT_PROFILE("m_storage.resize");
79  m_storage.resize(rows*cols);
80  }
81  clearSparseInfo();
82  }
83  int cols() const
84  {
85  return m_cols;
86  }
87  int rows() const
88  {
89  return m_rows;
90  }
92  /*T& operator() (int row,int col)
93  {
94  return m_storage[col*m_rows+row];
95  }
96  */
97 
98  void addElem(int row,int col, T val)
99  {
100  if (val)
101  {
102  if (m_storage[col+row*m_cols]==0.f)
103  {
104  setElem(row,col,val);
105  } else
106  {
107  m_storage[row*m_cols+col] += val;
108  }
109  }
110  }
111 
112  void copyLowerToUpperTriangle()
113  {
114  int count=0;
115  for (int row=0;row<m_rowNonZeroElements1.size();row++)
116  {
117  for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
118  {
119  int col = m_rowNonZeroElements1[row][j];
120  setElem(col,row, (*this)(row,col));
121  count++;
122 
123  }
124  }
125  //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
126  }
127  void setElem(int row,int col, T val)
128  {
129  m_setElemOperations++;
130  if (val)
131  {
132  if (m_storage[col+row*m_cols]==0.f)
133  {
134  m_rowNonZeroElements1[row].push_back(col);
135  m_colNonZeroElements[col].push_back(row);
136  }
137  m_storage[row*m_cols+col] = val;
138  }
139  }
140  const T& operator() (int row,int col) const
141  {
142  return m_storage[col+row*m_cols];
143  }
144 
145  void clearSparseInfo()
146  {
147  BT_PROFILE("clearSparseInfo=0");
148  m_rowNonZeroElements1.resize(m_rows);
149  m_colNonZeroElements.resize(m_cols);
150  for (int i=0;i<m_rows;i++)
151  m_rowNonZeroElements1[i].resize(0);
152  for (int j=0;j<m_cols;j++)
153  m_colNonZeroElements[j].resize(0);
154  }
155 
156  void setZero()
157  {
158  {
159  BT_PROFILE("storage=0");
160  btSetZero(&m_storage[0],m_storage.size());
161  //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
162  //for (int i=0;i<m_storage.size();i++)
163  // m_storage[i]=0;
164  }
165  {
166  BT_PROFILE("clearSparseInfo=0");
167  clearSparseInfo();
168  }
169  }
170 
171  void printMatrix(const char* msg)
172  {
173  printf("%s ---------------------\n",msg);
174  for (int i=0;i<rows();i++)
175  {
176  printf("\n");
177  for (int j=0;j<cols();j++)
178  {
179  printf("%2.1f\t",(*this)(i,j));
180  }
181  }
182  printf("\n---------------------\n");
183 
184  }
185  void printNumZeros(const char* msg)
186  {
187  printf("%s: ",msg);
188  int numZeros = 0;
189  for (int i=0;i<m_storage.size();i++)
190  if (m_storage[i]==0)
191  numZeros++;
192  int total = m_cols*m_rows;
193  int computedNonZero = total-numZeros;
194  int nonZero = 0;
195  for (int i=0;i<m_colNonZeroElements.size();i++)
196  nonZero += m_colNonZeroElements[i].size();
197  btAssert(computedNonZero==nonZero);
198  if(computedNonZero!=nonZero)
199  {
200  printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
201  }
202  //printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
203  printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
204  }
205  /*
206  void rowComputeNonZeroElements()
207  {
208  m_rowNonZeroElements1.resize(rows());
209  for (int i=0;i<rows();i++)
210  {
211  m_rowNonZeroElements1[i].resize(0);
212  for (int j=0;j<cols();j++)
213  {
214  if ((*this)(i,j)!=0.f)
215  {
216  m_rowNonZeroElements1[i].push_back(j);
217  }
218  }
219  }
220  }
221  */
222  btMatrixX transpose() const
223  {
224  //transpose is optimized for sparse matrices
225  btMatrixX tr(m_cols,m_rows);
226  tr.setZero();
227 #if 0
228  for (int i=0;i<m_cols;i++)
229  for (int j=0;j<m_rows;j++)
230  {
231  T v = (*this)(j,i);
232  if (v)
233  {
234  tr.setElem(i,j,v);
235  }
236  }
237 #else
238  for (int i=0;i<m_colNonZeroElements.size();i++)
239  for (int h=0;h<m_colNonZeroElements[i].size();h++)
240  {
241  int j = m_colNonZeroElements[i][h];
242  T v = (*this)(j,i);
243  tr.setElem(i,j,v);
244  }
245 #endif
246  return tr;
247  }
248 
249  void sortRowIndexArrays()
250  {
251  for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
252  {
253  m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
254  }
255  }
256 
257  void sortColIndexArrays()
258  {
259  for (int i=0;i<m_colNonZeroElements[i].size();i++)
260  {
261  m_colNonZeroElements[i].quickSort(btIntSortPredicate());
262  }
263  }
264 
265  btMatrixX operator*(const btMatrixX& other)
266  {
267  //btMatrixX*btMatrixX implementation, optimized for sparse matrices
268  btAssert(cols() == other.rows());
269 
270  btMatrixX res(rows(),other.cols());
271  res.setZero();
272 // BT_PROFILE("btMatrixX mul");
273  for (int j=0; j < res.cols(); ++j)
274  {
275  //int numZero=other.m_colNonZeroElements[j].size();
276  //if (numZero)
277  {
278  for (int i=0; i < res.rows(); ++i)
279  //for (int g = 0;g<m_colNonZeroElements[j].size();g++)
280  {
281  T dotProd=0;
282  T dotProd2=0;
283  int waste=0,waste2=0;
284 
285  bool doubleWalk = false;
286  if (doubleWalk)
287  {
288  int numRows = m_rowNonZeroElements1[i].size();
289  int numOtherCols = other.m_colNonZeroElements[j].size();
290  for (int ii=0;ii<numRows;ii++)
291  {
292  int vThis=m_rowNonZeroElements1[i][ii];
293  }
294 
295  for (int ii=0;ii<numOtherCols;ii++)
296  {
297  int vOther = other.m_colNonZeroElements[j][ii];
298  }
299 
300 
301  int indexRow = 0;
302  int indexOtherCol = 0;
303  while (indexRow < numRows && indexOtherCol < numOtherCols)
304  {
305  int vThis=m_rowNonZeroElements1[i][indexRow];
306  int vOther = other.m_colNonZeroElements[j][indexOtherCol];
307  if (vOther==vThis)
308  {
309  dotProd += (*this)(i,vThis) * other(vThis,j);
310  }
311  if (vThis<vOther)
312  {
313  indexRow++;
314  } else
315  {
316  indexOtherCol++;
317  }
318  }
319 
320  } else
321  {
322  bool useOtherCol = true;
323  if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
324  {
325  useOtherCol=true;
326  }
327  if (!useOtherCol )
328  {
329  for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
330  {
331  int v = other.m_colNonZeroElements[j][q];
332  T w = (*this)(i,v);
333  if (w!=0.f)
334  {
335  dotProd+=w*other(v,j);
336  }
337 
338  }
339  }
340  else
341  {
342  for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
343  {
344  int v=m_rowNonZeroElements1[i][q];
345  T w = (*this)(i,v);
346  if (other(v,j)!=0.f)
347  {
348  dotProd+=w*other(v,j);
349  }
350 
351  }
352  }
353  }
354  if (dotProd)
355  res.setElem(i,j,dotProd);
356  }
357  }
358  }
359  return res;
360  }
361 
362  // this assumes the 4th and 8th rows of B and C are zero.
363  void multiplyAdd2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther ,int row, int col)
364  {
365  const btScalar *bb = B;
366  for ( int i = 0;i<numRows;i++)
367  {
368  const btScalar *cc = C;
369  for ( int j = 0;j<numRowsOther;j++)
370  {
371  btScalar sum;
372  sum = bb[0]*cc[0];
373  sum += bb[1]*cc[1];
374  sum += bb[2]*cc[2];
375  sum += bb[4]*cc[4];
376  sum += bb[5]*cc[5];
377  sum += bb[6]*cc[6];
378  addElem(row+i,col+j,sum);
379  cc += 8;
380  }
381  bb += 8;
382  }
383  }
384 
385  void multiply2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
386  {
387  btAssert (numRows>0 && numRowsOther>0 && B && C);
388  const btScalar *bb = B;
389  for ( int i = 0;i<numRows;i++)
390  {
391  const btScalar *cc = C;
392  for ( int j = 0;j<numRowsOther;j++)
393  {
394  btScalar sum;
395  sum = bb[0]*cc[0];
396  sum += bb[1]*cc[1];
397  sum += bb[2]*cc[2];
398  sum += bb[4]*cc[4];
399  sum += bb[5]*cc[5];
400  sum += bb[6]*cc[6];
401  setElem(row+i,col+j,sum);
402  cc += 8;
403  }
404  bb += 8;
405  }
406  }
407 
408 };
409 
410 template <typename T>
411 struct btVectorX
412 {
413  btAlignedObjectArray<T> m_storage;
414 
415  btVectorX()
416  {
417  }
418  btVectorX(int numRows)
419  {
420  m_storage.resize(numRows);
421  }
422 
423  void resize(int rows)
424  {
425  m_storage.resize(rows);
426  }
427  int cols() const
428  {
429  return 1;
430  }
431  int rows() const
432  {
433  return m_storage.size();
434  }
435  int size() const
436  {
437  return rows();
438  }
439  void setZero()
440  {
441  // for (int i=0;i<m_storage.size();i++)
442  // m_storage[i]=0;
443  //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
444  btSetZero(&m_storage[0],m_storage.size());
445  }
446  const T& operator[] (int index) const
447  {
448  return m_storage[index];
449  }
450 
451  T& operator[] (int index)
452  {
453  return m_storage[index];
454  }
455 
456  T* getBufferPointerWritable()
457  {
458  return m_storage.size() ? &m_storage[0] : 0;
459  }
460 
461  const T* getBufferPointer() const
462  {
463  return m_storage.size() ? &m_storage[0] : 0;
464  }
465 
466 };
467 /*
468 template <typename T>
469 void setElem(btMatrixX<T>& mat, int row, int col, T val)
470 {
471  mat.setElem(row,col,val);
472 }
473 */
474 
475 
476 typedef btMatrixX<float> btMatrixXf;
477 typedef btVectorX<float> btVectorXf;
478 
479 typedef btMatrixX<double> btMatrixXd;
480 typedef btVectorX<double> btVectorXd;
481 
482 
483 
484 inline void setElem(btMatrixXd& mat, int row, int col, double val)
485 {
486  mat.setElem(row,col,val);
487 }
488 
489 inline void setElem(btMatrixXf& mat, int row, int col, float val)
490 {
491  mat.setElem(row,col,val);
492 }
493 
494 #ifdef BT_USE_DOUBLE_PRECISION
495  #define btVectorXu btVectorXd
496  #define btMatrixXu btMatrixXd
497 #else
498  #define btVectorXu btVectorXf
499  #define btMatrixXu btMatrixXf
500 #endif //BT_USE_DOUBLE_PRECISION
501 
502 
503 
504 #endif//BT_MATRIX_H_H
Generated on Wed Apr 9 2014 02:38:23 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2