1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 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);
}
};
|