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

Bullet Collision Detection & Physics Library

btMatrixX.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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{
32public:
33 bool operator()(const int& a, const int& b) const
34 {
35 return a < b;
36 }
37};
38
39template <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
151template <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 }
207
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 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
454 btMatrixX negative()
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
467 typedef btMatrixX<float> btMatrixXf;
468 typedef btVectorX<float> btVectorXf;
469
470 typedef btMatrixX<double> btMatrixXd;
471 typedef btVectorX<double> btVectorXd;
472
473#ifdef BT_DEBUG_OSTREAM
474template <typename T>
475std::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}
494template <typename T>
495std::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
btAlignedObjectArray.h
btVectorXd
btVectorX< double > btVectorXd
Definition btMatrixX.h:471
btMatrixXd
btMatrixX< double > btMatrixXd
Definition btMatrixX.h:470
btVectorXf
btVectorX< float > btVectorXf
Definition btMatrixX.h:468
btMatrixXf
btMatrixX< float > btMatrixXf
Definition btMatrixX.h:467
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition btMatrixX.h:514
btQuickprof.h
BT_PROFILE
#define BT_PROFILE(name)
Definition btQuickprof.h:198
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btSetZero
void btSetZero(T *a, int n)
Definition btScalar.h:739
BT_ONE
#define BT_ONE
Definition btScalar.h:545
btFabs
btScalar btFabs(btScalar x)
Definition btScalar.h:497
btAssert
#define btAssert(x)
Definition btScalar.h:153
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition btSoftBodyHelpers.cpp:95
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition btAlignedObjectArray.h:46
btIntSortPredicate
original version written by Erwin Coumans, October 2013
Definition btMatrixX.h:31
btIntSortPredicate::operator()
bool operator()(const int &a, const int &b) const
Definition btMatrixX.h:33
btMatrixX
Definition btMatrixX.h:153
btMatrixX::rowComputeNonZeroElements
void rowComputeNonZeroElements() const
Definition btMatrixX.h:301
btMatrixX::setZero
void setZero()
Definition btMatrixX.h:262
btMatrixX< float >::m_storage
btAlignedObjectArray< float > m_storage
Definition btMatrixX.h:160
btMatrixX< float >::rows
int rows() const
Definition btMatrixX.h:203
btMatrixX< float >::m_cols
int m_cols
Definition btMatrixX.h:155
btMatrixX::transpose
btMatrixX transpose() const
Definition btMatrixX.h:316
btMatrixX::operator*
btMatrixX operator*(const btMatrixX &other)
Definition btMatrixX.h:333
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition btMatrixX.h:415
btMatrixX::btMatrixX
btMatrixX()
Definition btMatrixX.h:172
btMatrixX::mulElem
void mulElem(int row, int col, T val)
Definition btMatrixX.h:235
btMatrixX::negative
btMatrixX negative()
Definition btMatrixX.h:454
btMatrixX::btMatrixX
btMatrixX(int rows, int cols)
Definition btMatrixX.h:180
btMatrixX< float >::m_rowNonZeroElements1
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition btMatrixX.h:161
btMatrixX::setIdentity
void setIdentity()
Definition btMatrixX.h:276
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition btMatrixX.h:429
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
btMatrixX< float >::m_setElemOperations
int m_setElemOperations
Definition btMatrixX.h:158
btMatrixX< float >::m_resizeOperations
int m_resizeOperations
Definition btMatrixX.h:157
btMatrixX::getBufferPointer
const T * getBufferPointer() const
Definition btMatrixX.h:168
btMatrixX::multiply2_p8r
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition btMatrixX.h:392
btMatrixX< float >::cols
int cols() const
Definition btMatrixX.h:199
btMatrixX::copyLowerToUpperTriangle
void copyLowerToUpperTriangle()
Definition btMatrixX.h:243
btMatrixX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition btMatrixX.h:163
btMatrixX::multiplyAdd2_p8r
void multiplyAdd2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition btMatrixX.h:370
btMatrixX::printMatrix
void printMatrix(const char *msg) const
Definition btMatrixX.h:287
btMatrixX::operator()
const T & operator()(int row, int col) const
Definition btMatrixX.h:257
btMatrixX::resize
void resize(int rows, int cols)
Definition btMatrixX.h:189
btMatrixX< float >::m_operations
int m_operations
Definition btMatrixX.h:156
btMatrixX::setElem
void setElem(int row, int col, T val)
Definition btMatrixX.h:229
btMatrixX< float >::m_rows
int m_rows
Definition btMatrixX.h:154
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition btMatrixX.h:441
btVectorX
Definition btMatrixX.h:41
btVectorX::size
int size() const
Definition btMatrixX.h:64
btVectorX::btVectorX
btVectorX(int numRows)
Definition btMatrixX.h:47
btVectorX::nrm2
T nrm2() const
Definition btMatrixX.h:69
btVectorX::resize
void resize(int rows)
Definition btMatrixX.h:52
btVectorX::btVectorX
btVectorX()
Definition btMatrixX.h:44
btVectorX< float >::m_storage
btAlignedObjectArray< float > m_storage
Definition btMatrixX.h:42
btVectorX::operator[]
const T & operator[](int index) const
Definition btMatrixX.h:123
btVectorX< float >::rows
int rows() const
Definition btMatrixX.h:60
btVectorX::operator[]
T & operator[](int index)
Definition btMatrixX.h:128
btVectorX::getBufferPointer
const T * getBufferPointer() const
Definition btMatrixX.h:138
btVectorX::cols
int cols() const
Definition btMatrixX.h:56
btVectorX::setZero
void setZero()
Definition btMatrixX.h:113
btVectorX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition btMatrixX.h:133