Ejemplo n.º 1
0
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 ){	// 2次元問題の設定
		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;
}
Ejemplo n.º 2
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;
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
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;
}