M & sr2 (M &m, const T &t, const V1 &v1, const V2 &v2) { #ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG return m += t * (outer_prod (v1, v2) + outer_prod (v2, v1)); #else return m = m + t * (outer_prod (v1, v2) + outer_prod (v2, v1)); #endif }
M & hr (M &m, const T &t, const V &v) { #ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG return m += t * outer_prod (v, conj (v)); #else return m = m + t * outer_prod (v, conj (v)); #endif }
M & hr (M &m, const T &t, const V &v) { #ifdef BOOST_UBLAS_USE_ET return m += t * outer_prod (v, conj (v)); #else return m = m + t * outer_prod (v, conj (v)); #endif }
M & sr2 (M &m, const T &t, const V1 &v1, const V2 &v2) { #ifdef BOOST_UBLAS_USE_ET return m += t * (outer_prod (v1, v2) + outer_prod (v2, v1)); #else return m = m + t * (outer_prod (v1, v2) + outer_prod (v2, v1)); #endif }
M & hr2 (M &m, const T &t, const V1 &v1, const V2 &v2) { #ifndef BOOST_UBLAS_SIMPLE_ET_DEBUG return m += t * outer_prod (v1, conj (v2)) + type_traits<T>::conj (t) * outer_prod (v2, conj (v1)); #else return m = m + t * outer_prod (v1, conj (v2)) + type_traits<T>::conj (t) * outer_prod (v2, conj (v1)); #endif }
M & gr (M &m, const T &t, const V1 &v1, const V2 &v2) { #ifdef BOOST_UBLAS_USE_ET return m += t * outer_prod (v1, v2); #else return m = m + t * outer_prod (v1, v2); #endif }
/// rotation about origin defined by axis and angle (radians) Transformation Transformation::rotation(const Vector3d& axis, double radians) { Matrix storage = identity_matrix<double>(4); Vector3d temp = axis; if (!temp.normalize()){ LOG(Error, "Could not normalize axis"); } Vector normalVector = temp.vector(); // Rodrigues' rotation formula / Rotation matrix from Euler axis/angle // I*cos(radians) + I*(1-cos(radians))*axis*axis^T + Q*sin(radians) // Q = [0, -axis[2], axis[1]; axis[2], 0, -axis[0]; -axis[1], axis[0], 0] Matrix P = outer_prod(normalVector, normalVector); Matrix I = identity_matrix<double>(3); Matrix Q(3, 3, 0.0); Q(0,1) = -normalVector(2); Q(0,2) = normalVector(1); Q(1,0) = normalVector(2); Q(1,2) = -normalVector(0); Q(2,0) = -normalVector(1); Q(2,1) = normalVector(0); // rotation matrix Matrix R = I*cos(radians) + (1-cos(radians))*P + Q*sin(radians); for (unsigned i = 0; i < 3; ++i){ for (unsigned j = 0; j < 3; ++j){ storage(i,j) = R(i,j); } } return Transformation(storage); }
main(int argc, char* argv[]){ int k,n,i,j; std::string op=argv[1], prob=argv[3]; std::istringstream tin(argv[2]); val t; tin>>t; val dx=1.0/K, dt=(val)T/N, xk, tn, tmp; val R=NU*dt/dx; // R = NU*dt/dx val alpha, beta; static banded_matrix<val> U(K+2, K+2, 2, 4); vector<fortran_int_t> p(K+2); static vec u(K+2), w(K+2), z(K+2), v(K+2); if(prob=="a"){ // Tridiagonal Matrix U for(i=0; i<U.size1(); i++){ U(i,i)=1.0-R-2*r; k=std::max(i-1,0); U(k,k+1)=r; U(k+1,k)=R+r; } // Boundary Conditions U(0,0)+=r; U(K+1,K+1)+=(R+r); w(0)=1.0; w(K+1)=-1.0; z(0)=r; z(K+1)=-(R+r); // Sherman-Morrison } // Test Boundary Conditions if(op=="test"){ mat wzt = outer_prod(w, z); mat B = U - wzt; printf("Matrix Q1\n"); matprintf(B); printf("Matrix B\n"); matprintf(U); printf("Matrix wz^T\n"); matprintf(wzt); } // Initial conditions for(k=0;k<u.size();k++){ xk=k*dx; u(k)=f(xk); } lapack::gbtrf(U, p); // LU-decompostion lapack::gbtrs(U, p, w); //B^-1w alpha=1.0/(1.0-inner_prod(z, w)); // alpha = 1/(1-z^T(b^-1w)) for(n=0;n<=N;n++){ tn=n*dt; lapack::gbtrs(U, p, u); // B^-1u beta=alpha*inner_prod(z, u); // beta = alpha*z^T(B^-1u) u+=beta*w; if(op=="pipe") plotu(u, tn, K); if(op=="approx" && tn==t) printu(u, tn, K); } return 0; }
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]){ double r1, r2, K; /* input scalars */ char *kID_1, *kID_2; /* input strings */ double *outMatrix; /* output matirx */ r1 = mxGetScalar(prhs[0]); kID_1 = mxArrayToString(prhs[1]); r2 = mxGetScalar(prhs[2]); kID_2 = mxArrayToString(prhs[3]); K = mxGetScalar(prhs[4]); plhs[0] = mxCreateDoubleMatrix((mwSize)w_SIZE, (mwSize)w_SIZE, mxREAL); outMatrix = mxGetPr(plhs[0]); double v1[] = {-2-r1, -1-r1, -r1, 1-r1, 2-r1, 3-r1}; double v2[] = {-2-r2, -1-r2, -r2, 1-r2, 2-r2, 3-r2}; phiweights(v1, kID_1, K); phiweights(v2, kID_2, K); mxFree(kID_1); mxFree(kID_2); /* free variables */ outer_prod(v1,v2, outMatrix, (mwSize)w_SIZE); }
void InfiniteDomainCondition<TDim,TNumNodes>::CalculateRHS( VectorType& rRightHandSideVector, const ProcessInfo& CurrentProcessInfo ) { KRATOS_TRY //const PropertiesType& Prop = this->GetProperties(); const GeometryType& Geom = this->GetGeometry(); const unsigned int element_size = TNumNodes; const GeometryType::IntegrationPointsArrayType& integration_points = Geom.IntegrationPoints( mThisIntegrationMethod ); const unsigned int NumGPoints = integration_points.size(); const unsigned int LocalDim = Geom.LocalSpaceDimension(); //Resetting the RHS if ( rRightHandSideVector.size() != element_size ) rRightHandSideVector.resize( element_size, false ); noalias( rRightHandSideVector ) = ZeroVector( element_size ); boost::numeric::ublas::bounded_matrix<double,TNumNodes,TNumNodes> DampingMatrix; //Defining the shape functions, the jacobian and the shape functions local gradients Containers array_1d<double,TNumNodes> Np; const Matrix& NContainer = Geom.ShapeFunctionsValues( mThisIntegrationMethod ); GeometryType::JacobiansType JContainer(NumGPoints); for(unsigned int i = 0; i<NumGPoints; i++) (JContainer[i]).resize(TDim,LocalDim,false); Geom.Jacobian( JContainer, mThisIntegrationMethod ); double IntegrationCoefficient; // Definition of the speed in the fluid //~ const double BulkModulus = Prop[BULK_MODULUS_FLUID]; //~ const double Water_density = Prop[DENSITY_WATER]; const double BulkModulus = 2.21e9; const double Water_density = 1000.0; const double inv_c_speed = 1.0 /sqrt(BulkModulus/Water_density); //Nodal Variables array_1d<double,TNumNodes> DtPressureVector; for(unsigned int i=0; i<TNumNodes; i++) { DtPressureVector[i] = Geom[i].FastGetSolutionStepValue(Dt_PRESSURE); } for ( unsigned int igauss = 0; igauss < NumGPoints; igauss++ ) { noalias(Np) = row(NContainer,igauss); //calculating weighting coefficient for integration this->CalculateIntegrationCoefficient( IntegrationCoefficient, JContainer[igauss], integration_points[igauss].Weight() ); // Mass matrix contribution noalias(DampingMatrix) = (inv_c_speed)*outer_prod(Np,Np)*IntegrationCoefficient; noalias(rRightHandSideVector) += -1.0*prod(DampingMatrix,DtPressureVector); } KRATOS_CATCH( "" ) }
c_vector<double,3> CalculateEigenvectorForSmallestNonzeroEigenvalue(c_matrix<double, 3, 3>& rA) { //Check for symmetry if (norm_inf( rA - trans(rA)) > 10*DBL_EPSILON) { EXCEPTION("Matrix should be symmetric"); } // Find the eigenvector by brute-force using the power method. // We can't use the inverse method, because the matrix might be singular c_matrix<double,3,3> copy_A(rA); //Eigenvalue 1 c_vector<double, 3> eigenvec1 = scalar_vector<double>(3, 1.0); double eigen1 = CalculateMaxEigenpair(copy_A, eigenvec1); // Take out maximum eigenpair c_matrix<double, 3, 3> wielandt_reduce_first_vector = identity_matrix<double>(3,3); wielandt_reduce_first_vector -= outer_prod(eigenvec1, eigenvec1); copy_A = prod(wielandt_reduce_first_vector, copy_A); c_vector<double, 3> eigenvec2 = scalar_vector<double>(3, 1.0); double eigen2 = CalculateMaxEigenpair(copy_A, eigenvec2); // Take out maximum (second) eigenpair c_matrix<double, 3, 3> wielandt_reduce_second_vector = identity_matrix<double>(3,3); wielandt_reduce_second_vector -= outer_prod(eigenvec2, eigenvec2); copy_A = prod(wielandt_reduce_second_vector, copy_A); c_vector<double, 3> eigenvec3 = scalar_vector<double>(3, 1.0); double eigen3 = CalculateMaxEigenpair(copy_A, eigenvec3); //Look backwards through the eigenvalues, checking that they are non-zero if (eigen3 >= DBL_EPSILON) { return eigenvec3; } if (eigen2 >= DBL_EPSILON) { return eigenvec2; } UNUSED_OPT(eigen1); assert( eigen1 > DBL_EPSILON); return eigenvec1; }
/** * For a continuum mechanics problem in mixed form (displacement-pressure or velocity-pressure), the matrix * has the form (except see comments about ordering above) * [A B1] * [B2^T C ] * The function is related to the pressure-pressure block, i.e. C * * @return C=M, the mass matrix. * * @param rLinearPhi All the linear basis functions on this element, evaluated at the current quad point * @param rGradLinearPhi Gradients of all the linear basis functions on this element, evaluated at the current quad point * @param rX Current location (physical position) * @param pElement Current element */ c_matrix<double,PRESSURE_BLOCK_SIZE_ELEMENTAL,PRESSURE_BLOCK_SIZE_ELEMENTAL> ComputePressurePressureMatrixTerm( c_vector<double, NUM_VERTICES_PER_ELEMENT>& rLinearPhi, c_matrix<double, DIM, NUM_VERTICES_PER_ELEMENT>& rGradLinearPhi, c_vector<double,DIM>& rX, Element<DIM,DIM>* pElement) { return outer_prod(rLinearPhi,rLinearPhi); }
vector<T> DavidonFletcherPowell(const Function<T> & f, vector<T> x, const double epsilon, std::size_t & number_iteration) { BOOST_ASSERT(f.function); BOOST_ASSERT(f.gradient); vector<T> grad(f.gradient(x)); matrix<T> eta(identity_matrix<T>(f.size)); vector<T> old_x = x; vector<T> old_grad = grad; vector<T> d; T alpha = T(0); vector<T> delta_x; vector<T> delta_g; matrix<T> A; matrix<T> B; for (; ublas::norm_2(grad) >= epsilon && number_iteration < 1E6; old_x = x, old_grad = grad, ++number_iteration) { d = -prod(eta, grad); // Looking for the minimum of F(x - alpha * d) using the method of one-dimensional optimization. alpha = details::find_min(f, x, d, T(0.0), T(1E3)); BOOST_ASSERT(alpha >= DBL_EPSILON); x = x + alpha * d; grad = f.gradient(x); delta_x = x - old_x; delta_g = grad - old_grad; A = (outer_prod(delta_x, delta_x) / inner_prod(delta_x, delta_g)); B = prod(outer_prod(static_cast<vector<T>>(prod(eta, delta_g)), delta_g), trans(eta)) / inner_prod(static_cast<vector<T>>(prod(static_cast<vector<T>>(prod(identity_matrix<T>(f.size), delta_g)), eta)), delta_g); eta = eta + A - B; } return x; }
matrix< double > &SR1Updater::update(const vector< double > &p, const vector< double > &q, matrix< double > &H) { vector< double > t = q - prod(H, p); double pt = inner_prod(p, t); if(fabs(pt) >= 1e-8 * norm_2(p)*norm_2(t)) H += outer_prod(t, t) / pt; return H; }
ublas::matrix<double> dp2(const ublas::vector<double>& p) const { ublas::matrix<double> result(parameterCount(), parameterCount()); result.clear(); for (typename Data::const_iterator it=data_.begin(); it!=data_.end(); ++it) { std::complex<double> diffconj = conj(std::complex<double>(f_(it->x, p) - it->y)); ublas::vector<value_type> dp = f_.dp(it->x,p); ublas::matrix<value_type> dp2 = f_.dp2(it->x,p); result += 2 * real(diffconj*dp2 + outer_prod(conj(dp),dp)); } return result; }
mat hessenberg(mat A, int n, int m){ unsigned int i,j,k; mat D(n,m),C(n,m),E(n,m),B(n,m),U(n,m),Uconj(n,m),temp(n,m); identity_matrix<val> I(n); zero_matrix<val> zeros(n,n); vec e1(m), v(m); val beta,alpha; e1.clear();e1(0)=1.0; printf("\n Reducing....\n"); for(k=0;k<A.size2()-2;k++){ printf(" Iteration %d\n",k+1); B=project(A,range(0,k+1),range(0,k+1)); D.clear();D.resize(n-k-1,k+1); project(D,range(0,n-k-1),range(k,k+1))=project(A,range(k+1,n),range(k,k+1)); printf(" d= %4.2s"," ");vecprintf(vec(column(D,k))); e1.resize(D.size1()); C=project(A,range(0,k+1),range(k+1,m)); E=project(A,range(k+1,n),range(k+1,m)); beta=-1.0*(D(0,k)/abs(D(0,k)))*norm_2(vec(column(D,k))); printf(" beta= %15.8lf\n",beta); alpha=sqrt(2)/norm_2(vec(column(D,k))-beta*e1); printf(" alpha= %15.8lf\n",alpha); v=alpha*(vec(column(D,k))-beta*e1); printf(" v= %4.2s"," ");vecprintf(v); I.resize(B.size1()); U.clear();Uconj.clear(); project(U,range(0,k+1),range(0,k+1))=I; project(Uconj,range(0,k+1),range(0,k+1))=I; I.resize(v.size()); mat vmat=I-outer_prod(v,conj(v)); printf(" U= \n");matprintf(vmat); project(U,range(k+1,n),range(k+1,m))=I-outer_prod(v,conj(v)); project(Uconj,range(k+1,n),range(k+1,m))=conj(I-outer_prod(v,conj(v))); temp=prod(A,conj(trans(U)));A=prod(U,temp); printf(" UAU*= \n");matprintf(A);printf("\n"); }printf("\n"); return A; }
ublas::vector<double> mvnormpdf(const ublas::matrix<double>& x, const ublas::vector<double>& mu, const ublas::matrix<double>& Omega) { //! Multivariate normal density size_t p = x.size1(); size_t n = x.size2(); double f = sqrt(det(Omega))/pow(2.0*PI, p/2.0); // cout << "O: " << Omega << "\n"; // cout << "f: " << f << "\n"; ublas::matrix<double> e(p, n); e.assign(x - outer_prod(mu, ublas::scalar_vector<double>(n, 1))); e = element_prod(e, prod(Omega, e)); return ublas::apply_to_all<functor::exp<double> > (-(ublas::matrix_sum(e, 0))/2.0)*f; }
void Stats::Impl::computeSums(const Stats::data_type& data) { if (data.size()>0) D_ = data[0].size(); sumData_ = Stats::vector_type(D_); sumOuterProducts_ = Stats::matrix_type(D_, D_); sumData_.clear(); sumOuterProducts_.clear(); for (Stats::data_type::const_iterator it=data.begin(); it!=data.end(); ++it) { if (it->size() != D_) { ostringstream message; message << "[Stats::Impl::computeSums()] " << D_ << "-dimensional data expected: " << *it; throw runtime_error(message.str()); } sumData_ += *it; sumOuterProducts_ += outer_prod(*it, *it); } }
void SolverUnconstrained<Data,Problem>::makeStep( vector_type & _x, vector_type & _s, value_type _Delta, f_type& __fx, vector_type & _Tgrad_fx, banded_matrix_type & _Hg, symmetric_matrix_type & _Thess_fxT, symmetric_matrix_type & _Htil ) { M_theta.update( _Delta, _x, _s ); _Tgrad_fx = prod( M_theta(), __fx.gradient( 0 ) ); banded_matrix_type __diag_grad_fx( _E_n, _E_n, 0, 0 ); matrix<value_type> __m( outer_prod( _Tgrad_fx, scalar_vector<value_type>( _Tgrad_fx.size(), 1 ) ) ); __diag_grad_fx = banded_adaptor<matrix<value_type> >( __m, 0, 0 ); _Hg = prod( M_theta.jacobian(), __diag_grad_fx ); _Thess_fxT = prod( M_theta(), prod( __fx.hessian( 0 ), M_theta() ) ); _Htil = _Thess_fxT + _Hg; }
void Bayes::sample_muOmega() { double tau = 10.0; int d0 = p + 2; // matrix_t S0(p, p); ublas::matrix<double> S0(p, p); S0 = d0 * ublas::identity_matrix<double>(p, p)/4.0; std::vector<int> idx; ublas::indirect_array<> irow(p); // projection - want every row for (size_t i=0; i<irow.size(); ++i) irow(i) = i; // size_t rank; ublas::matrix<double> S(p, p); // matrix_t SS(p, p); ublas::matrix<double> SS(p, p); ublas::matrix<double> Omega_inv(p, p); // identity matrix for inverting cholesky factorization ublas::matrix<double> I(p, p); I.assign(ublas::identity_matrix<double> (p, p)); ublas::symmetric_adaptor<ublas::matrix<double>, ublas::upper> SH(I); // triangular matrix for cholesky_decompose ublas::triangular_matrix<double, ublas::lower, ublas::row_major> L(p, p); int df; for (int j=0; j<k; ++j) { idx = find_all(z, j); int n_idx = idx.size(); ublas::matrix<double> xx(p, n_idx); ublas::matrix<double> e(p, n_idx); // ublas::matrix<double> m(p, 1); ublas::vector<double> m(p); ublas::indirect_array<> icol(n_idx); for (size_t i=0; i<idx.size(); ++i) icol(i) = idx[i]; if (n_idx > 0) { double a = tau/(1.0 + n_idx*tau); //! REFACTOR - should be able to do matrix_sum directly on projection rather than make a copy? xx.assign(project(x, irow, icol)); m.assign(ublas::matrix_sum(xx, 1)/n_idx); // e.assign(xx - outer_prod(column(m, 0), ublas::scalar_vector<double>(n_idx, 1))); e.assign(xx - outer_prod(m, ublas::scalar_vector<double>(n_idx, 1))); S.assign(prod(e, trans(e)) + outer_prod(m, m) * n_idx * a/tau); // SS = trans(S) + S0; SS = S + S0; df = d0 + n_idx; // Omega(j).assign(wishart_rnd(df, SS)); Omega(j).assign(wishart_InvA_rnd(df, SS)); SH.assign(Omega(j)); cholesky_decompose(SH, L); Omega_inv.assign(solve(SH, I, ublas::upper_tag())); mu(j).assign(a*n_idx*m + sqrt(a)*prod(Omega_inv, Rmath::rnorm(0, 1, p))); } else { // Omega(j).assign(wishart_rnd(d0, S0)); Omega(j).assign(wishart_InvA_rnd(d0, S0)); SH.assign(Omega(j)); cholesky_decompose(SH, L); Omega_inv.assign(solve(SH, I, ublas::upper_tag())); mu(j).assign(sqrt(tau) * prod(Omega_inv, Rmath::rnorm(0, 1, p))); } } }
VectorType solve(const MatrixType & matrix, VectorType const & rhs, gmres_tag const & tag, PreconditionerType const & precond) { typedef typename viennacl::result_of::value_type<VectorType>::type ScalarType; typedef typename viennacl::result_of::cpu_value_type<ScalarType>::type CPU_ScalarType; unsigned int problem_size = viennacl::traits::size(rhs); VectorType result(problem_size); viennacl::traits::clear(result); unsigned int krylov_dim = tag.krylov_dim(); if (problem_size < tag.krylov_dim()) krylov_dim = problem_size; //A Krylov space larger than the matrix would lead to seg-faults (mathematically, error is certain to be zero already) VectorType res(problem_size); VectorType v_k_tilde(problem_size); VectorType v_k_tilde_temp(problem_size); std::vector< std::vector<CPU_ScalarType> > R(krylov_dim); std::vector<CPU_ScalarType> projection_rhs(krylov_dim); std::vector<VectorType> U(krylov_dim); const CPU_ScalarType gpu_scalar_minus_1 = static_cast<CPU_ScalarType>(-1); //representing the scalar '-1' on the GPU. Prevents blocking write operations const CPU_ScalarType gpu_scalar_1 = static_cast<CPU_ScalarType>(1); //representing the scalar '1' on the GPU. Prevents blocking write operations const CPU_ScalarType gpu_scalar_2 = static_cast<CPU_ScalarType>(2); //representing the scalar '2' on the GPU. Prevents blocking write operations CPU_ScalarType norm_rhs = viennacl::linalg::norm_2(rhs); if (norm_rhs == 0) //solution is zero if RHS norm is zero return result; unsigned int k; for (k = 0; k < krylov_dim; ++k) { R[k].resize(tag.krylov_dim()); viennacl::traits::resize(U[k], problem_size); } //std::cout << "Starting GMRES..." << std::endl; tag.iters(0); for (unsigned int it = 0; it <= tag.max_restarts(); ++it) { //std::cout << "-- GMRES Start " << it << " -- " << std::endl; res = rhs; res -= viennacl::linalg::prod(matrix, result); //initial guess zero precond.apply(res); //std::cout << "Residual: " << res << std::endl; CPU_ScalarType rho_0 = viennacl::linalg::norm_2(res); CPU_ScalarType rho = static_cast<CPU_ScalarType>(1.0); //std::cout << "rho_0: " << rho_0 << std::endl; if (rho_0 / norm_rhs < tag.tolerance() || (norm_rhs == CPU_ScalarType(0.0)) ) { //std::cout << "Allowed Error reached at begin of loop" << std::endl; tag.error(rho_0 / norm_rhs); return result; } res /= rho_0; //std::cout << "Normalized Residual: " << res << std::endl; for (k=0; k<krylov_dim; ++k) { viennacl::traits::clear(R[k]); viennacl::traits::clear(U[k]); R[k].resize(krylov_dim); viennacl::traits::resize(U[k], problem_size); } for (k = 0; k < krylov_dim; ++k) { tag.iters( tag.iters() + 1 ); //increase iteration counter //compute v_k = A * v_{k-1} via Householder matrices if (k == 0) { v_k_tilde = viennacl::linalg::prod(matrix, res); precond.apply(v_k_tilde); } else { viennacl::traits::clear(v_k_tilde); v_k_tilde[k-1] = gpu_scalar_1; //Householder rotations part 1 for (int i = k-1; i > -1; --i) v_k_tilde -= U[i] * (viennacl::linalg::inner_prod(U[i], v_k_tilde) * gpu_scalar_2); v_k_tilde_temp = viennacl::linalg::prod(matrix, v_k_tilde); precond.apply(v_k_tilde_temp); v_k_tilde = v_k_tilde_temp; //Householder rotations part 2 for (unsigned int i = 0; i < k; ++i) v_k_tilde -= U[i] * (viennacl::linalg::inner_prod(U[i], v_k_tilde) * gpu_scalar_2); } //std::cout << "v_k_tilde: " << v_k_tilde << std::endl; viennacl::traits::clear(U[k]); viennacl::traits::resize(U[k], problem_size); //copy first k entries from v_k_tilde to U[k]: gmres_copy_helper(v_k_tilde, U[k], k); U[k][k] = std::sqrt( viennacl::linalg::inner_prod(v_k_tilde, v_k_tilde) - viennacl::linalg::inner_prod(U[k], U[k]) ); if (fabs(U[k][k]) < CPU_ScalarType(10 * std::numeric_limits<CPU_ScalarType>::epsilon())) break; //Note: Solution is essentially (up to round-off error) already in Krylov space. No need to proceed. //copy first k+1 entries from U[k] to R[k] gmres_copy_helper(U[k], R[k], k+1); U[k] -= v_k_tilde; //std::cout << "U[k] before normalization: " << U[k] << std::endl; U[k] *= gpu_scalar_minus_1 / viennacl::linalg::norm_2( U[k] ); //std::cout << "Householder vector U[k]: " << U[k] << std::endl; //DEBUG: Make sure that P_k v_k_tilde equals (rho_{1,k}, ... , rho_{k,k}, 0, 0 ) #ifdef VIENNACL_GMRES_DEBUG std::cout << "P_k v_k_tilde: " << (v_k_tilde - 2.0 * U[k] * inner_prod(U[k], v_k_tilde)) << std::endl; std::cout << "R[k]: [" << R[k].size() << "]("; for (size_t i=0; i<R[k].size(); ++i) std::cout << R[k][i] << ","; std::cout << ")" << std::endl; #endif //std::cout << "P_k res: " << (res - 2.0 * U[k] * inner_prod(U[k], res)) << std::endl; res -= U[k] * (viennacl::linalg::inner_prod( U[k], res ) * gpu_scalar_2); //std::cout << "zeta_k: " << viennacl::linalg::inner_prod( U[k], res ) * gpu_scalar_2 << std::endl; //std::cout << "Updated res: " << res << std::endl; #ifdef VIENNACL_GMRES_DEBUG VectorType v1(U[k].size()); v1.clear(); v1.resize(U[k].size()); v1(0) = 1.0; v1 -= U[k] * (viennacl::linalg::inner_prod( U[k], v1 ) * gpu_scalar_2); std::cout << "v1: " << v1 << std::endl; boost::numeric::ublas::matrix<ScalarType> P = -2.0 * outer_prod(U[k], U[k]); P(0,0) += 1.0; P(1,1) += 1.0; P(2,2) += 1.0; std::cout << "P: " << P << std::endl; #endif if (res[k] > rho) //machine precision reached res[k] = rho; if (res[k] < -1.0 * rho) //machine precision reached res[k] = -1.0 * rho; projection_rhs[k] = res[k]; rho *= std::sin( std::acos(projection_rhs[k] / rho) ); #ifdef VIENNACL_GMRES_DEBUG std::cout << "k-th component of r: " << res[k] << std::endl; std::cout << "New rho (norm of res): " << rho << std::endl; #endif if (std::fabs(rho * rho_0 / norm_rhs) < tag.tolerance()) { //std::cout << "Krylov space big enough" << endl; tag.error( std::fabs(rho*rho_0 / norm_rhs) ); ++k; break; } //std::cout << "Current residual: " << rho * rho_0 << std::endl; //std::cout << " - End of Krylov space setup - " << std::endl; } // for k #ifdef VIENNACL_GMRES_DEBUG //inplace solution of the upper triangular matrix: std::cout << "Upper triangular system:" << std::endl; std::cout << "Size of Krylov space: " << k << std::endl; for (size_t i=0; i<k; ++i) { for (size_t j=0; j<k; ++j) { std::cout << R[j][i] << ", "; } std::cout << " | " << projection_rhs[i] << std::endl; } #endif for (int i=k-1; i>-1; --i) { for (unsigned int j=i+1; j<k; ++j) //temp_rhs[i] -= R[i][j] * temp_rhs[j]; //if R is not transposed projection_rhs[i] -= R[j][i] * projection_rhs[j]; //R is transposed projection_rhs[i] /= R[i][i]; } #ifdef VIENNACL_GMRES_DEBUG std::cout << "Result of triangular solver: "; for (size_t i=0; i<k; ++i) std::cout << projection_rhs[i] << ", "; std::cout << std::endl; #endif res *= projection_rhs[0]; if (k > 0) { for (unsigned int i = 0; i < k-1; ++i) { res[i] += projection_rhs[i+1]; } } for (int i = k-1; i > -1; --i) res -= U[i] * (viennacl::linalg::inner_prod(U[i], res) * gpu_scalar_2); res *= rho_0; result += res; if ( std::fabs(rho*rho_0 / norm_rhs) < tag.tolerance() ) { //std::cout << "Allowed Error reached at end of loop" << std::endl; tag.error(std::fabs(rho*rho_0 / norm_rhs)); return result; } //res = rhs; //res -= viennacl::linalg::prod(matrix, result); //std::cout << "norm_2(r)=" << norm_2(r) << std::endl; //std::cout << "std::abs(rho*rho_0)=" << std::abs(rho*rho_0) << std::endl; //std::cout << r << std::endl; tag.error(std::fabs(rho*rho_0)); } return result; }
// len of array = num_point_qft double *compute_qft(double *array, int n_qft_point, int n) { int len_nums_complex = pow(2,n_qft_point); std::complex<double> *nums_complex = (std::complex<double> *)malloc(sizeof(std::complex<double>) *len_nums_complex); std::complex<double> im(0, 1); // // // printf("array: %d\n",n ); // for(int i=0;i<n_qft_point;i++) // { // printf("%f,",array[i] ); // } // printf("\n"); std::vector < ublas::vector<std::complex<double> > > v; // for (int i = n_qft_point-1; i >= 0; i-=1) // { // ublas::vector<std::complex<double> > v1(2); // v1(0)=1.0; // v1(1)=std::exp(2.0*im * pi * array[i]); // v.push_back(v1); // } for (int i = 0; i <n_qft_point; i+=1) { ublas::vector<std::complex<double> > v1(2); v1(0)=1.0; v1(1)=std::exp(2.0*im * pi * array[i]); v.push_back(v1); } // printf("expstart: %d\n",n); // for (int i=0; i<n_qft_point; i++) // { // printf("%f,%f\n",real(v[i](0) ),imag(v[i](0) ) ); // printf("%f,%f\n",real(v[i](1) ),imag(v[i](1) ) ); // } // ublas::matrix<std::complex<double> > m2(v[0].size(), v[1].size()); // m2 = outer_prod(v[0], v[1]); // int linear_i=0; // //printf("expstart: %d\n",n); // for(int i = 0;i< m2.size1();i++) // { // for(int j=0;j<m2.size2();j++) // { // //printf("%f,%f\n",real(m(j,i) ),imag(m(j,i) ) ); // nums_complex[linear_i] = m2(j,i); // linear_i++; // } // } // for loop for tensor // ublas::vector<std::complex<double> > v_current = v.back() ; // v.pop_back(); // ublas::vector<std::complex<double> > v_sec_current = v.back(); // v.pop_back(); // ublas::matrix<std::complex<double> > m = outer_prod(v_current, v_sec_current); // ublas::vector<std::complex<double> > v_tmp_product(v_current.size()*v_sec_current.size()); ublas::vector<std::complex<double> > v_current=v[0]; ublas::vector<std::complex<double> > v_sec_current=v[1]; ublas::matrix<std::complex<double> > m =outer_prod(v_current, v_sec_current); ublas::vector<std::complex<double> > v_tmp_product(v_current.size()*v_sec_current.size()); if(n_qft_point<=2) { for (int k = 1; k < n_qft_point; ++k) { // v_current = v.back(); // v.pop_back(); // v_sec_current = v.back(); // v.pop_back(); // m = outer_prod(v_current, v_sec_current); int cnt=0; for(int i = 0;i< m.size1();i++) { for(int j=0;j<m.size2();j++) { v_tmp_product[cnt]=m(i,j); cnt++; } } v_current.resize(cnt,0); v_current = v_tmp_product; v_sec_current.resize(2,0); v_sec_current = v[k]; m.resize(v_current.size(),v_sec_current.size(),0); m = outer_prod(v_current,v_sec_current); if(k!=n_qft_point-1) { v_tmp_product.resize(v_current.size()*v_sec_current.size(),0); } } } else { for (int k = 1; k < n_qft_point; ++k) { // v_current = v.back(); // v.pop_back(); // v_sec_current = v.back(); // v.pop_back(); // m = outer_prod(v_current, v_sec_current); // printf("expstart: %d\n",n); int cnt=0; for(int i = 0;i< m.size1();i++) { for(int j=0;j<m.size2();j++) { // printf("%f,%f\n",real(m(j,i) ),imag(m(j,i) ) ); v_tmp_product[cnt]=m(i,j); cnt++; } } v_current.resize(cnt,0); v_current = v_tmp_product; v_sec_current.resize(2,0); v_sec_current = v[k+1]; m.resize(v_current.size(),v_sec_current.size(),0); m = outer_prod(v_current,v_sec_current); if(k!=n_qft_point-1) { v_tmp_product.resize(v_current.size()*v_sec_current.size()); } } } for (int i = len_nums_complex-1; i >=0 ; i--) { // nums_complex[i] = v_tmp_product.back(); // v_tmp_product.pop_back(); nums_complex[i] = normalize*v_tmp_product[i];//(double)n+(double)n*im; } // printf("%d\n",v_tmp_product.size() ); // printf("expstart: %d\n",n); // for (int i = 0; i <len_nums_complex; i++) // { // //nums_complex[i] = std::exp(im * pi/4.0);//std::exp(im*pi/4.0);// exp(2*pi*array[i])+(double)n; // printf("%f,", real(nums_complex[i])); // printf("%f,", imag(nums_complex[i])); // printf("expend\n"); // // printf("world_rank: with number %f\n",nums[i] ); // } int len_nums = 2*pow(2,n_qft_point); double *nums = (double *)malloc(sizeof(double) * len_nums); assert(nums != NULL); // bringing the nums_complex 1d array to for (int i = 0; i < len_nums_complex; i++) { nums[2*i] = real(nums_complex[i]); nums[2*i+1]= imag(nums_complex[i]); //printf("world_rank: %d , %dth real number %f, img number %f\n",n,i,nums[2*i],nums[2*i+1] ); } return nums; }
void AbstractCardiacMechanicsSolver<ELASTICITY_SOLVER,DIM>::AddActiveStressAndStressDerivative(c_matrix<double,DIM,DIM>& rC, unsigned elementIndex, unsigned currentQuadPointGlobalIndex, c_matrix<double,DIM,DIM>& rT, FourthOrderTensor<DIM,DIM,DIM,DIM>& rDTdE, bool addToDTdE) { for(unsigned i=0; i<DIM; i++) { mCurrentElementFibreDirection(i) = this->mChangeOfBasisMatrix(i,0); } //Compute the active tension and add to the stress and stress-derivative double I4_fibre = inner_prod(mCurrentElementFibreDirection, prod(rC, mCurrentElementFibreDirection)); double lambda_fibre = sqrt(I4_fibre); double active_tension = 0; double d_act_tension_dlam = 0.0; // Set and used if assembleJacobian==true double d_act_tension_d_dlamdt = 0.0; // Set and used if assembleJacobian==true GetActiveTensionAndTensionDerivs(lambda_fibre, currentQuadPointGlobalIndex, addToDTdE, active_tension, d_act_tension_dlam, d_act_tension_d_dlamdt); double detF = sqrt(Determinant(rC)); rT += (active_tension*detF/I4_fibre)*outer_prod(mCurrentElementFibreDirection,mCurrentElementFibreDirection); // amend the stress and dTdE using the active tension double dTdE_coeff1 = -2*active_tension*detF/(I4_fibre*I4_fibre); // note: I4_fibre*I4_fibre = lam^4 double dTdE_coeff2 = active_tension*detF/I4_fibre; double dTdE_coeff_s1 = 0.0; // only set non-zero if we apply cross fibre tension (in 2/3D) double dTdE_coeff_s2 = 0.0; // only set non-zero if we apply cross fibre tension (in 2/3D) double dTdE_coeff_s3 = 0.0; // only set non-zero if we apply cross fibre tension and implicit (in 2/3D) double dTdE_coeff_n1 = 0.0; // only set non-zero if we apply cross fibre tension in 3D double dTdE_coeff_n2 = 0.0; // only set non-zero if we apply cross fibre tension in 3D double dTdE_coeff_n3 = 0.0; // only set non-zero if we apply cross fibre tension in 3D and implicit if(IsImplicitSolver()) { double dt = mNextTime-mCurrentTime; //std::cout << "d sigma / d lamda = " << d_act_tension_dlam << ", d sigma / d lamdat = " << d_act_tension_d_dlamdt << "\n" << std::flush; dTdE_coeff1 += (d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_fibre); // note: I4_fibre*lam = lam^3 } bool apply_cross_fibre_tension = (this->mrElectroMechanicsProblemDefinition.GetApplyCrossFibreTension()) && (DIM > 1); if(apply_cross_fibre_tension) { double sheet_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetTensionFraction(); for(unsigned i=0; i<DIM; i++) { mCurrentElementSheetDirection(i) = this->mChangeOfBasisMatrix(i,1); } double I4_sheet = inner_prod(mCurrentElementSheetDirection, prod(rC, mCurrentElementSheetDirection)); // amend the stress and dTdE using the active tension dTdE_coeff_s1 = -2*sheet_cross_fraction*detF*active_tension/(I4_sheet*I4_sheet); // note: I4*I4 = lam^4 if(IsImplicitSolver()) { double dt = mNextTime-mCurrentTime; dTdE_coeff_s3 = sheet_cross_fraction*(d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_sheet); // note: I4*lam = lam^3 } rT += sheet_cross_fraction*(active_tension*detF/I4_sheet)*outer_prod(mCurrentElementSheetDirection,mCurrentElementSheetDirection); dTdE_coeff_s2 = active_tension*sheet_cross_fraction*detF/I4_sheet; if (DIM>2) { double sheet_normal_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetNormalTensionFraction(); for(unsigned i=0; i<DIM; i++) { mCurrentElementSheetNormalDirection(i) = this->mChangeOfBasisMatrix(i,2); } double I4_sheet_normal = inner_prod(mCurrentElementSheetNormalDirection, prod(rC, mCurrentElementSheetNormalDirection)); dTdE_coeff_n1 =-2*sheet_normal_cross_fraction*detF*active_tension/(I4_sheet_normal*I4_sheet_normal); // note: I4*I4 = lam^4 rT += sheet_normal_cross_fraction*(active_tension*detF/I4_sheet_normal)*outer_prod(mCurrentElementSheetNormalDirection,mCurrentElementSheetNormalDirection); dTdE_coeff_n2 = active_tension*sheet_normal_cross_fraction*detF/I4_sheet_normal; if(IsImplicitSolver()) { double dt = mNextTime-mCurrentTime; dTdE_coeff_n3 = sheet_normal_cross_fraction*(d_act_tension_dlam + d_act_tension_d_dlamdt/dt)*detF/(lambda_fibre*I4_sheet_normal); // note: I4*lam = lam^3 } } } if(addToDTdE) { c_matrix<double,DIM,DIM> invC = Inverse(rC); for (unsigned M=0; M<DIM; M++) { for (unsigned N=0; N<DIM; N++) { for (unsigned P=0; P<DIM; P++) { for (unsigned Q=0; Q<DIM; Q++) { rDTdE(M,N,P,Q) += dTdE_coeff1 * mCurrentElementFibreDirection(M) * mCurrentElementFibreDirection(N) * mCurrentElementFibreDirection(P) * mCurrentElementFibreDirection(Q) + dTdE_coeff2 * mCurrentElementFibreDirection(M) * mCurrentElementFibreDirection(N) * invC(P,Q); if(apply_cross_fibre_tension) { rDTdE(M,N,P,Q) += dTdE_coeff_s1 * mCurrentElementSheetDirection(M) * mCurrentElementSheetDirection(N) * mCurrentElementSheetDirection(P) * mCurrentElementSheetDirection(Q) + dTdE_coeff_s2 * mCurrentElementSheetDirection(M) * mCurrentElementSheetDirection(N) * invC(P,Q) + dTdE_coeff_s3 * mCurrentElementSheetDirection(M) * mCurrentElementSheetDirection(N) * mCurrentElementFibreDirection(P) * mCurrentElementFibreDirection(Q); if (DIM>2) { rDTdE(M,N,P,Q) += dTdE_coeff_n1 * mCurrentElementSheetNormalDirection(M) * mCurrentElementSheetNormalDirection(N) * mCurrentElementSheetNormalDirection(P) * mCurrentElementSheetNormalDirection(Q) + dTdE_coeff_n2 * mCurrentElementSheetNormalDirection(M) * mCurrentElementSheetNormalDirection(N) * invC(P,Q) + dTdE_coeff_n3 * mCurrentElementSheetNormalDirection(M) * mCurrentElementSheetNormalDirection(N) * mCurrentElementFibreDirection(P) * mCurrentElementFibreDirection(Q); } } } } } } } // ///\todo #2180 The code below applies a cross fibre tension in the 2D case. Things that need doing: // // * Refactor the common code between the block below and the block above to avoid duplication. // // * Handle the 3D case. // if(this->mrElectroMechanicsProblemDefinition.GetApplyCrossFibreTension() && DIM > 1) // { // double sheet_cross_fraction = mrElectroMechanicsProblemDefinition.GetSheetTensionFraction(); // // for(unsigned i=0; i<DIM; i++) // { // mCurrentElementSheetDirection(i) = this->mChangeOfBasisMatrix(i,1); // } // // double I4_sheet = inner_prod(mCurrentElementSheetDirection, prod(rC, mCurrentElementSheetDirection)); // // // amend the stress and dTdE using the active tension // double dTdE_coeff_s1 = -2*sheet_cross_fraction*detF*active_tension/(I4_sheet*I4_sheet); // note: I4*I4 = lam^4 // // ///\todo #2180 The code below is specific to the implicit cardiac mechanics solver. Currently // // the cross-fibre code is only tested using the explicit solver so the code below fails coverage. // // This will need to be added back in once an implicit test is in place. // double lambda_sheet = sqrt(I4_sheet); // if(IsImplicitSolver()) // { // double dt = mNextTime-mCurrentTime; // dTdE_coeff_s1 += (d_act_tension_dlam + d_act_tension_d_dlamdt/dt)/(lambda_sheet*I4_sheet); // note: I4*lam = lam^3 // } // // rT += sheet_cross_fraction*(active_tension*detF/I4_sheet)*outer_prod(mCurrentElementSheetDirection,mCurrentElementSheetDirection); // // double dTdE_coeff_s2 = active_tension*detF/I4_sheet; // if(addToDTdE) // { // for (unsigned M=0; M<DIM; M++) // { // for (unsigned N=0; N<DIM; N++) // { // for (unsigned P=0; P<DIM; P++) // { // for (unsigned Q=0; Q<DIM; Q++) // { // rDTdE(M,N,P,Q) += dTdE_coeff_s1 * mCurrentElementSheetDirection(M) // * mCurrentElementSheetDirection(N) // * mCurrentElementSheetDirection(P) // * mCurrentElementSheetDirection(Q) // // + dTdE_coeff_s2 * mCurrentElementFibreDirection(M) // * mCurrentElementFibreDirection(N) // * invC(P,Q); // // } // } // } // } // } // } }
Stats::matrix_type Stats::Impl::covariance() const { Stats::vector_type m = mean(); return meanOuterProduct() - outer_prod(m, m); }
void InfiniteDomainCondition<TDim,TNumNodes>::CalculateLHS( MatrixType& rLeftHandSideMatrix, const ProcessInfo& CurrentProcessInfo ) { KRATOS_TRY //const PropertiesType& Prop = this->GetProperties(); const GeometryType& Geom = this->GetGeometry(); const unsigned int element_size = TNumNodes; const GeometryType::IntegrationPointsArrayType& integration_points = Geom.IntegrationPoints( mThisIntegrationMethod ); const unsigned int NumGPoints = integration_points.size(); const unsigned int LocalDim = Geom.LocalSpaceDimension(); //Resetting the LHS if ( rLeftHandSideMatrix.size1() != element_size ) rLeftHandSideMatrix.resize( element_size, element_size, false ); noalias( rLeftHandSideMatrix ) = ZeroMatrix( element_size, element_size ); //Defining the shape functions, the jacobian and the shape functions local gradients Containers array_1d<double,TNumNodes> Np; const Matrix& NContainer = Geom.ShapeFunctionsValues( mThisIntegrationMethod ); GeometryType::JacobiansType JContainer(NumGPoints); for(unsigned int i = 0; i<NumGPoints; i++) (JContainer[i]).resize(TDim,LocalDim,false); Geom.Jacobian( JContainer, mThisIntegrationMethod ); double IntegrationCoefficient; // Definition of the speed in the fluid //~ const double BulkModulus = Prop[BULK_MODULUS_FLUID]; //~ const double Water_density = Prop[DENSITY_WATER]; const double BulkModulus = 2.21e9; const double Water_density = 1000.0; const double inv_c_speed = 1.0 /sqrt(BulkModulus/Water_density); for ( unsigned int igauss = 0; igauss < NumGPoints; igauss++ ) { noalias(Np) = row(NContainer,igauss); //calculating weighting coefficient for integration this->CalculateIntegrationCoefficient( IntegrationCoefficient, JContainer[igauss], integration_points[igauss].Weight() ); // Mass matrix contribution noalias(rLeftHandSideMatrix) += CurrentProcessInfo[VELOCITY_PRESSURE_COEFFICIENT]*(inv_c_speed)*outer_prod(Np,Np)*IntegrationCoefficient; } KRATOS_CATCH( "" ) }
/* * Sample n Gaussians (with means X and covariances S) from HLL at sample points Q. */ void hll_sample(double **X, double ***S, double **Q, hll_t *hll, int n) { int i, j; if (hll->cache.n > 0) { for (i = 0; i < n; i++) { j = kdtree_NN(hll->cache.Q_kdtree, Q[i]); memcpy(X[i], hll->cache.X[j], hll->dx * sizeof(double)); matrix_copy(S[i], hll->cache.S[j], hll->dx, hll->dx); } return; } double r = hll->r; int nx = hll->dx; int nq = hll->dq; double **WS = new_matrix2(nx, nx); for (i = 0; i < n; i++) { //printf("q = [%.2f %.2f %.2f %.2f]\n", Q[0][0], Q[0][1], Q[0][2], Q[0][3]); //for (j = 0; j < hll->n; j++) // printf("hll->Q[%d] = [%.2f %.2f %.2f %.2f]\n", j, hll->Q[j][0], hll->Q[j][1], hll->Q[j][2], hll->Q[j][3]); // compute weights double dq, qdot; double w[hll->n]; //printf("w = ["); for (j = 0; j < hll->n; j++) { qdot = fabs(dot(Q[i], hll->Q[j], nq)); dq = acos(MIN(qdot, 1.0)); w[j] = exp(-(dq/r)*(dq/r)); //printf("%.2f ", w[j]); } //printf("]\n"); // threshold weights double wmax = arr_max(w, hll->n); double wthresh = wmax/50; //dbug: make this a parameter? for (j = 0; j < hll->n; j++) if (w[j] < wthresh) w[j] = 0; double wtot = hll->w0 + sum(w, hll->n); // compute posterior mean mult(X[i], hll->x0, hll->w0, nx); // X[i] = w0*x0 for (j = 0; j < hll->n; j++) { if (w[j] > 0) { double wx[nx]; mult(wx, hll->X[j], w[j], nx); add(X[i], X[i], wx, nx); // X[i] += wx } } mult(X[i], X[i], 1/wtot, nx); // X[i] /= wtot // compute posterior covariance matrix mult(S[i][0], hll->S0[0], hll->w0, nx*nx); // S[i] = w0*S0 for (j = 0; j < hll->n; j++) { if (w[j] > 0) { double wdx[nx]; sub(wdx, hll->X[j], X[i], nx); mult(wdx, wdx, w[j], nx); outer_prod(WS, wdx, wdx, nx, nx); // WS = wdx'*wdx matrix_add(S[i], S[i], WS, nx, nx); // S[i] += WS } } mult(S[i][0], S[i][0], 1/wtot, nx*nx); // S[i] /= wtot } free_matrix2(WS); }
int lob_scaat(SCAATState *out, STORAGE_TYPE z, STORAGE_TYPE s_x, STORAGE_TYPE s_y, STORAGE_TYPE dt, SCAATState *in, STORAGE_TYPE q, STORAGE_TYPE R){ STORAGE_TYPE **A; STORAGE_TYPE **Q; STORAGE_TYPE **Q1; // temporary matrix STORAGE_TYPE opt_z=0.0; STORAGE_TYPE *H; STORAGE_TYPE *K; //Kalman gain STORAGE_TYPE **I; //unit matrix STORAGE_TYPE **v; // for the eigenvectors int *nrot; // for the Jacobi method of eigenvalue computation STORAGE_TYPE *d; SCAATState pred; // prediction int i, j; int add_noise = 0; // section for temporary variables STORAGE_TYPE *t_v1; // vector STORAGE_TYPE **t_m1; // matrix STORAGE_TYPE **t_m2; // matrix STORAGE_TYPE **t_m3; // matrix STORAGE_TYPE t_s; //temporary scalar value STORAGE_TYPE temp_sum = 0.0; STORAGE_TYPE min_d = 0.0; // program start //A=[1 0 dt 0 dt*dt/2 0 // 0 1 0 dt 0 dt*dt/2 // 0 0 1 0 dt 0 // 0 0 0 1 0 dt // 0 0 0 0 1 0 // 0 0 0 0 0 1]; pred.x = VECTOR(1, STATE_NUM); //pred.P = MATRIX(1, STATE_NUM, 1, STATE_NUM); pred.P = dmatrix_alloc(STATE_NUM,STATE_NUM); t_v1 = VECTOR(1, STATE_NUM); //t_m1 = MATRIX(1, STATE_NUM, 1, STATE_NUM); //t_m2 = MATRIX(1, STATE_NUM, 1, STATE_NUM); //t_m3 = MATRIX(1, STATE_NUM, 1, STATE_NUM); t_m1 = dmatrix_alloc(STATE_NUM,STATE_NUM); t_m2 = dmatrix_alloc(STATE_NUM,STATE_NUM); t_m3 = dmatrix_alloc(STATE_NUM,STATE_NUM); //v = MATRIX(1, STATE_NUM, 1, STATE_NUM); v = dmatrix_alloc(STATE_NUM,STATE_NUM); nrot = ivector(1, STATE_NUM); d = VECTOR(1, STATE_NUM); //A = MATRIX(1, STATE_NUM, 1, STATE_NUM); //Q = MATRIX(1, STATE_NUM, 1, STATE_NUM); //Q1 = MATRIX(1, STATE_NUM, 1, STATE_NUM); A = dmatrix_alloc(STATE_NUM,STATE_NUM); Q = dmatrix_alloc(STATE_NUM,STATE_NUM); Q1 = dmatrix_alloc(STATE_NUM,STATE_NUM); H = VECTOR(1, STATE_NUM); K = VECTOR(1, STATE_NUM); //I = MATRIX(1, STATE_NUM, 1, STATE_NUM); I = dmatrix_alloc(STATE_NUM,STATE_NUM); // Initialize matrices A and Q for (i=1;i<=STATE_NUM;i++){ K[i] = 0.0; for (j=1;j<=STATE_NUM;j++){ A[i][i] = 0; Q[i][i] = 0; Q1[i][i] = 0; if (i==j) I[i][j]= 1.0; else I[i][j]= 0.0; } } for (i=1;i<=STATE_NUM;i++){ A[i][i] = 1; if (i+2<=STATE_NUM) A[i][i+2] = dt; if (i+4<=STATE_NUM) A[i][i+4] = dt*dt/2.0; } //% The state error covariance matrix is a function of the driving //% error which is assumed to only hit the accelaration directly. //% Indirectly this noise affects the velocity and position estimate //% through the dynamics model. //Q=q*[(dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 0 // 0 (dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 // (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 0 // 0 (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 // (dt^3)/6 0 (dt^2)/2 0 dt 0 // 0 (dt^3)/6 0 (dt^2)/2 0 dt]; Q[1][1] = q*pow(dt, 5)/20.0; Q[1][3] = q*pow(dt, 4)/8.0; Q[1][5] = q*pow(dt, 3)/6.0; Q[2][2] = q*pow(dt, 5)/20.0; Q[2][4] = q*pow(dt, 4)/8.0; Q[2][6] = q*pow(dt, 3)/6.0; Q[3][1] = q*pow(dt, 4)/8.0; Q[3][3] = q*pow(dt, 3)/3.0; Q[3][5] = q*pow(dt, 2)/2.0; Q[4][2] = q*pow(dt, 4)/8.0; Q[4][4] = q*pow(dt, 3)/3.0; Q[4][6] = q*pow(dt, 2)/2.0; Q[5][1] = q*pow(dt, 3)/6.0; Q[5][3] = q*pow(dt, 2)/2.0; Q[5][5] = q*dt; Q[6][2] = q*pow(dt, 3)/6.0; Q[6][4] = q*pow(dt, 2)/2.0; Q[6][6] = q*dt; /*for (i=1;i<=STATE_NUM;i++) { for (j=1;j<=STATE_NUM;j++) printf("%f ",Q[i][j]); printf("\n"); }*/ //%step b //pred_x=A*pre_x; %6 x 1 mat_vec_mult(pred.x, A, in->x, STATE_NUM, 0); //pred_P=A*pre_P*(A')+Q; %6 x 6 // A is transpose mat_mat_mult(t_m1, in->P, A, STATE_NUM, 0, 1); mat_mat_mult(t_m2, A, t_m1, STATE_NUM, 0, 0); mat_add(pred.P, t_m2, Q, STATE_NUM, 0); // %step c // % assume that 0<=z<2*pi // opt_z = 2*pi + atan2((pred_x(2)-s_y), (pred_x(1)-s_x)); %1 x 1 // if (opt_z>2*pi) // opt_z = opt_z - 2*pi; // end // %H is 1 x 6 // % measurement function is nonlinear // % // % z = atan(x(2) - s_y, x(1) - s_x); // % In order to linearize the problem we find the jacobian // % noticing that // % // % d(atan(x))/dx = 1/(1+x^2) // % // H=[ -(pred_x(2)-s_y)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), (pred_x(1)-s_x)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), 0, 0, 0, 0]; opt_z = 2*PI + atan2((pred.x[2]-s_y), (pred.x[1]-s_x)); H[1] = -(pred.x[2]-s_y)/ ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) ); H[2] = (pred.x[1]-s_x) / ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) ); H[3] = 0.0; H[4] = 0.0; H[5] = 0.0; H[6] = 0.0; //test=find(isnan(H)==1); //if (~isempty(test)) // disp('H goes to infinity due to node position is the same as the predicted value') // return; //end if (isnan(H[1]) || isnan(H[1])){ fprintf(stderr, "H goes to infinity due to node position is the same as the predicted value\n"); return(-1); } //%step d //%also test if P is still a positive definite matrix. If not, add some small noise to it to //%make it still positive. //n1=1; //Q1=n1*eye(6); //Q1(1,1)=0.1; //Q1(2,2)=0.1; //Q1(3,3)=5; //Q1(4,4)=5; Q1[1][1] = 0.1; Q1[2][2] = 0.1; Q1[3][3] = 5.0; Q1[4][4] = 5.0; Q1[5][5] = 1.0; Q1[6][6] = 1.0; //if isempty(find(eig(pred_P)<0.001)) // K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1 //else // pred_P=pred_P+Q1; // K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1 //end add_noise=0; // jacobi destroys the input vector mat_copy(t_m2, pred.P, STATE_NUM, 0); jacobi(t_m2, STATE_NUM, d, v, nrot); for(i=1;i<=STATE_NUM;i++){ if (d[i]< 0.001) add_noise=1; } if (add_noise){ mat_add(t_m1, pred.P, Q1, STATE_NUM, 0); mat_copy(pred.P, t_m1, STATE_NUM, 0); } mat_vec_mult(t_v1, pred.P, H, STATE_NUM, 1); inner_prod(&t_s, t_v1, H, STATE_NUM); t_s+= R; scal_vec_mult(K, 1.0/t_s, t_v1, STATE_NUM); //%step e and f //I=eye(6); //opt_x=pred_x+K*(z-opt_z); %6 x 1 scal_vec_mult(t_v1, z - opt_z, K, STATE_NUM); vec_add(out->x, pred.x, t_v1, STATE_NUM); // H: 1 x 6, K: 6 x 1 //%Joseph form to ward off round off problem //opt_P=(I-K*H)*pred_P*(I-K*H)'+K*R*K'; %6 x 6 scal_vec_mult(t_v1, R, K, STATE_NUM); // R*K' outer_prod(t_m1, K, t_v1, STATE_NUM); // K*(R*K') outer_prod(t_m2, K, H, STATE_NUM); // K*H mat_add(t_m3, I, t_m2, STATE_NUM, 1); // I-K*H mat_mat_mult(t_m2, pred.P, t_m3, STATE_NUM, 0, 1); mat_mat_mult(out->P, t_m3, t_m2, STATE_NUM, 0, 0); mat_add(t_m3, out->P, t_m1, STATE_NUM, 0); mat_copy(out->P, t_m3, STATE_NUM, 0); //% PRECAUTIONARY APPROACH. EVEN THOUGH IT HASN'T HAPPEN IN THE SIMULATION SO FAR //% also test if opt_P is still a positive definite matrix. If not, add some small noise to it to //% make it still positive. //% d: a diagonal matrix of eigenvalues //% v: matrix whose vectors are the eigenvectors //% X*V = V*D //if isempty(find(eig(opt_P)<0.001)) //else //[v d]=eig(opt_P); //c=find(d==min(diag(d))); //d(c)=0.001; //opt_P=v*d*v'; //end add_noise=0; // jacobi destroys the input vector mat_copy(t_m1, out->P, STATE_NUM, 0); jacobi(t_m1, STATE_NUM, d, v, nrot); min_d = d[1] ; for(i=1;i<=STATE_NUM;i++){ if ( min_d > d[i]) min_d = d[i]; if (d[i]< 0.001) add_noise=1; } // CAUTION - maybe the 2nd minimum is quite far from 0.001 if (add_noise){ // change diagonal matrix d for (i=1;i<STATE_NUM;i++){ if (d[i] == min_d) d[i] = 0.001; } // opt_P=v*d*v'; mat_copy(t_m1, v, STATE_NUM, 1); convert_vec_diag(t_m2, d, STATE_NUM); mat_mat_mult(t_m3, t_m2, t_m1, STATE_NUM, 0, 0); mat_mat_mult(out->P, v, t_m3, STATE_NUM, 0, 0); } //printf("The pred are x = %f y = %f\n",pred.x[1],pred.x[2]); // Free all MALLOCed matrices FREE_VECTOR_FUNCTION(pred.x, 1, STATE_NUM); FREE_MATRIX_FUNCTION(pred.P, 1, STATE_NUM, 1, STATE_NUM); FREE_VECTOR_FUNCTION(t_v1, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m1, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m2, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m3, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(v, 1, STATE_NUM, 1, STATE_NUM); free_ivector(nrot, 1, STATE_NUM); FREE_VECTOR_FUNCTION(d, 1, STATE_NUM); FREE_MATRIX_FUNCTION(A, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(Q, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(Q1, 1, STATE_NUM, 1, STATE_NUM); FREE_VECTOR_FUNCTION(H, 1, STATE_NUM); FREE_VECTOR_FUNCTION(K, 1, STATE_NUM); FREE_MATRIX_FUNCTION(I, 1, STATE_NUM, 1, STATE_NUM); return(0); }
//************************************************************************************ //************************************************************************************ //calculation by component of the fractional step velocity corresponding to the first stage void NDFluid2DCrankNicolson::Stage1(MatrixType& rLeftHandSideMatrix, VectorType& rRightHandSideVector, ProcessInfo& rCurrentProcessInfo, unsigned int ComponentIndex) { KRATOS_TRY; const unsigned int number_of_points = 3; if(rLeftHandSideMatrix.size1() != number_of_points) rLeftHandSideMatrix.resize(number_of_points,number_of_points,false); //false says not to preserve existing storage!! if(rRightHandSideVector.size() != number_of_points) rRightHandSideVector.resize(number_of_points,false); //false says not to preserve existing storage!! //getting data for the given geometry double Area; GeometryUtils::CalculateGeometryData(GetGeometry(),msDN_DX,msN,Area); //getting the velocity vector on the nodes //getting the velocity on the nodes const array_1d<double,3>& fv0 = GetGeometry()[0].FastGetSolutionStepValue(FRACT_VEL,0); const array_1d<double,3>& fv0_old = GetGeometry()[0].FastGetSolutionStepValue(VELOCITY,1); const array_1d<double,3>& w0 = GetGeometry()[0].FastGetSolutionStepValue(MESH_VELOCITY); const array_1d<double,3>& w0_old = GetGeometry()[0].FastGetSolutionStepValue(MESH_VELOCITY,1); const array_1d<double,3>& proj0 = GetGeometry()[0].FastGetSolutionStepValue(CONV_PROJ); const array_1d<double,3>& proj0_old = GetGeometry()[0].FastGetSolutionStepValue(CONV_PROJ,1); double p0old = GetGeometry()[0].FastGetSolutionStepValue(PRESSURE_OLD_IT); const double nu0 = GetGeometry()[0].FastGetSolutionStepValue(VISCOSITY); const double rho0 = GetGeometry()[0].FastGetSolutionStepValue(DENSITY); const double fcomp0 = GetGeometry()[0].FastGetSolutionStepValue(BODY_FORCE)[ComponentIndex]; const double eps0 = GetGeometry()[0].FastGetSolutionStepValue(POROSITY); const double dp0 = GetGeometry()[0].FastGetSolutionStepValue(DIAMETER); const array_1d<double,3>& fv1 = GetGeometry()[1].FastGetSolutionStepValue(FRACT_VEL); const array_1d<double,3>& fv1_old = GetGeometry()[1].FastGetSolutionStepValue(VELOCITY,1); const array_1d<double,3>& w1 = GetGeometry()[1].FastGetSolutionStepValue(MESH_VELOCITY); const array_1d<double,3>& w1_old = GetGeometry()[1].FastGetSolutionStepValue(MESH_VELOCITY,1); const array_1d<double,3>& proj1 = GetGeometry()[1].FastGetSolutionStepValue(CONV_PROJ); double p1old = GetGeometry()[1].FastGetSolutionStepValue(PRESSURE_OLD_IT); const array_1d<double,3>& proj1_old = GetGeometry()[1].FastGetSolutionStepValue(CONV_PROJ,1); const double nu1 = GetGeometry()[1].FastGetSolutionStepValue(VISCOSITY); const double rho1 = GetGeometry()[1].FastGetSolutionStepValue(DENSITY); const double fcomp1 = GetGeometry()[1].FastGetSolutionStepValue(BODY_FORCE)[ComponentIndex]; const double eps1 = GetGeometry()[1].FastGetSolutionStepValue(POROSITY); const double dp1 = GetGeometry()[1].FastGetSolutionStepValue(DIAMETER); const array_1d<double,3>& fv2 = GetGeometry()[2].FastGetSolutionStepValue(FRACT_VEL); const array_1d<double,3>& fv2_old = GetGeometry()[2].FastGetSolutionStepValue(VELOCITY,1); const array_1d<double,3>& w2 = GetGeometry()[2].FastGetSolutionStepValue(MESH_VELOCITY); const array_1d<double,3>& w2_old = GetGeometry()[2].FastGetSolutionStepValue(MESH_VELOCITY,1); const array_1d<double,3>& proj2 = GetGeometry()[2].FastGetSolutionStepValue(CONV_PROJ); const array_1d<double,3>& proj2_old = GetGeometry()[2].FastGetSolutionStepValue(CONV_PROJ,1); double p2old = GetGeometry()[2].FastGetSolutionStepValue(PRESSURE_OLD_IT); const double nu2 = GetGeometry()[2].FastGetSolutionStepValue(VISCOSITY); const double rho2 = GetGeometry()[2].FastGetSolutionStepValue(DENSITY); const double fcomp2 = GetGeometry()[2].FastGetSolutionStepValue(BODY_FORCE)[ComponentIndex]; const double eps2 = GetGeometry()[2].FastGetSolutionStepValue(POROSITY); const double dp2 = GetGeometry()[2].FastGetSolutionStepValue(DIAMETER); // // vel_gauss = sum( N[i]*(vel[i]-mesh_vel[i]), i=0, number_of_points) (note that the fractional step vel is used) ms_vel_gauss[0] = msN[0]*(fv0[0]-w0[0]) + msN[1]*(fv1[0]-w1[0]) + msN[2]*(fv2[0]-w2[0]); ms_vel_gauss[1] = msN[0]*(fv0[1]-w0[1]) + msN[1]*(fv1[1]-w1[1]) + msN[2]*(fv2[1]-w2[1]); //vel_gauss = sum( N[i]*(vel[i]-mesh_vel[i]), i=0, number_of_points) (note that the fractional step vel is used) ms_vel_gauss_old[0] = msN[0]*(fv0_old[0]-w0_old[0]) + msN[1]*(fv1_old[0]-w1_old[0]) + msN[2]*(fv2_old[0]-w2_old[0]); ms_vel_gauss_old[1] = msN[0]*(fv0_old[1]-w0_old[1]) + msN[1]*(fv1_old[1]-w1_old[1]) + msN[2]*(fv2_old[1]-w2_old[1]); //ms_vel_gauss = v at (n+1)/2; ms_vel_gauss[0] += ms_vel_gauss_old[0]; ms_vel_gauss[0] *= 0.5; ms_vel_gauss[1] += ms_vel_gauss_old[1]; ms_vel_gauss[1] *= 0.5; //calculating viscosity double nu = 0.333333333333333333333333*(nu0 + nu1 + nu2 ); double density = 0.3333333333333333333333*(rho0 + rho1 + rho2 ); //DIAMETER of the element double dp = 0.3333333333333333333333*(dp0 + dp1 + dp2); //POROSITY of the element: average value of the porosity double eps = 0.3333333333333333333333*(eps0 + eps1 + eps2 ); //1/PERMEABILITY of the element: average value of the porosity double kinv = 0.0; //Calculate the elemental Kinv in function of the nodal K of each element. //Version 1: we can calculate the elemental kinv from the nodal kinv; //THERE IS AN ERROR: IN THE INTERPHASE ELEMENTS A WATER NODE HAS TO BE ''MORE IMPORTANT'' THAN A POROUS ONE!!! // if(kinv0 != 0.0 || kinv1 != 0.0 || kinv2 != 0.0) //if it is a fluid element // { double k0 = 0.0; // double k1 = 0.0; // double k2 = 0.0; // if(kinv0 != 0.0) // k0 = 1.0/kinv0; // if(kinv1 != 0.0) // k1 = 1.0/kinv1; // if(kinv2 != 0.0) // k2 = 1.0/kinv2; // kinv = 3.0/(k0 + k1 + k2 ); // } // //Calculate kinv = 1/ k(eps_elem); // if(rLeftHandSideMatrix.size1() != number_of_points) // rLeftHandSideMatrix.resize(number_of_points,number_of_points,false); //Version 2:we can calculate the elemental kinv from the already calculate elemental eps; if( (eps0 != 1) | (eps1 != 1) | (eps2 != 1) ) kinv = 150*(1-eps)*(1-eps)/(eps*eps*eps*dp*dp); //getting the BDF2 coefficients (not fixed to allow variable time step) //the coefficients INCLUDE the time step //const Vector& BDFcoeffs = rCurrentProcessInfo[BDF_COEFFICIENTS]; //Getting delta time value for the restriction over tau. double Dt = rCurrentProcessInfo[DELTA_TIME]; array_1d<double,2> BDFcoeffs = ZeroVector(2); BDFcoeffs[0]= 1.0 / Dt; //coeff for step n+1; BDFcoeffs[1]= -1.0 / Dt; //coeff for step n; //calculating parameter tau (saved internally to each element) double c1 = 4.00; double c2 = 2.00; double h = sqrt(2.00*Area); double norm_u = ms_vel_gauss[0]*ms_vel_gauss[0] + ms_vel_gauss[1]*ms_vel_gauss[1]; norm_u = sqrt(norm_u); //norm_u calculated at (n+1)/2;// double tau = 1.00 / ( c1*nu/(h*h) + c2*norm_u/h ); // ***************************************** //CALCULATION OF THE LHS //CONVECTIVE CONTRIBUTION TO THE STIFFNESS MATRIX noalias(ms_u_DN) = prod(msDN_DX , ms_vel_gauss); noalias(rLeftHandSideMatrix) = 0.5 * outer_prod(msN,ms_u_DN)/(eps*eps); //CONVECTION STABILIZING CONTRIBUTION (Suu) noalias(rLeftHandSideMatrix) += 0.5 * tau/(eps*eps) * outer_prod(ms_u_DN,ms_u_DN); //VISCOUS CONTRIBUTION TO THE STIFFNESS MATRIX // rLeftHandSideMatrix += Laplacian * nu; noalias(rLeftHandSideMatrix) += 0.5 * nu/eps * prod(msDN_DX,trans(msDN_DX)); //INERTIA CONTRIBUTION // rLeftHandSideMatrix += M/Dt noalias(rLeftHandSideMatrix) += BDFcoeffs[0] * msMassFactors/eps; //DARCY linear CONTRIBUTION // rLeftHandSideMatrix -= nu/permeability (using the cinematic viscosity it is already divided for density: then we are going to multiplicate for density again); noalias(rLeftHandSideMatrix) -= 0.5 * nu*msMassFactors*kinv; //DARCY non linear CONTRIBUTION (brinkmann) //rLeftHandSideMatrix -= 1.75*|u(n+1/2)|/[(150*k)^0.5*eps^(3/2)] noalias(rLeftHandSideMatrix) -= 0.5 * msMassFactors*norm_u*1.75*sqrt(kinv)/12.2474487/sqrt(eps*eps*eps); //multiplication by the area rLeftHandSideMatrix *= (Area * density); // ***************************************** //CALCULATION OF THE RHS //external forces (component) double force_component = 0.3333333333333333*(fcomp0 + fcomp1 + fcomp2); // KRATOS_WATCH(force_component); // KRATOS_WATCH(p0old); // KRATOS_WATCH(p1old); // KRATOS_WATCH(p2old); //adding pressure gradient (integrated by parts) noalias(rRightHandSideVector) = (force_component )*msN; //3PG------------- //p_avg turn out to be p0_avg p1_avg and p2_avg //3PG------------- double p_avg = msN[0]*p0old + msN[1]*p1old + msN[2]*p2old; p_avg /= density; // KRATOS_WATCH(p_avg); rRightHandSideVector[0] += msDN_DX(0,ComponentIndex)*p_avg; rRightHandSideVector[1] += msDN_DX(1,ComponentIndex)*p_avg; rRightHandSideVector[2] += msDN_DX(2,ComponentIndex)*p_avg; // KRATOS_WATCH(rRightHandSideVector); //adding the inertia terms // RHS += M*vhistory //calculating the historical velocity noalias(ms_temp_vec_np) = ZeroVector(3); for(int iii = 0; iii<3; iii++) { const array_1d<double,3>& v = (GetGeometry()[iii].FastGetSolutionStepValue(VELOCITY,1) ); ms_temp_vec_np[iii] = BDFcoeffs[1]*v[ComponentIndex]; } noalias(rRightHandSideVector) -= prod(msMassFactors,ms_temp_vec_np)/eps ; // KRATOS_WATCH(prod(msMassFactors,ms_temp_vec_np)/eps); //3PG------------- //proj_component calculated on the 3 gauss points //3PG------------- //RHS += Suy * proj[component] double proj_component = msN[0]*proj0[ComponentIndex] + msN[1]*proj1[ComponentIndex] + msN[2]*proj2[ComponentIndex]; double proj_old_component = msN[0]*proj0_old[ComponentIndex] + msN[1]*proj1_old[ComponentIndex] + msN[2]*proj2_old[ComponentIndex]; proj_component += proj_old_component; proj_component *= 0.5; //proj_component calculate in t_n+1/2; noalias(rRightHandSideVector) += (tau*proj_component)/(eps*eps)*ms_u_DN; //multiplying by area rRightHandSideVector *= (Area * density); ms_temp_vec_np[0] = fv0_old[ComponentIndex]; ms_temp_vec_np[1] = fv1_old[ComponentIndex]; ms_temp_vec_np[2] = fv2_old[ComponentIndex]; //there is a part of the lhs which is already included; for(int iii = 0; iii<3; iii++) { ms_temp_vec_np[iii] *= BDFcoeffs[0]; } noalias(rRightHandSideVector) += (Area * density)*prod(msMassFactors,ms_temp_vec_np)/eps ; // KRATOS_WATCH((Area * density)*prod(msMassFactors,ms_temp_vec_np)/eps); //suubtracting the dirichlet term // RHS -= LHS*FracVel ms_temp_vec_np[0] = fv0[ComponentIndex] + fv0_old[ComponentIndex]; ms_temp_vec_np[1] = fv1[ComponentIndex] + fv1_old[ComponentIndex]; ms_temp_vec_np[2] = fv2[ComponentIndex] + fv2_old[ComponentIndex]; noalias(rRightHandSideVector) -= prod(rLeftHandSideMatrix,ms_temp_vec_np); // KRATOS_WATCH(prod(rLeftHandSideMatrix,ms_temp_vec_np)); // // KRATOS_WATCH(fv0); // KRATOS_WATCH(fv1); // KRATOS_WATCH(fv2); // KRATOS_WATCH(fv0_old); // KRATOS_WATCH(fv1_old); // KRATOS_WATCH(fv2_old); // KRATOS_WATCH(rRightHandSideVector); KRATOS_CATCH(""); }
void Isotropic_Rankine_Yield_Function::ReturnMapping(const Vector& StrainVector, const Vector& TrialStress, Vector& StressVector) { if(mcurrent_Ft>0.00) { //const double& Young = (*mprops)[YOUNG_MODULUS]; //const double& Poisson = (*mprops)[POISSON_RATIO]; //const double Gmodu = Young/(2.00 * (1.00 + Poisson) ); //const double Bulk = Young/(3.00 * (1.00-2.00*Poisson)); array_1d<double,3> PrincipalStress = ZeroVector(3); array_1d<double,3> Sigma = ZeroVector(3); array_1d<array_1d < double,3 > ,3> EigenVectors; array_1d<unsigned int,3> Order; //const double d3 = 0.3333333333333333333; const int dim = TrialStress.size(); Vector Stress(dim); Stress = ZeroVector(dim); /// computing the trial kirchooff strain SpectralDecomposition(TrialStress, PrincipalStress, EigenVectors); IdentifyMaximunAndMinumumPrincipalStres_CalculateOrder(PrincipalStress, Order); noalias(Sigma) = PrincipalStress; noalias(Stress) = TrialStress; const bool check = CheckPlasticAdmisibility(PrincipalStress); if(check==true) { Vector delta_lamda; // return to main plane bool check = One_Vector_Return_Mapping_To_Main_Plane(PrincipalStress, delta_lamda, Sigma); if(check==false) { //return to corner UpdateMaterial(); check = Two_Vector_Return_Mapping_To_Corner(PrincipalStress, delta_lamda, Sigma); if (check==false) { //return to apex UpdateMaterial(); Three_Vector_Return_Mapping_To_Apex (PrincipalStress, delta_lamda, Sigma); } } AssembleUpdateStressAndStrainTensor(Sigma, EigenVectors, Order, StrainVector, Stress); CalculatePlasticDamage(Sigma); Matrix PlasticTensor; PlasticTensor.resize(3,3,false); PlasticTensor = ZeroMatrix(3,3); /// Updating the elastic plastic strain tensor for(unsigned int i=0; i<3; i++) noalias(PlasticTensor) += mPrincipalPlasticStrain_current[i] * Matrix(outer_prod(EigenVectors[Order[i]], EigenVectors[Order[i]])); /// WARNING = Plane Strain mplastic_strain[0] = PlasticTensor(0,0); mplastic_strain[1] = PlasticTensor(1,1); mplastic_strain[2] = 2.00 * PlasticTensor(1,0); mplastic_strain[3] = PlasticTensor(2,2); //CalculatePlasticStrain(StrainVector, StressVector, mplastic_strain, Sigma[2]); } CalculateElasticStrain(Stress, mElastic_strain); /// WARNING = Plane Strain StressVector[0] = Stress[0]; StressVector[1] = Stress[1]; StressVector[2] = Stress[2]; //KRATOS_WATCH("----------------------------") } }