C++ SparseMat类代码示例

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);
	}
      }
    }
    {
//.........这里部分代码省略.........

/*	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	
	
	
}