//----------------------------------------------------------------------------- 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; }