diff options
Diffstat (limited to 'thirdparty/bullet/LinearMath/btMatrixX.h')
| -rw-r--r-- | thirdparty/bullet/LinearMath/btMatrixX.h | 387 |
1 files changed, 182 insertions, 205 deletions
diff --git a/thirdparty/bullet/LinearMath/btMatrixX.h b/thirdparty/bullet/LinearMath/btMatrixX.h index 42caed42ef..9df9e49469 100644 --- a/thirdparty/bullet/LinearMath/btMatrixX.h +++ b/thirdparty/bullet/LinearMath/btMatrixX.h @@ -24,24 +24,23 @@ subject to the following restrictions: //#define BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM #include <iostream> -#include <iomanip> // std::setw -#endif //BT_DEBUG_OSTREAM +#include <iomanip> // std::setw +#endif //BT_DEBUG_OSTREAM class btIntSortPredicate { - public: - bool operator() ( const int& a, const int& b ) const - { - return a < b; - } +public: + bool operator()(const int& a, const int& b) const + { + return a < b; + } }; - template <typename T> struct btVectorX { - btAlignedObjectArray<T> m_storage; - + btAlignedObjectArray<T> m_storage; + btVectorX() { } @@ -49,7 +48,7 @@ struct btVectorX { m_storage.resize(numRows); } - + void resize(int rows) { m_storage.resize(rows); @@ -66,13 +65,13 @@ struct btVectorX { return rows(); } - + T nrm2() const { T norm = T(0); - + int nn = rows(); - + { if (nn == 1) { @@ -82,11 +81,11 @@ struct btVectorX { T scale = 0.0; T ssq = 1.0; - + /* The following loop is equivalent to this call to the LAPACK auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */ - - for (int ix=0;ix<nn;ix++) + + for (int ix = 0; ix < nn; ix++) { if ((*this)[ix] != 0.0) { @@ -110,38 +109,36 @@ struct btVectorX } } return norm; - } - void setZero() + void setZero() { if (m_storage.size()) { // for (int i=0;i<m_storage.size();i++) // m_storage[i]=0; //memset(&m_storage[0],0,sizeof(T)*m_storage.size()); - btSetZero(&m_storage[0],m_storage.size()); + btSetZero(&m_storage[0], m_storage.size()); } } - const T& operator[] (int index) const + const T& operator[](int index) const { return m_storage[index]; } - - T& operator[] (int index) + + T& operator[](int index) { return m_storage[index]; } - + T* getBufferPointerWritable() { return m_storage.size() ? &m_storage[0] : 0; } - + const T* getBufferPointer() const { return m_storage.size() ? &m_storage[0] : 0; } - }; /* template <typename T> @@ -151,8 +148,7 @@ struct btVectorX } */ - -template <typename T> +template <typename T> struct btMatrixX { int m_rows; @@ -161,10 +157,10 @@ struct btMatrixX int m_resizeOperations; int m_setElemOperations; - btAlignedObjectArray<T> m_storage; - mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1; + btAlignedObjectArray<T> m_storage; + mutable btAlignedObjectArray<btAlignedObjectArray<int> > m_rowNonZeroElements1; - T* getBufferPointerWritable() + T* getBufferPointerWritable() { return m_storage.size() ? &m_storage[0] : 0; } @@ -174,21 +170,21 @@ struct btMatrixX return m_storage.size() ? &m_storage[0] : 0; } btMatrixX() - :m_rows(0), - m_cols(0), - m_operations(0), - m_resizeOperations(0), - m_setElemOperations(0) + : m_rows(0), + m_cols(0), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) { } - btMatrixX(int rows,int cols) - :m_rows(rows), - m_cols(cols), - m_operations(0), - m_resizeOperations(0), - m_setElemOperations(0) + btMatrixX(int rows, int cols) + : m_rows(rows), + m_cols(cols), + m_operations(0), + m_resizeOperations(0), + m_setElemOperations(0) { - resize(rows,cols); + resize(rows, cols); } void resize(int rows, int cols) { @@ -197,7 +193,7 @@ struct btMatrixX m_cols = cols; { BT_PROFILE("m_storage.resize"); - m_storage.resize(rows*cols); + m_storage.resize(rows * cols); } } int cols() const @@ -215,108 +211,99 @@ struct btMatrixX } */ - void addElem(int row,int col, T val) + void addElem(int row, int col, T val) { if (val) { - if (m_storage[col+row*m_cols]==0.f) + if (m_storage[col + row * m_cols] == 0.f) { - setElem(row,col,val); - } else + setElem(row, col, val); + } + else { - m_storage[row*m_cols+col] += val; + m_storage[row * m_cols + col] += val; } } } - - - void setElem(int row,int col, T val) + + void setElem(int row, int col, T val) { m_setElemOperations++; - m_storage[row*m_cols+col] = val; + m_storage[row * m_cols + col] = val; } - - void mulElem(int row,int col, T val) + + void mulElem(int row, int col, T val) { m_setElemOperations++; //mul doesn't change sparsity info - m_storage[row*m_cols+col] *= val; + m_storage[row * m_cols + col] *= val; } - - - - + void copyLowerToUpperTriangle() { - int count=0; - for (int row=0;row<rows();row++) + int count = 0; + for (int row = 0; row < rows(); row++) { - for (int col=0;col<row;col++) + for (int col = 0; col < row; col++) { - setElem(col,row, (*this)(row,col)); + setElem(col, row, (*this)(row, col)); count++; - } } //printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows()); } - - const T& operator() (int row,int col) const + + const T& operator()(int row, int col) const { - return m_storage[col+row*m_cols]; + return m_storage[col + row * m_cols]; } - void setZero() { { BT_PROFILE("storage=0"); - btSetZero(&m_storage[0],m_storage.size()); + btSetZero(&m_storage[0], m_storage.size()); //memset(&m_storage[0],0,sizeof(T)*m_storage.size()); //for (int i=0;i<m_storage.size();i++) - // m_storage[i]=0; + // m_storage[i]=0; } } - + void setIdentity() { btAssert(rows() == cols()); - + setZero(); - for (int row=0;row<rows();row++) + for (int row = 0; row < rows(); row++) { - setElem(row,row,1); + setElem(row, row, 1); } } - - - void printMatrix(const char* msg) + void printMatrix(const char* msg) { - printf("%s ---------------------\n",msg); - for (int i=0;i<rows();i++) + printf("%s ---------------------\n", msg); + for (int i = 0; i < rows(); i++) { printf("\n"); - for (int j=0;j<cols();j++) + for (int j = 0; j < cols(); j++) { - printf("%2.1f\t",(*this)(i,j)); + printf("%2.1f\t", (*this)(i, j)); } } printf("\n---------------------\n"); - } - void rowComputeNonZeroElements() const { m_rowNonZeroElements1.resize(rows()); - for (int i=0;i<rows();i++) + for (int i = 0; i < rows(); i++) { m_rowNonZeroElements1[i].resize(0); - for (int j=0;j<cols();j++) + for (int j = 0; j < cols(); j++) { - if ((*this)(i,j)!=0.f) + if ((*this)(i, j) != 0.f) { m_rowNonZeroElements1[i].push_back(j); } @@ -326,54 +313,52 @@ struct btMatrixX btMatrixX transpose() const { //transpose is optimized for sparse matrices - btMatrixX tr(m_cols,m_rows); + btMatrixX tr(m_cols, m_rows); tr.setZero(); - for (int i=0;i<m_cols;i++) - for (int j=0;j<m_rows;j++) + for (int i = 0; i < m_cols; i++) + for (int j = 0; j < m_rows; j++) { - T v = (*this)(j,i); + T v = (*this)(j, i); if (v) { - tr.setElem(i,j,v); + tr.setElem(i, j, v); } } return tr; } - btMatrixX operator*(const btMatrixX& other) { //btMatrixX*btMatrixX implementation, brute force btAssert(cols() == other.rows()); - btMatrixX res(rows(),other.cols()); + btMatrixX res(rows(), other.cols()); res.setZero(); -// BT_PROFILE("btMatrixX mul"); - for (int j=0; j < res.cols(); ++j) + // BT_PROFILE("btMatrixX mul"); + for (int j = 0; j < res.cols(); ++j) { { - for (int i=0; i < res.rows(); ++i) + for (int i = 0; i < res.rows(); ++i) { - T dotProd=0; -// T dotProd2=0; + T dotProd = 0; + // T dotProd2=0; //int waste=0,waste2=0; { -// bool useOtherCol = true; + // bool useOtherCol = true; { - for (int v=0;v<rows();v++) + for (int v = 0; v < rows(); v++) { - T w = (*this)(i,v); - if (other(v,j)!=0.f) + T w = (*this)(i, v); + if (other(v, j) != 0.f) { - dotProd+=w*other(v,j); + dotProd += w * other(v, j); } - } } } if (dotProd) - res.setElem(i,j,dotProd); + res.setElem(i, j, dotProd); } } } @@ -381,174 +366,166 @@ struct btMatrixX } // this assumes the 4th and 8th rows of B and C are zero. - void multiplyAdd2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther ,int row, int col) + void multiplyAdd2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col) { - const btScalar *bb = B; - for ( int i = 0;i<numRows;i++) + const btScalar* bb = B; + for (int i = 0; i < numRows; i++) { - const btScalar *cc = C; - for ( int j = 0;j<numRowsOther;j++) + const btScalar* cc = C; + for (int j = 0; j < numRowsOther; j++) { btScalar sum; - sum = bb[0]*cc[0]; - sum += bb[1]*cc[1]; - sum += bb[2]*cc[2]; - sum += bb[4]*cc[4]; - sum += bb[5]*cc[5]; - sum += bb[6]*cc[6]; - addElem(row+i,col+j,sum); + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + addElem(row + i, col + j, sum); cc += 8; } bb += 8; } } - void multiply2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col) + void multiply2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col) { - btAssert (numRows>0 && numRowsOther>0 && B && C); - const btScalar *bb = B; - for ( int i = 0;i<numRows;i++) + btAssert(numRows > 0 && numRowsOther > 0 && B && C); + const btScalar* bb = B; + for (int i = 0; i < numRows; i++) { - const btScalar *cc = C; - for ( int j = 0;j<numRowsOther;j++) + const btScalar* cc = C; + for (int j = 0; j < numRowsOther; j++) { btScalar sum; - sum = bb[0]*cc[0]; - sum += bb[1]*cc[1]; - sum += bb[2]*cc[2]; - sum += bb[4]*cc[4]; - sum += bb[5]*cc[5]; - sum += bb[6]*cc[6]; - setElem(row+i,col+j,sum); + sum = bb[0] * cc[0]; + sum += bb[1] * cc[1]; + sum += bb[2] * cc[2]; + sum += bb[4] * cc[4]; + sum += bb[5] * cc[5]; + sum += bb[6] * cc[6]; + setElem(row + i, col + j, sum); cc += 8; } bb += 8; } } - - void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value) + + void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value) { - int numRows = rowend+1-rowstart; - int numCols = colend+1-colstart; - - for (int row=0;row<numRows;row++) + int numRows = rowend + 1 - rowstart; + int numCols = colend + 1 - colstart; + + for (int row = 0; row < numRows; row++) { - for (int col=0;col<numCols;col++) + for (int col = 0; col < numCols; col++) { - setElem(rowstart+row,colstart+col,value); + setElem(rowstart + row, colstart + col, value); } } } - - void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block) + + void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX& block) { - btAssert(rowend+1-rowstart == block.rows()); - btAssert(colend+1-colstart == block.cols()); - for (int row=0;row<block.rows();row++) + btAssert(rowend + 1 - rowstart == block.rows()); + btAssert(colend + 1 - colstart == block.cols()); + for (int row = 0; row < block.rows(); row++) { - for (int col=0;col<block.cols();col++) + for (int col = 0; col < block.cols(); col++) { - setElem(rowstart+row,colstart+col,block(row,col)); + setElem(rowstart + row, colstart + col, block(row, col)); } } } - void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block) + void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX<T>& block) { - btAssert(rowend+1-rowstart == block.rows()); - btAssert(colend+1-colstart == block.cols()); - for (int row=0;row<block.rows();row++) + btAssert(rowend + 1 - rowstart == block.rows()); + btAssert(colend + 1 - colstart == block.cols()); + for (int row = 0; row < block.rows(); row++) { - for (int col=0;col<block.cols();col++) + for (int col = 0; col < block.cols(); col++) { - setElem(rowstart+row,colstart+col,block[row]); + setElem(rowstart + row, colstart + col, block[row]); } } } - - + btMatrixX negative() { - btMatrixX neg(rows(),cols()); - for (int i=0;i<rows();i++) - for (int j=0;j<cols();j++) + btMatrixX neg(rows(), cols()); + for (int i = 0; i < rows(); i++) + for (int j = 0; j < cols(); j++) { - T v = (*this)(i,j); - neg.setElem(i,j,-v); + T v = (*this)(i, j); + neg.setElem(i, j, -v); } return neg; } - }; - - typedef btMatrixX<float> btMatrixXf; typedef btVectorX<float> btVectorXf; typedef btMatrixX<double> btMatrixXd; typedef btVectorX<double> btVectorXd; - #ifdef BT_DEBUG_OSTREAM -template <typename T> -std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat) +template <typename T> +std::ostream& operator<<(std::ostream& os, const btMatrixX<T>& mat) +{ + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i = 0; i < mat.rows(); i++) { - - os << " ["; - //printf("%s ---------------------\n",msg); - for (int i=0;i<mat.rows();i++) + for (int j = 0; j < mat.cols(); j++) { - for (int j=0;j<mat.cols();j++) - { - os << std::setw(12) << mat(i,j); - } - if (i!=mat.rows()-1) - os << std::endl << " "; + os << std::setw(12) << mat(i, j); } - os << " ]"; - //printf("\n---------------------\n"); - - return os; + if (i != mat.rows() - 1) + os << std::endl + << " "; } -template <typename T> -std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat) - { - - os << " ["; - //printf("%s ---------------------\n",msg); - for (int i=0;i<mat.rows();i++) - { - os << std::setw(12) << mat[i]; - if (i!=mat.rows()-1) - os << std::endl << " "; - } - os << " ]"; - //printf("\n---------------------\n"); + os << " ]"; + //printf("\n---------------------\n"); - return os; + return os; +} +template <typename T> +std::ostream& operator<<(std::ostream& os, const btVectorX<T>& mat) +{ + os << " ["; + //printf("%s ---------------------\n",msg); + for (int i = 0; i < mat.rows(); i++) + { + os << std::setw(12) << mat[i]; + if (i != mat.rows() - 1) + os << std::endl + << " "; } + os << " ]"; + //printf("\n---------------------\n"); -#endif //BT_DEBUG_OSTREAM + return os; +} +#endif //BT_DEBUG_OSTREAM inline void setElem(btMatrixXd& mat, int row, int col, double val) { - mat.setElem(row,col,val); + mat.setElem(row, col, val); } inline void setElem(btMatrixXf& mat, int row, int col, float val) { - mat.setElem(row,col,val); + mat.setElem(row, col, val); } #ifdef BT_USE_DOUBLE_PRECISION - #define btVectorXu btVectorXd - #define btMatrixXu btMatrixXd +#define btVectorXu btVectorXd +#define btMatrixXu btMatrixXd #else - #define btVectorXu btVectorXf - #define btMatrixXu btMatrixXf -#endif //BT_USE_DOUBLE_PRECISION - - +#define btVectorXu btVectorXf +#define btMatrixXu btMatrixXf +#endif //BT_USE_DOUBLE_PRECISION -#endif//BT_MATRIX_H_H +#endif //BT_MATRIX_H_H |