phynexis-deps/mfem
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
f7b59cd1815d979aa430b3ca7ea62045bb12f3fa
mfem/linalg/densemat.cpp
T
Veselin Dobrev 40378a046b Introduce a new Memory class for handling host + device allocations 
and transfers.

The Memory class is now used by some MFEM classes (like Array and
Vector) which can be used on the Device. Such classes now provide
methods to access the underlying Memory object, e.g. GetMemory.

Updated ex1/ex1p and ex6/ex6p to not need to enable/disable the
Device at specific points -- the Device is now enabled just at the
start. Also, the same examples can now run on Device (e.g. -d cuda)
without the partial assembly option (-pa) -- full assembly will
be still done on CPU but the sparse matrix action and vector
operations will be done using the Device.

Reverted changes in class DenseMatrix related to using the Device.
At this point, DenseMatrix operations are only used for small matrices
and using the Device in this case is not a good option.
2019-05-16 14:53:39 -07:00

4518 lines
102 KiB
C++
Raw Blame History

// Copyright (c) 2010, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-443211. All Rights
// reserved. See file COPYRIGHT for details.
//
// This file is part of the MFEM library. For more information and source code
// availability see http://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the GNU Lesser General Public License (as published by the Free
// Software Foundation) version 2.1 dated February 1999.
// Implementation of data types dense matrix, inverse dense matrix
#include "vector.hpp"
#include "matrix.hpp"
#include "densemat.hpp"
#include "../general/table.hpp"
#include "../general/globals.hpp"
#include <iostream>
#include <iomanip>
#include <limits>
#include <algorithm>
#include <cstdlib>
#if defined(_MSC_VER) && (_MSC_VER < 1800)
#include <float.h>
#define copysign _copysign
#endif
#ifdef MFEM_USE_LAPACK
extern "C" void
dgemm_(char *, char *, int *, int *, int *, double *, double *,
int *, double *, int *, double *, double *, int *);
extern "C" void
dgetrf_(int *, int *, double *, int *, int *, int *);
extern "C" void
dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *);
extern "C" void
dgetri_(int *N, double *A, int *LDA, int *IPIV, double *WORK,
int *LWORK, int *INFO);
extern "C" void
dsyevr_(char *JOBZ, char *RANGE, char *UPLO, int *N, double *A, int *LDA,
double *VL, double *VU, int *IL, int *IU, double *ABSTOL, int *M,
double *W, double *Z, int *LDZ, int *ISUPPZ, double *WORK, int *LWORK,
int *IWORK, int *LIWORK, int *INFO);
extern "C" void
dsyev_(char *JOBZ, char *UPLO, int *N, double *A, int *LDA, double *W,
double *WORK, int *LWORK, int *INFO);
extern "C" void
dsygv_ (int *ITYPE, char *JOBZ, char *UPLO, int * N, double *A, int *LDA,
double *B, int *LDB, double *W, double *WORK, int *LWORK, int *INFO);
extern "C" void
dgesvd_(char *JOBU, char *JOBVT, int *M, int *N, double *A, int *LDA,
double *S, double *U, int *LDU, double *VT, int *LDVT, double *WORK,
int *LWORK, int *INFO);
#endif
namespace mfem
{
using namespace std;
DenseMatrix::DenseMatrix() : Matrix(0)
{
data = NULL;
capacity = 0;
}
DenseMatrix::DenseMatrix(const DenseMatrix &m) : Matrix(m.height, m.width)
{
const int hw = height * width;
if (hw > 0)
{
MFEM_ASSERT(m.data, "invalid source matrix");
data = new double[hw];
capacity = hw;
std::memcpy(data, m.data, sizeof(double)*hw);
}
else
{
data = NULL;
capacity = 0;
}
}
DenseMatrix::DenseMatrix(int s) : Matrix(s)
{
MFEM_ASSERT(s >= 0, "invalid DenseMatrix size: " << s);
capacity = s*s;
if (capacity > 0)
{
data = new double[capacity](); // init with zeroes
}
else
{
data = NULL;
}
}
DenseMatrix::DenseMatrix(int m, int n) : Matrix(m, n)
{
MFEM_ASSERT(m >= 0 && n >= 0,
"invalid DenseMatrix size: " << m << " x " << n);
capacity = m*n;
if (capacity > 0)
{
data = new double[capacity](); // init with zeroes
}
else
{
data = NULL;
}
}
DenseMatrix::DenseMatrix(const DenseMatrix &mat, char ch)
: Matrix(mat.width, mat.height)
{
capacity = height*width;
if (capacity > 0)
{
data = new double[capacity];
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
(*this)(i,j) = mat(j,i);
}
}
}
else
{
data = NULL;
}
}
void DenseMatrix::SetSize(int h, int w)
{
MFEM_ASSERT(h >= 0 && w >= 0,
"invalid DenseMatrix size: " << h << " x " << w);
if (Height() == h && Width() == w)
{
return;
}
height = h;
width = w;
const int hw = h*w;
if (hw > std::abs(capacity))
{
if (capacity > 0)
{
delete [] data;
}
capacity = hw;
data = new double[hw](); // init with zeroes
}
}
double &DenseMatrix::Elem(int i, int j)
{
return (*this)(i,j);
}
const double &DenseMatrix::Elem(int i, int j) const
{
return (*this)(i,j);
}
void DenseMatrix::Mult(const double *x, double *y) const
{
if (width == 0)
{
for (int row = 0; row < height; row++)
{
y[row] = 0.0;
}
return;
}
double *d_col = data;
double x_col = x[0];
for (int row = 0; row < height; row++)
{
y[row] = x_col*d_col[row];
}
d_col += height;
for (int col = 1; col < width; col++)
{
x_col = x[col];
for (int row = 0; row < height; row++)
{
y[row] += x_col*d_col[row];
}
d_col += height;
}
}
void DenseMatrix::Mult(const Vector &x, Vector &y) const
{
MFEM_ASSERT(height == y.Size() && width == x.Size(),
"incompatible dimensions");
Mult((const double *)x, (double *)y);
}
double DenseMatrix::operator *(const DenseMatrix &m) const
{
MFEM_ASSERT(Height() == m.Height() && Width() == m.Width(),
"incompatible dimensions");
const int hw = height * width;
double a = 0.0;
for (int i = 0; i < hw; i++)
{
a += data[i] * m.data[i];
}
return a;
}
void DenseMatrix::MultTranspose(const double *x, double *y) const
{
double *d_col = data;
for (int col = 0; col < width; col++)
{
double y_col = 0.0;
for (int row = 0; row < height; row++)
{
y_col += x[row]*d_col[row];
}
y[col] = y_col;
d_col += height;
}
}
void DenseMatrix::MultTranspose(const Vector &x, Vector &y) const
{
MFEM_ASSERT(height == x.Size() && width == y.Size(),
"incompatible dimensions");
MultTranspose((const double *)x, (double *)y);
}
void DenseMatrix::AddMult(const Vector &x, Vector &y) const
{
MFEM_ASSERT(height == y.Size() && width == x.Size(),
"incompatible dimensions");
const double *xp = x, *d_col = data;
double *yp = y;
for (int col = 0; col < width; col++)
{
double x_col = xp[col];
for (int row = 0; row < height; row++)
{
yp[row] += x_col*d_col[row];
}
d_col += height;
}
}
void DenseMatrix::AddMultTranspose(const Vector &x, Vector &y) const
{
MFEM_ASSERT(height == x.Size() && width == y.Size(),
"incompatible dimensions");
const double *d_col = data;
for (int col = 0; col < width; col++)
{
double y_col = 0.0;
for (int row = 0; row < height; row++)
{
y_col += x[row]*d_col[row];
}
y[col] += y_col;
d_col += height;
}
}
void DenseMatrix::AddMult_a(double a, const Vector &x, Vector &y) const
{
MFEM_ASSERT(height == y.Size() && width == x.Size(),
"incompatible dimensions");
const double *xp = x, *d_col = data;
double *yp = y;
for (int col = 0; col < width; col++)
{
const double x_col = a*xp[col];
for (int row = 0; row < height; row++)
{
yp[row] += x_col*d_col[row];
}
d_col += height;
}
}
void DenseMatrix::AddMultTranspose_a(double a, const Vector &x,
Vector &y) const
{
MFEM_ASSERT(height == x.Size() && width == y.Size(),
"incompatible dimensions");
const double *d_col = data;
for (int col = 0; col < width; col++)
{
double y_col = 0.0;
for (int row = 0; row < height; row++)
{
y_col += x[row]*d_col[row];
}
y[col] += a * y_col;
d_col += height;
}
}
double DenseMatrix::InnerProduct(const double *x, const double *y) const
{
double prod = 0.0;
for (int i = 0; i < height; i++)
{
double Axi = 0.0;
for (int j = 0; j < width; j++)
{
Axi += (*this)(i,j) * x[j];
}
prod += y[i] * Axi;
}
return prod;
}
// LeftScaling this = diag(s) * this
void DenseMatrix::LeftScaling(const Vector & s)
{
double * it_data = data;
for (int j = 0; j < width; ++j)
{
for (int i = 0; i < height; ++i)
{
*(it_data++) *= s(i);
}
}
}
// InvLeftScaling this = diag(1./s) * this
void DenseMatrix::InvLeftScaling(const Vector & s)
{
double * it_data = data;
for (int j = 0; j < width; ++j)
{
for (int i = 0; i < height; ++i)
{
*(it_data++) /= s(i);
}
}
}
// RightScaling: this = this * diag(s);
void DenseMatrix::RightScaling(const Vector & s)
{
double sj;
double * it_data = data;
for (int j = 0; j < width; ++j)
{
sj = s(j);
for (int i = 0; i < height; ++i)
{
*(it_data++) *= sj;
}
}
}
// InvRightScaling: this = this * diag(1./s);
void DenseMatrix::InvRightScaling(const Vector & s)
{
double * it_data = data;
for (int j = 0; j < width; ++j)
{
const double sj = 1./s(j);
for (int i = 0; i < height; ++i)
{
*(it_data++) *= sj;
}
}
}
// SymmetricScaling this = diag(sqrt(s)) * this * diag(sqrt(s))
void DenseMatrix::SymmetricScaling(const Vector & s)
{
if (height != width || s.Size() != height)
{
mfem_error("DenseMatrix::SymmetricScaling");
}
double * ss = new double[width];
double * it_s = s.GetData();
double * it_ss = ss;
for ( double * end_s = it_s + width; it_s != end_s; ++it_s)
{
*(it_ss++) = sqrt(*it_s);
}
double * it_data = data;
for (int j = 0; j < width; ++j)
{
for (int i = 0; i < height; ++i)
{
*(it_data++) *= ss[i]*ss[j];
}
}
delete[] ss;
}
// InvSymmetricScaling this = diag(sqrt(1./s)) * this * diag(sqrt(1./s))
void DenseMatrix::InvSymmetricScaling(const Vector & s)
{
if (height != width || s.Size() != width)
{
mfem_error("DenseMatrix::SymmetricScaling");
}
double * ss = new double[width];
double * it_s = s.GetData();
double * it_ss = ss;
for (double * end_s = it_s + width; it_s != end_s; ++it_s)
{
*(it_ss++) = 1./sqrt(*it_s);
}
double * it_data = data;
for (int j = 0; j < width; ++j)
{
for (int i = 0; i < height; ++i)
{
*(it_data++) *= ss[i]*ss[j];
}
}
delete[] ss;
}
double DenseMatrix::Trace() const
{
#ifdef MFEM_DEBUG
if (Width() != Height())
{
mfem_error("DenseMatrix::Trace() : not a square matrix!");
}
#endif
double t = 0.0;
for (int i = 0; i < width; i++)
{
t += (*this)(i, i);
}
return t;
}
MatrixInverse *DenseMatrix::Inverse() const
{
return new DenseMatrixInverse(*this);
}
double DenseMatrix::Det() const
{
MFEM_ASSERT(Height() == Width() && Height() > 0,
"The matrix must be square and "
<< "sized larger than zero to compute the determinant."
<< " Height() = " << Height()
<< ", Width() = " << Width());
switch (Height())
{
case 1:
return data[0];
case 2:
return data[0] * data[3] - data[1] * data[2];
case 3:
{
const double *d = data;
return
d[0] * (d[4] * d[8] - d[5] * d[7]) +
d[3] * (d[2] * d[7] - d[1] * d[8]) +
d[6] * (d[1] * d[5] - d[2] * d[4]);
}
case 4:
{
const double *d = data;
return
d[ 0] * (d[ 5] * (d[10] * d[15] - d[11] * d[14]) -
d[ 9] * (d[ 6] * d[15] - d[ 7] * d[14]) +
d[13] * (d[ 6] * d[11] - d[ 7] * d[10])
) -
d[ 4] * (d[ 1] * (d[10] * d[15] - d[11] * d[14]) -
d[ 9] * (d[ 2] * d[15] - d[ 3] * d[14]) +
d[13] * (d[ 2] * d[11] - d[ 3] * d[10])
) +
d[ 8] * (d[ 1] * (d[ 6] * d[15] - d[ 7] * d[14]) -
d[ 5] * (d[ 2] * d[15] - d[ 3] * d[14]) +
d[13] * (d[ 2] * d[ 7] - d[ 3] * d[ 6])
) -
d[12] * (d[ 1] * (d[ 6] * d[11] - d[ 7] * d[10]) -
d[ 5] * (d[ 2] * d[11] - d[ 3] * d[10]) +
d[ 9] * (d[ 2] * d[ 7] - d[ 3] * d[ 6])
);
}
default:
{
// In the general case we compute the determinant from the LU
// decomposition.
DenseMatrixInverse lu_factors(*this);
return lu_factors.Det();
}
}
// not reachable
}
double DenseMatrix::Weight() const
{
if (Height() == Width())
{
// return fabs(Det());
return Det();
}
else if ((Height() == 2) && (Width() == 1))
{
return sqrt(data[0] * data[0] + data[1] * data[1]);
}
else if ((Height() == 3) && (Width() == 1))
{
return sqrt(data[0] * data[0] + data[1] * data[1] + data[2] * data[2]);
}
else if ((Height() == 3) && (Width() == 2))
{
const double *d = data;
double E = d[0] * d[0] + d[1] * d[1] + d[2] * d[2];
double G = d[3] * d[3] + d[4] * d[4] + d[5] * d[5];
double F = d[0] * d[3] + d[1] * d[4] + d[2] * d[5];
return sqrt(E * G - F * F);
}
mfem_error("DenseMatrix::Weight()");
return 0.0;
}
void DenseMatrix::Set(double alpha, const double *A)
{
const int s = Width()*Height();
for (int i = 0; i < s; i++)
{
data[i] = alpha*A[i];
}
}
void DenseMatrix::Add(const double c, const DenseMatrix &A)
{
for (int j = 0; j < Width(); j++)
{
for (int i = 0; i < Height(); i++)
{
(*this)(i,j) += c * A(i,j);
}
}
}
DenseMatrix &DenseMatrix::operator=(double c)
{
const int s = Height()*Width();
for (int i = 0; i < s; i++)
{
data[i] = c;
}
return *this;
}
DenseMatrix &DenseMatrix::operator=(const double *d)
{
const int s = Height()*Width();
for (int i = 0; i < s; i++)
{
data[i] = d[i];
}
return *this;
}
DenseMatrix &DenseMatrix::operator=(const DenseMatrix &m)
{
SetSize(m.height, m.width);
const int hw = height * width;
for (int i = 0; i < hw; i++)
{
data[i] = m.data[i];
}
return *this;
}
DenseMatrix &DenseMatrix::operator+=(const double *m)
{
const int hw = Height()*Width();
for (int i = 0; i < hw; i++)
{
data[i] += m[i];
}
return *this;
}
DenseMatrix &DenseMatrix::operator+=(const DenseMatrix &m)
{
MFEM_ASSERT(Height() == m.Height() && Width() == m.Width(),
"incompatible matrix sizes.");
return *this += m.GetData();
}
DenseMatrix &DenseMatrix::operator-=(const DenseMatrix &m)
{
for (int j = 0; j < width; j++)
{
for (int i = 0; i < height; i++)
{
(*this)(i, j) -= m(i, j);
}
}
return *this;
}
DenseMatrix &DenseMatrix::operator*=(double c)
{
int s = Height()*Width();
for (int i = 0; i < s; i++)
{
data[i] *= c;
}
return *this;
}
void DenseMatrix::Neg()
{
const int hw = Height() * Width();
for (int i = 0; i < hw; i++)
{
data[i] = -data[i];
}
}
void DenseMatrix::Invert()
{
#ifdef MFEM_DEBUG
if (Height() <= 0 || Height() != Width())
{
mfem_error("DenseMatrix::Invert()");
}
#endif
#ifdef MFEM_USE_LAPACK
int *ipiv = new int[width];
int lwork = -1;
double qwork, *work;
int info;
dgetrf_(&width, &width, data, &width, ipiv, &info);
if (info)
{
mfem_error("DenseMatrix::Invert() : Error in DGETRF");
}
dgetri_(&width, data, &width, ipiv, &qwork, &lwork, &info);
lwork = (int) qwork;
work = new double[lwork];
dgetri_(&width, data, &width, ipiv, work, &lwork, &info);
if (info)
{
mfem_error("DenseMatrix::Invert() : Error in DGETRI");
}
delete [] work;
delete [] ipiv;
#else
int c, i, j, n = Width();
double a, b;
Array<int> piv(n);
for (c = 0; c < n; c++)
{
a = fabs((*this)(c, c));
i = c;
for (j = c + 1; j < n; j++)
{
b = fabs((*this)(j, c));
if (a < b)
{
a = b;
i = j;
}
}
if (a == 0.0)
{
mfem_error("DenseMatrix::Invert() : singular matrix");
}
piv[c] = i;
for (j = 0; j < n; j++)
{
Swap<double>((*this)(c, j), (*this)(i, j));
}
a = (*this)(c, c) = 1.0 / (*this)(c, c);
for (j = 0; j < c; j++)
{
(*this)(c, j) *= a;
}
for (j++; j < n; j++)
{
(*this)(c, j) *= a;
}
for (i = 0; i < c; i++)
{
(*this)(i, c) = a * (b = -(*this)(i, c));
for (j = 0; j < c; j++)
{
(*this)(i, j) += b * (*this)(c, j);
}
for (j++; j < n; j++)
{
(*this)(i, j) += b * (*this)(c, j);
}
}
for (i++; i < n; i++)
{
(*this)(i, c) = a * (b = -(*this)(i, c));
for (j = 0; j < c; j++)
{
(*this)(i, j) += b * (*this)(c, j);
}
for (j++; j < n; j++)
{
(*this)(i, j) += b * (*this)(c, j);
}
}
}
for (c = n - 1; c >= 0; c--)
{
j = piv[c];
for (i = 0; i < n; i++)
{
Swap<double>((*this)(i, c), (*this)(i, j));
}
}
#endif
}
void DenseMatrix::SquareRootInverse()
{
// Square root inverse using Denman--Beavers
#ifdef MFEM_DEBUG
if (Height() <= 0 || Height() != Width())
{
mfem_error("DenseMatrix::SquareRootInverse() matrix not square.");
}
#endif
DenseMatrix tmp1(Height());
DenseMatrix tmp2(Height());
DenseMatrix tmp3(Height());
tmp1 = (*this);
(*this) = 0.0;
for (int v = 0; v < Height() ; v++) { (*this)(v,v) = 1.0; }
for (int j = 0; j < 10; j++)
{
for (int i = 0; i < 10; i++)
{
tmp2 = tmp1;
tmp3 = (*this);
tmp2.Invert();
tmp3.Invert();
tmp1 += tmp3;
(*this) += tmp2;
tmp1 *= 0.5;
(*this) *= 0.5;
}
mfem::Mult((*this), tmp1, tmp2);
for (int v = 0; v < Height() ; v++) { tmp2(v,v) -= 1.0; }
if (tmp2.FNorm() < 1e-10) { break; }
}
if (tmp2.FNorm() > 1e-10)
{
mfem_error("DenseMatrix::SquareRootInverse not converged");
}
}
void DenseMatrix::Norm2(double *v) const
{
for (int j = 0; j < Width(); j++)
{
v[j] = 0.0;
for (int i = 0; i < Height(); i++)
{
v[j] += (*this)(i,j)*(*this)(i,j);
}
v[j] = sqrt(v[j]);
}
}
double DenseMatrix::MaxMaxNorm() const
{
int hw = Height()*Width();
const double *d = data;
double norm = 0.0, abs_entry;
for (int i = 0; i < hw; i++)
{
abs_entry = fabs(d[i]);
if (norm < abs_entry)
{
norm = abs_entry;
}
}
return norm;
}
void DenseMatrix::FNorm(double &scale_factor, double &scaled_fnorm2) const
{
int i, hw = Height() * Width();
double max_norm = 0.0, entry, fnorm2;
for (i = 0; i < hw; i++)
{
entry = fabs(data[i]);
if (entry > max_norm)
{
max_norm = entry;
}
}
if (max_norm == 0.0)
{
scale_factor = scaled_fnorm2 = 0.0;
return;
}
fnorm2 = 0.0;
for (i = 0; i < hw; i++)
{
entry = data[i] / max_norm;
fnorm2 += entry * entry;
}
scale_factor = max_norm;
scaled_fnorm2 = fnorm2;
}
void dsyevr_Eigensystem(DenseMatrix &a, Vector &ev, DenseMatrix *evect)
{
#ifdef MFEM_USE_LAPACK
ev.SetSize(a.Width());
char JOBZ = 'N';
char RANGE = 'A';
char UPLO = 'U';
int N = a.Width();
double *A = new double[N*N];
int LDA = N;
double VL = 0.0;
double VU = 1.0;
int IL = 0;
int IU = 1;
double ABSTOL = 0.0;
int M;
double *W = ev.GetData();
double *Z = NULL;
int LDZ = 1;
int *ISUPPZ = new int[2*N];
int LWORK = -1; // query optimal (double) workspace size
double QWORK;
double *WORK = NULL;
int LIWORK = -1; // query optimal (int) workspace size
int QIWORK;
int *IWORK = NULL;
int INFO;
if (evect) // Compute eigenvectors too
{
evect->SetSize(N);
JOBZ = 'V';
Z = evect->Data();
LDZ = N;
}
int hw = a.Height() * a.Width();
double *data = a.Data();
for (int i = 0; i < hw; i++)
{
A[i] = data[i];
}
dsyevr_( &JOBZ, &RANGE, &UPLO, &N, A, &LDA, &VL, &VU, &IL, &IU,
&ABSTOL, &M, W, Z, &LDZ, ISUPPZ, &QWORK, &LWORK,
&QIWORK, &LIWORK, &INFO );
LWORK = (int) QWORK;
LIWORK = QIWORK;
WORK = new double[LWORK];
IWORK = new int[LIWORK];
dsyevr_( &JOBZ, &RANGE, &UPLO, &N, A, &LDA, &VL, &VU, &IL, &IU,
&ABSTOL, &M, W, Z, &LDZ, ISUPPZ, WORK, &LWORK,
IWORK, &LIWORK, &INFO );
if (INFO != 0)
{
mfem::err << "dsyevr_Eigensystem(...): DSYEVR error code: "
<< INFO << endl;
mfem_error();
}
#ifdef MFEM_DEBUG
if (M < N)
{
mfem::err << "dsyevr_Eigensystem(...):\n"
<< " DSYEVR did not find all eigenvalues "
<< M << "/" << N << endl;
mfem_error();
}
if (CheckFinite(W, N) > 0)
{
mfem_error("dsyevr_Eigensystem(...): inf/nan values in W");
}
if (CheckFinite(Z, N*N) > 0)
{
mfem_error("dsyevr_Eigensystem(...): inf/nan values in Z");
}
VU = 0.0;
for (IL = 0; IL < N; IL++)
for (IU = 0; IU <= IL; IU++)
{
VL = 0.0;
for (M = 0; M < N; M++)
{
VL += Z[M+IL*N] * Z[M+IU*N];
}
if (IU < IL)
{
VL = fabs(VL);
}
else
{
VL = fabs(VL-1.0);
}
if (VL > VU)
{
VU = VL;
}
if (VU > 0.5)
{
mfem::err << "dsyevr_Eigensystem(...):"
<< " Z^t Z - I deviation = " << VU
<< "\n W[max] = " << W[N-1] << ", W[min] = "
<< W[0] << ", N = " << N << endl;
mfem_error();
}
}
if (VU > 1e-9)
{
mfem::err << "dsyevr_Eigensystem(...):"
<< " Z^t Z - I deviation = " << VU
<< "\n W[max] = " << W[N-1] << ", W[min] = "
<< W[0] << ", N = " << N << endl;
}
if (VU > 1e-5)
{
mfem_error("dsyevr_Eigensystem(...): ERROR: ...");
}
VU = 0.0;
for (IL = 0; IL < N; IL++)
for (IU = 0; IU < N; IU++)
{
VL = 0.0;
for (M = 0; M < N; M++)
{
VL += Z[IL+M*N] * W[M] * Z[IU+M*N];
}
VL = fabs(VL-data[IL+N*IU]);
if (VL > VU)
{
VU = VL;
}
}
if (VU > 1e-9)
{
mfem::err << "dsyevr_Eigensystem(...):"
<< " max matrix deviation = " << VU
<< "\n W[max] = " << W[N-1] << ", W[min] = "
<< W[0] << ", N = " << N << endl;
}
if (VU > 1e-5)
{
mfem_error("dsyevr_Eigensystem(...): ERROR: ...");
}
#endif
delete [] IWORK;
delete [] WORK;
delete [] ISUPPZ;
delete [] A;
#endif
}
void dsyev_Eigensystem(DenseMatrix &a, Vector &ev, DenseMatrix *evect)
{
#ifdef MFEM_USE_LAPACK
int N = a.Width();
char JOBZ = 'N';
char UPLO = 'U';
int LDA = N;
int LWORK = -1; /* query optimal workspace size */
int INFO;
ev.SetSize(N);
double *A = NULL;
double *W = ev.GetData();
double *WORK = NULL;
double QWORK;
if (evect)
{
JOBZ = 'V';
evect->SetSize(N);
A = evect->Data();
}
else
{
A = new double[N*N];
}
int hw = a.Height() * a.Width();
double *data = a.Data();
for (int i = 0; i < hw; i++)
{
A[i] = data[i];
}
dsyev_(&JOBZ, &UPLO, &N, A, &LDA, W, &QWORK, &LWORK, &INFO);
LWORK = (int) QWORK;
WORK = new double[LWORK];
dsyev_(&JOBZ, &UPLO, &N, A, &LDA, W, WORK, &LWORK, &INFO);
if (INFO != 0)
{
mfem::err << "dsyev_Eigensystem: DSYEV error code: " << INFO << endl;
mfem_error();
}
delete [] WORK;
if (evect == NULL) { delete [] A; }
#endif
}
void DenseMatrix::Eigensystem(Vector &ev, DenseMatrix *evect)
{
#ifdef MFEM_USE_LAPACK
// dsyevr_Eigensystem(*this, ev, evect);
dsyev_Eigensystem(*this, ev, evect);
#else
mfem_error("DenseMatrix::Eigensystem");
#endif
}
void dsygv_Eigensystem(DenseMatrix &a, DenseMatrix &b, Vector &ev,
DenseMatrix *evect)
{
#ifdef MFEM_USE_LAPACK
int N = a.Width();
int ITYPE = 1;
char JOBZ = 'N';
char UPLO = 'U';
int LDA = N;
int LDB = N;
int LWORK = -1; /* query optimal workspace size */
int INFO;
ev.SetSize(N);
double *A = NULL;
double *B = new double[N*N];
double *W = ev.GetData();
double *WORK = NULL;
double QWORK;
if (evect)
{
JOBZ = 'V';
evect->SetSize(N);
A = evect->Data();
}
else
{
A = new double[N*N];
}
int hw = a.Height() * a.Width();
double *a_data = a.Data();
double *b_data = b.Data();
for (int i = 0; i < hw; i++)
{
A[i] = a_data[i];
B[i] = b_data[i];
}
dsygv_(&ITYPE, &JOBZ, &UPLO, &N, A, &LDA, B, &LDB, W, &QWORK, &LWORK, &INFO);
LWORK = (int) QWORK;
WORK = new double[LWORK];
dsygv_(&ITYPE, &JOBZ, &UPLO, &N, A, &LDA, B, &LDB, W, WORK, &LWORK, &INFO);
if (INFO != 0)
{
mfem::err << "dsygv_Eigensystem: DSYGV error code: " << INFO << endl;
mfem_error();
}
delete [] WORK;
delete [] B;
if (evect == NULL) { delete [] A; }
#endif
}
void DenseMatrix::Eigensystem(DenseMatrix &b, Vector &ev,
DenseMatrix *evect)
{
#ifdef MFEM_USE_LAPACK
dsygv_Eigensystem(*this, b, ev, evect);
#else
mfem_error("DenseMatrix::Eigensystem for generalized eigenvalues");
#endif
}
void DenseMatrix::SingularValues(Vector &sv) const
{
#ifdef MFEM_USE_LAPACK
DenseMatrix copy_of_this = *this;
char jobu = 'N';
char jobvt = 'N';
int m = Height();
int n = Width();
double *a = copy_of_this.data;
sv.SetSize(min(m, n));
double *s = sv;
double *u = NULL;
double *vt = NULL;
double *work = NULL;
int lwork = -1;
int info;
double qwork;
dgesvd_(&jobu, &jobvt, &m, &n, a, &m,
s, u, &m, vt, &n, &qwork, &lwork, &info);
lwork = (int) qwork;
work = new double[lwork];
dgesvd_(&jobu, &jobvt, &m, &n, a, &m,
s, u, &m, vt, &n, work, &lwork, &info);
delete [] work;
if (info)
{
mfem::err << "DenseMatrix::SingularValues : info = " << info << endl;
mfem_error();
}
#else
// compiling without lapack
mfem_error("DenseMatrix::SingularValues");
#endif
}
int DenseMatrix::Rank(double tol) const
{
int rank=0;
Vector sv(min(Height(), Width()));
SingularValues(sv);
for (int i=0; i < sv.Size(); ++i)
if (sv(i) >= tol)
{
++rank;
}
return rank;
}
static const double sqrt_1_eps = sqrt(1./numeric_limits<double>::epsilon());
inline void Eigenvalues2S(const double &d12, double &d1, double &d2)
{
if (d12 != 0.)
{
// "The Symmetric Eigenvalue Problem", B. N. Parlett, pp.189-190
double t, zeta = (d2 - d1)/(2*d12); // inf/inf from overflows?
if (fabs(zeta) < sqrt_1_eps)
{
t = d12*copysign(1./(fabs(zeta) + sqrt(1. + zeta*zeta)), zeta);
}
else
{
t = d12*copysign(0.5/fabs(zeta), zeta);
}
d1 -= t;
d2 += t;
}
}
inline void Eigensystem2S(const double &d12, double &d1, double &d2,
double &c, double &s)
{
if (d12 == 0.)
{
c = 1.;
s = 0.;
}
else
{
// "The Symmetric Eigenvalue Problem", B. N. Parlett, pp.189-190
double t, zeta = (d2 - d1)/(2*d12);
if (fabs(zeta) < sqrt_1_eps)
{
t = copysign(1./(fabs(zeta) + sqrt(1. + zeta*zeta)), zeta);
}
else
{
t = copysign(0.5/fabs(zeta), zeta);
}
// c = 1./sqrt(1. + t*t);
c = sqrt(1./(1. + t*t));
s = c*t;
t *= d12;
d1 -= t;
d2 += t;
}
}
inline void vec_normalize3_aux(
const double &x1, const double &x2, const double &x3,
double &n1, double &n2, double &n3)
{
double m, t, r;
m = fabs(x1);
r = x2/m;
t = 1. + r*r;
r = x3/m;
t = sqrt(1./(t + r*r));
n1 = copysign(t, x1);
t /= m;
n2 = x2*t;
n3 = x3*t;
}
inline void vec_normalize3(const double &x1, const double &x2, const double &x3,
double &n1, double &n2, double &n3)
{
// should work ok when xk is the same as nk for some or all k
if (fabs(x1) >= fabs(x2))
{
if (fabs(x1) >= fabs(x3))
{
if (x1 != 0.)
{
vec_normalize3_aux(x1, x2, x3, n1, n2, n3);
}
else
{
n1 = n2 = n3 = 0.;
}
return;
}
}
else if (fabs(x2) >= fabs(x3))
{
vec_normalize3_aux(x2, x1, x3, n2, n1, n3);
return;
}
vec_normalize3_aux(x3, x1, x2, n3, n1, n2);
}
inline bool KernelVector2G(
const int &mode,
double &d1, double &d12, double &d21, double &d2)
{
// Find a vector (z1,z2) in the "near"-kernel of the matrix
// | d1 d12 |
// | d21 d2 |
// using QR factorization.
// The vector (z1,z2) is returned in (d1,d2). Return 'true' if the matrix
// is zero without setting (d1,d2).
// Note: in the current implementation |z1| + |z2| = 1.
// l1-norms of the columns
double n1 = fabs(d1) + fabs(d21);
double n2 = fabs(d2) + fabs(d12);
bool swap_columns = (n2 > n1);
double mu;
if (!swap_columns)
{
if (n1 == 0.)
{
return true;
}
if (mode == 0) // eliminate the larger entry in the column
{
if (fabs(d1) > fabs(d21))
{
Swap(d1, d21);
Swap(d12, d2);
}
}
else // eliminate the smaller entry in the column
{
if (fabs(d1) < fabs(d21))
{
Swap(d1, d21);
Swap(d12, d2);
}
}
}
else
{
// n2 > n1, swap columns 1 and 2
if (mode == 0) // eliminate the larger entry in the column
{
if (fabs(d12) > fabs(d2))
{
Swap(d1, d2);
Swap(d12, d21);
}
else
{
Swap(d1, d12);
Swap(d21, d2);
}
}
else // eliminate the smaller entry in the column
{
if (fabs(d12) < fabs(d2))
{
Swap(d1, d2);
Swap(d12, d21);
}
else
{
Swap(d1, d12);
Swap(d21, d2);
}
}
}
n1 = hypot(d1, d21);
if (d21 != 0.)
{
// v = (n1, n2)^t, |v| = 1
// Q = I - 2 v v^t, Q (d1, d21)^t = (mu, 0)^t
mu = copysign(n1, d1);
n1 = -d21*(d21/(d1 + mu)); // = d1 - mu
d1 = mu;
// normalize (n1,d21) to avoid overflow/underflow
// normalize (n1,d21) by the max-norm to avoid the sqrt call
if (fabs(n1) <= fabs(d21))
{
// (n1,n2) <-- (n1/d21,1)
n1 = n1/d21;
mu = (2./(1. + n1*n1))*(n1*d12 + d2);
d2 = d2 - mu;
d12 = d12 - mu*n1;
}
else
{
// (n1,n2) <-- (1,d21/n1)
n2 = d21/n1;
mu = (2./(1. + n2*n2))*(d12 + n2*d2);
d2 = d2 - mu*n2;
d12 = d12 - mu;
}
}
// Solve:
// | d1 d12 | | z1 | = | 0 |
// | 0 d2 | | z2 | | 0 |
// choose (z1,z2) to minimize |d1*z1 + d12*z2| + |d2*z2|
// under the condition |z1| + |z2| = 1, z2 >= 0 (for uniqueness)
// set t = z1, z2 = 1 - |t|, -1 <= t <= 1
// objective function is:
// |d1*t + d12*(1 - |t|)| + |d2|*(1 - |t|) -- piecewise linear with
// possible minima are -1,0,1,t1 where t1: d1*t1 + d12*(1 - |t1|) = 0
// values: @t=+/-1 -> |d1|, @t=0 -> |n1| + |d2|, @t=t1 -> |d2|*(1 - |t1|)
// evaluate z2 @t=t1
mu = -d12/d1;
// note: |mu| <= 1, if using l2-norm for column pivoting
// |mu| <= sqrt(2), if using l1-norm
n2 = 1./(1. + fabs(mu));
// check if |d1|<=|d2|*z2
if (fabs(d1) <= n2*fabs(d2))
{
d2 = 0.;
d1 = 1.;
}
else
{
d2 = n2;
// d1 = (n2 < 0.5) ? copysign(1. - n2, mu) : mu*n2;
d1 = mu*n2;
}
if (swap_columns)
{
Swap(d1, d2);
}
return false;
}
inline int KernelVector3G_aux(
const int &mode,
double &d1, double &d2, double &d3, double &c12, double &c13, double &c23,
double &c21, double &c31, double &c32)
{
int kdim;
double mu, n1, n2, n3, s1, s2, s3;
s1 = hypot(c21, c31);
n1 = hypot(d1, s1);
if (s1 != 0.)
{
// v = (s1, s2, s3)^t, |v| = 1
// Q = I - 2 v v^t, Q (d1, c12, c13)^t = (mu, 0, 0)^t
mu = copysign(n1, d1);
n1 = -s1*(s1/(d1 + mu)); // = d1 - mu
d1 = mu;
// normalize (n1,c21,c31) to avoid overflow/underflow
// normalize (n1,c21,c31) by the max-norm to avoid the sqrt call
if (fabs(n1) >= fabs(c21))
{
if (fabs(n1) >= fabs(c31))
{
// n1 is max, (s1,s2,s3) <-- (1,c21/n1,c31/n1)
s2 = c21/n1;
s3 = c31/n1;
mu = 2./(1. + s2*s2 + s3*s3);
n2 = mu*(c12 + s2*d2 + s3*c32);
n3 = mu*(c13 + s2*c23 + s3*d3);
c12 = c12 - n2;
d2 = d2 - s2*n2;
c32 = c32 - s3*n2;
c13 = c13 - n3;
c23 = c23 - s2*n3;
d3 = d3 - s3*n3;
goto done_column_1;
}
}
else if (fabs(c21) >= fabs(c31))
{
// c21 is max, (s1,s2,s3) <-- (n1/c21,1,c31/c21)
s1 = n1/c21;
s3 = c31/c21;
mu = 2./(1. + s1*s1 + s3*s3);
n2 = mu*(s1*c12 + d2 + s3*c32);
n3 = mu*(s1*c13 + c23 + s3*d3);
c12 = c12 - s1*n2;
d2 = d2 - n2;
c32 = c32 - s3*n2;
c13 = c13 - s1*n3;
c23 = c23 - n3;
d3 = d3 - s3*n3;
goto done_column_1;
}
// c31 is max, (s1,s2,s3) <-- (n1/c31,c21/c31,1)
s1 = n1/c31;
s2 = c21/c31;
mu = 2./(1. + s1*s1 + s2*s2);
n2 = mu*(s1*c12 + s2*d2 + c32);
n3 = mu*(s1*c13 + s2*c23 + d3);
c12 = c12 - s1*n2;
d2 = d2 - s2*n2;
c32 = c32 - n2;
c13 = c13 - s1*n3;
c23 = c23 - s2*n3;
d3 = d3 - n3;
}
done_column_1:
// Solve:
// | d2 c23 | | z2 | = | 0 |
// | c32 d3 | | z3 | | 0 |
if (KernelVector2G(mode, d2, c23, c32, d3))
{
// Have two solutions:
// two vectors in the kernel are P (-c12/d1, 1, 0)^t and
// P (-c13/d1, 0, 1)^t where P is the permutation matrix swapping
// entries 1 and col.
// A vector orthogonal to both these vectors is P (1, c12/d1, c13/d1)^t
d2 = c12/d1;
d3 = c13/d1;
d1 = 1.;
kdim = 2;
}
else
{
// solve for z1:
// note: |z1| <= a since |z2| + |z3| = 1, and
// max{|c12|,|c13|} <= max{norm(col. 2),norm(col. 3)}
// <= norm(col. 1) <= a |d1|
// a = 1, if using l2-norm for column pivoting
// a = sqrt(3), if using l1-norm
d1 = -(c12*d2 + c13*d3)/d1;
kdim = 1;
}
vec_normalize3(d1, d2, d3, d1, d2, d3);
return kdim;
}
inline int KernelVector3S(
const int &mode,
const double &d12, const double &d13, const double &d23,
double &d1, double &d2, double &d3)
{
// Find a unit vector (z1,z2,z3) in the "near"-kernel of the matrix
// | d1 d12 d13 |
// | d12 d2 d23 |
// | d13 d23 d3 |
// using QR factorization.
// The vector (z1,z2,z3) is returned in (d1,d2,d3).
// Returns the dimension of the kernel, kdim, but never zero.
// - if kdim == 3, then (d1,d2,d3) is not defined,
// - if kdim == 2, then (d1,d2,d3) is a vector orthogonal to the kernel,
// - otherwise kdim == 1 and (d1,d2,d3) is a vector in the "near"-kernel.
double c12 = d12, c13 = d13, c23 = d23;
double c21, c31, c32;
int col, row;
// l1-norms of the columns:
c32 = fabs(d1) + fabs(c12) + fabs(c13);
c31 = fabs(d2) + fabs(c12) + fabs(c23);
c21 = fabs(d3) + fabs(c13) + fabs(c23);
// column pivoting: choose the column with the largest norm
if (c32 >= c21)
{
col = (c32 >= c31) ? 1 : 2;
}
else
{
col = (c31 >= c21) ? 2 : 3;
}
switch (col)
{
case 1:
if (c32 == 0.) // zero matrix
{
return 3;
}
break;
case 2:
if (c31 == 0.) // zero matrix
{
return 3;
}
Swap(c13, c23);
Swap(d1, d2);
break;
case 3:
if (c21 == 0.) // zero matrix
{
return 3;
}
Swap(c12, c23);
Swap(d1, d3);
}
// row pivoting depending on 'mode'
if (mode == 0)
{
if (fabs(d1) <= fabs(c13))
{
row = (fabs(d1) <= fabs(c12)) ? 1 : 2;
}
else
{
row = (fabs(c12) <= fabs(c13)) ? 2 : 3;
}
}
else
{
if (fabs(d1) >= fabs(c13))
{
row = (fabs(d1) >= fabs(c12)) ? 1 : 2;
}
else
{
row = (fabs(c12) >= fabs(c13)) ? 2 : 3;
}
}
switch (row)
{
case 1:
c21 = c12;
c31 = c13;
c32 = c23;
break;
case 2:
c21 = d1;
c31 = c13;
c32 = c23;
d1 = c12;
c12 = d2;
d2 = d1;
c13 = c23;
c23 = c31;
break;
case 3:
c21 = c12;
c31 = d1;
c32 = c12;
d1 = c13;
c12 = c23;
c13 = d3;
d3 = d1;
}
row = KernelVector3G_aux(mode, d1, d2, d3, c12, c13, c23, c21, c31, c32);
// row is kdim
switch (col)
{
case 2:
Swap(d1, d2);
break;
case 3:
Swap(d1, d3);
}
return row;
}
inline int Reduce3S(
const int &mode,
double &d1, double &d2, double &d3, double &d12, double &d13, double &d23,
double &z1, double &z2, double &z3, double &v1, double &v2, double &v3,
double &g)
{
// Given the matrix
// | d1 d12 d13 |
// A = | d12 d2 d23 |
// | d13 d23 d3 |
// and a unit eigenvector z=(z1,z2,z3), transform the matrix A into the
// matrix B = Q P A P Q that has the form
// | b1 0 0 |
// B = Q P A P Q = | 0 b2 b23 |
// | 0 b23 b3 |
// where P is the permutation matrix switching entries 1 and k, and
// Q is the reflection matrix Q = I - g v v^t, defined by: set y = P z and
// v = c(y - e_1); if y = e_1, then v = 0 and Q = I.
// Note: Q y = e_1, Q e_1 = y ==> Q P A P Q e_1 = ... = lambda e_1.
// The entries (b1,b2,b3,b23) are returned in (d1,d2,d3,d23), and the
// return value of the function is k. The variable g = 2/(v1^2+v2^2+v3^3).
int k;
double s, w1, w2, w3;
if (mode == 0)
{
// choose k such that z^t e_k = zk has the smallest absolute value, i.e.
// the angle between z and e_k is closest to pi/2
if (fabs(z1) <= fabs(z3))
{
k = (fabs(z1) <= fabs(z2)) ? 1 : 2;
}
else
{
k = (fabs(z2) <= fabs(z3)) ? 2 : 3;
}
}
else
{
// choose k such that zk is the largest by absolute value
if (fabs(z1) >= fabs(z3))
{
k = (fabs(z1) >= fabs(z2)) ? 1 : 2;
}
else
{
k = (fabs(z2) >= fabs(z3)) ? 2 : 3;
}
}
switch (k)
{
case 2:
Swap(d13, d23);
Swap(d1, d2);
Swap(z1, z2);
break;
case 3:
Swap(d12, d23);
Swap(d1, d3);
Swap(z1, z3);
}
s = hypot(z2, z3);
if (s == 0.)
{
// s can not be zero, if zk is the smallest (mode == 0)
v1 = v2 = v3 = 0.;
g = 1.;
}
else
{
g = copysign(1., z1);
v1 = -s*(s/(z1 + g)); // = z1 - g
// normalize (v1,z2,z3) by its max-norm, avoiding the sqrt call
g = fabs(v1);
if (fabs(z2) > g) { g = fabs(z2); }
if (fabs(z3) > g) { g = fabs(z3); }
v1 = v1/g;
v2 = z2/g;
v3 = z3/g;
g = 2./(v1*v1 + v2*v2 + v3*v3);
// Compute Q A Q = A - v w^t - w v^t, where
// w = u - (g/2)(v^t u) v, and u = g A v
// set w = g A v
w1 = g*( d1*v1 + d12*v2 + d13*v3);
w2 = g*(d12*v1 + d2*v2 + d23*v3);
w3 = g*(d13*v1 + d23*v2 + d3*v3);
// w := w - (g/2)(v^t w) v
s = (g/2)*(v1*w1 + v2*w2 + v3*w3);
w1 -= s*v1;
w2 -= s*v2;
w3 -= s*v3;
// dij -= vi*wj + wi*vj
d1 -= 2*v1*w1;
d2 -= 2*v2*w2;
d23 -= v2*w3 + v3*w2;
d3 -= 2*v3*w3;
// compute the offdiagonal entries on the first row/column of B which
// should be zero (for debugging):
#if 0
s = d12 - v1*w2 - v2*w1; // b12 = 0
s = d13 - v1*w3 - v3*w1; // b13 = 0
#endif
}
switch (k)
{
case 2:
Swap(z1, z2);
break;
case 3:
Swap(z1, z3);
}
return k;
}
inline void GetScalingFactor(const double &d_max, double &mult)
{
int d_exp;
if (d_max > 0.)
{
mult = frexp(d_max, &d_exp);
if (d_exp == numeric_limits<double>::max_exponent)
{
mult *= numeric_limits<double>::radix;
}
mult = d_max/mult;
}
else
{
mult = 1.;
}
// mult = 2^d_exp is such that d_max/mult is in [0.5,1)
// or in other words d_max is in the interval [0.5,1)*mult
}
double DenseMatrix::CalcSingularvalue(const int i) const
{
MFEM_ASSERT(Height() == Width() && Height() > 0 && Height() < 4,
"The matrix must be square and sized 1, 2, or 3 to compute the"
" singular values."
<< " Height() = " << Height()
<< ", Width() = " << Width());
const int n = Height();
const double *d = data;
if (n == 1)
{
return d[0];
}
else if (n == 2)
{
double d0, d1, d2, d3;
d0 = d[0];
d1 = d[1];
d2 = d[2];
d3 = d[3];
double mult;
{
double d_max = fabs(d0);
if (d_max < fabs(d1)) { d_max = fabs(d1); }
if (d_max < fabs(d2)) { d_max = fabs(d2); }
if (d_max < fabs(d3)) { d_max = fabs(d3); }
GetScalingFactor(d_max, mult);
}
d0 /= mult;
d1 /= mult;
d2 /= mult;
d3 /= mult;
// double b11 = d[0]*d[0] + d[1]*d[1];
// double b12 = d[0]*d[2] + d[1]*d[3];
// double b22 = d[2]*d[2] + d[3]*d[3];
// t = 0.5*(a+b).(a-b) = 0.5*(|a|^2-|b|^2)
// with a,b - the columns of (*this)
// double t = 0.5*(b11 - b22);
double t = 0.5*((d0+d2)*(d0-d2)+(d1-d3)*(d1+d3));
// double s = sqrt(0.5*(b11 + b22) + sqrt(t*t + b12*b12));
double s = d0*d2 + d1*d3;
s = sqrt(0.5*(d0*d0 + d1*d1 + d2*d2 + d3*d3) + sqrt(t*t + s*s));
if (s == 0.0)
{
return 0.0;
}
t = fabs(d0*d3 - d1*d2) / s;
if (t > s)
{
if (i == 0)
{
return t*mult;
}
return s*mult;
}
if (i == 0)
{
return s*mult;
}
return t*mult;
}
else
{
double d0, d1, d2, d3, d4, d5, d6, d7, d8;
d0 = d[0]; d3 = d[3]; d6 = d[6];
d1 = d[1]; d4 = d[4]; d7 = d[7];
d2 = d[2]; d5 = d[5]; d8 = d[8];
double mult;
{
double d_max = fabs(d0);
if (d_max < fabs(d1)) { d_max = fabs(d1); }
if (d_max < fabs(d2)) { d_max = fabs(d2); }
if (d_max < fabs(d3)) { d_max = fabs(d3); }
if (d_max < fabs(d4)) { d_max = fabs(d4); }
if (d_max < fabs(d5)) { d_max = fabs(d5); }
if (d_max < fabs(d6)) { d_max = fabs(d6); }
if (d_max < fabs(d7)) { d_max = fabs(d7); }
if (d_max < fabs(d8)) { d_max = fabs(d8); }
GetScalingFactor(d_max, mult);
}
d0 /= mult; d1 /= mult; d2 /= mult;
d3 /= mult; d4 /= mult; d5 /= mult;
d6 /= mult; d7 /= mult; d8 /= mult;
double b11 = d0*d0 + d1*d1 + d2*d2;
double b12 = d0*d3 + d1*d4 + d2*d5;
double b13 = d0*d6 + d1*d7 + d2*d8;
double b22 = d3*d3 + d4*d4 + d5*d5;
double b23 = d3*d6 + d4*d7 + d5*d8;
double b33 = d6*d6 + d7*d7 + d8*d8;
// double a, b, c;
// a = -(b11 + b22 + b33);
// b = b11*(b22 + b33) + b22*b33 - b12*b12 - b13*b13 - b23*b23;
// c = b11*(b23*b23 - b22*b33) + b12*(b12*b33 - 2*b13*b23) + b13*b13*b22;
// double Q = (a * a - 3 * b) / 9;
// double Q = (b12*b12 + b13*b13 + b23*b23 +
// ((b11 - b22)*(b11 - b22) +
// (b11 - b33)*(b11 - b33) +
// (b22 - b33)*(b22 - b33))/6)/3;
// Q = (3*(b12^2 + b13^2 + b23^2) +
// ((b11 - b22)^2 + (b11 - b33)^2 + (b22 - b33)^2)/2)/9
// or
// Q = (1/6)*|B-tr(B)/3|_F^2
// Q >= 0 and
// Q = 0 <==> B = scalar * I
// double R = (2 * a * a * a - 9 * a * b + 27 * c) / 54;
double aa = (b11 + b22 + b33)/3; // aa = tr(B)/3
double c1, c2, c3;
// c1 = b11 - aa; // ((b11 - b22) + (b11 - b33))/3
// c2 = b22 - aa; // ((b22 - b11) + (b22 - b33))/3
// c3 = b33 - aa; // ((b33 - b11) + (b33 - b22))/3
{
double b11_b22 = ((d0-d3)*(d0+d3)+(d1-d4)*(d1+d4)+(d2-d5)*(d2+d5));
double b22_b33 = ((d3-d6)*(d3+d6)+(d4-d7)*(d4+d7)+(d5-d8)*(d5+d8));
double b33_b11 = ((d6-d0)*(d6+d0)+(d7-d1)*(d7+d1)+(d8-d2)*(d8+d2));
c1 = (b11_b22 - b33_b11)/3;
c2 = (b22_b33 - b11_b22)/3;
c3 = (b33_b11 - b22_b33)/3;
}
double Q, R;
Q = (2*(b12*b12 + b13*b13 + b23*b23) + c1*c1 + c2*c2 + c3*c3)/6;
R = (c1*(b23*b23 - c2*c3)+ b12*(b12*c3 - 2*b13*b23) +b13*b13*c2)/2;
// R = (-1/2)*det(B-(tr(B)/3)*I)
// Note: 54*(det(S))^2 <= |S|_F^6, when S^t=S and tr(S)=0, S is 3x3
// Therefore: R^2 <= Q^3
if (Q <= 0.) { ; }
// else if (fabs(R) >= sqrtQ3)
// {
// double det = (d[0] * (d[4] * d[8] - d[5] * d[7]) +
// d[3] * (d[2] * d[7] - d[1] * d[8]) +
// d[6] * (d[1] * d[5] - d[2] * d[4]));
//
// if (R > 0.)
// {
// if (i == 2)
// // aa -= 2*sqrtQ;
// return fabs(det)/(aa + sqrtQ);
// else
// aa += sqrtQ;
// }
// else
// {
// if (i != 0)
// aa -= sqrtQ;
// // aa = fabs(det)/sqrt(aa + 2*sqrtQ);
// else
// aa += 2*sqrtQ;
// }
// }
else
{
double sqrtQ = sqrt(Q);
double sqrtQ3 = Q*sqrtQ;
// double sqrtQ3 = sqrtQ*sqrtQ*sqrtQ;
// double sqrtQ3 = pow(Q, 1.5);
double r;
if (fabs(R) >= sqrtQ3)
{
if (R < 0.)
{
// R = -1.;
r = 2*sqrtQ;
}
else
{
// R = 1.;
r = -2*sqrtQ;
}
}
else
{
R = R/sqrtQ3;
// if (fabs(R) <= 0.95)
if (fabs(R) <= 0.9)
{
if (i == 2)
{
aa -= 2*sqrtQ*cos(acos(R)/3); // min
}
else if (i == 0)
{
aa -= 2*sqrtQ*cos((acos(R) + 2.0*M_PI)/3); // max
}
else
{
aa -= 2*sqrtQ*cos((acos(R) - 2.0*M_PI)/3); // mid
}
goto have_aa;
}
if (R < 0.)
{
r = -2*sqrtQ*cos((acos(R) + 2.0*M_PI)/3); // max
if (i == 0)
{
aa += r;
goto have_aa;
}
}
else
{
r = -2*sqrtQ*cos(acos(R)/3); // min
if (i == 2)
{
aa += r;
goto have_aa;
}
}
}
// (tr(B)/3 + r) is the root which is separated from the other
// two roots which are close to each other when |R| is close to 1
c1 -= r;
c2 -= r;
c3 -= r;
// aa += r;
// Type of Householder reflections: z --> mu ek, where k is the index
// of the entry in z with:
// mode == 0: smallest absolute value --> angle closest to pi/2
// (eliminate large entries)
// mode == 1: largest absolute value --> angle farthest from pi/2
// (eliminate small entries)
const int mode = 1;
// Find a unit vector z = (z1,z2,z3) in the "near"-kernel of
// | c1 b12 b13 |
// | b12 c2 b23 | = B - aa*I
// | b13 b23 c3 |
// This vector is also an eigenvector for B corresponding to aa
// The vector z overwrites (c1,c2,c3).
switch (KernelVector3S(mode, b12, b13, b23, c1, c2, c3))
{
case 3:
aa += r;
goto have_aa;
case 2:
// ok, continue with the returned vector orthogonal to the kernel
case 1:
// ok, continue with the returned vector in the "near"-kernel
;
}
// Using the eigenvector c = (c1,c2,c3) to transform B into
// | b11 0 0 |
// B <-- Q P B P Q = | 0 b22 b23 |
// | 0 b23 b33 |
double v1, v2, v3, g;
Reduce3S(mode, b11, b22, b33, b12, b13, b23,
c1, c2, c3, v1, v2, v3, g);
// Q = I - g v v^t
// P - permutation matrix switching rows and columns 1 and k
// find the eigenvalues of
// | b22 b23 |
// | b23 b33 |
Eigenvalues2S(b23, b22, b33);
if (i == 2)
{
aa = std::min(std::min(b11, b22), b33);
}
else if (i == 1)
{
if (b11 <= b22)
{
aa = (b22 <= b33) ? b22 : std::max(b11, b33);
}
else
{
aa = (b11 <= b33) ? b11 : std::max(b33, b22);
}
}
else
{
aa = std::max(std::max(b11, b22), b33);
}
}
have_aa:
return sqrt(fabs(aa))*mult; // take abs before we sort?
}
}
void DenseMatrix::CalcEigenvalues(double *lambda, double *vec) const
{
#ifdef MFEM_DEBUG
if (Height() != Width() || Height() < 2 || Height() > 3)
{
mfem_error("DenseMatrix::CalcEigenvalues");
}
#endif
const int n = Height();
const double *d = data;
if (n == 2)
{
double d0 = d[0];
double d2 = d[2]; // use the upper triangular entry
double d3 = d[3];
double c, s;
Eigensystem2S(d2, d0, d3, c, s);
if (d0 <= d3)
{
lambda[0] = d0;
lambda[1] = d3;
vec[0] = c;
vec[1] = -s;
vec[2] = s;
vec[3] = c;
}
else
{
lambda[0] = d3;
lambda[1] = d0;
vec[0] = s;
vec[1] = c;
vec[2] = c;
vec[3] = -s;
}
}
else
{
double d11 = d[0];
double d12 = d[3]; // use the upper triangular entries
double d22 = d[4];
double d13 = d[6];
double d23 = d[7];
double d33 = d[8];
double mult;
{
double d_max = fabs(d11);
if (d_max < fabs(d22)) { d_max = fabs(d22); }
if (d_max < fabs(d33)) { d_max = fabs(d33); }
if (d_max < fabs(d12)) { d_max = fabs(d12); }
if (d_max < fabs(d13)) { d_max = fabs(d13); }
if (d_max < fabs(d23)) { d_max = fabs(d23); }
GetScalingFactor(d_max, mult);
}
d11 /= mult; d22 /= mult; d33 /= mult;
d12 /= mult; d13 /= mult; d23 /= mult;
double aa = (d11 + d22 + d33)/3; // aa = tr(A)/3
double c1 = d11 - aa;
double c2 = d22 - aa;
double c3 = d33 - aa;
double Q, R;
Q = (2*(d12*d12 + d13*d13 + d23*d23) + c1*c1 + c2*c2 + c3*c3)/6;
R = (c1*(d23*d23 - c2*c3)+ d12*(d12*c3 - 2*d13*d23) + d13*d13*c2)/2;
if (Q <= 0.)
{
lambda[0] = lambda[1] = lambda[2] = aa;
vec[0] = 1.; vec[3] = 0.; vec[6] = 0.;
vec[1] = 0.; vec[4] = 1.; vec[7] = 0.;
vec[2] = 0.; vec[5] = 0.; vec[8] = 1.;
}
else
{
double sqrtQ = sqrt(Q);
double sqrtQ3 = Q*sqrtQ;
// double sqrtQ3 = sqrtQ*sqrtQ*sqrtQ;
// double sqrtQ3 = pow(Q, 1.5);
double r;
if (fabs(R) >= sqrtQ3)
{
if (R < 0.)
{
// R = -1.;
r = 2*sqrtQ;
}
else
{
// R = 1.;
r = -2*sqrtQ;
}
}
else
{
R = R/sqrtQ3;
if (R < 0.)
{
r = -2*sqrtQ*cos((acos(R) + 2.0*M_PI)/3); // max
}
else
{
r = -2*sqrtQ*cos(acos(R)/3); // min
}
}
aa += r;
c1 = d11 - aa;
c2 = d22 - aa;
c3 = d33 - aa;
// Type of Householder reflections: z --> mu ek, where k is the index
// of the entry in z with:
// mode == 0: smallest absolute value --> angle closest to pi/2
// mode == 1: largest absolute value --> angle farthest from pi/2
// Observations:
// mode == 0 produces better eigenvectors, less accurate eigenvalues?
// mode == 1 produces better eigenvalues, less accurate eigenvectors?
const int mode = 0;
// Find a unit vector z = (z1,z2,z3) in the "near"-kernel of
// | c1 d12 d13 |
// | d12 c2 d23 | = A - aa*I
// | d13 d23 c3 |
// This vector is also an eigenvector for A corresponding to aa.
// The vector z overwrites (c1,c2,c3).
switch (KernelVector3S(mode, d12, d13, d23, c1, c2, c3))
{
case 3:
// 'aa' is a triple eigenvalue
lambda[0] = lambda[1] = lambda[2] = aa;
vec[0] = 1.; vec[3] = 0.; vec[6] = 0.;
vec[1] = 0.; vec[4] = 1.; vec[7] = 0.;
vec[2] = 0.; vec[5] = 0.; vec[8] = 1.;
goto done_3d;
case 2:
// ok, continue with the returned vector orthogonal to the kernel
case 1:
// ok, continue with the returned vector in the "near"-kernel
;
}
// Using the eigenvector c=(c1,c2,c3) transform A into
// | d11 0 0 |
// A <-- Q P A P Q = | 0 d22 d23 |
// | 0 d23 d33 |
double v1, v2, v3, g;
int k = Reduce3S(mode, d11, d22, d33, d12, d13, d23,
c1, c2, c3, v1, v2, v3, g);
// Q = I - 2 v v^t
// P - permutation matrix switching entries 1 and k
// find the eigenvalues and eigenvectors for
// | d22 d23 |
// | d23 d33 |
double c, s;
Eigensystem2S(d23, d22, d33, c, s);
// d22 <-> P Q (0, c, -s), d33 <-> P Q (0, s, c)
double *vec_1, *vec_2, *vec_3;
if (d11 <= d22)
{
if (d22 <= d33)
{
lambda[0] = d11; vec_1 = vec;
lambda[1] = d22; vec_2 = vec + 3;
lambda[2] = d33; vec_3 = vec + 6;
}
else if (d11 <= d33)
{
lambda[0] = d11; vec_1 = vec;
lambda[1] = d33; vec_3 = vec + 3;
lambda[2] = d22; vec_2 = vec + 6;
}
else
{
lambda[0] = d33; vec_3 = vec;
lambda[1] = d11; vec_1 = vec + 3;
lambda[2] = d22; vec_2 = vec + 6;
}
}
else
{
if (d11 <= d33)
{
lambda[0] = d22; vec_2 = vec;
lambda[1] = d11; vec_1 = vec + 3;
lambda[2] = d33; vec_3 = vec + 6;
}
else if (d22 <= d33)
{
lambda[0] = d22; vec_2 = vec;
lambda[1] = d33; vec_3 = vec + 3;
lambda[2] = d11; vec_1 = vec + 6;
}
else
{
lambda[0] = d33; vec_3 = vec;
lambda[1] = d22; vec_2 = vec + 3;
lambda[2] = d11; vec_1 = vec + 6;
}
}
vec_1[0] = c1;
vec_1[1] = c2;
vec_1[2] = c3;
d22 = g*(v2*c - v3*s);
d33 = g*(v2*s + v3*c);
vec_2[0] = - v1*d22; vec_3[0] = - v1*d33;
vec_2[1] = c - v2*d22; vec_3[1] = s - v2*d33;
vec_2[2] = -s - v3*d22; vec_3[2] = c - v3*d33;
switch (k)
{
case 2:
Swap(vec_2[0], vec_2[1]);
Swap(vec_3[0], vec_3[1]);
break;
case 3:
Swap(vec_2[0], vec_2[2]);
Swap(vec_3[0], vec_3[2]);
}
}
done_3d:
lambda[0] *= mult;
lambda[1] *= mult;
lambda[2] *= mult;
}
}
void DenseMatrix::GetRow(int r, Vector &row) const
{
int m = Height();
int n = Width();
row.SetSize(n);
const double* rp = data + r;
double* vp = row.GetData();
for (int i = 0; i < n; i++)
{
vp[i] = *rp;
rp += m;
}
}
void DenseMatrix::GetColumn(int c, Vector &col) const
{
int m = Height();
col.SetSize(m);
double *cp = data + c * m;
double *vp = col.GetData();
for (int i = 0; i < m; i++)
{
vp[i] = cp[i];
}
}
void DenseMatrix::GetDiag(Vector &d) const
{
if (height != width)
{
mfem_error("DenseMatrix::GetDiag\n");
}
d.SetSize(height);
for (int i = 0; i < height; ++i)
{
d(i) = (*this)(i,i);
}
}
void DenseMatrix::Getl1Diag(Vector &l) const
{
if (height != width)
{
mfem_error("DenseMatrix::Getl1Diag\n");
}
l.SetSize(height);
l = 0.0;
for (int j = 0; j < width; ++j)
for (int i = 0; i < height; ++i)
{
l(i) += fabs((*this)(i,j));
}
}
void DenseMatrix::GetRowSums(Vector &l) const
{
l.SetSize(height);
for (int i = 0; i < height; i++)
{
double d = 0.0;
for (int j = 0; j < width; j++)
{
d += operator()(i, j);
}
l(i) = d;
}
}
void DenseMatrix::Diag(double c, int n)
{
SetSize(n);
const int N = n*n;
for (int i = 0; i < N; i++)
{
data[i] = 0.0;
}
for (int i = 0; i < n; i++)
{
data[i*(n+1)] = c;
}
}
void DenseMatrix::Diag(double *diag, int n)
{
SetSize(n);
int i, N = n*n;
for (i = 0; i < N; i++)
{
data[i] = 0.0;
}
for (i = 0; i < n; i++)
{
data[i*(n+1)] = diag[i];
}
}
void DenseMatrix::Transpose()
{
int i, j;
double t;
if (Width() == Height())
{
for (i = 0; i < Height(); i++)
for (j = i+1; j < Width(); j++)
{
t = (*this)(i,j);
(*this)(i,j) = (*this)(j,i);
(*this)(j,i) = t;
}
}
else
{
DenseMatrix T(*this,'t');
(*this) = T;
}
}
void DenseMatrix::Transpose(const DenseMatrix &A)
{
SetSize(A.Width(),A.Height());
for (int i = 0; i < Height(); i++)
for (int j = 0; j < Width(); j++)
{
(*this)(i,j) = A(j,i);
}
}
void DenseMatrix::Symmetrize()
{
#ifdef MFEM_DEBUG
if (Width() != Height())
{
mfem_error("DenseMatrix::Symmetrize() : not a square matrix!");
}
#endif
for (int i = 0; i < Height(); i++)
for (int j = 0; j < i; j++)
{
double a = 0.5 * ((*this)(i,j) + (*this)(j,i));
(*this)(j,i) = (*this)(i,j) = a;
}
}
void DenseMatrix::Lump()
{
for (int i = 0; i < Height(); i++)
{
double L = 0.0;
for (int j = 0; j < Width(); j++)
{
L += (*this)(i, j);
(*this)(i, j) = 0.0;
}
(*this)(i, i) = L;
}
}
void DenseMatrix::GradToCurl(DenseMatrix &curl)
{
int n = Height();
#ifdef MFEM_DEBUG
if ((Width() != 2 || curl.Width() != 1 || 2*n != curl.Height()) &&
(Width() != 3 || curl.Width() != 3 || 3*n != curl.Height()))
{
mfem_error("DenseMatrix::GradToCurl(...)");
}
#endif
if (Width() == 2)
{
for (int i = 0; i < n; i++)
{
// (x,y) is grad of Ui
double x = (*this)(i,0);
double y = (*this)(i,1);
int j = i+n;
// curl of (Ui,0)
curl(i,0) = -y;
// curl of (0,Ui)
curl(j,0) = x;
}
}
else
{
for (int i = 0; i < n; i++)
{
// (x,y,z) is grad of Ui
double x = (*this)(i,0);
double y = (*this)(i,1);
double z = (*this)(i,2);
int j = i+n;
int k = j+n;
// curl of (Ui,0,0)
curl(i,0) = 0.;
curl(i,1) = z;
curl(i,2) = -y;
// curl of (0,Ui,0)
curl(j,0) = -z;
curl(j,1) = 0.;
curl(j,2) = x;
// curl of (0,0,Ui)
curl(k,0) = y;
curl(k,1) = -x;
curl(k,2) = 0.;
}
}
}
void DenseMatrix::GradToDiv(Vector &div)
{
MFEM_ASSERT(Width()*Height() == div.Size(), "incompatible Vector 'div'!");
// div(dof*j+i) <-- (*this)(i,j)
const int n = height * width;
double *ddata = div.GetData();
for (int i = 0; i < n; i++)
{
ddata[i] = data[i];
}
}
void DenseMatrix::CopyRows(const DenseMatrix &A, int row1, int row2)
{
SetSize(row2 - row1 + 1, A.Width());
for (int j = 0; j < Width(); j++)
{
for (int i = row1; i <= row2; i++)
{
(*this)(i-row1,j) = A(i,j);
}
}
}
void DenseMatrix::CopyCols(const DenseMatrix &A, int col1, int col2)
{
SetSize(A.Height(), col2 - col1 + 1);
for (int j = col1; j <= col2; j++)
{
for (int i = 0; i < Height(); i++)
{
(*this)(i,j-col1) = A(i,j);
}
}
}
void DenseMatrix::CopyMN(const DenseMatrix &A, int m, int n, int Aro, int Aco)
{
SetSize(m,n);
for (int j = 0; j < n; j++)
{
for (int i = 0; i < m; i++)
{
(*this)(i,j) = A(Aro+i,Aco+j);
}
}
}
void DenseMatrix::CopyMN(const DenseMatrix &A, int row_offset, int col_offset)
{
double *v = A.data;
for (int j = 0; j < A.Width(); j++)
{
for (int i = 0; i < A.Height(); i++)
{
(*this)(row_offset+i,col_offset+j) = *(v++);
}
}
}
void DenseMatrix::CopyMNt(const DenseMatrix &A, int row_offset, int col_offset)
{
double *v = A.data;
for (int i = 0; i < A.Width(); i++)
{
for (int j = 0; j < A.Height(); j++)
{
(*this)(row_offset+i,col_offset+j) = *(v++);
}
}
}
void DenseMatrix::CopyMN(const DenseMatrix &A, int m, int n, int Aro, int Aco,
int row_offset, int col_offset)
{
MFEM_VERIFY(row_offset+m <= this->Height() && col_offset+n <= this->Width(),
"this DenseMatrix is too small to accomodate the submatrix. "
<< "row_offset = " << row_offset
<< ", m = " << m
<< ", this->Height() = " << this->Height()
<< ", col_offset = " << col_offset
<< ", n = " << n
<< ", this->Width() = " << this->Width()
);
MFEM_VERIFY(Aro+m <= A.Height() && Aco+n <= A.Width(),
"The A DenseMatrix is too small to accomodate the submatrix. "
<< "Aro = " << Aro
<< ", m = " << m
<< ", A.Height() = " << A.Height()
<< ", Aco = " << Aco
<< ", n = " << n
<< ", A.Width() = " << A.Width()
);
for (int j = 0; j < n; j++)
{
for (int i = 0; i < m; i++)
{
(*this)(row_offset+i,col_offset+j) = A(Aro+i,Aco+j);
}
}
}
void DenseMatrix::CopyMNDiag(double c, int n, int row_offset, int col_offset)
{
for (int i = 0; i < n; i++)
{
for (int j = i+1; j < n; j++)
{
(*this)(row_offset+i,col_offset+j) =
(*this)(row_offset+j,col_offset+i) = 0.0;
}
}
for (int i = 0; i < n; i++)
{
(*this)(row_offset+i,col_offset+i) = c;
}
}
void DenseMatrix::CopyMNDiag(double *diag, int n, int row_offset,
int col_offset)
{
for (int i = 0; i < n; i++)
{
for (int j = i+1; j < n; j++)
{
(*this)(row_offset+i,col_offset+j) =
(*this)(row_offset+j,col_offset+i) = 0.0;
}
}
for (int i = 0; i < n; i++)
{
(*this)(row_offset+i,col_offset+i) = diag[i];
}
}
void DenseMatrix::CopyExceptMN(const DenseMatrix &A, int m, int n)
{
SetSize(A.Width()-1,A.Height()-1);
int i, j, i_off = 0, j_off = 0;
for (j = 0; j < A.Width(); j++)
{
if ( j == n )
{
j_off = 1;
continue;
}
for (i = 0; i < A.Height(); i++)
{
if ( i == m )
{
i_off = 1;
continue;
}
(*this)(i-i_off,j-j_off) = A(i,j);
}
i_off = 0;
}
}
void DenseMatrix::AddMatrix(DenseMatrix &A, int ro, int co)
{
int h, ah, aw;
double *p, *ap;
h = Height();
ah = A.Height();
aw = A.Width();
#ifdef MFEM_DEBUG
if (co+aw > Width() || ro+ah > h)
{
mfem_error("DenseMatrix::AddMatrix(...) 1");
}
#endif
p = data + ro + co * h;
ap = A.data;
for (int c = 0; c < aw; c++)
{
for (int r = 0; r < ah; r++)
{
p[r] += ap[r];
}
p += h;
ap += ah;
}
}
void DenseMatrix::AddMatrix(double a, const DenseMatrix &A, int ro, int co)
{
int h, ah, aw;
double *p, *ap;
h = Height();
ah = A.Height();
aw = A.Width();
#ifdef MFEM_DEBUG
if (co+aw > Width() || ro+ah > h)
{
mfem_error("DenseMatrix::AddMatrix(...) 2");
}
#endif
p = data + ro + co * h;
ap = A.data;
for (int c = 0; c < aw; c++)
{
for (int r = 0; r < ah; r++)
{
p[r] += a * ap[r];
}
p += h;
ap += ah;
}
}
void DenseMatrix::AddToVector(int offset, Vector &v) const
{
const int n = height * width;
double *vdata = v.GetData() + offset;
for (int i = 0; i < n; i++)
{
vdata[i] += data[i];
}
}
void DenseMatrix::GetFromVector(int offset, const Vector &v)
{
const int n = height * width;
const double *vdata = v.GetData() + offset;
for (int i = 0; i < n; i++)
{
data[i] = vdata[i];
}
}
void DenseMatrix::AdjustDofDirection(Array<int> &dofs)
{
const int n = Height();
#ifdef MFEM_DEBUG
if (dofs.Size() != n || Width() != n)
{
mfem_error("DenseMatrix::AdjustDofDirection(...)");
}
#endif
int *dof = dofs;
for (int i = 0; i < n-1; i++)
{
const int s = (dof[i] < 0) ? (-1) : (1);
for (int j = i+1; j < n; j++)
{
const int t = (dof[j] < 0) ? (-s) : (s);
if (t < 0)
{
(*this)(i,j) = -(*this)(i,j);
(*this)(j,i) = -(*this)(j,i);
}
}
}
}
void DenseMatrix::SetRow(int row, double value)
{
for (int j = 0; j < Width(); j++)
{
(*this)(row, j) = value;
}
}
void DenseMatrix::SetCol(int col, double value)
{
for (int i = 0; i < Height(); i++)
{
(*this)(i, col) = value;
}
}
void DenseMatrix::SetRow(int r, const Vector &row)
{
for (int j = 0; j < Width(); j++)
{
(*this)(r, j) = row[j];
}
}
void DenseMatrix::SetCol(int c, const Vector &col)
{
for (int i = 0; i < Height(); i++)
{
(*this)(i, c) = col[i];
}
}
void DenseMatrix::Threshold(double eps)
{
for (int col = 0; col < Width(); col++)
{
for (int row = 0; row < Height(); row++)
{
if (std::abs(operator()(row,col)) <= eps)
{
operator()(row,col) = 0.0;
}
}
}
}
void DenseMatrix::Print(std::ostream &out, int width_) const
{
// save current output flags
ios::fmtflags old_flags = out.flags();
// output flags = scientific + show sign
out << setiosflags(ios::scientific | ios::showpos);
for (int i = 0; i < height; i++)
{
out << "[row " << i << "]\n";
for (int j = 0; j < width; j++)
{
out << (*this)(i,j);
if (j+1 == width || (j+1) % width_ == 0)
{
out << '\n';
}
else
{
out << ' ';
}
}
}
// reset output flags to original values
out.flags(old_flags);
}
void DenseMatrix::PrintMatlab(std::ostream &out) const
{
// save current output flags
ios::fmtflags old_flags = out.flags();
// output flags = scientific + show sign
out << setiosflags(ios::scientific | ios::showpos);
for (int i = 0; i < height; i++)
{
for (int j = 0; j < width; j++)
{
out << (*this)(i,j);
out << ' ';
}
out << "\n";
}
// reset output flags to original values
out.flags(old_flags);
}
void DenseMatrix::PrintT(std::ostream &out, int width_) const
{
// save current output flags
ios::fmtflags old_flags = out.flags();
// output flags = scientific + show sign
out << setiosflags(ios::scientific | ios::showpos);
for (int j = 0; j < width; j++)
{
out << "[col " << j << "]\n";
for (int i = 0; i < height; i++)
{
out << (*this)(i,j);
if (i+1 == height || (i+1) % width_ == 0)
{
out << '\n';
}
else
{
out << ' ';
}
}
}
// reset output flags to original values
out.flags(old_flags);
}
void DenseMatrix::TestInversion()
{
DenseMatrix copy(*this), C(width);
Invert();
mfem::Mult(*this, copy, C);
for (int i = 0; i < width; i++)
{
C(i,i) -= 1.0;
}
mfem::out << "size = " << width << ", i_max = " << C.MaxMaxNorm()
<< ", cond_F = " << FNorm()*copy.FNorm() << endl;
}
DenseMatrix::~DenseMatrix()
{
if (capacity > 0)
{
delete [] data;
}
}
void Add(const DenseMatrix &A, const DenseMatrix &B,
double alpha, DenseMatrix &C)
{
for (int j = 0; j < C.Width(); j++)
{
for (int i = 0; i < C.Height(); i++)
{
C(i,j) = A(i,j) + alpha * B(i,j);
}
}
}
void Add(double alpha, const double *A,
double beta, const double *B, DenseMatrix &C)
{
const int m = C.Height()*C.Width();
double *C_data = C.GetData();
for (int i = 0; i < m; i++)
{
C_data[i] = alpha*A[i] + beta*B[i];
}
}
void Add(double alpha, const DenseMatrix &A,
double beta, const DenseMatrix &B, DenseMatrix &C)
{
MFEM_ASSERT(A.Height() == C.Height(), "");
MFEM_ASSERT(B.Height() == C.Height(), "");
MFEM_ASSERT(A.Width() == C.Width(), "");
MFEM_ASSERT(B.Width() == C.Width(), "");
Add(alpha, A.GetData(), beta, B.GetData(), C);
}
void Mult(const DenseMatrix &b, const DenseMatrix &c, DenseMatrix &a)
{
MFEM_ASSERT(a.Height() == b.Height() && a.Width() == c.Width() &&
b.Width() == c.Height(), "incompatible dimensions");
#ifdef MFEM_USE_LAPACK
static char transa = 'N', transb = 'N';
static double alpha = 1.0, beta = 0.0;
int m = b.Height(), n = c.Width(), k = b.Width();
dgemm_(&transa, &transb, &m, &n, &k, &alpha, b.Data(), &m,
c.Data(), &k, &beta, a.Data(), &m);
#else
const int ah = a.Height();
const int aw = a.Width();
const int bw = b.Width();
double *ad = a.Data();
const double *bd = b.Data();
const double *cd = c.Data();
for (int i = 0; i < ah*aw; i++)
{
ad[i] = 0.0;
}
for (int j = 0; j < aw; j++)
{
for (int k = 0; k < bw; k++)
{
for (int i = 0; i < ah; i++)
{
ad[i+j*ah] += bd[i+k*ah] * cd[k+j*bw];
}
}
}
#endif
}
void AddMult(const DenseMatrix &b, const DenseMatrix &c, DenseMatrix &a)
{
MFEM_ASSERT(a.Height() == b.Height() && a.Width() == c.Width() &&
b.Width() == c.Height(), "incompatible dimensions");
#ifdef MFEM_USE_LAPACK
static char transa = 'N', transb = 'N';
static double alpha = 1.0, beta = 1.0;
int m = b.Height(), n = c.Width(), k = b.Width();
dgemm_(&transa, &transb, &m, &n, &k, &alpha, b.Data(), &m,
c.Data(), &k, &beta, a.Data(), &m);
#else
const int ah = a.Height();
const int aw = a.Width();
const int bw = b.Width();
double *ad = a.Data();
const double *bd = b.Data();
const double *cd = c.Data();
for (int j = 0; j < aw; j++)
{
for (int k = 0; k < bw; k++)
{
for (int i = 0; i < ah; i++)
{
ad[i+j*ah] += bd[i+k*ah] * cd[k+j*bw];
}
}
}
#endif
}
void CalcAdjugate(const DenseMatrix &a, DenseMatrix &adja)
{
#ifdef MFEM_DEBUG
if (a.Width() > a.Height() || a.Width() < 1 || a.Height() > 3)
{
mfem_error("CalcAdjugate(...)");
}
if (a.Width() != adja.Height() || a.Height() != adja.Width())
{
mfem_error("CalcAdjugate(...)");
}
#endif
if (a.Width() < a.Height())
{
const double *d = a.Data();
double *ad = adja.Data();
if (a.Width() == 1)
{
// N x 1, N = 2,3
ad[0] = d[0];
ad[1] = d[1];
if (a.Height() == 3)
{
ad[2] = d[2];
}
}
else
{
// 3 x 2
double e, g, f;
e = d[0]*d[0] + d[1]*d[1] + d[2]*d[2];
g = d[3]*d[3] + d[4]*d[4] + d[5]*d[5];
f = d[0]*d[3] + d[1]*d[4] + d[2]*d[5];
ad[0] = d[0]*g - d[3]*f;
ad[1] = d[3]*e - d[0]*f;
ad[2] = d[1]*g - d[4]*f;
ad[3] = d[4]*e - d[1]*f;
ad[4] = d[2]*g - d[5]*f;
ad[5] = d[5]*e - d[2]*f;
}
return;
}
if (a.Width() == 1)
{
adja(0,0) = 1.0;
}
else if (a.Width() == 2)
{
adja(0,0) = a(1,1);
adja(0,1) = -a(0,1);
adja(1,0) = -a(1,0);
adja(1,1) = a(0,0);
}
else
{
adja(0,0) = a(1,1)*a(2,2)-a(1,2)*a(2,1);
adja(0,1) = a(0,2)*a(2,1)-a(0,1)*a(2,2);
adja(0,2) = a(0,1)*a(1,2)-a(0,2)*a(1,1);
adja(1,0) = a(1,2)*a(2,0)-a(1,0)*a(2,2);
adja(1,1) = a(0,0)*a(2,2)-a(0,2)*a(2,0);
adja(1,2) = a(0,2)*a(1,0)-a(0,0)*a(1,2);
adja(2,0) = a(1,0)*a(2,1)-a(1,1)*a(2,0);
adja(2,1) = a(0,1)*a(2,0)-a(0,0)*a(2,1);
adja(2,2) = a(0,0)*a(1,1)-a(0,1)*a(1,0);
}
}
void CalcAdjugateTranspose(const DenseMatrix &a, DenseMatrix &adjat)
{
#ifdef MFEM_DEBUG
if (a.Height() != a.Width() || adjat.Height() != adjat.Width() ||
a.Width() != adjat.Width() || a.Width() < 1 || a.Width() > 3)
{
mfem_error("CalcAdjugateTranspose(...)");
}
#endif
if (a.Width() == 1)
{
adjat(0,0) = 1.0;
}
else if (a.Width() == 2)
{
adjat(0,0) = a(1,1);
adjat(1,0) = -a(0,1);
adjat(0,1) = -a(1,0);
adjat(1,1) = a(0,0);
}
else
{
adjat(0,0) = a(1,1)*a(2,2)-a(1,2)*a(2,1);
adjat(1,0) = a(0,2)*a(2,1)-a(0,1)*a(2,2);
adjat(2,0) = a(0,1)*a(1,2)-a(0,2)*a(1,1);
adjat(0,1) = a(1,2)*a(2,0)-a(1,0)*a(2,2);
adjat(1,1) = a(0,0)*a(2,2)-a(0,2)*a(2,0);
adjat(2,1) = a(0,2)*a(1,0)-a(0,0)*a(1,2);
adjat(0,2) = a(1,0)*a(2,1)-a(1,1)*a(2,0);
adjat(1,2) = a(0,1)*a(2,0)-a(0,0)*a(2,1);
adjat(2,2) = a(0,0)*a(1,1)-a(0,1)*a(1,0);
}
}
void CalcInverse(const DenseMatrix &a, DenseMatrix &inva)
{
MFEM_ASSERT(a.Width() <= a.Height() && a.Width() >= 1 && a.Height() <= 3, "");
MFEM_ASSERT(inva.Height() == a.Width(), "incorrect dimensions");
MFEM_ASSERT(inva.Width() == a.Height(), "incorrect dimensions");
double t;
if (a.Width() < a.Height())
{
const double *d = a.Data();
double *id = inva.Data();
if (a.Height() == 2)
{
t = 1.0 / (d[0]*d[0] + d[1]*d[1]);
id[0] = d[0] * t;
id[1] = d[1] * t;
}
else
{
if (a.Width() == 1)
{
t = 1.0 / (d[0]*d[0] + d[1]*d[1] + d[2]*d[2]);
id[0] = d[0] * t;
id[1] = d[1] * t;
id[2] = d[2] * t;
}
else
{
double e, g, f;
e = d[0]*d[0] + d[1]*d[1] + d[2]*d[2];
g = d[3]*d[3] + d[4]*d[4] + d[5]*d[5];
f = d[0]*d[3] + d[1]*d[4] + d[2]*d[5];
t = 1.0 / (e*g - f*f);
e *= t; g *= t; f *= t;
id[0] = d[0]*g - d[3]*f;
id[1] = d[3]*e - d[0]*f;
id[2] = d[1]*g - d[4]*f;
id[3] = d[4]*e - d[1]*f;
id[4] = d[2]*g - d[5]*f;
id[5] = d[5]*e - d[2]*f;
}
}
return;
}
#ifdef MFEM_DEBUG
t = a.Det();
MFEM_ASSERT(std::abs(t) > 1.0e-14 * pow(a.FNorm()/a.Width(), a.Width()),
"singular matrix!");
t = 1.0 / t;
#else
t = 1.0 / a.Det();
#endif
switch (a.Height())
{
case 1:
inva(0,0) = t;
break;
case 2:
inva(0,0) = a(1,1) * t ;
inva(0,1) = -a(0,1) * t ;
inva(1,0) = -a(1,0) * t ;
inva(1,1) = a(0,0) * t ;
break;
case 3:
inva(0,0) = (a(1,1)*a(2,2)-a(1,2)*a(2,1))*t;
inva(0,1) = (a(0,2)*a(2,1)-a(0,1)*a(2,2))*t;
inva(0,2) = (a(0,1)*a(1,2)-a(0,2)*a(1,1))*t;
inva(1,0) = (a(1,2)*a(2,0)-a(1,0)*a(2,2))*t;
inva(1,1) = (a(0,0)*a(2,2)-a(0,2)*a(2,0))*t;
inva(1,2) = (a(0,2)*a(1,0)-a(0,0)*a(1,2))*t;
inva(2,0) = (a(1,0)*a(2,1)-a(1,1)*a(2,0))*t;
inva(2,1) = (a(0,1)*a(2,0)-a(0,0)*a(2,1))*t;
inva(2,2) = (a(0,0)*a(1,1)-a(0,1)*a(1,0))*t;
break;
}
}
void CalcInverseTranspose(const DenseMatrix &a, DenseMatrix &inva)
{
#ifdef MFEM_DEBUG
if ( (a.Width() != a.Height()) || ( (a.Height()!= 1) && (a.Height()!= 2)
&& (a.Height()!= 3) ) )
{
mfem_error("CalcInverseTranspose(...)");
}
#endif
double t = 1. / a.Det() ;
switch (a.Height())
{
case 1:
inva(0,0) = 1.0 / a(0,0);
break;
case 2:
inva(0,0) = a(1,1) * t ;
inva(1,0) = -a(0,1) * t ;
inva(0,1) = -a(1,0) * t ;
inva(1,1) = a(0,0) * t ;
break;
case 3:
inva(0,0) = (a(1,1)*a(2,2)-a(1,2)*a(2,1))*t;
inva(1,0) = (a(0,2)*a(2,1)-a(0,1)*a(2,2))*t;
inva(2,0) = (a(0,1)*a(1,2)-a(0,2)*a(1,1))*t;
inva(0,1) = (a(1,2)*a(2,0)-a(1,0)*a(2,2))*t;
inva(1,1) = (a(0,0)*a(2,2)-a(0,2)*a(2,0))*t;
inva(2,1) = (a(0,2)*a(1,0)-a(0,0)*a(1,2))*t;
inva(0,2) = (a(1,0)*a(2,1)-a(1,1)*a(2,0))*t;
inva(1,2) = (a(0,1)*a(2,0)-a(0,0)*a(2,1))*t;
inva(2,2) = (a(0,0)*a(1,1)-a(0,1)*a(1,0))*t;
break;
}
}
void CalcOrtho(const DenseMatrix &J, Vector &n)
{
MFEM_ASSERT( ((J.Height() == 2 && J.Width() == 1)
|| (J.Height() == 3 && J.Width() == 2))
&& (J.Height() == n.Size()),
"Matrix must be 3x2 or 2x1, "
<< "and the Vector must be sized with the rows. "
<< " J.Height() = " << J.Height()
<< ", J.Width() = " << J.Width()
<< ", n.Size() = " << n.Size()
);
const double *d = J.Data();
if (J.Height() == 2)
{
n(0) = d[1];
n(1) = -d[0];
}
else
{
n(0) = d[1]*d[5] - d[2]*d[4];
n(1) = d[2]*d[3] - d[0]*d[5];
n(2) = d[0]*d[4] - d[1]*d[3];
}
}
void MultAAt(const DenseMatrix &a, DenseMatrix &aat)
{
const int height = a.Height();
const int width = a.Width();
for (int i = 0; i < height; i++)
{
for (int j = 0; j <= i; j++)
{
double temp = 0.;
for (int k = 0; k < width; k++)
{
temp += a(i,k) * a(j,k);
}
aat(j,i) = aat(i,j) = temp;
}
}
}
void AddMultADAt(const DenseMatrix &A, const Vector &D, DenseMatrix &ADAt)
{
for (int i = 0; i < A.Height(); i++)
{
for (int j = 0; j < i; j++)
{
double t = 0.;
for (int k = 0; k < A.Width(); k++)
{
t += D(k) * A(i, k) * A(j, k);
}
ADAt(i, j) += t;
ADAt(j, i) += t;
}
}
// process diagonal
for (int i = 0; i < A.Height(); i++)
{
double t = 0.;
for (int k = 0; k < A.Width(); k++)
{
t += D(k) * A(i, k) * A(i, k);
}
ADAt(i, i) += t;
}
}
void MultADAt(const DenseMatrix &A, const Vector &D, DenseMatrix &ADAt)
{
for (int i = 0; i < A.Height(); i++)
{
for (int j = 0; j <= i; j++)
{
double t = 0.;
for (int k = 0; k < A.Width(); k++)
{
t += D(k) * A(i, k) * A(j, k);
}
ADAt(j, i) = ADAt(i, j) = t;
}
}
}
void MultABt(const DenseMatrix &A, const DenseMatrix &B, DenseMatrix &ABt)
{
#ifdef MFEM_DEBUG
if (A.Height() != ABt.Height() || B.Height() != ABt.Width() ||
A.Width() != B.Width())
{
mfem_error("MultABt(...)");
}
#endif
#ifdef MFEM_USE_LAPACK
static char transa = 'N', transb = 'T';
static double alpha = 1.0, beta = 0.0;
int m = A.Height(), n = B.Height(), k = A.Width();
dgemm_(&transa, &transb, &m, &n, &k, &alpha, A.Data(), &m,
B.Data(), &n, &beta, ABt.Data(), &m);
#elif 1
const int ah = A.Height();
const int bh = B.Height();
const int aw = A.Width();
const double *ad = A.Data();
const double *bd = B.Data();
double *cd = ABt.Data();
for (int i = 0, s = ah*bh; i < s; i++)
{
cd[i] = 0.0;
}
for (int k = 0; k < aw; k++)
{
double *cp = cd;
for (int j = 0; j < bh; j++)
{
const double bjk = bd[j];
for (int i = 0; i < ah; i++)
{
cp[i] += ad[i] * bjk;
}
cp += ah;
}
ad += ah;
bd += bh;
}
#elif 1
const int ah = A.Height();
const int bh = B.Height();
const int aw = A.Width();
const double *ad = A.Data();
const double *bd = B.Data();
double *cd = ABt.Data();
for (int j = 0; j < bh; j++)
for (int i = 0; i < ah; i++)
{
double d = 0.0;
const double *ap = ad + i;
const double *bp = bd + j;
for (int k = 0; k < aw; k++)
{
d += (*ap) * (*bp);
ap += ah;
bp += bh;
}
*(cd++) = d;
}
#else
int i, j, k;
double d;
for (i = 0; i < A.Height(); i++)
for (j = 0; j < B.Height(); j++)
{
d = 0.0;
for (k = 0; k < A.Width(); k++)
{
d += A(i, k) * B(j, k);
}
ABt(i, j) = d;
}
#endif
}
void MultADBt(const DenseMatrix &A, const Vector &D,
const DenseMatrix &B, DenseMatrix &ADBt)
{
#ifdef MFEM_DEBUG
if (A.Height() != ADBt.Height() || B.Height() != ADBt.Width() ||
A.Width() != B.Width() || A.Width() != D.Size())
{
mfem_error("MultADBt(...)");
}
#endif
const int ah = A.Height();
const int bh = B.Height();
const int aw = A.Width();
const double *ad = A.Data();
const double *bd = B.Data();
const double *dd = D.GetData();
double *cd = ADBt.Data();
for (int i = 0, s = ah*bh; i < s; i++)
{
cd[i] = 0.0;
}
for (int k = 0; k < aw; k++)
{
double *cp = cd;
for (int j = 0; j < bh; j++)
{
const double dk_bjk = dd[k] * bd[j];
for (int i = 0; i < ah; i++)
{
cp[i] += ad[i] * dk_bjk;
}
cp += ah;
}
ad += ah;
bd += bh;
}
}
void AddMultABt(const DenseMatrix &A, const DenseMatrix &B, DenseMatrix &ABt)
{
#ifdef MFEM_DEBUG
if (A.Height() != ABt.Height() || B.Height() != ABt.Width() ||
A.Width() != B.Width())
{
mfem_error("AddMultABt(...)");
}
#endif
#ifdef MFEM_USE_LAPACK
static char transa = 'N', transb = 'T';
static double alpha = 1.0, beta = 1.0;
int m = A.Height(), n = B.Height(), k = A.Width();
dgemm_(&transa, &transb, &m, &n, &k, &alpha, A.Data(), &m,
B.Data(), &n, &beta, ABt.Data(), &m);
#elif 1
const int ah = A.Height();
const int bh = B.Height();
const int aw = A.Width();
const double *ad = A.Data();
const double *bd = B.Data();
double *cd = ABt.Data();
for (int k = 0; k < aw; k++)
{
double *cp = cd;
for (int j = 0; j < bh; j++)
{
const double bjk = bd[j];
for (int i = 0; i < ah; i++)
{
cp[i] += ad[i] * bjk;
}
cp += ah;
}
ad += ah;
bd += bh;
}
#else
int i, j, k;
double d;
for (i = 0; i < A.Height(); i++)
for (j = 0; j < B.Height(); j++)
{
d = 0.0;
for (k = 0; k < A.Width(); k++)
{
d += A(i, k) * B(j, k);
}
ABt(i, j) += d;
}
#endif
}
void AddMultADBt(const DenseMatrix &A, const Vector &D,
const DenseMatrix &B, DenseMatrix &ADBt)
{
#ifdef MFEM_DEBUG
if (A.Height() != ADBt.Height() || B.Height() != ADBt.Width() ||
A.Width() != B.Width() || A.Width() != D.Size())
{
mfem_error("AddMultADBt(...)");
}
#endif
const int ah = A.Height();
const int bh = B.Height();
const int aw = A.Width();
const double *ad = A.Data();
const double *bd = B.Data();
const double *dd = D.GetData();
double *cd = ADBt.Data();
for (int k = 0; k < aw; k++)
{
double *cp = cd;
for (int j = 0; j < bh; j++)
{
const double dk_bjk = dd[k] * bd[j];
for (int i = 0; i < ah; i++)
{
cp[i] += ad[i] * dk_bjk;
}
cp += ah;
}
ad += ah;
bd += bh;
}
}
void AddMult_a_ABt(double a, const DenseMatrix &A, const DenseMatrix &B,
DenseMatrix &ABt)
{
#ifdef MFEM_DEBUG
if (A.Height() != ABt.Height() || B.Height() != ABt.Width() ||
A.Width() != B.Width())
{
mfem_error("AddMult_a_ABt(...)");
}
#endif
#ifdef MFEM_USE_LAPACK
static char transa = 'N', transb = 'T';
double alpha = a;
static double beta = 1.0;
int m = A.Height(), n = B.Height(), k = A.Width();
dgemm_(&transa, &transb, &m, &n, &k, &alpha, A.Data(), &m,
B.Data(), &n, &beta, ABt.Data(), &m);
#elif 1
const int ah = A.Height();
const int bh = B.Height();
const int aw = A.Width();
const double *ad = A.Data();
const double *bd = B.Data();
double *cd = ABt.Data();
for (int k = 0; k < aw; k++)
{
double *cp = cd;
for (int j = 0; j < bh; j++)
{
const double bjk = a * bd[j];
for (int i = 0; i < ah; i++)
{
cp[i] += ad[i] * bjk;
}
cp += ah;
}
ad += ah;
bd += bh;
}
#else
int i, j, k;
double d;
for (i = 0; i < A.Height(); i++)
for (j = 0; j < B.Height(); j++)
{
d = 0.0;
for (k = 0; k < A.Width(); k++)
{
d += A(i, k) * B(j, k);
}
ABt(i, j) += a * d;
}
#endif
}
void MultAtB(const DenseMatrix &A, const DenseMatrix &B, DenseMatrix &AtB)
{
#ifdef MFEM_DEBUG
if (A.Width() != AtB.Height() || B.Width() != AtB.Width() ||
A.Height() != B.Height())
{
mfem_error("MultAtB(...)");
}
#endif
#ifdef MFEM_USE_LAPACK
static char transa = 'T', transb = 'N';
static double alpha = 1.0, beta = 0.0;
int m = A.Width(), n = B.Width(), k = A.Height();
dgemm_(&transa, &transb, &m, &n, &k, &alpha, A.Data(), &k,
B.Data(), &k, &beta, AtB.Data(), &m);
#elif 1
const int ah = A.Height();
const int aw = A.Width();
const int bw = B.Width();
const double *ad = A.Data();
const double *bd = B.Data();
double *cd = AtB.Data();
for (int j = 0; j < bw; j++)
{
const double *ap = ad;
for (int i = 0; i < aw; i++)
{
double d = 0.0;
for (int k = 0; k < ah; k++)
{
d += ap[k] * bd[k];
}
*(cd++) = d;
ap += ah;
}
bd += ah;
}
#else
int i, j, k;
double d;
for (i = 0; i < A.Width(); i++)
for (j = 0; j < B.Width(); j++)
{
d = 0.0;
for (k = 0; k < A.Height(); k++)
{
d += A(k, i) * B(k, j);
}
AtB(i, j) = d;
}
#endif
}
void AddMult_a_AAt(double a, const DenseMatrix &A, DenseMatrix &AAt)
{
double d;
for (int i = 0; i < A.Height(); i++)
{
for (int j = 0; j < i; j++)
{
d = 0.;
for (int k = 0; k < A.Width(); k++)
{
d += A(i,k) * A(j,k);
}
AAt(i, j) += (d *= a);
AAt(j, i) += d;
}
d = 0.;
for (int k = 0; k < A.Width(); k++)
{
d += A(i,k) * A(i,k);
}
AAt(i, i) += a * d;
}
}
void Mult_a_AAt(double a, const DenseMatrix &A, DenseMatrix &AAt)
{
for (int i = 0; i < A.Height(); i++)
{
for (int j = 0; j <= i; j++)
{
double d = 0.;
for (int k = 0; k < A.Width(); k++)
{
d += A(i,k) * A(j,k);
}
AAt(i, j) = AAt(j, i) = a * d;
}
}
}
void MultVVt(const Vector &v, DenseMatrix &vvt)
{
for (int i = 0; i < v.Size(); i++)
{
for (int j = 0; j <= i; j++)
{
vvt(i,j) = vvt(j,i) = v(i) * v(j);
}
}
}
void MultVWt(const Vector &v, const Vector &w, DenseMatrix &VWt)
{
#ifdef MFEM_DEBUG
if (v.Size() != VWt.Height() || w.Size() != VWt.Width())
{
mfem_error("MultVWt(...)");
}
#endif
for (int i = 0; i < v.Size(); i++)
{
const double vi = v(i);
for (int j = 0; j < w.Size(); j++)
{
VWt(i, j) = vi * w(j);
}
}
}
void AddMultVWt(const Vector &v, const Vector &w, DenseMatrix &VWt)
{
const int m = v.Size(), n = w.Size();
#ifdef MFEM_DEBUG
if (VWt.Height() != m || VWt.Width() != n)
{
mfem_error("AddMultVWt(...)");
}
#endif
for (int i = 0; i < m; i++)
{
const double vi = v(i);
for (int j = 0; j < n; j++)
{
VWt(i, j) += vi * w(j);
}
}
}
void AddMultVVt(const Vector &v, DenseMatrix &VVt)
{
const int n = v.Size();
#ifdef MFEM_DEBUG
if (VVt.Height() != n || VVt.Width() != n)
{
mfem_error("AddMultVVt(...)");
}
#endif
for (int i = 0; i < n; i++)
{
const double vi = v(i);
for (int j = 0; j < i; j++)
{
const double vivj = vi * v(j);
VVt(i, j) += vivj;
VVt(j, i) += vivj;
}
VVt(i, i) += vi * vi;
}
}
void AddMult_a_VWt(const double a, const Vector &v, const Vector &w,
DenseMatrix &VWt)
{
const int m = v.Size(), n = w.Size();
#ifdef MFEM_DEBUG
if (VWt.Height() != m || VWt.Width() != n)
{
mfem_error("AddMult_a_VWt(...)");
}
#endif
for (int j = 0; j < n; j++)
{
const double awj = a * w(j);
for (int i = 0; i < m; i++)
{
VWt(i, j) += v(i) * awj;
}
}
}
void AddMult_a_VVt(const double a, const Vector &v, DenseMatrix &VVt)
{
MFEM_ASSERT(VVt.Height() == v.Size() && VVt.Width() == v.Size(),
"incompatible dimensions!");
const int n = v.Size();
for (int i = 0; i < n; i++)
{
double avi = a * v(i);
for (int j = 0; j < i; j++)
{
const double avivj = avi * v(j);
VVt(i, j) += avivj;
VVt(j, i) += avivj;
}
VVt(i, i) += avi * v(i);
}
}
void LUFactors::Factor(int m)
{
#ifdef MFEM_USE_LAPACK
int info = 0;
if (m) { dgetrf_(&m, &m, data, &m, ipiv, &info); }
MFEM_VERIFY(!info, "LAPACK: error in DGETRF");
#else
// compiling without LAPACK
double *data = this->data;
for (int i = 0; i < m; i++)
{
// pivoting
{
int piv = i;
double a = std::abs(data[piv+i*m]);
for (int j = i+1; j < m; j++)
{
const double b = std::abs(data[j+i*m]);
if (b > a)
{
a = b;
piv = j;
}
}
ipiv[i] = piv;
if (piv != i)
{
// swap rows i and piv in both L and U parts
for (int j = 0; j < m; j++)
{
Swap<double>(data[i+j*m], data[piv+j*m]);
}
}
}
MFEM_ASSERT(data[i+i*m] != 0.0, "division by zero");
const double a_ii_inv = 1.0/data[i+i*m];
for (int j = i+1; j < m; j++)
{
data[j+i*m] *= a_ii_inv;
}
for (int k = i+1; k < m; k++)
{
const double a_ik = data[i+k*m];
for (int j = i+1; j < m; j++)
{
data[j+k*m] -= a_ik * data[j+i*m];
}
}
}
#endif
}
double LUFactors::Det(int m) const
{
double det = 1.0;
for (int i=0; i<m; i++)
{
if (ipiv[i] != i-ipiv_base)
{
det *= -data[m * i + i];
}
else
{
det *= data[m * i + i];
}
}
return det;
}
void LUFactors::Mult(int m, int n, double *X) const
{
const double *data = this->data;
const int *ipiv = this->ipiv;
double *x = X;
for (int k = 0; k < n; k++)
{
// X <- U X
for (int i = 0; i < m; i++)
{
double x_i = x[i] * data[i+i*m];
for (int j = i+1; j < m; j++)
{
x_i += x[j] * data[i+j*m];
}
x[i] = x_i;
}
// X <- L X
for (int i = m-1; i >= 0; i--)
{
double x_i = x[i];
for (int j = 0; j < i; j++)
{
x_i += x[j] * data[i+j*m];
}
x[i] = x_i;
}
// X <- P^{-1} X
for (int i = m-1; i >= 0; i--)
{
Swap<double>(x[i], x[ipiv[i]-ipiv_base]);
}
x += m;
}
}
void LUFactors::LSolve(int m, int n, double *X) const
{
const double *data = this->data;
const int *ipiv = this->ipiv;
double *x = X;
for (int k = 0; k < n; k++)
{
// X <- P X
for (int i = 0; i < m; i++)
{
Swap<double>(x[i], x[ipiv[i]-ipiv_base]);
}
// X <- L^{-1} X
for (int j = 0; j < m; j++)
{
const double x_j = x[j];
for (int i = j+1; i < m; i++)
{
x[i] -= data[i+j*m] * x_j;
}
}
x += m;
}
}
void LUFactors::USolve(int m, int n, double *X) const
{
const double *data = this->data;
double *x = X;
// X <- U^{-1} X
for (int k = 0; k < n; k++)
{
for (int j = m-1; j >= 0; j--)
{
const double x_j = ( x[j] /= data[j+j*m] );
for (int i = 0; i < j; i++)
{
x[i] -= data[i+j*m] * x_j;
}
}
x += m;
}
}
void LUFactors::Solve(int m, int n, double *X) const
{
#ifdef MFEM_USE_LAPACK
char trans = 'N';
int info = 0;
if (m > 0 && n > 0) { dgetrs_(&trans, &m, &n, data, &m, ipiv, X, &m, &info); }
MFEM_VERIFY(!info, "LAPACK: error in DGETRS");
#else
// compiling without LAPACK
LSolve(m, n, X);
USolve(m, n, X);
#endif
}
void LUFactors::GetInverseMatrix(int m, double *X) const
{
// A^{-1} = U^{-1} L^{-1} P
const double *data = this->data;
const int *ipiv = this->ipiv;
// X <- U^{-1} (set only the upper triangular part of X)
double *x = X;
for (int k = 0; k < m; k++)
{
const double minus_x_k = -( x[k] = 1.0/data[k+k*m] );
for (int i = 0; i < k; i++)
{
x[i] = data[i+k*m] * minus_x_k;
}
for (int j = k-1; j >= 0; j--)
{
const double x_j = ( x[j] /= data[j+j*m] );
for (int i = 0; i < j; i++)
{
x[i] -= data[i+j*m] * x_j;
}
}
x += m;
}
// X <- X L^{-1} (use input only from the upper triangular part of X)
{
int k = m-1;
for (int j = 0; j < k; j++)
{
const double minus_L_kj = -data[k+j*m];
for (int i = 0; i <= j; i++)
{
X[i+j*m] += X[i+k*m] * minus_L_kj;
}
for (int i = j+1; i < m; i++)
{
X[i+j*m] = X[i+k*m] * minus_L_kj;
}
}
}
for (int k = m-2; k >= 0; k--)
{
for (int j = 0; j < k; j++)
{
const double L_kj = data[k+j*m];
for (int i = 0; i < m; i++)
{
X[i+j*m] -= X[i+k*m] * L_kj;
}
}
}
// X <- X P
for (int k = m-1; k >= 0; k--)
{
const int piv_k = ipiv[k]-ipiv_base;
if (k != piv_k)
{
for (int i = 0; i < m; i++)
{
Swap<double>(X[i+k*m], X[i+piv_k*m]);
}
}
}
}
void LUFactors::SubMult(int m, int n, int r, const double *A21,
const double *X1, double *X2)
{
// X2 <- X2 - A21 X1
for (int k = 0; k < r; k++)
{
for (int j = 0; j < m; j++)
{
const double x1_jk = X1[j+k*m];
for (int i = 0; i < n; i++)
{
X2[i+k*n] -= A21[i+j*n] * x1_jk;
}
}
}
}
void LUFactors::BlockFactor(
int m, int n, double *A12, double *A21, double *A22) const
{
const double *data = this->data;
// A12 <- L^{-1} P A12
LSolve(m, n, A12);
// A21 <- A21 U^{-1}
for (int j = 0; j < m; j++)
{
const double u_jj_inv = 1.0/data[j+j*m];
for (int i = 0; i < n; i++)
{
A21[i+j*n] *= u_jj_inv;
}
for (int k = j+1; k < m; k++)
{
const double u_jk = data[j+k*m];
for (int i = 0; i < n; i++)
{
A21[i+k*n] -= A21[i+j*n] * u_jk;
}
}
}
// A22 <- A22 - A21 A12
SubMult(m, n, n, A21, A12, A22);
}
void LUFactors::BlockForwSolve(int m, int n, int r, const double *L21,
double *B1, double *B2) const
{
// B1 <- L^{-1} P B1
LSolve(m, r, B1);
// B2 <- B2 - L21 B1
SubMult(m, n, r, L21, B1, B2);
}
void LUFactors::BlockBackSolve(int m, int n, int r, const double *U12,
const double *X2, double *Y1) const
{
// Y1 <- Y1 - U12 X2
SubMult(n, m, r, U12, X2, Y1);
// Y1 <- U^{-1} Y1
USolve(m, r, Y1);
}
DenseMatrixInverse::DenseMatrixInverse(const DenseMatrix &mat)
: MatrixInverse(mat)
{
MFEM_ASSERT(height == width, "not a square matrix");
a = &mat;
lu.data = new double[width*width];
lu.ipiv = new int[width];
Factor();
}
DenseMatrixInverse::DenseMatrixInverse(const DenseMatrix *mat)
: MatrixInverse(*mat)
{
MFEM_ASSERT(height == width, "not a square matrix");
a = mat;
lu.data = new double[width*width];
lu.ipiv = new int[width];
}
void DenseMatrixInverse::Factor()
{
MFEM_ASSERT(a, "DenseMatrix is not given");
const double *adata = a->data;
const int s = width*width;
for (int i = 0; i < s; i++)
{
lu.data[i] = adata[i];
}
lu.Factor(width);
}
void DenseMatrixInverse::GetInverseMatrix(DenseMatrix &Ainv) const
{
Ainv.SetSize(width);
lu.GetInverseMatrix(width, Ainv.Data());
}
void DenseMatrixInverse::Factor(const DenseMatrix &mat)
{
MFEM_VERIFY(mat.height == mat.width, "DenseMatrix is not square!");
if (width != mat.width)
{
height = width = mat.width;
delete [] lu.data;
lu.data = new double[width*width];
delete [] lu.ipiv;
lu.ipiv = new int[width];
}
a = &mat;
Factor();
}
void DenseMatrixInverse::SetOperator(const Operator &op)
{
const DenseMatrix *p = dynamic_cast<const DenseMatrix*>(&op);
MFEM_VERIFY(p != NULL, "Operator is not a DenseMatrix!");
Factor(*p);
}
void DenseMatrixInverse::Mult(const Vector &x, Vector &y) const
{
y = x;
lu.Solve(width, 1, y.GetData());
}
void DenseMatrixInverse::Mult(const DenseMatrix &B, DenseMatrix &X) const
{
X = B;
lu.Solve(width, X.Width(), X.Data());
}
void DenseMatrixInverse::TestInversion()
{
DenseMatrix C(width);
Mult(*a, C);
for (int i = 0; i < width; i++)
{
C(i,i) -= 1.0;
}
mfem::out << "size = " << width << ", i_max = " << C.MaxMaxNorm() << endl;
}
DenseMatrixInverse::~DenseMatrixInverse()
{
delete [] lu.data;
delete [] lu.ipiv;
}
DenseMatrixEigensystem::DenseMatrixEigensystem(DenseMatrix &m)
: mat(m)
{
n = mat.Width();
EVal.SetSize(n);
EVect.SetSize(n);
ev.SetDataAndSize(NULL, n);
#ifdef MFEM_USE_LAPACK
jobz = 'V';
uplo = 'U';
lwork = -1;
double qwork;
dsyev_(&jobz, &uplo, &n, EVect.Data(), &n, EVal.GetData(),
&qwork, &lwork, &info);
lwork = (int) qwork;
work = new double[lwork];
#endif
}
DenseMatrixEigensystem::DenseMatrixEigensystem(
const DenseMatrixEigensystem &other)
: mat(other.mat), EVal(other.EVal), EVect(other.EVect), ev(NULL, other.n),
n(other.n)
{
#ifdef MFEM_USE_LAPACK
jobz = other.jobz;
uplo = other.uplo;
lwork = other.lwork;
work = new double[lwork];
#endif
}
void DenseMatrixEigensystem::Eval()
{
#ifdef MFEM_DEBUG
if (mat.Width() != n)
{
mfem_error("DenseMatrixEigensystem::Eval()");
}
#endif
#ifdef MFEM_USE_LAPACK
EVect = mat;
dsyev_(&jobz, &uplo, &n, EVect.Data(), &n, EVal.GetData(),
work, &lwork, &info);
if (info != 0)
{
mfem::err << "DenseMatrixEigensystem::Eval(): DSYEV error code: "
<< info << endl;
mfem_error();
}
#else
mfem_error("DenseMatrixEigensystem::Eval(): Compiled without LAPACK");
#endif
}
DenseMatrixEigensystem::~DenseMatrixEigensystem()
{
#ifdef MFEM_USE_LAPACK
delete [] work;
#endif
}
DenseMatrixSVD::DenseMatrixSVD(DenseMatrix &M)
{
m = M.Height();
n = M.Width();
Init();
}
DenseMatrixSVD::DenseMatrixSVD(int h, int w)
{
m = h;
n = w;
Init();
}
void DenseMatrixSVD::Init()
{
#ifdef MFEM_USE_LAPACK
sv.SetSize(min(m, n));
jobu = 'N';
jobvt = 'N';
double qwork;
lwork = -1;
dgesvd_(&jobu, &jobvt, &m, &n, NULL, &m, sv.GetData(), NULL, &m,
NULL, &n, &qwork, &lwork, &info);
lwork = (int) qwork;
work = new double[lwork];
#else
mfem_error("DenseMatrixSVD::Init(): Compiled without LAPACK");
#endif
}
void DenseMatrixSVD::Eval(DenseMatrix &M)
{
#ifdef MFEM_DEBUG
if (M.Height() != m || M.Width() != n)
{
mfem_error("DenseMatrixSVD::Eval()");
}
#endif
#ifdef MFEM_USE_LAPACK
dgesvd_(&jobu, &jobvt, &m, &n, M.Data(), &m, sv.GetData(), NULL, &m,
NULL, &n, work, &lwork, &info);
if (info)
{
mfem::err << "DenseMatrixSVD::Eval() : info = " << info << endl;
mfem_error();
}
#else
mfem_error("DenseMatrixSVD::Eval(): Compiled without LAPACK");
#endif
}
DenseMatrixSVD::~DenseMatrixSVD()
{
#ifdef MFEM_USE_LAPACK
delete [] work;
#endif
}
void DenseTensor::AddMult(const Table &elem_dof, const Vector &x, Vector &y)
const
{
int n = SizeI(), ne = SizeK();
const int *I = elem_dof.GetI(), *J = elem_dof.GetJ(), *dofs;
const double *d_col = tdata;
double *yp = y, x_col;
const double *xp = x;
// the '4' here can be tuned for given platform and compiler
if (n <= 4)
{
for (int i = 0; i < ne; i++)
{
dofs = J + I[i];
for (int col = 0; col < n; col++)
{
x_col = xp[dofs[col]];
for (int row = 0; row < n; row++)
{
yp[dofs[row]] += x_col*d_col[row];
}
d_col += n;
}
}
}
else
{
Vector ye(n);
for (int i = 0; i < ne; i++)
{
dofs = J + I[i];
x_col = xp[dofs[0]];
for (int row = 0; row < n; row++)
{
ye(row) = x_col*d_col[row];
}
d_col += n;
for (int col = 1; col < n; col++)
{
x_col = xp[dofs[col]];
for (int row = 0; row < n; row++)
{
ye(row) += x_col*d_col[row];
}
d_col += n;
}
for (int row = 0; row < n; row++)
{
yp[dofs[row]] += ye(row);
}
}
}
}
DenseTensor &DenseTensor::operator=(double c)
{
int s = SizeI() * SizeJ() * SizeK();
for (int i=0; i<s; i++)
{
tdata[i] = c;
}
return *this;
}
}
Reference in New Issue View Git Blame Copy Permalink