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