?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Bullet 2.85 update
[Torque-3d.git] / Engine / lib / bullet / src / LinearMath / btMatrixX.h
blob42caed42eff9dbd5bd60628b27aaa5a2010cbe9a
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 */
15 ///original version written by Erwin Coumans, October 2013
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
40 template <typename T>
41 struct btVectorX
42 {
43 btAlignedObjectArray<T> m_storage;
44
45 btVectorX()
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
135 T* getBufferPointerWritable()
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
155 template <typename T>
156 struct btMatrixX
157 {
158 int m_rows;
159 int m_cols;
160 int m_operations;
161 int m_resizeOperations;
162 int m_setElemOperations;
163
164 btAlignedObjectArray<T> m_storage;
165 mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
166
167 T* getBufferPointerWritable()
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 }
176 btMatrixX()
177 :m_rows(0),
178 m_cols(0),
179 m_operations(0),
180 m_resizeOperations(0),
181 m_setElemOperations(0)
182 {
183 }
184 btMatrixX(int rows,int cols)
185 :m_rows(rows),
186 m_cols(cols),
187 m_operations(0),
188 m_resizeOperations(0),
189 m_setElemOperations(0)
190 {
191 resize(rows,cols);
192 }
193 void resize(int rows, int cols)
194 {
195 m_resizeOperations++;
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 }
211 ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead
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 {
235 m_setElemOperations++;
236 m_storage[row*m_cols+col] = val;
237 }
238
239 void mulElem(int row,int col, T val)
240 {
241 m_setElemOperations++;
242 //mul doesn't change sparsity info
243
244 m_storage[row*m_cols+col] *= val;
245 }
246
247
248
249
250 void copyLowerToUpperTriangle()
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
282 void setIdentity()
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
311 void rowComputeNonZeroElements() const
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 }
326 btMatrixX transpose() const
327 {
328 //transpose is optimized for sparse matrices
329 btMatrixX tr(m_cols,m_rows);
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
344 btMatrixX operator*(const btMatrixX& other)
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 {
392 btScalar sum;
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 {
415 btScalar sum;
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
469 btMatrixX negative()
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
485 typedef btMatrixX<float> btMatrixXf;
486 typedef btVectorX<float> btVectorXf;
487
488 typedef btMatrixX<double> btMatrixXd;
489 typedef btVectorX<double> btVectorXd;
490
491
492 #ifdef BT_DEBUG_OSTREAM
493 template <typename T>
494 std::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 }
513 template <typename T>
514 std::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
534 inline void setElem(btMatrixXd& mat, int row, int col, double val)
535 {
536 mat.setElem(row,col,val);
537 }
538
539 inline 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