4384 lines
98 KiB
C++
4384 lines
98 KiB
C++
// Copyright (c) 2010-2022, Lawrence Livermore National Security, LLC. Produced
|
||
// at the Lawrence Livermore National Laboratory. All Rights reserved. See files
|
||
// LICENSE and NOTICE for details. LLNL-CODE-806117.
|
||
//
|
||
// This file is part of the MFEM library. For more information and source code
|
||
// availability visit https://mfem.org.
|
||
//
|
||
// MFEM is free software; you can redistribute it and/or modify it under the
|
||
// terms of the BSD-3 license. We welcome feedback and contributions, see file
|
||
// CONTRIBUTING.md for details.
|
||
|
||
|
||
// Implementation of data types dense matrix, inverse dense matrix
|
||
|
||
|
||
#include "kernels.hpp"
|
||
#include "vector.hpp"
|
||
#include "matrix.hpp"
|
||
#include "densemat.hpp"
|
||
#include "kernels.hpp"
|
||
#include "../general/forall.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);
|
||
extern "C" void
|
||
dtrsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n,
|
||
double *alpha, double *a, int *lda, double *b, int *ldb);
|
||
extern "C" void
|
||
dggev_(char *jobvl, char *jobvr, int *n, double *a, int *lda, double *B,
|
||
int *ldb, double *alphar, double *alphai, double *beta, double *vl,
|
||
int * ldvl, double * vr, int * ldvr, double * work, int * lwork, int* info);
|
||
|
||
// Cholesky factorizations/solves
|
||
extern "C" void
|
||
dpotrf_(char *, int *, double *, int *, int *);
|
||
// Solve
|
||
extern "C" void
|
||
dpotrs_(char *, int *, int *, double *, int *, double *, int *, int *);
|
||
// Triangular Solves
|
||
extern "C" void
|
||
dtrtrs_(char *, char*, char *, int *, int *, double *, int *, double *, int *,
|
||
int *);
|
||
extern "C" void
|
||
dpotri_(char *, int *, double *, int*, int *);
|
||
#endif
|
||
|
||
|
||
namespace mfem
|
||
{
|
||
|
||
using namespace std;
|
||
|
||
DenseMatrix::DenseMatrix() : Matrix(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(hw);
|
||
std::memcpy(data, m.data, sizeof(double)*hw);
|
||
}
|
||
}
|
||
|
||
DenseMatrix::DenseMatrix(int s) : Matrix(s)
|
||
{
|
||
MFEM_ASSERT(s >= 0, "invalid DenseMatrix size: " << s);
|
||
if (s > 0)
|
||
{
|
||
data.New(s*s);
|
||
*this = 0.0; // init with zeroes
|
||
}
|
||
}
|
||
|
||
DenseMatrix::DenseMatrix(int m, int n) : Matrix(m, n)
|
||
{
|
||
MFEM_ASSERT(m >= 0 && n >= 0,
|
||
"invalid DenseMatrix size: " << m << " x " << n);
|
||
const int capacity = m*n;
|
||
if (capacity > 0)
|
||
{
|
||
data.New(capacity);
|
||
*this = 0.0; // init with zeroes
|
||
}
|
||
}
|
||
|
||
DenseMatrix::DenseMatrix(const DenseMatrix &mat, char ch)
|
||
: Matrix(mat.width, mat.height)
|
||
{
|
||
MFEM_CONTRACT_VAR(ch);
|
||
const int capacity = height*width;
|
||
if (capacity > 0)
|
||
{
|
||
data.New(capacity);
|
||
|
||
for (int i = 0; i < height; i++)
|
||
{
|
||
for (int j = 0; j < width; j++)
|
||
{
|
||
(*this)(i,j) = mat(j,i);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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 > data.Capacity())
|
||
{
|
||
data.Delete();
|
||
data.New(hw);
|
||
*this = 0.0; // 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
|
||
{
|
||
kernels::Mult(height, width, Data(), x, y);
|
||
}
|
||
|
||
void DenseMatrix::Mult(const double *x, Vector &y) const
|
||
{
|
||
MFEM_ASSERT(height == y.Size(), "incompatible dimensions");
|
||
|
||
Mult(x, y.GetData());
|
||
}
|
||
|
||
void DenseMatrix::Mult(const Vector &x, double *y) const
|
||
{
|
||
MFEM_ASSERT(width == x.Size(), "incompatible dimensions");
|
||
|
||
Mult(x.GetData(), y);
|
||
}
|
||
|
||
void DenseMatrix::Mult(const Vector &x, Vector &y) const
|
||
{
|
||
MFEM_ASSERT(height == y.Size() && width == x.Size(),
|
||
"incompatible dimensions");
|
||
|
||
Mult(x.GetData(), y.GetData());
|
||
}
|
||
|
||
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 double *x, Vector &y) const
|
||
{
|
||
MFEM_ASSERT(width == y.Size(), "incompatible dimensions");
|
||
|
||
MultTranspose(x, y.GetData());
|
||
}
|
||
|
||
void DenseMatrix::MultTranspose(const Vector &x, double *y) const
|
||
{
|
||
MFEM_ASSERT(height == x.Size(), "incompatible dimensions");
|
||
|
||
MultTranspose(x.GetData(), y);
|
||
}
|
||
|
||
void DenseMatrix::MultTranspose(const Vector &x, Vector &y) const
|
||
{
|
||
MFEM_ASSERT(height == x.Size() && width == y.Size(),
|
||
"incompatible dimensions");
|
||
|
||
MultTranspose(x.GetData(), y.GetData());
|
||
}
|
||
|
||
void DenseMatrix::AddMult(const Vector &x, Vector &y, const double a) const
|
||
{
|
||
if (a != 1.0)
|
||
{
|
||
AddMult_a(a, x, y);
|
||
return;
|
||
}
|
||
MFEM_ASSERT(height == y.Size() && width == x.Size(),
|
||
"incompatible dimensions");
|
||
|
||
const double *xp = x.GetData(), *d_col = data;
|
||
double *yp = y.GetData();
|
||
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 double a) const
|
||
{
|
||
if (a != 1.0)
|
||
{
|
||
AddMultTranspose_a(a, x, y);
|
||
return;
|
||
}
|
||
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.GetData(), *d_col = data;
|
||
double *yp = y.GetData();
|
||
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: dimension mismatch");
|
||
}
|
||
|
||
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::InvSymmetricScaling: dimension mismatch");
|
||
}
|
||
|
||
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(): mismatched or unsupported dimensions");
|
||
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);
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::Add(const double c, const double *A)
|
||
{
|
||
const int s = Width()*Height();
|
||
for (int i = 0; i < s; i++)
|
||
{
|
||
data[i] += c*A[i];
|
||
}
|
||
}
|
||
|
||
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)
|
||
{
|
||
kernels::Add(Height(), Width(), m, (double*)data);
|
||
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(): dimension mismatch");
|
||
}
|
||
#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++)
|
||
{
|
||
mfem::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++)
|
||
{
|
||
mfem::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;
|
||
#else
|
||
MFEM_CONTRACT_VAR(a);
|
||
MFEM_CONTRACT_VAR(ev);
|
||
MFEM_CONTRACT_VAR(evect);
|
||
#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; }
|
||
#else
|
||
MFEM_CONTRACT_VAR(a);
|
||
MFEM_CONTRACT_VAR(ev);
|
||
MFEM_CONTRACT_VAR(evect);
|
||
#endif
|
||
}
|
||
|
||
void DenseMatrix::Eigensystem(Vector &ev, DenseMatrix *evect)
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
|
||
// dsyevr_Eigensystem(*this, ev, evect);
|
||
|
||
dsyev_Eigensystem(*this, ev, evect);
|
||
|
||
#else
|
||
|
||
MFEM_CONTRACT_VAR(ev);
|
||
MFEM_CONTRACT_VAR(evect);
|
||
mfem_error("DenseMatrix::Eigensystem: Compiled without LAPACK");
|
||
|
||
#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; }
|
||
#else
|
||
MFEM_CONTRACT_VAR(a);
|
||
MFEM_CONTRACT_VAR(b);
|
||
MFEM_CONTRACT_VAR(ev);
|
||
MFEM_CONTRACT_VAR(evect);
|
||
#endif
|
||
}
|
||
|
||
void DenseMatrix::Eigensystem(DenseMatrix &b, Vector &ev,
|
||
DenseMatrix *evect)
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
|
||
dsygv_Eigensystem(*this, b, ev, evect);
|
||
|
||
#else
|
||
MFEM_CONTRACT_VAR(b);
|
||
MFEM_CONTRACT_VAR(ev);
|
||
MFEM_CONTRACT_VAR(evect);
|
||
mfem_error("DenseMatrix::Eigensystem(generalized): Compiled without LAPACK");
|
||
#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.GetData();
|
||
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
|
||
MFEM_CONTRACT_VAR(sv);
|
||
// compiling without lapack
|
||
mfem_error("DenseMatrix::SingularValues: Compiled without LAPACK");
|
||
#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;
|
||
}
|
||
|
||
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)
|
||
{
|
||
return kernels::CalcSingularvalue<2>(d,i);
|
||
}
|
||
else
|
||
{
|
||
return kernels::CalcSingularvalue<3>(d,i);
|
||
}
|
||
}
|
||
|
||
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)
|
||
{
|
||
kernels::CalcEigenvalues<2>(d, lambda, vec);
|
||
}
|
||
else
|
||
{
|
||
kernels::CalcEigenvalues<3>(d, lambda, vec);
|
||
}
|
||
}
|
||
|
||
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
|
||
kernels::Symmetrize(Height(), Data());
|
||
}
|
||
|
||
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(...): dimension mismatch");
|
||
}
|
||
#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::GradToVectorCurl2D(DenseMatrix &curl)
|
||
{
|
||
MFEM_VERIFY(Width() == 2,
|
||
"DenseMatrix::GradToVectorCurl2D(...): dimension must be 2")
|
||
|
||
int n = Height();
|
||
// rotate gradient
|
||
for (int i = 0; i < n; i++)
|
||
{
|
||
curl(i,0) = (*this)(i,1);
|
||
curl(i,1) = -(*this)(i,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 accommodate 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 accommodate 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 : dimension mismatch");
|
||
}
|
||
#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 : dimension mismatch");
|
||
}
|
||
#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::GetSubMatrix(const Array<int> & idx, DenseMatrix & A) const
|
||
{
|
||
int k = idx.Size();
|
||
int idx_max = idx.Max();
|
||
MFEM_VERIFY(idx.Min() >=0 && idx_max < this->height && idx_max < this->width,
|
||
"DenseMatrix::GetSubMatrix: Index out of bounds");
|
||
A.SetSize(k);
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = idx[i];
|
||
for (int j = 0; j<k; j++)
|
||
{
|
||
jj = idx[j];
|
||
adata[i+j*k] = this->data[ii+jj*height];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::GetSubMatrix(const Array<int> & idx_i,
|
||
const Array<int> & idx_j, DenseMatrix & A) const
|
||
{
|
||
int k = idx_i.Size();
|
||
int l = idx_j.Size();
|
||
|
||
MFEM_VERIFY(idx_i.Min() >=0 && idx_i.Max() < this->height,
|
||
"DenseMatrix::GetSubMatrix: Row index out of bounds");
|
||
MFEM_VERIFY(idx_j.Min() >=0 && idx_j.Max() < this->width,
|
||
"DenseMatrix::GetSubMatrix: Col index out of bounds");
|
||
|
||
A.SetSize(k,l);
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = idx_i[i];
|
||
for (int j = 0; j<l; j++)
|
||
{
|
||
jj = idx_j[j];
|
||
adata[i+j*k] = this->data[ii+jj*height];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::GetSubMatrix(int ibeg, int iend, DenseMatrix & A)
|
||
{
|
||
MFEM_VERIFY(iend >= ibeg, "DenseMatrix::GetSubMatrix: Inconsistent range");
|
||
MFEM_VERIFY(ibeg >=0,
|
||
"DenseMatrix::GetSubMatrix: Negative index");
|
||
MFEM_VERIFY(iend <= this->height && iend <= this->width,
|
||
"DenseMatrix::GetSubMatrix: Index bigger than upper bound");
|
||
|
||
int k = iend - ibeg;
|
||
A.SetSize(k);
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = ibeg + i;
|
||
for (int j = 0; j<k; j++)
|
||
{
|
||
jj = ibeg + j;
|
||
adata[i+j*k] = this->data[ii+jj*height];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::GetSubMatrix(int ibeg, int iend, int jbeg, int jend,
|
||
DenseMatrix & A)
|
||
{
|
||
MFEM_VERIFY(iend >= ibeg,
|
||
"DenseMatrix::GetSubMatrix: Inconsistent row range");
|
||
MFEM_VERIFY(jend >= jbeg,
|
||
"DenseMatrix::GetSubMatrix: Inconsistent col range");
|
||
MFEM_VERIFY(ibeg >=0,
|
||
"DenseMatrix::GetSubMatrix: Negative row index");
|
||
MFEM_VERIFY(jbeg >=0,
|
||
"DenseMatrix::GetSubMatrix: Negative row index");
|
||
MFEM_VERIFY(iend <= this->height,
|
||
"DenseMatrix::GetSubMatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(jend <= this->width,
|
||
"DenseMatrix::GetSubMatrix: Index bigger than col upper bound");
|
||
|
||
int k = iend - ibeg;
|
||
int l = jend - jbeg;
|
||
A.SetSize(k,l);
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = ibeg + i;
|
||
for (int j = 0; j<l; j++)
|
||
{
|
||
jj = jbeg + j;
|
||
adata[i+j*k] = this->data[ii+jj*height];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::SetSubMatrix(const Array<int> & idx, const DenseMatrix & A)
|
||
{
|
||
int k = idx.Size();
|
||
MFEM_VERIFY(A.Height() == k && A.Width() == k,
|
||
"DenseMatrix::SetSubMatrix:Inconsistent matrix dimensions");
|
||
|
||
int idx_max = idx.Max();
|
||
|
||
MFEM_VERIFY(idx.Min() >=0,
|
||
"DenseMatrix::SetSubMatrix: Negative index");
|
||
MFEM_VERIFY(idx_max < this->height,
|
||
"DenseMatrix::SetSubMatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(idx_max < this->width,
|
||
"DenseMatrix::SetSubMatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = idx[i];
|
||
for (int j = 0; j<k; j++)
|
||
{
|
||
jj = idx[j];
|
||
this->data[ii+jj*height] = adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::SetSubMatrix(const Array<int> & idx_i,
|
||
const Array<int> & idx_j, const DenseMatrix & A)
|
||
{
|
||
int k = idx_i.Size();
|
||
int l = idx_j.Size();
|
||
MFEM_VERIFY(k == A.Height() && l == A.Width(),
|
||
"DenseMatrix::SetSubMatrix:Inconsistent matrix dimensions");
|
||
MFEM_VERIFY(idx_i.Min() >=0,
|
||
"DenseMatrix::SetSubMatrix: Negative row index");
|
||
MFEM_VERIFY(idx_j.Min() >=0,
|
||
"DenseMatrix::SetSubMatrix: Negative col index");
|
||
MFEM_VERIFY(idx_i.Max() < this->height,
|
||
"DenseMatrix::SetSubMatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(idx_j.Max() < this->width,
|
||
"DenseMatrix::SetSubMatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = idx_i[i];
|
||
for (int j = 0; j<l; j++)
|
||
{
|
||
jj = idx_j[j];
|
||
this->data[ii+jj*height] = adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::SetSubMatrix(int ibeg, const DenseMatrix & A)
|
||
{
|
||
int k = A.Height();
|
||
|
||
MFEM_VERIFY(A.Width() == k, "DenseMatrix::SetSubmatrix: A is not square");
|
||
MFEM_VERIFY(ibeg >=0,
|
||
"DenseMatrix::SetSubmatrix: Negative index");
|
||
MFEM_VERIFY(ibeg + k <= this->height,
|
||
"DenseMatrix::SetSubmatrix: index bigger than row upper bound");
|
||
MFEM_VERIFY(ibeg + k <= this->width,
|
||
"DenseMatrix::SetSubmatrix: index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = ibeg + i;
|
||
for (int j = 0; j<k; j++)
|
||
{
|
||
jj = ibeg + j;
|
||
this->data[ii+jj*height] = adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::SetSubMatrix(int ibeg, int jbeg, const DenseMatrix & A)
|
||
{
|
||
int k = A.Height();
|
||
int l = A.Width();
|
||
|
||
MFEM_VERIFY(ibeg>=0,
|
||
"DenseMatrix::SetSubmatrix: Negative row index");
|
||
MFEM_VERIFY(jbeg>=0,
|
||
"DenseMatrix::SetSubmatrix: Negative col index");
|
||
MFEM_VERIFY(ibeg + k <= this->height,
|
||
"DenseMatrix::SetSubmatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(jbeg + l <= this->width,
|
||
"DenseMatrix::SetSubmatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = ibeg + i;
|
||
for (int j = 0; j<l; j++)
|
||
{
|
||
jj = jbeg + j;
|
||
this->data[ii+jj*height] = adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::AddSubMatrix(const Array<int> & idx, const DenseMatrix & A)
|
||
{
|
||
int k = idx.Size();
|
||
MFEM_VERIFY(A.Height() == k && A.Width() == k,
|
||
"DenseMatrix::AddSubMatrix:Inconsistent matrix dimensions");
|
||
|
||
int idx_max = idx.Max();
|
||
|
||
MFEM_VERIFY(idx.Min() >=0, "DenseMatrix::AddSubMatrix: Negative index");
|
||
MFEM_VERIFY(idx_max < this->height,
|
||
"DenseMatrix::AddSubMatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(idx_max < this->width,
|
||
"DenseMatrix::AddSubMatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = idx[i];
|
||
for (int j = 0; j<k; j++)
|
||
{
|
||
jj = idx[j];
|
||
this->data[ii+jj*height] += adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::AddSubMatrix(const Array<int> & idx_i,
|
||
const Array<int> & idx_j, const DenseMatrix & A)
|
||
{
|
||
int k = idx_i.Size();
|
||
int l = idx_j.Size();
|
||
MFEM_VERIFY(k == A.Height() && l == A.Width(),
|
||
"DenseMatrix::AddSubMatrix:Inconsistent matrix dimensions");
|
||
|
||
MFEM_VERIFY(idx_i.Min() >=0,
|
||
"DenseMatrix::AddSubMatrix: Negative row index");
|
||
MFEM_VERIFY(idx_j.Min() >=0,
|
||
"DenseMatrix::AddSubMatrix: Negative col index");
|
||
MFEM_VERIFY(idx_i.Max() < this->height,
|
||
"DenseMatrix::AddSubMatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(idx_j.Max() < this->width,
|
||
"DenseMatrix::AddSubMatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = idx_i[i];
|
||
for (int j = 0; j<l; j++)
|
||
{
|
||
jj = idx_j[j];
|
||
this->data[ii+jj*height] += adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::AddSubMatrix(int ibeg, const DenseMatrix & A)
|
||
{
|
||
int k = A.Height();
|
||
MFEM_VERIFY(A.Width() == k, "DenseMatrix::AddSubmatrix: A is not square");
|
||
|
||
MFEM_VERIFY(ibeg>=0,
|
||
"DenseMatrix::AddSubmatrix: Negative index");
|
||
MFEM_VERIFY(ibeg + k <= this->Height(),
|
||
"DenseMatrix::AddSubmatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(ibeg + k <= this->Width(),
|
||
"DenseMatrix::AddSubmatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = ibeg + i;
|
||
for (int j = 0; j<k; j++)
|
||
{
|
||
jj = ibeg + j;
|
||
this->data[ii+jj*height] += adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::AddSubMatrix(int ibeg, int jbeg, const DenseMatrix & A)
|
||
{
|
||
int k = A.Height();
|
||
int l = A.Width();
|
||
|
||
MFEM_VERIFY(ibeg>=0,
|
||
"DenseMatrix::AddSubmatrix: Negative row index");
|
||
MFEM_VERIFY(jbeg>=0,
|
||
"DenseMatrix::AddSubmatrix: Negative col index");
|
||
MFEM_VERIFY(ibeg + k <= this->height,
|
||
"DenseMatrix::AddSubmatrix: Index bigger than row upper bound");
|
||
MFEM_VERIFY(jbeg + l <= this->width,
|
||
"DenseMatrix::AddSubmatrix: Index bigger than col upper bound");
|
||
|
||
double * adata = A.Data();
|
||
|
||
int ii, jj;
|
||
for (int i = 0; i<k; i++)
|
||
{
|
||
ii = ibeg + i;
|
||
for (int j = 0; j<l; j++)
|
||
{
|
||
jj = jbeg + j;
|
||
this->data[ii+jj*height] += adata[i+j*k];
|
||
}
|
||
}
|
||
}
|
||
|
||
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(...): dimension mismatch");
|
||
}
|
||
#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 double* row)
|
||
{
|
||
MFEM_ASSERT(row != nullptr, "supplied row pointer is null");
|
||
for (int j = 0; j < Width(); j++)
|
||
{
|
||
(*this)(r, j) = row[j];
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::SetRow(int r, const Vector &row)
|
||
{
|
||
MFEM_ASSERT(Width() == row.Size(), "");
|
||
SetRow(r, row.GetData());
|
||
}
|
||
|
||
void DenseMatrix::SetCol(int c, const double* col)
|
||
{
|
||
MFEM_ASSERT(col != nullptr, "supplied column pointer is null");
|
||
for (int i = 0; i < Height(); i++)
|
||
{
|
||
(*this)(i, c) = col[i];
|
||
}
|
||
}
|
||
|
||
void DenseMatrix::SetCol(int c, const Vector &col)
|
||
{
|
||
MFEM_ASSERT(Height() == col.Size(), "");
|
||
SetCol(c, col.GetData());
|
||
}
|
||
|
||
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 &os, int width_) const
|
||
{
|
||
// save current output flags
|
||
ios::fmtflags old_flags = os.flags();
|
||
// output flags = scientific + show sign
|
||
os << setiosflags(ios::scientific | ios::showpos);
|
||
for (int i = 0; i < height; i++)
|
||
{
|
||
os << "[row " << i << "]\n";
|
||
for (int j = 0; j < width; j++)
|
||
{
|
||
os << (*this)(i,j);
|
||
if (j+1 == width || (j+1) % width_ == 0)
|
||
{
|
||
os << '\n';
|
||
}
|
||
else
|
||
{
|
||
os << ' ';
|
||
}
|
||
}
|
||
}
|
||
// reset output flags to original values
|
||
os.flags(old_flags);
|
||
}
|
||
|
||
void DenseMatrix::PrintMatlab(std::ostream &os) const
|
||
{
|
||
// save current output flags
|
||
ios::fmtflags old_flags = os.flags();
|
||
// output flags = scientific + show sign
|
||
os << setiosflags(ios::scientific | ios::showpos);
|
||
for (int i = 0; i < height; i++)
|
||
{
|
||
for (int j = 0; j < width; j++)
|
||
{
|
||
os << (*this)(i,j);
|
||
os << ' ';
|
||
}
|
||
os << "\n";
|
||
}
|
||
// reset output flags to original values
|
||
os.flags(old_flags);
|
||
}
|
||
|
||
void DenseMatrix::PrintT(std::ostream &os, int width_) const
|
||
{
|
||
// save current output flags
|
||
ios::fmtflags old_flags = os.flags();
|
||
// output flags = scientific + show sign
|
||
os << setiosflags(ios::scientific | ios::showpos);
|
||
for (int j = 0; j < width; j++)
|
||
{
|
||
os << "[col " << j << "]\n";
|
||
for (int i = 0; i < height; i++)
|
||
{
|
||
os << (*this)(i,j);
|
||
if (i+1 == height || (i+1) % width_ == 0)
|
||
{
|
||
os << '\n';
|
||
}
|
||
else
|
||
{
|
||
os << ' ';
|
||
}
|
||
}
|
||
}
|
||
// reset output flags to original values
|
||
os.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;
|
||
}
|
||
|
||
void DenseMatrix::Swap(DenseMatrix &other)
|
||
{
|
||
mfem::Swap(width, other.width);
|
||
mfem::Swap(height, other.height);
|
||
mfem::Swap(data, other.data);
|
||
}
|
||
|
||
DenseMatrix::~DenseMatrix()
|
||
{
|
||
data.Delete();
|
||
}
|
||
|
||
|
||
|
||
void Add(const DenseMatrix &A, const DenseMatrix &B,
|
||
double alpha, DenseMatrix &C)
|
||
{
|
||
kernels::Add(C.Height(), C.Width(), alpha, A.Data(), B.Data(), C.Data());
|
||
}
|
||
|
||
void Add(double alpha, const double *A,
|
||
double beta, const double *B, DenseMatrix &C)
|
||
{
|
||
kernels::Add(C.Height(), C.Width(), alpha, A, beta, B, C.Data());
|
||
}
|
||
|
||
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);
|
||
}
|
||
|
||
bool LinearSolve(DenseMatrix& A, double* X, double TOL)
|
||
{
|
||
MFEM_VERIFY(A.IsSquare(), "A must be a square matrix!");
|
||
MFEM_ASSERT(A.NumCols() > 0, "supplied matrix, A, is empty!");
|
||
MFEM_ASSERT(X != nullptr, "supplied vector, X, is null!");
|
||
|
||
int N = A.NumCols();
|
||
|
||
switch (N)
|
||
{
|
||
case 1:
|
||
{
|
||
double det = A(0,0);
|
||
if (std::abs(det) <= TOL) { return false; } // singular
|
||
|
||
X[0] /= det;
|
||
break;
|
||
}
|
||
case 2:
|
||
{
|
||
double det = A.Det();
|
||
if (std::abs(det) <= TOL) { return false; } // singular
|
||
|
||
double invdet = 1. / det;
|
||
|
||
double b0 = X[0];
|
||
double b1 = X[1];
|
||
|
||
X[0] = ( A(1,1)*b0 - A(0,1)*b1) * invdet;
|
||
X[1] = (-A(1,0)*b0 + A(0,0)*b1) * invdet;
|
||
break;
|
||
}
|
||
default:
|
||
{
|
||
// default to LU factorization for the general case
|
||
Array<int> ipiv(N);
|
||
LUFactors lu(A.Data(), ipiv);
|
||
|
||
if (!lu.Factor(N,TOL)) { return false; } // singular
|
||
|
||
lu.Solve(N, 1, X);
|
||
}
|
||
|
||
} // END switch
|
||
|
||
return true;
|
||
}
|
||
|
||
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();
|
||
kernels::Mult(ah,aw,bw,bd,cd,ad);
|
||
#endif
|
||
}
|
||
|
||
void AddMult_a(double alpha, 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 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] += alpha * 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(...): unsupported dimensions");
|
||
}
|
||
if (a.Width() != adja.Height() || a.Height() != adja.Width())
|
||
{
|
||
mfem_error("CalcAdjugate(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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!");
|
||
#endif
|
||
|
||
switch (a.Height())
|
||
{
|
||
case 1:
|
||
inva(0,0) = 1.0 / a.Det();
|
||
break;
|
||
case 2:
|
||
kernels::CalcInverse<2>(a.Data(), inva.Data());
|
||
break;
|
||
case 3:
|
||
kernels::CalcInverse<3>(a.Data(), inva.Data());
|
||
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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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();
|
||
|
||
kernels::MultABt(ah, aw, bh, ad, bd, cd);
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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(...): dimension mismatch");
|
||
}
|
||
#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 RAP(const DenseMatrix &A, const DenseMatrix &P, DenseMatrix & RAP)
|
||
{
|
||
DenseMatrix RA(P.Width(),A.Width());
|
||
MultAtB(P,A,RA);
|
||
RAP.SetSize(RA.Height(), P.Width());
|
||
Mult(RA,P, RAP);
|
||
}
|
||
|
||
void RAP(const DenseMatrix &Rt, const DenseMatrix &A,
|
||
const DenseMatrix &P, DenseMatrix & RAP)
|
||
{
|
||
DenseMatrix RA(Rt.Width(),A.Width());
|
||
MultAtB(Rt,A,RA);
|
||
RAP.SetSize(RA.Height(), P.Width());
|
||
Mult(RA,P, RAP);
|
||
}
|
||
|
||
bool LUFactors::Factor(int m, double TOL)
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
int info = 0;
|
||
if (m) { dgetrf_(&m, &m, data, &m, ipiv, &info); }
|
||
return info == 0;
|
||
#else
|
||
// compiling without LAPACK
|
||
double *data_ptr = this->data;
|
||
for (int i = 0; i < m; i++)
|
||
{
|
||
// pivoting
|
||
{
|
||
int piv = i;
|
||
double a = std::abs(data_ptr[piv+i*m]);
|
||
for (int j = i+1; j < m; j++)
|
||
{
|
||
const double b = std::abs(data_ptr[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++)
|
||
{
|
||
mfem::Swap<double>(data_ptr[i+j*m], data_ptr[piv+j*m]);
|
||
}
|
||
}
|
||
}
|
||
|
||
if (abs(data_ptr[i + i*m]) <= TOL)
|
||
{
|
||
return false; // failed
|
||
}
|
||
|
||
const double a_ii_inv = 1.0 / data_ptr[i+i*m];
|
||
for (int j = i+1; j < m; j++)
|
||
{
|
||
data_ptr[j+i*m] *= a_ii_inv;
|
||
}
|
||
for (int k = i+1; k < m; k++)
|
||
{
|
||
const double a_ik = data_ptr[i+k*m];
|
||
for (int j = i+1; j < m; j++)
|
||
{
|
||
data_ptr[j+k*m] -= a_ik * data_ptr[j+i*m];
|
||
}
|
||
}
|
||
}
|
||
#endif
|
||
|
||
return true; // success
|
||
}
|
||
|
||
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
|
||
{
|
||
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--)
|
||
{
|
||
mfem::Swap<double>(x[i], x[ipiv[i]-ipiv_base]);
|
||
}
|
||
x += m;
|
||
}
|
||
}
|
||
|
||
void LUFactors::LSolve(int m, int n, double *X) const
|
||
{
|
||
double *x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
// X <- P X
|
||
for (int i = 0; i < m; i++)
|
||
{
|
||
mfem::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
|
||
{
|
||
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::RightSolve(int m, int n, double *X) const
|
||
{
|
||
double *x;
|
||
#ifdef MFEM_USE_LAPACK
|
||
char n_ch = 'N', side = 'R', u_ch = 'U', l_ch = 'L';
|
||
double alpha = 1.0;
|
||
if (m > 0 && n > 0)
|
||
{
|
||
dtrsm_(&side,&u_ch,&n_ch,&n_ch,&n,&m,&alpha,data,&m,X,&n);
|
||
dtrsm_(&side,&l_ch,&n_ch,&u_ch,&n,&m,&alpha,data,&m,X,&n);
|
||
}
|
||
#else
|
||
// compiling without LAPACK
|
||
// X <- X U^{-1}
|
||
x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
for (int j = 0; j < m; j++)
|
||
{
|
||
const double x_j = ( x[j*n] /= data[j+j*m]);
|
||
for (int i = j+1; i < m; i++)
|
||
{
|
||
x[i*n] -= data[j + i*m] * x_j;
|
||
}
|
||
}
|
||
++x;
|
||
}
|
||
|
||
// X <- X L^{-1}
|
||
x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
for (int j = m-1; j >= 0; j--)
|
||
{
|
||
const double x_j = x[j*n];
|
||
for (int i = 0; i < j; i++)
|
||
{
|
||
x[i*n] -= data[j + i*m] * x_j;
|
||
}
|
||
}
|
||
++x;
|
||
}
|
||
#endif
|
||
// X <- X P
|
||
x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
for (int i = m-1; i >= 0; --i)
|
||
{
|
||
mfem::Swap<double>(x[i*n], x[(ipiv[i]-ipiv_base)*n]);
|
||
}
|
||
++x;
|
||
}
|
||
}
|
||
|
||
void LUFactors::GetInverseMatrix(int m, double *X) const
|
||
{
|
||
// A^{-1} = U^{-1} L^{-1} P
|
||
// 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
|
||
{
|
||
// 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);
|
||
}
|
||
|
||
|
||
bool CholeskyFactors::Factor(int m, double TOL)
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
int info = 0;
|
||
char uplo = 'L';
|
||
MFEM_VERIFY(data, "Matrix data not set");
|
||
if (m) {dpotrf_(&uplo, &m, data, &m, &info);}
|
||
return info == 0;
|
||
#else
|
||
// Cholesky–Crout algorithm
|
||
for (int j = 0; j<m; j++)
|
||
{
|
||
double a = 0.;
|
||
for (int k = 0; k<j; k++)
|
||
{
|
||
a+=data[j+k*m]*data[j+k*m];
|
||
}
|
||
|
||
MFEM_VERIFY(data[j+j*m] - a > 0.,
|
||
"CholeskyFactors::Factor: The matrix is not SPD");
|
||
|
||
data[j+j*m] = std::sqrt(data[j+j*m] - a);
|
||
|
||
if (data[j + j*m] <= TOL)
|
||
{
|
||
return false; // failed
|
||
}
|
||
|
||
for (int i = j+1; i<m; i++)
|
||
{
|
||
a = 0.;
|
||
for (int k = 0; k<j; k++)
|
||
{
|
||
a+= data[i+k*m]*data[j+k*m];
|
||
}
|
||
data[i+j*m] = 1./data[j+m*j]*(data[i+j*m] - a);
|
||
}
|
||
}
|
||
return true; // success
|
||
#endif
|
||
}
|
||
|
||
double CholeskyFactors::Det(int m) const
|
||
{
|
||
double det = 1.0;
|
||
for (int i=0; i<m; i++)
|
||
{
|
||
det *= data[i + i*m];
|
||
}
|
||
return det;
|
||
}
|
||
|
||
void CholeskyFactors::LMult(int m, int n, double * X) const
|
||
{
|
||
// X <- L X
|
||
double *x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
for (int j = m-1; j >= 0; j--)
|
||
{
|
||
double x_j = x[j] * data[j+j*m];
|
||
for (int i = 0; i < j; i++)
|
||
{
|
||
x_j += x[i] * data[j+i*m];
|
||
}
|
||
x[j] = x_j;
|
||
}
|
||
x += m;
|
||
}
|
||
}
|
||
|
||
void CholeskyFactors::UMult(int m, int n, double * X) const
|
||
{
|
||
double *x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
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[j+i*m];
|
||
}
|
||
x[i] = x_i;
|
||
}
|
||
x += m;
|
||
}
|
||
}
|
||
|
||
void CholeskyFactors::LSolve(int m, int n, double * X) const
|
||
{
|
||
|
||
#ifdef MFEM_USE_LAPACK
|
||
char uplo = 'L';
|
||
char trans = 'N';
|
||
char diag = 'N';
|
||
int info = 0;
|
||
|
||
dtrtrs_(&uplo, &trans, &diag, &m, &n, data, &m, X, &m, &info);
|
||
MFEM_VERIFY(!info, "CholeskyFactors:LSolve:: info");
|
||
|
||
#else
|
||
double *x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
// X <- L^{-1} X
|
||
for (int j = 0; j < m; j++)
|
||
{
|
||
const double x_j = (x[j] /= data[j+j*m]);
|
||
for (int i = j+1; i < m; i++)
|
||
{
|
||
x[i] -= data[i+j*m] * x_j;
|
||
}
|
||
}
|
||
x += m;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void CholeskyFactors::USolve(int m, int n, double * X) const
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
|
||
char uplo = 'L';
|
||
char trans = 'T';
|
||
char diag = 'N';
|
||
int info = 0;
|
||
|
||
dtrtrs_(&uplo, &trans, &diag, &m, &n, data, &m, X, &m, &info);
|
||
MFEM_VERIFY(!info, "CholeskyFactors:USolve:: info");
|
||
|
||
#else
|
||
// X <- L^{-t} X
|
||
double *x = 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[j+i*m] * x_j;
|
||
}
|
||
}
|
||
x += m;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void CholeskyFactors::Solve(int m, int n, double * X) const
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
char uplo = 'L';
|
||
int info = 0;
|
||
dpotrs_(&uplo, &m, &n, data, &m, X, &m, &info);
|
||
MFEM_VERIFY(!info, "CholeskyFactors:Solve:: info");
|
||
|
||
#else
|
||
LSolve(m, n, X);
|
||
USolve(m, n, X);
|
||
#endif
|
||
}
|
||
|
||
void CholeskyFactors::RightSolve(int m, int n, double * X) const
|
||
{
|
||
#ifdef MFEM_USE_LAPACK
|
||
char side = 'R';
|
||
char uplo = 'L';
|
||
char transt = 'T';
|
||
char trans = 'N';
|
||
char diag = 'N';
|
||
|
||
double alpha = 1.0;
|
||
if (m > 0 && n > 0)
|
||
{
|
||
dtrsm_(&side,&uplo,&transt,&diag,&n,&m,&alpha,data,&m,X,&n);
|
||
dtrsm_(&side,&uplo,&trans,&diag,&n,&m,&alpha,data,&m,X,&n);
|
||
}
|
||
#else
|
||
// X <- X L^{-t}
|
||
double *x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
for (int j = 0; j < m; j++)
|
||
{
|
||
const double x_j = ( x[j*n] /= data[j+j*m]);
|
||
for (int i = j+1; i < m; i++)
|
||
{
|
||
x[i*n] -= data[i + j*m] * x_j;
|
||
}
|
||
}
|
||
++x;
|
||
}
|
||
// X <- X L^{-1}
|
||
x = X;
|
||
for (int k = 0; k < n; k++)
|
||
{
|
||
for (int j = m-1; j >= 0; j--)
|
||
{
|
||
const double x_j = (x[j*n] /= data[j+j*m]);
|
||
for (int i = 0; i < j; i++)
|
||
{
|
||
x[i*n] -= data[j + i*m] * x_j;
|
||
}
|
||
}
|
||
++x;
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void CholeskyFactors::GetInverseMatrix(int m, double * X) const
|
||
{
|
||
// A^{-1} = L^{-t} L^{-1}
|
||
#ifdef MFEM_USE_LAPACK
|
||
// copy the lower triangular part of L to X
|
||
for (int i = 0; i<m; i++)
|
||
{
|
||
for (int j = i; j<m; j++)
|
||
{
|
||
X[j+i*m] = data[j+i*m];
|
||
}
|
||
}
|
||
char uplo = 'L';
|
||
int info = 0;
|
||
dpotri_(&uplo, &m, X, &m, &info);
|
||
MFEM_VERIFY(!info, "CholeskyFactors:GetInverseMatrix:: info");
|
||
// fill in the upper triangular part
|
||
for (int i = 0; i<m; i++)
|
||
{
|
||
for (int j = i+1; j<m; j++)
|
||
{
|
||
X[i+j*m] = X[j+i*m];
|
||
}
|
||
}
|
||
#else
|
||
// L^-t * L^-1 (in place)
|
||
for (int k = 0; k<m; k++)
|
||
{
|
||
X[k+k*m] = 1./data[k+k*m];
|
||
for (int i = k+1; i < m; i++)
|
||
{
|
||
double s=0.;
|
||
for (int j=k; j<i; j++)
|
||
{
|
||
s -= data[i+j*m] * X[j+k*m]/data[i+i*m];
|
||
}
|
||
X[i+k*m] = s;
|
||
}
|
||
}
|
||
for (int i = 0; i < m; i++)
|
||
{
|
||
for (int j = i; j < m; j++)
|
||
{
|
||
double s = 0.;
|
||
for (int k=j; k<m; k++)
|
||
{
|
||
s += X[k+i*m] * X[k+j*m];
|
||
}
|
||
X[i+j*m] = X[j+i*m] = s;
|
||
}
|
||
}
|
||
#endif
|
||
}
|
||
|
||
|
||
void DenseMatrixInverse::Init(int m)
|
||
{
|
||
if (spd)
|
||
{
|
||
factors = new CholeskyFactors();
|
||
}
|
||
else
|
||
{
|
||
factors = new LUFactors();
|
||
}
|
||
if (m>0)
|
||
{
|
||
factors->data = new double[m*m];
|
||
if (!spd)
|
||
{
|
||
dynamic_cast<LUFactors *>(factors)->ipiv = new int[m];
|
||
}
|
||
own_data = true;
|
||
}
|
||
}
|
||
|
||
DenseMatrixInverse::DenseMatrixInverse(const DenseMatrix &mat, bool spd_)
|
||
: MatrixInverse(mat), spd(spd_)
|
||
{
|
||
MFEM_ASSERT(height == width, "not a square matrix");
|
||
a = &mat;
|
||
Init(width);
|
||
Factor();
|
||
}
|
||
|
||
DenseMatrixInverse::DenseMatrixInverse(const DenseMatrix *mat, bool spd_)
|
||
: MatrixInverse(*mat), spd(spd_)
|
||
{
|
||
MFEM_ASSERT(height == width, "not a square matrix");
|
||
a = mat;
|
||
Init(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++)
|
||
{
|
||
factors->data[i] = adata[i];
|
||
}
|
||
factors->Factor(width);
|
||
}
|
||
|
||
void DenseMatrixInverse::GetInverseMatrix(DenseMatrix &Ainv) const
|
||
{
|
||
Ainv.SetSize(width);
|
||
factors->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;
|
||
if (own_data) { delete [] factors->data; }
|
||
factors->data = new double[width*width];
|
||
|
||
if (!spd)
|
||
{
|
||
LUFactors * lu = dynamic_cast<LUFactors *>(factors);
|
||
if (own_data) { delete [] lu->ipiv; }
|
||
lu->ipiv = new int[width];
|
||
}
|
||
own_data = true;
|
||
}
|
||
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 double *x, double *y) const
|
||
{
|
||
for (int row = 0; row < height; row++)
|
||
{
|
||
y[row] = x[row];
|
||
}
|
||
factors->Solve(width, 1, y);
|
||
}
|
||
|
||
void DenseMatrixInverse::Mult(const Vector &x, Vector &y) const
|
||
{
|
||
y = x;
|
||
factors->Solve(width, 1, y.GetData());
|
||
}
|
||
|
||
void DenseMatrixInverse::Mult(const DenseMatrix &B, DenseMatrix &X) const
|
||
{
|
||
X = B;
|
||
factors->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()
|
||
{
|
||
if (own_data)
|
||
{
|
||
delete [] factors->data;
|
||
if (!spd)
|
||
{
|
||
delete [] dynamic_cast<LUFactors *>(factors)->ipiv;
|
||
}
|
||
}
|
||
delete factors;
|
||
}
|
||
|
||
#ifdef MFEM_USE_LAPACK
|
||
|
||
DenseMatrixEigensystem::DenseMatrixEigensystem(DenseMatrix &m)
|
||
: mat(m)
|
||
{
|
||
n = mat.Width();
|
||
EVal.SetSize(n);
|
||
EVect.SetSize(n);
|
||
ev.SetDataAndSize(NULL, n);
|
||
|
||
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];
|
||
}
|
||
|
||
DenseMatrixEigensystem::DenseMatrixEigensystem(
|
||
const DenseMatrixEigensystem &other)
|
||
: mat(other.mat), EVal(other.EVal), EVect(other.EVect), ev(NULL, other.n),
|
||
n(other.n)
|
||
{
|
||
jobz = other.jobz;
|
||
uplo = other.uplo;
|
||
lwork = other.lwork;
|
||
|
||
work = new double[lwork];
|
||
}
|
||
|
||
void DenseMatrixEigensystem::Eval()
|
||
{
|
||
#ifdef MFEM_DEBUG
|
||
if (mat.Width() != n)
|
||
{
|
||
mfem_error("DenseMatrixEigensystem::Eval(): dimension mismatch");
|
||
}
|
||
#endif
|
||
|
||
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();
|
||
}
|
||
}
|
||
|
||
DenseMatrixEigensystem::~DenseMatrixEigensystem()
|
||
{
|
||
delete [] work;
|
||
}
|
||
|
||
|
||
DenseMatrixGeneralizedEigensystem::DenseMatrixGeneralizedEigensystem(
|
||
DenseMatrix &a, DenseMatrix &b,
|
||
bool left_eigen_vectors,
|
||
bool right_eigen_vectors)
|
||
: A(a), B(b)
|
||
{
|
||
MFEM_VERIFY(A.Height() == A.Width(), "A has to be a square matrix");
|
||
MFEM_VERIFY(B.Height() == B.Width(), "B has to be a square matrix");
|
||
n = A.Width();
|
||
MFEM_VERIFY(B.Height() == n, "A and B dimension mismatch");
|
||
|
||
jobvl = 'N';
|
||
jobvr = 'N';
|
||
A_copy.SetSize(n);
|
||
B_copy.SetSize(n);
|
||
if (left_eigen_vectors)
|
||
{
|
||
jobvl = 'V';
|
||
Vl.SetSize(n);
|
||
}
|
||
if (right_eigen_vectors)
|
||
{
|
||
jobvr = 'V';
|
||
Vr.SetSize(n);
|
||
}
|
||
|
||
lwork = -1;
|
||
double qwork;
|
||
|
||
alphar = new double[n];
|
||
alphai = new double[n];
|
||
beta = new double[n];
|
||
|
||
int nl = max(1,Vl.Height());
|
||
int nr = max(1,Vr.Height());
|
||
|
||
dggev_(&jobvl,&jobvr,&n,A_copy.Data(),&n,B_copy.Data(),&n,alphar,
|
||
alphai, beta, Vl.Data(), &nl, Vr.Data(), &nr,
|
||
&qwork, &lwork, &info);
|
||
|
||
lwork = (int) qwork;
|
||
work = new double[lwork];
|
||
}
|
||
|
||
void DenseMatrixGeneralizedEigensystem::Eval()
|
||
{
|
||
int nl = max(1,Vl.Height());
|
||
int nr = max(1,Vr.Height());
|
||
|
||
A_copy = A;
|
||
B_copy = B;
|
||
dggev_(&jobvl,&jobvr,&n,A_copy.Data(),&n,B_copy.Data(),&n,alphar,
|
||
alphai, beta, Vl.Data(), &nl, Vr.Data(), &nr,
|
||
work, &lwork, &info);
|
||
|
||
if (info != 0)
|
||
{
|
||
mfem::err << "DenseMatrixGeneralizedEigensystem::Eval(): DGGEV error code: "
|
||
<< info << endl;
|
||
mfem_error();
|
||
}
|
||
evalues_r.SetSize(n);
|
||
evalues_i.SetSize(n);
|
||
for (int i = 0; i<n; i++)
|
||
{
|
||
if (beta[i] != 0.)
|
||
{
|
||
evalues_r(i) = alphar[i]/beta[i];
|
||
evalues_i(i) = alphai[i]/beta[i];
|
||
}
|
||
else
|
||
{
|
||
evalues_r(i) = infinity();
|
||
evalues_i(i) = infinity();
|
||
}
|
||
}
|
||
}
|
||
|
||
DenseMatrixGeneralizedEigensystem::~DenseMatrixGeneralizedEigensystem()
|
||
{
|
||
delete [] alphar;
|
||
delete [] alphai;
|
||
delete [] beta;
|
||
delete [] work;
|
||
}
|
||
|
||
DenseMatrixSVD::DenseMatrixSVD(DenseMatrix &M,
|
||
bool left_singular_vectors,
|
||
bool right_singular_vectors)
|
||
{
|
||
m = M.Height();
|
||
n = M.Width();
|
||
jobu = (left_singular_vectors)? 'S' : 'N';
|
||
jobvt = (right_singular_vectors)? 'S' : 'N';
|
||
Init();
|
||
}
|
||
|
||
DenseMatrixSVD::DenseMatrixSVD(int h, int w,
|
||
bool left_singular_vectors,
|
||
bool right_singular_vectors)
|
||
{
|
||
m = h;
|
||
n = w;
|
||
jobu = (left_singular_vectors)? 'S' : 'N';
|
||
jobvt = (right_singular_vectors)? 'S' : 'N';
|
||
Init();
|
||
}
|
||
|
||
void DenseMatrixSVD::Init()
|
||
{
|
||
sv.SetSize(min(m, 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];
|
||
}
|
||
|
||
void DenseMatrixSVD::Eval(DenseMatrix &M)
|
||
{
|
||
#ifdef MFEM_DEBUG
|
||
if (M.Height() != m || M.Width() != n)
|
||
{
|
||
mfem_error("DenseMatrixSVD::Eval()");
|
||
}
|
||
#endif
|
||
double * datau = nullptr;
|
||
double * datavt = nullptr;
|
||
if (jobu == 'S')
|
||
{
|
||
U.SetSize(m,min(m,n));
|
||
datau = U.Data();
|
||
}
|
||
if (jobvt == 'S')
|
||
{
|
||
Vt.SetSize(min(m,n),n);
|
||
datavt = Vt.Data();
|
||
}
|
||
Mc = M;
|
||
dgesvd_(&jobu, &jobvt, &m, &n, Mc.Data(), &m, sv.GetData(), datau, &m,
|
||
datavt, &n, work, &lwork, &info);
|
||
|
||
if (info)
|
||
{
|
||
mfem::err << "DenseMatrixSVD::Eval() : info = " << info << endl;
|
||
mfem_error();
|
||
}
|
||
}
|
||
|
||
DenseMatrixSVD::~DenseMatrixSVD()
|
||
{
|
||
delete [] work;
|
||
}
|
||
|
||
#endif // if MFEM_USE_LAPACK
|
||
|
||
|
||
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 = mfem::HostRead(tdata, n*SizeJ()*ne);
|
||
double *yp = y.HostReadWrite();
|
||
double x_col;
|
||
const double *xp = x.HostRead();
|
||
// 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;
|
||
}
|
||
|
||
DenseTensor &DenseTensor::operator=(const DenseTensor &other)
|
||
{
|
||
DenseTensor new_tensor(other);
|
||
Swap(new_tensor);
|
||
return *this;
|
||
}
|
||
|
||
void BatchLUFactor(DenseTensor &Mlu, Array<int> &P, const double TOL)
|
||
{
|
||
const int m = Mlu.SizeI();
|
||
const int NE = Mlu.SizeK();
|
||
P.SetSize(m*NE);
|
||
|
||
auto data_all = mfem::Reshape(Mlu.ReadWrite(), m, m, NE);
|
||
auto ipiv_all = mfem::Reshape(P.Write(), m, NE);
|
||
Array<bool> pivot_flag(1);
|
||
pivot_flag[0] = true;
|
||
bool *d_pivot_flag = pivot_flag.ReadWrite();
|
||
|
||
MFEM_FORALL(e, NE,
|
||
{
|
||
for (int i = 0; i < m; i++)
|
||
{
|
||
// pivoting
|
||
{
|
||
int piv = i;
|
||
double a = fabs(data_all(piv,i,e));
|
||
for (int j = i+1; j < m; j++)
|
||
{
|
||
const double b = fabs(data_all(j,i,e));
|
||
if (b > a)
|
||
{
|
||
a = b;
|
||
piv = j;
|
||
}
|
||
}
|
||
ipiv_all(i,e) = piv;
|
||
if (piv != i)
|
||
{
|
||
// swap rows i and piv in both L and U parts
|
||
for (int j = 0; j < m; j++)
|
||
{
|
||
mfem::kernels::internal::Swap<double>(data_all(i,j,e), data_all(piv,j,e));
|
||
}
|
||
}
|
||
} // pivot end
|
||
|
||
if (abs(data_all(i,i,e)) <= TOL)
|
||
{
|
||
d_pivot_flag[0] = false;
|
||
}
|
||
|
||
const double a_ii_inv = 1.0 / data_all(i,i,e);
|
||
for (int j = i+1; j < m; j++)
|
||
{
|
||
data_all(j,i,e) *= a_ii_inv;
|
||
}
|
||
|
||
for (int k = i+1; k < m; k++)
|
||
{
|
||
const double a_ik = data_all(i,k,e);
|
||
for (int j = i+1; j < m; j++)
|
||
{
|
||
data_all(j,k,e) -= a_ik * data_all(j,i,e);
|
||
}
|
||
}
|
||
|
||
} // m loop
|
||
|
||
});
|
||
|
||
MFEM_ASSERT(pivot_flag.HostRead()[0], "Batch LU factorization failed \n");
|
||
}
|
||
|
||
void BatchLUSolve(const DenseTensor &Mlu, const Array<int> &P, Vector &X)
|
||
{
|
||
|
||
const int m = Mlu.SizeI();
|
||
const int NE = Mlu.SizeK();
|
||
|
||
auto data_all = mfem::Reshape(Mlu.Read(), m, m, NE);
|
||
auto piv_all = mfem::Reshape(P.Read(), m, NE);
|
||
auto x_all = mfem::Reshape(X.ReadWrite(), m, NE);
|
||
|
||
MFEM_FORALL(e, NE,
|
||
{
|
||
kernels::LUSolve(&data_all(0, 0,e), m, &piv_all(0, e), &x_all(0,e));
|
||
});
|
||
|
||
}
|
||
|
||
} // namespace mfem
|