static bool AddLinearSystem_Diffusion3D_P1(
		double rho, double alpha, double source,
		CLinearSystem_SaveDiaM_Newmark& ls, 
		unsigned int id_field_val, const CFieldWorld& world,
		unsigned int id_ea )
{
//	std::cout << "Diffusion3D Tet savemat" << std::endl;

	assert( world.IsIdEA(id_ea) );
	const CElemAry& ea = world.GetEA(id_ea);
	assert( ea.ElemType() == TET );

	if( !world.IsIdField(id_field_val) ) return false;
	const CField& field_val = world.GetField(id_field_val);

	const double gamma = ls.GetGamma();
	const double dt = ls.GetDt();

	const CElemAry::CElemSeg& es_c = field_val.GetElemSeg(id_ea,CORNER,true,world);

	const unsigned int nno = 4;
	const unsigned int ndim = 3;

	unsigned int no_c[nno];	// 要素節点の全体節点番号

	double val_c[nno];		// 要素節点の値
	double vval_c[nno];		// 要素節点の値
	double coord_c[nno][ndim];	// 要素節点の座標
				
	double emat[nno][nno];
	double eCmat[nno][nno];	// 要素剛性行列
	double eMmat[nno][nno];	// 要素剛性行列
	double eqf_out_c[nno];	// 要素節点等価内力、外力、残差ベクトル
				
	CMatDia_BlkCrs& mat_cc  = ls.GetMatrix(id_field_val,CORNER,world);
	CVector_Blk&    force_c = ls.GetForce( id_field_val,CORNER,world);
	
	CMat_BlkCrs& mat_cc_bound = ls.GetMatrix_Boundary(id_field_val,CORNER,  id_field_val,CORNER,  world);
	CDiaMat_Blk& mat_mass     = ls.GetDiaMassMatrix(  id_field_val,CORNER,world);

	const CNodeAry::CNodeSeg& ns_c_val = field_val.GetNodeSeg(CORNER,true,world,VALUE);	
	const CNodeAry::CNodeSeg& ns_c_vval = field_val.GetNodeSeg(CORNER,true,world,VELOCITY);
	const CNodeAry::CNodeSeg& ns_c_co = field_val.GetNodeSeg(CORNER,false,world,VALUE);	

	for(unsigned int ielem=0;ielem<ea.Size();ielem++)
	{
		// 要素配列から要素セグメントの節点番号を取り出す
		es_c.GetNodes(ielem,no_c);
		// 節点の値を取って来る
		for(unsigned int ino=0;ino<nno;ino++){
			ns_c_co.GetValue(no_c[ino],coord_c[ino]);
			ns_c_val.GetValue(no_c[ino],&val_c[ino]);
			ns_c_vval.GetValue(no_c[ino],&vval_c[ino]);
		}

		////////////////////////////////////////////////////////////////

		// 面積を求める
		const double vol = TetVolume(coord_c[0],coord_c[1],coord_c[2],coord_c[3]);
		// 形状関数の微分を求める
		double dldx[nno][ndim];	// 形状関数のxy微分
		double const_term[nno];	// 形状関数の定数項
		TetDlDx(dldx,const_term,coord_c[0],coord_c[1],coord_c[2],coord_c[3]);
		// 要素剛性行列を作る
		for(unsigned int ino=0;ino<nno;ino++){
		for(unsigned int jno=0;jno<nno;jno++){
			eCmat[ino][jno] = alpha*vol*( dldx[ino][0]*dldx[jno][0]+dldx[ino][1]*dldx[jno][1]+dldx[ino][2]*dldx[jno][2]);
		}
		}
		{
			const double dtmp1 = rho*vol*0.05;
			for(unsigned int ino=0;ino<nno;ino++){
				for(unsigned int jno=0;jno<nno;jno++){
					eMmat[ino][jno] = dtmp1;
				}
				eMmat[ino][ino] += dtmp1;
			}
		}
		// 要素節点等価外力ベクトルを求める
		for(unsigned int ino=0;ino<nno;ino++){
			eqf_out_c[ino] = source*vol*0.25;
		}

		////////////////////////////////////////////////////////////////

		{	// 要素係数行列を求める
			double dtmp1 = gamma*dt;
			for(unsigned int i=0;i<nno*nno;i++){
				(&emat[0][0])[i] = (&eMmat[0][0])[i]+dtmp1*(&eCmat[0][0])[i];
			}
		}

		// 剛性行列にマージする
		mat_cc      .Mearge(nno,no_c,nno,no_c,  1, &emat[0][0]);
		mat_cc_bound.Mearge(nno,no_c,nno,no_c,  1,&eCmat[0][0]);
		for(unsigned int ino=0;ino<nno;ino++){
			mat_mass.Mearge( no_c[ino], 1, &eMmat[0][0] );
        }

		// 残差ベクトルにマージする
		for(unsigned int ino=0;ino<nno;ino++){
			force_c.AddValue( no_c[ino],0,eqf_out_c[ino]);
		}
	}
	return true;
}
static bool AddLinSys_AdvectionDiffusion_NonStatic_Newmark_P1P1(
		double rho, double myu, double source, 
		CLinearSystem_SaveDiaM_Newmark& ls, 
		const unsigned int id_field_val, const unsigned int id_field_velo, const CFieldWorld& world, 
		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& val_field = world.GetField(id_field_val);

	if( !world.IsIdField(id_field_velo) ) return false;
	const CField& field_velo = world.GetField(id_field_velo);

	const double gamma = ls.GetGamma();
	const double dt = ls.GetDt();

	const unsigned int nno = 3;
	const unsigned int ndim = 2;

	const CElemAry::CElemSeg& es_c_val = val_field.GetElemSeg(id_ea,CORNER,true,world);

	double emat[nno][nno];	// 要素剛性行列
	double eqf_out_c[nno];	// 要素節点等価内力、外力、残差ベクトル
				
	CMatDia_BlkCrs& mat_cc  = ls.GetMatrix(id_field_val,CORNER,world);
	CVector_Blk&    force_c = ls.GetForce( id_field_val,CORNER,world);

	CMat_BlkCrs& mat_cc_bound = ls.GetMatrix_Boundary(id_field_val,CORNER,id_field_val,CORNER,world);

	const CNodeAry::CNodeSeg& ns_c_val = val_field.GetNodeSeg(CORNER,true,world,VALUE);		//na_c_val.GetSeg(id_ns_c_val);
	const CNodeAry::CNodeSeg& ns_c_vval = val_field.GetNodeSeg(CORNER,true,world,VELOCITY);	//na_c_val.GetSeg(id_ns_c_vval);
	const CNodeAry::CNodeSeg& ns_c_velo = field_velo.GetNodeSeg(CORNER,true,world,VELOCITY);//na_c_velo.GetSeg(id_ns_c_velo);
	const CNodeAry::CNodeSeg& ns_c_co = field_velo.GetNodeSeg(CORNER,false,world,VALUE);	//na_c_co.GetSeg(id_ns_c_co);

	for(unsigned int ielem=0;ielem<ea.Size();ielem++)
	{
		// 要素配列から要素セグメントの節点番号を取り出す
		unsigned int no_c[nno];	// 要素節点の全体節点番号
		es_c_val.GetNodes(ielem,no_c);
		// 節点の値を取って来る
		double val_c[nno], vval_c[nno];		// 要素節点の値
		double coord_c[nno][ndim];	// 要素節点の座標
		double velo_c[nno][ndim];	// advection velocity
		for(unsigned int ino=0;ino<nno;ino++){
			ns_c_val.GetValue(no_c[ino],&val_c[ino]);
			ns_c_vval.GetValue(no_c[ino],&vval_c[ino]);
			ns_c_velo.GetValue(no_c[ino],velo_c[ino]);
			ns_c_co.GetValue(no_c[ino],coord_c[ino]);
		}

		////////////////////////////////////////////////////////////////

		// 面積を求める
		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]);

		double eCmat[nno][nno];
		// 要素剛性行列を作る
		for(unsigned int ino=0;ino<nno;ino++){
		for(unsigned int jno=0;jno<nno;jno++){
			eCmat[ino][jno] = myu*area*(dldx[ino][0]*dldx[jno][0]+dldx[ino][1]*dldx[jno][1]);
		}
		}
		{
			const double dtmp1 = area/12.0;
			for(unsigned int ino=0;ino<nno;ino++){
				const double dtmp_0 = dtmp1*(velo_c[0][0]+velo_c[1][0]+velo_c[2][0]+velo_c[ino][0]);
				const double dtmp_1 = dtmp1*(velo_c[0][1]+velo_c[1][1]+velo_c[2][1]+velo_c[ino][1]);
                for(unsigned int jno=0;jno<nno;jno++){
					eCmat[ino][jno] += dldx[jno][0]*dtmp_0+dldx[jno][1]*dtmp_1;
				}
			}
		}

		// Calc Stabilization Parameter
		double tau;
		{
			const double velo_ave[2] = { 
				(velo_c[0][0]+velo_c[1][0]+velo_c[2][0])/3.0, 
				(velo_c[0][1]+velo_c[1][1]+velo_c[2][1])/3.0 };
			const double norm_v = sqrt(velo_ave[0]*velo_ave[0]+velo_ave[1]*velo_ave[1]);
			const double velo_dir[2] = { velo_ave[0]/norm_v, velo_ave[1]/norm_v };			
			double h;
			{	// calc element length along the direction of velocity
				double dtmp1 = 0;
				for(int inode=0;inode<3;inode++){
					dtmp1 += fabs(velo_dir[0]*dldx[inode][0]+velo_dir[1]*dldx[inode][1]);
				}
				h = 2.0/dtmp1;
			}
			// calc stabilization parameter
			if( norm_v*h*rho < 6.0*myu ){
				const double re_c = 0.5*norm_v*h*rho/myu;	// 0.5*norm_v*h*rho/myu;
				tau = h * 0.5 / norm_v * re_c / 3.0;
			}
			else{ tau = h * 0.5 / norm_v; }
//			tau *= 0.1;
		}

		{
			double tmp_mat[ndim][ndim];
			for(unsigned int idim=0;idim<ndim;idim++){
			for(unsigned int jdim=0;jdim<ndim;jdim++){
				double dtmp1 = 0.0;
				for(unsigned int ino=0;ino<nno;ino++){
					for(unsigned int jno=0;jno<nno;jno++){
						dtmp1 += velo_c[ino][idim]*velo_c[jno][jdim];
					}
					dtmp1 += velo_c[ino][idim]*velo_c[ino][jdim];
				}
				tmp_mat[idim][jdim] = area*tau*dtmp1/12.0;
			}
			}
			for(unsigned int ino=0;ino<nno;ino++){
			for(unsigned int jno=0;jno<nno;jno++){
				double dtmp1 = 0.0;
				for(unsigned int idim=0;idim<ndim;idim++){
				for(unsigned int jdim=0;jdim<ndim;jdim++){
					dtmp1 += dldx[ino][idim]*dldx[jno][jdim]*tmp_mat[idim][jdim];
				}
				}
				eCmat[ino][jno] += dtmp1;
			}
			}
		}

		double eMmat[nno][nno];
		{
			const double dtmp1 = rho*area*0.08333333333333333333333333;
			for(unsigned int ino=0;ino<nno;ino++){
				for(unsigned int jno=0;jno<nno;jno++){
					eMmat[ino][jno] = dtmp1;
				}
				eMmat[ino][ino] += dtmp1;
			}
		}

		// 要素節点等価外力ベクトルを求める
		for(unsigned int ino=0;ino<nno;ino++){
			eqf_out_c[ino] = source*area*0.333333333333333333333;
		}

		////////////////////////////////////////////////////////////////

		{	// 要素係数行列を求める
			double dtmp1 = gamma*dt;
			for(unsigned int i=0;i<nno*nno;i++){
				(&emat[0][0])[i] = (&eMmat[0][0])[i]+dtmp1*(&eCmat[0][0])[i];
			}
		}
		// 要素剛性行列にマージする
		mat_cc      .Mearge(nno,no_c,nno,no_c,1, &emat[0][0]);
		mat_cc_bound.Mearge(nno,no_c,nno,no_c,1,&eCmat[0][0]);
		// 残差ベクトルにマージする
		for(unsigned int ino=0;ino<nno;ino++){
			force_c.AddValue( no_c[ino],0,eqf_out_c[ino]);
		}
	}
	return true;
}
static bool AddLinearSystem_Diffusion2D_P1(
		double rho, double alpha, double source,
		CLinearSystem_SaveDiaM_Newmark& ls, 
		unsigned int id_field_val, const CFieldWorld& world,
		const unsigned int id_ea)
{
//	std::cout << "Diffusion2D Tri P1 savemat " << gamma << " " << dt << std::endl;

	const double gamma = ls.GetGamma();
	const double dt = ls.GetDt();

	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_val = 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 coord_c[nno][ndim];	// 要素節点の座標
				
	double emat[nno][nno];
	double eCmat[nno][nno];	// 要素剛性行列
	double eMmat[nno];	// 要素剛性行列
	double eqf_out_c[nno];	// 要素節点等価内力、外力、残差ベクトル
				
	CMatDia_BlkCrs& mat_cc   = ls.GetMatrix(id_field_val,CORNER,world);
	CVector_Blk&    force_c  = ls.GetForce( id_field_val,CORNER,world);
	
	CMat_BlkCrs& mat_cc_bound = ls.GetMatrix_Boundary(id_field_val,CORNER,  id_field_val,CORNER,  world);
	const CNodeAry::CNodeSeg& ns_c_co   = field_val.GetNodeSeg(CORNER,false,world,VALUE);

	for(unsigned int ielem=0;ielem<ea.Size();ielem++)
	{
		// 要素配列から要素セグメントの節点番号を取り出す
		es_c_co.GetNodes(ielem,no_c);
		for(unsigned int ino=0;ino<nno;ino++){
			ns_c_co.GetValue(no_c[ino],coord_c[ino]);
		}
		es_c_val.GetNodes(ielem,no_c);

		////////////////////////////////////////////////////////////////

		// 面積を求める
		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++){
			eCmat[ino][jno] = alpha*area*(dldx[ino][0]*dldx[jno][0]+dldx[ino][1]*dldx[jno][1]);
		}
		}
		{
			const double dtmp1 = rho*area/3.0;
			for(unsigned int ino=0;ino<nno;ino++){
				eMmat[ino] = dtmp1;
			}
		}
		// 要素節点等価外力ベクトルを求める
		for(unsigned int ino=0;ino<nno;ino++){
			eqf_out_c[ino] = source*area*0.33333333333333;
		}

		////////////////////////////////////////////////////////////////

		{	// 要素係数行列を求める
			const double dtmp1 = gamma*dt;
			for(unsigned int ino=0;ino<nno;ino++){
				for(unsigned int jno=0;jno<nno;jno++){
					emat[ino][jno] = dtmp1*eCmat[ino][jno];
				}
				emat[ino][ino] += eMmat[ino];
			}
		}

		// 剛性行列にマージする
		mat_cc      .Mearge(nno,no_c,nno,no_c,  1, &emat[0][0]);
		mat_cc_bound.Mearge(nno,no_c,nno,no_c,  1,&eCmat[0][0]);
		// 残差ベクトルにマージする
		for(unsigned int ino=0;ino<nno;ino++){
			force_c.AddValue( no_c[ino],0,eqf_out_c[ino]);
		}
	}
	return true;
}