//-----------------------------------------------------------------------------
void DirichletBC::zero(GenericMatrix& A) const
{
// Check arguments
check_arguments(&A, NULL, NULL, 0);
// A map to hold the mapping from boundary dofs to boundary values
Map boundary_values;
// Create local data for application of boundary conditions
dolfin_assert(_function_space);
LocalData data(*_function_space);
// Compute dofs and values
compute_bc(boundary_values, data, _method);
// Copy boundary value data to arrays
std::vector<dolfin::la_index> dofs(boundary_values.size());
std::size_t i = 0;
for (auto bv = boundary_values.begin(); bv != boundary_values.end(); ++bv)
dofs[i++] = bv->first;
// Modify linear system (A_ii = 1)
A.zero_local(boundary_values.size(), dofs.data());
// Finalise changes to A
A.apply("insert");
}
GenericMatrix IKSolver::pseudoJacobian(std::vector<Joint*> *bones, int type)
{
GenericMatrix j = jacobian(bones,type);
// return j.transpose();
if(type!=0){
return j.transpose();
}
GenericMatrix j_jt_inverse = (j*j.transpose());
//j_jt_inverse.debugPrint("pre-inverse");
j_jt_inverse = j_jt_inverse.inverse();
if( j_jt_inverse.cols() == 1 ) return j.transpose();
//j_jt_inverse.debugPrint("jjtinv");
//bool nan= false;
// If transpose only
// Pseudo Inverse
// Verify if there is inverse
// for(int i=0;i<j_jt_inverse.cols();i++){
// for(int j=0;j<j_jt_inverse.rows();j++){
// if (isnan(j_jt_inverse.get(j,i))){
// nan = true;
// break;
// }
// }
// }
// if(nan){
// return j.transpose();
// }
return (j.transpose()*j_jt_inverse);
}
//-----------------------------------------------------------------------------
CoordinateMatrix::CoordinateMatrix(const GenericMatrix& A, bool symmetric,
bool base_one)
: _symmetric(symmetric), _base_one(base_one)
{
_size[0] = A.size(0);
_size[1] = A.size(1);
// Iterate over local rows
const std::pair<std::size_t, std::size_t> local_row_range = A.local_range(0);
if (!_symmetric)
{
for (std::size_t i = local_row_range.first; i < local_row_range.second; ++i)
{
// Get column and value data for row
std::vector<std::size_t> columns;
std::vector<double> values;
A.getrow(i, columns, values);
// Insert data at end
_rows.insert(_rows.end(), columns.size(), i);
_cols.insert(_cols.end(), columns.begin(), columns.end());
_vals.insert(_vals.end(), values.begin(), values.end());
}
assert(_rows.size() == _cols.size());
}
else
{
assert(_size[0] == _size[1]);
for (std::size_t i = local_row_range.first; i < local_row_range.second; ++i)
{
// Get column and value data for row
std::vector<std::size_t> columns;
std::vector<double> values;
A.getrow(i, columns, values);
for (std::size_t j = 0; j < columns.size(); ++j)
{
if (columns[j] >= i)
{
_rows.push_back(i);
_cols.push_back(columns[j]);
_vals.push_back(values[j]);
}
}
}
assert(_rows.size() == _cols.size());
}
// Add 1 for Fortran-style indices
if (base_one)
{
for (std::size_t i = 0; i < _cols.size(); ++i)
{
_rows[i]++;
_cols[i]++;
}
}
}
//---------------------------------------------------------------------------
void EigenMatrix::axpy(double a, const GenericMatrix& A,
bool same_nonzero_pattern)
{
// Check for same size
if (size(0) != A.size(0) or size(1) != A.size(1))
{
dolfin_error("EigenMatrix.cpp",
"perform axpy operation with Eigen matrix",
"Dimensions don't match");
}
_matA += (a)*(as_type<const EigenMatrix>(A).mat());
}
GenericMatrix<T> GenericMatrix<T>::negative() const
{
GenericMatrix foo = *this;
unsigned dim = foo.rows() * foo.cols();
for (unsigned el = 0; el < dim; ++el)
{
T bar = foo.data()[ el ];
if (bar > T( 0 ))
foo.data()[ el ] = T( 0 );
}
return foo;
}
// [[Rcpp::export]]
List matrix_generic( GenericMatrix m){
List output( m.ncol() ) ;
for( size_t i=0 ; i<4; i++){
output[i] = m(i,i) ;
}
return output ;
}
T cross(const GenericMatrix<T> &a, const GenericMatrix<T> &b, int dim) {
casadi_assert_message(a.size1()==b.size1() && a.size2()==b.size2(),"cross(a,b): Inconsistent dimensions. Dimension of a (" << a.dimString() << " ) must equal that of b (" << b.dimString() << ").");
casadi_assert_message(a.size1()==3 || a.size2()==3,"cross(a,b): One of the dimensions of a should have length 3, but got " << a.dimString() << ".");
casadi_assert_message(dim==-1 || dim==1 || dim==2,"cross(a,b,dim): Dim must be 1, 2 or -1 (automatic).");
std::vector<T> ret(3);
bool t = a.size1()==3;
if (dim==1) t = true;
if (dim==2) t = false;
T a1 = t ? a(0,ALL) : a(ALL,0);
T a2 = t ? a(1,ALL) : a(ALL,1);
T a3 = t ? a(2,ALL) : a(ALL,2);
T b1 = t ? b(0,ALL) : b(ALL,0);
T b2 = t ? b(1,ALL) : b(ALL,1);
T b3 = t ? b(2,ALL) : b(ALL,2);
ret[0] = a2*b3-a3*b2;
ret[1] = a3*b1-a1*b3;
ret[2] = a1*b2-a2*b1;
return t ? vertcat(ret) : horzcat(ret);
}
// [[Rcpp::export]]
IntegerVector runit_GenericMatrix_row( GenericMatrix m ){
GenericMatrix::Row first_row = m.row(0) ;
IntegerVector out( first_row.size() ) ;
std::transform(
first_row.begin(), first_row.end(),
out.begin(),
unary_call<SEXP,int>( Function("length" ) ) ) ;
return out ;
}
// [[Rcpp::export]]
IntegerVector runit_GenericMatrix_column( GenericMatrix m ){
GenericMatrix::Column col = m.column(0) ;
IntegerVector out( col.size() ) ;
std::transform(
col.begin(), col.end(),
out.begin(),
unary_call<SEXP,int>( Function("length" ) )
) ;
return wrap(out) ;
}
GenericMatrix IKSolver::jacobian(std::vector<Joint*>* bones, int type)
{
// Joint * effector = bones->back();
Joint * effector = bones->front();
Joint * joint = effector;
//int joint_count=0;
// if(!(root==NULL)){
/*while(joint!=NULL){
// if(root->getParent()==NULL){
// root->DrawObject();
// }
joint_count++;
joint = joint->parent();
}*/
// }else{
// joint_count=1;
// }
//joint = effector;
GenericMatrix jacobian = GenericMatrix(3,3*bones->size());
qglviewer::Vec posEffector = effector->globalEffectorPosition();
for(unsigned int i = 0 ; i < bones->size() ; i++) {
joint = bones->at(i);
qglviewer::Vec derivatex,derivatey,derivatez;
qglviewer::Vec posJoint = joint->globalPosition();
qglviewer::Vec posrelative = posEffector - posJoint;
GenericMatrix globalTransform = joint->globalTransformationMatrix().transpose();
if(type!=2){
derivatex.setValue(globalTransform.get(0),globalTransform.get(1),globalTransform.get(2));
derivatey.setValue(globalTransform.get(4),globalTransform.get(5),globalTransform.get(6));
derivatez.setValue(globalTransform.get(8),globalTransform.get(9),globalTransform.get(10));
// Cross Product
derivatex = derivatex^posrelative;
derivatey = derivatey^posrelative;
derivatez = derivatez^posrelative;
double vetx[3] = {derivatex.x,derivatey.x,derivatez.x};
double vety[3] = {derivatex.y,derivatey.y,derivatez.y};
double vetz[3] = {derivatex.z,derivatey.z,derivatez.z};
for(int k=0;k<3;k++){
jacobian.set( 0 , k+(3*i) , vetx[k] );
jacobian.set( 1 , k+(3*i) , vety[k] );
jacobian.set( 2 , k+(3*i) , vetz[k] );
}
}else{
posrelative = ( effector->globalEffectorPosition() - joint->globalPosition() );
jacobian.set(0, (3*i), 0);
jacobian.set(0, 1+(3*i), posrelative.z);
jacobian.set(0, 2+(3*i), posrelative.y);
jacobian.set(1, (3*i), -posrelative.z);
jacobian.set(1, 1+(3*i), 0);
jacobian.set(1, 2+(3*i), posrelative.x);
jacobian.set(2, (3*i), posrelative.y);
jacobian.set(2, 1+(3*i), -posrelative.x);
jacobian.set(2, 2+(3*i), 0);
}
}
// jacobian.debugPrint("jacobian");
return jacobian;
}
int ILSConjugateGradients::solveLin ( const GenericMatrix & gm, const Vector & b, Vector & x )
{
Timer t;
if ( timeAnalysis )
t.start();
if ( b.size() != gm.rows() ) {
fthrow(Exception, "Size of vector b (" << b.size() << ") mismatches with the size of the given GenericMatrix (" << gm.rows() << ").");
}
if ( x.size() != gm.cols() )
{
x.resize(gm.cols());
x.set(0.0); // bad initial solution, but whatever
}
// CG-Method: http://www.netlib.org/templates/templates.pdf
//
Vector a;
// compute r^0 = b - A*x^0
gm.multiply( a, x );
Vector r = b - a;
if ( timeAnalysis ) {
t.stop();
cerr << "r = " << r << endl;
cerr << "ILSConjugateGradients: TIME " << t.getSum() << " " << r.normL2() << " " << r.normInf() << endl;
t.start();
}
Vector rold ( r.size(), 0.0 );
// store optimal values
double res_min = r.scalarProduct(r);
Vector current_x = x;
double rhoold = 0.0;
Vector z ( r.size() );
Vector p ( z.size() );
uint i = 1;
while ( i <= maxIterations )
{
// pre-conditioned vector, currently M=I, i.e. no pre-condition
// otherwise set z = M * r
if ( jacobiPreconditioner.size() != r.size() )
z = r;
else {
// use simple Jacobi pre-conditioning
for ( uint jj = 0 ; jj < z.size() ; jj++ )
z[jj] = r[jj] / jacobiPreconditioner[jj];
}
double rho = z.scalarProduct( r );
if ( verbose ) {
cerr << "ILSConjugateGradients: iteration " << i << " / " << maxIterations << endl;
if ( current_x.size() <= 20 )
cerr << "ILSConjugateGradients: current solution " << current_x << endl;
}
if ( i == 1 ) {
p = z;
} else {
double beta;
if ( useFlexibleVersion ) {
beta = ( rho - z.scalarProduct(rold) ) / rhoold;
} else {
beta = rho / rhoold;
}
p = z + beta * p;
}
Vector q ( gm.rows() );
// q = A*p
gm.multiply ( q, p );
// sp = p^T A p
// if A is next to zero this gets nasty, because we divide by sp
// later on
double sp = p.scalarProduct(q);
if ( fabs(sp) < 1e-20 ) {
// we achieved some kind of convergence, at least this
// is a termination condition used in the wiki article
if ( verbose )
cerr << "ILSConjugateGradients: p^T*q is quite small" << endl;
break;
}
double alpha = rho / sp;
current_x = current_x + alpha * p;
rold = r;
r = r - alpha * q;
double res = r.scalarProduct(r);
double resMax = r.normInf();
// store optimal x that produces smallest residual
if (res < res_min) {
x = current_x;
res_min = res;
}
// check convergence
double delta = fabs(alpha) * p.normL2();
if ( verbose ) {
cerr << "ILSConjugateGradients: delta = " << delta << " lower bound = " << minDelta << endl;
cerr << "ILSConjugateGradients: residual = " << res << endl;
cerr << "ILSConjugateGradients: L_inf residual = " << resMax << " lower bound = " << minResidual << endl;
if (resMax < 0)
{
std::cerr << "WARNING: resMax is smaller than zero! " << std::endl;
std::cerr << "ILSConjugateGradients: vector r: " << r << std::endl;
}
}
if ( timeAnalysis ) {
t.stop();
cerr << "ILSConjugateGradients: TIME " << t.getSum() << " " << res << " " << resMax << endl;
t.start();
}
if ( delta < minDelta ) {
if ( verbose )
cerr << "ILSConjugateGradients: small delta" << endl;
break;
}
//fabs was necessary, since the IPP-implementation from normInf was not working correctly
// if ( fabs(resMax) < minResidual ) {
if ( resMax < minResidual ) {
if ( verbose )
{
cerr << "ILSConjugateGradients: small residual -- resMax: "<< resMax << " minRes: " << minResidual << endl;
}
break;
}
rhoold = rho;
i++;
}
if (verbose)
{
cerr << "ILSConjugateGradients: iterations needed: " << std::min<uint>(i,maxIterations) << endl;
cerr << "ILSConjugateGradients: minimal residual achieved: " << res_min << endl;
}
if (verbose)
{
if ( x.size() <= 20 )
cerr << "ILSConjugateGradients: optimal solution: " << x << endl;
}
return 0;
}
//-----------------------------------------------------------------------------
void DirichletBC::zero_columns(GenericMatrix& A,
GenericVector& b,
double diag_val) const
{
// Check arguments
check_arguments(&A, &b, NULL, 1);
// A map to hold the mapping from boundary dofs to boundary values
Map bv_map;
get_boundary_values(bv_map);
// Create lookup table of dofs
//const std::size_t nrows = A.size(0); // should be equal to b.size()
const std::size_t ncols = A.size(1); // should be equal to max possible dof+1
std::pair<std::size_t, std::size_t> rows = A.local_range(0);
std::vector<char> is_bc_dof(ncols);
std::vector<double> bc_dof_val(ncols);
for (Map::const_iterator bv = bv_map.begin(); bv != bv_map.end(); ++bv)
{
is_bc_dof[bv->first] = 1;
bc_dof_val[bv->first] = bv->second;
}
// Scan through all columns of all rows, setting to zero if
// is_bc_dof[column]. At the same time, we collect corrections to
// the RHS
std::vector<std::size_t> cols;
std::vector<double> vals;
std::vector<double> b_vals;
std::vector<dolfin::la_index> b_rows;
for (std::size_t row = rows.first; row < rows.second; row++)
{
// If diag_val is nonzero, the matrix is a diagonal block
// (nrows==ncols), and we can set the whole BC row
if (diag_val != 0.0 && is_bc_dof[row])
{
A.getrow(row, cols, vals);
for (std::size_t j = 0; j < cols.size(); j++)
vals[j] = (cols[j] == row)*diag_val;
A.setrow(row, cols, vals);
A.apply("insert");
b.setitem(row, bc_dof_val[row]*diag_val);
}
else // Otherwise, we scan the row for BC columns
{
A.getrow(row, cols, vals);
bool row_changed = false;
for (std::size_t j = 0; j < cols.size(); j++)
{
const std::size_t col = cols[j];
// Skip columns that aren't BC, and entries that are zero
if (!is_bc_dof[col] || vals[j] == 0.0)
continue;
// We're going to change the row, so make room for it
if (!row_changed)
{
row_changed = true;
b_rows.push_back(row);
b_vals.push_back(0.0);
}
b_vals.back() -= bc_dof_val[col]*vals[j];
vals[j] = 0.0;
}
if (row_changed)
{
A.setrow(row, cols, vals);
A.apply("insert");
}
}
}
b.add_local(&b_vals.front(), b_rows.size(), &b_rows.front());
b.apply("add");
}
void trymatd()
{
Tracer et("Thirteenth test of Matrix package");
Tracer::PrintTrace();
Matrix X(5,20);
int i,j;
for (j=1;j<=20;j++) X(1,j) = j+1;
for (i=2;i<=5;i++) for (j=1;j<=20; j++) X(i,j) = (long)X(i-1,j) * j % 1001;
SymmetricMatrix S; S << X * X.t();
Matrix SM = X * X.t() - S;
Print(SM);
LowerTriangularMatrix L = Cholesky(S);
Matrix Diff = L*L.t()-S; Clean(Diff, 0.000000001);
Print(Diff);
{
Tracer et1("Stage 1");
LowerTriangularMatrix L1(5);
Matrix Xt = X.t(); Matrix Xt2 = Xt;
QRZT(X,L1);
Diff = L - L1; Clean(Diff,0.000000001); Print(Diff);
UpperTriangularMatrix Ut(5);
QRZ(Xt,Ut);
Diff = L - Ut.t(); Clean(Diff,0.000000001); Print(Diff);
Matrix Y(3,20);
for (j=1;j<=20;j++) Y(1,j) = 22-j;
for (i=2;i<=3;i++) for (j=1;j<=20; j++)
Y(i,j) = (long)Y(i-1,j) * j % 101;
Matrix Yt = Y.t(); Matrix M,Mt; Matrix Y2=Y;
QRZT(X,Y,M); QRZ(Xt,Yt,Mt);
Diff = Xt - X.t(); Clean(Diff,0.000000001); Print(Diff);
Diff = Yt - Y.t(); Clean(Diff,0.000000001); Print(Diff);
Diff = Mt - M.t(); Clean(Diff,0.000000001); Print(Diff);
Diff = Y2 * Xt2 * S.i() - M * L.i();
Clean(Diff,0.000000001); Print(Diff);
}
ColumnVector C1(5);
{
Tracer et1("Stage 2");
X.ReSize(5,5);
for (j=1;j<=5;j++) X(1,j) = j+1;
for (i=2;i<=5;i++) for (j=1;j<=5; j++)
X(i,j) = (long)X(i-1,j) * j % 1001;
for (i=1;i<=5;i++) C1(i) = i*i;
CroutMatrix A = X;
ColumnVector C2 = A.i() * C1; C1 = X.i() * C1;
X = C1 - C2; Clean(X,0.000000001); Print(X);
}
{
Tracer et1("Stage 3");
X.ReSize(7,7);
for (j=1;j<=7;j++) X(1,j) = j+1;
for (i=2;i<=7;i++) for (j=1;j<=7; j++)
X(i,j) = (long)X(i-1,j) * j % 1001;
C1.ReSize(7);
for (i=1;i<=7;i++) C1(i) = i*i;
RowVector R1 = C1.t();
Diff = R1 * X.i() - ( X.t().i() * R1.t() ).t(); Clean(Diff,0.000000001);
Print(Diff);
}
{
Tracer et1("Stage 4");
X.ReSize(5,5);
for (j=1;j<=5;j++) X(1,j) = j+1;
for (i=2;i<=5;i++) for (j=1;j<=5; j++)
X(i,j) = (long)X(i-1,j) * j % 1001;
C1.ReSize(5);
for (i=1;i<=5;i++) C1(i) = i*i;
CroutMatrix A1 = X*X;
ColumnVector C2 = A1.i() * C1; C1 = X.i() * C1; C1 = X.i() * C1;
X = C1 - C2; Clean(X,0.000000001); Print(X);
}
{
Tracer et1("Stage 5");
int n = 40;
SymmetricBandMatrix B(n,2); B = 0.0;
for (i=1; i<=n; i++)
{
B(i,i) = 6;
if (i<=n-1) B(i,i+1) = -4;
if (i<=n-2) B(i,i+2) = 1;
}
B(1,1) = 5; B(n,n) = 5;
SymmetricMatrix A = B;
ColumnVector X(n);
X(1) = 429;
for (i=2;i<=n;i++) X(i) = (long)X(i-1) * 31 % 1001;
X = X / 100000L;
// the matrix B is rather ill-conditioned so the difficulty is getting
// good agreement (we have chosen X very small) may not be surprising;
// maximum element size in B.i() is around 1400
ColumnVector Y1 = A.i() * X;
LowerTriangularMatrix C1 = Cholesky(A);
ColumnVector Y2 = C1.t().i() * (C1.i() * X) - Y1;
Clean(Y2, 0.000000001); Print(Y2);
UpperTriangularMatrix CU = C1.t().i();
LowerTriangularMatrix CL = C1.i();
Y2 = CU * (CL * X) - Y1;
Clean(Y2, 0.000000001); Print(Y2);
Y2 = B.i() * X - Y1; Clean(Y2, 0.000000001); Print(Y2);
LowerBandMatrix C2 = Cholesky(B);
Matrix M = C2 - C1; Clean(M, 0.000000001); Print(M);
ColumnVector Y3 = C2.t().i() * (C2.i() * X) - Y1;
Clean(Y3, 0.000000001); Print(Y3);
CU = C1.t().i();
CL = C1.i();
Y3 = CU * (CL * X) - Y1;
Clean(Y3, 0.000000001); Print(Y3);
Y3 = B.i() * X - Y1; Clean(Y3, 0.000000001); Print(Y3);
SymmetricMatrix AI = A.i();
Y2 = AI*X - Y1; Clean(Y2, 0.000000001); Print(Y2);
SymmetricMatrix BI = B.i();
BandMatrix C = B; Matrix CI = C.i();
M = A.i() - CI; Clean(M, 0.000000001); Print(M);
M = B.i() - CI; Clean(M, 0.000000001); Print(M);
M = AI-BI; Clean(M, 0.000000001); Print(M);
M = AI-CI; Clean(M, 0.000000001); Print(M);
M = A; AI << M; M = AI-A; Clean(M, 0.000000001); Print(M);
C = B; BI << C; M = BI-B; Clean(M, 0.000000001); Print(M);
}
{
Tracer et1("Stage 5");
SymmetricMatrix A(4), B(4);
A << 5
<< 1 << 4
<< 2 << 1 << 6
<< 1 << 0 << 1 << 7;
B << 8
<< 1 << 5
<< 1 << 0 << 9
<< 2 << 1 << 0 << 6;
LowerTriangularMatrix AB = Cholesky(A) * Cholesky(B);
Matrix M = Cholesky(A) * B * Cholesky(A).t() - AB*AB.t();
Clean(M, 0.000000001); Print(M);
M = A * Cholesky(B); M = M * M.t() - A * B * A;
Clean(M, 0.000000001); Print(M);
}
{
Tracer et1("Stage 6");
int N=49;
int i;
SymmetricBandMatrix S(N,1);
Matrix B(N,N+1); B=0;
for (i=1;i<=N;i++) { S(i,i)=1; B(i,i)=1; B(i,i+1)=-1; }
for (i=1;i<N; i++) S(i,i+1)=-.5;
DiagonalMatrix D(N+1); D = 1;
B = B.t()*S.i()*B - (D-1.0/(N+1))*2.0;
Clean(B, 0.000000001); Print(B);
}
{
Tracer et1("Stage 7");
// Copying and moving CroutMatrix
Matrix A(7,7);
A.Row(1) << 3 << 2 << -1 << 4 << -3 << 5 << 9;
A.Row(2) << -8 << 7 << 2 << 0 << 7 << 0 << -1;
A.Row(3) << 2 << -2 << 3 << 1 << 9 << 0 << 3;
A.Row(4) << -1 << 5 << 2 << 2 << 5 << -1 << 2;
A.Row(5) << 4 << -4 << 1 << 9 << -8 << 7 << 5;
A.Row(6) << 1 << -2 << 5 << -1 << -2 << 5 << 1;
A.Row(7) << -6 << 3 << -1 << 8 << -1 << 2 << 2;
RowVector D(30); D = 0;
Real x = determinant(A);
CroutMatrix B = A;
D(1) = determinant(B) / x - 1;
Matrix C = A * Inverter1(B) - IdentityMatrix(7);
Clean(C, 0.000000001); Print(C);
// Test copy constructor (in Inverter2 and ordinary copy)
CroutMatrix B1; B1 = B;
D(2) = determinant(B1) / x - 1;
C = A * Inverter2(B1) - IdentityMatrix(7);
Clean(C, 0.000000001); Print(C);
// Do it again with release
B.release(); B1 = B;
D(2) = B.nrows(); D(3) = B.ncols(); D(4) = B.size();
D(5) = determinant(B1) / x - 1;
B1.release();
C = A * Inverter2(B1) - IdentityMatrix(7);
D(6) = B1.nrows(); D(7) = B1.ncols(); D(8) = B1.size();
Clean(C, 0.000000001); Print(C);
// see if we get an implicit invert
B1 = -A;
D(9) = determinant(B1) / x + 1; // odd number of rows - sign will change
C = -A * Inverter2(B1) - IdentityMatrix(7);
Clean(C, 0.000000001); Print(C);
// check for_return
B = LU1(A); B1 = LU2(A); CroutMatrix B2 = LU3(A);
C = A * B.i() - IdentityMatrix(7); Clean(C, 0.000000001); Print(C);
D(10) = (B == B1 ? 0 : 1) + (B == B2 ? 0 : 1);
// check lengths
D(13) = B.size()-49;
// check release(2)
B1.release(2);
B2 = B1; D(15) = B == B2 ? 0 : 1;
CroutMatrix B3 = B1; D(16) = B == B3 ? 0 : 1;
D(17) = B1.size();
// some oddments
B1 = B; B1 = B1.i(); C = A - B1.i(); Clean(C, 0.000000001); Print(C);
B1 = B; B1.release(); B1 = B1; B2 = B1;
D(19) = B == B1 ? 0 : 1; D(20) = B == B2 ? 0 : 1;
B1.cleanup(); B2 = B1; D(21) = B1.size(); D(22) = B2.size();
GenericMatrix GM = B; C = A.i() - GM.i(); Clean(C, 0.000000001); Print(C);
B1 = GM; D(23) = B == B1 ? 0 : 1;
B1 = A * 0; B2 = B1; D(24) = B2.is_singular() ? 0 : 1;
// check release again - see if memory moves
const Real* d = B.const_data();
const int* i = B.const_data_indx();
B1 = B;
const Real* d1 = B1.const_data();
const int* i1 = B1.const_data_indx();
B1.release(); B2 = B1;
const Real* d2 = B2.const_data();
const int* i2 = B2.const_data_indx();
D(25) = (d != d1 ? 0 : 1) + (d1 == d2 ? 0 : 1)
+ (i != i1 ? 0 : 1) + (i1 == i2 ? 0 : 1);
Clean(D, 0.000000001); Print(D);
}
{
Tracer et1("Stage 8");
// Same for BandLUMatrix
BandMatrix A(7,3,2);
A.Row(1) << 3 << 2 << -1;
A.Row(2) << -8 << 7 << 2 << 0;
A.Row(3) << 2 << -2 << 3 << 1 << 9;
A.Row(4) << -1 << 5 << 2 << 2 << 5 << -1;
A.Row(5) << -4 << 1 << 9 << -8 << 7 << 5;
A.Row(6) << 5 << -1 << -2 << 5 << 1;
A.Row(7) << 8 << -1 << 2 << 2;
RowVector D(30); D = 0;
Real x = determinant(A);
BandLUMatrix B = A;
D(1) = determinant(B) / x - 1;
Matrix C = A * Inverter1(B) - IdentityMatrix(7);
Clean(C, 0.000000001); Print(C);
// Test copy constructor (in Inverter2 and ordinary copy)
BandLUMatrix B1; B1 = B;
D(2) = determinant(B1) / x - 1;
C = A * Inverter2(B1) - IdentityMatrix(7);
Clean(C, 0.000000001); Print(C);
// Do it again with release
B.release(); B1 = B;
D(2) = B.nrows(); D(3) = B.ncols(); D(4) = B.size();
D(5) = determinant(B1) / x - 1;
B1.release();
C = A * Inverter2(B1) - IdentityMatrix(7);
D(6) = B1.nrows(); D(7) = B1.ncols(); D(8) = B1.size();
Clean(C, 0.000000001); Print(C);
// see if we get an implicit invert
B1 = -A;
D(9) = determinant(B1) / x + 1; // odd number of rows - sign will change
C = -A * Inverter2(B1) - IdentityMatrix(7);
Clean(C, 0.000000001); Print(C);
// check for_return
B = LU1(A); B1 = LU2(A); BandLUMatrix B2 = LU3(A);
C = A * B.i() - IdentityMatrix(7); Clean(C, 0.000000001); Print(C);
D(10) = (B == B1 ? 0 : 1) + (B == B2 ? 0 : 1);
// check lengths
D(11) = B.bandwidth().lower()-3;
D(12) = B.bandwidth().upper()-2;
D(13) = B.size()-42;
D(14) = B.size2()-21;
// check release(2)
B1.release(2);
B2 = B1; D(15) = B == B2 ? 0 : 1;
BandLUMatrix B3 = B1; D(16) = B == B3 ? 0 : 1;
D(17) = B1.size();
// Compare with CroutMatrix
CroutMatrix CM = A;
C = CM.i() - B.i(); Clean(C, 0.000000001); Print(C);
D(18) = determinant(CM) / x - 1;
// some oddments
B1 = B; CM = B1.i(); C = A - CM.i(); Clean(C, 0.000000001); Print(C);
B1 = B; B1.release(); B1 = B1; B2 = B1;
D(19) = B == B1 ? 0 : 1; D(20) = B == B2 ? 0 : 1;
B1.cleanup(); B2 = B1; D(21) = B1.size(); D(22) = B2.size();
GenericMatrix GM = B; C = A.i() - GM.i(); Clean(C, 0.000000001); Print(C);
B1 = GM; D(23) = B == B1 ? 0 : 1;
B1 = A * 0; B2 = B1; D(24) = B2.is_singular() ? 0 : 1;
// check release again - see if memory moves
const Real* d = B.const_data(); const Real* dd = B.const_data();
const int* i = B.const_data_indx();
B1 = B;
const Real* d1 = B1.const_data(); const Real* dd1 = B1.const_data();
const int* i1 = B1.const_data_indx();
B1.release(); B2 = B1;
const Real* d2 = B2.const_data(); const Real* dd2 = B2.const_data();
const int* i2 = B2.const_data_indx();
D(25) = (d != d1 ? 0 : 1) + (d1 == d2 ? 0 : 1)
+ (dd != dd1 ? 0 : 1) + (dd1 == dd2 ? 0 : 1)
+ (i != i1 ? 0 : 1) + (i1 == i2 ? 0 : 1);
Clean(D, 0.000000001); Print(D);
}
{
Tracer et1("Stage 9");
// Modification of Cholesky decomposition
int i, j;
// Build test matrix
Matrix X(100, 10);
MultWithCarry mwc; // Uniform random number generator
for (i = 1; i <= 100; ++i) for (j = 1; j <= 10; ++j)
X(i, j) = 2.0 * (mwc.Next() - 0.5);
Matrix X1 = X; // save copy
// Form sums of squares and products matrix and Cholesky decompose
SymmetricMatrix A; A << X.t() * X;
UpperTriangularMatrix U1 = Cholesky(A).t();
// Do QR decomposition of X and check we get same triangular matrix
UpperTriangularMatrix U2;
QRZ(X, U2);
Matrix Diff = U1 - U2; Clean(Diff, 0.000000001); Print(Diff);
// Try adding new row to X and updating triangular matrix
RowVector NewRow(10);
for (j = 1; j <= 10; ++j) NewRow(j) = 2.0 * (mwc.Next() - 0.5);
UpdateCholesky(U2, NewRow);
X = X1 & NewRow; QRZ(X, U1);
Diff = U1 - U2; Clean(Diff, 0.000000001); Print(Diff);
// Try removing two rows and updating triangular matrix
DowndateCholesky(U2, X1.Row(20));
DowndateCholesky(U2, X1.Row(35));
X = X1.Rows(1,19) & X1.Rows(21,34) & X1.Rows(36,100) & NewRow; QRZ(X, U1);
Diff = U1 - U2; Clean(Diff, 0.000000001); Print(Diff);
// Circular shifts
CircularShift(X, 3,6);
CircularShift(X, 5,5);
CircularShift(X, 4,5);
CircularShift(X, 1,6);
CircularShift(X, 6,10);
}
{
Tracer et1("Stage 10");
// Try updating QRZ, QRZT decomposition
TestUpdateQRZ tuqrz1(10, 100, 50, 25); tuqrz1.DoTest();
tuqrz1.Reset(); tuqrz1.ClearRow(1); tuqrz1.DoTest();
tuqrz1.Reset(); tuqrz1.ClearRow(1); tuqrz1.ClearRow(2); tuqrz1.DoTest();
tuqrz1.Reset(); tuqrz1.ClearRow(5); tuqrz1.ClearRow(6); tuqrz1.DoTest();
tuqrz1.Reset(); tuqrz1.ClearRow(10); tuqrz1.DoTest();
TestUpdateQRZ tuqrz2(15, 100, 0, 0); tuqrz2.DoTest();
tuqrz2.Reset(); tuqrz2.ClearRow(1); tuqrz2.DoTest();
tuqrz2.Reset(); tuqrz2.ClearRow(1); tuqrz2.ClearRow(2); tuqrz2.DoTest();
tuqrz2.Reset(); tuqrz2.ClearRow(5); tuqrz2.ClearRow(6); tuqrz2.DoTest();
tuqrz2.Reset(); tuqrz2.ClearRow(15); tuqrz2.DoTest();
TestUpdateQRZ tuqrz3(5, 0, 10, 0); tuqrz3.DoTest();
}
// cout << "\nEnd of Thirteenth test\n";
}
// Base case for all divergence computations.
// Compute divergence of vector field u.
void cr_divergence_matrix(GenericMatrix& M, GenericMatrix& A,
const FunctionSpace& DGscalar,
const FunctionSpace& CRvector)
{
std::shared_ptr<const GenericDofMap>
CR1_dofmap = CRvector.dofmap(),
DG0_dofmap = DGscalar.dofmap();
// Figure out about the local dofs of DG0
std::pair<std::size_t, std::size_t>
first_last_dof = DG0_dofmap->ownership_range();
std::size_t first_dof = first_last_dof.first;
std::size_t last_dof = first_last_dof.second;
std::size_t n_local_dofs = last_dof - first_dof;
// Get topological dimension so that we know what Facet is
const Mesh mesh = *DGscalar.mesh();
std::size_t tdim = mesh.topology().dim();
std::size_t gdim = mesh.geometry().dim();
std::vector<std::size_t> columns;
std::vector<double> values;
// Fill the values
for(CellIterator cell(mesh); !cell.end(); ++cell)
{
auto dg_dofs = DG0_dofmap->cell_dofs(cell->index());
// There is only one DG0 dof per cell
dolfin::la_index cell_dof = dg_dofs[0];
Point cell_mp = cell->midpoint();
double cell_volume = cell->volume();
std::size_t local_facet_index = 0;
auto cr_dofs = CR1_dofmap->cell_dofs(cell->index());
for(FacetIterator facet(*cell); !facet.end(); ++facet)
{
double facet_measure=0;
if(tdim == 2)
facet_measure = Edge(mesh, facet->index()).length();
else if(tdim == 3)
facet_measure = Face(mesh, facet->index()).area();
// Tdim 1 will not happen because CR is not defined there
Point facet_normal = facet->normal();
// Flip the normal if it is not outer already
Point facet_mp = facet->midpoint();
double sign = (facet_normal.dot(facet_mp - cell_mp) > 0.0) ? 1.0 : -1.0;
facet_normal *= (sign*facet_measure/cell_volume);
// Dofs of CR on the facet, local order
std::vector<std::size_t> facet_dofs;
CR1_dofmap->tabulate_facet_dofs(facet_dofs, local_facet_index);
for (std::size_t j = 0 ; j < facet_dofs.size(); j++)
{
columns.push_back(cr_dofs[facet_dofs[j]]);
values.push_back(facet_normal[j]);
}
local_facet_index += 1;
}
M.setrow(cell_dof, columns, values);
columns.clear();
values.clear();
}
M.apply("insert");
//std::shared_ptr<GenericMatrix> Cp = MatMatMult(M, A);
//return Cp;
}
int ILSSymmLqLanczos::solveLin ( const GenericMatrix & gm, const Vector & b, Vector & x )
{
if ( b.size() != gm.rows() ) {
fthrow(Exception, "Size of vector b (" << b.size() << ") mismatches with the size of the given GenericMatrix (" << gm.rows() << ").");
}
if ( x.size() != gm.cols() )
{
x.resize(gm.cols());
x.set(0.0); // bad initial solution, but whatever
}
// if ( verbose ) cerr << "initial solution: " << x << endl;
// SYMMLQ-Method based on Lanczos vectors: implementation based on the following:
//
// C.C. Paige and M.A. Saunders: "Solution of sparse indefinite systems of linear equations". SIAM Journal on Numerical Analysis, p. 617--629, vol. 12, no. 4, 1975
//
// http://www.netlib.org/templates/templates.pdf
//
// declare some helpers
double gamma = 0.0;
double gamma_bar = 0.0;
double alpha = 0.0; // alpha_j = v_j^T * A * v_j for new Lanczos vector v_j
double beta = b.normL2(); // beta_1 = norm(b), in general beta_j = norm(v_j) for new Lanczos vector v_j
double beta_next = 0.0; // beta_{j+1}
double c_new = 0.0;
double c_old = -1.0;
double s_new = 0.0;
double s_old = 0.0;
double z_new = 0.0;
double z_old = 0.0;
double z_older = 0.0;
double delta_new = 0.0;
double epsilon_next = 0.0;
// init some helping vectors
Vector Av(b.size(),0.0); // Av = A * v_j
Vector Ac(b.size(),0.0); // Ac = A * c_j
Vector *v_new = new Vector(b.size(),0.0); // new Lanczos vector v_j
Vector *v_old = 0; // Lanczos vector of the iteration before: v_{j-1}
Vector *v_next = new Vector(b.size(),0.0); // Lanczos vector of the next iteration: v_{j+1}
Vector *w_new = new Vector(b.size(),0.0);
Vector *w_bar = new Vector(b.size(),0.0);
Vector x_L (b.size(),0.0);
// Vector x_C (b.size(),0.0); // x_C is a much better approximation than x_L (according to the paper mentioned above)
// NOTE we store x_C in output variable x and only update this solution if the residual decreases (we are able to calculate the residual of x_C without calculating x_C)
// first iteration + initialization, where b will be used as the first Lanczos vector
*v_new = (1/beta)*b; // init v_1, v_1 = b / norm(b)
gm.multiply(Av,*v_new); // Av = A * v_1
alpha = v_new->scalarProduct(Av); // alpha_1 = v_1^T * A * v_1
gamma_bar = alpha; // (gamma_bar_1 is equal to alpha_1 in ILSConjugateGradientsLanczos)
*v_next = Av - (alpha*(*v_new));
beta_next = v_next->normL2();
v_next->normalizeL2();
gamma = sqrt( (gamma_bar*gamma_bar) + (beta_next*beta_next) );
c_new = gamma_bar/gamma;
s_new = beta_next/gamma;
z_new = beta/gamma;
*w_bar = *v_new;
*w_new = c_new*(*w_bar) + s_new*(*v_next);
*w_bar = s_new*(*w_bar) - c_new*(*v_next);
x_L = z_new*(*w_new); // first approximation of x
// calculate current residual of x_C
double res_x_C = (beta*beta)*(s_new*s_new)/(c_new*c_new);
// store minimal residual
double res_x_C_min = res_x_C;
// store optimal solution x_C in output variable x instead of additional variable x_C
x = x_L + (z_new/c_new)*(*w_bar); // x_C = x_L + (z_new/c_new)*(*w_bar);
// calculate delta of x_L
double delta_x_L = fabs(z_new) * w_new->normL2();
if ( verbose ) {
cerr << "ILSSymmLqLanczos: iteration 1 / " << maxIterations << endl;
if ( x.size() <= 20 )
cerr << "ILSSymmLqLanczos: current solution x_L: " << x_L << endl;
cerr << "ILSSymmLqLanczos: delta_x_L = " << delta_x_L << endl;
}
// start with second iteration
uint j = 2;
while (j <= maxIterations )
{
// prepare next iteration
if ( v_old == 0 ) v_old = v_new;
else {
delete v_old;
v_old = v_new;
}
v_new = v_next;
v_next = new Vector(b.size(),0.0);
beta = beta_next;
z_older = z_old;
z_old = z_new;
s_old = s_new;
res_x_C *= (c_new*c_new);
// start next iteration:
// calculate next Lanczos vector v_ {j+1} based on older ones
gm.multiply(Av,*v_new);
alpha = v_new->scalarProduct(Av);
*v_next = Av - (alpha*(*v_new)) - (beta*(*v_old)); // calculate v_{j+1}
beta_next = v_next->normL2(); // calculate beta_{j+1}
v_next->normalizeL2(); // normalize v_{j+1}
// calculate elements of matrix L_bar_{j}
gamma_bar = -c_old*s_new*beta - c_new*alpha; // calculate gamma_bar_{j}
delta_new = -c_old*c_new*beta + s_new*alpha; // calculate delta_{j}
//NOTE updating c_old after using it to calculate gamma_bar and delta_new is important!!
c_old = c_new;
// calculate helpers (equation 5.6 in the paper mentioned above)
gamma = sqrt( (gamma_bar*gamma_bar) + (beta_next*beta_next) ); // calculate gamma_{j}
c_new = gamma_bar/gamma; // calculate c_{j-1}
s_new = beta_next/gamma; // calculate s_{j-1}
// calculate next component z_{j} of vector z
z_new = - (delta_new*z_old + epsilon_next*z_older)/gamma;
//NOTE updating epsilon_next after using it to calculate z_new is important!!
epsilon_next = s_old*beta_next; // calculate epsilon_{j+1} of matrix L_bar_{j+1}
// calculate residual of current solution x_C without computing this solution x_C before
res_x_C *= (s_new*s_new)/(c_new*c_new);
// we only update our solution x (originally x_C ) if the residual is smaller
if ( res_x_C < res_x_C_min )
{
x = x_L + (z_new/c_new)*(*w_bar); // x_C = x_L + (z_new/c_new)*(*w_bar); // update x
res_x_C_min = res_x_C;
}
// calculate new vectors w_{j} and w_bar_{j+1} according to equation 5.9 of the paper mentioned above
*w_new = c_new*(*w_bar) + s_new*(*v_next);
*w_bar = s_new*(*w_bar) - c_new*(*v_next);
x_L += z_new*(*w_new); // update x_L
if ( verbose ) {
cerr << "ILSSymmLqLanczos: iteration " << j << " / " << maxIterations << endl;
if ( x.size() <= 20 )
cerr << "ILSSymmLqLanczos: current solution x_L: " << x_L << endl;
}
// check convergence
delta_x_L = fabs(z_new) * w_new->normL2();
if ( verbose ) {
cerr << "ILSSymmLqLanczos: delta_x_L = " << delta_x_L << endl;
cerr << "ILSSymmLqLanczos: residual = " << res_x_C << endl;
}
if ( delta_x_L < minDelta ) {
if ( verbose )
cerr << "ILSSymmLqLanczos: small delta_x_L" << endl;
break;
}
j++;
}
// if ( verbose ) {
cerr << "ILSSymmLqLanczos: iterations needed: " << std::min<uint>(j,maxIterations) << endl;
cerr << "ILSSymmLqLanczos: minimal residual achieved: " << res_x_C_min << endl;
if ( x.size() <= 20 )
cerr << "ILSSymmLqLanczos: optimal solution: " << x << endl;
// }
// WE DO NOT WANT TO CALCULATE THE RESIDUAL EXPLICITLY
//
// Vector tmp;
// gm.multiply(tmp,x_C);
// Vector res ( b - tmp );
// double res_x_C = res.scalarProduct(res);
//
// gm.multiply(tmp,x_L);
// res = b - tmp;
// double res_x_L = res.scalarProduct(res);
//
// if ( res_x_L < res_x_C )
// {
// x = x_L;
// if ( verbose )
// cerr << "ILSSymmLqLanczos: x_L used with residual " << res_x_L << " < " << res_x_C << endl;
//
// } else
// {
//
// x = x_C;
// if ( verbose )
// cerr << "ILSSymmLqLanczos: x_C used with residual " << res_x_C << " < " << res_x_L << endl;
//
// }
delete v_new;
delete v_old;
delete v_next;
delete w_new;
delete w_bar;
return 0;
}
//-----------------------------------------------------------------------------
void NewDirichletBC::apply(GenericMatrix& A, GenericVector& b,
const GenericVector* x, const DofMap& dof_map, const ufc::form& form)
{
bool reassemble = dolfin_get("PDE reassemble matrix");
std::cout << "newdirichlet: " << reassemble << std::endl;
// FIXME: How do we reuse the dof map for u?
if (method == topological)
message("Applying Dirichlet boundary conditions to linear system.");
/*
else if (method == geometrical)
message("Applying Dirichlet boundary conditions to linear system (geometrical approach).");
else
message("Applying Dirichlet boundary conditions to linear system (pointwise approach).");
*/
// Make sure we have the facet - cell connectivity
const uint D = _mesh.topology().dim();
if (method == topological)
_mesh.init(D - 1, D);
// Create local data for application of boundary conditions
BoundaryCondition::LocalData data(form, _mesh, dof_map, sub_system);
// A map to hold the mapping from boundary dofs to boundary values
std::map<uint, real> boundary_values;
if (method == pointwise)
{
Progress p("Applying Dirichlet boundary conditions", _mesh.size(D));
for (CellIterator cell(_mesh); !cell.end(); ++cell)
{
computeBCPointwise(boundary_values, *cell, data);
p++;
}
}
else
{
// Iterate over the facets of the mesh
Progress p("Applying Dirichlet boundary conditions", _mesh.size(D - 1));
for (FacetIterator facet(_mesh); !facet.end(); ++facet)
{
// Skip facets not inside the sub domain
if ((*sub_domains)(*facet) != sub_domain)
{
p++;
continue;
}
// Chose strategy
if (method == topological)
computeBCTopological(boundary_values, *facet, data);
else
computeBCGeometrical(boundary_values, *facet, data);
// Update process
p++;
}
}
// Copy boundary value data to arrays
uint* dofs = new uint[boundary_values.size()];
real* values = new real[boundary_values.size()];
std::map<uint, real>::const_iterator boundary_value;
uint i = 0;
for (boundary_value = boundary_values.begin(); boundary_value != boundary_values.end(); ++boundary_value)
{
dofs[i] = boundary_value->first;
values[i++] = boundary_value->second;
}
// Modify boundary values for nonlinear problems
if (x)
{
real* x_values = new real[boundary_values.size()];
x->get(x_values, boundary_values.size(), dofs);
for (uint i = 0; i < boundary_values.size(); i++)
values[i] -= x_values[i];
delete[] x_values;
}
// Modify RHS vector (b[i] = value)
b.set(values, boundary_values.size(), dofs);
if(reassemble)
{
// Modify linear system (A_ii = 1)
A.ident(boundary_values.size(), dofs);
}
// Clear temporary arrays
delete [] dofs;
delete [] values;
// Finalise changes to b
b.apply();
}
void IKSolver::solve(std::vector<Joint*>* bones, qglviewer::Vec goal, int type)
{
//cout << type << endl;
Joint* effector = bones->front();
qglviewer::Vec posEffector = effector->globalEffectorPosition();
if ((goal-posEffector).norm()<GOAL_DISTANCE_ERROR) return;
Joint *root;
root = effector;
// int joint_count=0;
// if(!(root==NULL)){
// while(root!=NULL){
// joint_count++;
// root = root->parent();
// }
// }else{
// joint_count=1;
// }
// root = effector;
GenericMatrix jacobianMatrix = pseudoJacobian(bones,type);
qglviewer::Vec e;
if(type==1){
if((goal-posEffector).norm()<MAX_DISTANCE_FRAME){
return;
}else{
e = ((goal-posEffector)*MAX_DISTANCE_FRAME);
//e = ( ((goal-posEffector)*MAX_DISTANCE_FRAME) / ((goal-posEffector).norm()) );
}
}else{
e = (goal-posEffector)*D;
}
GenericMatrix position = GenericMatrix(3,1);
position.set( 0 , 0 , e.x);
position.set( 1 , 0 , e.y);
position.set( 2 , 0 , e.z);
GenericMatrix application = (jacobianMatrix * position);
// application.debugPrint("application");
int i = 0;
for(int j = 0 ; j < bones->size() ; j++ ) {
// Smoothing Factor s
Joint* root = bones->at(j);
double s=1;
double angle;
qglviewer::Vec vector;
qglviewer::Quaternion qresult = root->orientation();
if(type == 2){
vector = qglviewer::Vec(1,0,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qglviewer::Quaternion(vector,angle) * qresult;
i++;
vector = qglviewer::Vec(0,1,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qglviewer::Quaternion(vector,angle) * qresult;
i++;
vector = qglviewer::Vec(0,0,1);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qglviewer::Quaternion(vector,angle) * qresult;
i++;
}else{
vector = qglviewer::Vec(1,0,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qresult * qglviewer::Quaternion(vector,angle);
i++;
vector = qglviewer::Vec(0,1,0);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qresult * qglviewer::Quaternion(vector,angle);
i++;
vector = qglviewer::Vec(0,0,1);
angle = ( application.get(i,0)/**(180.0/M_PI)*/ );
qresult = qresult * qglviewer::Quaternion(vector,angle);
i++;
}
root->setNewOrientation( qresult );
}
}
void Transposer(const GenericMatrix& GM1, GenericMatrix&GM2)
{ GM2 = GM1.t(); }
void trymat1()
{
// cout << "\nFirst test of Matrix package\n\n";
Tracer et("First test of Matrix package");
Tracer::PrintTrace();
{
Tracer et1("Stage 1");
int i,j;
LowerTriangularMatrix L(10);
for (i=1;i<=10;i++) for (j=1;j<=i;j++) L(i,j)=2.0+i*i+j;
SymmetricMatrix S(10);
for (i=1;i<=10;i++) for (j=1;j<=i;j++) S(i,j)=i*j+1.0;
SymmetricMatrix S1 = S / 2.0;
S = S1 * 2.0;
UpperTriangularMatrix U=L.t()*2.0;
Print(LowerTriangularMatrix(L-U.t()*0.5));
DiagonalMatrix D(10);
for (i=1;i<=10;i++) D(i,i)=(i-4)*(i-5)*(i-6);
Matrix M=(S+U-D+L)*(L+U-D+S);
DiagonalMatrix DD=D*D;
LowerTriangularMatrix LD=L*D;
// expressions split for Turbo C
Matrix M1 = S*L + U*L - D*L + L*L + 10.0;
{ M1 = M1 + S*U + U*U - D*U + L*U - S*D; }
{ M1 = M1 - U*D + DD - LD + S*S; }
{ M1 = M1 + U*S - D*S + L*S - 10.0; }
M=M1-M;
Print(M);
}
{
Tracer et1("Stage 2");
int i,j;
LowerTriangularMatrix L(9);
for (i=1;i<=9;i++) for (j=1;j<=i;j++) L(i,j)=1.0+j;
UpperTriangularMatrix U1(9);
for (j=1;j<=9;j++) for (i=1;i<=j;i++) U1(i,j)=1.0+i;
LowerTriangularMatrix LX(9);
for (i=1;i<=9;i++) for (j=1;j<=i;j++) LX(i,j)=1.0+i*i;
UpperTriangularMatrix UX(9);
for (j=1;j<=9;j++) for (i=1;i<=j;i++) UX(i,j)=1.0+j*j;
{
L=L+LX/0.5; L=L-LX*3.0; L=LX*2.0+L;
U1=U1+UX*2.0; U1=U1-UX*3.0; U1=UX*2.0+U1;
}
SymmetricMatrix S(9);
for (i=1;i<=9;i++) for (j=1;j<=i;j++) S(i,j)=i*i+j;
{
SymmetricMatrix S1 = S;
S=S1+5.0;
S=S-3.0;
}
DiagonalMatrix D(9);
for (i=1;i<=9;i++) D(i,i)=S(i,i);
UpperTriangularMatrix U=L.t()*2.0;
{
U1=U1*2.0 - U; Print(U1);
L=L*2.0-D; U=U-D;
}
Matrix M=U+L; S=S*2.0; M=S-M; Print(M);
}
{
Tracer et1("Stage 3");
int i,j;
Matrix M(10,3), N(10,3);
for (i = 1; i<=10; i++) for (j = 1; j<=3; j++)
{ M(i,j) = 2*i-j; N(i,j) = i*j + 20; }
Matrix MN = M + N, M1;
M1 = M; M1 += N; M1 -= MN; Print(M1);
M1 = M; M1 += M1; M1 = M1 - M * 2; Print(M1);
M1 = M; M1 += N * 2; M1 -= (MN + N); Print(M1);
M1 = M; M1 -= M1; Print(M1);
M1 = M; M1 -= MN + M1; M1 += N + M; Print(M1);
M1 = M; M1 -= 5; M1 -= M; M1 *= 0.2; M1 = M1 + 1; Print(M1);
Matrix NT = N.t();
M1 = M; M1 *= NT; M1 -= M * N.t(); Print(M1);
M = M * M.t();
DiagonalMatrix D(10); D = 2;
M1 = M; M1 += D; M1 -= M; M1 = M1 - D; Print(M1);
M1 = M; M1 -= D; M1 -= M; M1 = M1 + D; Print(M1);
M1 = M; M1 *= D; M1 /= 2; M1 -= M; Print(M1);
SymmetricMatrix SM; SM << M;
// UpperTriangularMatrix SM; SM << M;
SM += 10; M1 = SM - M; M1 /=10; M1 = M1 - 1; Print(M1);
}
{
Tracer et1("Stage 4");
int i,j;
Matrix M(10,3), N(10,5);
for (i = 1; i<=10; i++) for (j = 1; j<=3; j++) M(i,j) = 2*i-j;
for (i = 1; i<=10; i++) for (j = 1; j<=5; j++) N(i,j) = i*j + 20;
Matrix M1;
M1 = M; M1 |= N; M1 &= N | M;
M1 -= (M | N) & (N | M); Print(M1);
M1 = M; M1 |= M1; M1 &= M1;
M1 -= (M | M) & (M | M); Print(M1);
}
{
Tracer et1("Stage 5");
int i,j;
BandMatrix BM1(10,2,3), BM2(10,4,1); Matrix M1(10,10), M2(10,10);
for (i=1;i<=10;i++) for (j=1;j<=10;j++)
{ M1(i,j) = 0.5*i+j*j-50; M2(i,j) = (i*101 + j*103) % 13; }
BM1.Inject(M1); BM2.Inject(M2);
BandMatrix BM = BM1; BM += BM2;
Matrix M1X = BM1; Matrix M2X = BM2; Matrix MX = BM;
MX -= M1X + M2X; Print(MX);
MX = BM1; MX += BM2; MX -= M1X; MX -= M2X; Print(MX);
SymmetricBandMatrix SM1; SM1 << BM1 * BM1.t();
SymmetricBandMatrix SM2; SM2 << BM2 * BM2.t();
SM1 *= 5.5;
M1X *= M1X.t(); M1X *= 5.5; M2X *= M2X.t();
SM1 -= SM2; M1 = SM1 - M1X + M2X; Print(M1);
M1 = BM1; BM1 *= SM1; M1 = M1 * SM1 - BM1; Print(M1);
M1 = BM1; BM1 -= SM1; M1 = M1 - SM1 - BM1; Print(M1);
M1 = BM1; BM1 += SM1; M1 = M1 + SM1 - BM1; Print(M1);
}
{
Tracer et1("Stage 6");
int i,j;
Matrix M(10,10), N(10,10);
for (i = 1; i<=10; i++) for (j = 1; j<=10; j++)
{ M(i,j) = 2*i-j; N(i,j) = i*j + 20; }
GenericMatrix GM = M;
GM += N; Matrix M1 = GM - N - M; Print(M1);
DiagonalMatrix D(10); D = 3;
GM = D; GM += N; GM += M; GM += D;
M1 = D*2 - GM + M + N; Print(M1);
GM = D; GM *= 4; GM += 16; GM /= 8; GM -= 2;
GM -= D / 2; M1 = GM; Print(M1);
GM = D; GM *= M; GM *= N; GM /= 3; M1 = M*N - GM; Print(M1);
GM = D; GM |= M; GM &= N | D; M1 = GM - ((D | M) & (N | D));
Print(M1);
GM = M; M1 = M; GM += 5; GM *= 3; M *= 3; M += 15; M1 = GM - M;
Print(M1);
D.ReSize(10); for (i = 1; i<=10; i++) D(i) = i;
M1 = D + 10; GM = D; GM += 10; M1 -= GM; Print(M1);
GM = M; GM -= D; M1 = GM; GM = D; GM -= M; M1 += GM; Print(M1);
GM = M; GM *= D; M1 = GM; GM = D; GM *= M.t();
M1 -= GM.t(); Print(M1);
GM = M; GM += 2 * GM; GM -= 3 * M; M1 = GM; Print(M1);
GM = M; GM |= GM; GM -= (M | M); M1 = GM; Print(M1);
GM = M; GM &= GM; GM -= (M & M); M1 = GM; Print(M1);
M1 = M; M1 = (M1.t() & M.t()) - (M | M).t(); Print(M1);
M1 = M; M1 = (M1.t() | M.t()) - (M & M).t(); Print(M1);
}
{
Tracer et1("Stage 7");
// test for bug in MS VC5
int n = 3;
int k; int j;
Matrix A(n,n), B(n,n);
//first version - MS VC++ 5 mis-compiles if optimisation is on
for (k=1; k<=n; k++)
{
for (j = 1; j <= n; j++) A(k,j) = ((k-1) * (2*j-1));
}
//second version
for (k=1; k<=n; k++)
{
const int k1 = k-1; // otherwise Visual C++ 5 fails
for (j = 1; j <= n; j++) B(k,j) = (k1 * (2*j-1));
}
if (A != B)
{
cout << "\nVisual C++ version 5 compiler error?";
cout << "\nTurn off optimisation";
}
A -= B; Print(A);
}
// cout << "\nEnd of first test\n";
}
void compute_DG0_to_CG_weight_matrix(GenericMatrix& A, Function& DG)
{
compute_weight(DG);
std::vector<std::size_t> columns;
std::vector<double> values;
std::vector<std::vector<std::size_t> > allcolumns;
std::vector<std::vector<double> > allvalues;
const std::pair<std::size_t, std::size_t> row_range = A.local_range(0);
const std::size_t m = row_range.second - row_range.first;
GenericVector& weight = *DG.vector();
const std::pair<std::size_t, std::size_t> weight_range = weight.local_range();
std::vector<std::size_t> weight_range_vec(2);
weight_range_vec[0] = weight_range.first;
weight_range_vec[1] = weight_range.second;
int dm = weight_range.second-weight_range.first;
const MPI_Comm mpi_comm = DG.function_space()->mesh()->mpi_comm();
// Communicate local_ranges of weights
std::vector<std::vector<std::size_t> > all_ranges;
MPI::all_gather(mpi_comm, weight_range_vec, all_ranges);
// Number of MPI processes
std::size_t num_processes = MPI::size(mpi_comm);
// Some weights live on other processes and need to be communicated
// Create list of off-process weights
std::vector<std::vector<std::size_t> > dofs_needed(num_processes);
for (std::size_t row = 0; row < m; row++)
{
// Get global row number
const std::size_t global_row = row + row_range.first;
A.getrow(global_row, columns, values);
for (std::size_t i = 0; i < columns.size(); i++)
{
std::size_t dof = columns[i];
if (dof < weight_range.first || dof >= weight_range.second)
{
std::size_t owner = dof_owner(all_ranges, dof);
dofs_needed[owner].push_back(dof);
}
}
}
// Communicate to all which weights are needed by the process
std::vector<std::vector<std::size_t> > dofs_needed_recv;
MPI::all_to_all(mpi_comm, dofs_needed, dofs_needed_recv);
// Fetch the weights that must be communicated
std::vector<std::vector<double> > weights_to_send(num_processes);
for (std::size_t p = 0; p < num_processes; p++)
{
if (p == MPI::rank(mpi_comm))
continue;
std::vector<std::size_t> dofs = dofs_needed_recv[p];
std::map<std::size_t, double> send_weights;
for (std::size_t k = 0; k < dofs.size(); k++)
{
weights_to_send[p].push_back(weight[dofs[k]-weight_range.first]);
}
}
std::vector<std::vector<double> > weights_to_send_recv;
MPI::all_to_all(mpi_comm, weights_to_send, weights_to_send_recv);
// Create a map for looking up received weights
std::map<std::size_t, double> received_weights;
for (std::size_t p = 0; p < num_processes; p++)
{
if (p == MPI::rank(mpi_comm))
continue;
for (std::size_t k = 0; k < dofs_needed[p].size(); k++)
{
received_weights[dofs_needed[p][k]] = weights_to_send_recv[p][k];
}
}
for (std::size_t row = 0; row < m; row++)
{
// Get global row number
const std::size_t global_row = row + row_range.first;
A.getrow(global_row, columns, values);
for (std::size_t i = 0; i < values.size(); i++)
{
std::size_t dof = columns[i];
if (dof < weight_range.first || dof >= weight_range.second)
{
values[i] = received_weights[dof];
}
else
{
values[i] = weight[columns[i]-weight_range.first];
}
// values[i] = 1./values[i];
}
double s = std::accumulate(values.begin(), values.end(), 0.0);
std::transform(values.begin(), values.end(), values.begin(),
std::bind2nd(std::multiplies<double>(), 1./s));
for (std::size_t i=0; i<values.size(); i++)
{
double w;
std::size_t dof = columns[i];
if (dof < weight_range.first || dof >= weight_range.second)
{
w = received_weights[dof];
}
else
{
w = weight[dof-weight_range.first];
}
values[i] = values[i]*w;
// values[i] = values[i]*values[i];
}
allvalues.push_back(values);
allcolumns.push_back(columns);
}
for (std::size_t row = 0; row < m; row++)
{
// Get global row number
const std::size_t global_row = row + row_range.first;
A.setrow(global_row, allcolumns[row], allvalues[row]);
}
A.apply("insert");
}
int ILSConjugateGradientsLanczos::solveLin ( const GenericMatrix & gm, const Vector & b, Vector & x )
{
if ( b.size() != gm.rows() ) {
fthrow(Exception, "Size of vector b (" << b.size() << ") mismatches with the size of the given GenericMatrix (" << gm.rows() << ").");
}
if ( x.size() != gm.cols() )
{
x.resize(gm.cols());
x.set(0.0); // bad initial solution, but whatever
}
// if ( verbose ) cerr << "initial solution: " << x << endl;
// CG-Method based on Lanczos vectors: implementation based on the following:
//
// C.C. Paige and M.A. Saunders: "Solution of sparse indefinite systems of linear equations". SIAM Journal on Numerical Analysis, p. 617--629, vol. 12, no. 4, 1975
//
// http://www.netlib.org/templates/templates.pdf
//
// init some helping vectors
Vector Av(b.size(),0.0); // Av = A * v_j
Vector Ac(b.size(),0.0); // Ac = A * c_j
Vector r(b.size(),0.0); // current residual
Vector *v_new = new Vector(x.size(),0.0); // new Lanczos vector v_j
Vector *v_old = new Vector(x.size(),0.0); // Lanczos vector v_{j-1} of the iteration before
Vector *v_older = 0; // Lanczos vector v_{j-2} of the iteration before
Vector *c_new = new Vector(x.size(),0.0); // current update vector c_j for the solution x
Vector *c_old = 0; // update vector of iteration before
// declare some helpers
double d_new = 0; // current element of diagonal matrix D normally obtained from Cholesky factorization of tridiagonal matrix T, where T consists alpha and beta as below
double d_old = 0; // corresponding element of the iteration before
double l_new = 0; // current element of lower unit bidiagonal matrix L normally obtained from Cholesky factorization of tridiagonal matrix T
double p_new = 0; // current element of vector p, where p is the solution of the modified linear system
double p_old = 0; // corresponding element of the iteration before
double alpha = 0; // alpha_j = v_j^T * A * v_j for new Lanczos vector v_j
double beta = b.normL2(); // beta_1 = norm(b), in general beta_j = norm(v_j) for new Lanczos vector v_j
// first iteration + initialization, where b will be used as the first Lanczos vector
*v_new = (1/beta)*b; // init v_1, v_1 = b / norm(b)
gm.multiply(Av,*v_new); // Av = A * v_1
alpha = v_new->scalarProduct(Av); // alpha_1 = v_1^T * A * v_1
d_new=alpha; // d_1 = alpha_1, d_1 is the first element of diagonal matrix D
p_new = beta/d_new; // p_1 = beta_1 / d_1
*c_new = *v_new; // c_1 = v_1
Ac = Av; // A*c_1 = A*v_1
// store current solution
Vector current_x = (p_new*(*c_new)); // first approx. of x: x_1 = p_1 * c_1
// calculate current residual
r = b - (p_new*Ac);
double res = r.scalarProduct(r);
// store minimal residual
double res_min = res;
// store optimal solution in output variable x
x = current_x;
double delta_x = fabs(p_new) * c_new->normL2();
if ( verbose ) {
cerr << "ILSConjugateGradientsLanczos: iteration 1 / " << maxIterations << endl;
if ( current_x.size() <= 20 )
cerr << "ILSConjugateGradientsLanczos: current solution " << current_x << endl;
cerr << "ILSConjugateGradientsLanczos: delta_x = " << delta_x << endl;
cerr << "ILSConjugateGradientsLanczos: residual = " << r.scalarProduct(r) << endl;
}
// start with second iteration
uint j = 2;
while (j <= maxIterations )
{
// prepare d and p for next iteration
d_old = d_new;
p_old = p_new;
// prepare vectors v_older, v_old, v_new for next iteration
if ( v_older == 0) v_older = v_old;
else {
delete v_older;
v_older = v_old;
}
v_old = v_new;
v_new = new Vector(v_old->size(),0.0);
// prepare vectors c_old, c_new for next iteration
if ( c_old == 0 ) c_old = c_new;
else {
delete c_old;
c_old = c_new;
}
c_new = new Vector(c_old->size(),0.0);
//start next iteration:
// calulate new Lanczos vector v_j based on older ones
*v_new = Av - (alpha*(*v_old)) - (beta*(*v_older)); // unnormalized v_j = ( A * v_{j-1} ) - ( alpha_{j-1} * v_{j-1} ) - ( beta_{j-1} * v_{j-2} )
// calculate new weight beta_j and normalize v_j
beta = v_new->normL2(); // beta_j = norm(v_j)
v_new->normalizeL2(); // normalize v_j
// calculate new weight alpha_j
gm.multiply(Av,*v_new); // Av = A * v_j
alpha = v_new->scalarProduct(Av); // alpha_j = v_j^T * A * v_j
// calculate l_j and d_j as the elements of the Cholesky Factorization of current tridiagonal matrix T, where T = L * D * L^T with diagonal matrix D and
// lower bidiagonal matrix L; l_j and d_j are necessary for computing the new update vector c_j for the solution x and the corresponding weight
l_new = beta/sqrt(d_old); // unnormalized l_j = beta_j / sqrt(d_{j-1})
d_new = alpha-(l_new*l_new); // d_j = alpha_j - l_j^2
l_new/=sqrt(d_old); // normalize l_j by sqrt(d_{j-1})
// calculate the new weight p_j of the new update vector c_j for the solution x
p_new = -p_old*l_new*d_old/d_new;
// calculate the new update vector c_j for the solution x
*c_new = *v_new - (l_new*(*c_old));
// calculate new residual vector
Ac = Av - (l_new*Ac);
r-=p_new*Ac;
res = r.scalarProduct(r);
// update solution x
current_x+=(p_new*(*c_new));
if ( verbose ) {
cerr << "ILSConjugateGradientsLanczos: iteration " << j << " / " << maxIterations << endl;
if ( current_x.size() <= 20 )
cerr << "ILSConjugateGradientsLanczos: current solution " << current_x << endl;
}
// store optimal x that produces smallest residual
if (res < res_min) {
x = current_x;
res_min = res;
}
// check convergence
delta_x = fabs(p_new) * c_new->normL2();
if ( verbose ) {
cerr << "ILSConjugateGradientsLanczos: delta_x = " << delta_x << endl;
cerr << "ILSConjugateGradientsLanczos: residual = " << r.scalarProduct(r) << endl;
}
if ( delta_x < minDelta ) {
if ( verbose )
cerr << "ILSConjugateGradientsLanczos: small delta_x" << endl;
break;
}
j++;
}
// if ( verbose ) {
cerr << "ILSConjugateGradientsLanczos: iterations needed: " << std::min<uint>(j,maxIterations) << endl;
cerr << "ILSConjugateGradientsLanczos: minimal residual achieved: " << res_min << endl;
if ( x.size() <= 20 )
cerr << "ILSConjugateGradientsLanczos: optimal solution: " << x << endl;
// }
delete v_new;
delete v_old;
delete v_older;
delete c_new;
delete c_old;
return 0;
}
