rigidSphereContactTerms.cpp

//
// contact base term
//
// Description: contact with a rigid sphere
//
//
// Author:  <Tianyu ZHANG>, (C) 2020
//
// Copyright: See COPYING file that comes with this distribution
//
#include "rigidSphereContactTerms.h"
#include "contactDomain.h"
#include "unknownField.h"

massRigidSphere::massRigidSphere(rigidSphereContactDomain *cdom, rigidContactSpaceBase *spc) : 
                 _radius(cdom->getRadius()),_rho(cdom->getDensity()),_spc(spc){}

void massRigidSphere::get(MElement *ele,int npts,IntPt *GP,fullMatrix<double> &m) const
{
  // get the number of sphere component (same mass in all direction)
  int nbdofs = _spc->getNumKeysOfGravityCenter();
  m.resize(nbdofs,nbdofs,true); // true --> setAll(0.)
  // non-distinguish full or hollow sphere
  double masse = _rho*4.*M_PI*_radius*_radius*_radius/3.;

  for(int i=0;i<nbdofs;i++)
    m(i,i) = masse;
}

 forceRigidSphereContact::forceRigidSphereContact(const rigidSphereContactDomain *cdom,
                            rigidContactSpaceBase *sp, const unknownField *ufield) : 
	rigidContactLinearTermBase<double>(sp,ufield), _cdom(cdom),_radius(cdom->getRadius()),_GC(cdom->getGC()->point())
{
  if(_cdom->getDomain()->IsInterfaceTerms()){
    partDomain *dom = _cdom->getDomain();
    if(dom->getFormulation()){
      _fdgfactor = 1.;
      _facGC =0.5;
    }
    else{
      _fdgfactor = 0.5;
      _facGC =1.;
    }
  }
  else{
    _fdgfactor = 0.5;
    _facGC = 1.;
  }
  _penalty = _fdgfactor*_cdom->getPenalty();
} // (need to be verified)

void forceRigidSphereContact::get(const MVertex *ver, const double disp[6],double mvalue[6]) const
{
  // B = gravity center of sphere
  // A = vertex for which we look for contact (iteration on every vertex which is potentially in contact (defined in .py and execute at solver level))
  B = _GC;
  Bdisp.setPosition(disp[3],disp[4],disp[5]);
  B+=Bdisp;
  // Distance between vertex and gravity center of sphere
  // line BA
  A = ver->point();
  Adisp.setPosition(disp[0],disp[1],disp[2]);
  A+=Adisp;
  SVector3 _lastNormalContact(B,A); // direction : from Master to Slave
  double d = _lastNormalContact.norm();

  // test to know if contact
  if(d < _radius){
    _lastNormalContact.normalize(); 
    double penetration = _radius-d;
    double penpen = penetration*_penalty;
    for(int j=0;j<3;j++){
      mvalue[j]+= penpen*_lastNormalContact(j);
      mvalue[j+3] -=_facGC*penpen*_lastNormalContact(j); // count two times
    }
  }
}

// Protected can only be called by stiffnessRigidSphereContact::get(MElement *ele, int npts, IntPt *GP, fullMatrix<double> &m) (??)
// NO RESIZE (??)
void stiffnessRigidSphereContact::get(MElement *ele, const int verIndex, fullMatrix<double> &m) const
{
  double _perturbation = 1e-10; // numerical perturbation
  double _doublepertexpm1 = 1./(2.e-10);
  // Data for get function (Allocated Once)
  double fp[6];
  double fm[6];
  int nbdofs;
  MVertex *ver;
  std::vector<Dof> R;
  std::vector<double> disp;
  double pertdisp[6];

  // perturbation numeric
  rigidContactLinearTermBase<double>* rlterm = static_cast<rigidContactLinearTermBase<double>*>(_lterm);
  int nbdofselem = _spc->getNumKeys(ele) - _spc->getNumKeysOfGravityCenter();
  int nbFF = ele->getNumVertices();
  ver = ele->getVertex(verIndex);
  disp.clear(); R.clear();
  _spc->getKeys(ele,verIndex,R);
  _ufield->get(R,disp);
  for(int j=0;j<6;j++)
    pertdisp[j] = disp[j];

  // perturbation on each Dofs 3 for vertex and 3 for GC of sphere
  for(int i=0;i<6;i++){
    // set force component to zero
    for(int j=0;j<6;j++)
      fp[j] = fm[j] = 0.;

    // Force -
    pertdisp[i] = disp[i] - _perturbation;
    rlterm->get(ver,pertdisp,fm);

    // Force +
    pertdisp[i] = disp[i] + _perturbation;
    rlterm->get(ver,pertdisp,fp);

    // Assembly in matrix
    if(i<3){ // perturbation on vertex
      for(int j=0; j<3; j++){
        m(verIndex+nbFF*j,verIndex+nbFF*i) -= (fp[j]-fm[j])*_doublepertexpm1;
        m(nbdofselem+j,verIndex+nbFF*i) -= (fp[j+3]-fm[j+3])*_doublepertexpm1;
      }
    }
    else{ // perturbation on gravity center
      for(int j=0; j<3; j++){
        m(verIndex+nbFF*j,nbdofselem+i-3) -= (fp[j]-fm[j])*_doublepertexpm1;
        m(nbdofselem+j,nbdofselem+i-3) -= (fp[j+3]-fm[j+3])*_doublepertexpm1;
      }
    }
    // virgin pertdisp
    pertdisp[i] = disp[i];
  }
}
void stiffnessRigidSphereContact::get(MElement *ele, int npts, IntPt *GP, fullMatrix<double> &m) const
{
  int nbdofs = _spc->getNumKeys(ele);
  m.resize(nbdofs,nbdofs,true); // true --> setAll(0.)
  // search the vertices for which the matrix has to be computed
  contactNodeStiffness * contactNodes = _lterm->getContactNode();
  for(contactNodeStiffness::nodeContainer::const_iterator itn = contactNodes->nBegin(); itn!= contactNodes->nEnd(); ++itn){
    if((*itn)->_ele == ele){ // contact
      this->get(ele,(*itn)->_verIndex,m);
    }
  }
}