add vcglib

vcglib/wrap/math/system_interface_ldl.h

@@ -0,0 +1,155 @@
+extern "C"{
+#include <ldl.h>
+}
+#include <vector>
+#include <map>
+#include <algorithm>
+#include <sparse_matrix.h>
+
+class SystemLDL:SparseMatrix<double>{
+private:
+	typedef std::map<IndexType,int> mapType;
+	std::vector<double> _B;
+	std::vector<double> _X;
+	
+	std::vector<double> Lx,D,Y ;
+    std::vector<int> Li,Lp,Parent,Lnz,Flag,Pattern;
+	
+	mapType _M;
+	
+public:
+
+///allocate the resources for the system of equations
+void Initalize(int dimension)
+{
+  _dimension=dimension;
+  _Ap.resize(_dimension+1);
+  _M.clear();
+  _B.resize(_dimension);
+  _X.resize(_dimension);
+}
+
+double &A(int row,int col)
+{
+	
+	IndexType I=IndexType(row,col);
+	mapType::const_iterator ci=_M.find(I);
+	if (ci==_M.end())
+	{
+		std::swap<int>(I.first,I.second);
+		ci=_M.find(I);
+	}
+	assert(ci!=_M.end());
+	int index=(*ci).second;
+
+	return(_Ax[index]);
+}
+
+double &B(int i)
+{return(_B[i]);}
+
+double &X(int i)
+{return (_X[i]);}
+
+
+void Solve()
+{
+	int d,i;
+
+  //   /* factorize A into LDL' (P and Pinv not used) */
+    ldl_symbolic (_dimension, &(*_Ap.begin()), &(*_Ai.begin()), &(*Lp.begin()), 
+		&(*Parent.begin()), &(*Lnz.begin()), &(*Flag.begin()), NULL, NULL) ;
+
+  //  printf ("Nonzeros in L, excluding diagonal: %d\n", Lp [_dimension]) ;
+
+	d = ldl_numeric (_dimension, &(*_Ap.begin()), &(*_Ai.begin()), &(*_Ax.begin()), &(*Lp.begin())
+		, &(*Parent.begin()), &(*Lnz.begin()), &(*Li.begin()), &(*Lx.begin()), 
+		&(*D.begin()), &(*Y.begin()), &(*Pattern.begin()),
+	&(*Flag.begin()), NULL, NULL) ;
+
+    if (d == _dimension)
+    {
+	/* solve Ax=b, overwriting b with the solution x */
+	ldl_lsolve (_dimension, &(*_B.begin()), &(*Lp.begin()), &(*Li.begin()), &(*Lx.begin())) ;
+	ldl_dsolve (_dimension, &(*_B.begin()), (&*D.begin()) );
+	ldl_ltsolve (_dimension, &(*_B.begin()), &(*Lp.begin()), &(*Li.begin()), &(*Lx.begin())) ;
+
+	for (i = 0 ; i < _dimension ; i++) 
+		_X[i]=_B[i];//printf ("x [%d] = %g\n", i, b [i]) ;
+    }
+	 else///dl_numeric failed
+    {
+		assert(0);
+    }
+}
+
+bool IsSymmetric()
+{return true;}
+
+void Zero()
+{
+	for (int i=0;i<Size();i++)
+		_Ax[i]=0;
+}
+
+int Size(){return (_dimension);}
+
+///K is symmetric positice definite matrix
+void CreateSparse(std::vector<IndexType> Entries)
+{
+	_Ax.clear();
+	_Ai.clear();
+	int _nonzero=0;
+
+	///put the index of vertices for each edge 
+	///in the right order for simmetry of the sistem
+	std::vector<IndexType>::iterator Vi;
+	for (Vi=Entries.begin();Vi<Entries.end();Vi++)
+	{
+		assert((*Vi).first>=0);
+		assert((*Vi).second>=0);
+		if ((*Vi).first>(*Vi).second)
+			std::swap<int>((*Vi).first,(*Vi).second);
+	}
+	
+	///the sort and erase duplicates
+	std::sort(Entries.begin(),Entries.end());
+	std::vector<IndexType>::iterator Vend=std::unique(Entries.begin(),Entries.end());
+	Entries.erase(Vend,Entries.end());
+	
+	_Ax.resize(Entries.size());
+	_Ai.resize(Entries.size());
+	_M.clear();
+
+	int col=0;
+	int i=0;
+	Vi=Entries.begin();
+	while (Vi<Entries.end())
+	{
+		col=(*Vi).first;
+		_Ap[i]=_nonzero;
+		//go to next colummn
+		while ((col==(*Vi).first)&&(Vi<Entries.end()))
+		{
+			IndexType I=IndexType((*Vi).first,(*Vi).second);
+			_M.insert(std::pair<IndexType,int>(I,_nonzero));
+			_Ai[_nonzero]=(*Vi).second;
+			_nonzero++;
+			Vi++;
+		}
+		i++;
+	}
+
+	_Ap[_dimension]=_nonzero;
+	Lx.resize(_nonzero);
+	D.resize(_dimension);
+	Y.resize(_dimension);
+    Li.resize(_nonzero);
+	Lp.resize(_dimension+1);
+	Parent.resize(_dimension);
+	Lnz.resize(_dimension);
+	Flag.resize(_dimension);
+	Pattern.resize(_dimension);
+}
+
+};