Bullet Collision Detection & Physics Library: btMatrixX.h Source File

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 #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 }
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 r = rows();
350 int c = cols();
351
352 for (int k = 0; k < cols(); k++)
353 {
354 T w = (*this)(i, k);
355 if (other(k, j) != 0.f)
356 {
357 dotProd += w * other(k, j);
358 }
359 }
360 }
361 }
362 if (dotProd)
363 res.setElem(i, j, dotProd);
364 }
365 }
366 }
367 return res;
368 }
369
370 // this assumes the 4th and 8th rows of B and C are zero.
371 void multiplyAdd2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
372 {
373 const btScalar* bb = B;
374 for (int i = 0; i < numRows; i++)
375 {
376 const btScalar* cc = C;
377 for (int j = 0; j < numRowsOther; j++)
378 {
379 btScalar sum;
380 sum = bb[0] * cc[0];
381 sum += bb[1] * cc[1];
382 sum += bb[2] * cc[2];
383 sum += bb[4] * cc[4];
384 sum += bb[5] * cc[5];
385 sum += bb[6] * cc[6];
386 addElem(row + i, col + j, sum);
387 cc += 8;
388 }
389 bb += 8;
390 }
391 }
392
393 void multiply2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
394 {
395 btAssert(numRows > 0 && numRowsOther > 0 && B && C);
396 const btScalar* bb = B;
397 for (int i = 0; i < numRows; i++)
398 {
399 const btScalar* cc = C;
400 for (int j = 0; j < numRowsOther; j++)
401 {
402 btScalar sum;
403 sum = bb[0] * cc[0];
404 sum += bb[1] * cc[1];
405 sum += bb[2] * cc[2];
406 sum += bb[4] * cc[4];
407 sum += bb[5] * cc[5];
408 sum += bb[6] * cc[6];
409 setElem(row + i, col + j, sum);
410 cc += 8;
411 }
412 bb += 8;
413 }
414 }
415
416 void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
417 {
418 int numRows = rowend + 1 - rowstart;
419 int numCols = colend + 1 - colstart;
420
421 for (int row = 0; row < numRows; row++)
422 {
423 for (int col = 0; col < numCols; col++)
424 {
425 setElem(rowstart + row, colstart + col, value);
426 }
427 }
428 }
429
430 void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX& block)
431 {
432 btAssert(rowend + 1 - rowstart == block.rows());
433 btAssert(colend + 1 - colstart == block.cols());
434 for (int row = 0; row < block.rows(); row++)
435 {
436 for (int col = 0; col < block.cols(); col++)
437 {
438 setElem(rowstart + row, colstart + col, block(row, col));
439 }
440 }
441 }
442 void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX<T>& 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]);
451 }
452 }
453 }
454
455 btMatrixX negative()
456 {
457 btMatrixX neg(rows(), cols());
458 for (int i = 0; i < rows(); i++)
459 for (int j = 0; j < cols(); j++)
460 {
461 T v = (*this)(i, j);
462 neg.setElem(i, j, -v);
463 }
464 return neg;
465 }
466 };
467
468 typedef btMatrixX<float> btMatrixXf;
469 typedef btVectorX<float> btVectorXf;
470
471 typedef btMatrixX<double> btMatrixXd;
472 typedef btVectorX<double> btVectorXd;
473
474 #ifdef BT_DEBUG_OSTREAM
475 template <typename T>
476 std::ostream& operator<<(std::ostream& os, const btMatrixX<T>& mat)
477 {
478 os << " [";
479 //printf("%s ---------------------\n",msg);
480 for (int i = 0; i < mat.rows(); i++)
481 {
482 for (int j = 0; j < mat.cols(); j++)
483 {
484 os << std::setw(12) << mat(i, j);
485 }
486 if (i != mat.rows() - 1)
487 os << std::endl
488 << " ";
489 }
490 os << " ]";
491 //printf("\n---------------------\n");
492
493 return os;
494 }
495 template <typename T>
496 std::ostream& operator<<(std::ostream& os, const btVectorX<T>& mat)
497 {
498 os << " [";
499 //printf("%s ---------------------\n",msg);
500 for (int i = 0; i < mat.rows(); i++)
501 {
502 os << std::setw(12) << mat[i];
503 if (i != mat.rows() - 1)
504 os << std::endl
505 << " ";
506 }
507 os << " ]";
508 //printf("\n---------------------\n");
509
510 return os;
511 }
512
513 #endif //BT_DEBUG_OSTREAM
514
515 inline void setElem(btMatrixXd& mat, int row, int col, double val)
516 {
517 mat.setElem(row, col, val);
518 }
519
520 inline void setElem(btMatrixXf& mat, int row, int col, float val)
521 {
522 mat.setElem(row, col, val);
523 }
524
525 #ifdef BT_USE_DOUBLE_PRECISION
526 #define btVectorXu btVectorXd
527 #define btMatrixXu btMatrixXd
528 #else
529 #define btVectorXu btVectorXf
530 #define btMatrixXu btMatrixXf
531 #endif //BT_USE_DOUBLE_PRECISION
532
533 #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:371
btMatrixX::m_storage
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:160
btVectorXd
btVectorX< double > btVectorXd
Definition: btMatrixX.h:472
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:416
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:314
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:333
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:301
btVectorX::btVectorX
btVectorX(int numRows)
Definition: btMatrixX.h:47
btVectorX
Definition: btMatrixX.h:41
btMatrixX::btMatrixX
btMatrixX()
Definition: btMatrixX.h:172
btMatrixX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:163
BT_ONE
#define BT_ONE
Definition: btScalar.h:545
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btMatrixX::m_rowNonZeroElements1
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:161
btVectorXf
btVectorX< float > btVectorXf
Definition: btMatrixX.h:469
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btVectorX::rows
int rows() const
Definition: btMatrixX.h:60
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btMatrixX::negative
btMatrixX negative()
Definition: btMatrixX.h:455
btVectorX::m_storage
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:42
btMatrixXd
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:471
btIntSortPredicate
original version written by Erwin Coumans, October 2013
Definition: btMatrixX.h:31
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:46
btMatrixX::setIdentity
void setIdentity()
Definition: btMatrixX.h:276
btMatrixXf
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:468
btMatrixX::multiply2_p8r
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:393
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:739
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:442
btMatrixX::rows
int rows() const
Definition: btMatrixX.h:203
btVectorX::setZero
void setZero()
Definition: btMatrixX.h:113
btMatrixX::printMatrix
void printMatrix(const char *msg) const
Definition: btMatrixX.h:287
btMatrixX
Definition: btMatrixX.h:153
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:430
btAlignedObjectArray.h
btMatrixX::transpose
btMatrixX transpose() const
Definition: btMatrixX.h:316
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:515
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
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:198
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
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