void LUforw ( ptrdiff_t first, ptrdiff_t last, ptrdiff_t n, ptrdiff_t nu, ptrdiff_t maxmod,
double eps, double *L, double *U, double *y )
{
/* ------------------------------------------------------------------
LUforw updates the factors LC = U by performing a forward sweep
to eliminate subdiagonals in a row spike in row 'last' of U.
L is n by n.
U is n by nu.
y[*] contains the row spike. The first nonzero to be eliminated
is in y[first] or later.
The 'spike' row of L begins at L[ls].
18 Mar 1990: First version with L and U stored row-wise.
------------------------------------------------------------------ */
ptrdiff_t lu, ll, ls, incu, ly, numu;
double cs, sn;
lu = (first - 1)*maxmod + (3 - first)*first/2;
ll = (first - 1)*maxmod + 1;
ls = (last - 1)*maxmod + 1;
incu = maxmod + 1 - first;
for (ly = first; ly<last; ly++) {
/* See if this element of y is worth eliminating.
We compare y[ly] with the corresponding diagonal of U. */
if ( fabs(y[ly]) > eps * fabs(U[lu]) ) {
/* Generate a 2x2 elimination and apply it to U and L. */
numu = nu - ly;
elmgen( &U[lu], &y[ly] , eps , &cs, &sn );
if (numu > 0)
elm ( 1, numu , &U[lu], &y[ly], cs, sn );
elm ( 1, n, &L[subvec(ll)], &L[subvec(ls)], cs, sn );
}
ll += maxmod;
lu += incu;
incu--;
}
/* Copy the remaining part of y into U. */
numu = nu - last + 1;
if (numu > 0)
LUdcopy ( numu, &y[subvec(last)], 1, &U[subvec(lu)], 1 );
/* End of LUforw */
}
int CGSolver(SparseMatrix A, // CSR format matrix
const std::vector<double> &b,
std::vector<double> &u,
double tol,
std::map<int, std::vector<double>> &soln_iter) {
unsigned int n_iter = 0;
long unsigned int max_iter = A.GetRows();
soln_iter[n_iter] = u; // store initial state
auto A_u = A.MulVec(u); // A*u
std::vector<double> r = subvec(b, A_u); // b - A*u
auto L2normr0 = L2norm(r);
std::vector<double> p = r; // p =r
while (n_iter < max_iter) {
n_iter++;
auto A_p = A.MulVec(p); // A * p
double alpha = vecvec(r, r) / vecvec(p, A_p); // (r * r)/(p * A * p)
auto alpha_p = scalvec(alpha, p); // alpha * p
u = addvec(u, alpha_p); // u = u + alpha * p
/**
* alpha * A * p
* r_n+1 = r_n - alpha * A * p
*/
std::vector<double> alpha_A_p = scalvec(alpha, A_p);
std::vector<double> r_next = subvec(r, alpha_A_p);
auto L2normr = L2norm(r_next); // L2normr = L2norm(r_n+1)
if (L2normr / L2normr0 < tol) {
soln_iter[n_iter] = u;
std::cout << "SUCCESS: CG solver converged in "
<< n_iter << " iterations" << std::endl;
break;
} else {
double beta = vecvec(r_next, r_next) / vecvec(r, r);
auto beta_p = scalvec(beta, p);
p = addvec(r_next, beta_p);
r = r_next;
if ((n_iter % 10 == 0) || (n_iter == 0)) {
soln_iter[n_iter] = u;
}
if (n_iter == max_iter) {
std::cout << "FAILURE: CG solver failed to converge" << std::endl;
return 1;
}
}
}
return 0;
}
void Usolve ( int mode, ptrdiff_t maxmod, ptrdiff_t n,
double *U, double *y )
{
double sum, hold;
ptrdiff_t lu, incu, nu, i;
/* ------------------------------------------------------------------
If mode = 1, Usolve solves U * y[new] = y[old].
If mode = 2, Usolve solves U[transpose] * y[new] = y[old].
U is upper triangular, stored by rows.
y is overwritten by the solution.
------------------------------------------------------------------ */
if (mode == 1) {
lu = (n - 1)*maxmod + (3 - n)*n/2;
hold = U[lu];
y[n] /= hold;
incu = maxmod + 1 - n;
nu = 0;
for (i = n-1; i>=1; i--) {
nu++;
incu++;
lu -= incu;
sum = y[i] - LUddot ( nu, &U[subvec(lu+1)], 1, &y[subvec(i+1)], 1 );
hold = U[lu];
y[i] = sum / hold;
}
}
else {
lu = 1;
incu = maxmod;
nu = n - 1;
for (i = 1; i<n; i++) {
hold = U[lu];
y[i] /= hold;
LUdaxpy ( nu, -y[i], &U[subvec(lu+1)], 1, &y[subvec(i+1)], 1 );
lu += incu;
incu--;
nu--;
}
hold = U[lu];
y[n] /= hold;
}
/* End of Usolve */
}
arma_inline
const subview_col<eT>
Col<eT>::operator()(const span& row_span) const
{
arma_extra_debug_sigprint();
return subvec(row_span);
}
arma_inline
subview_col<eT>
Col<eT>::rows(const span& row_span)
{
arma_extra_debug_sigprint();
return subvec(row_span);
}
arma_inline
const subview_row<eT>
Row<eT>::operator()(const span& col_span) const
{
arma_extra_debug_sigprint();
return subvec(col_span);
}
arma_inline
subview_row<eT>
Row<eT>::cols(const span& col_span)
{
arma_extra_debug_sigprint();
return subvec(col_span);
}
void Lprod ( int mode, ptrdiff_t maxmod, ptrdiff_t n,
double *L, double *y, double *z )
{
ptrdiff_t ll, i;
/* ------------------------------------------------------------------
If mode = 1, Lprod computes z = L*y.
If mode = 2, Lprod computes z = L[transpose]*y.
L is stored by rows in L[*]. It is equivalent to storing
L[transpose] by columns in a 2-D array L[maxmod,n].
y is not altered.
------------------------------------------------------------------ */
ll = 1;
if (mode == 1) {
for (i = 1; i<=n; i++) {
z[i] = LUddot ( n, &L[subvec(ll)], 1, y, 1 );
ll += maxmod;
}
}
else {
/* call dzero ( n, z, 1 ) */
for (i = 1; i<=n; i++)
z[i] = 0;
for (i = 1; i<=n; i++) {
LUdaxpy ( n, y[i], &L[subvec(ll)], 1, z, 1 );
ll += maxmod;
}
}
/* End of Lprod */
}
Vector<T> Vector<T>::get(int i, int l)
{
if ( (i + l) > rows() )
{
#ifdef USE_EXCEPTION
throw MatrixErr() ;
#else
Error error("Vector<T>::get(int,Vector<T>)");
error << "Vector is too long to extract from i= " << i << "from l=" << l << endl ;
error.fatal() ;
#endif
}
Vector<T> subvec(l) ;
T *aptr, *bptr ;
aptr = &this->x[i] - 1 ;
bptr = subvec.x -1 ;
for ( int j = l; j > 0; --j)
*(++bptr) = *(++aptr) ;
return subvec ;
}
int submat( int nrowb, int row, int col)
{
return( nrowb*(col-1) + subvec(row) );
}
void LUback ( ptrdiff_t first, ptrdiff_t *last, ptrdiff_t n, ptrdiff_t nu,
ptrdiff_t maxmod, double eps,
double *L, double *U, double *y, double *z )
{
/* ------------------------------------------------------------------
LUback updates the factors LC = U by performing a backward sweep
to eliminate all but the 'last' nonzero in the column vector z[*],
stopping at z[first].
If 'last' is positive, LUback searches backwards for a nonzero
element in z and possibly alters 'last' accordingly.
Otherwise, 'last' will be reset to abs(last) and so used.
L is n by n.
U is n by nu.
y[*] will eventually contain a row spike in row 'last' of U.
The 'spike' row of L begins at L[ls].
18 Mar 1990: First version with L and U stored row-wise.
29 Jun 1999: Save w[last] = zlast at end so column replace
can do correct forward sweep.
------------------------------------------------------------------ */
ptrdiff_t i, lz, lu, ll,ls, incu, numu;
double zero = 0.0;
double zlast, cs, sn;
if ((*last) > 0) {
/* Find the last significant element in z[*]. */
for (i = (*last); i>first; i--)
if (fabs(z[i]) > eps) break;
(*last) = i;
}
else
(*last) = abs((*last));
/* Load the 'last' row of U into the end of y
and do the backward sweep. */
zlast = z[(*last)];
lu = ((*last) - 1)*maxmod + (3 - (*last))*(*last)/2;
ll = ((*last) - 1)*maxmod + 1;
ls = ll;
incu = maxmod + 1 - (*last);
numu = nu + 1 - (*last);
if (numu > 0)
LUdcopy ( numu, &U[subvec(lu)], 1, &y[subvec((*last))], 1 );
for (lz = (*last) - 1; lz>=first; lz--) {
ll -= maxmod;
incu++;
lu -= incu;
y[lz] = zero;
/* See if this element of z is worth eliminating.
We compare z[lz] with the current last nonzero, zlast. */
if ( fabs(z[lz]) <= eps * fabs(zlast) ) continue;
/* Generate a 2x2 elimination and apply it to U and L. */
numu = nu + 1 - lz;
elmgen( &zlast, &z[lz], eps , &cs, &sn );
elm ( 1, numu , &y[subvec(lz)], &U[subvec(lu)], cs, sn );
elm ( 1, n , &L[subvec(ls)], &L[subvec(ll)], cs, sn );
}
z[(*last)] = zlast;
/* End of LUback */
}
void LUmod ( int mode, ptrdiff_t maxmod, ptrdiff_t n, ptrdiff_t krow, ptrdiff_t kcol,
double *L, double *U, double *y, double *z, double *w)
{
ptrdiff_t first, last, n1, i, j, lastu, lastl, ls, ll, lu, incu;
double zero = 0.0;
double one = 1.0;
double eps = MACHINEPREC; /* The machine precision -- A value slightly too large is OK. */
n1 = n - 1;
if (mode == 1) {
/* ---------------------------------------------------------------
mode = 1. Add a row y and a column z.
The LU factors will expand in dimension from n-1 to n.
The new diagonal element of C is in y[n].
--------------------------------------------------------------- */
lastu = n1*maxmod + (3 - n)*n/2;
lastl = n1*maxmod + n;
ls = n1*maxmod + 1;
L[lastl] = one;
if (n == 1) {
U[lastu] = y[n];
return;
}
/* Compute L*z and temporarily store it in w;
(changed to w from last row of L by KE). */
Lprod ( 1, maxmod, n1, L, z, w );
/* Copy L*z into the new last column of U.
Border L with zeros. */
ll = ls;
lu = n;
incu = maxmod - 1;
for (j = 1; j<=n1; j++) {
U[lu] = w[j];
L[ll] = zero;
ll++;
lu += incu;
incu--;
}
ll = n;
for (i = 1; i<=n1; i++) {
L[ll] = zero;
ll += maxmod;
}
/* Add row y to the factorization
using a forward sweep of eliminations. */
last = n;
LUforw ( 1, last, n, n, maxmod, eps, L, U, y );
}
else if (mode == 2) {
/* ---------------------------------------------------------------
mode=2. Replace the kcol-th column of C by the vector z.
---------------------------------------------------------------*/
/* Compute w = L*z. */
Lprod ( 1, maxmod, n, L, z, w );
/* Copy the top of w into column kcol of U. */
lu = kcol;
incu = maxmod - 1;
for (i = 1; i<=kcol; i++) {
U[lu] = w[i];
lu += incu;
incu--;
}
if (kcol < n) {
/* Find w[last], the last nonzero in the bottom part of w.
Eliminate elements last-1, last-2, ... kcol+1 of w[*]
using a partial backward sweep of eliminations. */
first = kcol + 1;
last = n;
LUback( first, &last, n, n, maxmod, eps, L, U, y, w );
y[kcol] = w[last];
/* Eliminate elements kcol, kcol+1, ... last-1 of y[*]
using a partial forward sweep of eliminations. */
LUforw ( kcol, last, n, n, maxmod, eps, L, U, y );
}
}
else if (mode == 3) {
/* ---------------------------------------------------------------
mode=3. Replace the krow-th row of C by the vector y.
--------------------------------------------------------------- */
if (n == 1) {
L[1] = one;
U[1] = y[1];
return;
}
/* Copy the krow-th column of L into w, and zero the column. */
ll = krow;
for (i = 1; i<=n; i++) {
w[i] = L[ll];
L[ll] = zero;
ll += maxmod;
}
/* Reduce the krow-th column of L to the unit vector e(last).
where 'last' is determined by LUback.
This is done by eliminating elements last-1, last-2, ..., 1
using a backward sweep of eliminations.
On exit, row 'last' of U is a spike stored in z, whose first
nonzero entry is in z[first]. However, z will be discarded. */
first = 1;
last = n;
LUback ( first, &last, n, n, maxmod, eps, L, U, z, w );
/* Replace the 'last' row of L by the krow-th unit vector. */
ll = (last - 1)*maxmod;
for (j = 1; j<=n; j++)
L[ll + j] = zero;
L[ll + krow] = one;
/* Eliminate the elements of the new row y,
using a forward sweep of eliminations. */
LUforw ( 1, last, n, n, maxmod, eps, L, U, y );
}
else if (mode == 4) {
/* ---------------------------------------------------------------
mode=4. Delete the krow-th row and the kcol-th column of C.
Replace them by the last row and column respectively.
--------------------------------------------------------------- */
/* First, move the last column into position kcol. */
if (kcol < n) {
/* Set w = last column of U. */
lu = n;
incu = maxmod - 1;
for (i = 1; i<=n; i++) {
w[i] = U[lu];
lu += incu;
incu--;
}
/* Copy the top of w into column kcol of U. */
lu = kcol;
incu = maxmod - 1;
for (i = 1; i<=kcol; i++) {
U[lu] = w[i];
lu += incu;
incu--;
}
/* U now has only n-1 columns.
Find w[last], the last nonzero in the bottom part of w.
Eliminate elements last-1, last-2, ... kcol+1 of w[*]
using a partial backward sweep of eliminations. */
first = kcol + 1;
last = n;
LUback ( first, &last, n, n1, maxmod, eps, L, U, y, w );
y[kcol] = w[last];
/* Eliminate elements kcol, kcol+1, ... last-1 of y[*]
using a partial forward sweep of eliminations. */
LUforw ( kcol, last, n, n1, maxmod, eps, L, U, y );
}
/* Now, move the last row into position krow. */
/* Swap columns krow and n of L, using w = krow-th column of L. */
LUdcopy ( n, &L[subvec(krow)], maxmod, w, 1 );
if (krow < n)
LUdcopy ( n, &L[subvec(n)], maxmod, &L[subvec(krow)], maxmod );
/* Reduce the last column of L (in w) to the unit vector e(n).
This is done by eliminating elements n-1, n-2, ..., 1
using a backward sweep of eliminations. */
last = - n;
LUback ( 1, &last, n, n1, maxmod, eps, L, U, z, w );
/* printvec(n, w, 0); */
/* printmatUT(maxmod, n, U, 0); */
/* printmatSQ(maxmod, n, L, 0); */
}
/* End of LUmod */
}
void NeumannVolume<D> ::
T_CalcElementVector (const FiniteElement & base_fel,
const ElementTransformation & eltrans,
FlatVector<SCAL> elvec,
LocalHeap & lh) const {
const CompoundFiniteElement & cfel
= dynamic_cast<const CompoundFiniteElement&> (base_fel);
const ScalarFiniteElement<D> & fel =
dynamic_cast<const ScalarFiniteElement<D>&> (cfel[indx]);
FlatVector<> ushape(fel.GetNDof(), lh);
elvec = SCAL(0);
IntRange re = cfel.GetRange(indx);
int ndofe = re.Size();
FlatVector<SCAL> subvec(ndofe,lh);
subvec = SCAL(0);
const IntegrationRule ir(fel.ElementType(), 2*fel.Order());
ELEMENT_TYPE eltype = base_fel.ElementType();
int nfacet = ElementTopology::GetNFacets(eltype);
Facet2ElementTrafo transform(eltype);
FlatVector< Vec<D> > normals = ElementTopology::GetNormals<D>(eltype);
const MeshAccess & ma = *(const MeshAccess*)eltrans.GetMesh();
Array<int> fnums, sels;
ma.GetElFacets (eltrans.GetElementNr(), fnums);
for (int k = 0; k < nfacet; k++) {
ma.GetFacetSurfaceElements (fnums[k], sels);
// if interior element, then do nothing:
if (sels.Size() == 0) continue;
// else:
Vec<D> normal_ref = normals[k];
ELEMENT_TYPE etfacet = ElementTopology::GetFacetType (eltype, k);
IntegrationRule ir_facet(etfacet, 2*fel.Order());
// map the facet integration points to volume reference elt ipts
IntegrationRule & ir_facet_vol = transform(k, ir_facet, lh);
// ... and further to the physical element
MappedIntegrationRule<D,D> mir(ir_facet_vol, eltrans, lh);
for (int i = 0 ; i < ir_facet_vol.GetNIP(); i++) {
SCAL G[3] ;
G[0] = coeff_Gx -> T_Evaluate<SCAL>(mir[i]);
G[1] = coeff_Gy -> T_Evaluate<SCAL>(mir[i]);
if (D==3) G[2] = coeff_Gz -> T_Evaluate<SCAL>(mir[i]);
FlatVector<SCAL> Gval(D,lh);
for (int dd=0; dd<D; dd++) Gval[dd] = G[dd];
SCAL g = coeff_g -> T_Evaluate<SCAL>(mir[i]);
// this is contrived to get the surface measure in "len"
Mat<D> inv_jac = mir[i].GetJacobianInverse();
double det = mir[i].GetMeasure();
Vec<D> normal = det * Trans (inv_jac) * normal_ref;
double len = L2Norm (normal);
SCAL gg = (InnerProduct(Gval,normal) + g*len)
* ir_facet[i].Weight();
fel.CalcShape (ir_facet_vol[i], ushape);
subvec += gg * ushape;
}
}
elvec.Rows(re) += subvec;
}
