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 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
298 void rowComputeNonZeroElements() const
299 {
300 m_rowNonZeroElements1.resize(rows());
301 for (int i = 0; i < rows(); i++)
302 {
303 m_rowNonZeroElements1[i].resize(0);
304 for (int j = 0; j < cols(); j++)
305 {
306 if ((*this)(i, j) != 0.f)
307 {
308 m_rowNonZeroElements1[i].push_back(j);
309 }
310 }
311 }
312 }
313 btMatrixX transpose() const
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
330 btMatrixX operator*(const btMatrixX& other)
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
453 btMatrixX negative()
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
466 typedef btMatrixX<float> btMatrixXf;
467 typedef btVectorX<float> btVectorXf;
468
469 typedef btMatrixX<double> btMatrixXd;
470 typedef btVectorX<double> btVectorXd;
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::setElem
void setElem(int row, int col, T val)
Definition: btMatrixX.h:229
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:89
btVectorX::resize
void resize(int rows)
Definition: btMatrixX.h:52
btVectorXd
btVectorX< double > btVectorXd
Definition: btMatrixX.h:470
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:264
btVectorX::m_storage
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:42
btVectorX::btVectorX
btVectorX()
Definition: btMatrixX.h:44
btMatrixX::m_storage
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:160
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:52
btIntSortPredicate::operator()
bool operator()(const int &a, const int &b) const
Definition: btMatrixX.h:33
btVectorXf
btVectorX< float > btVectorXf
Definition: btMatrixX.h:467
BT_ONE
#define BT_ONE
Definition: btScalar.h:525
btVectorX::setZero
void setZero()
Definition: btMatrixX.h:113
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMatrixX::mulElem
void mulElem(int row, int col, T val)
Definition: btMatrixX.h:235
btIntSortPredicate
original version written by Erwin Coumans, October 2013
Definition: btMatrixX.h:30
btMatrixX::m_resizeOperations
int m_resizeOperations
Definition: btMatrixX.h:157
btMatrixX::printMatrix
void printMatrix(const char *msg)
Definition: btMatrixX.h:284
btVectorX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:133
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:428
btMatrixX::addElem
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:214
btMatrixX::setIdentity
void setIdentity()
Definition: btMatrixX.h:273
btVectorX::size
int size() const
Definition: btMatrixX.h:64
btMatrixX::m_setElemOperations
int m_setElemOperations
Definition: btMatrixX.h:158
btVectorX::operator[]
const T & operator[](int index) const
Definition: btMatrixX.h:123
btVectorX::btVectorX
btVectorX(int numRows)
Definition: btMatrixX.h:47
btVectorX::getBufferPointer
const T * getBufferPointer() const
Definition: btMatrixX.h:138
btMatrixX::m_operations
int m_operations
Definition: btMatrixX.h:156
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition: btMatrixX.h:440
btMatrixX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:163
btMatrixX
Definition: btMatrixX.h:152
btSetZero
void btSetZero(T *a, int n)
Definition: btScalar.h:719
btMatrixX::cols
int cols() const
Definition: btMatrixX.h:199
btMatrixX::rows
int rows() const
Definition: btMatrixX.h:203
btVectorX::rows
int rows() const
Definition: btMatrixX.h:60
btMatrixX::setZero
void setZero()
Definition: btMatrixX.h:262
btMatrixXd
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:469
btMatrixX::transpose
btMatrixX transpose() const
Definition: btMatrixX.h:313
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
btMatrixX::multiply2_p8r
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:391
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:414
btMatrixX::rowComputeNonZeroElements
void rowComputeNonZeroElements() const
Definition: btMatrixX.h:298
btMatrixX::getBufferPointer
const T * getBufferPointer() const
Definition: btMatrixX.h:168
btMatrixXf
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:466
btMatrixX::btMatrixX
btMatrixX()
Definition: btMatrixX.h:172
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btQuickprof.h
btVectorX::operator[]
T & operator[](int index)
Definition: btMatrixX.h:128
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:513
btVectorX
Definition: btMatrixX.h:40
btMatrixX::operator()
const T & operator()(int row, int col) const
Definition: btMatrixX.h:257
btMatrixX::m_rows
int m_rows
Definition: btMatrixX.h:154
btAlignedObjectArray.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_cols
int m_cols
Definition: btMatrixX.h:155
btMatrixX::negative
btMatrixX negative()
Definition: btMatrixX.h:453
btMatrixX::copyLowerToUpperTriangle
void copyLowerToUpperTriangle()
Definition: btMatrixX.h:243
btMatrixX::resize
void resize(int rows, int cols)
Definition: btMatrixX.h:189
btVectorX::cols
int cols() const
Definition: btMatrixX.h:56
btMatrixX::operator*
btMatrixX operator*(const btMatrixX &other)
Definition: btMatrixX.h:330
btMatrixX::m_rowNonZeroElements1
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:161
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::btMatrixX
btMatrixX(int rows, int cols)
Definition: btMatrixX.h:180
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:477