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("----------------------------")
}
}
