533 lines
11 KiB
C++
533 lines
11 KiB
C++
/*
|
|
Bullet Continuous Collision Detection and Physics Library
|
|
Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
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.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
///original version written by Erwin Coumans, October 2013
|
|
|
|
#ifndef BT_MATRIX_X_H
|
|
#define BT_MATRIX_X_H
|
|
|
|
#include "LinearMath/btQuickprof.h"
|
|
#include "LinearMath/btAlignedObjectArray.h"
|
|
#include <stdio.h>
|
|
|
|
//#define BT_DEBUG_OSTREAM
|
|
#ifdef BT_DEBUG_OSTREAM
|
|
#include <iostream>
|
|
#include <iomanip> // std::setw
|
|
#endif //BT_DEBUG_OSTREAM
|
|
|
|
class btIntSortPredicate
|
|
{
|
|
public:
|
|
bool operator()(const int& a, const int& b) const
|
|
{
|
|
return a < b;
|
|
}
|
|
};
|
|
|
|
template <typename T>
|
|
struct btVectorX
|
|
{
|
|
btAlignedObjectArray<T> m_storage;
|
|
|
|
btVectorX()
|
|
{
|
|
}
|
|
btVectorX(int numRows)
|
|
{
|
|
m_storage.resize(numRows);
|
|
}
|
|
|
|
void resize(int rows)
|
|
{
|
|
m_storage.resize(rows);
|
|
}
|
|
int cols() const
|
|
{
|
|
return 1;
|
|
}
|
|
int rows() const
|
|
{
|
|
return m_storage.size();
|
|
}
|
|
int size() const
|
|
{
|
|
return rows();
|
|
}
|
|
|
|
T nrm2() const
|
|
{
|
|
T norm = T(0);
|
|
|
|
int nn = rows();
|
|
|
|
{
|
|
if (nn == 1)
|
|
{
|
|
norm = btFabs((*this)[0]);
|
|
}
|
|
else
|
|
{
|
|
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++)
|
|
{
|
|
if ((*this)[ix] != 0.0)
|
|
{
|
|
T absxi = btFabs((*this)[ix]);
|
|
if (scale < absxi)
|
|
{
|
|
T temp;
|
|
temp = scale / absxi;
|
|
ssq = ssq * (temp * temp) + BT_ONE;
|
|
scale = absxi;
|
|
}
|
|
else
|
|
{
|
|
T temp;
|
|
temp = absxi / scale;
|
|
ssq += temp * temp;
|
|
}
|
|
}
|
|
}
|
|
norm = scale * sqrt(ssq);
|
|
}
|
|
}
|
|
return norm;
|
|
}
|
|
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());
|
|
}
|
|
}
|
|
const T& operator[](int index) const
|
|
{
|
|
return m_storage[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>
|
|
void setElem(btMatrixX<T>& mat, int row, int col, T val)
|
|
{
|
|
mat.setElem(row,col,val);
|
|
}
|
|
*/
|
|
|
|
template <typename T>
|
|
struct btMatrixX
|
|
{
|
|
int m_rows;
|
|
int m_cols;
|
|
int m_operations;
|
|
int m_resizeOperations;
|
|
int m_setElemOperations;
|
|
|
|
btAlignedObjectArray<T> m_storage;
|
|
mutable btAlignedObjectArray<btAlignedObjectArray<int> > m_rowNonZeroElements1;
|
|
|
|
T* getBufferPointerWritable()
|
|
{
|
|
return m_storage.size() ? &m_storage[0] : 0;
|
|
}
|
|
|
|
const T* getBufferPointer() const
|
|
{
|
|
return m_storage.size() ? &m_storage[0] : 0;
|
|
}
|
|
btMatrixX()
|
|
: 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)
|
|
{
|
|
resize(rows, cols);
|
|
}
|
|
void resize(int rows, int cols)
|
|
{
|
|
m_resizeOperations++;
|
|
m_rows = rows;
|
|
m_cols = cols;
|
|
{
|
|
BT_PROFILE("m_storage.resize");
|
|
m_storage.resize(rows * cols);
|
|
}
|
|
}
|
|
int cols() const
|
|
{
|
|
return m_cols;
|
|
}
|
|
int rows() const
|
|
{
|
|
return m_rows;
|
|
}
|
|
///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)
|
|
{
|
|
return m_storage[col*m_rows+row];
|
|
}
|
|
*/
|
|
|
|
void addElem(int row, int col, T val)
|
|
{
|
|
if (val)
|
|
{
|
|
if (m_storage[col + row * m_cols] == 0.f)
|
|
{
|
|
setElem(row, col, val);
|
|
}
|
|
else
|
|
{
|
|
m_storage[row * m_cols + col] += val;
|
|
}
|
|
}
|
|
}
|
|
|
|
void setElem(int row, int col, T val)
|
|
{
|
|
m_setElemOperations++;
|
|
m_storage[row * m_cols + col] = val;
|
|
}
|
|
|
|
void mulElem(int row, int col, T val)
|
|
{
|
|
m_setElemOperations++;
|
|
//mul doesn't change sparsity info
|
|
|
|
m_storage[row * m_cols + col] *= val;
|
|
}
|
|
|
|
void copyLowerToUpperTriangle()
|
|
{
|
|
int count = 0;
|
|
for (int row = 0; row < rows(); row++)
|
|
{
|
|
for (int col = 0; col < 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
|
|
{
|
|
return m_storage[col + row * m_cols];
|
|
}
|
|
|
|
void setZero()
|
|
{
|
|
{
|
|
BT_PROFILE("storage=0");
|
|
if (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;
|
|
}
|
|
}
|
|
|
|
void setIdentity()
|
|
{
|
|
btAssert(rows() == cols());
|
|
|
|
setZero();
|
|
for (int row = 0; row < rows(); row++)
|
|
{
|
|
setElem(row, row, 1);
|
|
}
|
|
}
|
|
|
|
void printMatrix(const char* msg) const
|
|
{
|
|
printf("%s ---------------------\n", msg);
|
|
for (int i = 0; i < rows(); i++)
|
|
{
|
|
printf("\n");
|
|
for (int j = 0; j < cols(); 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++)
|
|
{
|
|
m_rowNonZeroElements1[i].resize(0);
|
|
for (int j = 0; j < cols(); j++)
|
|
{
|
|
if ((*this)(i, j) != 0.f)
|
|
{
|
|
m_rowNonZeroElements1[i].push_back(j);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
btMatrixX transpose() const
|
|
{
|
|
//transpose is optimized for sparse matrices
|
|
btMatrixX tr(m_cols, m_rows);
|
|
tr.setZero();
|
|
for (int i = 0; i < m_cols; i++)
|
|
for (int j = 0; j < m_rows; j++)
|
|
{
|
|
T v = (*this)(j, i);
|
|
if (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());
|
|
res.setZero();
|
|
// BT_PROFILE("btMatrixX mul");
|
|
for (int i = 0; i < rows(); ++i)
|
|
{
|
|
{
|
|
for (int j = 0; j < other.cols(); ++j)
|
|
{
|
|
T dotProd = 0;
|
|
{
|
|
{
|
|
int r = rows();
|
|
int c = cols();
|
|
|
|
for (int k = 0; k < cols(); k++)
|
|
{
|
|
T w = (*this)(i, k);
|
|
if (other(k, j) != 0.f)
|
|
{
|
|
dotProd += w * other(k, j);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (dotProd)
|
|
res.setElem(i, j, dotProd);
|
|
}
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
|
|
// 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)
|
|
{
|
|
const btScalar* bb = B;
|
|
for (int i = 0; i < numRows; i++)
|
|
{
|
|
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);
|
|
cc += 8;
|
|
}
|
|
bb += 8;
|
|
}
|
|
}
|
|
|
|
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++)
|
|
{
|
|
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);
|
|
cc += 8;
|
|
}
|
|
bb += 8;
|
|
}
|
|
}
|
|
|
|
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++)
|
|
{
|
|
for (int col = 0; col < numCols; col++)
|
|
{
|
|
setElem(rowstart + row, colstart + col, value);
|
|
}
|
|
}
|
|
}
|
|
|
|
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++)
|
|
{
|
|
for (int col = 0; col < block.cols(); col++)
|
|
{
|
|
setElem(rowstart + row, colstart + col, block(row, col));
|
|
}
|
|
}
|
|
}
|
|
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++)
|
|
{
|
|
for (int col = 0; col < block.cols(); col++)
|
|
{
|
|
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++)
|
|
{
|
|
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)
|
|
{
|
|
os << " [";
|
|
//printf("%s ---------------------\n",msg);
|
|
for (int i = 0; i < mat.rows(); i++)
|
|
{
|
|
for (int j = 0; j < mat.cols(); j++)
|
|
{
|
|
os << std::setw(12) << mat(i, j);
|
|
}
|
|
if (i != mat.rows() - 1)
|
|
os << std::endl
|
|
<< " ";
|
|
}
|
|
os << " ]";
|
|
//printf("\n---------------------\n");
|
|
|
|
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");
|
|
|
|
return os;
|
|
}
|
|
|
|
#endif //BT_DEBUG_OSTREAM
|
|
|
|
inline void setElem(btMatrixXd& mat, int row, int col, double val)
|
|
{
|
|
mat.setElem(row, col, val);
|
|
}
|
|
|
|
inline void setElem(btMatrixXf& mat, int row, int col, float val)
|
|
{
|
|
mat.setElem(row, col, val);
|
|
}
|
|
|
|
#ifdef BT_USE_DOUBLE_PRECISION
|
|
#define btVectorXu btVectorXd
|
|
#define btMatrixXu btMatrixXd
|
|
#else
|
|
#define btVectorXu btVectorXf
|
|
#define btMatrixXu btMatrixXf
|
|
#endif //BT_USE_DOUBLE_PRECISION
|
|
|
|
#endif //BT_MATRIX_H_H
|