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; }