#include #include namespace netgen { DenseMatrix :: DenseMatrix () { data = NULL; height = 0; width = 0; } DenseMatrix :: DenseMatrix (int h, int w) { if (!w) w = h; width = w; height = h; if (h*w) data = new double[h*w]; else data = 0; for (int i = 0 ; i < (h * w); i++) data[i] = 0; } /* DenseMatrix :: DenseMatrix (int h, int w, const double * d) : BaseMatrix (h, w) { int size = h * w; int i; if (size) { data = new double[size]; for (i = 0; i < size; i++) data[i] = d[i]; } else data = NULL; } */ DenseMatrix :: DenseMatrix (const DenseMatrix & m2) { data = NULL; height = width = 0; SetSize (m2.Height(), m2.Width()); memcpy (data, m2.data, sizeof(double) * Height() * Width()); } DenseMatrix :: ~DenseMatrix () { delete [] data; } void DenseMatrix :: SetSize (int h, int w) { if (!w) w = h; if (height == h && width == w) return; height = h; width = w; delete[] data; if (h*w) data = new double[h*w]; else data = NULL; } /* DenseMatrix & DenseMatrix :: operator= (const BaseMatrix & m2) { int i, j; SetSize (m2.Height(), m2.Width()); if (data) for (i = 1; i <= Height(); i++) for (j = 1; j <= Width(); j++) Set (i, j, m2(i, j)); else (*myerr) << "DenseMatrix::Operator=: Matrix not allocated" << endl; return *this; } */ DenseMatrix & DenseMatrix :: operator= (const DenseMatrix & m2) { SetSize (m2.Height(), m2.Width()); if (data) memcpy (data, m2.data, sizeof(double) * m2.Height() * m2.Width()); return *this; } DenseMatrix & DenseMatrix :: operator+= (const DenseMatrix & m2) { int i; double * p, * q; if (Height() != m2.Height() || Width() != m2.Width()) { (*myerr) << "DenseMatrix::Operator+=: Sizes don't fit" << endl; return *this; } if (data) { p = data; q = m2.data; for (i = Width() * Height(); i > 0; i--) { *p += *q; p++; q++; } } else (*myerr) << "DenseMatrix::Operator+=: Matrix not allocated" << endl; return *this; } DenseMatrix & DenseMatrix :: operator-= (const DenseMatrix & m2) { int i; double * p, * q; if (Height() != m2.Height() || Width() != m2.Width()) { (*myerr) << "DenseMatrix::Operator-=: Sizes don't fit" << endl; return *this; } if (data) { p = data; q = m2.data; for (i = Width() * Height(); i > 0; i--) { *p -= *q; p++; q++; } } else (*myerr) << "DenseMatrix::Operator-=: Matrix not allocated" << endl; return *this; } /* double & DenseMatrix :: operator() (int i, int j) { if (i >= 1 && j >= 1 && i <= height && j <= width) return Elem(i,j); else (*myerr) << "DenseMatrix: index (" << i << "," << j << ") out of range (1.." << height << ",1.." << width << ")\n"; static double dummy = 0; return dummy; } double DenseMatrix :: operator() (int i, int j) const { if (i >= 1 && j >= 1 && i <= height && j <= width) return Get(i,j); else (*myerr) << "DenseMatrix: index (" << i << "," << j << ") out of range (1.." << height << ",1.." << width << ")\n"; static double dummy = 0; return dummy; } */ DenseMatrix & DenseMatrix :: operator= (double v) { int i; double * p = data; if (data) for (i = width*height; i > 0; i--, p++) *p = v; return *this; } DenseMatrix & DenseMatrix :: operator*= (double v) { int i; double * p = data; if (data) for (i = width*height; i > 0; i--, p++) *p *= v; return *this; } double DenseMatrix :: Det () const { if (width != height) { (*myerr) << "DenseMatrix :: Det: width != height" << endl; return 0; } switch (width) { case 1: return data[0]; case 2: return data[0] * data[3] - data[1] * data[2]; case 3: return data[0] * data[4] * data[8] + data[1] * data[5] * data[6] + data[2] * data[3] * data[7] - data[0] * data[5] * data[7] - data[1] * data[3] * data[8] - data[2] * data[4] * data[6]; default: { (*myerr) << "Matrix :: Det: general size not implemented (size=" << width << ")" << endl; return 0; } } } void CalcInverse (const DenseMatrix & m1, DenseMatrix & m2) { double det; if (m1.width != m1.height) { (*myerr) << "CalcInverse: matrix not symmetric" << endl; return; } if (m1.width != m2.width || m1.height != m2.height) { (*myerr) << "CalcInverse: dim(m2) != dim(m1)" << endl; return; } if (m1.Width() <= 3) { det = m1.Det(); if (det == 0) { (*myerr) << "CalcInverse: Matrix singular" << endl; return; } det = 1.0 / det; switch (m1.width) { case 1: { m2(0,0) = det; return; } case 2: { m2(0,0) = det * m1(3); m2(1,1) = det * m1(0); m2(0,1) = -det * m1(1); m2(1,0) = - det * m1(2); return; } case 3: { m2(0, 0) = det * (m1(4) * m1(8) - m1(5) * m1(7)); m2(1, 0) = -det * (m1(3) * m1(8) - m1(5) * m1(6)); m2(2, 0) = det * (m1(3) * m1(7) - m1(4) * m1(6)); m2(0, 1) = -det * (m1(1) * m1(8) - m1(2) * m1(7)); m2(1, 1) = det * (m1(0) * m1(8) - m1(2) * m1(6)); m2(2, 1) = -det * (m1(0) * m1(7) - m1(1) * m1(6)); m2(0, 2) = det * (m1(1) * m1(5) - m1(2) * m1(4)); m2(1, 2) = -det * (m1(0) * m1(5) - m1(2) * m1(3)); m2(2, 2) = det * (m1(0) * m1(4) - m1(1) * m1(3)); return; } } } else { int i, j, k, n; n = m1.Height(); #ifdef CHOL int dots = (n > 200); // Cholesky double x; Vector p(n); m2 = m1; /* m2.SetSymmetric(); if (!m2.Symmetric()) cerr << "m should be symmetric for Cholesky" << endl; */ for (i = 1; i <= n; i++) for (j = 1; j < i; j++) m2.Elem(j, i) = m2.Get(i, j); for (i = 1; i <= n; i++) { if (dots && i % 10 == 0) (*mycout) << "." << flush; for (j = i; j <= n; j++) { x = m2.Get(i, j); const double * pik = &m2.Get(i, 1); const double * pjk = &m2.Get(j, 1); for (k = i-2; k >= 0; --k, ++pik, ++pjk) x -= (*pik) * (*pjk); // for (k = i-1; k >= 1; --k) // x -= m2.Get(j, k) * m2.Get(i, k); if (i == j) { if (x <= 0) { cerr << "Matrix indefinite 1" << endl; return; } p.Elem(i) = 1 / sqrt(x); } else { m2.Elem(j, i) = x * p.Get(i); } } } for (i = 1; i <= n; i++) m2.Elem(i, i) = 1 / p.Get(i); // check: A = L L^t // for (i = 1; i <= n; i++) // for (j = 1; j <= n; j++) // { // x = 0; // for (k = 1; k <= i && k <= j; k++) // x += m2.Get(i, k) * m2.Get(j, k); // (*testout) << "err " << i << "," << j << " = " << (m1.Get(i, j) - x) << endl; // } // calc L^{-1}, store upper triangle // DenseMatrix hm(n); // hm = m2; for (i = 1; i <= n; i++) { if (dots && i % 10 == 0) (*mycout) << "+" << flush; for (j = i; j <= n; j++) { x = 0; if (j == i) x = 1; const double * pjk = &m2.Get(j, i); const double * pik = &m2.Get(i, i); for (k = i; k < j; k++, ++pjk, ++pik) x -= *pik * *pjk; // for (k = i; k < j; k++) // x -= m2.Get(j, k) * m2.Get(i, k); m2.Elem(i, j) = x / m2.Get(j, j); } } // (*testout) << "check L^-1" << endl; // for (i = 1; i <= n; i++) // for (j = 1; j <= n; j++) // { // x = 0; // for (k = j; k <= i; k++) // x += hm.Get(i, k) * m2.Get(j, k); // (*testout) << "i, j = " << i << "," << j << " x = " << x << endl; // } // calc A^-1 = L^-T * L^-1 for (i = 1; i <= n; i++) { if (dots && i % 10 == 0) (*mycout) << "-" << flush; for (j = 1; j <= i; j++) { x = 0; k = i; if (j > i) k = j; const double * pik = &m2.Get(i, k); const double * pjk = &m2.Get(j, k); for ( ; k <= n; ++k, ++pik, ++pjk) x += *pik * *pjk; // for ( ; k <= n; k++) // x += m2.Get(i, k) * m2.Get(j, k); m2.Elem(i, j) = x; } } for (i = 1; i <= n; i++) for (j = 1; j < i; j++) m2.Elem(j, i) = m2.Get(i, j); if (dots) (*mycout) << endl; #endif // Gauss - Jordan - algorithm int r, hi; double max, hr; Array p(n); // pivot-permutation Vector hv(n); m2 = m1; /* if (m2.Symmetric()) for (i = 1; i <= n; i++) for (j = 1; j < i; j++) m2.Elem(j, i) = m2.Get(i, j); */ // Algorithm of Stoer, Einf. i. d. Num. Math, S 145 for (j = 1; j <= n; j++) p.Set(j, j); for (j = 1; j <= n; j++) { // pivot search max = fabs(m2.Get(j, j)); r = j; for (i = j+1; i <= n ;i++) if (fabs (m2.Get(i, j)) > max) { r = i; max = fabs (m2.Get(i, j)); } if (max < 1e-20) { cerr << "Inverse matrix: matrix singular" << endl; return; } r = j; // exchange rows if (r > j) { for (k = 1; k <= n; k++) { hr = m2.Get(j, k); m2.Elem(j, k) = m2.Get(r, k); m2.Elem(r, k) = hr; } hi = p.Get(j); p.Elem(j) = p.Get(r); p.Elem(r) = hi; } // transformation hr = 1 / m2.Get(j, j); for (i = 1; i <= n; i++) m2.Elem(i, j) *= hr; m2.Elem(j, j) = hr; for (k = 1; k <= n; k++) if (k != j) { for (i = 1; i <= n; i++) if (i != j) m2.Elem(i, k) -= m2.Elem(i, j) * m2.Elem(j, k); m2.Elem(j, k) *= -hr; } } // col exchange for (i = 1; i <= n; i++) { for (k = 1; k <= n; k++) hv(p.Get(k)-1) = m2.Get(i, k); for (k = 1; k <= n; k++) m2.Elem(i, k) = hv(k-1); } /* if (m1.Symmetric()) for (i = 1; i <= n; i++) for (j = 1; j < i; j++) m1.Elem(j, i) = m1.Get(i, j); m2 = 0; for (i = 1; i <= n; i++) m2.Elem(i, i) = 1; for (i = 1; i <= n; i++) { // (*mycout) << '.' << flush; q = m1.Get(i, i); for (k = 1; k <= n; k++) { m1.Elem(i, k) /= q; m2.Elem(i, k) /= q; } for (j = i+1; j <= n; j++) { q = m1.Elem(j, i); double * m1pi = &m1.Elem(i, i); double * m1pj = &m1.Elem(j, i); for (k = n; k >= i; --k, ++m1pi, ++m1pj) *m1pj -= q * (*m1pi); double * m2pi = &m2.Elem(i, 1); double * m2pj = &m2.Elem(j, 1); for (k = i; k > 0; --k, ++m2pi, ++m2pj) *m2pj -= q * (*m2pi); // for (k = 1; k <= n; k++) // { // m1.Elem(j, k) -= q * m1.Elem(i, k); // m2.Elem(j, k) -= q * m2.Elem(i, k); // } } } for (i = n; i >= 1; i--) { // (*mycout) << "+" << flush; for (j = 1; j < i; j++) { q = m1.Elem(j, i); double * m2pi = &m2.Elem(i, 1); double * m2pj = &m2.Elem(j, 1); for (k = n; k > 0; --k, ++m2pi, ++m2pj) *m2pj -= q * (*m2pi); // for (k = 1; k <= n; k++) // { // m1.Elem(j, k) -= q * m1.Elem(i, k); // m2.Elem(j, k) -= q * m2.Elem(i, k); // } } } if (m2.Symmetric()) { for (i = 1; i <= n; i++) for (j = 1; j < i; j++) m2.Elem(i, j) = m2.Elem(j, i); } */ } } void CalcAAt (const DenseMatrix & a, DenseMatrix & m2) { int n1 = a.Height(); int n2 = a.Width(); int i, j, k; double sum; const double *p, *q, *p0; if (m2.Height() != n1 || m2.Width() != n1) { (*myerr) << "CalcAAt: sizes don't fit" << endl; return; } for (i = 1; i <= n1; i++) { sum = 0; p = &a.ConstElem(i, 1); for (k = 1; k <= n2; k++) { sum += *p * *p; p++; } m2.Set(i, i, sum); p0 = &a.ConstElem(i, 1); q = a.data; for (j = 1; j < i; j++) { sum = 0; p = p0; for (k = 1; k <= n2; k++) { sum += *p * *q; p++; q++; } m2.Set(i, j, sum); m2.Set(j, i, sum); } } } void CalcAtA (const DenseMatrix & a, DenseMatrix & m2) { int n1 = a.Height(); int n2 = a.Width(); int i, j, k; double sum; if (m2.Height() != n2 || m2.Width() != n2) { (*myerr) << "CalcAtA: sizes don't fit" << endl; return; } for (i = 1; i <= n2; i++) for (j = 1; j <= n2; j++) { sum = 0; for (k = 1; k <= n1; k++) sum += a.Get(k, i) * a.Get(k, j); m2.Elem(i, j) = sum; } } void CalcABt (const DenseMatrix & a, const DenseMatrix & b, DenseMatrix & m2) { int n1 = a.Height(); int n2 = a.Width(); int n3 = b.Height(); int i, j, k; double sum; if (m2.Height() != n1 || m2.Width() != n3 || b.Width() != n2) { (*myerr) << "CalcABt: sizes don't fit" << endl; return; } double * pm2 = &m2.Elem(1, 1); const double * pa1 = &a.Get(1, 1); for (i = 1; i <= n1; i++) { const double * pb = &b.Get(1, 1); for (j = 1; j <= n3; j++) { sum = 0; const double * pa = pa1; for (k = 1; k <= n2; k++) { sum += *pa * *pb; pa++; pb++; } *pm2 = sum; pm2++; } pa1 += n2; } } void CalcAtB (const DenseMatrix & a, const DenseMatrix & b, DenseMatrix & m2) { int n1 = a.Height(); int n2 = a.Width(); int n3 = b.Width(); int i, j, k; if (m2.Height() != n2 || m2.Width() != n3 || b.Height() != n1) { (*myerr) << "CalcAtB: sizes don't fit" << endl; return; } for (i = 1; i <= n2 * n3; i++) m2.data[i-1] = 0; for (i = 1; i <= n1; i++) for (j = 1; j <= n2; j++) { const double va = a.Get(i, j); double * pm2 = &m2.Elem(j, 1); const double * pb = &b.Get(i, 1); for (k = 1; k <= n3; ++k, ++pm2, ++pb) *pm2 += va * *pb; // for (k = 1; k <= n3; k++) // m2.Elem(j, k) += va * b.Get(i, k); } /* for (i = 1; i <= n2; i++) for (j = 1; j <= n3; j++) { sum = 0; for (k = 1; k <= n1; k++) sum += a.Get(k, i) * b.Get(k, j); m2.Elem(i, j) = sum; } */ } DenseMatrix operator* (const DenseMatrix & m1, const DenseMatrix & m2) { DenseMatrix temp (m1.Height(), m2.Width()); if (m1.Width() != m2.Height()) { (*myerr) << "DenseMatrix :: operator*: Matrix Size does not fit" << endl; } else if (temp.Height() != m1.Height()) { (*myerr) << "DenseMatrix :: operator*: temp not allocated" << endl; } else { Mult (m1, m2, temp); } return temp; } void Mult (const DenseMatrix & m1, const DenseMatrix & m2, DenseMatrix & m3) { double sum; double *p1, *p1s, *p1sn, *p1snn, *p2, *p2s, *p2sn, *p3; if (m1.Width() != m2.Height() || m1.Height() != m3.Height() || m2.Width() != m3.Width() ) { (*myerr) << "DenseMatrix :: Mult: Matrix Size does not fit" << endl; (*myerr) << "m1: " << m1.Height() << " x " << m1.Width() << endl; (*myerr) << "m2: " << m2.Height() << " x " << m2.Width() << endl; (*myerr) << "m3: " << m3.Height() << " x " << m3.Width() << endl; return; } /* else if (m1.Symmetric() || m2.Symmetric() || m3.Symmetric()) { (*myerr) << "DenseMatrix :: Mult: not implemented for symmetric matrices" << endl; return; } */ else { // int i, j, k; int n1 = m1.Height(); int n2 = m2.Width(); int n3 = m1.Width(); /* for (i = n1 * n2-1; i >= 0; --i) m3.data[i] = 0; const double * pm1 = &m1.Get(1, 1); for (i = 1; i <= n1; i++) { const double * pm2 = &m2.Get(1, 1); double * pm3i = &m3.Elem(i, 1); for (j = 1; j <= n3; j++) { const double vm1 = *pm1; ++pm1; // const double vm1 = m1.Get(i, j); double * pm3 = pm3i; // const double * pm2 = &m2.Get(j, 1); for (k = 0; k < n2; k++) { *pm3 += vm1 * *pm2; ++pm2; ++pm3; } // for (k = 1; k <= n2; k++) // m3.Elem(i, k) += m1.Get(i, j) * m2.Get(j, k); } } */ /* for (i = 1; i <= n1; i++) for (j = 1; j <= n2; j++) { sum = 0; for (k = 1; k <= n3; k++) sum += m1.Get(i, k) * m2.Get(k, j); m3.Set(i, j, sum); } */ /* for (i = 1; i <= n1; i++) { const double pm1i = &m1.Get(i, 1); const double pm2j = &m2.Get(1, 1); for (j = 1; j <= n2; j++) { double sum = 0; const double * pm1 = pm1i; const double * pm2 = pm2j; pm2j++; for (k = 1; k <= n3; k++) { sum += *pm1 * *pm2; ++pm1; pm2 += n2; } m3.Set (i, j, sum); } } */ p3 = m3.data; p1s = m1.data; p2sn = m2.data + n2; p1snn = p1s + n1 * n3; while (p1s != p1snn) { p1sn = p1s + n3; p2s = m2.data; while (p2s != p2sn) { sum = 0; p1 = p1s; p2 = p2s; p2s++; while (p1 != p1sn) { sum += *p1 * *p2; p1++; p2 += n2; } *p3++ = sum; } p1s = p1sn; } } } DenseMatrix operator+ (const DenseMatrix & m1, const DenseMatrix & m2) { DenseMatrix temp (m1.Height(), m1.Width()); int i, j; if (m1.Width() != m2.Width() || m1.Height() != m2.Height()) { (*myerr) << "BaseMatrix :: operator+: Matrix Size does not fit" << endl; } else if (temp.Height() != m1.Height()) { (*myerr) << "BaseMatrix :: operator+: temp not allocated" << endl; } else { for (i = 1; i <= m1.Height(); i++) for (j = 1; j <= m1.Width(); j++) { temp.Set(i, j, m1.Get(i, j) + m2.Get(i, j)); } } return temp; } void Transpose (const DenseMatrix & m1, DenseMatrix & m2) { int w = m1.Width(); int h = m1.Height(); int i, j; m2.SetSize (w, h); double * pm2 = &m2.Elem(1, 1); for (j = 1; j <= w; j++) { const double * pm1 = &m1.Get(1, j); for (i = 1; i <= h; i++) { *pm2 = *pm1; pm2 ++; pm1 += w; } } } /* void DenseMatrix :: Mult (const Vector & v, Vector & prod) const { double sum, val; const double * mp, * sp; double * dp; // const Vector & v = bv.CastToVector(); // Vector & prod = bprod.CastToVector(); int n = Height(); int m = Width(); if (prod.Size() != n) prod.SetSize (n); #ifdef DEVELOP if (!n) { cout << "DenseMatrix::Mult mheight = 0" << endl; } if (!m) { cout << "DenseMatrix::Mult mwidth = 0" << endl; } if (m != v.Size()) { (*myerr) << "\nMatrix and Vector don't fit" << endl; } else if (Height() != prod.Size()) { (*myerr) << "Base_Matrix::operator*(Vector): prod vector not ok" << endl; } else #endif { if (Symmetric()) { int i, j; for (i = 1; i <= n; i++) { sp = &v.Get(1); dp = &prod.Elem(1); mp = &Get(i, 1); val = v.Get(i); sum = Get(i, i) * val; for (j = 1; j < i; ++j, ++mp, ++sp, ++dp) { sum += *mp * *sp; *dp += val * *mp; } prod.Elem(i) = sum; } } else { mp = data; dp = &prod.Elem(1); for (int i = 1; i <= n; i++) { sum = 0; sp = &v.Get(1); for (int j = 1; j <= m; j++) { // sum += Get(i,j) * v.Get(j); sum += *mp * *sp; mp++; sp++; } // prod.Set (i, sum); *dp = sum; dp++; } } } } */ void DenseMatrix :: MultTrans (const Vector & v, Vector & prod) const { // const Vector & v = (const Vector&)bv; // .CastToVector(); // Vector & prod = (Vector & )bprod; // .CastToVector(); /* if (Height() != v.Size()) { (*myerr) << "\nMatrix and Vector don't fit" << endl; } else if (Width() != prod.Size()) { (*myerr) << "Base_Matrix::operator*(Vector): prod vector not ok" << endl; } else */ { int i, j; int w = Width(), h = Height(); if (prod.Size() != w) prod.SetSize (w); const double * pmat = &Get(1, 1); const double * pv = &v(0); prod = 0; for (i = 1; i <= h; i++) { double val = *pv; ++pv; double * pprod = &prod(0); for (j = w-1; j >= 0; --j, ++pmat, ++pprod) { *pprod += val * *pmat; } } /* double sum; for (i = 1; i <= Width(); i++) { sum = 0; for (int j = 1; j <= Height(); j++) sum += Get(j, i) * v.Get(j); prod.Set (i, sum); } */ } } void DenseMatrix :: Residuum (const Vector & x, const Vector & b, Vector & res) const { double sum; // const Vector & x = bx.CastToVector(); // const Vector & b = bb.CastToVector(); // Vector & res = bres.CastToVector(); res.SetSize (Height()); if (Width() != x.Size() || Height() != b.Size()) { (*myerr) << "\nMatrix and Vector don't fit" << endl; } else if (Height() != res.Size()) { (*myerr) << "Base_Matrix::operator*(Vector): prod vector not ok" << endl; } else { int h = Height(); int w = Width(); const double * mp = &Get(1, 1); for (int i = 1; i <= h; i++) { sum = b(i-1); const double * xp = &x(0); for (int j = 1; j <= w; ++j, ++mp, ++xp) sum -= *mp * *xp; res(i-1) = sum; } } } #ifdef ABC double DenseMatrix :: EvaluateBilinearform (const Vector & hx) const { double sum = 0, hsum; // const Vector & hx = x.CastToVector(); int i, j; if (Width() != hx.Size() || Height() != hx.Size()) { (*myerr) << "Matrix::EvaluateBilinearForm: sizes don't fit" << endl; } else { for (i = 1; i <= Height(); i++) { hsum = 0; for (j = 1; j <= Height(); j++) { hsum += Get(i, j) * hx.Get(j); } sum += hsum * hx.Get(i); } } // testout << "sum = " << sum << endl; return sum; } void DenseMatrix :: MultElementMatrix (const Array & pnum, const Vector & hx, Vector & hy) { int i, j; // const Vector & hx = x.CastToVector(); // Vector & hy = y.CastToVector(); if (Symmetric()) { for (i = 1; i <= Height(); i++) { for (j = 1; j < i; j++) { hy.Elem(pnum.Get(i)) += Get(i, j) * hx.Get(pnum.Get(j)); hy.Elem(pnum.Get(j)) += Get(i, j) * hx.Get(pnum.Get(i)); } hy.Elem(pnum.Get(j)) += Get(i, i) * hx.Get(pnum.Get(i)); } } else for (i = 1; i <= Height(); i++) for (j = 1; j <= Width(); j++) hy.Elem(pnum.Get(i)) += Get(i, j) * hx.Get(pnum.Get(j)); } void DenseMatrix :: MultTransElementMatrix (const Array & pnum, const Vector & hx, Vector & hy) { int i, j; // const Vector & hx = x.CastToVector(); // Vector & hy = y.CastToVector(); if (Symmetric()) MultElementMatrix (pnum, hx, hy); else for (i = 1; i <= Height(); i++) for (j = 1; j <= Width(); j++) hy.Elem(pnum.Get(i)) += Get(j, i) * hx.Get(pnum.Get(j)); } #endif void DenseMatrix :: Solve (const Vector & v, Vector & sol) const { DenseMatrix temp (*this); temp.SolveDestroy (v, sol); } void DenseMatrix :: SolveDestroy (const Vector & v, Vector & sol) { double q; if (Width() != Height()) { (*myerr) << "SolveDestroy: Matrix not square"; return; } if (Width() != v.Size()) { (*myerr) << "SolveDestroy: Matrix and Vector don't fit"; return; } sol = v; if (Height() != sol.Size()) { (*myerr) << "SolveDestroy: Solution Vector not ok"; return; } if (0 /* Symmetric() */) { // Cholesky factorization int i, j, k, n; n = Height(); // Cholesky double x; Vector p(n); for (i = 1; i <= n; i++) for (j = 1; j < i; j++) Elem(j, i) = Get(i, j); for (i = 1; i <= n; i++) { // (*mycout) << "." << flush; for (j = i; j <= n; j++) { x = Get(i, j); const double * pik = &Get(i, 1); const double * pjk = &Get(j, 1); for (k = i-2; k >= 0; --k, ++pik, ++pjk) x -= (*pik) * (*pjk); // for (k = i-1; k >= 1; --k) // x -= Get(j, k) * Get(i, k); if (i == j) { if (x <= 0) { cerr << "Matrix indefinite" << endl; return; } p(i-1) = 1 / sqrt(x); } else { Elem(j, i) = x * p(i-1); } } } for (int i = 1; i <= n; i++) Elem(i, i) = 1 / p(i-1); // A = L L^t // L stored in left-lower triangle sol = v; // Solve L sol = sol for (int i = 1; i <= n; i++) { double val = sol(i-1); const double * pij = &Get(i, 1); const double * solj = &sol(0); for (int j = 1; j < i; j++, ++pij, ++solj) val -= *pij * *solj; // for (j = 1; j < i; j++) // val -= Get(i, j) * sol.Get(j); sol(i-1) = val / Get(i, i); } // Solve L^t sol = sol for (int i = n; i >= 1; i--) { double val = sol(i-1) / Get(i, i); sol(i-1) = val; double * solj = &sol(0); const double * pij = &Get(i, 1); for (j = 1; j < i; ++j, ++pij, ++solj) *solj -= val * *pij; // for (j = 1; j < i; j++) // sol.Elem(j) -= Get(i, j) * val; } } else { // (*mycout) << "gauss" << endl; int n = Height(); for (int i = 1; i <= n; i++) { for (int j = i+1; j <= n; j++) { q = Get(j,i) / Get(i,i); if (q) { const double * pik = &Get(i, i+1); double * pjk = &Elem(j, i+1); for (int k = i+1; k <= n; ++k, ++pik, ++pjk) *pjk -= q * *pik; // for (k = i+1; k <= Height(); k++) // Elem(j, k) -= q * Get(i,k); sol(j-1) -= q * sol(i-1); } } } for (int i = n; i >= 1; i--) { q = sol(i-1); for (int j = i+1; j <= n; j++) q -= Get(i,j) * sol(j-1); sol(i-1) = q / Get(i,i); } } } /* BaseMatrix * DenseMatrix :: Copy () const { return new DenseMatrix (*this); } */ ostream & operator<< (ostream & ost, const DenseMatrix & m) { for (int i = 0; i < m.Height(); i++) { for (int j = 0; j < m.Width(); j++) ost << m.Get(i+1,j+1) << " "; ost << endl; } return ost; } }