bool Ig2_Facet_Sphere_L3Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const State& state1, const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& I){
const Facet& facet(s1->cast<Facet>());
Real radius=s2->cast<Sphere>().radius;
// begin facet-local coordinates
Vector3r cogLine=state1.ori.conjugate()*(state2.pos+shift2-state1.pos); // connect centers of gravity
Vector3r normal=facet.normal; // trial contact normal
Real planeDist=normal.dot(cogLine);
if(abs(planeDist)>radius && !I->isReal() && !force) return false; // sphere too far
if(planeDist<0){normal*=-1; planeDist*=-1; }
Vector3r planarPt=cogLine-planeDist*normal; // project sphere center to the facet plane
Vector3r contactPt; // facet's point closes to the sphere
Real normDotPt[3]; // edge outer normals dot products
for(int i=0; i<3; i++) normDotPt[i]=facet.ne[i].dot(planarPt-facet.vertices[i]);
short w=(normDotPt[0]>0?1:0)+(normDotPt[1]>0?2:0)+(normDotPt[2]>0?4:0); // bitmask whether the closest point is outside (1,2,4 for respective edges)
switch(w){
case 0: contactPt=planarPt; break; // ---: inside triangle
case 1: contactPt=getClosestSegmentPt(planarPt,facet.vertices[0],facet.vertices[1]); break; // +-- (n1)
case 2: contactPt=getClosestSegmentPt(planarPt,facet.vertices[1],facet.vertices[2]); break; // -+- (n2)
case 4: contactPt=getClosestSegmentPt(planarPt,facet.vertices[2],facet.vertices[0]); break; // --+ (n3)
case 3: contactPt=facet.vertices[1]; break; // ++- (v1)
case 5: contactPt=facet.vertices[0]; break; // +-+ (v0)
case 6: contactPt=facet.vertices[2]; break; // -++ (v2)
case 7: throw logic_error("Ig2_Facet_Sphere_L3Geom: Impossible sphere-facet intersection (all points are outside the edges). (please report bug)"); // +++ (impossible)
default: throw logic_error("Ig2_Facet_Sphere_L3Geom: Nonsense intersection value. (please report bug)");
}
normal=cogLine-contactPt; // normal is now the contact normal, still in local coords
if(!I->isReal() && normal.squaredNorm()>radius*radius && !force) { return false; } // fast test before sqrt
Real dist=normal.norm(); normal/=dist; // normal is unit vector now
// end facet-local coordinates
normal=state1.ori*normal; // normal is in global coords now
handleSpheresLikeContact(I,state1,state2,shift2,/*is6Dof*/false,normal,/*contact pt*/state2.pos+shift2-normal*dist,dist-radius,0,radius);
return true;
}
bool Cg2_Wall_Sphere_L6Geom::go(const shared_ptr<Shape>& sh1, const shared_ptr<Shape>& sh2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C) {
if(scene->isPeriodic && scene->cell->hasShear()) throw std::logic_error("Cg2_Wall_Sphere_L6Geom does not handle periodic boundary conditions with skew (Scene.cell.trsf is not diagonal).");
const Wall& wall=sh1->cast<Wall>();
const Sphere& sphere=sh2->cast<Sphere>();
assert(wall.numNodesOk());
assert(sphere.numNodesOk());
const Real& radius=sphere.radius;
const int& ax=wall.axis;
const int& sense=wall.sense;
const Vector3r& wallPos=wall.nodes[0]->pos;
Vector3r spherePos=sphere.nodes[0]->pos+shift2;
Real dist=spherePos[ax]-wallPos[ax]; // signed "distance" between centers
if(!C->isReal() && abs(dist)>radius && !force) {
return false; // wall and sphere too far from each other
}
// contact point is sphere center projected onto the wall
Vector3r contPt=spherePos;
contPt[ax]=wallPos[ax];
Vector3r normal=Vector3r::Zero();
// wall interacting from both sides: normal depends on sphere's position
assert(sense==-1 || sense==0 || sense==1);
if(sense==0) {
// for new contacts, normal given by the sense of approaching the wall
if(!C->geom) normal[ax]=dist>0?1.:-1.;
// for existing contacts, use the previous normal
else normal[ax]=C->geom->cast<L6Geom>().trsf.col(0)[ax];
}
else normal[ax]=(sense==1?1.:-1);
Real uN=normal[ax]*dist-radius; // takes in account sense, radius and distance
// this may not happen anymore as per conditions above
assert(!(C->geom && C->geom->cast<L6Geom>().trsf.col(0)!=normal));
#if 0
// check that the normal did not change orientation (would be abrupt here)
if(C->geom && C->geom->cast<L6Geom>().trsf.col(0)!=normal.transpose()) {
throw std::logic_error((boost::format("Cg2_Wall_Sphere_L6Geom: normal changed from %s to %s in Wall+Sphere ##%d+%d (with Wall.sense=0, a particle might cross the Wall plane if Δt is too high, repulsive force to small or velocity too high.")%C->geom->cast<L6Geom>().trsf.col(0)%normal.transpose()%C->leakPA()->id%C->leakPB()->id).str());
}
#endif
const DemData& dyn1(wall.nodes[0]->getData<DemData>());
const DemData& dyn2(sphere.nodes[0]->getData<DemData>());
handleSpheresLikeContact(C,wallPos,dyn1.vel,dyn1.angVel,spherePos,dyn2.vel,dyn2.angVel,normal,contPt,uN,/*r1*/-radius,radius);
return true;
};
bool Ig2_Sphere_Sphere_L3Geom::genericGo(bool is6Dof, const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const State& state1, const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& I){
// temporary hack only, to not have elastic potential energy in rigid packings with overlapping spheres
//if(state1.blockedDOFs==State::DOF_ALL && state2.blockedDOFs==State::DOF_ALL) return false;
const Real& r1=s1->cast<Sphere>().radius; const Real& r2=s2->cast<Sphere>().radius;
Vector3r relPos=state2.pos+shift2-state1.pos;
Real unDistSq=relPos.squaredNorm()-pow(abs(distFactor)*(r1+r2),2);
if (unDistSq>0 && !I->isReal() && !force) return false;
// contact exists, go ahead
Real dist=relPos.norm();
Real uN=dist-(r1+r2);
Vector3r normal=relPos/dist;
Vector3r contPt=state1.pos+(r1+0.5*uN)*normal;
handleSpheresLikeContact(I,state1,state2,shift2,is6Dof,normal,contPt,uN,r1,r2);
return true;
};
bool Ig2_Wall_Sphere_L3Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const State& state1, const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& I){
if(scene->isPeriodic) throw std::logic_error("Ig2_Wall_Sphere_L3Geom does not handle periodic boundary conditions.");
const Real& radius=s2->cast<Sphere>().radius; const int& ax(s1->cast<Wall>().axis); const int& sense(s1->cast<Wall>().sense);
Real dist=state2.pos[ax]+shift2[ax]-state1.pos[ax]; // signed "distance" between centers
if(!I->isReal() && abs(dist)>radius && !force) { return false; }// wall and sphere too far from each other
// contact point is sphere center projected onto the wall
Vector3r contPt=state2.pos+shift2; contPt[ax]=state1.pos[ax];
Vector3r normal=Vector3r::Zero();
// wall interacting from both sides: normal depends on sphere's position
assert(sense==-1 || sense==0 || sense==1);
if(sense==0) normal[ax]=dist>0?1.:-1.;
else normal[ax]=(sense==1?1.:-1);
Real uN=normal[ax]*dist-radius; // takes in account sense, radius and distance
// check that the normal did not change orientation (would be abrup here)
if(I->geom && I->geom->cast<L3Geom>().normal!=normal){
ostringstream oss; oss<<"Ig2_Wall_Sphere_L3Geom: normal changed from ("<<I->geom->cast<L3Geom>().normal<<" to "<<normal<<" in Wall+Sphere ##"<<I->getId1()<<"+"<<I->getId2()<<" (with Wall.sense=0, a particle might cross the Wall plane, if Δt is too high)"; throw std::logic_error(oss.str().c_str());
}
handleSpheresLikeContact(I,state1,state2,shift2,/*is6Dof*/false,normal,contPt,uN,/*r1*/0,/*r2*/radius);
return true;
};
bool Cg2_Sphere_Sphere_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
const Real& r1=s1->cast<Sphere>().radius; const Real& r2=s2->cast<Sphere>().radius;
assert(s1->numNodesOk()); assert(s2->numNodesOk());
assert(s1->nodes[0]->hasData<DemData>()); assert(s2->nodes[0]->hasData<DemData>());
const DemData& dyn1(s1->nodes[0]->getData<DemData>()); const DemData& dyn2(s2->nodes[0]->getData<DemData>());
Vector3r relPos=s2->nodes[0]->pos+shift2-s1->nodes[0]->pos;
Real unDistSq=relPos.squaredNorm()-pow(abs(distFactor)*(r1+r2),2);
if (unDistSq>0 && !C->isReal() && !force) return false;
// contact exists, go ahead
Real dist=relPos.norm();
Real uN=dist-(r1+r2);
Vector3r normal=relPos/dist;
Vector3r contPt=s1->nodes[0]->pos+(r1+0.5*uN)*normal;
handleSpheresLikeContact(C,s1->nodes[0]->pos,dyn1.vel,dyn1.angVel,s2->nodes[0]->pos+shift2,dyn2.vel,dyn2.angVel,normal,contPt,uN,r1,r2);
return true;
};
bool Cg2_Wall_Facet_L6Geom::go(const shared_ptr<Shape>& sh1, const shared_ptr<Shape>& sh2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C) {
if(scene->isPeriodic && scene->cell->hasShear()) throw std::logic_error("Cg2_Wall_Facet_L6Geom does not handle periodic boundary conditions with skew (Scene.cell.trsf is not diagonal).");
const Wall& wall=sh1->cast<Wall>();
const Facet& facet=sh2->cast<Facet>();
assert(wall.numNodesOk());
assert(facet.numNodesOk());
if(!(facet.halfThick>0.)) {
LOG_WARN("Cg2_Wall_Facet_L6Geom: Contact of Wall with zero-thickness facet is always false.");
return false;
}
const Real& radius=facet.halfThick;
const int& ax=wall.axis;
const int& sense=wall.sense;
const Vector3r& wallPos=wall.nodes[0]->pos;
Vector3r fPos[]= {facet.nodes[0]->pos+shift2,facet.nodes[1]->pos+shift2,facet.nodes[2]->pos+shift2};
Eigen::Array<Real,3,1> dist(fPos[0][ax]-wallPos[ax],fPos[1][ax]-wallPos[ax],fPos[2][ax]-wallPos[ax]);
if(!C->isReal() && abs(dist[0])>radius && abs(dist[1])>radius && abs(dist[2])>radius && !force) return false;
Vector3r normal=Vector3r::Zero();
if(sense==0) {
// for new contacts, normal is given by the sense the wall is being approached
// use average distance (which means distance to the facet midpoint) to determine that
if(!C->geom) normal[ax]=dist.sum()>0?1.:-1.;
// use the previous normal for existing contacts
else normal[ax]=C->geom->cast<L6Geom>().trsf.col(0)[ax];
}
else normal[ax]=(sense==1?1.:-1);
Vector3r contPt=Vector3r::Zero();
Real contPtWeightSum=0.;
Real uN=Inf;
short minIx=-1;
for(int i: {
0,1,2
}) {
// negative uN0 means overlap
Real uNi=dist[i]*normal[ax]-radius;
// minimal distance vertex
if(uNi<uN) {
uN=uNi;
minIx=i;
}
// with no overlap, skip the vertex, it will not contribute to the contact point
if(uNi>=0) continue;
Vector3r c=fPos[i];
const Real& weight=uNi;
contPt+=c*weight;
contPtWeightSum+=weight;
}
// some vertices overlapping, use weighted average on those
if(contPtWeightSum!=0.) contPt/=contPtWeightSum;
// no overlapping vertices, use the closest one
else contPt=fPos[minIx];
contPt[ax]=wallPos[ax];
Vector3r fCenter=facet.getCentroid();
Vector3r fLinVel,fAngVel;
std::tie(fLinVel,fAngVel)=facet.interpolatePtLinAngVel(fCenter);
const DemData& wallDyn(wall.nodes[0]->getData<DemData>());
handleSpheresLikeContact(C,wallPos,wallDyn.vel,wallDyn.angVel,fCenter,fLinVel,fAngVel,normal,contPt,uN,/*r1*/-radius,radius);
return true;
}
