bool CLinearSystem_SaveDiaM_NewmarkBeta::UpdateValueOfField(
unsigned int id_field, Fem::Field::CFieldWorld& world, Fem::Field::FIELD_DERIVATION_TYPE fdt )
{
// std::cout << "CLinearSystem_SaveDiaM_NewmarkBeta::UpdateValueOfField" << std::endl;
if( !world.IsIdField(id_field) ) return false;
CField& field = world.GetField(id_field);
assert( fdt == ACCELERATION );
{
unsigned int id_na_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
unsigned int id_ns_a = field.GetNodeSegInNodeAry(CORNER).id_ns_ac;
if( id_na_val != 0 ){
const unsigned int ilss0 = this->FindIndexArray_Seg(id_field,CORNER,world);
const CLinSysSeg_Field& lss = this->m_aSegField[ilss0];
CVector_Blk* pUpdate = this->m_ls.m_Update[ilss0];
assert( pUpdate != 0 );
CNodeAry& na = world.GetNA(id_na_val);
const unsigned int nblk = na.Size();
CNodeAry::CNodeSeg& ns_u = field.GetNodeSeg(CORNER,true,world,VALUE);
CNodeAry::CNodeSeg& ns_v = field.GetNodeSeg(CORNER,true,world,VELOCITY);
CNodeAry::CNodeSeg& ns_a = field.GetNodeSeg(CORNER,true,world,ACCELERATION);
assert( ns_v.Length() == ns_u.Length() );
const unsigned int nlen = ns_u.Length();
const CBCFlag& bc_flag = m_ls.GetBCFlag(ilss0);//this->m_ls.m_BCFlag[ilss0];
assert( nblk == bc_flag.NBlk() );
double* velo0 = new double [nlen];
double* acc0 = new double [nlen];
if( lss.id_field == id_field ){
for(unsigned int iblk=0;iblk<nblk;iblk++){
ns_v.GetValue(iblk,velo0);
ns_a.GetValue(iblk, acc0);
for(unsigned int ilen=0;ilen<nlen;ilen++){
if( bc_flag.GetBCFlag(iblk,ilen) == 0 ){ // 固定境界条件が入ってなかったら
const double acc1 = pUpdate->GetValue(iblk,ilen); // 更新後の速度
// 値を更新
ns_u.AddValue(iblk,ilen,dt*velo0[ilen]+dt*dt*(0.5-beta)*acc0[ilen]+dt*dt*beta*acc1);
ns_v.AddValue(iblk,ilen,dt*(1-gamma)*acc0[ilen]+gamma*dt*acc1);
ns_a.AddValue(iblk,ilen,acc1);
}
else{ // 固定境界条件が入ってなかったら
// Updateに固定境界条件つき速度を入れる
pUpdate->SetValue(iblk,ilen,acc0[ilen]);
}
}
}
na.SetValueToNodeSegment(id_ns_a,*pUpdate);
}
else{
assert( lss.id_field2 == id_field );
unsigned int nlen0;
{
const unsigned int id_field0 = lss.id_field;
const CField& field = world.GetField(id_field0);
nlen0 = field.GetNLenValue();
}
for(unsigned int iblk=0;iblk<nblk;iblk++){
ns_v.GetValue(iblk,velo0);
ns_a.GetValue(iblk, acc0);
for(unsigned int ilen=0;ilen<nlen;ilen++){
if( bc_flag.GetBCFlag(iblk,ilen+nlen0) == 0 ){ // 固定境界条件が入ってなかったら
const double acc1 = pUpdate->GetValue(iblk,ilen+nlen0); // 更新後の速度
// 値を更新
ns_u.AddValue(iblk,ilen,dt*velo0[ilen]+dt*dt*(0.5-beta)*acc0[ilen]+dt*dt*beta*acc1);
ns_v.AddValue(iblk,ilen,dt*(1-gamma)*acc0[ilen]+gamma*dt*acc1);
ns_a.AddValue(iblk,ilen,acc1);
}
else{ // 固定境界条件が入ってなかったら
// Updateに固定境界条件つき速度を入れる
pUpdate->SetValue(iblk,ilen+nlen0,acc0[ilen]);
}
}
}
na.SetValueToNodeSegment(id_ns_a,*pUpdate,nlen0);
}
delete[] velo0;
delete[] acc0;
// 加速度の更新
}
}
return true;
}
bool CDrawerFace::Update
(const Fem::Field::CFieldWorld& world)
{
const Fem::Field::CField& field = world.GetField(m_id_field);
// set the vertex array
unsigned int id_na_c_co = field.GetNodeSegInNodeAry(CORNER).id_na_co;
assert( world.IsIdNA(id_na_c_co) );
const Fem::Field::CNodeAry& na_c_co = world.GetNA(id_na_c_co);
assert( field.IsNodeSeg(CORNER,false,world) );
const Fem::Field::CNodeAry::CNodeSeg& ns_c_co = field.GetNodeSeg(CORNER,false,world);
const unsigned int ndim = ns_c_co.Length();
const unsigned int npoin_co = na_c_co.Size();
unsigned int npoin;
{ // calc the number of point
if( m_is_draw_nsv ){
unsigned int id_na_c_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
assert( world.IsIdNA(id_na_c_val) );
const Fem::Field::CNodeAry& na_c_val = world.GetNA(id_na_c_val);
npoin = na_c_val.Size();
}
else{ npoin = npoin_co; }
}
// std::cout << m_vertex_ary.NPoin() << " " << npoin << std::endl;
assert( m_vertex_ary.NPoin() == npoin );
if( !m_isnt_value_disp ){ // including displacement
unsigned int id_na_c_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
assert( world.IsIdNA(id_na_c_val) );
const Fem::Field::CNodeAry& na_c_val = world.GetNA(id_na_c_val);
const Fem::Field::CNodeAry::CNodeSeg& ns_c_val = field.GetNodeSeg(CORNER,true,world,VALUE|VELOCITY|ACCELERATION);
if( ndim == 2 && (field.GetFieldType()==SCALAR||field.GetFieldType()==ZSCALAR) ) // 垂直方向の変位として捉える
{
assert( m_vertex_ary.NDim() == 3 );
double coord[2], value[2];
for(unsigned int ipoin=0;ipoin<npoin;ipoin++){
unsigned int ipoin_co = field.GetMapVal2Co(ipoin);
assert( ipoin_co < npoin_co );
ns_c_val.GetValue(ipoin,value);
ns_c_co.GetValue(ipoin_co,coord);
this->m_vertex_ary.pVertexArray[ipoin*3+0] = coord[0];
this->m_vertex_ary.pVertexArray[ipoin*3+1] = coord[1];
this->m_vertex_ary.pVertexArray[ipoin*3+2] = value[0];
}
}
else{
assert( m_vertex_ary.NDim() == ndim );
assert( ndim == ns_c_val.Length() ); // Coordの次元とValueの次元が合ってなければならない
assert( npoin_co == na_c_co.Size() );
assert( na_c_val.Size() == npoin );
assert( ndim <= 3 );
double coord[3], value[3];
for(unsigned int ipoin=0;ipoin<npoin;ipoin++){
unsigned int ipoin_co = field.GetMapVal2Co(ipoin);
assert( ipoin_co < npoin_co );
ns_c_val.GetValue(ipoin,value);
ns_c_co.GetValue(ipoin_co,coord);
for(unsigned int idim=0;idim<ndim;idim++){
this->m_vertex_ary.pVertexArray[ipoin*ndim+idim] = coord[idim]+value[idim];
}
}
}
}
else{
assert( m_vertex_ary.NDim() == ndim );
for(unsigned int ipoin=0;ipoin<npoin;ipoin++){
unsigned int ipoin_co = field.GetMapVal2Co(ipoin);
assert( ipoin_co < npoin_co );
double* pval = &this->m_vertex_ary.pVertexArray[ipoin*ndim];
ns_c_co.GetValue(ipoin_co,pval);
}
}
////////////////////////////////////////////////
// make color
if( world.IsIdField(id_field_val) )
{
const Fem::Field::CField& field_val = world.GetField(id_field_val);
Fem::Field::FIELD_DERIVATION_TYPE fdt;
{ // 描画する値の微分タイプ(できるだけVALUEを選択)
unsigned int fdt_all = field_val.GetFieldDerivativeType();
if( fdt_all & VALUE ){ fdt = VALUE; }
else if( fdt_all & VELOCITY ){ fdt = VELOCITY; }
else if( fdt_all & ACCELERATION ){ fdt = ACCELERATION; }
else{ assert(0); }
}
if( !color_map->IsMinMaxFix() ){ // 値の最大値最小値を求める
double min_val, max_val;
field_val.GetMinMaxValue(min_val,max_val,world,0,fdt);
color_map->SetMinMax(min_val,max_val);
}
unsigned int id_na_c_val = field_val.GetNodeSegInNodeAry(CORNER).id_na_va;
unsigned int id_na_b_val = field_val.GetNodeSegInNodeAry(BUBBLE).id_na_va;
if( world.IsIdNA(id_na_c_val) ){
const CNodeAry::CNodeSeg& ns_v = field_val.GetNodeSeg(CORNER,true,world,fdt);
double val[10];
if( pColorArray == 0 ){ pColorArray = new float [npoin*4]; }
for(unsigned int ipoin=0;ipoin<npoin;ipoin++){
ns_v.GetValue(ipoin,val);
color_map->GetColor(&pColorArray[ipoin*4],val[0]);
pColorArray[ipoin*4+3] = 0.0f;
}
}
else if( world.IsIdNA(id_na_b_val) ){
unsigned int id_ns_v = 0;
if( fdt == VALUE ){ id_ns_v = field_val.GetNodeSegInNodeAry(BUBBLE).id_ns_va; }
else if( fdt == VELOCITY ){ id_ns_v = field_val.GetNodeSegInNodeAry(BUBBLE).id_ns_ve; }
else if( fdt == ACCELERATION ){ id_ns_v = field_val.GetNodeSegInNodeAry(BUBBLE).id_ns_ac; }
for(unsigned int idp=0;idp<this->m_apIndexArrayElem.size();idp++){
View::CIndexArrayElem* pIA = this->m_apIndexArrayElem[idp];
unsigned int id_ea = pIA->GetIdEA();
unsigned int id_es_v = field_val.GetIdElemSeg(id_ea,BUBBLE,true,world);
pIA->SetColor(id_es_v,id_ns_v,world,color_map);
}
}
}
/////////////////////
// make normal
if( pNormalArray != 0 ){ MakeNormal(); }
/////////////////////
// make uv map
if( pUVArray != 0 ){
for(unsigned int ino=0;ino<ns_c_co.Size();ino++){
double c[3]; ns_c_co.GetValue(ino,c);
pUVArray[ino*2+0] = c[0]*tex_scale;
pUVArray[ino*2+1] = c[1]*tex_scale;
}
}
return true;
}
bool CLinearSystem_Save::UpdateValueOfField(
unsigned int id_field, Fem::Field::CFieldWorld& world,FIELD_DERIVATION_TYPE fdt)
{
if( !world.IsIdField(id_field) ){ assert(0); return false; }
const CField& field = world.GetField(id_field);
{ // コーナー節点について値を更新
unsigned int id_na_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
unsigned int id_ns_val = field.GetNodeSegInNodeAry(CORNER).id_ns_va;
if( id_na_val != 0 ){
const int ilss0 = this->FindIndexArray_Seg(id_field,CORNER,world);
assert( ilss0 >= 0 && ilss0 < (int)this->GetNLynSysSeg() );
const CLinSysSeg_Field& lss0 = this->m_aSegField[ilss0];
CVector_Blk& upd = this->m_ls.GetVector(-2,ilss0); // 固定境界条件をセットするので，constにできない
const CBCFlag& bc_flag = m_ls.GetBCFlag(ilss0);//this->m_ls.m_BCFlag[ilss0];
// const Fem::Field::CField& field = world.GetField(id_field);
CNodeAry& na = world.GetNA(id_na_val);
CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns_val);
if( lss0.id_field == id_field ){
{ // Updateに固定境界条件の値をセット
const unsigned int nblk = na.Size();
const unsigned int nlen = ns.Length();
assert( nblk == bc_flag.NBlk() );
double* val = new double [nlen];
for(unsigned int iblk=0;iblk<nblk;iblk++){
ns.GetValue(iblk,val);
for(unsigned int ilen=0;ilen<nlen;ilen++){
if( bc_flag.GetBCFlag(iblk,ilen) == 0 ) continue;
upd.SetValue(iblk,ilen,val[ilen]);
}
}
delete[] val;
}
na.SetValueToNodeSegment(id_ns_val,upd);
}
else{
assert( lss0.id_field2 == id_field );
const Fem::Field::CField& field1 = world.GetField(lss0.id_field);
const unsigned int ilen1 = field1.GetNLenValue();
{ // Updateに固定境界条件の値をセット
const unsigned int nblk = na.Size();
const unsigned int nlen = ns.Length();
assert( nblk == bc_flag.NBlk() );
assert( (int)(nlen+ilen1) == bc_flag.LenBlk() );
double* val = new double [nlen];
for(unsigned int iblk=0;iblk<nblk;iblk++){
ns.GetValue(iblk,val);
for(unsigned int ilen=0;ilen<nlen;ilen++){
if( bc_flag.GetBCFlag(iblk,ilen+ilen1) == 0 ) continue;
upd.SetValue(iblk,ilen+ilen1,val[ilen]);
}
}
delete[] val;
}
na.SetValueToNodeSegment(id_ns_val,upd,ilen1);
}
}
}
{ // 辺節点について値を更新
unsigned int id_na_val = field.GetNodeSegInNodeAry(EDGE).id_na_va;
unsigned int id_ns_val = field.GetNodeSegInNodeAry(EDGE).id_ns_va;
if( id_na_val != 0 ){
const unsigned int ilss0 = this->FindIndexArray_Seg(id_field,EDGE,world);
const CVector_Blk& upd = m_ls.GetVector(-2,ilss0);
CNodeAry& na = world.GetNA(id_na_val);
na.AddValueToNodeSegment(id_ns_val,upd,1.0);
// CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns_val);
// ns.is_updated = true;
}
}
{ // バブル節点について値を更新
unsigned int id_na_val = field.GetNodeSegInNodeAry(BUBBLE).id_na_va;
unsigned int id_ns_val = field.GetNodeSegInNodeAry(BUBBLE).id_ns_va;
if( id_na_val != 0 ){
const unsigned int ilss0 = this->FindIndexArray_Seg(id_field,EDGE,world);
const CVector_Blk& upd = m_ls.GetVector(-2,ilss0);
CNodeAry& na = world.GetNA(id_na_val);
na.AddValueToNodeSegment(id_ns_val,upd,1.0);
// CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns_val);
// ns.is_updated = true;
}
}
return true;
}
double CLinearSystem_SaveDiaM_NewmarkBeta::MakeResidual(const Fem::Field::CFieldWorld& world)
{
const unsigned int nseg = this->m_aSegField.size();
if( nseg == 0 ) return 0.0;
// 外力項をセット
for(unsigned int iseg=0;iseg<nseg;iseg++){
(*m_ls.m_Residual[iseg]) = (*m_Force[iseg]);
}
////////////////////////////////
// updateにKを掛けるための値をセットする
for(unsigned int ilss=0;ilss<nseg;ilss++){
const CLinSysSeg_Field& ls = this->m_aSegField[ilss];
CVector_Blk* update = this->m_ls.m_Update[ilss];
unsigned int ilen1 = 0;
{
const Fem::Field::CField& field = world.GetField(ls.id_field);
ilen1 = field.GetNLenValue();
unsigned int id_na_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
assert( world.IsIdNA(id_na_val) );
const CNodeAry& na = world.GetNA(id_na_val);
unsigned int id_ns_u = field.GetNodeSegInNodeAry(CORNER).id_ns_va;
unsigned int id_ns_v = field.GetNodeSegInNodeAry(CORNER).id_ns_ve;
unsigned int id_ns_a = field.GetNodeSegInNodeAry(CORNER).id_ns_ac;
na.GetValueFromNodeSegment( id_ns_u,*update);
na.AddValueFromNodeSegment(dt, id_ns_v,*update);
na.AddValueFromNodeSegment(dt*dt*(0.5-beta),id_ns_a,*update);
}
if( world.IsIdField(ls.id_field2) ){
const Fem::Field::CField& field = world.GetField(ls.id_field2);
unsigned int id_na_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
assert( world.IsIdNA(id_na_val) );
const CNodeAry& na = world.GetNA(id_na_val);
unsigned int id_ns_u = field.GetNodeSegInNodeAry(CORNER).id_ns_va;
unsigned int id_ns_v = field.GetNodeSegInNodeAry(CORNER).id_ns_ve;
unsigned int id_ns_a = field.GetNodeSegInNodeAry(CORNER).id_ns_ac;
na.GetValueFromNodeSegment( id_ns_u,*update,ilen1);
na.AddValueFromNodeSegment(dt, id_ns_v,*update,ilen1);
na.AddValueFromNodeSegment(dt*dt*(0.5-beta),id_ns_a,*update,ilen1);
}
}
// 行列ベクトル積により残差を計算する
for(unsigned int iseg=0;iseg<nseg;iseg++){
for(unsigned int jseg=0;jseg<nseg;jseg++){
if( m_Matrix_Boundary[iseg][jseg] == 0 ) continue;
m_Matrix_Boundary[iseg][jseg]->MatVec( -1.0, *m_ls.m_Update[jseg], 1.0, *m_ls.m_Residual[iseg], true );
}
}
/*
////////////////////////////////
// updateにMを掛けるため加速度値をセットする
for(unsigned int ilss=0;ilss<nseg;ilss++){
const CLinSysSeg_Field& ls = this->m_aSegField[ilss];
CVector_Blk* update = this->m_Update[ilss];
unsigned int ilen1 = 0;
{
const Fem::Field::CField& field = world.GetField(ls.id_field);
ilen1 = field.GetNLenValue();
unsigned int id_na_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
assert( world.IsIdNA(id_na_val) );
const CNodeAry& na = world.GetNA(id_na_val);
unsigned int id_ns_a = field.GetNodeSegInNodeAry(CORNER).id_ns_ac;
na.GetValueFromNodeSegment(id_ns_a,*update);
}
if( world.IsIdField(ls.id_field2) ){
const Fem::Field::CField& field = world.GetField(ls.id_field2);
unsigned int id_na_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
assert( world.IsIdNA(id_na_val) );
const CNodeAry& na = world.GetNA(id_na_val);
unsigned int id_ns_a = field.GetNodeSegInNodeAry(CORNER).id_ns_ac;
na.GetValueFromNodeSegment(id_ns_a,*update,ilen1);
}
}
// 行列ベクトル積により残差を計算する
for(unsigned int iseg=0;iseg<nseg;iseg++){
m_DiaMassMatrix[iseg]->MatVec( -1.0, *m_Update[iseg], 1.0, *m_Residual[iseg] );
}
*/
// 残差への境界条件のセット
for(unsigned int iseg=0;iseg<nseg;iseg++){
// m_ls.m_BCFlag[iseg]->SetZeroToBCDof(*m_ls.m_Residual[iseg]);
m_ls.GetBCFlag(iseg).SetZeroToBCDof(*m_ls.m_Residual[iseg]);
}
return sqrt(this->DOT(-1,-1));
}
bool CEqnSystem_Fluid2D::UpdateDomain_Field(unsigned int id_base, Fem::Field::CFieldWorld& world)
{
m_id_press = world.MakeField_FieldElemDim(id_base,2,
Fem::Field::SCALAR, VELOCITY|ACCELERATION,CORNER);
assert( world.IsIdField(m_id_press) );
// std::cout << "press " << m_id_press << std::endl;
if( m_IsntInterpolationBubble) {
std::cout << "corner intp" << std::endl;
m_id_velo = world.MakeField_FieldElemDim(id_base,2,
Fem::Field::VECTOR2,VELOCITY|ACCELERATION,CORNER);
m_IsntCombine = false;
}
else {
std::cout << "bubble intp" << std::endl;
m_id_velo = world.MakeField_FieldElemDim(id_base,2,
Fem::Field::VECTOR2,VELOCITY|ACCELERATION,CORNER|BUBBLE);
m_IsntCombine = true;
}
// std::cout << "velo : " << m_id_velo << std::endl;
assert( world.IsIdField(m_id_velo) );
{ // 同じ要素配列IDを持つ方程式があったら，それを使う．なければ新規に追加
std::vector<CEqn_Fluid2D> aEqn_old = m_aEqn;
m_aEqn.clear();
const CField& field = world.GetField(m_id_velo);
const std::vector<unsigned int>& aIdEA = field.GetAryIdEA();
for(unsigned int iiea=0; iiea<aIdEA.size(); iiea++) {
const unsigned int id_ea = aIdEA[iiea];
unsigned int ieqn0=0;
for(; ieqn0<aEqn_old.size(); ieqn0++) {
if( aEqn_old[ieqn0].GetIdEA() == id_ea ) {
const unsigned int ieqn1 = m_aEqn.size();
m_aEqn.push_back( aEqn_old[ieqn0] );
m_aEqn[ieqn1].SetIdFieldVelocity(m_id_velo);
m_aEqn[ieqn1].SetIdFieldPressure(m_id_press);
break;
}
}
if( ieqn0 != aEqn_old.size() ) {
continue;
}
CEqn_Fluid2D eqn1(id_ea,m_id_velo,m_id_press);
eqn1.SetRho(m_rho_back);
eqn1.SetMyu(m_myu_back);
if( this->m_is_stokes_back ) {
eqn1.SetStokes();
}
else {
eqn1.SetNavierStokes();
}
m_aEqn.push_back( eqn1 );
}
}
/*
std::cout << "Size Eqn : " << m_aEqn.size() << std::endl;
for(unsigned int ieqn=0;ieqn<m_aEqn.size();ieqn++){
const CEqn_Fluid2D& eqn = m_aEqn[ieqn];
std::cout << ieqn << " " << eqn.GetMyu() << " " << eqn.GetRho() << " " << eqn.IsNavierStokes() << std::endl;
}
*/
if( !world.IsIdField(m_id_velo) ) return false;
this->ClearLinearSystemPreconditioner();
return true;
}
bool CEqn_Fluid3D::AddFixField(const unsigned int id_field, Fem::Field::CFieldWorld& world, int idof)
{
if( !world.IsIdField( id_field ) ) return false;
m_aIdFixField.push_back( std::make_pair(id_field,idof) );
return true;
}
void Fem::Field::CFieldValueSetter::SetGradient
(unsigned int id_field_from, Fem::Field::CFieldWorld& world)
{
if( !world.IsIdField(id_field_from) ){ return; }
this->id_field_gradient_ = id_field_from;
}
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;
}
//! set constant value to the field
bool Fem::Field::SetFieldValue_Constant
(unsigned int id_field_to, unsigned int idofns, Fem::Field::FIELD_DERIVATION_TYPE fdt,
Fem::Field::CFieldWorld& world,
double val)
{
if( !world.IsIdField(id_field_to) ){ return false; }
Fem::Field::CField& field = world.GetField(id_field_to);
if( idofns >= field.GetNLenValue() ) return false;
if( !(field.GetFieldDerivativeType() & fdt) ) return false;
const std::vector<unsigned int>& aIdEA = field.GetAryIdEA();
if( field.GetNodeSegInNodeAry(CORNER).id_na_va != 0 ){
const CField::CNodeSegInNodeAry& nsna = field.GetNodeSegInNodeAry(CORNER);
assert( world.IsIdNA(nsna.id_na_va) );
CNodeAry& na = world.GetNA(nsna.id_na_va);
unsigned int id_ns = 0; // target node segment;
{
if( fdt & VALUE ) id_ns = nsna.id_ns_va;
else if( fdt & VELOCITY ) id_ns = nsna.id_ns_ve;
else if( fdt & ACCELERATION ) id_ns = nsna.id_ns_ac;
else{ assert(0); }
assert( na.IsSegID(id_ns) );
const CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns);
assert( idofns < ns.Length() );
}
/*
if( m_aElemIntp.size() == 0 || !this->IsPartial() ){ // 剛体の場合
const unsigned int nnode = na.Size();
CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns);
for(unsigned int inode=0;inode<nnode;inode++){
ns.SetValue(inode,idofns,const_val);
}
}*/
{
for(unsigned int iei=0;iei<aIdEA.size();iei++){
const unsigned int id_ea = aIdEA[iei];
assert( world.IsIdEA(id_ea) );
const CElemAry& ea = world.GetEA(id_ea);
unsigned int id_es_c_va = field.GetIdElemSeg(id_ea,CORNER,true,world);
assert( ea.IsSegID(id_es_c_va) );
na.SetValueToNodeSegment(ea,id_es_c_va,id_ns,idofns,val);
}
}
}
/* if( m_na_e.id_na_va != 0 ){
assert( world.IsIdNA(m_na_e.id_na_va) );
CNodeAry& na = world.GetNA(m_na_e.id_na_va);
const CNodeAry::CNodeSeg& ns_val = na.GetSeg(m_na_e.id_ns_va);
for(unsigned int iei=0;iei<m_aElemIntp.size();iei++){
unsigned int id_ea = m_aElemIntp[iei].id_ea;
assert( world.IsIdEA(id_ea) );
const CElemAry& ea = world.GetEA(id_ea);
assert( ea.IsSegID( m_aElemIntp[iei].id_es_e_va ) );
na.SetValueToNodeSegment(ea,m_aElemIntp[iei].id_es_e_va,
m_na_e.id_ns_va,idofns,const_val);
}
}*/
if( field.GetNodeSegInNodeAry(BUBBLE).id_na_va != 0 ){
const CField::CNodeSegInNodeAry& nsna = field.GetNodeSegInNodeAry(BUBBLE);
assert( world.IsIdNA(nsna.id_na_va) );
CNodeAry& na = world.GetNA(nsna.id_na_va);
unsigned int id_ns = 0; // target node segment;
{
if( fdt & VALUE ) id_ns = nsna.id_ns_va;
else if( fdt & VELOCITY ) id_ns = nsna.id_ns_ve;
else if( fdt & ACCELERATION ) id_ns = nsna.id_ns_ac;
else{ assert(0); }
assert( na.IsSegID(id_ns) );
const CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns);
assert( idofns < ns.Length() );
}
// const CNodeAry::CNodeSeg& ns_val = na.GetSeg(id_ns);
for(unsigned int iea=0;iea<aIdEA.size();iea++){
const unsigned int id_ea = aIdEA[iea];
assert( world.IsIdEA(id_ea) );
const CElemAry& ea = world.GetEA(id_ea);
unsigned int id_es_b_va = field.GetIdElemSeg(id_ea,BUBBLE,true,world);
assert( ea.IsSegID(id_es_b_va) );
na.SetValueToNodeSegment(ea,id_es_b_va,id_ns,idofns,val);
}
}
return true;
}
//! set gradient value to the field
bool Fem::Field::SetFieldValue_Gradient
(unsigned int id_field_to, Fem::Field::CFieldWorld& world,
unsigned int id_field_from)
{
if( !world.IsIdField(id_field_from) ) return false;
Fem::Field::CField& field_from = world.GetField(id_field_from);
if( !world.IsIdField(id_field_to) ) return false;
Fem::Field::CField& field_to = world.GetField(id_field_to);
if( field_to.GetAryIdEA().size() != 1 ){
std::cout << "Error!-->Not Implimented" << std::endl;
getchar();
assert(0);
}
Fem::Field::INTERPOLATION_TYPE type_from, type_to;
{
const std::vector<unsigned int>& aIdEA_from = field_from.GetAryIdEA();
const std::vector<unsigned int>& aIdEA_to = field_to.GetAryIdEA();
if( aIdEA_from.size() != aIdEA_to.size() ) return false;
const unsigned int niea = aIdEA_from.size();
assert( niea == 1 );
if( aIdEA_from[0] != aIdEA_to[0] ) return false;
const unsigned int id_ea = aIdEA_from[0];
type_from = field_from.GetInterpolationType(id_ea,world);
type_to = field_to.GetInterpolationType(id_ea,world);
}
unsigned int nnoes, ndim;
if( type_from==HEX11 && type_to==HEX1001 ){
nnoes = 8; ndim = 3;
}
else if( type_from==TET11 && type_to==TET1001 ){
nnoes = 4; ndim = 3;
}
else if( type_from==TRI11 && type_to==TRI1001 ){
nnoes = 3; ndim = 2;
}
else{
std::cout << "NotImplimented!" << std::endl;
assert(0);
getchar();
}
unsigned int id_ea = field_to.GetAryIdEA()[0];
const CElemAry& ea = world.GetEA(id_ea);
const CElemAry::CElemSeg& es_c_co = field_from.GetElemSeg(id_ea,CORNER,false,world);
const CElemAry::CElemSeg& es_c_va = field_from.GetElemSeg(id_ea,CORNER,true, world);
const CElemAry::CElemSeg& es_b_va = field_to.GetElemSeg(id_ea,BUBBLE,true, world);
const Fem::Field::CField::CNodeSegInNodeAry& nsna_c = field_from.GetNodeSegInNodeAry(CORNER);
assert( world.IsIdNA(nsna_c.id_na_co) );
assert( world.IsIdNA(nsna_c.id_na_va) );
const CNodeAry& na_c_co = world.GetNA(nsna_c.id_na_co);
const CNodeAry::CNodeSeg& ns_c_co = na_c_co.GetSeg(nsna_c.id_ns_co);
const CNodeAry& na_c_va = world.GetNA(nsna_c.id_na_va);
const CNodeAry::CNodeSeg& ns_c_va = na_c_va.GetSeg(nsna_c.id_ns_va);
unsigned int id_na_b_va = field_to.GetNodeSegInNodeAry(BUBBLE).id_na_va;
unsigned int id_ns_b_va = field_to.GetNodeSegInNodeAry(BUBBLE).id_ns_va;
assert( world.IsIdNA(id_na_b_va) );
CNodeAry& na_b_va = world.GetNA(id_na_b_va);
CNodeAry::CNodeSeg& ns_b_va = na_b_va.GetSeg(id_ns_b_va);
double coord[16][3];
double value[16];
double grad[3];
// const unsigned int nnoes_c = es_c_co.GetSizeNoes();
unsigned int noes[64];
for(unsigned int ielem=0;ielem<ea.Size();ielem++){
{ // 座標(coord)と値(value)を作る
es_c_co.GetNodes(ielem,noes);
for(unsigned int inoes=0;inoes<nnoes;inoes++){
unsigned int ipoi0 = noes[inoes];
assert( ipoi0 < na_c_co.Size() );
ns_c_co.GetValue(ipoi0,coord[inoes]);
}
/* if( id_es_c_va == id_es_c_co ){
for(unsigned int inoes=0;inoes<nnoes;inoes++){
unsigned int ipoi0 = noes[inoes];
assert( ipoi0 < na_c_va.Size() );
na_c_va.GetValueFromNode(ipoi0,id_ns_c_va,0,val);
value[inoes] = val;
}
}
else{*/
es_c_va.GetNodes(ielem,noes);
for(unsigned int inoes=0;inoes<nnoes;inoes++){
unsigned int ipoi0 = noes[inoes];
assert( ipoi0 < na_c_va.Size() );
ns_c_va.GetValue(ipoi0,&value[inoes]);
}
// }
}
if( type_from == HEX11 ){
double dndx[8][3];
double an[8];
double detjac;
ShapeFunc_Hex8(0.0,0.0,0.0, coord, detjac,dndx,an);
for(unsigned int idim=0;idim<ndim;idim++){ grad[idim] = 0.0; }
for(unsigned int inoes=0;inoes<nnoes;inoes++){
for(unsigned int idim=0;idim<ndim;idim++){
grad[idim] += value[inoes]*dndx[inoes][idim];
}
}
}
else if( type_from == TET11 ){
double dldx[4][3];
double const_term[4];
TetDlDx(dldx,const_term, coord[0],coord[1],coord[2],coord[3]);
for(unsigned int idim=0;idim<ndim;idim++){ grad[idim] = 0.0; }
for(unsigned int inoes=0;inoes<nnoes;inoes++){
for(unsigned int idim=0;idim<ndim;idim++){
grad[idim] += value[inoes]*dldx[inoes][idim];
}
}
}
else if( type_from == TRI11 ){
double dldx[3][2];
double const_term[3];
TriDlDx(dldx,const_term, coord[0],coord[1],coord[2]);
for(unsigned int idim=0;idim<ndim;idim++){ grad[idim] = 0.0; }
for(unsigned int inoes=0;inoes<nnoes;inoes++){
for(unsigned int idim=0;idim<ndim;idim++){
grad[idim] += value[inoes]*dldx[inoes][idim];
}
}
grad[2] = 0.0;
}
// std::cout << grad[0] << " " << grad[1] << " " << grad[2] << std::endl;
{
double norm = sqrt(grad[0]*grad[0]+grad[1]*grad[1]+grad[2]*grad[2]);
norm *=0.2;
// norm = 15.0;
grad[0] /= norm;
grad[1] /= norm;
grad[2] /= norm;
}
{
unsigned int noes[16];
es_b_va.GetNodes(ielem,noes);
unsigned int ipoi0 = noes[0];
assert( ipoi0 < na_b_va.Size() );
for(unsigned int idim=0;idim<ndim;idim++){
ns_b_va.SetValue(ipoi0,idim,grad[idim]);
}
}
}
return true;
}
// ÉXÉJÉâÅ[ÇÃâûóÕëäìñílÇí«â¡Ç∑ÇÈÅDǪÇÃǧÇøÉÇÅ[ÉhÇǬÇØÇÈ
// mode = 0 : É~Å[É[ÉX
// mode = 1 : ç≈ëÂâûóÕ
// äÙâΩäwìIÇ»îÒê¸å`ê´Ççló∂Ç∑ÇÈǩǫǧǩÇÕÅCEqnObjÇå©ÇƒåàÇflÇÈÇÊǧDžÇ∑ÇÈ
bool CEqnSystem_Solid2D::SetEquivStressValue(unsigned int id_field_str, Fem::Field::CFieldWorld& world)
{
if( !world.IsIdField(id_field_str) ) return false;
Fem::Field::CField& field_str = world.GetField(id_field_str);
if( field_str.GetFieldType() != SCALAR ) return false;
if( !world.IsIdField(m_IdFieldDisp) ) return false;
Fem::Field::CField& field_dis = world.GetField(m_IdFieldDisp);
const std::vector<unsigned int>& aIdEA_from = field_dis.GetAryIdEA();
const std::vector<unsigned int>& aIdEA_to = field_str.GetAryIdEA();
if( aIdEA_from.size() != aIdEA_to.size() ) return false;
const unsigned int niea = aIdEA_from.size();
for(unsigned int iiea=0;iiea<niea;iiea++)
{
Fem::Field::INTERPOLATION_TYPE type_from, type_to;
{
if( aIdEA_from[iiea] != aIdEA_to[iiea] ){
assert(0);
return false;
}
const unsigned int id_ea = aIdEA_from[iiea];
type_from = field_dis.GetInterpolationType(id_ea,world);
type_to = field_str.GetInterpolationType(id_ea,world);
}
unsigned int nnoes, ndim;
if( type_from==TRI11 && type_to==TRI1001 ){
nnoes = 3; ndim = 2;
}
else{
std::cout << "Error!-->Not Implimented!" << std::endl;
std::cout << type_from << " " << type_to << std::endl;
assert(0);
getchar();
}
unsigned int id_ea = aIdEA_to[iiea];
const CElemAry& ea = world.GetEA(id_ea);
const CElemAry::CElemSeg& es_c_co = field_dis.GetElemSeg(id_ea,CORNER,false,world);
const CElemAry::CElemSeg& es_c_va = field_dis.GetElemSeg(id_ea,CORNER,true, world);
const CElemAry::CElemSeg& es_b_va = field_str.GetElemSeg(id_ea,BUBBLE,true, world);
Fem::Field::CField::CNodeSegInNodeAry nans_c = field_dis.GetNodeSegInNodeAry(CORNER);
assert( world.IsIdNA(nans_c.id_na_co) );
assert( world.IsIdNA(nans_c.id_na_va) );
const CNodeAry& na_c_co = world.GetNA( nans_c.id_na_co);
const CNodeAry::CNodeSeg& ns_c_co = na_c_co.GetSeg( nans_c.id_ns_co);
const CNodeAry& na_c_va = world.GetNA( nans_c.id_na_va);
const CNodeAry::CNodeSeg& ns_c_va = na_c_va.GetSeg( nans_c.id_ns_va);
Fem::Field::CField::CNodeSegInNodeAry nans_b = field_str.GetNodeSegInNodeAry(BUBBLE);
unsigned int id_na_b_va = nans_b.id_na_va;
unsigned int id_ns_b_va = nans_b.id_ns_va;
assert( world.IsIdNA(id_na_b_va) );
CNodeAry& na_b_va = world.GetNA(id_na_b_va);
CNodeAry::CNodeSeg& ns_b_va = na_b_va.GetSeg(id_ns_b_va);
const unsigned int nnoes_c = es_c_co.Length();
assert( nnoes_c < 64 );
unsigned int noes[64];
const CEqn_Solid2D& eqn = this->GetEquation(id_ea);
for(unsigned int ielem=0;ielem<ea.Size();ielem++){
////////////////
double coord[16][3];
// ç¿ïW(coord)Ç∆íl(value)ÇçÏÇÈ
es_c_co.GetNodes(ielem,noes);
for(unsigned int inoes=0;inoes<nnoes;inoes++){
unsigned int ipoi0 = noes[inoes];
assert( ipoi0 < na_c_co.Size() );
ns_c_co.GetValue(ipoi0,coord[inoes]);
}
////////////////
double disp[16][3]; // êflì_ïœà
es_c_va.GetNodes(ielem,noes);
for(unsigned int inoes=0;inoes<nnoes;inoes++){
unsigned int ipoi0 = noes[inoes];
assert( ipoi0 < na_c_va.Size() );
for(unsigned int idim=0;idim<ndim;idim++){
ns_c_va.GetValue(ipoi0,disp[inoes]);
}
}
////////////////
double dudx[2][2]; // ïœå`å˘îz
if( type_from == TRI11 ){
double dldx[3][2];
double const_term[3];
TriDlDx(dldx,const_term, coord[0],coord[1],coord[2]);
for(unsigned int i=0;i<ndim*ndim;i++){ (&dudx[0][0])[i] = 0.0; }
for(unsigned int knoes=0;knoes<nnoes;knoes++){
dudx[0][0] += disp[knoes][0]*dldx[knoes][0];
dudx[0][1] += disp[knoes][0]*dldx[knoes][1];
dudx[1][0] += disp[knoes][1]*dldx[knoes][0];
dudx[1][1] += disp[knoes][1]*dldx[knoes][1];
}
}
double strain[2][2]; // òc
if( eqn.IsGeometricalNonlinear() ){ // äÙâΩäwìIîÒê¸å`Ç ÇË
strain[0][0] = 0.5*(dudx[0][0]+dudx[0][0]+dudx[0][0]*dudx[0][0]+dudx[1][0]*dudx[1][0]);
strain[0][1] = 0.5*(dudx[0][1]+dudx[1][0]+dudx[0][0]*dudx[0][1]+dudx[1][1]*dudx[1][0]);
strain[1][0] = 0.5*(dudx[1][0]+dudx[0][1]+dudx[0][1]*dudx[0][0]+dudx[1][0]*dudx[1][1]);
strain[1][1] = 0.5*(dudx[1][1]+dudx[1][1]+dudx[0][1]*dudx[0][1]+dudx[1][1]*dudx[1][1]);
}
else { // äÙâΩäwìIîÒê¸å`ǻǵ
strain[0][0] = 0.5*(dudx[0][0]+dudx[0][0]);
strain[0][1] = 0.5*(dudx[0][1]+dudx[1][0]);
strain[1][0] = 0.5*(dudx[1][0]+dudx[0][1]);
strain[1][1] = 0.5*(dudx[1][1]+dudx[1][1]);
}
double stress[2][2];
{
double myu, lambda;
eqn.GetLambdaMyu(lambda,myu);
stress[0][0] = myu*strain[0][0];
stress[0][1] = myu*strain[0][1];
stress[1][0] = myu*strain[1][0];
stress[1][1] = myu*strain[1][1];
const double dtmp1 = lambda*(strain[0][0]+strain[1][1]);
stress[0][0] += dtmp1;
stress[1][1] += dtmp1;
}
double mises;
{
const double dtmp1 = 0.5*stress[0][0]*stress[0][0]+0.5*stress[1][1]*stress[1][1]
+0.5*(stress[1][1]-stress[0][0])*(stress[1][1]-stress[0][0])
+3*stress[0][1]*stress[0][1];
mises = sqrt(dtmp1);
}
double maxprinciple;
{
const double d1 = stress[0][0]+stress[1][1];
const double d2 = stress[0][0]*stress[1][1]-stress[0][1]*stress[1][0];
const double d3 = d1*d1-4*d2;
assert( d3 >= 0 );
maxprinciple = 0.5*(d1+sqrt(d3));
}
{
unsigned int noes[16];
es_b_va.GetNodes(ielem,noes);
unsigned int ipoi0 = noes[0];
assert( ipoi0 < na_b_va.Size() );
ns_b_va.SetValue(ipoi0,0,mises);
}
}
}
return true;
}
bool Ls::CLinearSystem_RigidField2::UpdateValueOfField_NewmarkBeta(
double gamma, double beta, double dt,
unsigned int id_field_val, Fem::Field::CFieldWorld& world, bool IsInitial )
{
if( !world.IsIdField(id_field_val) ) return false;
const Fem::Field::CField& field_val = world.GetField(id_field_val);
if( field_val.GetNodeSegInNodeAry(Fem::Field::CORNER).id_na_va != 0 ){ // 角節点を更新
unsigned int id_na_val=0, id_ns_a,id_ns_v,id_ns_u;
{
const Fem::Field::CField::CNodeSegInNodeAry& nsna = field_val.GetNodeSegInNodeAry(Fem::Field::CORNER);
id_na_val = nsna.id_na_va;
id_ns_u = nsna.id_ns_va;
id_ns_v = nsna.id_ns_ve;
id_ns_a = nsna.id_ns_ac;
}
const int ilss = this->FindIndexArray_Seg(id_field_val,Fem::Field::CORNER,world);
assert( ilss != -1 );
const CLinSysSegRF& lss = this->m_aSegRF[ilss];
const MatVec::CVector_Blk& upd = m_ls.GetVector(-2,ilss);
Fem::Field::CNodeAry& na_val = world.GetNA(id_na_val);
// 値を更新する
if( IsInitial ){
na_val.AddValueToNodeSegment(id_ns_u ,id_ns_v, dt);
na_val.AddValueToNodeSegment(id_ns_u ,id_ns_a, 0.5*dt*dt);
}
na_val.AddValueToNodeSegment(id_ns_u, upd, beta*dt*dt);
// 速度を更新する．
if( IsInitial ){
na_val.AddValueToNodeSegment(id_ns_v ,id_ns_a, dt);
}
na_val.AddValueToNodeSegment(id_ns_v, upd, gamma*dt);
// 加速度を更新する
na_val.AddValueToNodeSegment(id_ns_a, upd, 1.0);
}
if( field_val.GetNodeSegInNodeAry(Fem::Field::BUBBLE).id_na_va != 0 ){ // バブル節点を更新
unsigned int id_na_val=0, id_ns_a,id_ns_v,id_ns_u;
{
const Fem::Field::CField::CNodeSegInNodeAry& nsna = field_val.GetNodeSegInNodeAry(Fem::Field::BUBBLE);
id_na_val = nsna.id_na_va;
id_ns_u = nsna.id_ns_va;
id_ns_v = nsna.id_ns_ve;
id_ns_a = nsna.id_ns_ac;
}
const int ilss = this->FindIndexArray_Seg(id_field_val,Fem::Field::BUBBLE,world);
assert( ilss != -1 );
const CLinSysSegRF& lss = this->m_aSegRF[ilss];
const MatVec::CVector_Blk& upd = m_ls.GetVector(-2,ilss);
Fem::Field::CNodeAry& na_val = world.GetNA(id_na_val);
// 値を更新する
if( IsInitial ){
na_val.AddValueToNodeSegment(id_ns_u ,id_ns_v, dt);
na_val.AddValueToNodeSegment(id_ns_u ,id_ns_a, 0.5*dt*dt);
}
na_val.AddValueToNodeSegment(id_ns_u, upd, beta*dt*dt);
// 速度を更新する．
if( IsInitial ){
na_val.AddValueToNodeSegment(id_ns_v ,id_ns_a, dt);
}
na_val.AddValueToNodeSegment(id_ns_v, upd, gamma*dt);
// 加速度を更新する
na_val.AddValueToNodeSegment(id_ns_a, upd, 1.0);
}
if( field_val.GetNodeSegInNodeAry(Fem::Field::EDGE).id_na_va != 0 ){ // 角節点を更新
assert(0);
}
return true;
}
// fieldで初期化する、fieldの中の非ゼロパターンで行列を作る
bool Ls::CLinearSystem_RigidField2::AddPattern_Field(
unsigned int id_field1,
unsigned int id_field2,
const Fem::Field::CFieldWorld& world)
{
if( !world.IsIdField(id_field1) ) return false;
const Fem::Field::CField& field1 = world.GetField(id_field1);
unsigned int id_field_parent;
{
if( field1.GetIDFieldParent() == 0 ){ id_field_parent = id_field1; }
else{ id_field_parent = field1.GetIDFieldParent(); }
}
const unsigned int nlen_value = field1.GetNLenValue();
int ilss_add_c;
int ilss_add_b;
{ // Cornerセグメントを作る
unsigned int id_na_val = field1.GetNodeSegInNodeAry(Fem::Field::CORNER).id_na_va;
if( id_na_val != 0 ){
assert( world.IsIdNA(id_na_val) );
const Fem::Field::CNodeAry& na = world.GetNA(id_na_val);
assert( m_ls.GetNLinSysSeg() == this->m_aSegRF.size() );
ilss_add_c = m_ls.GetNLinSysSeg();
this->m_aSegRF.push_back( CLinSysSegRF(id_field_parent,Fem::Field::CORNER) );
int ires = m_ls.AddLinSysSeg( na.Size(), field1.GetNLenValue() );
assert( ires == ilss_add_c );
}
else{ ilss_add_c = -1; }
}
{ // Bubbleセグメントを作る
unsigned int id_na_val = field1.GetNodeSegInNodeAry(Fem::Field::BUBBLE).id_na_va;
if( id_na_val != 0 ){
assert( world.IsIdNA(id_na_val) );
const Fem::Field::CNodeAry& na = world.GetNA(id_na_val);
assert( m_ls.GetNLinSysSeg() == this->m_aSegRF.size() );
ilss_add_b = m_ls.GetNLinSysSeg();
this->m_aSegRF.push_back( CLinSysSegRF(id_field_parent,Fem::Field::BUBBLE) );
int ires = m_ls.AddLinSysSeg( na.Size(), field1.GetNLenValue() );
assert( ires == ilss_add_b );
}
}
const Fem::Field::CField& field2 = world.GetField(id_field2);
const std::vector<unsigned int>& aIdEA1 = field1.GetAryIdEA();
const std::vector<unsigned int>& aIdEA2 = field2.GetAryIdEA();
for(;;){ // ダミーのfor文を使ってbreakで抜けられるようにする
// Corner-Corner関係を作る
if( ilss_add_c == -1 ) break;
const unsigned int id_na_va1 = field1.GetNodeSegInNodeAry(Fem::Field::CORNER).id_na_va;
const unsigned int id_na_co1 = field1.GetNodeSegInNodeAry(Fem::Field::CORNER).id_na_co;
const unsigned int id_na_va2 = field2.GetNodeSegInNodeAry(Fem::Field::CORNER).id_na_va;
const unsigned int id_na_co2 = field2.GetNodeSegInNodeAry(Fem::Field::CORNER).id_na_co;
assert( aIdEA1.size() > 0 );
if( id_na_co1 != id_na_co2 ) break;
for(unsigned int iiea1=0;iiea1<aIdEA1.size();iiea1++){
const unsigned int id_ea1 = aIdEA1[iiea1];
if( field1.GetIdElemSeg(id_ea1,Fem::Field::CORNER,true,world) == 0 ) continue;
assert( world.IsIdEA(id_ea1) );
const Fem::Field::CElemAry& ea1 = world.GetEA(id_ea1);
const unsigned int id_es_c1 = field1.GetIdElemSeg(id_ea1,Fem::Field::CORNER,true,world);
assert( ea1.IsSegID(id_es_c1) );
// CORNER1-CORNER1
{
Com::CIndexedArray crs;
ea1.MakePattern_FEM(id_es_c1,crs);
m_ls.AddMat_Dia(ilss_add_c, crs ); // cc行列を作る
}
////////////////
for(unsigned int iiea2=0;iiea2<aIdEA2.size();iiea2++)
{
const unsigned int id_ea2 = aIdEA2[iiea2];
if( id_ea1 == id_ea2 ){
// CORNER1-CORNER2
const int ils2_c = this->FindIndexArray_Seg(id_field2,Fem::Field::CORNER,world);
if( ils2_c != -1 ){
assert( ils2_c >= 0 && ils2_c < this->GetNLinSysSeg() );
const unsigned int id_es_c2 = field2.GetIdElemSeg(id_ea2,Fem::Field::CORNER,true,world);
assert( id_es_c1 == id_es_c2 );
Com::CIndexedArray crs;
ea1.MakePattern_FEM(id_es_c1,id_es_c2,crs);
assert( crs.CheckValid() );
m_ls.AddMat_NonDia(ilss_add_c,ils2_c, crs); // c1c2行列を足す
const unsigned int nnode2 = m_ls.GetBlkSizeLinSeg(ils2_c);
Com::CIndexedArray crs_inv;
crs_inv.SetTranspose( nnode2, crs );
m_ls.AddMat_NonDia(ils2_c,ilss_add_c, crs_inv); // c2c1行列を足す
}
// CORNER1-BUBBLE2
const int ils2_b = this->FindIndexArray_Seg(id_field2,Fem::Field::BUBBLE,world);
if( ils2_b != -1 ){
assert(0);
}
}
else{
const Fem::Field::CNodeAry& na1 = world.GetNA(id_na_co1);
const unsigned int id_es_c_co1 = field1.GetIdElemSeg(id_ea1,Fem::Field::CORNER,false,world);
const unsigned int id_es_c_co2 = field2.GetIdElemSeg(id_ea2,Fem::Field::CORNER,false,world);
if( na1.IsIncludeEaEs_InEaEs(
std::make_pair(id_ea1,id_es_c_co1),
std::make_pair(id_ea2,id_es_c_co2) ) )
{
std::cout << "ea : " << id_ea1 << " is included in " << id_ea2 << std::endl;
assert( ea1.IsSegID(id_es_c_co1) );
Com::CIndexedArray crs;
ea1.MakePattern_FEM(id_es_c1,id_es_c1,crs); // 自分も含む
assert( crs.CheckValid() );
if( field2.IsPartial() ){
std::cout << "Error!-->未実装" << std::endl;
assert(0);
}
for(unsigned int icrs=0;icrs<crs.array.size();icrs++){
unsigned int jno_va = crs.array[icrs];
unsigned int jno_co = field1.GetMapVal2Co(jno_va);
// std::cout << jno_van << " " << jno_co << std::endl;
// 本当はfield2.GetMapVal2Coの逆写像を使って求める．
unsigned int jno_va2 = jno_co;
crs.array[icrs] = jno_va2;
}
int ils2 = this->FindIndexArray_Seg(id_field2,Fem::Field::CORNER,world);
assert( ils2 >= 0 && ils2 < this->GetNLinSysSeg() );
// std::cout << "ils_seg : " << ils0 << " " << ils2 << std::endl;
m_ls.AddMat_NonDia(ilss_add_c,ils2, crs);
const unsigned int nnode2 = m_ls.GetBlkSizeLinSeg(ils2);
Com::CIndexedArray crs_inv;
crs_inv.SetTranspose( nnode2, crs );
m_ls.AddMat_NonDia(ils2,ilss_add_c, crs_inv);
}
}
}
}
break;
}
for(unsigned int iiea=0;iiea<aIdEA1.size();iiea++){
const unsigned int id_ea1 = aIdEA1[iiea];
const unsigned int id_es_b1 = field1.GetIdElemSeg(id_ea1,Fem::Field::BUBBLE,true,world);
if( id_es_b1 == 0 ) continue;
const Fem::Field::CElemAry& ea1 = world.GetEA(id_ea1);
assert( world.IsIdEA(id_ea1) );
assert( ea1.IsSegID(id_es_b1) );
const unsigned int ils0 = ilss_add_b;
// BUBLE1-BUBBLE2
{
Com::CIndexedArray crs;
ea1.MakePattern_FEM(id_es_b1,crs);
m_ls.AddMat_Dia(ilss_add_b, crs );
}
const unsigned int id_ea2 = aIdEA2[iiea];
assert( id_ea1 == id_ea2 );
const unsigned int id_es_c2 = field2.GetIdElemSeg(id_ea2,Fem::Field::CORNER,true,world);
assert( ea1.IsSegID(id_es_c2) );
int ils2 = this->FindIndexArray_Seg(id_field2,Fem::Field::CORNER,world);
assert( ils2 >= 0 && ils2 < this->GetNLinSysSeg() );
assert( ils2 >= 0 && ils2 < m_ls.GetNLinSysSeg() );
{
Com::CIndexedArray crs;
ea1.MakePattern_FEM(id_es_b1,id_es_c2,crs);
assert( crs.CheckValid() );
m_ls.AddMat_NonDia(ils0,ils2, crs); // b1c2行列を作る
Com::CIndexedArray crs_inv;
const unsigned nnode2 = m_ls.GetBlkSizeLinSeg(ils2);
crs_inv.SetTranspose( nnode2, crs );
m_ls.AddMat_NonDia(ils2,ils0, crs_inv); // c2b1行列を作る
}
}
// いろいろと追加が必要
// まずは足りない部分を要求するようになったらエラーを出す関数を実装しよう。
return true;
}
bool CDrawerFace::Set
(unsigned int id_field, const Fem::Field::CFieldWorld& world, bool isnt_value_disp,
unsigned int id_field_val)
{
if( !world.IsIdField(id_field) ){ return false; }
////////////////
this->m_id_field = id_field;
this->id_field_val = id_field_val;
this->m_isnt_value_disp = isnt_value_disp;
const Fem::Field::CField& field = world.GetField(id_field);
// setting of vertex array
unsigned int id_na_c_co = field.GetNodeSegInNodeAry(CORNER).id_na_co;
unsigned int id_na_c_val = field.GetNodeSegInNodeAry(CORNER).id_na_va;
////////////////////////////////
// decide whether draw ns of value or coord
if( id_na_c_val == 0 ){
this->m_is_draw_nsv = false; // draw NS of Coord
this->m_isnt_value_disp = true; // don't include displacement
}
else{ this->m_is_draw_nsv = true;}
////////////////
assert( field.IsNodeSeg(CORNER,false,world,VALUE) );
unsigned int ndim_field = field.GetNDimCoord();
////////////////
unsigned int npoin;
if( m_is_draw_nsv ){
assert( world.IsIdNA(id_na_c_val) );
const Fem::Field::CNodeAry& na_c_val = world.GetNA(id_na_c_val);
npoin = na_c_val.Size();
}
else{
assert( id_na_c_co != 0 );
const Fem::Field::CNodeAry& na_c_co = world.GetNA(id_na_c_co);
npoin = na_c_co.Size();
}
////////////////
// set size to vertex array
unsigned int ndim_draw;
if( this->m_isnt_value_disp == false
&& ndim_field == 2
&& (field.GetFieldType()==SCALAR||field.GetFieldType()==ZSCALAR) ){
ndim_draw = 3;
}
else{ ndim_draw = ndim_field; }
this->m_vertex_ary.SetSize(npoin,ndim_draw);
{ // normal
const bool is_normal = ( pNormalArray != 0 );
if( pNormalArray != 0 ){ delete pNormalArray; pNormalArray = 0; }
if( is_normal ){ pNormalArray = new double [npoin*3]; }
}
{ // uv map
const bool is_uv = ( pUVArray != 0 );
if( pUVArray != 0 ){ delete pUVArray; pUVArray = 0; }
if( is_uv ){ pUVArray = new double [npoin*2]; }
}
////////////////
if( ndim_draw == 2 ){ sutable_rot_mode = 1; }
else if( ndim_field == 3 ){ sutable_rot_mode = 3; }
else { sutable_rot_mode = 2; }
CDrawerFace::Update(world);
////////////////////////////////
{ // setting of element array
const std::vector<unsigned int>& aIdEA = field.GetAryIdEA();
for(unsigned int iiea=0;iiea<aIdEA.size();iiea++){
const unsigned int id_ea = aIdEA[iiea];
assert( world.IsIdEA(id_ea) );
unsigned int id_es_c;
if( m_is_draw_nsv ){ id_es_c = field.GetIdElemSeg(id_ea,CORNER,true, world); }
else{ id_es_c = field.GetIdElemSeg(id_ea,CORNER,false,world); }
assert( id_es_c != 0 );
CIndexArrayElem* pIAE = new CIndexArrayElem(id_ea,id_es_c,world);
pIAE->ilayer = field.GetLayer(id_ea);
this->m_apIndexArrayElem.push_back( pIAE );
}
}
////////////////////////////////
// color setting
if( world.IsIdField(id_field_val) ){
const Fem::Field::CField& field_val = world.GetField(id_field_val);
unsigned int id_na_c_val = field_val.GetNodeSegInNodeAry(CORNER).id_na_va;
unsigned int id_na_b_val = field_val.GetNodeSegInNodeAry(BUBBLE).id_na_va;
if( world.IsIdNA(id_na_c_val) ){
if( pColorArray != 0 ){ delete[] pColorArray; pColorArray=0; }
}
else if( world.IsIdNA(id_na_b_val) ){
}
}
this->Update(world);
return true;
}
bool CDrawerImageBasedFlowVis::Update(const Fem::Field::CFieldWorld& world)
{
if( !world.IsIdField(this->m_IdFieldVelo) ){ return false; }
const Fem::Field::CField& fv = world.GetField(this->m_IdFieldVelo);
////////////////////////////////
{ // 要素配列の設定
const std::vector<unsigned int>& aIdEA = fv.GetAryIdEA();
for(unsigned int iiea=0;iiea<aIdEA.size();iiea++){
const unsigned int id_ea = aIdEA[iiea];
assert( world.IsIdEA(id_ea) );
const unsigned int id_es_v_c = fv.GetIdElemSeg(id_ea,CORNER,true, world);
assert( id_es_v_c != 0 );
CIndexArrayElem* pIAE = new CIndexArrayElem(id_ea,id_es_v_c,world);
pIAE->ilayer = fv.GetLayer(id_ea);
this->m_apIndexArrayElem.push_back( pIAE );
}
}
/* const std::vector<unsigned int>& aIdEA = fv.GetAryIdEA();
{
unsigned int nelem0 = 0;
for(unsigned int iiea=0;iiea<aIdEA.size();iiea++){
const unsigned int id_ea = aIdEA[iiea];
assert( world.IsIdEA(id_ea) );
const Fem::Field::CElemAry& ea = world.GetEA(id_ea);
nelem0 += ea.Size();
}
if( nelem != nelem0 ){
nelem = nelem0;
if( aXYVeloElem != 0 ){ delete[] aXYVeloElem; }
if( aXYElem != 0 ){ delete[] aXYElem; }
aXYVeloElem = new double [nelem*6];
aXYElem = new double [nelem*6];
if( world.IsIdField(m_IdFieldColor) ){
if( aColorElem != 0 ){ delete[] aColorElem; }
aColorElem = new double [nelem*3];
}
}
}*/
const Fem::Field::CNodeAry::CNodeSeg& ns_v = fv.GetNodeSeg(CORNER,true, world,VELOCITY);
const Fem::Field::CNodeAry::CNodeSeg& ns_c = fv.GetNodeSeg(CORNER,false,world,VELOCITY);
nnode = ns_v.Size();
if( aVelo ==0 ){ aVelo = new double [nnode*2]; }
if( aCoord == 0 ){ aCoord = new double [nnode*2]; }
for(unsigned int inode=0;inode<nnode;inode++){
ns_v.GetValue(inode,aVelo+inode*2);
unsigned int inoc = fv.GetMapVal2Co(inode);
ns_c.GetValue(inoc,aCoord+inode*2);
}
/*
unsigned int ielem_cur = 0;
for(unsigned int iiea=0;iiea<aIdEA.size();iiea++)
{
const unsigned int id_ea = aIdEA[iiea];
assert( world.IsIdEA(id_ea) );
const Fem::Field::CElemAry::CElemSeg& es_v = fv.GetElemSeg(id_ea,CORNER,true,world);
const Fem::Field::CElemAry::CElemSeg& es_c = fv.GetElemSeg(id_ea,CORNER,false,world);
for(unsigned int ielem=0;ielem<es_v.GetSizeElem();ielem++){
unsigned int noes[3];
es_c.GetNodes(ielem,noes);
double co[3][2];
ns_c.GetValue(noes[0],co[0]);
ns_c.GetValue(noes[1],co[1]);
ns_c.GetValue(noes[2],co[2]);
////////////////
es_v.GetNodes(ielem,noes);
double velo[3][2];
ns_v.GetValue(noes[0],velo[0]);
ns_v.GetValue(noes[1],velo[1]);
ns_v.GetValue(noes[2],velo[2]);
////////////////
aXYElem[ielem_cur*6+0] = co[0][0];
aXYElem[ielem_cur*6+1] = co[0][1];
aXYElem[ielem_cur*6+2] = co[1][0];
aXYElem[ielem_cur*6+3] = co[1][1];
aXYElem[ielem_cur*6+4] = co[2][0];
aXYElem[ielem_cur*6+5] = co[2][1];
////////////////
const double r = velo_scale;
aXYVeloElem[ielem_cur*6+0] = co[0][0] + r*velo[0][0];
aXYVeloElem[ielem_cur*6+1] = co[0][1] + r*velo[0][1];
aXYVeloElem[ielem_cur*6+2] = co[1][0] + r*velo[1][0];
aXYVeloElem[ielem_cur*6+3] = co[1][1] + r*velo[1][1];
aXYVeloElem[ielem_cur*6+4] = co[2][0] + r*velo[2][0];
aXYVeloElem[ielem_cur*6+5] = co[2][1] + r*velo[2][1];
ielem_cur++;
}
}
assert( ielem_cur == nelem );
*/
if( world.IsIdField(m_IdFieldColor) ){
if( aValColor==0 ){ aValColor = new double [nnode]; }
const Fem::Field::CField& fc = world.GetField(this->m_IdFieldColor);
assert( fc.GetNodeSegInNodeAry(CORNER).id_na_va == fv.GetNodeSegInNodeAry(CORNER).id_na_va );
const Fem::Field::CNodeAry::CNodeSeg& nsv_c = fc.GetNodeSeg(CORNER,true, world,VALUE);
for(unsigned int inode=0;inode<nnode;inode++){
nsv_c.GetValue(inode,aValColor+inode);
}
}
else{
if( aValColor != 0 ){ delete[] aValColor; aValColor = 0; }
}
for(unsigned int iec=0;iec<aEdgeColor.size();iec++){
aEdgeColor[iec].Update(world);
}
return true;
}
bool CZLinearSystem::UpdateValueOfField(
unsigned int id_field, Fem::Field::CFieldWorld& world, Fem::Field::FIELD_DERIVATION_TYPE fdt )
{
if( !world.IsIdField(id_field) ) return false;
const CField& field = world.GetField(id_field);
{
unsigned int id_na_val=0, id_ns_val=0;
{ // CORNERについて更新
const CField::CNodeSegInNodeAry& nsna_c = field.GetNodeSegInNodeAry(CORNER);
id_na_val = nsna_c.id_na_va;
if( fdt == VALUE ) id_ns_val = nsna_c.id_ns_va;
else if( fdt == VELOCITY ) id_ns_val = nsna_c.id_ns_ve;
else if( fdt == ACCELERATION ) id_ns_val = nsna_c.id_ns_ac;
else{ assert(0); }
}
if( id_na_val != 0 ){
CZVector_Blk* m_Update = this->GetUpdatePtr(id_field,CORNER,world);
assert( m_Update != 0 );
CNodeAry& na = world.GetNA(id_na_val);
assert( na.IsSegID(id_ns_val) );
na.AddValueToNodeSegment(id_ns_val,*m_Update,1.0);
// CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns_val);
}
}
{
unsigned int id_na_val=0, id_ns_val=0;
{ // EDGEについて更新
const CField::CNodeSegInNodeAry& nsna_c = field.GetNodeSegInNodeAry(EDGE);
id_na_val = nsna_c.id_na_va;
if( fdt == VALUE ) id_ns_val = nsna_c.id_ns_va;
else if( fdt == VELOCITY ) id_ns_val = nsna_c.id_ns_ve;
else if( fdt == ACCELERATION ) id_ns_val = nsna_c.id_ns_ac;
else{ assert(0); }
}
if( id_na_val != 0 ){
CZVector_Blk* m_Update = this->GetUpdatePtr(id_field,EDGE,world);
assert( m_Update != 0 );
CNodeAry& na = world.GetNA(id_na_val);
assert( na.IsSegID(id_ns_val) );
na.AddValueToNodeSegment(id_ns_val,*m_Update,1.0);
// CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns_val);
// ns.is_updated = true;
}
}
{
unsigned int id_na_val=0, id_ns_val=0;
{ // BUBBLEについて更新
const CField::CNodeSegInNodeAry& nsna_c = field.GetNodeSegInNodeAry(BUBBLE);
id_na_val = nsna_c.id_na_va;
if( fdt == VALUE ) id_ns_val = nsna_c.id_ns_va;
else if( fdt == VELOCITY ) id_ns_val = nsna_c.id_ns_ve;
else if( fdt == ACCELERATION ) id_ns_val = nsna_c.id_ns_ac;
else{ assert(0); }
}
if( id_na_val != 0 ){
CZVector_Blk* m_Update = this->GetUpdatePtr(id_field,BUBBLE,world);
assert( m_Update != 0 );
CNodeAry& na = world.GetNA(id_na_val);
assert( na.IsSegID(id_ns_val) );
na.AddValueToNodeSegment(id_ns_val,*m_Update,1.0);
// CNodeAry::CNodeSeg& ns = na.GetSeg(id_ns_val);
// ns.is_updated = true;
}
}
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;
}