netgen/libsrc/linalg/densemat.cpp

1411 lines
31 KiB
C++
Raw Normal View History

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