Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/LinearMath/btMatrixX.h

Changeset View

Standalone View

View Options

extern/bullet2/src/LinearMath/btMatrixX.h

Show All 18 Lines
#include "LinearMath/btQuickprof.h" #include "LinearMath/btQuickprof.h"
#include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btAlignedObjectArray.h"
#include <stdio.h> #include <stdio.h>
//#define BT_DEBUG_OSTREAM //#define BT_DEBUG_OSTREAM
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
#include <iostream> #include <iostream>
#include <iomanip> // std::setw #include <iomanip> // std::setw
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
class btIntSortPredicate class btIntSortPredicate
{ {
public: public:
bool operator() ( const int& a, const int& b ) const bool operator()(const int& a, const int& b) const
{ {
return a < b; return a < b;
} }
}; };
template <typename T> template <typename T>
struct btVectorX struct btVectorX
{ {
btAlignedObjectArray<T> m_storage; btAlignedObjectArray<T> m_storage;
btVectorX() btVectorX()
{ {
} }
btVectorX(int numRows) btVectorX(int numRows)
{ {
m_storage.resize(numRows); m_storage.resize(numRows);
} }
void resize(int rows) void resize(int rows)
{ {
m_storage.resize(rows); m_storage.resize(rows);
} }
int cols() const int cols() const
{ {
return 1; return 1;
} }
int rows() const int rows() const
{ {
return m_storage.size(); return m_storage.size();
} }
int size() const int size() const
{ {
return rows(); return rows();
} }
T nrm2() const T nrm2() const
{ {
T norm = T(0); T norm = T(0);
int nn = rows(); int nn = rows();
{ {
if (nn == 1) if (nn == 1)
{ {
norm = btFabs((*this)[0]); norm = btFabs((*this)[0]);
} }
else else
{ {
T scale = 0.0; T scale = 0.0;
T ssq = 1.0; T ssq = 1.0;
/* The following loop is equivalent to this call to the LAPACK /* The following loop is equivalent to this call to the LAPACK
auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */ 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) if ((*this)[ix] != 0.0)
{ {
T absxi = btFabs((*this)[ix]); T absxi = btFabs((*this)[ix]);
if (scale < absxi) if (scale < absxi)
{ {
T temp; T temp;
temp = scale / absxi; temp = scale / absxi;
ssq = ssq * (temp * temp) + BT_ONE; ssq = ssq * (temp * temp) + BT_ONE;
scale = absxi; scale = absxi;
} }
else else
{ {
T temp; T temp;
temp = absxi / scale; temp = absxi / scale;
ssq += temp * temp; ssq += temp * temp;
} }
} }
} }
norm = scale * sqrt(ssq); norm = scale * sqrt(ssq);
} }
} }
return norm; return norm;
} }
void setZero() void setZero()
{ {
if (m_storage.size()) if (m_storage.size())
{ {
// for (int i=0;i<m_storage.size();i++) // for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0; // m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size()); //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]; return m_storage[index];
} }
T& operator[] (int index) T& operator[](int index)
{ {
return m_storage[index]; return m_storage[index];
} }
T* getBufferPointerWritable() T* getBufferPointerWritable()
{ {
return m_storage.size() ? &m_storage[0] : 0; return m_storage.size() ? &m_storage[0] : 0;
} }
const T* getBufferPointer() const const T* getBufferPointer() const
{ {
return m_storage.size() ? &m_storage[0] : 0; return m_storage.size() ? &m_storage[0] : 0;
} }
}; };
/* /*
template <typename T> template <typename T>
void setElem(btMatrixX<T>& mat, int row, int col, T val) void setElem(btMatrixX<T>& mat, int row, int col, T val)
{ {
mat.setElem(row,col,val); mat.setElem(row,col,val);
} }
*/ */
template <typename T> template <typename T>
struct btMatrixX struct btMatrixX
{ {
int m_rows; int m_rows;
int m_cols; int m_cols;
int m_operations; int m_operations;
int m_resizeOperations; int m_resizeOperations;
int m_setElemOperations; int m_setElemOperations;
btAlignedObjectArray<T> m_storage; btAlignedObjectArray<T> m_storage;
mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1; mutable btAlignedObjectArray<btAlignedObjectArray<int> > m_rowNonZeroElements1;
T* getBufferPointerWritable() T* getBufferPointerWritable()
{ {
return m_storage.size() ? &m_storage[0] : 0; return m_storage.size() ? &m_storage[0] : 0;
} }
const T* getBufferPointer() const const T* getBufferPointer() const
{ {
return m_storage.size() ? &m_storage[0] : 0; return m_storage.size() ? &m_storage[0] : 0;
} }
btMatrixX() btMatrixX()
:m_rows(0), : m_rows(0),
m_cols(0), m_cols(0),
m_operations(0), m_operations(0),
m_resizeOperations(0), m_resizeOperations(0),
m_setElemOperations(0) m_setElemOperations(0)
{ {
} }
btMatrixX(int rows,int cols) btMatrixX(int rows, int cols)
:m_rows(rows), : m_rows(rows),
m_cols(cols), m_cols(cols),
m_operations(0), m_operations(0),
m_resizeOperations(0), m_resizeOperations(0),
m_setElemOperations(0) m_setElemOperations(0)
{ {
resize(rows,cols); resize(rows, cols);
} }
void resize(int rows, int cols) void resize(int rows, int cols)
{ {
m_resizeOperations++; m_resizeOperations++;
m_rows = rows; m_rows = rows;
m_cols = cols; m_cols = cols;
{ {
BT_PROFILE("m_storage.resize"); BT_PROFILE("m_storage.resize");
m_storage.resize(rows*cols); m_storage.resize(rows * cols);
} }
} }
int cols() const int cols() const
{ {
return m_cols; return m_cols;
} }
int rows() const int rows() const
{ {
return m_rows; return m_rows;
} }
///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead ///we don't want this read/write operator(), because we cannot keep track of non-zero elements, use setElem instead
/*T& operator() (int row,int col) /*T& operator() (int row,int col)
{ {
return m_storage[col*m_rows+row]; return m_storage[col*m_rows+row];
} }
*/ */
void addElem(int row,int col, T val) void addElem(int row, int col, T val)
{ {
if (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); setElem(row, col, val);
} else }
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_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++; m_setElemOperations++;
//mul doesn't change sparsity info //mul doesn't change sparsity info
m_storage[row*m_cols+col] *= val; m_storage[row * m_cols + col] *= val;
} }
void copyLowerToUpperTriangle() void copyLowerToUpperTriangle()
{ {
int count=0; int count = 0;
for (int row=0;row<rows();row++) 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++; count++;
} }
} }
//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows()); //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() void setZero()
{ {
{ {
BT_PROFILE("storage=0"); BT_PROFILE("storage=0");
if (m_storage.size())
{
btSetZero(&m_storage[0],m_storage.size()); btSetZero(&m_storage[0], m_storage.size());
}
//memset(&m_storage[0],0,sizeof(T)*m_storage.size()); //memset(&m_storage[0],0,sizeof(T)*m_storage.size());
//for (int i=0;i<m_storage.size();i++) //for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0; // m_storage[i]=0;
} }
} }
void setIdentity() void setIdentity()
{ {
btAssert(rows() == cols()); btAssert(rows() == cols());
setZero(); 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) const
void printMatrix(const char* msg)
{ {
printf("%s ---------------------\n",msg); printf("%s ---------------------\n", msg);
for (int i=0;i<rows();i++) for (int i = 0; i < rows(); i++)
{ {
printf("\n"); 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"); printf("\n---------------------\n");
} }
void rowComputeNonZeroElements() const void rowComputeNonZeroElements() const
{ {
m_rowNonZeroElements1.resize(rows()); m_rowNonZeroElements1.resize(rows());
for (int i=0;i<rows();i++) for (int i = 0; i < rows(); i++)
{ {
m_rowNonZeroElements1[i].resize(0); 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); m_rowNonZeroElements1[i].push_back(j);
} }
} }
} }
} }
btMatrixX transpose() const btMatrixX transpose() const
{ {
//transpose is optimized for sparse matrices //transpose is optimized for sparse matrices
btMatrixX tr(m_cols,m_rows); btMatrixX tr(m_cols, m_rows);
tr.setZero(); tr.setZero();
for (int i=0;i<m_cols;i++) for (int i = 0; i < m_cols; i++)
for (int j=0;j<m_rows;j++) for (int j = 0; j < m_rows; j++)
{ {
T v = (*this)(j,i); T v = (*this)(j, i);
if (v) if (v)
{ {
tr.setElem(i,j,v); tr.setElem(i, j, v);
} }
} }
return tr; return tr;
} }
btMatrixX operator*(const btMatrixX& other) btMatrixX operator*(const btMatrixX& other)
{ {
//btMatrixX*btMatrixX implementation, brute force //btMatrixX*btMatrixX implementation, brute force
btAssert(cols() == other.rows()); btAssert(cols() == other.rows());
btMatrixX res(rows(),other.cols()); btMatrixX res(rows(), other.cols());
res.setZero(); res.setZero();
// BT_PROFILE("btMatrixX mul"); // BT_PROFILE("btMatrixX mul");
for (int j=0; j < res.cols(); ++j) for (int i = 0; i < rows(); ++i)
{ {
{ {
for (int i=0; i < res.rows(); ++i) for (int j = 0; j < other.cols(); ++j)
{ {
T dotProd=0; T dotProd = 0;
// T dotProd2=0;
//int waste=0,waste2=0;
{ {
// bool useOtherCol = true;
{ {
for (int v=0;v<rows();v++) int c = cols();
for (int k = 0; k < c; k++)
{ {
T w = (*this)(i,v); T w = (*this)(i, k);
if (other(v,j)!=0.f) if (other(k, j) != 0.f)
{ {
dotProd+=w*other(v,j); dotProd += w * other(k, j);
} }
} }
} }
} }
if (dotProd) if (dotProd)
res.setElem(i,j,dotProd); res.setElem(i, j, dotProd);
} }
} }
} }
return res; return res;
} }
// this assumes the 4th and 8th rows of B and C are zero. // 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; const btScalar* bb = B;
for ( int i = 0;i<numRows;i++) for (int i = 0; i < numRows; i++)
{ {
const btScalar *cc = C; const btScalar* cc = C;
for ( int j = 0;j<numRowsOther;j++) for (int j = 0; j < numRowsOther; j++)
{ {
btScalar sum; btScalar sum;
sum = bb[0]*cc[0]; sum = bb[0] * cc[0];
sum += bb[1]*cc[1]; sum += bb[1] * cc[1];
sum += bb[2]*cc[2]; sum += bb[2] * cc[2];
sum += bb[4]*cc[4]; sum += bb[4] * cc[4];
sum += bb[5]*cc[5]; sum += bb[5] * cc[5];
sum += bb[6]*cc[6]; sum += bb[6] * cc[6];
addElem(row+i,col+j,sum); addElem(row + i, col + j, sum);
cc += 8; cc += 8;
} }
bb += 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); btAssert(numRows > 0 && numRowsOther > 0 && B && C);
const btScalar *bb = B; const btScalar* bb = B;
for ( int i = 0;i<numRows;i++) for (int i = 0; i < numRows; i++)
{ {
const btScalar *cc = C; const btScalar* cc = C;
for ( int j = 0;j<numRowsOther;j++) for (int j = 0; j < numRowsOther; j++)
{ {
btScalar sum; btScalar sum;
sum = bb[0]*cc[0]; sum = bb[0] * cc[0];
sum += bb[1]*cc[1]; sum += bb[1] * cc[1];
sum += bb[2]*cc[2]; sum += bb[2] * cc[2];
sum += bb[4]*cc[4]; sum += bb[4] * cc[4];
sum += bb[5]*cc[5]; sum += bb[5] * cc[5];
sum += bb[6]*cc[6]; sum += bb[6] * cc[6];
setElem(row+i,col+j,sum); setElem(row + i, col + j, sum);
cc += 8; cc += 8;
} }
bb += 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 numRows = rowend + 1 - rowstart;
int numCols = colend+1-colstart; int numCols = colend + 1 - colstart;
for (int row=0;row<numRows;row++) 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(rowend + 1 - rowstart == block.rows());
btAssert(colend+1-colstart == block.cols()); btAssert(colend + 1 - colstart == block.cols());
for (int row=0;row<block.rows();row++) 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(rowend + 1 - rowstart == block.rows());
btAssert(colend+1-colstart == block.cols()); btAssert(colend + 1 - colstart == block.cols());
for (int row=0;row<block.rows();row++) 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 negative()
{ {
btMatrixX neg(rows(),cols()); btMatrixX neg(rows(), cols());
for (int i=0;i<rows();i++) for (int i = 0; i < rows(); i++)
for (int j=0;j<cols();j++) for (int j = 0; j < cols(); j++)
{ {
T v = (*this)(i,j); T v = (*this)(i, j);
neg.setElem(i,j,-v); neg.setElem(i, j, -v);
} }
return neg; return neg;
} }
}; };
typedef btMatrixX<float> btMatrixXf; typedef btMatrixX<float> btMatrixXf;
typedef btVectorX<float> btVectorXf; typedef btVectorX<float> btVectorXf;
typedef btMatrixX<double> btMatrixXd; typedef btMatrixX<double> btMatrixXd;
typedef btVectorX<double> btVectorXd; typedef btVectorX<double> btVectorXd;
#ifdef BT_DEBUG_OSTREAM #ifdef BT_DEBUG_OSTREAM
template <typename T> template <typename T>
std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat) std::ostream& operator<<(std::ostream& os, const btMatrixX<T>& mat)
{ {
os << " ["; os << " [";
//printf("%s ---------------------\n",msg); //printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++) 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); os << std::setw(12) << mat(i, j);
} }
if (i!=mat.rows()-1) if (i != mat.rows() - 1)
os << std::endl << " "; os << std::endl
<< " ";
} }
os << " ]"; os << " ]";
//printf("\n---------------------\n"); //printf("\n---------------------\n");
return os; return os;
} }
template <typename T> template <typename T>
std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat) std::ostream& operator<<(std::ostream& os, const btVectorX<T>& mat)
{ {
os << " ["; os << " [";
//printf("%s ---------------------\n",msg); //printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++) for (int i = 0; i < mat.rows(); i++)
{ {
os << std::setw(12) << mat[i]; os << std::setw(12) << mat[i];
if (i!=mat.rows()-1) if (i != mat.rows() - 1)
os << std::endl << " "; os << std::endl
<< " ";
} }
os << " ]"; os << " ]";
//printf("\n---------------------\n"); //printf("\n---------------------\n");
return os; return os;
} }
#endif //BT_DEBUG_OSTREAM #endif //BT_DEBUG_OSTREAM
inline void setElem(btMatrixXd& mat, int row, int col, double val) 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) 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 #ifdef BT_USE_DOUBLE_PRECISION
#define btVectorXu btVectorXd #define btVectorXu btVectorXd
#define btMatrixXu btMatrixXd #define btMatrixXu btMatrixXd
#else #else
#define btVectorXu btVectorXf #define btVectorXu btVectorXf
#define btMatrixXu btMatrixXf #define btMatrixXu btMatrixXf
#endif //BT_USE_DOUBLE_PRECISION #endif //BT_USE_DOUBLE_PRECISION
#endif//BT_MATRIX_H_H #endif //BT_MATRIX_H_H