void SetNewProblem()
{
const unsigned int nprob = 2;
static unsigned int iprob = 0;
static int id_val_bc0=0, id_val_bc1=0, id_val_bc2=0;
if( iprob == 0 ){ // ２次元問題の設定
Cad::CCadObj2D cad_2d;
{ // 形を作る
std::vector<Com::CVector2D> vec_ary;
vec_ary.push_back( Com::CVector2D(-0.5,-0.5) );
vec_ary.push_back( Com::CVector2D( 0.5,-0.5) );
vec_ary.push_back( Com::CVector2D( 0.5, 0.5) );
vec_ary.push_back( Com::CVector2D(-0.5, 0.5) );
cad_2d.AddPolygon( vec_ary );
cad_2d.AddVertex(Cad::LOOP,1,Com::CVector2D(0.0,0.0));
}
world.Clear();
const unsigned int id_base = world.AddMesh( Msh::CMesher2D(cad_2d,0.05) );
const Fem::Field::CIDConvEAMshCad& conv = world.GetIDConverter(id_base);
unsigned int id_field_rot = world.MakeField_FieldElemDim(id_base,2,VECTOR2,VALUE,CORNER);
unsigned int id_field_deflect = world.MakeField_FieldElemDim(id_base,2,SCALAR,VALUE,CORNER);
Fem::Ls::CLinearSystem_Field ls;
LsSol::CPreconditioner_ILU prec;
ls.AddPattern_Field(id_field_deflect,world);
ls.AddPattern_Field(id_field_rot,id_field_deflect,world);
prec.SetLinearSystem(ls.m_ls);
unsigned int id_field_rot_fix0 = world.GetPartialField(id_field_rot,conv.GetIdEA_fromCad(2,Cad::EDGE));
unsigned int id_field_rot_fix1 = world.GetPartialField(id_field_rot,conv.GetIdEA_fromCad(4,Cad::EDGE));
ls.SetFixedBoundaryCondition_Field(id_field_rot_fix0,world);
ls.SetFixedBoundaryCondition_Field(id_field_rot_fix1,world);
unsigned int id_field_def_fix0 = world.GetPartialField(id_field_deflect,conv.GetIdEA_fromCad(2,Cad::EDGE));
unsigned int id_field_def_fix1 = world.GetPartialField(id_field_deflect,conv.GetIdEA_fromCad(4,Cad::EDGE));
Fem::Field::SetFieldValue_Constant(id_field_rot_fix1,1,Fem::Field::VALUE,world, -1);
ls.SetFixedBoundaryCondition_Field(id_field_def_fix0,world);
ls.SetFixedBoundaryCondition_Field(id_field_def_fix1,world);
ls.InitializeMarge();
Fem::Eqn::AddLinearSystem_DKT2D_Static(ls,world,id_field_deflect,id_field_rot);
double res = ls.FinalizeMarge();
prec.SetValue(ls.m_ls);
std::cout << "Residual : " << res << std::endl;
{
double tol = 1.0e-6;
unsigned int iter = 10000;
LsSol::Solve_CG(tol,iter,ls);
// Fem::Sol::Solve_PCG(tol,iter,ls,prec);
std::cout << iter << " " << tol << std::endl;
}
ls.UpdateValueOfField(id_field_deflect,world,VALUE);
ls.UpdateValueOfField(id_field_rot,world,VALUE);
drawer_ary.Clear();
// drawer_ary.PushBack( new View::CDrawerVector(id_field_rot,world) );
drawer_ary.PushBack( new View::CDrawerFace(id_field_deflect,false,world) );
drawer_ary.PushBack( new View::CDrawerEdge(id_field_deflect,false,world) );
drawer_ary.PushBack( new View::CDrawerEdge(id_field_deflect,true,world) );
camera.Fit( drawer_ary.GetBoundingBox(camera.GetRotMatrix3() ) );
}
else if( iprob == 1 ){
Msh::CMesher2D msh;
msh.ReadFromFile_GiDMsh("../input_file/rect_tri.msh");
world.Clear();
const unsigned int id_base = world.AddMesh( msh );
unsigned int id_field_rot = world.MakeField_FieldElemDim(id_base,2, VECTOR2,VALUE,CORNER);
unsigned int id_field_deflect = world.MakeField_FieldElemDim(id_base,2, SCALAR,VALUE,CORNER);
unsigned int id_field_rot_fix0 = world.GetPartialField(id_field_rot,3);
unsigned int id_field_rot_fix1 = world.GetPartialField(id_field_rot,4);
unsigned int id_field_def_fix0 = world.GetPartialField(id_field_deflect,3);
unsigned int id_field_def_fix1 = world.GetPartialField(id_field_deflect,4);
Fem::Field::SetFieldValue_Constant(id_field_def_fix1,0,Fem::Field::VALUE,world, -3);
Fem::Ls::CLinearSystem_Field ls;
LsSol::CPreconditioner_ILU prec;
ls.AddPattern_Field(id_field_deflect,world);
ls.AddPattern_Field(id_field_rot,id_field_deflect,world);
prec.SetLinearSystem(ls.m_ls);
ls.SetFixedBoundaryCondition_Field(id_field_rot_fix0,world);
ls.SetFixedBoundaryCondition_Field(id_field_rot_fix1,world);
ls.SetFixedBoundaryCondition_Field(id_field_def_fix0,world);
ls.SetFixedBoundaryCondition_Field(id_field_def_fix1,world);
ls.InitializeMarge();
Fem::Eqn::AddLinearSystem_DKT2D_Static(ls,world,id_field_deflect,id_field_rot);
double res = ls.FinalizeMarge();
prec.SetValue(ls.m_ls);
std::cout << "Residual : " << res << std::endl;
{
double tol = 1.0e-6;
unsigned int iter = 10000;
LsSol::Solve_CG(tol,iter,ls);
// Fem::Sol::Solve_PCG(tol,iter,ls,prec);
std::cout << iter << " " << tol << std::endl;
}
ls.UpdateValueOfField(id_field_deflect,world,VALUE);
ls.UpdateValueOfField(id_field_rot,world,VALUE);
drawer_ary.Clear();
// drawer_ary.PushBack( new View::CDrawerVector(id_field_rot,world) );
drawer_ary.PushBack( new View::CDrawerFace(id_field_deflect,false,world) );
drawer_ary.PushBack( new View::CDrawerEdge(id_field_deflect,false,world) );
drawer_ary.PushBack( new View::CDrawerEdge(id_field_deflect,true,world) );
camera.Fit( drawer_ary.GetBoundingBox(camera.GetRotMatrix3() ) );
}
iprob++;
if( iprob == nprob ) iprob=0;
}
static bool AddLinearSystem_Poisson2D_P1
(double alpha, double source,
Fem::Ls::CLinearSystem_Field& ls,
const unsigned int id_field_val, const CFieldWorld& world,
const unsigned int id_ea )
{
// std::cout << "Poisson2D Triangle 3-point 1st order" << std::endl;
assert( world.IsIdEA(id_ea) );
const CElemAry& ea = world.GetEA(id_ea);
assert( ea.ElemType() == TRI );
if( !world.IsIdField(id_field_val) ) return false;
const CField& field_val = world.GetField(id_field_val);
const CElemAry::CElemSeg& es_c_va = field_val.GetElemSeg(id_ea,CORNER,true, world);
const CElemAry::CElemSeg& es_c_co = field_val.GetElemSeg(id_ea,CORNER,false,world);
const unsigned int nno = 3;
const unsigned int ndim = 2;
unsigned int no_c[nno]; // 要素節点の全体節点番号
double value_c[nno]; // 要素節点の値
double coord_c[nno][ndim]; // 要素節点の座標
double emat[nno][nno]; // 要素剛性行列
double eres_c[nno]; // 要素節点等価内力、外力、残差ベクトル
CMatDia_BlkCrs& mat_cc = ls.GetMatrix( id_field_val,CORNER,world);
CVector_Blk& res_c = ls.GetResidual(id_field_val,CORNER,world);
const CNodeAry::CNodeSeg& ns_c_val = field_val.GetNodeSeg(CORNER,true,world);
const CNodeAry::CNodeSeg& ns_c_co = field_val.GetNodeSeg(CORNER,false,world);
for(unsigned int ielem=0;ielem<ea.Size();ielem++){
// 要素配列から要素セグメントの節点番号を取り出す
es_c_co.GetNodes(ielem,no_c);
for(unsigned int inoes=0;inoes<nno;inoes++){
ns_c_co.GetValue(no_c[inoes],coord_c[inoes]);
}
// 節点の値を取って来る
es_c_va.GetNodes(ielem,no_c);
for(unsigned int inoes=0;inoes<nno;inoes++){
ns_c_val.GetValue(no_c[inoes],&value_c[inoes]);
}
// 面積を求める
const double area = TriArea(coord_c[0],coord_c[1],coord_c[2]);
// 形状関数の微分を求める
double dldx[nno][ndim]; // 形状関数のxy微分
double const_term[nno]; // 形状関数の定数項
TriDlDx(dldx,const_term,coord_c[0],coord_c[1],coord_c[2]);
// 要素剛性行列を作る
for(unsigned int ino=0;ino<nno;ino++){
for(unsigned int jno=0;jno<nno;jno++){
emat[ino][jno] = alpha*area*(dldx[ino][0]*dldx[jno][0]+dldx[ino][1]*dldx[jno][1]);
}
}
// 要素節点等価外力ベクトルを求める
for(unsigned int ino=0;ino<nno;ino++){
eres_c[ino] = source*area*0.33333333333333333;
}
// 要素節点等価内力ベクトルを求める
for(unsigned int ino=0;ino<nno;ino++){
for(unsigned int jno=0;jno<nno;jno++){
eres_c[ino] -= emat[ino][jno]*value_c[jno];
}
}
// 要素剛性行列にマージする
mat_cc.Mearge(nno,no_c,nno,no_c,1,&emat[0][0]);
// 残差ベクトルにマージする
for(unsigned int inoes=0;inoes<nno;inoes++){
res_c.AddValue( no_c[inoes],0,eres_c[inoes]);
}
}
return true;
}
static bool AddLinearSystem_Poisson3D_Q1(
double alpha, double source,
Fem::Ls::CLinearSystem_Field& ls,
const unsigned int id_field_val, const CFieldWorld& world,
const unsigned int id_ea)
{
// std::cout << "Poisson3D Hex" << std::endl;
assert( world.IsIdEA(id_ea) );
const CElemAry& ea = world.GetEA(id_ea);
assert( ea.ElemType() == HEX );
if( !world.IsIdField(id_field_val) ) return false;
const CField& field_val = world.GetField(id_field_val);
const CElemAry::CElemSeg& es_c_co = field_val.GetElemSeg(id_ea,CORNER,false,world);
const CElemAry::CElemSeg& es_c_va = field_val.GetElemSeg(id_ea,CORNER,true, world);
unsigned int num_integral = 1;
const unsigned int nInt = NIntLineGauss[num_integral];
const double (*Gauss)[2] = LineGauss[num_integral];
const unsigned int nno = 8;
const unsigned int ndim = 3;
unsigned int no_c_co[nno]; // 要素節点の全体節点番号
unsigned int no_c_va[nno]; // 要素節点の全体節点番号
double value_c[nno]; // 要素節点の値
double coord_c[nno][ndim]; // 要素節点の座標
double dndx[nno][ndim]; // 形状関数のxy微分
double an_c[nno];
double emat[nno][nno]; // 要素剛性行列
double eres_c[nno]; // 要素節点等価内力、外力、残差ベクトル
CMatDia_BlkCrs& mat_cc = ls.GetMatrix( id_field_val,CORNER,world);
CVector_Blk& res_c = ls.GetResidual(id_field_val,CORNER,world);
const CNodeAry::CNodeSeg& ns_c_val = field_val.GetNodeSeg(CORNER,true, world);
const CNodeAry::CNodeSeg& ns_c_co = field_val.GetNodeSeg(CORNER,false,world);
for(unsigned int ielem=0;ielem<ea.Size();ielem++){
// 要素配列から要素セグメントの節点番号を取り出す
es_c_co.GetNodes(ielem,no_c_co);
for(unsigned int ino=0;ino<nno;ino++){
ns_c_co.GetValue(no_c_co[ino],coord_c[ino]);
}
es_c_va.GetNodes(ielem,no_c_va);
for(unsigned int ino=0;ino<nno;ino++){
ns_c_val.GetValue(no_c_va[ino],&value_c[ino]);
}
for(unsigned int ino=0;ino<nno;ino++){
for(unsigned int jno=0;jno<nno;jno++){
emat[ino][jno] = 0.0;
}
}
for(unsigned int ino=0;ino<nno;ino++){
eres_c[ino] = 0.0;
}
double vol = 0.0;
for(unsigned int ir1=0;ir1<nInt;ir1++){
for(unsigned int ir2=0;ir2<nInt;ir2++){
for(unsigned int ir3=0;ir3<nInt;ir3++){
const double r1 = Gauss[ir1][0];
const double r2 = Gauss[ir2][0];
const double r3 = Gauss[ir3][0];
double detjac;
ShapeFunc_Hex8(r1,r2,r3,coord_c,detjac,dndx,an_c);
const double detwei = detjac*Gauss[ir1][1]*Gauss[ir2][1]*Gauss[ir3][1];
vol += detwei;
for(unsigned int ino=0;ino<nno;ino++){
for(unsigned int jno=0;jno<nno;jno++){
emat[ino][jno] += alpha*detwei*(dndx[ino][0]*dndx[jno][0]+dndx[ino][1]*dndx[jno][1]+dndx[ino][2]*dndx[jno][2]);
}
}
// 要素節点等価外力ベクトルを積n分毎に足し合わせる
for(unsigned int ino=0;ino<nno;ino++){
eres_c[ino] += detwei*source*an_c[ino];
}
}
}
}
// 要素節点等価内力ベクトルを求める
for(unsigned int ino=0;ino<nno;ino++){
for(unsigned int jno=0;jno<nno;jno++){
eres_c[ino] -= emat[ino][jno]*value_c[jno];
}
}
// 要素剛性行列にマージする
mat_cc.Mearge(nno,no_c_va,nno,no_c_va,1,&emat[0][0]);
// 残差ベクトルにマージする
for(unsigned int ino=0;ino<nno;ino++){
res_c.AddValue( no_c_va[ino],0,eres_c[ino]);
}
}
return true;
}
static bool AddLinearSystem_Poisson2D_P1b(
double alpha, double source,
Fem::Ls::CLinearSystem_Field& ls,
const unsigned int id_field_val, const CFieldWorld& world,
const unsigned int id_ea)
{
// std::cout << "Poisson2D TriP1b" << std::endl;
assert( world.IsIdEA(id_ea) );
const CElemAry& ea = world.GetEA(id_ea);
assert( ea.ElemType() == TRI );
if( !world.IsIdField(id_field_val) ) return false;
const CField& field_val = world.GetField(id_field_val);
const CElemAry::CElemSeg& es_c = field_val.GetElemSeg(id_ea,CORNER,true,world);
const CElemAry::CElemSeg& es_b = field_val.GetElemSeg(id_ea,BUBBLE,true,world);
const unsigned int nno_c = 3;
const unsigned int nno_b = 1;
const unsigned int ndim = 2;
unsigned int no_c[nno_c]; // 要素節点の全体節点番号
unsigned int no_b; // 要素節点の全体節点番号
double value_c[nno_c], value_b; // 要素節点の値
double coord_c[nno_c][ndim]; // 要素節点の座標
double dldx[nno_c][ndim]; // 形状関数のxy微分
double const_term[nno_c]; // 形状関数の定数項
double emat_cc[nno_c][nno_c], emat_bb, emat_cb[nno_c], emat_bc[nno_c]; // 要素剛性行列
double eqf_in_c[nno_c], eqf_out_c[nno_c], eres_c[nno_c]; // 要素節点等価内力、外力、残差ベクトル
double eqf_in_b, eqf_out_b, eres_b; // 要素節点等価内力、外力、残差ベクトル
CMatDia_BlkCrs& mat_cc = ls.GetMatrix(id_field_val,CORNER,world);
CMatDia_BlkCrs& mat_bb = ls.GetMatrix(id_field_val,BUBBLE,world);
CMat_BlkCrs& mat_cb = ls.GetMatrix(id_field_val,CORNER, id_field_val,BUBBLE, world);
CMat_BlkCrs& mat_bc = ls.GetMatrix(id_field_val,BUBBLE, id_field_val,CORNER, world);
CVector_Blk& res_c = ls.GetResidual(id_field_val,CORNER,world);
CVector_Blk& res_b = ls.GetResidual(id_field_val,BUBBLE,world);
const CNodeAry::CNodeSeg& ns_c_val = field_val.GetNodeSeg(CORNER,true, world);
const CNodeAry::CNodeSeg& ns_b_val = field_val.GetNodeSeg(BUBBLE,true, world);
const CNodeAry::CNodeSeg& ns_c_co = field_val.GetNodeSeg(CORNER,false,world);
for(unsigned int ielem=0;ielem<ea.Size();ielem++)
{
// 要素配列から要素セグメントの節点番号を取り出す
es_c.GetNodes(ielem,no_c);
es_b.GetNodes(ielem,&no_b);
// 節点の値を取ってくる
for(unsigned int inoes=0;inoes<nno_c;inoes++){
ns_c_co.GetValue(no_c[inoes],coord_c[inoes]);
ns_c_val.GetValue(no_c[inoes],&value_c[inoes]);
}
ns_b_val.GetValue(no_b,&value_b);
// 面積を求める
const double area = TriArea(coord_c[0],coord_c[1],coord_c[2]);
// 形状関数の微分を求める
TriDlDx(dldx,const_term,coord_c[0],coord_c[1],coord_c[2]);
{ // 要素剛性行列を作る
double vc_b[4];
vc_b[0] = 1.0/3.0; vc_b[1] = 1.0/3.0; vc_b[2] = 1.0/3.0; vc_b[3] = 27.0;
const double tmp_val1 = vc_b[3]*vc_b[3]*area/180.0*(
dldx[0][0]*dldx[0][0]+dldx[0][1]*dldx[0][1]+
dldx[1][0]*dldx[1][0]+dldx[1][1]*dldx[1][1]+
dldx[2][0]*dldx[2][0]+dldx[2][1]*dldx[2][1] );
double tmp1;
for(unsigned int ino_c=0;ino_c<nno_c;ino_c++){
for(unsigned int jno_c=0;jno_c<nno_c;jno_c++){
tmp1 =
area*(dldx[ino_c][0]*dldx[jno_c][0]+dldx[ino_c][1]*dldx[jno_c][1])
+vc_b[ino_c]*vc_b[jno_c]*tmp_val1;
emat_cc[ino_c][jno_c] = tmp1;
}
}
for(unsigned int ino_c=0;ino_c<nno_c;ino_c++){
tmp1 = -1.0*vc_b[ino_c]*tmp_val1;
emat_cb[ino_c] = tmp1;
emat_bc[ino_c] = tmp1;
}
emat_bb = tmp_val1;
}
// 要素節点等価外力ベクトルを求める
for(unsigned int ino_c=0;ino_c<nno_c;ino_c++){
eqf_out_c[ino_c] = source*area*11.0/60.0;
}
eqf_out_b = source*area*27.0/60.0;
// 要素節点等価内力ベクトルを求める
for(unsigned int ino_c=0;ino_c<nno_c;ino_c++){
eqf_in_c[ino_c] = 0.0;
for(unsigned int jno_c=0;jno_c<nno_c;jno_c++){
eqf_in_c[ino_c] += emat_cc[ino_c][jno_c]*value_c[jno_c];
}
eqf_in_c[ino_c] += emat_cb[ino_c]*value_b;
}
eqf_in_b = 0.0;
for(unsigned int jno_c=0;jno_c<nno_c;jno_c++){
eqf_in_b += emat_bc[jno_c]*value_c[jno_c];
}
eqf_in_b += emat_bb*value_b;
// 要素節点等価残差ベクトルを求める
for(unsigned int ino_c=0;ino_c<nno_c;ino_c++){
eres_c[ino_c] = eqf_out_c[ino_c] - eqf_in_c[ino_c];
}
eres_b = eqf_out_b - eqf_in_b;
// 要素剛性行列の全体剛性行列へのマージ
mat_cc.Mearge(nno_c,no_c,nno_c,no_c, 1,&emat_cc[0][0]);
mat_cb.Mearge(nno_c,no_c,nno_b,&no_b, 1,&emat_cb[0] );
mat_bc.Mearge(nno_b,&no_b,nno_c,no_c, 1,&emat_bc[0] );
mat_bb.Mearge(nno_b,&no_b,nno_b,&no_b, 1,&emat_bb );
// 残差ベクトルのマージ
for(unsigned int inoes=0;inoes<nno_c;inoes++){
res_c.AddValue( no_c[inoes],0,eres_c[inoes]);
}
res_b.AddValue( no_b,0,eres_b );
}
return true;
}
bool AddLinSys_FrictionalContact_Penalty_NonStatic_Sensitivity
(Fem::Ls::CLinearSystem_Field& ls,
const CContactTarget3D& ct, double stiff_n, double stiff_f, double myu_s, double myu_k,
double offset,
unsigned int id_field_disp,
Fem::Field::CFieldWorld& world,
std::vector<CFrictionPoint>& aFrictionPoint )
{
if( !world.IsIdField(id_field_disp) ) return false;
const Fem::Field::CField& field_disp = world.GetField(id_field_disp);
if( field_disp.GetFieldType() != Fem::Field::VECTOR3 ) return false;
////////////////
MatVec::CMatDia_BlkCrs& pmat_dd = ls.GetMatrix(id_field_disp, CORNER,world);
MatVec::CVector_Blk& res_d = ls.GetResidual(id_field_disp, CORNER,world);
const unsigned int ndim = 3;
const CNodeAry::CNodeSeg& ns_co = field_disp.GetNodeSeg( CORNER,false,world,VALUE);
const CNodeAry::CNodeSeg& ns_udisp = field_disp.GetNodeSeg( CORNER,true, world,VALUE);
const CNodeAry::CNodeSeg& ns_vdisp = field_disp.GetNodeSeg( CORNER,true, world,VELOCITY);
assert( aFrictionPoint.size() == ns_co.Size() );
for(unsigned int inode=0;inode<ns_co.Size();inode++)
{
double Co[ndim]; ns_co.GetValue( inode,Co);
double ud[ndim]; ns_udisp.GetValue(inode,ud);
double co[3] = { Co[0]+ud[0], Co[1]+ud[1], Co[2]+ud[2] };
CFrictionPoint& fp = aFrictionPoint[inode];
double n0[3];
const double pd = ct.Projection(co[0],co[1],co[2], n0)+offset;
fp.pd = pd;
if( pd < 0 ){
fp.itype_contact = 0;
continue;
}
double eKmat[3][3];
for(unsigned int i=0;i<3;i++){
for(unsigned int j=0;j<3;j++){
eKmat[i][j] = stiff_n*n0[i]*n0[j];
}
}
double eres_d[3];
eres_d[0] = stiff_n*n0[0]*pd;
eres_d[1] = stiff_n*n0[1]*pd;
eres_d[2] = stiff_n*n0[2]*pd;
for(unsigned int i=0;i<3;i++){
for(unsigned int j=0;j<3;j++){
eKmat[i][j] += -n0[i]*n0[j]*stiff_f;
}
eKmat[i][i] += stiff_f;
}
double ap_t[3] = { co[0]-fp.aloc[0], co[1]-fp.aloc[1], co[2]-fp.aloc[2] };
for(unsigned int i=0;i<3;i++){ eres_d[i] += -stiff_f*ap_t[i]; }
pmat_dd.Mearge(1,&inode, 1,&inode, 9, &eKmat[0][0]);
res_d.AddValue(inode,0,eres_d[0]);
res_d.AddValue(inode,1,eres_d[1]);
res_d.AddValue(inode,2,eres_d[2]);
}
return true;
}
bool AddLinSys_FrictionalContact_Penalty_NonStatic_BackwardEular
(double dt,
Fem::Ls::CLinearSystem_Field& ls,
const CContactTarget3D& ct, double stiff_n, double stiff_f, double myu_s, double myu_k,
double offset,
unsigned int id_field_disp,
Fem::Field::CFieldWorld& world,
std::vector<CFrictionPoint>& aFrictionPoint )
{
if( !world.IsIdField(id_field_disp) ) return false;
const Fem::Field::CField& field_disp = world.GetField(id_field_disp);
if( field_disp.GetFieldType() != Fem::Field::VECTOR3 ) return false;
////////////////
MatVec::CMatDia_BlkCrs& pmat_dd = ls.GetMatrix(id_field_disp, CORNER,world);
MatVec::CVector_Blk& res_d = ls.GetResidual(id_field_disp, CORNER,world);
const unsigned int ndim = 3;
const CNodeAry::CNodeSeg& ns_co = field_disp.GetNodeSeg( CORNER,false,world,VALUE);
const CNodeAry::CNodeSeg& ns_udisp = field_disp.GetNodeSeg( CORNER,true, world,VALUE);
const CNodeAry::CNodeSeg& ns_vdisp = field_disp.GetNodeSeg( CORNER,true, world,VELOCITY);
assert( aFrictionPoint.size() == ns_co.Size() );
for(unsigned int inode=0;inode<ns_co.Size();inode++)
{
double Co[ndim]; ns_co.GetValue( inode,Co);
double ud[ndim]; ns_udisp.GetValue(inode,ud);
double uv[ndim]; ns_vdisp.GetValue(inode,uv);
double co[3] = { Co[0]+ud[0], Co[1]+ud[1], Co[2]+ud[2] };
CFrictionPoint& fp = aFrictionPoint[inode];
if( fp.is_pin )
{
double n[3] = { fp.aloc[0]-co[0], fp.aloc[1]-co[1], fp.aloc[2]-co[2] };
double eKmat[3][3] = { {0,0,0},{0,0,0},{0,0,0} };
for(unsigned int i=0;i<3;i++){ eKmat[i][i] = stiff_n; }
double eres_d[3];
eres_d[0] = stiff_n*n[0]*dt;
eres_d[1] = stiff_n*n[1]*dt;
eres_d[2] = stiff_n*n[2]*dt;
double emat_dd[3][3];
for(unsigned int i=0;i<9;i++){ (&emat_dd[0][0])[i] = dt*dt*(&eKmat[0][0])[i]; }
{
eres_d[0] -= (eKmat[0][0]*uv[0]+eKmat[0][1]*uv[1]+eKmat[0][2]*uv[2])*dt*dt;
eres_d[1] -= (eKmat[1][0]*uv[0]+eKmat[1][1]*uv[1]+eKmat[1][2]*uv[2])*dt*dt;
eres_d[2] -= (eKmat[2][0]*uv[0]+eKmat[2][1]*uv[1]+eKmat[2][2]*uv[2])*dt*dt;
}
pmat_dd.Mearge(1,&inode, 1,&inode, 9, &emat_dd[0][0]);
res_d.AddValue(inode,0,eres_d[0]);
res_d.AddValue(inode,1,eres_d[1]);
res_d.AddValue(inode,2,eres_d[2]);
continue;
}
double n0[3];
const double pd = ct.Projection(co[0],co[1],co[2], n0)+offset;
fp.pd = pd;
if( pd < 0 ){
fp.itype_contact = 0;
continue;
}
double eKmat[3][3];
for(unsigned int i=0;i<3;i++){
for(unsigned int j=0;j<3;j++){
eKmat[i][j] = stiff_n*n0[i]*n0[j];
}
}
double eres_d[3];
eres_d[0] = stiff_n*n0[0]*pd*dt;
eres_d[1] = stiff_n*n0[1]*pd*dt;
eres_d[2] = stiff_n*n0[2]*pd*dt;
// friction handling
double ap_t[3] = { co[0]-fp.aloc[0], co[1]-fp.aloc[1], co[2]-fp.aloc[2] };
{ // tangent vector from anchor to point
const double t = Com::Dot3D(n0,ap_t);
for(unsigned int i=0;i<3;i++){ ap_t[i] -= t*n0[i]; }
}
double velo_t[3] = { uv[0],uv[1],uv[2] };
{ // tangent velocity
const double t = Com::Dot3D(n0,velo_t);
for(unsigned int i=0;i<3;i++){ velo_t[i] -= t*n0[i]; }
}
const double len_ap_t = Com::Length3D(ap_t);
const double len_velo_t = Com::Length3D(velo_t);
const double force_f = len_ap_t*stiff_f;
const double force_n = pd*stiff_n;
if( force_f < force_n*myu_s && len_velo_t < 1.0e-1 ){
fp.itype_contact = 1;
for(unsigned int i=0;i<3;i++){ eres_d[i] += -dt*stiff_f*ap_t[i]; }
for(unsigned int i=0;i<3;i++){
for(unsigned int j=0;j<3;j++){
eKmat[i][j] += -n0[i]*n0[j]*stiff_f;
}
eKmat[i][i] += stiff_f;
}
}
else{
// std::cout << "dynamic friction" << std::endl;
fp.itype_contact = 2;
if( len_velo_t > 1.0e-10 ){
const double invlen = 1.0/len_velo_t;
for(unsigned int i=0;i<3;i++){ velo_t[i] *= invlen; }
for(unsigned int i=0;i<3;i++){ eres_d[i] += -dt*velo_t[i]*force_n*myu_k; }
for(unsigned int i=0;i<3;i++){
for(unsigned int j=0;j<3;j++){
eKmat[i][j] += -velo_t[i]*velo_t[j]*force_n*myu_k*invlen;
}
eKmat[i][i] += force_n*myu_k*invlen;
}
}
}
////////////////
double emat_dd[3][3];
for(unsigned int i=0;i<9;i++){ (&emat_dd[0][0])[i] = dt*dt*(&eKmat[0][0])[i]; }
{
eres_d[0] -= (eKmat[0][0]*uv[0]+eKmat[0][1]*uv[1]+eKmat[0][2]*uv[2])*dt*dt;
eres_d[1] -= (eKmat[1][0]*uv[0]+eKmat[1][1]*uv[1]+eKmat[1][2]*uv[2])*dt*dt;
eres_d[2] -= (eKmat[2][0]*uv[0]+eKmat[2][1]*uv[1]+eKmat[2][2]*uv[2])*dt*dt;
}
pmat_dd.Mearge(1,&inode, 1,&inode, 9, &emat_dd[0][0]);
res_d.AddValue(inode,0,eres_d[0]);
res_d.AddValue(inode,1,eres_d[1]);
res_d.AddValue(inode,2,eres_d[2]);
}
return true;
}
