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; }