int CFeasibilityMap::test_feasible_test_fn(CData &Data, ColumnVector &x_tilde_i, ColumnVector &s_i){
int is_feasible = 0 ;
int n_var = x_tilde_i.nrows() ;
int n_EditVec = Data.EditVec.nrows() ;
int sum_s_1 = s_i.sum() ; int sum_s_0 = n_var - sum_s_1 ;
ColumnVector x_0(sum_s_0); x_0 = 0;
Matrix A_0(n_EditVec,sum_s_0) ; A_0 = 0;
Matrix A_1(n_EditVec,sum_s_1) ; A_1 = 0;
int count0 = 0;
int count1 = 0;
for (int j_var=1; j_var<=n_var; j_var++){
if ((int)s_i(j_var)==0){
count0++;
x_0(count0) = x_tilde_i(j_var) ;
A_0.column(count0) = Data.EditMat.column(j_var) ;
} else {
count1++;
A_1.column(count1) = Data.EditMat.column(j_var) ;
}
}
ColumnVector which_rows(n_EditVec); which_rows = 0 ;
for (int i_row=1; i_row<=n_EditVec; i_row++){
int n_zero = 0 ;
for (int j=1; j<=sum_s_1; j++){
if (A_1(i_row,j)==0) n_zero++;
}
if (n_zero<sum_s_1) which_rows(i_row) = 1 ;
}
Matrix A_1_red(which_rows.sum(),sum_s_1);
ColumnVector b_1_red(which_rows.sum());
Matrix A_0_red(which_rows.sum(),sum_s_0);
for (int i_row=1, count_row = 0; i_row<=n_EditVec; i_row++){
if (which_rows(i_row)==1){
count_row++;
A_1_red.row(count_row) = A_1.row(i_row) ; // Constraints
b_1_red.row(count_row) = Data.EditVec.row(i_row) ;
A_0_red.row(count_row) = A_0.row(i_row) ;
}
}
ColumnVector EditVec_shr = b_1_red - A_0_red * x_0 ;
is_feasible = SolveLP(A_1_red, EditVec_shr);
return is_feasible?1:0;
}
void ElasticMembranePressure<PressureType>::element_time_derivative
( bool compute_jacobian, AssemblyContext & context )
{
unsigned int u_var = this->_disp_vars.u();
unsigned int v_var = this->_disp_vars.v();
unsigned int w_var = this->_disp_vars.w();
const unsigned int n_u_dofs = context.get_dof_indices(u_var).size();
const std::vector<libMesh::Real> &JxW =
this->get_fe(context)->get_JxW();
const std::vector<std::vector<libMesh::Real> >& u_phi =
this->get_fe(context)->get_phi();
const MultiphysicsSystem & system = context.get_multiphysics_system();
unsigned int u_dot_var = system.get_second_order_dot_var(u_var);
unsigned int v_dot_var = system.get_second_order_dot_var(v_var);
unsigned int w_dot_var = system.get_second_order_dot_var(w_var);
libMesh::DenseSubVector<libMesh::Number> &Fu = context.get_elem_residual(u_dot_var);
libMesh::DenseSubVector<libMesh::Number> &Fv = context.get_elem_residual(v_dot_var);
libMesh::DenseSubVector<libMesh::Number> &Fw = context.get_elem_residual(w_dot_var);
libMesh::DenseSubMatrix<libMesh::Number>& Kuv = context.get_elem_jacobian(u_dot_var,v_var);
libMesh::DenseSubMatrix<libMesh::Number>& Kuw = context.get_elem_jacobian(u_dot_var,w_var);
libMesh::DenseSubMatrix<libMesh::Number>& Kvu = context.get_elem_jacobian(v_dot_var,u_var);
libMesh::DenseSubMatrix<libMesh::Number>& Kvw = context.get_elem_jacobian(v_dot_var,w_var);
libMesh::DenseSubMatrix<libMesh::Number>& Kwu = context.get_elem_jacobian(w_dot_var,u_var);
libMesh::DenseSubMatrix<libMesh::Number>& Kwv = context.get_elem_jacobian(w_dot_var,v_var);
unsigned int n_qpoints = context.get_element_qrule().n_points();
// All shape function gradients are w.r.t. master element coordinates
const std::vector<std::vector<libMesh::Real> >& dphi_dxi =
this->get_fe(context)->get_dphidxi();
const std::vector<std::vector<libMesh::Real> >& dphi_deta =
this->get_fe(context)->get_dphideta();
const libMesh::DenseSubVector<libMesh::Number>& u_coeffs = context.get_elem_solution( u_var );
const libMesh::DenseSubVector<libMesh::Number>& v_coeffs = context.get_elem_solution( v_var );
const libMesh::DenseSubVector<libMesh::Number>& w_coeffs = context.get_elem_solution( w_var );
const std::vector<libMesh::RealGradient>& dxdxi = this->get_fe(context)->get_dxyzdxi();
const std::vector<libMesh::RealGradient>& dxdeta = this->get_fe(context)->get_dxyzdeta();
for (unsigned int qp=0; qp != n_qpoints; qp++)
{
// sqrt(det(a_cov)), a_cov being the covariant metric tensor of undeformed body
libMesh::Real sqrt_a = sqrt( dxdxi[qp]*dxdxi[qp]*dxdeta[qp]*dxdeta[qp]
- dxdxi[qp]*dxdeta[qp]*dxdeta[qp]*dxdxi[qp] );
// Gradients are w.r.t. master element coordinates
libMesh::Gradient grad_u, grad_v, grad_w;
for( unsigned int d = 0; d < n_u_dofs; d++ )
{
libMesh::RealGradient u_gradphi( dphi_dxi[d][qp], dphi_deta[d][qp] );
grad_u += u_coeffs(d)*u_gradphi;
grad_v += v_coeffs(d)*u_gradphi;
grad_w += w_coeffs(d)*u_gradphi;
}
libMesh::RealGradient dudxi( grad_u(0), grad_v(0), grad_w(0) );
libMesh::RealGradient dudeta( grad_u(1), grad_v(1), grad_w(1) );
libMesh::RealGradient A_1 = dxdxi[qp] + dudxi;
libMesh::RealGradient A_2 = dxdeta[qp] + dudeta;
libMesh::RealGradient A_3 = A_1.cross(A_2);
// Compute pressure at this quadrature point
libMesh::Real press = (*_pressure)(context,qp);
// Small optimization
libMesh::Real p_over_sa = press/sqrt_a;
/* The formula here is actually
P*\sqrt{\frac{A}{a}}*A_3, where A_3 is a unit vector
But, |A_3| = \sqrt{A} so the normalizing part kills
the \sqrt{A} in the numerator, so we can leave it out
and *not* normalize A_3.
*/
libMesh::RealGradient traction = p_over_sa*A_3;
for (unsigned int i=0; i != n_u_dofs; i++)
{
// Small optimization
libMesh::Real phi_times_jac = u_phi[i][qp]*JxW[qp];
Fu(i) -= traction(0)*phi_times_jac;
Fv(i) -= traction(1)*phi_times_jac;
Fw(i) -= traction(2)*phi_times_jac;
if( compute_jacobian )
{
for (unsigned int j=0; j != n_u_dofs; j++)
{
libMesh::RealGradient u_gradphi( dphi_dxi[j][qp], dphi_deta[j][qp] );
const libMesh::Real dt0_dv = p_over_sa*(u_gradphi(0)*A_2(2) - A_1(2)*u_gradphi(1));
const libMesh::Real dt0_dw = p_over_sa*(A_1(1)*u_gradphi(1) - u_gradphi(0)*A_2(1));
const libMesh::Real dt1_du = p_over_sa*(A_1(2)*u_gradphi(1) - u_gradphi(0)*A_2(2));
const libMesh::Real dt1_dw = p_over_sa*(u_gradphi(0)*A_2(0) - A_1(0)*u_gradphi(1));
const libMesh::Real dt2_du = p_over_sa*(u_gradphi(0)*A_2(1) - A_1(1)*u_gradphi(1));
const libMesh::Real dt2_dv = p_over_sa*(A_1(0)*u_gradphi(1) - u_gradphi(0)*A_2(0));
Kuv(i,j) -= dt0_dv*phi_times_jac;
Kuw(i,j) -= dt0_dw*phi_times_jac;
Kvu(i,j) -= dt1_du*phi_times_jac;
Kvw(i,j) -= dt1_dw*phi_times_jac;
Kwu(i,j) -= dt2_du*phi_times_jac;
Kwv(i,j) -= dt2_dv*phi_times_jac;
}
}
}
}
}
int CFeasibilityMap::feasible_test_fn(CData &Data, ColumnVector &x_tilde_i,
ColumnVector &s_i, int i_original, bool initD_S, float epsilon, ColumnVector &x){
int is_feasible = 0 ;
if (initD_S) {
if (!Data.PassStep0_for_init(x_tilde_i,s_i,epsilon)) { return is_feasible;}
} else {
// Step 0 - check balance edits hold, but only one s_ij has the value 1.
if (!Data.PassStep0(s_i,i_original)) { return is_feasible;}
}
// Step 1
int n_var = x_tilde_i.nrows() ;
int n_EditVec = Data.EditVec.nrows() ;
int sum_s_1 = s_i.sum() ; int sum_s_0 = n_var - sum_s_1 ;
ColumnVector x_0(sum_s_0); x_0 = 0;
Matrix A_0(n_EditVec,sum_s_0) ; A_0 = 0;
Matrix A_1(n_EditVec,sum_s_1) ; A_1 = 0;
int count0 = 0;
int count1 = 0;
for (int j_var=1; j_var<=n_var; j_var++){
if ((int)s_i(j_var)==0){
count0++;
x_0(count0) = x_tilde_i(j_var) ;
A_0.column(count0) = Data.EditMat.column(j_var) ;
} else {
count1++;
A_1.column(count1) = Data.EditMat.column(j_var) ;
}
}
Data.Debug = Debug;
if (!Data.PassStep1(s_i, A_0, x_0)) { return is_feasible;}
// Step 2: lpSolve to check whether s_i has a feasible solution of x_i
ColumnVector which_rows(n_EditVec); which_rows = 0 ;
for (int i_row=1; i_row<=n_EditVec; i_row++){
int n_zero = 0 ;
for (int j=1; j<=sum_s_1; j++){
if (A_1(i_row,j)==0) n_zero++;
}
if (n_zero<sum_s_1) which_rows(i_row) = 1 ;
}
Matrix A_1_red(which_rows.sum(),sum_s_1);
ColumnVector b_1_red(which_rows.sum());
Matrix A_0_red(which_rows.sum(),sum_s_0);
for (int i_row=1, count_row = 0; i_row<=n_EditVec; i_row++){
if (which_rows(i_row)==1){
count_row++;
A_1_red.row(count_row) = A_1.row(i_row) ; // Constraints
b_1_red.row(count_row) = Data.EditVec.row(i_row) ;
A_0_red.row(count_row) = A_0.row(i_row) ;
}
}
ColumnVector EditVec_shr = b_1_red - A_0_red * x_0 ;
if (initD_S) {
is_feasible = SolveLP(A_1_red, EditVec_shr,x);
} else {
is_feasible = SolveLP(A_1_red, EditVec_shr);
}
return is_feasible?1:0;
}
bool BlockMatrix::invert(double * logDeterminant)
{
if( this->nBlockRows != this->nBlockCols )
{
misc.error("Error: An internal error was happened. A block matrix with different number of blocks in the rows and in the columns cannot be inverted.", 0);
}
if( this->nBlockRows == 1 && this->nBlockCols == 1)
{
if( this->m[0][0]->symmetric == false || this->m[0][0]->distribution != diagonalDistribution )
{
misc.error("Error: An internal error was happened. A block matrix can only be inverted if all blocks are diagonal.", 0);
}
this->m[0][0]->symmetricInvert(logDeterminant);
}
else
{
//Divide this matrix in four blocks
// [ A B ]
// [ C D ]
// Where D contains only the last block row and column, amd A, B, C are defined accordingly. Then use the block inversion when matrix is formed by four blocks.
std::vector< std::vector<Matrix*> > ABlocks;
std::vector< std::vector<Matrix*> > BBlocks;
std::vector< std::vector<Matrix*> > CBlocks;
std::vector< std::vector<Matrix*> > DBlocks;
for(int r = 0; r < this->nBlockRows; r++)
{
std::vector<Matrix*> blockRowLeft;
std::vector<Matrix*> blockRowRight;
for(int c = 0; c < this->nBlockCols; c++)
{
if( c < (this->nBlockCols - 1) )
{
blockRowLeft.push_back(this->m[r][c]);
}
else
{
blockRowRight.push_back(this->m[r][c]);
}
}
if( r < (this->nBlockRows - 1) )
{
ABlocks.push_back(blockRowLeft);
BBlocks.push_back(blockRowRight);
}
else
{
CBlocks.push_back(blockRowLeft);
DBlocks.push_back(blockRowRight);
}
}
BlockMatrix A(ABlocks);
BlockMatrix B(BBlocks);
BlockMatrix C(CBlocks);
BlockMatrix D(DBlocks);
bool inverted;
double DLogDeterminant;
BlockMatrix D_1(D);
inverted = D_1.invert(&DLogDeterminant);
if( inverted == false )
{
return false;
}
BlockMatrix D_1C;
D_1C.multiply(D_1, C);
BlockMatrix A_BD_1C_1;
A_BD_1C_1.multiply(B, D_1C);
A_BD_1C_1.add(A, -1., 1.);
double A_BD_1CLogDeterminant;
inverted = A_BD_1C_1.invert(&A_BD_1CLogDeterminant);
if( inverted == false )
{
return false;
}
BlockMatrix minD_1CA_BD_1C_1;
minD_1CA_BD_1C_1.multiply(D_1C, A_BD_1C_1, -1.);
D_1C.clear();
BlockMatrix A_1(A);
inverted = A_1.invert();
if( inverted == false )
{
return false;
}
BlockMatrix A_1B;
A_1B.multiply(A_1, B);
BlockMatrix D_CA_1B_1;
D_CA_1B_1.multiply(C, A_1B);
D_CA_1B_1.add(D, -1., 1.);
inverted = D_CA_1B_1.invert();
if( inverted == false )
{
return false;
}
BlockMatrix minA_1BD_CA_1B_1;
minA_1BD_CA_1B_1.multiply(A_1B, D_CA_1B_1, -1.);
A_1B.clear();
clear();
for(int r = 0; r < A.nBlockRows; r++)
{
std::vector<Matrix*> blockRow;
for(int c = 0; c < A.nBlockCols; c++)
{
blockRow.push_back(A_BD_1C_1.m[r][c]);
A_BD_1C_1.m[r][c] = NULL;
}
blockRow.push_back(minA_1BD_CA_1B_1.m[r][0]);
minA_1BD_CA_1B_1.m[r][0] = NULL;
addBlockRow(blockRow);
}
std::vector<Matrix*> blockRow;
for(int c = 0; c < A.nBlockCols; c++)
{
blockRow.push_back(minD_1CA_BD_1C_1.m[0][c]);
minD_1CA_BD_1C_1.m[0][c] = NULL;
}
blockRow.push_back(D_CA_1B_1.m[0][0]);
D_CA_1B_1.m[0][0] = NULL;
addBlockRow(blockRow);
A_BD_1C_1.clear();
minA_1BD_CA_1B_1.clear();
D_CA_1B_1.clear();
minD_1CA_BD_1C_1.clear();
if(logDeterminant != NULL)
{
*logDeterminant = DLogDeterminant + A_BD_1CLogDeterminant;
communicator->broadcast(logDeterminant, 1);
}
}
return true;
}
void ElasticMembraneConstantPressure::element_time_derivative( bool compute_jacobian,
AssemblyContext& context,
CachedValues& /*cache*/ )
{
const unsigned int n_u_dofs = context.get_dof_indices(_disp_vars.u()).size();
const std::vector<libMesh::Real> &JxW =
this->get_fe(context)->get_JxW();
const std::vector<std::vector<libMesh::Real> >& u_phi =
this->get_fe(context)->get_phi();
libMesh::DenseSubVector<libMesh::Number> &Fu = context.get_elem_residual(_disp_vars.u());
libMesh::DenseSubVector<libMesh::Number> &Fv = context.get_elem_residual(_disp_vars.v());
libMesh::DenseSubVector<libMesh::Number> &Fw = context.get_elem_residual(_disp_vars.w());
libMesh::DenseSubMatrix<libMesh::Number>& Kuv = context.get_elem_jacobian(_disp_vars.u(),_disp_vars.v());
libMesh::DenseSubMatrix<libMesh::Number>& Kuw = context.get_elem_jacobian(_disp_vars.u(),_disp_vars.w());
libMesh::DenseSubMatrix<libMesh::Number>& Kvu = context.get_elem_jacobian(_disp_vars.v(),_disp_vars.u());
libMesh::DenseSubMatrix<libMesh::Number>& Kvw = context.get_elem_jacobian(_disp_vars.v(),_disp_vars.w());
libMesh::DenseSubMatrix<libMesh::Number>& Kwu = context.get_elem_jacobian(_disp_vars.w(),_disp_vars.u());
libMesh::DenseSubMatrix<libMesh::Number>& Kwv = context.get_elem_jacobian(_disp_vars.w(),_disp_vars.v());
unsigned int n_qpoints = context.get_element_qrule().n_points();
// All shape function gradients are w.r.t. master element coordinates
const std::vector<std::vector<libMesh::Real> >& dphi_dxi =
this->get_fe(context)->get_dphidxi();
const std::vector<std::vector<libMesh::Real> >& dphi_deta =
this->get_fe(context)->get_dphideta();
const libMesh::DenseSubVector<libMesh::Number>& u_coeffs = context.get_elem_solution( _disp_vars.u() );
const libMesh::DenseSubVector<libMesh::Number>& v_coeffs = context.get_elem_solution( _disp_vars.v() );
const libMesh::DenseSubVector<libMesh::Number>& w_coeffs = context.get_elem_solution( _disp_vars.w() );
const std::vector<libMesh::RealGradient>& dxdxi = this->get_fe(context)->get_dxyzdxi();
const std::vector<libMesh::RealGradient>& dxdeta = this->get_fe(context)->get_dxyzdeta();
for (unsigned int qp=0; qp != n_qpoints; qp++)
{
// sqrt(det(a_cov)), a_cov being the covariant metric tensor of undeformed body
libMesh::Real sqrt_a = sqrt( dxdxi[qp]*dxdxi[qp]*dxdeta[qp]*dxdeta[qp]
- dxdxi[qp]*dxdeta[qp]*dxdeta[qp]*dxdxi[qp] );
// Gradients are w.r.t. master element coordinates
libMesh::Gradient grad_u, grad_v, grad_w;
for( unsigned int d = 0; d < n_u_dofs; d++ )
{
libMesh::RealGradient u_gradphi( dphi_dxi[d][qp], dphi_deta[d][qp] );
grad_u += u_coeffs(d)*u_gradphi;
grad_v += v_coeffs(d)*u_gradphi;
grad_w += w_coeffs(d)*u_gradphi;
}
libMesh::RealGradient dudxi( grad_u(0), grad_v(0), grad_w(0) );
libMesh::RealGradient dudeta( grad_u(1), grad_v(1), grad_w(1) );
libMesh::RealGradient A_1 = dxdxi[qp] + dudxi;
libMesh::RealGradient A_2 = dxdeta[qp] + dudeta;
libMesh::RealGradient A_3 = A_1.cross(A_2);
/* The formula here is actually
P*\sqrt{\frac{A}{a}}*A_3, where A_3 is a unit vector
But, |A_3| = \sqrt{A} so the normalizing part kills
the \sqrt{A} in the numerator, so we can leave it out
and *not* normalize A_3.
*/
libMesh::RealGradient traction = _pressure/sqrt_a*A_3;
libMesh::Real jac = JxW[qp];
for (unsigned int i=0; i != n_u_dofs; i++)
{
Fu(i) -= traction(0)*u_phi[i][qp]*jac;
Fv(i) -= traction(1)*u_phi[i][qp]*jac;
Fw(i) -= traction(2)*u_phi[i][qp]*jac;
if( compute_jacobian )
{
for (unsigned int j=0; j != n_u_dofs; j++)
{
libMesh::RealGradient u_gradphi( dphi_dxi[j][qp], dphi_deta[j][qp] );
const libMesh::Real dt0_dv = _pressure/sqrt_a*(u_gradphi(0)*A_2(2) - A_1(2)*u_gradphi(1));
const libMesh::Real dt0_dw = _pressure/sqrt_a*(A_1(1)*u_gradphi(1) - u_gradphi(0)*A_2(1));
const libMesh::Real dt1_du = _pressure/sqrt_a*(A_1(2)*u_gradphi(1) - u_gradphi(0)*A_2(2));
const libMesh::Real dt1_dw = _pressure/sqrt_a*(u_gradphi(0)*A_2(0) - A_1(0)*u_gradphi(1));
const libMesh::Real dt2_du = _pressure/sqrt_a*(u_gradphi(0)*A_2(1) - A_1(1)*u_gradphi(1));
const libMesh::Real dt2_dv = _pressure/sqrt_a*(A_1(0)*u_gradphi(1) - u_gradphi(0)*A_2(0));
Kuv(i,j) -= dt0_dv*u_phi[i][qp]*jac;
Kuw(i,j) -= dt0_dw*u_phi[i][qp]*jac;
Kvu(i,j) -= dt1_du*u_phi[i][qp]*jac;
Kvw(i,j) -= dt1_dw*u_phi[i][qp]*jac;
Kwu(i,j) -= dt2_du*u_phi[i][qp]*jac;
Kwv(i,j) -= dt2_dv*u_phi[i][qp]*jac;
}
}
}
}
return;
}
