Blender  V3.3
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  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 
302  {
304  for (int i = 0; i < rows(); i++)
305  {
307  for (int j = 0; j < cols(); j++)
308  {
309  if ((*this)(i, j) != 0.f)
310  {
312  }
313  }
314  }
315  }
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 
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 
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 
469 
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
sqrt(x)+1/max(0
#define C
Definition: RandGen.cpp:25
ATTR_WARN_UNUSED_RESULT const BMVert * v
btVectorX< double > btVectorXd
Definition: btMatrixX.h:471
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:470
btVectorX< float > btVectorXf
Definition: btMatrixX.h:468
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:467
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:514
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:119
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
SIMD_FORCE_INLINE void btSetZero(T *a, int n)
Definition: btScalar.h:739
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_ONE
Definition: btScalar.h:545
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define btAssert(x)
Definition: btScalar.h:295
static T sum(const btAlignedObjectArray< T > &items)
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:263
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE void push_back(const T &_Val)
original version written by Erwin Coumans, October 2013
Definition: btMatrixX.h:31
bool operator()(const int &a, const int &b) const
Definition: btMatrixX.h:33
uint col
int count
#define T
#define B
static unsigned c
Definition: RandGen.cpp:83
static unsigned a[3]
Definition: RandGen.cpp:78
std::ostream & operator<<(std::ostream &stream, const AssetCatalogPath &path_to_append)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
void rowComputeNonZeroElements() const
Definition: btMatrixX.h:301
void setZero()
Definition: btMatrixX.h:262
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:160
int rows() const
Definition: btMatrixX.h:203
int m_cols
Definition: btMatrixX.h:155
btMatrixX transpose() const
Definition: btMatrixX.h:316
btMatrixX operator*(const btMatrixX &other)
Definition: btMatrixX.h:333
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:415
void mulElem(int row, int col, T val)
Definition: btMatrixX.h:235
btMatrixX negative()
Definition: btMatrixX.h:454
btMatrixX(int rows, int cols)
Definition: btMatrixX.h:180
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:161
void setIdentity()
Definition: btMatrixX.h:276
T * getBufferPointerWritable()
Definition: btMatrixX.h:163
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:429
const T & operator()(int row, int col) const
Definition: btMatrixX.h:257
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
int m_setElemOperations
Definition: btMatrixX.h:158
int m_resizeOperations
Definition: btMatrixX.h:157
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:392
int cols() const
Definition: btMatrixX.h:199
void copyLowerToUpperTriangle()
Definition: btMatrixX.h:243
void multiplyAdd2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:370
void printMatrix(const char *msg) const
Definition: btMatrixX.h:287
void resize(int rows, int cols)
Definition: btMatrixX.h:189
int m_operations
Definition: btMatrixX.h:156
void setElem(int row, int col, T val)
Definition: btMatrixX.h:229
int m_rows
Definition: btMatrixX.h:154
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition: btMatrixX.h:441
const T * getBufferPointer() const
Definition: btMatrixX.h:168
int size() const
Definition: btMatrixX.h:64
const T * getBufferPointer() const
Definition: btMatrixX.h:138
btVectorX(int numRows)
Definition: btMatrixX.h:47
T & operator[](int index)
Definition: btMatrixX.h:128
T nrm2() const
Definition: btMatrixX.h:69
void resize(int rows)
Definition: btMatrixX.h:52
btVectorX()
Definition: btMatrixX.h:44
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:42
const T & operator[](int index) const
Definition: btMatrixX.h:123
int rows() const
Definition: btMatrixX.h:60
int cols() const
Definition: btMatrixX.h:56
void setZero()
Definition: btMatrixX.h:113
T * getBufferPointerWritable()
Definition: btMatrixX.h:133