Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / LinearMath / btMatrixX.h
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