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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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 //#define BT_DEBUG_OSTREAM
24 #ifdef BT_DEBUG_OSTREAM
25 #include <iostream>
26 #include <iomanip> // std::setw
27 #endif //BT_DEBUG_OSTREAM
28
29 class btIntSortPredicate
30 {
31 public:
32 bool operator() ( const int& a, const int& b ) const
33 {
34 return a < b;
35 }
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 }
114 void setZero()
115 {
116 if (m_storage.size())
117 {
118 // for (int i=0;i<m_storage.size();i++)
119 // m_storage[i]=0;
120 //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
121 btSetZero(&m_storage[0],m_storage.size());
122 }
123 }
124 const T& operator[] (int index) const
125 {
126 return m_storage[index];
127 }
128
129 T& operator[] (int index)
130 {
131 return m_storage[index];
132 }
133
134 T* getBufferPointerWritable()
135 {
136 return m_storage.size() ? &m_storage[0] : 0;
137 }
138
139 const T* getBufferPointer() const
140 {
141 return m_storage.size() ? &m_storage[0] : 0;
142 }
143
144 };
145 /*
146 template <typename T>
147 void setElem(btMatrixX<T>& mat, int row, int col, T val)
148 {
149 mat.setElem(row,col,val);
150 }
151 */
152
153
154 template <typename T>
155 struct btMatrixX
156 {
157 int m_rows;
158 int m_cols;
159 int m_operations;
160 int m_resizeOperations;
161 int m_setElemOperations;
162
163 btAlignedObjectArray<T> m_storage;
164 mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
165
166 T* getBufferPointerWritable()
167 {
168 return m_storage.size() ? &m_storage[0] : 0;
169 }
170
171 const T* getBufferPointer() const
172 {
173 return m_storage.size() ? &m_storage[0] : 0;
174 }
175 btMatrixX()
176 :m_rows(0),
177 m_cols(0),
178 m_operations(0),
179 m_resizeOperations(0),
180 m_setElemOperations(0)
181 {
182 }
183 btMatrixX(int rows,int cols)
184 :m_rows(rows),
185 m_cols(cols),
186 m_operations(0),
187 m_resizeOperations(0),
188 m_setElemOperations(0)
189 {
190 resize(rows,cols);
191 }
192 void resize(int rows, int cols)
193 {
194 m_resizeOperations++;
195 m_rows = rows;
196 m_cols = cols;
197 {
198 BT_PROFILE("m_storage.resize");
199 m_storage.resize(rows*cols);
200 }
201 }
202 int cols() const
203 {
204 return m_cols;
205 }
206 int rows() const
207 {
208 return m_rows;
209 }
211 /*T& operator() (int row,int col)
212 {
213 return m_storage[col*m_rows+row];
214 }
215 */
216
217 void addElem(int row,int col, T val)
218 {
219 if (val)
220 {
221 if (m_storage[col+row*m_cols]==0.f)
222 {
223 setElem(row,col,val);
224 } else
225 {
226 m_storage[row*m_cols+col] += val;
227 }
228 }
229 }
230
231
232 void setElem(int row,int col, T val)
233 {
234 m_setElemOperations++;
235 m_storage[row*m_cols+col] = val;
236 }
237
238 void mulElem(int row,int col, T val)
239 {
240 m_setElemOperations++;
241 //mul doesn't change sparsity info
242
243 m_storage[row*m_cols+col] *= val;
244 }
245
246
247
248
249 void copyLowerToUpperTriangle()
250 {
251 int count=0;
252 for (int row=0;row<rows();row++)
253 {
254 for (int col=0;col<row;col++)
255 {
256 setElem(col,row, (*this)(row,col));
257 count++;
258
259 }
260 }
261 //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
262 }
263
264 const T& operator() (int row,int col) const
265 {
266 return m_storage[col+row*m_cols];
267 }
268
269
270 void setZero()
271 {
272 {
273 BT_PROFILE("storage=0");
274 btSetZero(&m_storage[0],m_storage.size());
275 //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
276 //for (int i=0;i<m_storage.size();i++)
277 // m_storage[i]=0;
278 }
279 }
280
281 void setIdentity()
282 {
283 btAssert(rows() == cols());
284
285 setZero();
286 for (int row=0;row<rows();row++)
287 {
288 setElem(row,row,1);
289 }
290 }
291
292
293
294 void printMatrix(const char* msg)
295 {
296 printf("%s ---------------------\n",msg);
297 for (int i=0;i<rows();i++)
298 {
299 printf("\n");
300 for (int j=0;j<cols();j++)
301 {
302 printf("%2.1f\t",(*this)(i,j));
303 }
304 }
305 printf("\n---------------------\n");
306
307 }
308
309
310 void rowComputeNonZeroElements() const
311 {
312 m_rowNonZeroElements1.resize(rows());
313 for (int i=0;i<rows();i++)
314 {
315 m_rowNonZeroElements1[i].resize(0);
316 for (int j=0;j<cols();j++)
317 {
318 if ((*this)(i,j)!=0.f)
319 {
320 m_rowNonZeroElements1[i].push_back(j);
321 }
322 }
323 }
324 }
325 btMatrixX transpose() const
326 {
327 //transpose is optimized for sparse matrices
328 btMatrixX tr(m_cols,m_rows);
329 tr.setZero();
330 for (int i=0;i<m_cols;i++)
331 for (int j=0;j<m_rows;j++)
332 {
333 T v = (*this)(j,i);
334 if (v)
335 {
336 tr.setElem(i,j,v);
337 }
338 }
339 return tr;
340 }
341
342
343 btMatrixX operator*(const btMatrixX& other)
344 {
345 //btMatrixX*btMatrixX implementation, brute force
346 btAssert(cols() == other.rows());
347
348 btMatrixX res(rows(),other.cols());
349 res.setZero();
350 // BT_PROFILE("btMatrixX mul");
351 for (int j=0; j < res.cols(); ++j)
352 {
353 {
354 for (int i=0; i < res.rows(); ++i)
355 {
356 T dotProd=0;
357 // T dotProd2=0;
358 //int waste=0,waste2=0;
359
360 {
361 // bool useOtherCol = true;
362 {
363 for (int v=0;v<rows();v++)
364 {
365 T w = (*this)(i,v);
366 if (other(v,j)!=0.f)
367 {
368 dotProd+=w*other(v,j);
369 }
370
371 }
372 }
373 }
374 if (dotProd)
375 res.setElem(i,j,dotProd);
376 }
377 }
378 }
379 return res;
380 }
381
382 // this assumes the 4th and 8th rows of B and C are zero.
383 void multiplyAdd2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther ,int row, int col)
384 {
385 const btScalar *bb = B;
386 for ( int i = 0;i<numRows;i++)
387 {
388 const btScalar *cc = C;
389 for ( int j = 0;j<numRowsOther;j++)
390 {
391 btScalar sum;
392 sum = bb[0]*cc[0];
393 sum += bb[1]*cc[1];
394 sum += bb[2]*cc[2];
395 sum += bb[4]*cc[4];
396 sum += bb[5]*cc[5];
397 sum += bb[6]*cc[6];
398 addElem(row+i,col+j,sum);
399 cc += 8;
400 }
401 bb += 8;
402 }
403 }
404
405 void multiply2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
406 {
407 btAssert (numRows>0 && numRowsOther>0 && B && C);
408 const btScalar *bb = B;
409 for ( int i = 0;i<numRows;i++)
410 {
411 const btScalar *cc = C;
412 for ( int j = 0;j<numRowsOther;j++)
413 {
414 btScalar sum;
415 sum = bb[0]*cc[0];
416 sum += bb[1]*cc[1];
417 sum += bb[2]*cc[2];
418 sum += bb[4]*cc[4];
419 sum += bb[5]*cc[5];
420 sum += bb[6]*cc[6];
421 setElem(row+i,col+j,sum);
422 cc += 8;
423 }
424 bb += 8;
425 }
426 }
427
428 void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
429 {
430 int numRows = rowend+1-rowstart;
431 int numCols = colend+1-colstart;
432
433 for (int row=0;row<numRows;row++)
434 {
435 for (int col=0;col<numCols;col++)
436 {
437 setElem(rowstart+row,colstart+col,value);
438 }
439 }
440 }
441
442 void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& 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,col));
451 }
452 }
453 }
454 void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
455 {
456 btAssert(rowend+1-rowstart == block.rows());
457 btAssert(colend+1-colstart == block.cols());
458 for (int row=0;row<block.rows();row++)
459 {
460 for (int col=0;col<block.cols();col++)
461 {
462 setElem(rowstart+row,colstart+col,block[row]);
463 }
464 }
465 }
466
467
468 btMatrixX negative()
469 {
470 btMatrixX neg(rows(),cols());
471 for (int i=0;i<rows();i++)
472 for (int j=0;j<cols();j++)
473 {
474 T v = (*this)(i,j);
475 neg.setElem(i,j,-v);
476 }
477 return neg;
478 }
479
480 };
481
482
483
484 typedef btMatrixX<float> btMatrixXf;
485 typedef btVectorX<float> btVectorXf;
486
487 typedef btMatrixX<double> btMatrixXd;
488 typedef btVectorX<double> btVectorXd;
489
490
491 #ifdef BT_DEBUG_OSTREAM
492 template <typename T>
493 std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
494 {
495
496 os << " [";
497 //printf("%s ---------------------\n",msg);
498 for (int i=0;i<mat.rows();i++)
499 {
500 for (int j=0;j<mat.cols();j++)
501 {
502 os << std::setw(12) << mat(i,j);
503 }
504 if (i!=mat.rows()-1)
505 os << std::endl << " ";
506 }
507 os << " ]";
508 //printf("\n---------------------\n");
509
510 return os;
511 }
512 template <typename T>
513 std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
514 {
515
516 os << " [";
517 //printf("%s ---------------------\n",msg);
518 for (int i=0;i<mat.rows();i++)
519 {
520 os << std::setw(12) << mat[i];
521 if (i!=mat.rows()-1)
522 os << std::endl << " ";
523 }
524 os << " ]";
525 //printf("\n---------------------\n");
526
527 return os;
528 }
529
530 #endif //BT_DEBUG_OSTREAM
531
532
533 inline void setElem(btMatrixXd& mat, int row, int col, double val)
534 {
535 mat.setElem(row,col,val);
536 }
537
538 inline void setElem(btMatrixXf& mat, int row, int col, float val)
539 {
540 mat.setElem(row,col,val);
541 }
542
543 #ifdef BT_USE_DOUBLE_PRECISION
544 #define btVectorXu btVectorXd
545 #define btMatrixXu btMatrixXd
546 #else
547 #define btVectorXu btVectorXf
548 #define btMatrixXu btMatrixXf
549 #endif //BT_USE_DOUBLE_PRECISION
550
551
552
553 #endif//BT_MATRIX_H_H
btMatrixX::setElem
void setElem(int row, int col, T val)
Definition: btMatrixX.h:232
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
btVectorX::resize
void resize(int rows)
Definition: btMatrixX.h:52
btVectorXd
btVectorX< double > btVectorXd
Definition: btMatrixX.h:488
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:276
btMatrixX::getBufferPointer
const T * getBufferPointer() const
Definition: btMatrixX.h:171
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:163
btIntSortPredicate::operator()
bool operator()(const int &a, const int &b) const
Definition: btMatrixX.h:32
btAlignedObjectArray
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
Definition: btAlignedObjectArray.h:47
btVectorXf
btVectorX< float > btVectorXf
Definition: btMatrixX.h:485
BT_ONE
#define BT_ONE
Definition: btScalar.h:496
btVectorX::operator[]
const T & operator[](int index) const
Definition: btMatrixX.h:124
btVectorX::setZero
void setZero()
Definition: btMatrixX.h:114
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMatrixX::mulElem
void mulElem(int row, int col, T val)
Definition: btMatrixX.h:238
btVectorX::nrm2
T nrm2() const
Definition: btMatrixX.h:69
btIntSortPredicate
original version written by Erwin Coumans, October 2013
Definition: btMatrixX.h:29
btMatrixX::m_resizeOperations
int m_resizeOperations
Definition: btMatrixX.h:160
btMatrixX::printMatrix
void printMatrix(const char *msg)
Definition: btMatrixX.h:294
btVectorX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:134
btVectorX::rows
int rows() const
Definition: btMatrixX.h:60
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:442
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:217
btMatrixX::setIdentity
void setIdentity()
Definition: btMatrixX.h:281
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btMatrixX::m_setElemOperations
int m_setElemOperations
Definition: btMatrixX.h:161
btVectorX::btVectorX
btVectorX(int numRows)
Definition: btMatrixX.h:47
btMatrixX::rows
int rows() const
Definition: btMatrixX.h:206
btMatrixX::m_operations
int m_operations
Definition: btMatrixX.h:159
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition: btMatrixX.h:454
btMatrixX::getBufferPointerWritable
T * getBufferPointerWritable()
Definition: btMatrixX.h:166
btVectorX::size
int size() const
Definition: btMatrixX.h:64
btMatrixX
Definition: btMatrixX.h:155
btSetZero
void btSetZero(T *a, int n)
Definition: btScalar.h:684
btMatrixX::setZero
void setZero()
Definition: btMatrixX.h:270
btMatrixXd
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:487
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
btMatrixX::multiply2_p8r
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:405
btMatrixX::setSubMatrix
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:428
btMatrixXf
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:484
btMatrixX::btMatrixX
btMatrixX()
Definition: btMatrixX.h:175
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:220
btQuickprof.h
btMatrixX::rowComputeNonZeroElements
void rowComputeNonZeroElements() const
Definition: btMatrixX.h:310
btMatrixX::operator()
const T & operator()(int row, int col) const
Definition: btMatrixX.h:264
setElem
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:533
btVectorX::getBufferPointer
const T * getBufferPointer() const
Definition: btMatrixX.h:139
btVectorX
Definition: btMatrixX.h:40
btMatrixX::m_rows
int m_rows
Definition: btMatrixX.h:157
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:383
btMatrixX::m_cols
int m_cols
Definition: btMatrixX.h:158
btMatrixX::negative
btMatrixX negative()
Definition: btMatrixX.h:468
btVectorX::cols
int cols() const
Definition: btMatrixX.h:56
btMatrixX::cols
int cols() const
Definition: btMatrixX.h:202
btMatrixX::copyLowerToUpperTriangle
void copyLowerToUpperTriangle()
Definition: btMatrixX.h:249
btMatrixX::resize
void resize(int rows, int cols)
Definition: btMatrixX.h:192
btMatrixX::operator*
btMatrixX operator*(const btMatrixX &other)
Definition: btMatrixX.h:343
btMatrixX::transpose
btMatrixX transpose() const
Definition: btMatrixX.h:325
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMatrixX::m_rowNonZeroElements1
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:164
btMatrixX::btMatrixX
btMatrixX(int rows, int cols)
Definition: btMatrixX.h:183
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449