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{
32public:
33 bool operator()(const int& a, const int& b) const
34 {
35 return a < b;
36 }
37};
38
39template <typename T>
41{
43
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
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>
153{
159
162
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 }
173 : m_rows(0),
174 m_cols(0),
175 m_operations(0),
178 {
179 }
181 : m_rows(rows),
182 m_cols(cols),
183 m_operations(0),
186 {
187 resize(rows, cols);
188 }
189 void resize(int rows, int cols)
190 {
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 {
232 m_storage[row * m_cols + col] = val;
233 }
234
235 void mulElem(int row, int col, T val)
236 {
238 //mul doesn't change sparsity info
239
240 m_storage[row * m_cols + col] *= val;
241 }
242
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
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
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 }
317 {
318 //transpose is optimized for sparse matrices
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
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 {
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 {
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
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
470
473
474#ifdef BT_DEBUG_OSTREAM
475template <typename T>
476std::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}
495template <typename T>
496std::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
515inline void setElem(btMatrixXd& mat, int row, int col, double val)
516{
517 mat.setElem(row, col, val);
518}
519
520inline 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
btVectorX< double > btVectorXd
Definition: btMatrixX.h:472
btMatrixX< double > btMatrixXd
Definition: btMatrixX.h:471
btVectorX< float > btVectorXf
Definition: btMatrixX.h:469
btMatrixX< float > btMatrixXf
Definition: btMatrixX.h:468
void setElem(btMatrixXd &mat, int row, int col, double val)
Definition: btMatrixX.h:515
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
void btSetZero(T *a, int n)
Definition: btScalar.h:739
#define BT_ONE
Definition: btScalar.h:545
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
#define btAssert(x)
Definition: btScalar.h:153
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:301
void setZero()
Definition: btMatrixX.h:262
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:160
int rows() const
Definition: btMatrixX.h:203
int m_cols
Definition: btMatrixX.h:155
btMatrixX transpose() const
Definition: btMatrixX.h:316
btMatrixX operator*(const btMatrixX &other)
Definition: btMatrixX.h:333
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
Definition: btMatrixX.h:416
void mulElem(int row, int col, T val)
Definition: btMatrixX.h:235
btMatrixX negative()
Definition: btMatrixX.h:455
btMatrixX(int rows, int cols)
Definition: btMatrixX.h:180
btAlignedObjectArray< btAlignedObjectArray< int > > m_rowNonZeroElements1
Definition: btMatrixX.h:161
void setIdentity()
Definition: btMatrixX.h:276
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX &block)
Definition: btMatrixX.h:430
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
int m_setElemOperations
Definition: btMatrixX.h:158
int m_resizeOperations
Definition: btMatrixX.h:157
const T * getBufferPointer() const
Definition: btMatrixX.h:168
void multiply2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:393
int cols() const
Definition: btMatrixX.h:199
void copyLowerToUpperTriangle()
Definition: btMatrixX.h:243
T * getBufferPointerWritable()
Definition: btMatrixX.h:163
void multiplyAdd2_p8r(const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
Definition: btMatrixX.h:371
void printMatrix(const char *msg) const
Definition: btMatrixX.h:287
const T & operator()(int row, int col) const
Definition: btMatrixX.h:257
void resize(int rows, int cols)
Definition: btMatrixX.h:189
int m_operations
Definition: btMatrixX.h:156
void setElem(int row, int col, T val)
Definition: btMatrixX.h:229
int m_rows
Definition: btMatrixX.h:154
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX< T > &block)
Definition: btMatrixX.h:442
int size() const
Definition: btMatrixX.h:64
btVectorX(int numRows)
Definition: btMatrixX.h:47
T nrm2() const
Definition: btMatrixX.h:69
void resize(int rows)
Definition: btMatrixX.h:52
btVectorX()
Definition: btMatrixX.h:44
btAlignedObjectArray< T > m_storage
Definition: btMatrixX.h:42
const T & operator[](int index) const
Definition: btMatrixX.h:123
int rows() const
Definition: btMatrixX.h:60
T & operator[](int index)
Definition: btMatrixX.h:128
const T * getBufferPointer() const
Definition: btMatrixX.h:138
int cols() const
Definition: btMatrixX.h:56
void setZero()
Definition: btMatrixX.h:113
T * getBufferPointerWritable()
Definition: btMatrixX.h:133