/** Check the filling of EigenMatrix per rows of blocks. Return true if the test succeeds.*/
bool checkEigenMatrixBlockRowFilling()
{
EigenBlockSparseMatrix mb;
FullMatrix ma;
unsigned br=3, bc=3;
ma.resize(br*BROWS,bc*BCOLS);
mb.resizeBlocks(br,bc);
for( unsigned i=0; i<br; i++ )
{
mb.beginBlockRow(i);
if( i%2==0 ) // leave some rows empty
{
for( int j=bc-1; j>=0; j--) // set the blocs in reverse order, for fun.
{
// create a block and give it some value
BlockMN b;
for( unsigned k=0; k<BROWS && k<BCOLS; k++ ){
b[k][k] = i+j;
ma.set(i*BROWS+k, j*BCOLS+k, i+j);
}
// insert the block in the matrix
mb.createBlock(j,b);
}
}
mb.endBlockRow();
}
mb.compress();
// serr()<<"MatrixTest<Real,RN,CN>::checkEigenMatrixBlockRowFilling, ma = " << ma << endl;
// serr()<<"MatrixTest<Real,RN,CN>::checkEigenMatrixBlockRowFilling, mb = " << mb << endl;
return Sofa_test<_Real>::matricesAreEqual(ma,mb);
}
/// Create the context for the matrix tests.
TestSparseMatrices()
{
//std::cout<<"Matrix_test "<<NumRows<<" "<<NumCols<<" "<<BlockRows<<" "<<BlockCols<<std::endl;
// resize and fill the matrices
generateRandomMat( mat, true );
copyFromMat( crs1, mat );
copyFromMat( crs2, mat );
copyFromMat( fullMat, mat );
copyFromMat( mapMat, mat );
copyFromMat( eiBlock1, mat );
copyFromMat( eiBlock2, mat );
// copyFromMat( eiBlock3, mat );
copyFromMat( eiBase, mat );
eiBlock3.copyFrom(crs1);
// resize and fill the vectors
fullVec_ncols.resize(NCOLS);
fullVec_nrows_reference.resize(NROWS);
fullVec_nrows_result.resize(NROWS);
eiVecM.resize(NROWS);
eiVecN.resize(NCOLS);
for( unsigned i=0; i<NCOLS; i++)
{
fullVec_ncols[i] = i;
vecN[i] = i;
eiVecN[i] = i;
}
fullMat.mul(fullVec_nrows_reference,fullVec_ncols); // cerr<<"MatrixTest: vref = " << vref << endl;
vecM = mat * vecN;
// matrix multiplication
generateRandomMat( matMultiplier, true );
copyFromMat( crsMultiplier, matMultiplier );
copyFromMat( fullMultiplier, matMultiplier );
copyFromMat( eiBaseMultiplier, matMultiplier );
eiDenseMultiplier = Eigen::Map< EigenDenseMatrix >( &(matMultiplier.transposed())[0][0], NCOLS, NROWS ); // need to transpose because EigenDenseMatrix is ColMajor
matMultiplication = mat * matMultiplier;
crs1.mul( crsMultiplication, crsMultiplier );
fullMat.mul( fullMultiplication, fullMultiplier );
eiBase.mul_MT( eiBaseMultiplication, eiBaseMultiplier ); // sparse x sparse
eiBase.mul_MT( eiDenseMultiplication, eiDenseMultiplier ); // sparse x dense
matTransposeMultiplication = mat.multTranspose( mat );
crs1.mulTranspose( crsTransposeMultiplication, crs1 );
fullMat.mulT( fullTransposeMultiplication, fullMat );
}
void getInverseMatrix(const CellIter& c,
FullMatrix<Scalar,SP,FortranOrdering>& Binv) const
{
// Binv = (N*lambda*K*N' + t*diag(A)*(I - Q*Q')*diag(A))/vol
// ^ ^^^^^^^^^^^^^^^^^^^^^^^^^^
// precomputed: n_ precomputed: second_term_
// t = 6/dim * trace(lambda*K)
int ci = c->index();
int nf = Binv.numRows();
ImmutableFortranMatrix n(nf, dim, &n_[ci][0]);
ImmutableFortranMatrix t2(nf, nf, &second_term_[ci][0]);
Binv = t2;
ImmutableFortranMatrix lambdaK(dim, dim, lambdaK_.data());
SharedFortranMatrix T2(nf, dim, &t2_[0]);
// T2 <- N*lambda*K
matMulAdd_NN(Scalar(1.0), n, lambdaK, Scalar(0.0), T2);
// Binv <- (T2*N' + t*Binv) / vol(c)
// == (N*lambda*K*N' + t*(diag(A) * (I - Q*Q') * diag(A))) / vol(c)
//
// where t = 6/d * TRACE(lambda*K) (== 2*TRACE(lambda*K) for 3D).
//
Scalar t = Scalar(6.0) * trace(lambdaK) / dim;
matMulAdd_NT(Scalar(1.0) / c->volume(), T2, n,
t / c->volume(), Binv );
}
void MatMult(const FullMatrix& A,
const dTensor1& invec,
dTensor1& outvec)
{
for (int i=1; i<=A.get_NumRows(); i++)
{
int jmax = A.get_NZrow(i);
double tmp = 0.0;
for (int j=1; j<=jmax; j++)
{
int ind = A.get_Index(i,j);
double val = A.get_Value(i,j);
tmp = tmp + val*invec.get(ind);
}
outvec.set(i, tmp );
}
}
void ConstructA_CG2(const mesh& Mesh, FullMatrix& A)
{
const int NumPhysElems = Mesh.get_NumPhysElems();
const int NumBndNodes = Mesh.get_SubNumBndNodes();
const int Asize = Mesh.get_SubNumPhysNodes();
assert_eq(Asize,A.get_NumRows());
assert_eq(Asize,A.get_NumCols());
dTensor1 A1(6);
dTensor1 A2(6);
dTensor1 A3(6);
dTensor1 A4(6);
dTensor1 A5(6);
dTensor1 A6(6);
A1.set(1, -oneninth );
A1.set(2, 4.0*oneninth );
A1.set(3, -oneninth );
A1.set(4, 4.0*oneninth );
A1.set(5, 4.0*oneninth );
A1.set(6, -oneninth );
A2.set(1, -onethird );
A2.set(2, 0.0 );
A2.set(3, onethird );
A2.set(4, -4.0*onethird );
A2.set(5, 4.0*onethird );
A2.set(6, 0.0 );
A3.set(1, -onethird );
A3.set(2, -4.0*onethird );
A3.set(3, 0.0 );
A3.set(4, 0.0 );
A3.set(5, 4.0*onethird );
A3.set(6, onethird );
A4.set(1, 4.0 );
A4.set(2, -4.0 );
A4.set(3, 0.0 );
A4.set(4, -4.0 );
A4.set(5, 4.0 );
A4.set(6, 0.0 );
A5.set(1, 2.0 );
A5.set(2, -4.0 );
A5.set(3, 2.0 );
A5.set(4, 0.0 );
A5.set(5, 0.0 );
A5.set(6, 0.0 );
A6.set(1, 2.0 );
A6.set(2, 0.0 );
A6.set(3, 0.0 );
A6.set(4, -4.0 );
A6.set(5, 0.0 );
A6.set(6, 2.0 );
dTensor2 spts(3,2);
spts.set(1,1, 1.0/3.0 );
spts.set(1,2, -1.0/6.0 );
spts.set(2,1, -1.0/6.0 );
spts.set(2,2, -1.0/6.0 );
spts.set(3,1, -1.0/6.0 );
spts.set(3,2, 1.0/3.0 );
dTensor1 wgts(3);
wgts.set(1, 1.0/6.0 );
wgts.set(2, 1.0/6.0 );
wgts.set(3, 1.0/6.0 );
// Loop over all elements in the mesh
for (int i=1; i<=NumPhysElems; i++)
{
// Information for element i
iTensor1 tt(6);
for (int k=1; k<=6; k++)
{ tt.set(k, Mesh.get_node_subs(i,k) ); }
// Evaluate gradients of the Lagrange polynomials on Gauss quadrature points
dTensor2 gpx(6,3);
dTensor2 gpy(6,3);
for (int m=1; m<=3; m++)
{
double xi = spts.get(m,1);
double eta = spts.get(m,2);
for (int k=1; k<=6; k++)
{
double gp_xi = A2.get(k) + 2.0*A5.get(k)*xi + A4.get(k)*eta;
double gp_eta = A3.get(k) + A4.get(k)*xi + 2.0*A6.get(k)*eta;
gpx.set(k,m, Mesh.get_jmat(i,1,1)*gp_xi
+ Mesh.get_jmat(i,1,2)*gp_eta );
gpy.set(k,m, Mesh.get_jmat(i,2,1)*gp_xi
+ Mesh.get_jmat(i,2,2)*gp_eta );
}
}
// Entries of the stiffness matrix A
double Area = Mesh.get_area_prim(i);
for (int j=1; j<=6; j++)
for (int k=1; k<=6; k++)
{
double tmp = A.get(tt.get(j),tt.get(k));
for (int m=1; m<=3; m++)
{
tmp = tmp + 2.0*Area*wgts.get(m)*(gpx.get(j,m)*gpx.get(k,m)+gpy.get(j,m)*gpy.get(k,m));
}
A.set(tt.get(j),tt.get(k), tmp );
}
}
// Replace boundary node equations by Dirichlet boundary condition enforcement
for (int i=1; i<=NumBndNodes; i++)
{
const int j=Mesh.get_sub_bnd_node(i);
for (int k=1; k<=A.get_NumCols(); k++)
{
A.set(j,k, 0.0 );
}
for (int k=1; k<=A.get_NumRows(); k++)
{
A.set(k,j, 0.0 );
}
A.set(j,j, 1.0 );
}
// Get sparse structure representation
A.Sparsify();
}
void ConjugateGradient(const int MaxIters,
const double TOL,
const FullMatrix& A,
const dTensor1& rhs,
dTensor1& phi)
{
double DotProd(const dTensor1& avec,
const dTensor1& bvec);
void MatMult(const FullMatrix& A,
const dTensor1& invec,
dTensor1& outvec);
const int size = A.get_NumRows();
int NumIters = 0;
int mflag = 0;
dTensor1 r(size);
dTensor1 d(size);
dTensor1 Ad(size);
for (int i=1; i<=size; i++)
{ phi.set(i, 0.0 ); }
MatMult(A,phi,r);
for (int i=1; i<=size; i++)
{ r.set(i, rhs.get(i)-r.get(i) ); }
for (int i=1; i<=size; i++)
{ d.set(i, r.get(i) ); }
double rhs_norm = sqrt(DotProd(rhs,rhs));
if (fabs(rhs_norm)<=1.0e-12)
{ rhs_norm = 1.0; }
//printf(" rhs_norm = %e\n",rhs_norm);
double rdotr = DotProd(r,r);
//printf(" sqrt(rdotr) = %e\n",sqrt(rdotr));
double rel_res_norm = sqrt(rdotr)/rhs_norm;
if(rel_res_norm < TOL)
{ mflag = 1; }
NumIters = 1;
while(mflag==0)
{
MatMult(A,d,Ad);
double alpha = rdotr/DotProd(d,Ad);
for (int i=1; i<=size; i++)
{ phi.set(i, phi.get(i)+alpha*d.get(i) ); }
for (int i=1; i<=size; i++)
{ r.set(i, r.get(i)-alpha*Ad.get(i) ); }
double rdotr_old = rdotr;
rdotr = DotProd(r,r);
rel_res_norm = sqrt(rdotr)/rhs_norm;
if(rel_res_norm < TOL)
{ mflag = 1; }
//printf(" NumIters = %i, rel_res_norm = %e\n",NumIters,rel_res_norm);
if (NumIters==MaxIters)
{ mflag = 1; }
if (mflag==0)
{
double beta = rdotr/rdotr_old;
for (int i=1; i<=size; i++)
{ d.set(i, r.get(i)+beta*d.get(i) ); }
NumIters = NumIters+1;
}
}
printf(" |----------------------------\n");
printf(" | Conjugate Gradient Results:\n");
printf(" |----------------------------\n");
printf(" | MaxIters = %i\n",MaxIters);
printf(" | TOL = %e\n",TOL);
printf(" | NumIters = %i\n",NumIters);
printf(" | residual = %e\n",rel_res_norm);
printf(" |----------------------------\n");
printf("\n");
}
