SparseMat::SparseMat(const SparseMat &source,
const DoFMap &rowmap,
const DoFMap &colmap)
: nnz_(0),
nrows_(rowmap.range()),
ncols_(colmap.range()),
consolidated_(true) // true, because mtx is initially empty
{
data.resize(nrows_);
nonempty_col.resize(ncols_, false);
for(SparseMatConstIterator ij=source.begin(); ij<source.end(); ++ij) {
assert(ij.row() < rowmap.domain()); // row is unsigned, dont check >= 0
int i = rowmap[ij.row()];
assert(data.size()!=0 || i==-1);
assert(i < (int) rowmap.range());
if(i >= 0) {
assert(ij.col() < colmap.domain());
int j = colmap[ij.col()];
assert(j < (int) colmap.range() && j >= -1);
if(j >= 0) {
insert(i, j, *ij); // unsets consolidated_.
}
}
}
consolidate();
}
/* multiplication between sparse and classic matrix A=D*Q
* hists: sparse matrix (typically matrix of database words)
* nhist: matrix (typically matrix of query words)
* return: hists*nhists (matrix)
*/
Mat mul(const SparseMat &hists, const Mat &nhist)
{
Mat answer = Mat::zeros(hists.size(0),1,CV_32F);
SparseMatConstIterator
it = hists.begin(),
it_end = hists.end();
for (; it!=it_end;++it)
{
const SparseMat::Node* n = it.node();
answer.at<float>(n->idx[0])+=nhist.at<float>(n->idx[1])*it.value<float>();
}
return answer;
}
void SparseMat::tile(unsigned int i, unsigned int j,
const SparseMat &other)
{
// Add other to this, offset by i rows and j columns.
assert(i + other.nrows() <= nrows());
assert(j + other.ncols() <= ncols());
for(SparseMat::const_iterator kl = other.begin(); kl<other.end(); ++kl) {
unsigned int ii = kl.row() + i;
unsigned int jj = kl.col() + j;
assert(0 <= ii && ii < nrows_);
assert(0 <= jj && jj < ncols_);
insert(ii, jj, *kl);
}
}
void mmadump(const std::string &filename, const std::string &mtxname,
const SparseMat &m)
{
std::ofstream os(filename.c_str());
os << mtxname << " = Table[Table[0, {i, 1, " << m.ncols() << "}], {j, 1, "
<< m.nrows() << "}]" << std::endl;
for(SparseMat::const_iterator ij=m.begin(); ij<m.end(); ++ij) {
std::string val(to_string(*ij));
int epos = val.find("e");
if(epos >= 0)
val.replace(epos, 1, "*^");
os << mtxname << "[[" << ij.row()+1 << "," << ij.col()+1 << "]] = " << val
<< std::endl;
}
os.close();
}
SparseMat SparseMat::operator*(const SparseMat &right) const {
if(!consolidated() || !right.consolidated())
throw ErrProgrammingError("Attempt to multiply unconsolidated SparseMats!",
__FILE__, __LINE__);
SparseMat result(nrows(), right.ncols());
const SparseMat rightt = right.transpose();
for(unsigned int i=0; i<nrows(); i++) {
for(unsigned int j=0; j<rightt.nrows(); j++) {
const_row_iterator ai = begin(i);
const_row_iterator bj = rightt.begin(j);
while(ai < end(i) && bj < rightt.end(j)) {
if(ai.col() == bj.col()) {
result.insert(i, j, (*ai)*(*bj));
++ai;
++bj;
}
else if(ai.col() < bj.col())
++ai;
else
++bj;
}
}
result.consolidate_row(i);
}
// result.consolidate();
result.consolidated_ = true;
return result;
}
IGL_INLINE void igl::hessian(
const Eigen::MatrixBase<DerivedV> & V,
const Eigen::MatrixBase<DerivedF> & F,
Eigen::SparseMatrix<Scalar>& H)
{
typedef typename DerivedV::Scalar denseScalar;
typedef typename Eigen::Matrix<denseScalar, Eigen::Dynamic, 1> VecXd;
typedef typename Eigen::SparseMatrix<Scalar> SparseMat;
typedef typename Eigen::DiagonalMatrix
<Scalar, Eigen::Dynamic, Eigen::Dynamic> DiagMat;
int dim = V.cols();
assert((dim==2 || dim==3) &&
"The dimension of the vertices should be 2 or 3");
//Construct the combined gradient matric
SparseMat G;
igl::grad(Eigen::PlainObjectBase<DerivedV>(V),
Eigen::PlainObjectBase<DerivedF>(F),
G, false);
SparseMat GG(F.rows(), dim*V.rows());
GG.reserve(G.nonZeros());
for(int i=0; i<dim; ++i)
GG.middleCols(i*G.cols(),G.cols()) = G.middleRows(i*F.rows(),F.rows());
SparseMat D;
igl::repdiag(GG,dim,D);
//Compute area matrix
VecXd areas;
igl::doublearea(V, F, areas);
DiagMat A = (0.5*areas).replicate(dim,1).asDiagonal();
//Compute FEM Hessian
H = D.transpose()*A*G;
}
SparseMatRowIterator::SparseMatRowIterator(SparseMat& mat, unsigned int row)
: mat(mat),
row_(row)
{
if(row >= mat.nrows())
throw ErrProgrammingError("SparseMatRowIterator row index out of range",
__FILE__, __LINE__);
coliter = mat.data[row].begin(); // iterates over the row
}
static double getValue(SparseMat& M, const int* idx, RNG& rng)
{
int d = M.dims();
size_t hv = 0, *phv = 0;
if( (unsigned)rng % 2 )
{
hv = d == 2 ? M.hash(idx[0], idx[1]) :
d == 3 ? M.hash(idx[0], idx[1], idx[2]) : M.hash(idx);
phv = &hv;
}
const uchar* ptr = d == 2 ? M.ptr(idx[0], idx[1], false, phv) :
d == 3 ? M.ptr(idx[0], idx[1], idx[2], false, phv) :
M.ptr(idx, false, phv);
return !ptr ? 0 : M.type() == CV_32F ? *(float*)ptr : M.type() == CV_64F ? *(double*)ptr : 0;
}
void CmShow::showTinySparseMat(CStr &title, const SparseMat &A1d)
{
int N = A1d.nzcount();
if (N > 1000)
return;
struct NodeSM {
int r, c;
double v;
bool operator < (const NodeSM &n) {
if (r < n.r)
return true;
else if (r > n.r)
return false;
else
return c < n.c;
}
void print() {
if (abs(v) > 1e-8) printf("(%d, %d) %-12g\t", r, c, v);
}
};
vector<NodeSM> nodes(N);
SparseMatConstIterator it = A1d.begin();
for (int i = 0; i < N; i++, ++it) {
const int* idx = it.node()->idx;
nodes[i].r = idx[0] + 1;
nodes[i].c = idx[1] + 1;
nodes[i].v = it.value<double>();
}
sort(nodes.begin(), nodes.end());
for (int i = 0; i < N; i++)
nodes[i].print();
printf("\n");
Mat m;
A1d.convertTo(m, CV_64F);
showTinyMat(title, m);
}
static void setValue(SparseMat& M, const int* idx, double value, RNG& rng)
{
int d = M.dims();
size_t hv = 0, *phv = 0;
if( (unsigned)rng % 2 )
{
hv = d == 2 ? M.hash(idx[0], idx[1]) :
d == 3 ? M.hash(idx[0], idx[1], idx[2]) : M.hash(idx);
phv = &hv;
}
uchar* ptr = d == 2 ? M.ptr(idx[0], idx[1], true, phv) :
d == 3 ? M.ptr(idx[0], idx[1], idx[2], true, phv) :
M.ptr(idx, true, phv);
if( M.type() == CV_32F )
*(float*)ptr = (float)value;
else if( M.type() == CV_64F )
*(double*)ptr = value;
else
CV_Error(CV_StsUnsupportedFormat, "");
}
static void eraseValue(SparseMat& M, const int* idx, RNG& rng)
{
int d = M.dims();
size_t hv = 0, *phv = 0;
if( (unsigned)rng % 2 )
{
hv = d == 2 ? M.hash(idx[0], idx[1]) :
d == 3 ? M.hash(idx[0], idx[1], idx[2]) : M.hash(idx);
phv = &hv;
}
if( d == 2 )
M.erase(idx[0], idx[1], phv);
else if( d == 3 )
M.erase(idx[0], idx[1], idx[2], phv);
else
M.erase(idx, phv);
}
void ConformalResizing::AddConstrain(const vector<ConstrainUnits>& units, SparseMat& A, bool recalculateM)
{
CvMat* M = NULL;
//timer.Start("testaaa");
for (size_t i = 0; i < units.size(); i++)
{
if (recalculateM == true || i == 0)
{
if (M != NULL)
cvReleaseMat(&M);
Constrian(units[i], M);
}
int* ind = new int[units[i].n * 2];
for (int j = 0; j < units[i].n; j++)
{
ind[j*2] = units[i].ind[j]*2;
ind[j*2 + 1] = units[i].ind[j]*2 + 1;
}
int nPos = 0;
for (int y = 0; y < M->height; y++)
{
A.m++;
for (int x = 0; x < M->width; x++, nPos++)
{
A.Add(A.m-1, ind[x], M->data.db[nPos] * units[i].imp);
//debug
//double d=0;//M->data.db[nPos] * units[i].imp;
//A.elements.push_back(SparseMat::Ele(A.m-1, ind[x], d));
}
}
delete []ind;
}
//timer.End();
cvReleaseMat(&M);
}
void testMatrixUtils(const Ring& R, const SparseMat& A)
{
Context<Ring> ctx (R);
std::ostream &report = commentator.report(Commentator::LEVEL_NORMAL, INTERNAL_DESCRIPTION);
typedef SparseBlocMatrix<ContiguousBloc<typename Ring::Element, uint16> > Matrix;
commentator.start("TESTING ArrangementTopDown_LeftRight", "ArrangementTopDown_LeftRight");
{
Matrix M0(A.rowdim(), A.coldim(), Matrix::ArrangementTopDown_LeftRight);
SparseMatrix<typename Ring::Element> C(A.rowdim(), A.coldim());
report << " BLOC HEIGHT" << M0.bloc_height () << endl;
commentator.start("Copy SparseMatrix => SparseBlocMatrix");
MatrixUtils::copy(A, M0);
commentator.stop(MSG_DONE);
commentator.start("Copy SparseMatrix => SparseBlocMatrix");
MatrixUtils::copy(M0, C);
commentator.stop(MSG_DONE);
//MatrixUtil::dumpMatrixAsPbmImage(A, "A.pbm");
//MatrixUtil::dumpMatrixAsPbmImage(C, "C.pbm");
report << endl;
if(BLAS3::equal(ctx, A, C))
report << "<<<<<<<<<<<<<<<<<<<<<<<<<<<RESULT OK>>>>>>>>>>>>>>>>>>>>>>>>";
else
report << ">>>>>>>>>>>>>>>>>>>>>>>>>>>RESULT WRONG<<<<<<<<<<<<<<<<<<<<<";
report << endl;
MatrixUtils::show_mem_usage("ArrangementTopDown_LeftRight");
}
commentator.stop("ArrangementTopDown_LeftRight");
commentator.start("TESTING ArrangementDownTop_LeftRight", "ArrangementDownTop_LeftRight");
{
Matrix M0(A.rowdim(), A.coldim(), Matrix::ArrangementDownTop_LeftRight);
SparseMatrix<typename Ring::Element> C(A.rowdim(), A.coldim());
report << " BLOC HEIGHT" << M0.bloc_height () << endl;
commentator.start("Copy SparseMatrix => SparseBlocMatrix");
MatrixUtils::copy(A, M0);
commentator.stop(MSG_DONE);
commentator.start("Copy SparseMatrix => SparseBlocMatrix");
MatrixUtils::copy(M0, C);
commentator.stop(MSG_DONE);
//MatrixUtil::dumpMatrixAsPbmImage(A, "A.pbm");
//MatrixUtil::dumpMatrixAsPbmImage(C, "C.pbm");
report << endl;
if(BLAS3::equal(ctx, A, C))
report << "<<<<<<<<<<<<<<<<<<<<<<<<<<<RESULT OK>>>>>>>>>>>>>>>>>>>>>>>>";
else
report << ">>>>>>>>>>>>>>>>>>>>>>>>>>>RESULT WRONG<<<<<<<<<<<<<<<<<<<<<";
report << endl;
MatrixUtils::show_mem_usage("ArrangementDownTop_LeftRight");
}
commentator.stop("ArrangementDownTop_LeftRight");
commentator.start("TESTING ArrangementDownTop_RightLeft", "ArrangementDownTop_RightLeft");
{
Matrix M0(A.rowdim(), A.coldim(), Matrix::ArrangementDownTop_RightLeft);
SparseMatrix<typename Ring::Element> C(A.rowdim(), A.coldim());
report << " BLOC HEIGHT" << M0.bloc_height () << endl;
commentator.start("Copy SparseMatrix => SparseBlocMatrix");
MatrixUtils::copy(A, M0);
commentator.stop(MSG_DONE);
commentator.start("Copy SparseMatrix => SparseBlocMatrix");
MatrixUtils::copy(M0, C);
commentator.stop(MSG_DONE);
//MatrixUtil::dumpMatrixAsPbmImage(A, "A.pbm");
//MatrixUtil::dumpMatrixAsPbmImage(C, "C.pbm");
report << endl;
if(BLAS3::equal(ctx, A, C))
report << "<<<<<<<<<<<<<<<<<<<<<<<<<<<RESULT OK>>>>>>>>>>>>>>>>>>>>>>>>";
else
report << ">>>>>>>>>>>>>>>>>>>>>>>>>>>RESULT WRONG<<<<<<<<<<<<<<<<<<<<<";
report << endl;
MatrixUtils::show_mem_usage("ArrangementDownTop_RightLeft");
}
commentator.stop("ArrangementDownTop_RightLeft");
}
int main() {
{ // Test lower_bound and min_max
using jcui::algorithm::lower_bound;
using jcui::algorithm::min_max;
{
int xa[] = {};
int ya[] = {};
vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
eq(0, index, 0);
eq(0, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 0);
}
{
int xa[] = {3, 4, 5, 6};
int ya[] = {5, 4, 3, 1};
vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
eq(1, index, 1);
eq(1, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 4);
}
{
int xa[] = {3, 3, 5, 6};
int ya[] = {5, 4, 3, 1};
vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
eq(2, index, 2);
eq(2, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 4);
}
{
int xa[] = {13, 14, 15, 16};
int ya[] = {5, 4, 3, 1};
vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
eq(3, index, 0);
eq(3, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 13);
}
{
int xa[] = {3, 4, 5, 6};
int ya[] = {15, 14, 13, 11};
vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya));
int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin();
eq(4, index, 4);
eq(4, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 11);
}
}
{ // For SparseMat
using jcui::algorithm::SparseMat;
{
SparseMat<int> a(100, 100), b(100, 100);
SparseMat<int> c = a * b;
eq(c.get(3, 10), 0);
eq(c.get(0, 0), 0);
}
{
// a = [1, 2; 3, 0], b = [6, 0, 0; 0, 1, 4], a * b = [6, 2, 8; 18, 0, 0]
SparseMat<int> a(100, 100), b(100, 100);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(1, 0, 3);
b.set(0, 0, 6);
b.set(1, 1, 1);
b.set(1, 2, 4);
SparseMat<int> c = a * b;
eq(c.get(0, 0), 6);
eq(c.get(0, 1), 2);
eq(c.get(0, 2), 8);
eq(c.get(1, 0), 18);
eq(c.get(1, 1), 0);
eq(c.get(1, 2), 0);
}
{
// a = [1, 2; 3, 0]
SparseMat<long> a(100, 100);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(1, 0, 3);
SparseMat<long> c = pow(a, 17);
eq(c.get(0, 0), 77431669L);
eq(c.get(0, 1), 51708494L);
eq(c.get(1, 0), 77562741L);
eq(c.get(1, 1), 51577422L);
}
{
// a = [1, 2; 3, 0]
int N = 10;
SparseMat<float> a(N, N);
for (int i = 0; i < N; ++i) {
a.set(i, i, 0.9f);
a.set(i, (i + 1) % N, 0.05f);
a.set(i, (i + N - 1) % N, 0.05f);
}
SparseMat<float> c = pow(a, 20000);
for (int i = 0; i < N; ++i) {
for (int j = 0; j < N; ++j) {
eq(fabs(c.get(i, j) - 0.1f) < 1e-3f, true);
}
}
}
{
// a = [1, 2; 3, 0]
SparseMat<long> a(2, 2);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(1, 0, 3);
long xa[] = {4, 5};
vector<long> x(xa, xa + ARRAYSIZE(xa));
vector<long> y = a * x;
eq(y[0], 14L);
eq(y[1], 12L);
}
{
// a = [1, 2, 1; 3, 3, 0]
SparseMat<int> a(2, 3);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(0, 2, 1);
a.set(1, 0, 3);
a.set(1, 1, 3);
vector<int> y = SparseMat<int>::row_sum(a);
eq(y[0], 4);
eq(y[1], 5);
eq(y[2], 1);
}
{
// a = [1, 2, 1; 3, 3, 0]
SparseMat<float> a(2, 3);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(0, 2, 1);
a.set(1, 0, 3);
a.set(1, 1, 3);
a.normalize_by_row_sum();
eq(a.get(0, 0), 0.25f);
eq(a.get(0, 1), 0.4f);
eq(a.get(0, 2), 1.0f);
eq(a.get(1, 0), 0.75f);
eq(a.get(1, 1), 0.6f);
eq(a.get(1, 2), 0.0f);
}
}
{
using jcui::algorithm::RingBuffer;
{
RingBuffer<int> R(3);
R.push_back(3);
eq(R.get(0), 3);
eq(R.get(-10), 0);
eq(R.get(10), 0);
eq(R.get(3), 0);
R.push_back(2);
eq(R.get(-1), 3);
eq(R.get(0), 2);
R.push_back(1);
eq(R.get(-2), 3);
eq(R.get(-1), 2);
eq(R.get(0), 1);
R.push_back(7);
eq(R.get(-2), 2);
eq(R.get(-1), 1);
eq(R.get(0), 7);
eq(R.get(-3), 0);
eq(R.get(3), 0);
}
}
{
using jcui::algorithm::Mat;
{
// a = [1, 2; 3, 0], b = [6, 0, 0; 0, 1, 4], a * b = [6, 2, 8; 18, 0, 0]
Mat<int> a(100, 100), b(100, 100);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(1, 0, 3);
b.set(0, 0, 6);
b.set(1, 1, 1);
b.set(1, 2, 4);
Mat<int> c = a * b;
eq(c.get(0, 0), 6);
eq(c.get(0, 1), 2);
eq(c.get(0, 2), 8);
eq(c.get(1, 0), 18);
eq(c.get(1, 1), 0);
eq(c.get(1, 2), 0);
}
{
// a = [1, 2; 3, 0]
Mat<long> a(100, 100);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(1, 0, 3);
Mat<long> c = pow(a, 17);
eq(c.get(0, 0), 77431669L);
eq(c.get(0, 1), 51708494L);
eq(c.get(1, 0), 77562741L);
eq(c.get(1, 1), 51577422L);
eq(c.get(10, 20), 0L);
}
{
Mat<long> a(2, 2);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(1, 0, 3);
long xa[] = {4, 5};
vector<long> x(xa, xa + ARRAYSIZE(xa));
vector<long> y = a * x;
eq(y[0], 14L);
eq(y[1], 12L);
}
{
// a = [1, 2, 1; 3, 3, 0]
Mat<float> a(2, 3);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(0, 2, 1);
a.set(1, 0, 3);
a.set(1, 1, 3);
a.normalize_by_row_sum();
eq(a.get(0, 0), 0.25f);
eq(a.get(0, 1), 0.4f);
eq(a.get(0, 2), 1.0f);
eq(a.get(1, 0), 0.75f);
eq(a.get(1, 1), 0.6f);
eq(a.get(1, 2), 0.0f);
}
}
{
using jcui::algorithm::SparseMat;
using jcui::algorithm::SparseMatCSR;
{
// a = [1, 2, 1; 3, 3, 0]
SparseMat<int> a(2, 3);
a.set(0, 0, 1);
a.set(0, 1, 2);
a.set(0, 2, 1);
a.set(1, 0, 3);
a.set(1, 1, 3);
SparseMatCSR<int> b(a);
int v[] = {1, 2, 1, 3, 3};
eq(b.v, VI(v));
int col[] = {0, 1, 2, 0, 1};
eq(b.col, VI(col));
int cum_n[] = {0, 3, 5};
eq(b.cum_n, VI(cum_n));
int c[] = {0, 5, 7};
int res[] = {17, 15};
eq(b * VI(c), VI(res));
}
}
{
using namespace jcui::algorithm;
{
vector<vector<int> > res = split_no_repeat(8, 7);
eq((int)res.size(), 5);
}
{
vector<vector<int> > res = split_no_repeat(10, 9);
eq((int)res.size(), 9);
}
}
}
/* FORCE BASED : */
void SimultaneousImpulseBasedConstraintSolverStrategy::SolveForceBased(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext )
{
Mat<float> qdotminus(qdot);
this->dt = dt;
Mat<float> tempInvMFext( dt*(invM * Fext) ) ;
computeConstraintsANDJacobian(c,q,qdot);
Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) );
Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian );
//---------------------------
Mat<float> invA( invGJ(A) );
//Construct b and compute the solution.
//-----------------------------------
Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) );
//-----------------------------------
//Solutions :
//------------------------------------
lambda = tempLambda;
Mat<float> udot( tConstraintsJacobian * tempLambda);
//------------------------------------
if(isnanM(udot))
udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn());
float clampingVal = 1e4f;
for(int i=1;i<=udot.getLine();i++)
{
if(udot.get(i,1) > clampingVal)
{
udot.set( clampingVal,i,1);
}
}
#ifdef debuglvl1
std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl;
transpose(udot).afficher();
transpose(lambda).afficher();
transpose( tConstraintsJacobian*lambda).afficher();
#endif
//Assumed model :
qdot += tempInvMFext + dt*(invM*udot);
float clampingValQ = 1e3f;
for(int i=1;i<=qdot.getLine();i++)
{
if( fabs_(qdot.get(i,1)) > clampingValQ)
{
qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1);
}
}
//--------------------------------------
#ifdef debuglvl2
//std::cout << " computed Pc : " << std::endl;
//(tConstraintsJacobian*tempLambda).afficher();
//std::cout << " q+ : " << std::endl;
//transpose(q).afficher();
std::cout << " qdot+ : " << std::endl;
transpose(qdot).afficher();
std::cout << " qdotminus : " << std::endl;
transpose(qdotminus).afficher();
#endif
//END OF PREVIOUS METHOD :
//--------------------------------
#ifdef debuglvl4
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
std::cout << "tConstraints : norme = " << norme2(C) << std::endl;
transpose(C).afficher();
std::cout << "Cdot : " << std::endl;
(constraintsJacobian*qdot).afficher();
std::cout << " JACOBIAN : " << std::endl;
//transpose(constraintsJacobian).afficher();
constraintsJacobian.afficher();
std::cout << " Qdot+ : " << std::endl;
transpose(qdot).afficher();
#endif
}
bool write(const std::string& fileName, const std::string& mode, const SiconosMatrix& m, const std::string& outputType)
{
// Open file and various checks
std::ofstream outfile;
if (mode == "ascii")
outfile.open(fileName.c_str(), std::ofstream::out);
else if (mode == "binary")
outfile.open(fileName.c_str(), std::ofstream::binary);
else
SiconosMatrixException::selfThrow("ioMatrix::write Incorrect mode for writing");
if (!outfile.good())
SiconosMatrixException::selfThrow("ioMatrix:: write error : Fail to open \"" + fileName + "\"");
if (m.isBlock())
SiconosMatrixException::selfThrow("ioMatrix:: write error : not yet implemented for BlockMatrix");
outfile.precision(15);
outfile.setf(std::ios::scientific);
// Writing
if (outputType != "noDim")
outfile << m.size(0) << " " << m.size(1) << std::endl;
if (m.getNum() == 1)
{
// DenseMat * p = m.dense();
DenseMat::iterator1 row;
DenseMat::iterator2 col;
double tmp;
for (unsigned int i = 0; i < m.size(0); i++)
{
for (unsigned int j = 0; j < m.size(1); j++)
{
tmp = m(i, j);
if (fabs(tmp) < std::numeric_limits<double>::min()) tmp = 0.0;
outfile << tmp << " " ;
assert(outfile.good());
}
outfile << std::endl;
}
}
else if (m.getNum() == 2)
{
TriangMat * p = m.triang();
TriangMat::iterator1 row;
for (row = p->begin1(); row != p->end1() ; ++row)
{
std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
outfile << std::endl;
}
}
else if (m.getNum() == 3)
{
SymMat * p = m.sym();
SymMat::iterator1 row;
for (row = p->begin1(); row != p->end1() ; ++row)
{
std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
outfile << std::endl;
}
}
else if (m.getNum() == 4)
{
SparseMat * p = m.sparse();
SparseMat::iterator1 row;
for (row = p->begin1(); row != p->end1() ; ++row)
{
std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
outfile << std::endl;
}
}
else
{
BandedMat * p = m.banded();
BandedMat::iterator1 row;
for (row = p->begin1(); row != p->end1() ; ++row)
{
std::copy(row.begin(), row.end(), std::ostream_iterator<double>(outfile, " "));
outfile << std::endl;
}
}
outfile.close();
return true;
}
void IterativeImpulseBasedConstraintSolverStrategy::computeConstraintsANDJacobian(std::vector<std::unique_ptr<IConstraint> >& c, const Mat<float>& q, const Mat<float>& qdot, const SparseMat<float>& invM)
{
//-------------------------------------
//-------------------------------------
//-------------------------------------
size_t size = c.size();
int n = sim->simulatedObjects.size();
float baumgarteBAS = 0.0f;//1e-1f;
float baumgarteC = -2e0f;
float baumgarteH = 0.0f;//1e-1f;
//---------------
// RESETTING :
constraintsC.clear();
constraintsJacobians.clear();
constraintsOffsets.clear();
constraintsIndexes.clear();
constraintsInvM.clear();
constraintsV.clear();
//----------------------
if( size > 0)
{
for(int k=0;k<size;k++)
{
int idA = ( c[k]->rbA.getID() );
int idB = ( c[k]->rbB.getID() );
std::vector<int> indexes(2);
//indexes are set during the creation of the simulation and they begin at 0.
indexes[0] = idA;
indexes[1] = idB;
constraintsIndexes.push_back( indexes );
//---------------------------
//Constraint :
c[k]->computeJacobians();
Mat<float> tJA(c[k]->getJacobianA());
Mat<float> tJB(c[k]->getJacobianB());
Mat<float> tC(c[k]->getConstraint());
constraintsC.push_back( tC );
int nbrlineJ = tJA.getLine();
Mat<float> JacobianAB( operatorL(tJA, tJB) );
constraintsJacobians.push_back( JacobianAB );
//----------------------------------------
//BAUMGARTE STABILIZATION
//----------------------------------------
//Contact offset :
if( c[k]->getType() == CTContactConstraint)
{
//----------------------------------------
//SLOP METHOD :
/*
float slop = 1e0f;
float pdepth = ((ContactConstraint*)(c[k].get()))->penetrationDepth;
std::cout << " ITERATIVE SOLVER :: CONTACT : pDepth = " << pdepth << std::endl;
tC *= baumgarteC/this->dt * fabs_(fabs_(pdepth)-slop);
*/
//----------------------------------------
//----------------------------------------
//DEFAULT METHOD :
tC *= baumgarteC/this->dt;
//----------------------------------------
//----------------------------------------
//METHOD 2 :
/*
float restitFactor = ( (ContactConstraint*) (c[k].get()) )->getRestitutionFactor();
std::cout << " ITERATIVE SOLVER :: CONTACT : restitFactor = " << restitFactor << std::endl;
Mat<float> Vrel( ( (ContactConstraint*) (c[k].get()) )->getRelativeVelocity() );
Mat<float> normal( ( (ContactConstraint*) (c[k].get()) )->getNormalVector() );
std::cout << " ITERATIVE SOLVER :: CONTACT : Normal vector : " << std::endl;
transpose(normal).afficher();
tC += restitFactor * transpose(Vrel)*normal;
*/
//----------------------------------------
std::cout << " ITERATIVE SOLVER :: CONTACT : Contact Constraint : " << std::endl;
transpose(tC).afficher();
std::cout << " ITERATIVE SOLVER :: CONTACT : Relative Velocity vector : " << std::endl;
transpose(( (ContactConstraint*) (c[k].get()) )->getRelativeVelocity()).afficher();
//std::cout << " ITERATIVE SOLVER :: CONTACT : First derivative of Contact Constraint : " << std::endl;
//(transpose(tJA)*).afficher();
}
//BAS JOINT :
if( c[k]->getType() == CTBallAndSocketJoint)
{
tC *= baumgarteBAS/this->dt;
}
//HINGE JOINT :
if( c[k]->getType() == CTHingeJoint)
{
tC *= baumgarteH/this->dt;
}
//BAUMGARTE OFFSET for the moments...
constraintsOffsets.push_back( tC );
//----------------------------------------
//----------------------------------------
//-------------------
// invM matrixes :
Mat<float> invmij(0.0f,12,12);
for(int k=0;k<=1;k++)
{
for(int i=1;i<=6;i++)
{
for(int j=1;j<=6;j++)
{
invmij.set( invM.get( indexes[k]*6+i, indexes[k]*6+j), k*6+i,k*6+j);
}
}
}
constraintsInvM.push_back( invmij);
//-------------------
// Vdot matrixes :
Mat<float> vij(0.0f,12,1);
for(int k=0;k<=1;k++)
{
for(int i=1;i<=6;i++)
{
vij.set( qdot.get( indexes[k]*6+i, 1), k*6+i,1);
}
}
constraintsV.push_back( vij);
}
}
}
/*----------------------------------------------------------------------------*/
System Surface::allocate_system()
{
const int n = 4*w*h;
const int m = w*h;
const long double w2 = (long double)(w)/2.;
const long double h2 = (long double)(h)/2.;
const long double sqrt_L1 = sqrtl(LAMBDA_1);
const long double sqrt_1ML1 = sqrtl(1.-LAMBDA_1);
const long double sqrt_L2 = sqrtl(LAMBDA_2);
// builds B
long double *b = new long double[n];
for(int u=0; u<w; u++)
for(int v=0; v<h; v++)
{
int i = u * h + v;
b[i] = sqrt_L1 * mu(u,v) * depth_map(u,v);
}
for(int i=m; i<n; i++)
b[i] = 0.;
Zvect *B = new Zvect(n, b);
// builds A
SparseMat *A = new SparseMat(n,m);
// part 1
for(int u=0; u<w; u++)
for(int v=0; v<h; v++)
{
int i = u * h + v;
A->add_coeff(i, i, sqrt_L1*mu(u,v));
}
// part 2
long double dx_filter[] = {-1./12., 0. , 1./12.,
-4./12., 0. , 4./12.,
-1./12., 0. , 1./12.};
for(int u=0; u<w; u++)
for(int v=0; v<h; v++)
{
int j = u * h + v;
int i = m + j;
long double x = ((long double)(u)-w2)/fx;
long double y = ((long double)(v)-h2)/fy;
const vec& n = caracs(u,v).n;
long double dx_weight = sqrt_1ML1 * (n[2] - (n[0]*x + n[1]*y));
A->add_row_coeff(i, make_filter(u,v, dx_filter, dx_weight));
A->add_coeff(i, j, -sqrt_1ML1 * n[0] / fx);
}
// part 3
long double dy_filter[] = { 1./12., 4./12. , 1./12.,
0., 0. , 0.,
-1./12., -4./12. , -1./12.,};
for(int u=0; u<w; u++)
for(int v=0; v<h; v++)
{
int j = u * h + v;
int i = 2*m + j;
long double x = ((long double)(u)-w2)/fx;
long double y = ((long double)(v)-h2)/fy;
const vec& n = caracs(u,v).n;
long double dy_weight = sqrt_1ML1 * (n[2] - (n[0]*x + n[1]*y));
A->add_row_coeff(i, make_filter(u,v, dy_filter, dy_weight));
A->add_coeff(i, j, -sqrt_1ML1 * n[1] / fy);
}
// part 4
long double laplacian_filter[] = { -1., -1. , -1.,
-1., 8. , -1.,
-1., -1. , -1.};
for(int u=0; u<w; u++)
for(int v=0; v<h; v++)
{
int i = 3*m + u * h + v;
A->add_row_coeff(i, make_filter(u,v, laplacian_filter, sqrt_L2));
}
return pair<SparseMat*, Zvect*>(A,B);
}
void SimultaneousImpulseBasedConstraintSolverStrategy::Solve(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext )
{
//std::cout << "STATE :" << std::endl;
//q.afficher();
Mat<float> qdotminus(qdot);
this->dt = dt;
//computeConstraintsJacobian(c);
Mat<float> tempInvMFext( dt*(invM * Fext) ) ;
//qdot += tempInvMFext;
//computeConstraintsJacobian(c,q,qdot);
computeConstraintsANDJacobian(c,q,qdot);
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
//std::cout << "Constraints : norme = " << norme2(C) << std::endl;
//C.afficher();
Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) );
//std::cout << "t Constraints Jacobian :" << std::endl;
//tConstraintsJacobian.afficher();
//PREVIOUS METHOD :
//--------------------------------
//David Baraff 96 STAR.pdf Interactive Simulation of Rigid Body Dynamics in Computer Graphics : Lagrange Multipliers Method :
//Construct A :
/*
Mat<float> A( (-1.0f)*tConstraintsJacobian );
Mat<float> M( invGJ( invM.SM2mat() ) );
A = operatorL( M, A);
A = operatorC( A , operatorL( constraintsJacobian, Mat<float>((float)0,constraintsJacobian.getLine(), constraintsJacobian.getLine()) ) );
*/
//----------------------------
Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian );
//---------------------------
Mat<float> invA( invGJ(A) );//invM*tConstraintsJacobian ) * constraintsJacobian );
//Construct b and compute the solution.
//----------------------------------
//Mat<float> tempLambda( invA * operatorC( Mat<float>((float)0,invA.getLine()-constraintsJacobian.getLine(),1) , (-1.0f)*(constraintsJacobian*(invM*Fext) + offset) ) );
//-----------------------------------
Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) );
//-----------------------------------
//Solutions :
//------------------------------------
//lambda = extract( &tempLambda, qdot.getLine()+1, 1, tempLambda.getLine(), 1);
//if(isnanM(lambda))
// lambda = Mat<float>(0.0f,lambda.getLine(),lambda.getColumn());
//Mat<float> udot( extract( &tempLambda, 1,1, qdot.getLine(), 1) );
//------------------------------------
lambda = tempLambda;
Mat<float> udot( tConstraintsJacobian * tempLambda);
//------------------------------------
if(isnanM(udot))
udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn());
float clampingVal = 1e4f;
for(int i=1;i<=udot.getLine();i++)
{
if(udot.get(i,1) > clampingVal)
{
udot.set( clampingVal,i,1);
}
}
#ifdef debuglvl1
std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl;
transpose(udot).afficher();
transpose(lambda).afficher();
transpose( tConstraintsJacobian*lambda).afficher();
#endif
//Assumed model :
//qdot = tempInvMFext + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1);
//qdot = tempInvMFext + udot;
qdot += tempInvMFext + invM*udot;
//qdot += invM*udot;
//qdot += tempInvMFext + udot;
float clampingValQ = 1e3f;
for(int i=1;i<=qdot.getLine();i++)
{
if( fabs_(qdot.get(i,1)) > clampingValQ)
{
qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1);
}
}
//qdot = udot;
//Assumed model if the update of the integration is applied after that constraints solver :
//qdot += dt*extract( &tempLambda, 1,1, qdot.getLine(), 1);//+tempInvMFext
Mat<float> t( dt*( S*qdot ) );
float clampQuat = 1e-1f;
float idxQuat = 3;
while(idxQuat < t.getLine())
{
for(int i=1;i<4;i++)
{
if( fabs_(t.get(idxQuat+i,1)) > clampQuat)
{
t.set( clampQuat*(t.get(idxQuat+i,1))/t.get(idxQuat+i,1), idxQuat+i,1);
}
}
idxQuat += 7;
}
//the update is done by the update via an accurate integration and we must construct q and qdot at every step
//q += t;
//--------------------------------------
//let us normalize each quaternion :
/*
idxQuat = 3;
while(idxQuat < q.getLine())
{
float scaler = q.get( idxQuat+4,1);
if(scaler != 0.0f)
{
for(int i=1;i<=4;i++)
{
q.set( q.get(idxQuat+i,1)/scaler, idxQuat+i,1);
}
}
idxQuat += 7;
}
*/
//--------------------------------------
#ifdef debuglvl2
//std::cout << " computed Pc : " << std::endl;
//(tConstraintsJacobian*tempLambda).afficher();
//std::cout << " q+ : " << std::endl;
//transpose(q).afficher();
std::cout << " qdot+ : " << std::endl;
transpose(qdot).afficher();
std::cout << " qdotminus : " << std::endl;
transpose(qdotminus).afficher();
#endif
#ifdef debuglvl3
std::cout << "SOME VERIFICATION ON : J*qdot + c = 0 : " << std::endl;
transpose(constraintsJacobian*qdot+offset).afficher();
float normC = (transpose(C)*C).get(1,1);
Mat<float> Cdot( constraintsJacobian*qdot);
float normCdot = (transpose(Cdot)*Cdot).get(1,1);
float normQdot = (transpose(qdot)*qdot).get(1,1);
//rs->ltadd(std::string("normC"),normC);
//rs->ltadd(std::string("normCdot"),normCdot);
rs->ltadd(std::string("normQdot"),normQdot);
char name[5];
for(int i=1;i<=t.getLine();i++)
{
sprintf(name,"dq%d",i);
rs->ltadd(std::string(name), t.get(i,1));
}
rs->tWriteFileTABLE();
#endif
//END OF PREVIOUS METHOD :
//--------------------------------
//--------------------------------
//--------------------------------
//Second Method :
/*
//According to Tonge Richar's Physucs For Game pdf :
Mat<float> tempLambda( (-1.0f)*invGJ( constraintsJacobian*invM.SM2mat()*tConstraintsJacobian)*constraintsJacobian*qdot );
qdot += invM*tConstraintsJacobian*tempLambda;
//qdot += tempInvMFext; //contraints not satisfied.
//qdot += tempInvMFext; //qdot+ = qdot- + dt*M-1Fext;
//Mat<float> qdotreal( qdot + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //qdotreal = qdot+ + Pc;
Mat<float> t( dt*( S*qdot ) );
q += t;
*/
//--------------------------------
//--------------------------------
//End of second method...
//--------------------------------
//--------------------------------
//--------------------------------
//THIRD METHOD :
//--------------------------------
//With reference to A Unified Framework for Rigid Body Dynamics Chap. 4.6.2.Simultaneous Force-based methods :
//which refers to Bara96 :
/*
Mat<float> iM(invM.SM2mat());
Mat<float> b((-1.0f)*constraintsJacobian*iM*Fext+offset);
Mat<float> tempLambda( invGJ( constraintsJacobian*iM*tConstraintsJacobian) * b );
//Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda+Fext));
Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda));
qdot += dt*qdoubledot;
//qdot += tempInvMFext; //contraints not satisfied.
//qdot += tempInvMFext; //qdot+ = qdot- + dt*M-1Fext;
//Mat<float> qdotreal( qdot + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //qdotreal = qdot+ + Pc;
Mat<float> t( dt*( S*qdot ) );
q += t;
std::cout << " computed Pc : " << std::endl;
(tConstraintsJacobian*tempLambda).afficher();
std::cout << " q+ : " << std::endl;
q.afficher();
std::cout << " qdot+ : " << std::endl;
qdot.afficher();
*/
//END OF THIRD METHOD :
//--------------------------------
//S.print();
//std::cout << " computed Pc : " << std::endl;
//(tConstraintsJacobian*lambda).afficher();
//std::cout << " delta state = S * qdotreal : " << std::endl;
//t.afficher();
//std::cout << " S & qdotreal : " << std::endl;
//S.print();
//qdot.afficher();
//std::cout << "invM*Fext : " << std::endl;
//tempInvMFext.afficher();
//temp.afficher();
//(constraintsJacobian*(invM*Fext)).afficher();
//(invM*Fext).afficher();
//std::cout << " A : " << std::endl;
//A.afficher();
//std::cout << " SVD A*tA : S : " << std::endl;
//SVD<float> instanceSVD(A*transpose(A));
//instanceSVD.getS().afficher();
//std::cout << " invA : " << std::endl;
//invA.afficher();
//std::cout << " LAMBDA : " << std::endl;
//lambda.afficher();
//std::cout << " qdot+ & qdot- : " << std::endl;
//qdot.afficher();
//qdotminus.afficher();
//std::cout << " q+ : " << std::endl;
//q.afficher();
#ifdef debuglvl4
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
std::cout << "tConstraints : norme = " << norme2(C) << std::endl;
transpose(C).afficher();
std::cout << "Cdot : " << std::endl;
(constraintsJacobian*qdot).afficher();
std::cout << " JACOBIAN : " << std::endl;
//transpose(constraintsJacobian).afficher();
constraintsJacobian.afficher();
std::cout << " Qdot+ : " << std::endl;
transpose(qdot).afficher();
#endif
//BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function....
//std::cout << "Constraints : norme = " << norme2(C) << std::endl;
//C.afficher();
}
SparseMat operator*(const SparseMat &m, double a) {
SparseMat prod(m.clone());
prod *= a;
return prod;
}
