template<> void
PAlgebraModTmpl<zz_pX,vec_zz_pX,zz_pXModulus>::mapToFt(zz_pX& r,
const zz_pX& G,unsigned t,const zz_pX* rF1) const
{
int i = zmStar.indexOfRep(t);
if (i < 0) { r=zz_pX::zero(); return; }
if (rF1==NULL) { // Compute the representation "from scratch"
zz_pE::init(factors[i]); // work with the extension field GF_2[X]/Ft(X)
zz_pEX Ga=to_zz_pEX((zz_pX&)G);// G is polynomial over the extension field
r=rep(FindRoot(Ga)); // Find a root of G in this field
return;
}
// if rF1 is set, then use it instead, setting r = rF1(X^t) mod Ft(X)
zz_pXModulus Ft(factors[i]);
// long tInv = InvMod(t,m);
zz_pX X2t = PowerXMod(t,Ft); // X2t = X^t mod Ft
r = CompMod(*rF1,X2t,Ft); // r = F1(X2t) mod Ft
/* Debugging sanity-check: G(r)=0 in the extension field (Z/2Z)[X]/Ft(X)
zz_pE::init(factors[i]);
zz_pEX Ga=to_zz_pEX((zz_pX&)G);// G as a polynomial over the extension field
zz_pE ra =to_zz_pE(r); // r is an element in the extension field
eval(ra,Ga,ra); // ra = Ga(ra)
if (!IsZero(ra)) {// check that Ga(r)=0 in this extension field
cout << "rF1(X^t) mod Ft(X) != root of G mod Ft, t=" << t << endl;
exit(0);
}*******************************************************************/
}
void PAlgebraModDerived<type>::mapToFt(RX& w,
const RX& G,unsigned long t,const RX* rF1) const
{
if (isDryRun()) {
w = RX::zero();
return;
}
long i = zMStar.indexOfRep(t);
if (i < 0) { clear(w); return; }
if (rF1==NULL) { // Compute the representation "from scratch"
// special case
if (G == factors[i]) {
SetX(w);
return;
}
//special case
if (deg(G) == 1) {
w = -ConstTerm(G);
return;
}
// the general case: currently only works when r == 1
assert(r == 1);
REBak bak; bak.save();
RE::init(factors[i]); // work with the extension field GF_p[X]/Ft(X)
REX Ga;
conv(Ga, G); // G as a polynomial over the extension field
vec_RE roots;
FindRoots(roots, Ga); // Find roots of G in this field
RE* first = &roots[0];
RE* last = first + roots.length();
RE* smallest = min_element(first, last);
// make a canonical choice
w=rep(*smallest);
return;
}
// if rF1 is set, then use it instead, setting w = rF1(X^t) mod Ft(X)
RXModulus Ft(factors[i]);
// long tInv = InvMod(t,m);
RX X2t = PowerXMod(t,Ft); // X2t = X^t mod Ft
w = CompMod(*rF1,X2t,Ft); // w = F1(X2t) mod Ft
/* Debugging sanity-check: G(w)=0 in the extension field (Z/2Z)[X]/Ft(X)
RE::init(factors[i]);
REX Ga;
conv(Ga, G); // G as a polynomial over the extension field
RE ra;
conv(ra, w); // w is an element in the extension field
eval(ra,Ga,ra); // ra = Ga(ra)
if (!IsZero(ra)) {// check that Ga(w)=0 in this extension field
cout << "rF1(X^t) mod Ft(X) != root of G mod Ft, t=" << t << endl;
exit(0);
}*******************************************************************/
}
void AnchoredRectangleHandler::initRectangle(const Eigen::VectorXd& Fw,
double lambda, const Eigen::VectorXd& z, Eigen::VectorXd& shapeParamshat,
Eigen::VectorXd& FOhphat, Eigen::VectorXd &FOqhat) {
//Get the points
Eigen::Vector3d m1(z[0], z[1], 1);
Eigen::Vector3d m2(z[2], z[3], 1);
Eigen::Vector3d m3(z[4], z[5], 1);
Eigen::Vector3d m4(z[6], z[7], 1);
Eigen::Vector3d Ft(Fw[0], Fw[1], Fw[2]);
Eigen::Quaterniond Fq(Fw[3], Fw[4], Fw[5], Fw[6]);
//compute normals
double c2 = (m1.cross(m3).transpose() * m4)[0]
/ (m2.cross(m3).transpose() * m4)[0];
double c3 = (m1.cross(m3).transpose() * m2)[0]
/ (m4.cross(m3).transpose() * m2)[0];
Eigen::Vector3d n2 = c2 * m2 - m1;
Eigen::Vector3d n3 = c3 * m4 - m1;
//Compute rotation matrix columns
Eigen::Vector3d R1 = _K.inverse() * n2;
R1 = R1 / R1.norm();
Eigen::Vector3d R2 = _K.inverse() * n3;
R2 = R2 / R2.norm();
Eigen::Vector3d R3 = R1.cross(R2);
//Compute rotation from camera to object
Eigen::Matrix3d R;
R << R1, R2, R3;
Eigen::Quaterniond FOq_e(R);
// and initialize the of the object with respect to the anchor frame
FOqhat << FOq_e.w(), FOq_e.x(), FOq_e.y(), FOq_e.z();
// now initialize lower left corner homogeneous point
FOhphat << z[0], z[1], 1.0;
FOhphat = _K.inverse() * FOhphat;
FOhphat(2) = 1.0 / lambda; // 1/d distance of the plane parallel to the image plane on which features are initialized.
//Compute frame transaltion
Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
double ff = sqrt(
(n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);
//compute shape parameters
Eigen::Vector3d X = _K * R1;
Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;
double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];
//Write the results
shapeParamshat << ff, w / lambda;
}
bool Law2_L6Geom_PelletPhys_Pellet::go(const shared_ptr<CGeom>& cg, const shared_ptr<CPhys>& cp, const shared_ptr<Contact>& C){
const L6Geom& g(cg->cast<L6Geom>()); PelletPhys& ph(cp->cast<PelletPhys>());
Real& Fn(ph.force[0]); Eigen::Map<Vector2r> Ft(&ph.force[1]);
if(C->isFresh(scene)) C->data=make_shared<PelletCData>();
Real& uNPl(C->data->cast<PelletCData>().uNPl);
Real& uN0(C->data->cast<PelletCData>().uN0);
assert(C->data && dynamic_pointer_cast<PelletCData>(C->data));
if(iniEqlb && C->isFresh(scene)) uN0=g.uN;
Real uN=g.uN-uN0; // normal displacement, taking iniEqlb in account
// break contact
if(uN>0) return false;
Real d0=g.lens.sum();
if(ph.normPlastCoeff<=0) uNPl=0.;
const Vector2r velT(g.vel[1],g.vel[2]);
ph.torque=Vector3r::Zero();
// normal force
Fn=ph.kn*(uN-uNPl); // trial force
if(ph.normPlastCoeff>0){ // normal plasticity activated
if(Fn>0){
if(ph.ka<=0) Fn=0;
else{ Fn=min(Fn,adhesionForce(uN,uNPl,ph.ka)); assert(Fn>0); }
} else {
Real Fy=yieldForce(uN,d0,ph.kn,ph.normPlastCoeff);
// normal plastic slip
if(Fn<Fy){
Real uNPl0=uNPl; // needed when tracking energy
uNPl=uN-Fy/ph.kn;
if(unlikely(scene->trackEnergy)){
// backwards trapezoid integration
Real Fy0=Fy+yieldForceDerivative(uN,d0,ph.kn,ph.normPlastCoeff)*(uNPl0-uNPl);
Real dissip=.5*abs(Fy0+Fy)*abs(uNPl-uNPl0);
scene->energy->add(dissip,plastSplit?"normPlast":"plast",plastSplit?normPlastIx:plastIx,EnergyTracker::IsIncrement | EnergyTracker::ZeroDontCreate);
tryAddDissipState(DISSIP_NORM_PLAST,dissip,C);
}
if(thinRate>0 && thinRelRMin<1.){
const Vector2r bendVel(g.angVel[1],g.angVel[2]);
Real dRad_0=thinRate*(uNPl0-uNPl)*(scene->dt*bendVel.norm());
for(const Particle* p:{C->leakPA(),C->leakPB()}){
if(!dynamic_cast<Sphere*>(p->shape.get())) continue;
Real dRad=dRad_0; // copy to be modified
auto& s=p->shape->cast<Sphere>();
//Real r0=(C->geom->cast<L6Geom>().lens[p.get()==C->pA.get()?0:1]);
Real r0=cbrt(3*s.nodes[0]->getData<DemData>().mass/(4*M_PI*p->material->density));
Real rMin=r0*thinRelRMin;
if(thinRefRad>0.) rMin*=pow(r0/thinRefRad,thinMinExp);
if(s.radius<=rMin) continue;
// 0..1 norm between rMin and r0
Real r01=(s.radius-rMin)/(r0-rMin);
if(thinExp>0) dRad*=pow(r01,thinExp);
if(thinRefRad>0.) dRad*=pow(r0/thinRefRad,thinRateExp);
boost::mutex::scoped_lock lock(s.nodes[0]->getData<DemData>().lock);
// cerr<<"#"<<p->id<<": radius "<<s.radius<<" -> "<<s.radius-dRad<<endl;
s.radius=max(rMin,s.radius-dRad*r01);
s.color=CompUtils::clamped(1-(s.radius-rMin)/(r0-rMin),0,1);
}
}
Fn=Fy;
}
// in the elastic regime, Fn is trial force already
}
}
/* add fake confinement */
if(confSigma!=0) Fn-=g.contA*confSigma*(confRefRad>0.?pow(g.contA/(M_PI*pow(confRefRad,2)),confExp):1.);
// shear force
Ft+=scene->dt*ph.kt*velT;
Real maxFt=abs(Fn)*ph.tanPhi; assert(maxFt>=0);
// shear plastic slip
if(Ft.squaredNorm()>pow(maxFt,2)){
Real FtNorm=Ft.norm();
Real ratio=maxFt/FtNorm;
if(unlikely(scene->trackEnergy)){
Real dissip=(.5*(FtNorm-maxFt)+maxFt)*(FtNorm-maxFt)/ph.kt;
scene->energy->add(dissip,"plast",plastIx,EnergyTracker::IsIncrement | EnergyTracker::ZeroDontCreate);
tryAddDissipState(DISSIP_SHEAR_PLAST,dissip,C);
}
Ft*=ratio;
}
assert(!isnan(Fn)); assert(!isnan(Ft[0]) && !isnan(Ft[1]));
// elastic potential energy
if(unlikely(scene->trackEnergy)) scene->energy->add(0.5*(pow(Fn,2)/ph.kn+Ft.squaredNorm()/ph.kt),"elast",elastPotIx,EnergyTracker::IsResettable);
return true;
}
bool Law2_L6Geom_HertzPhys_DMT::go(const shared_ptr<CGeom>& cg, const shared_ptr<CPhys>& cp, const shared_ptr<Contact>& C){
const L6Geom& g(cg->cast<L6Geom>()); HertzPhys& ph(cp->cast<HertzPhys>());
Real& Fn(ph.force[0]); Eigen::Map<Vector2r> Ft(&ph.force[1]);
ph.torque=Vector3r::Zero();
const Real& dt(scene->dt);
const Real& velN(g.vel[0]);
const Vector2r velT(g.vel[1],g.vel[2]);
// current normal stiffness
Real kn0=ph.K*sqrt(ph.R);
// TODO: this not really valid for Schwarz?
ph.kn=(3/2.)*kn0*sqrt(-g.uN);
// normal elastic and adhesion forces
// those are only split in the DMT model, Fna is zero for Schwarz or Hertz
Real Fne, Fna=0.;
if(ph.alpha==0.){
if(g.uN>0){
// TODO: track nonzero energy of broken contact with adhesion
// TODO: take residual shear force in account?
// if(unlikely(scene->trackEnergy)) scene->energy->add(normalElasticEnergy(ph.kn0,0),"dmtComeGo",dmtIx,EnergyTracker::IsIncrement|EnergyTracker::ZeroDontCreate);
return false;
}
// pure Hertz/DMT
Fne=-kn0*pow_i_2(-g.uN,3); // elastic force
// DMT adhesion ("sticking") force
// See Dejaguin1975 ("Effect of Contact Deformation on the Adhesion of Particles"), eq (44)
// Derjaguin has Fs=2πRφ(ε), which is derived for sticky sphere (with surface energy)
// in contact with a rigid plane (without surface energy); therefore the value here is twice that of Derjaguin
// See also Chiara's thesis, pg 39 eq (3.19).
Fna=4*M_PI*ph.R*ph.gamma;
} else {
// Schwarz model
// new contacts with adhesion add energy to the system, which is then taken away again
if(unlikely(scene->trackEnergy) && C->isFresh(scene)){
// TODO: scene->energy->add(???,"dmtComeGo",dmtIx,EnergyTracker::IsIncrement)
}
const Real& gamma(ph.gamma); const Real& R(ph.R); const Real& alpha(ph.alpha); const Real& K(ph.K);
Real delta=-g.uN; // inverse convention
Real Pc=-6*M_PI*R*gamma/(pow(alpha,2)+3);
Real xi=sqrt(((2*M_PI*gamma)/(3*K))*(1-3/(pow(alpha,2)+3)));
Real deltaMin=-3*cbrt(R*pow(xi,4)); // -3R(-1/3)*ξ^(-4/3)
// broken contact
if(delta<deltaMin){
// TODO: track energy
return false;
}
// solution brackets
// XXX: a is for sure also greater than delta(a) for Hertz model, with delta>0
// this should be combined with aMin which gives the function apex
Real aMin=pow_i_3(xi*R,2); // (ξR)^(2/3)
Real a0=pow_i_3(4,2)*aMin; // (4ξR)^(2/3)=4^(2/3) (ξR)^(2/3)
Real aLo=(delta<0?aMin:a0);
Real aHi=aMin+sqrt(R*(delta-deltaMin));
auto delta_diff_ddiff=[&](const Real& a){
Real aInvSqrt=1/sqrt(a);
return boost::math::make_tuple(
pow(a,2)/R-4*xi*sqrt(a)-delta, // subtract delta as we need f(x)=0
2*a/R-2*xi*aInvSqrt,
2/R+xi*pow(aInvSqrt,3)
);
};
// use a0 (defined as δ(a0)=0) as intial guess for new contacts, since they are likely close to the equilibrium condition
// use the previous value for old contacts
Real aInit=(C->isFresh(scene)?a0:ph.contRad);
boost::uintmax_t iter=100;
Real a=boost::math::tools::halley_iterate(delta_diff_ddiff,aInit,aLo,aHi,digits,iter);
// increment call and iteration count
#ifdef WOO_SCHWARZ_COUNTERS
nCallsIters.add(0,1);
nCallsIters.add(1,iter);
#endif
ph.contRad=a;
Real Pne=pow(sqrt(pow(a,3)*(K/R))-alpha*sqrt(-Pc),2)+Pc;
Fne=-Pne; // inverse convention
if(isnan(Pne)){
cerr<<"R="<<R<<", K="<<K<<", xi="<<xi<<", alpha="<<alpha<<endl;
cerr<<"delta="<<delta<<", deltaMin="<<deltaMin<<", aMin="<<aMin<<", aLo="<<aLo<<", aHi="<<aHi<<", a="<<a<<", iter="<<iter<<", Pne="<<Pne<<"; \n\n";
abort();
}
}
// viscous coefficient, both for normal and tangential force
// Antypov2012 (10)
// XXX: max(-g.uN,0.) for adhesive models so that eta is not NaN
Real eta=(ph.alpha_sqrtMK>0?ph.alpha_sqrtMK*pow_1_4(max(-g.uN,0.)):0.);
// cerr<<"eta="<<eta<<", -g.uN="<<-g.uN<<"; ";
Real Fnv=eta*velN; // viscous force
// DMT ONLY (for now at least):
if(ph.alpha==0. && noAttraction && Fne+Fnv>0) Fnv=-Fne; // avoid viscosity which would induce attraction with DMT
// total normal force
Fn=Fne+Fna+Fnv;
// normal viscous dissipation
if(unlikely(scene->trackEnergy)) scene->energy->add(Fnv*velN*dt,"viscN",viscNIx,EnergyTracker::IsIncrement|EnergyTracker::ZeroDontCreate);
// shear sense; zero shear stiffness in tension (XXX: should be different with adhesion)
ph.kt=ph.kt0*sqrt(g.uN<0?-g.uN:0);
Ft+=dt*ph.kt*velT;
// sliding: take adhesion in account
Real maxFt=std::abs(min(0.,Fn)*ph.tanPhi);
if(Ft.squaredNorm()>pow(maxFt,2)){
// sliding
Real FtNorm=Ft.norm();
Real ratio=maxFt/FtNorm;
// sliding dissipation
if(unlikely(scene->trackEnergy)) scene->energy->add((.5*(FtNorm-maxFt)+maxFt)*(FtNorm-maxFt)/ph.kt,"plast",plastIx,EnergyTracker::IsIncrement | EnergyTracker::ZeroDontCreate);
// cerr<<"uN="<<g.uN<<",Fn="<<Fn<<",|Ft|="<<Ft.norm()<<",maxFt="<<maxFt<<",ratio="<<ratio<<",Ft2="<<(Ft*ratio).transpose()<<endl;
Ft*=ratio;
} else {
// viscous tangent force (only applied in the absence of sliding)
Ft+=eta*velT;
if(unlikely(scene->trackEnergy)) scene->energy->add(eta*velT.dot(velT)*dt,"viscT",viscTIx,EnergyTracker::IsIncrement|EnergyTracker::ZeroDontCreate);
}
assert(!isnan(Fn)); assert(!isnan(Ft[0]) && !isnan(Ft[1]));
// elastic potential energy
if(unlikely(scene->trackEnergy)){
// XXX: this is incorrect with adhesion
// skip if in tension, since we would get NaN from delta^(2/5)
if(g.uN<0) scene->energy->add(normalElasticEnergy(kn0,-g.uN)+0.5*Ft.squaredNorm()/ph.kt,"elast",elastPotIx,EnergyTracker::IsResettable);
}
LOG_DEBUG("uN="<<g.uN<<", Fn="<<Fn<<"; duT/dt="<<velT[0]<<","<<velT[1]<<", Ft="<<Ft[0]<<","<<Ft[1]);
return true;
}
