notInNotch(const Vector3r& _c, const Vector3r& _edge, const Vector3r& _normal, Real _aperture){ c=_c; edge=_edge; edge.normalize(); normal=_normal; normal-=edge*edge.dot(normal); normal.normalize(); inside=edge.cross(normal); aperture=_aperture; // LOG_DEBUG("edge="<<edge<<", normal="<<normal<<", inside="<<inside<<", aperture="<<aperture); }
void TorqueRecorder::action(){ totalTorque=0; Vector3r tmpAxis = rotationAxis.normalized(); FOREACH(Body::id_t id, ids){ if (!(scene->bodies->exists(id))) continue; Body* b=Body::byId(id,scene).get(); Vector3r tmpPos = b->state->pos; Vector3r radiusVector = tmpAxis.cross(tmpAxis.cross(tmpPos - zeroPoint)); totalTorque+=tmpAxis.dot(scene->forces.getTorque(id)+radiusVector.cross(scene->forces.getForce(id))); }; //Save data to a file out<<scene->iter<<" "<<totalTorque<<"\n"; out.close(); }
//! O/ bool Ig2_Sphere_GridConnection_ScGridCoGeom::go( const shared_ptr<Shape>& cm1, const shared_ptr<Shape>& cm2, const State& state1, const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& c) { // Useful variables : const State* sphereSt = YADE_CAST<const State*>(&state1); //const State* gridCoSt = YADE_CAST<const State*>(&state2); Sphere* sphere = YADE_CAST<Sphere*>(cm1.get()); GridConnection* gridCo = YADE_CAST<GridConnection*>(cm2.get()); //GridNode* gridNo1 = YADE_CAST<GridNode*>(gridCo->node1->shape.get()); //GridNode* gridNo2 = YADE_CAST<GridNode*>(gridCo->node2->shape.get()); State* gridNo1St = YADE_CAST<State*>(gridCo->node1->state.get()); State* gridNo2St = YADE_CAST<State*>(gridCo->node2->state.get()); bool isNew = !c->geom; shared_ptr<ScGridCoGeom> scm; if (!isNew) scm = YADE_PTR_CAST<ScGridCoGeom>(c->geom); Vector3r segt = gridCo->getSegment(); float len = gridCo->getLength(); Vector3r branch = sphereSt->pos - gridNo1St->pos; float relPos = branch.dot(segt)/(len*len); Vector3r fictiousPos=gridNo1St->pos+relPos*segt; Vector3r branchP = fictiousPos - sphereSt->pos; float dist = branchP.norm(); if(isNew){ if(dist > (sphere->radius + gridCo->radius)) return false; else {scm=shared_ptr<ScGridCoGeom>(new ScGridCoGeom()); c->geom=scm;} } if(dist <= (sphere->radius + gridCo->radius)){ scm->refR1=sphere->radius; //FIXME don't know why I have to do that ... scm->refR2=gridCo->radius; scm->id3=gridCo->node1->getId(); scm->id4=gridCo->node2->getId(); scm->relPos=relPos; Vector3r normal=branchP/dist; scm->penetrationDepth=sphere->radius+gridCo->radius-dist; scm->fictiousState.pos = gridNo1St->pos+segt*relPos; scm->fictiousState.vel = (1-relPos)*gridNo1St->vel + relPos*gridNo2St->vel; scm->fictiousState.angVel = ((1-relPos)*gridNo1St->angVel + relPos*gridNo2St->angVel).dot(segt/len)*segt/len //twist part : interpolated + segt.cross(gridNo2St->vel - gridNo1St->vel);// non-twist part : defined from nodes velocities scm->contactPoint = sphereSt->pos+normal*(sphere->radius-0.5*scm->penetrationDepth); scm->precompute(state1,scm->fictiousState,scene,c,normal,isNew,shift2,true);//use sphere-sphere precompute (with a virtual sphere) } return true; }
/* Law2_ScGeom_ViscElPhys_Basic */ void Law2_ScGeom_ViscElPhys_Basic::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){ const ScGeom& geom=*static_cast<ScGeom*>(_geom.get()); ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get()); const int id1 = I->getId1(); const int id2 = I->getId2(); if (geom.penetrationDepth<0) { if (phys.liqBridgeCreated and -geom.penetrationDepth<phys.sCrit and phys.Capillar) { phys.normalForce = -calculateCapillarForce(geom, phys)*geom.normal; if (I->isActive) { addForce (id1,-phys.normalForce,scene); addForce (id2, phys.normalForce,scene); }; return; } else { scene->interactions->requestErase(I); return; }; }; const BodyContainer& bodies = *scene->bodies; const State& de1 = *static_cast<State*>(bodies[id1]->state.get()); const State& de2 = *static_cast<State*>(bodies[id2]->state.get()); /* * This part for implementation of the capillar model. * All main equations are in calculateCapillarForce function. * There is only the determination of critical distance between spheres, * after that the liquid bridge will be broken. */ if (not(phys.liqBridgeCreated) and phys.Capillar) { phys.liqBridgeCreated = true; Sphere* s1=dynamic_cast<Sphere*>(bodies[id1]->shape.get()); Sphere* s2=dynamic_cast<Sphere*>(bodies[id2]->shape.get()); if (s1 and s2) { phys.R = 2 * s1->radius * s2->radius / (s1->radius + s2->radius); } else if (s1 and not(s2)) { phys.R = s1->radius; } else { phys.R = s2->radius; } const Real Vstar = phys.Vb/(phys.R*phys.R*phys.R); const Real Sstar = (1+0.5*phys.theta)*(pow(Vstar,1/3.0) + 0.1*pow(Vstar,2.0/3.0)); // [Willett2000], equation (15), use the full-length e.g 2*Sc phys.sCrit = Sstar*phys.R; } Vector3r& shearForce = phys.shearForce; if (I->isFresh(scene)) shearForce=Vector3r(0,0,0); const Real& dt = scene->dt; shearForce = geom.rotate(shearForce); // Handle periodicity. const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); const Vector3r c1x = (geom.contactPoint - de1.pos); const Vector3r c2x = (geom.contactPoint - de2.pos - shift2); const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel; const Real normalVelocity = geom.normal.dot(relativeVelocity); const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal; // As Chiara Modenese suggest, we store the elastic part // and then add the viscous part if we pass the Mohr-Coulomb criterion. // See http://www.mail-archive.com/[email protected]/msg01391.html shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity // Prevent appearing of attraction forces due to a viscous component // [Radjai2011], page 3, equation [1.7] // [Schwager2007] const Real normForceReal = phys.kn * geom.penetrationDepth + phys.cn * normalVelocity; if (normForceReal < 0) { phys.normalForce = Vector3r::Zero(); } else { phys.normalForce = normForceReal * geom.normal; } Vector3r momentResistance = Vector3r::Zero(); if (phys.mR>0.0) { const Vector3r relAngVel = de1.angVel - de2.angVel; relAngVel.normalized(); if (phys.mRtype == 1) { momentResistance = -phys.mR*phys.normalForce.norm()*relAngVel; // [Zhou1999536], equation (3) } else if (phys.mRtype == 2) { momentResistance = -phys.mR*(c1x.cross(de1.angVel) - c2x.cross(de2.angVel)).norm()*phys.normalForce.norm()*relAngVel; // [Zhou1999536], equation (4) } } const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2); if( shearForce.squaredNorm() > maxFs ) { // Then Mohr-Coulomb is violated (so, we slip), // we have the max value of the shear force, so // we consider only friction damping. const Real ratio = sqrt(maxFs) / shearForce.norm(); shearForce *= ratio; } else { // Then no slip occurs we consider friction damping + viscous damping. shearForceVisc = phys.cs*shearVelocity; } if (I->isActive) { const Vector3r f = phys.normalForce + shearForce + shearForceVisc; addForce (id1,-f,scene); addForce (id2, f,scene); addTorque(id1,-c1x.cross(f)+momentResistance,scene); addTorque(id2, c2x.cross(f)-momentResistance,scene); } }
bool computeForceTorqueViscEl(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I, Vector3r & force, Vector3r & torque1, Vector3r & torque2) { ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get()); const ScGeom& geom=*static_cast<ScGeom*>(_geom.get()); Scene* scene=Omega::instance().getScene().get(); #ifdef YADE_SPH //======================================================================================================= if (phys.SPHmode) { if (computeForceSPH(_geom, _phys, I, force)) { return true; } else { return false; } } //======================================================================================================= #endif const int id1 = I->getId1(); const int id2 = I->getId2(); if (geom.penetrationDepth<0) { return false; } else { const BodyContainer& bodies = *scene->bodies; const State& de1 = *static_cast<State*>(bodies[id1]->state.get()); const State& de2 = *static_cast<State*>(bodies[id2]->state.get()); Vector3r& shearForce = phys.shearForce; if (I->isFresh(scene)) shearForce=Vector3r(0,0,0); const Real& dt = scene->dt; shearForce = geom.rotate(shearForce); // Handle periodicity. const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); const Vector3r c1x = (geom.contactPoint - de1.pos); const Vector3r c2x = (geom.contactPoint - de2.pos - shift2); const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel; const Real normalVelocity = geom.normal.dot(relativeVelocity); const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal; // As Chiara Modenese suggest, we store the elastic part // and then add the viscous part if we pass the Mohr-Coulomb criterion. // See http://www.mail-archive.com/[email protected]/msg01391.html shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity // Prevent appearing of attraction forces due to a viscous component // [Radjai2011], page 3, equation [1.7] // [Schwager2007] phys.Fn = phys.kn * geom.penetrationDepth; phys.Fv = phys.cn * normalVelocity; const Real normForceReal = phys.Fn + phys.Fv; if (normForceReal < 0) { phys.normalForce = Vector3r::Zero(); } else { phys.normalForce = normForceReal * geom.normal; } Vector3r momentResistance = Vector3r::Zero(); if (phys.mR>0.0) { const Vector3r relAngVel = de1.angVel - de2.angVel; relAngVel.normalized(); if (phys.mRtype == 1) { momentResistance = -phys.mR*phys.normalForce.norm()*relAngVel; // [Zhou1999536], equation (3) } else if (phys.mRtype == 2) { momentResistance = -phys.mR*(c1x.cross(de1.angVel) - c2x.cross(de2.angVel)).norm()*phys.normalForce.norm()*relAngVel; // [Zhou1999536], equation (4) } } const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2); if( shearForce.squaredNorm() > maxFs ) { // Then Mohr-Coulomb is violated (so, we slip), // we have the max value of the shear force, so // we consider only friction damping. const Real ratio = sqrt(maxFs) / shearForce.norm(); shearForce *= ratio; } else { // Then no slip occurs we consider friction damping + viscous damping. shearForceVisc = phys.cs*shearVelocity; } force = phys.normalForce + shearForce + shearForceVisc; torque1 = -c1x.cross(force)+momentResistance; torque2 = c2x.cross(force)-momentResistance; return true; } }
/* Law2_ScGeom_ViscElPhys_Basic */ void Law2_ScGeom_ViscElPhys_Basic::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){ const ScGeom& geom=*static_cast<ScGeom*>(_geom.get()); ViscElPhys& phys=*static_cast<ViscElPhys*>(_phys.get()); const int id1 = I->getId1(); const int id2 = I->getId2(); if (geom.penetrationDepth<0) { scene->interactions->requestErase(I); return; } const BodyContainer& bodies = *scene->bodies; const State& de1 = *static_cast<State*>(bodies[id1]->state.get()); const State& de2 = *static_cast<State*>(bodies[id2]->state.get()); Vector3r& shearForce = phys.shearForce; if (I->isFresh(scene)) shearForce=Vector3r(0,0,0); const Real& dt = scene->dt; //Vector3r axis = phys.prevNormal.cross(geom.normal); //shearForce -= shearForce.cross(axis); //const Real angle = dt*0.5*geom.normal.dot(de1.angVel + de2.angVel); //axis = angle*geom.normal; //shearForce -= shearForce.cross(axis); shearForce = geom.rotate(shearForce); // Handle periodicity. const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); const Vector3r c1x = (geom.contactPoint - de1.pos); const Vector3r c2x = (geom.contactPoint - de2.pos - shift2); const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel; const Real normalVelocity = geom.normal.dot(relativeVelocity); const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal; // As Chiara Modenese suggest, we store the elastic part // and then add the viscous part if we pass the Mohr-Coulomb criterion. // See http://www.mail-archive.com/[email protected]/msg01391.html shearForce += phys.ks*dt*shearVelocity; // the elastic shear force have a history, but Vector3r shearForceVisc = Vector3r::Zero(); // the viscous shear damping haven't a history because it is a function of the instant velocity phys.normalForce = ( phys.kn * geom.penetrationDepth + phys.cn * normalVelocity ) * geom.normal; //phys.prevNormal = geom.normal; const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2); if( shearForce.squaredNorm() > maxFs ) { // Then Mohr-Coulomb is violated (so, we slip), // we have the max value of the shear force, so // we consider only friction damping. const Real ratio = sqrt(maxFs) / shearForce.norm(); shearForce *= ratio; } else { // Then no slip occurs we consider friction damping + viscous damping. shearForceVisc = phys.cs*shearVelocity; } const Vector3r f = phys.normalForce + shearForce + shearForceVisc; addForce (id1,-f,scene); addForce (id2, f,scene); addTorque(id1,-c1x.cross(f),scene); addTorque(id2, c2x.cross(f),scene); }
//! O/ bool Ig2_Sphere_GridConnection_ScGridCoGeom::go( const shared_ptr<Shape>& cm1, const shared_ptr<Shape>& cm2, const State& state1, const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& c) { // Useful variables : const State* sphereSt = YADE_CAST<const State*>(&state1); //const State* gridCoSt = YADE_CAST<const State*>(&state2); Sphere* sphere = YADE_CAST<Sphere*>(cm1.get()); GridConnection* gridCo = YADE_CAST<GridConnection*>(cm2.get()); GridNode* gridNo1 = YADE_CAST<GridNode*>(gridCo->node1->shape.get()); GridNode* gridNo2 = YADE_CAST<GridNode*>(gridCo->node2->shape.get()); State* gridNo1St = YADE_CAST<State*>(gridCo->node1->state.get()); State* gridNo2St = YADE_CAST<State*>(gridCo->node2->state.get()); bool isNew = !c->geom; shared_ptr<ScGridCoGeom> scm; if (!isNew) scm = YADE_PTR_CAST<ScGridCoGeom>(c->geom); else {scm = shared_ptr<ScGridCoGeom>(new ScGridCoGeom());} Vector3r segt = gridCo->getSegment(); Real len = gridCo->getLength(); Vector3r spherePos = sphereSt->pos - shift2; Vector3r branch = spherePos - gridNo1St->pos; Vector3r branchN = spherePos - gridNo2St->pos; for(int i=0;i<3;i++){ if(abs(branch[i])<1e-14) branch[i]=0.0; if(abs(branchN[i])<1e-14) branchN[i]=0.0; } Real relPos = branch.dot(segt)/(len*len); if(scm->isDuplicate==2 && scm->trueInt!=c->id2)return true; //the contact will be deleted into the Law. scm->isDuplicate=0; scm->trueInt=-1; if(relPos<=0){ // if the sphere projection is BEFORE the segment ... if(gridNo1->ConnList.size()>1){// if the node is not an extremity of the Grid (only one connection) for(int unsigned i=0;i<gridNo1->ConnList.size();i++){ // ... loop on all the Connections of the same Node ... GridConnection* GC = (GridConnection*)gridNo1->ConnList[i]->shape.get(); if(GC==gridCo)continue;// self comparison. Vector3r segtCandidate1 = GC->node1->state->pos - gridNo1St->pos; // (be sure of the direction of segtPrev to compare relPosPrev.) Vector3r segtCandidate2 = GC->node2->state->pos - gridNo1St->pos; Vector3r segtPrev = segtCandidate1.norm()>segtCandidate2.norm() ? segtCandidate1:segtCandidate2; for(int j=0;j<3;j++){ if(abs(segtPrev[j])<1e-14) segtPrev[j]=0.0; } Real relPosPrev = (branch.dot(segtPrev))/(segtPrev.norm()*segtPrev.norm()); // ... and check whether the sphere projection is before the neighbours connections too. const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo1->ConnList[i]->getId()); if(relPosPrev<=0){ //if the sphere projection is outside both the current Connection AND this neighbouring connection, then create the interaction if the neighbour did not already do it before. if(intr && intr->isReal() && isNew) return false; if(intr && intr->isReal() && !isNew) {scm->isDuplicate=1;/*cout<<"Declare "<<c->id1<<"-"<<c->id2<<" as duplicated."<<endl;*/} } else{//the sphere projection is outside the current Connection but inside the previous neighbour. The contact has to be handled by the Prev GridConnection, not here. if (isNew)return false; else { //cout<<"The contact "<<c->id1<<"-"<<c->id2<<" HAVE to be copied and deleted NOW."<<endl ; scm->isDuplicate=1; scm->trueInt=-1; return true;} } } } } //Exactly the same but in the case the sphere projection is AFTER the segment. else if(relPos>=1){ if(gridNo2->ConnList.size()>1){ for(int unsigned i=0;i<gridNo2->ConnList.size();i++){ GridConnection* GC = (GridConnection*)gridNo2->ConnList[i]->shape.get(); if(GC==gridCo)continue;// self comparison. Vector3r segtCandidate1 = GC->node1->state->pos - gridNo2St->pos; Vector3r segtCandidate2 = GC->node2->state->pos - gridNo2St->pos; Vector3r segtNext = segtCandidate1.norm()>segtCandidate2.norm() ? segtCandidate1:segtCandidate2; for(int j=0;j<3;j++){ if(abs(segtNext[j])<1e-14) segtNext[j]=0.0; } Real relPosNext = (branchN.dot(segtNext))/(segtNext.norm()*segtNext.norm()); const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo2->ConnList[i]->getId()); if(relPosNext<=0){ //if the sphere projection is outside both the current Connection AND this neighbouring connection, then create the interaction if the neighbour did not already do it before. if(intr && intr->isReal() && isNew) return false; if(intr && intr->isReal() && !isNew) {scm->isDuplicate=1;/*cout<<"Declare "<<c->id1<<"-"<<c->id2<<" as duplicated."<<endl;*/} } else{//the sphere projection is outside the current Connection but inside the previous neighbour. The contact has to be handled by the Prev GridConnection, not here. if (isNew)return false; else {//cout<<"The contact "<<c->id1<<"-"<<c->id2<<" HAVE to be copied and deleted NOW."<<endl ; scm->isDuplicate=1 ; scm->trueInt=-1 ; return true;} } } } } else if (isNew && relPos<=0.5){ if(gridNo1->ConnList.size()>1){// if the node is not an extremity of the Grid (only one connection) for(int unsigned i=0;i<gridNo1->ConnList.size();i++){ // ... loop on all the Connections of the same Node ... GridConnection* GC = (GridConnection*)gridNo1->ConnList[i]->shape.get(); if(GC==gridCo)continue;// self comparison. Vector3r segtCandidate1 = GC->node1->state->pos - gridNo1St->pos; // (be sure of the direction of segtPrev to compare relPosPrev.) Vector3r segtCandidate2 = GC->node2->state->pos - gridNo1St->pos; Vector3r segtPrev = segtCandidate1.norm()>segtCandidate2.norm() ? segtCandidate1:segtCandidate2; for(int j=0;j<3;j++){ if(abs(segtPrev[j])<1e-14) segtPrev[j]=0.0; } Real relPosPrev = (branch.dot(segtPrev))/(segtPrev.norm()*segtPrev.norm()); if(relPosPrev<=0){ //the sphere projection is inside the current Connection and outide this neighbour connection. const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo1->ConnList[i]->getId()); if( intr && intr->isReal() ){// if an ineraction exist between the sphere and the previous connection, import parameters. //cout<<"Copying contact geom and phys from "<<intr->id1<<"-"<<intr->id2<<" to here ("<<c->id1<<"-"<<c->id2<<")"<<endl; scm=YADE_PTR_CAST<ScGridCoGeom>(intr->geom); c->geom=scm; c->phys=intr->phys; scm->trueInt=c->id2; scm->isDuplicate=2; //command the old contact deletion. isNew=0; break; } } } } } else if (isNew && relPos>0.5){ if(gridNo2->ConnList.size()>1){ for(int unsigned i=0;i<gridNo2->ConnList.size();i++){ GridConnection* GC = (GridConnection*)gridNo2->ConnList[i]->shape.get(); if(GC==gridCo)continue;// self comparison. Vector3r segtCandidate1 = GC->node1->state->pos - gridNo2St->pos; Vector3r segtCandidate2 = GC->node2->state->pos - gridNo2St->pos; Vector3r segtNext = segtCandidate1.norm()>segtCandidate2.norm() ? segtCandidate1:segtCandidate2; for(int j=0;j<3;j++){ if(abs(segtNext[j])<1e-14) segtNext[j]=0.0; } Real relPosNext = (branchN.dot(segtNext))/(segtNext.norm()*segtNext.norm()); if(relPosNext<=0){ //the sphere projection is inside the current Connection and outide this neighbour connection. const shared_ptr<Interaction> intr = scene->interactions->find(c->id1,gridNo2->ConnList[i]->getId()); if( intr && intr->isReal() ){// if an ineraction exist between the sphere and the previous connection, import parameters. //cout<<"Copying contact geom and phys from "<<intr->id1<<"-"<<intr->id2<<" to here ("<<c->id1<<"-"<<c->id2<<")"<<endl; scm=YADE_PTR_CAST<ScGridCoGeom>(intr->geom); c->geom=scm; c->phys=intr->phys; scm->trueInt=c->id2; scm->isDuplicate=2; //command the old contact deletion. isNew=0; break; } } } } } relPos=relPos<0?0:relPos; //min value of relPos : 0 relPos=relPos>1?1:relPos; //max value of relPos : 1 Vector3r fictiousPos=gridNo1St->pos+relPos*segt; Vector3r branchF = fictiousPos - spherePos; Real dist = branchF.norm(); if(isNew && (dist > (sphere->radius + gridCo->radius))) return false; // Create the geometry : if(isNew) c->geom=scm; scm->radius1=sphere->radius; scm->radius2=gridCo->radius; scm->id3=gridCo->node1->getId(); scm->id4=gridCo->node2->getId(); scm->relPos=relPos; Vector3r normal=branchF/dist; scm->penetrationDepth = sphere->radius+gridCo->radius-dist; scm->fictiousState.pos = fictiousPos; scm->contactPoint = spherePos + normal*(scm->radius1 - 0.5*scm->penetrationDepth); scm->fictiousState.vel = (1-relPos)*gridNo1St->vel + relPos*gridNo2St->vel; scm->fictiousState.angVel = ((1-relPos)*gridNo1St->angVel + relPos*gridNo2St->angVel).dot(segt/len)*segt/len //twist part : interpolated + segt.cross(gridNo2St->vel - gridNo1St->vel);// non-twist part : defined from nodes velocities scm->precompute(state1,scm->fictiousState,scene,c,normal,isNew,shift2,true);//use sphere-sphere precompute (with a virtual sphere) return true; }
/*! Constitutive law */ void FCPM::go(shared_ptr<IGeom>& _geom, shared_ptr<IPhys>& _phys, Interaction* I){ const ScGeom& geom=*static_cast<ScGeom*>(_geom.get()); FreshConcretePhys& phys=*static_cast<FreshConcretePhys*>(_phys.get()); const int id1 = I->getId1(); const int id2 = I->getId2(); const BodyContainer& bodies = *scene->bodies; const State& de1 = *static_cast<State*>(bodies[id1]->state.get()); const State& de2 = *static_cast<State*>(bodies[id2]->state.get()); //Calculation of the max penetretion and the radius of the overlap area Sphere* s1=dynamic_cast<Sphere*>(bodies[id1]->shape.get()); Sphere* s2=dynamic_cast<Sphere*>(bodies[id2]->shape.get()); Real dist; Real contactRadius; Real OverlapRadius; if (s1 and s2) { phys.maxPenetration=s1->radius * phys.Penetration1 + s2->radius * phys.Penetration2; dist = s1->radius + s2->radius - geom.penetrationDepth; OverlapRadius = pow(((4 * pow(dist,2) * pow(s1->radius,2) - pow((pow(dist,2) - pow(s2->radius,2) + pow(s1->radius,2)),2)) / (4 * pow(dist,2))),(1.0/2.0)); //contactRadius = (pow(s1->radius,2) + pow(s2->radius,2))/(s1->radius + s2->radius); contactRadius = s1->radius; } else if (s1 and not(s2)) { phys.maxPenetration=s1->radius * phys.Penetration1; dist = s1->radius - geom.penetrationDepth; OverlapRadius =pow((pow(s1->radius,2) - pow(dist,2)),(1.0/2.0)); contactRadius = s1->radius; } else { phys.maxPenetration=s2->radius * phys.Penetration2; dist = s2->radius - geom.penetrationDepth; OverlapRadius = pow((pow(s2->radius,2) - pow(dist,2)),(1.0/2.0)); contactRadius = s2->radius; } Vector3r& shearForce = phys.shearForce; if (I->isFresh(scene)) shearForce=Vector3r(0,0,0); const Real& dt = scene->dt; shearForce = geom.rotate(shearForce); // Handle periodicity. const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(I->cellDist): Vector3r::Zero(); const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(I->cellDist): Vector3r::Zero(); const Vector3r c1x = (geom.contactPoint - de1.pos); const Vector3r c2x = (geom.contactPoint - de2.pos - shift2); const Vector3r relativeVelocity = (de1.vel+de1.angVel.cross(c1x)) - (de2.vel+de2.angVel.cross(c2x)) + shiftVel; const Real normalVelocity = geom.normal.dot(relativeVelocity); const Vector3r shearVelocity = relativeVelocity-normalVelocity*geom.normal; //Normal Force Real OverlapArea; Real MaxArea; OverlapArea = 3.1415 * pow(OverlapRadius,2); MaxArea = 3.1415 * pow(contactRadius,2); Real Mult; if(OverlapRadius < contactRadius){ Mult = OverlapArea / MaxArea; } else{ Mult = 1; } Real Fn; if(geom.penetrationDepth>=0){ //Compression if(normalVelocity>=0){ if (geom.penetrationDepth >= phys.maxPenetration){ Fn = phys.knI * (geom.penetrationDepth - phys.previousun) + phys.previousElasticN + phys.cnI * normalVelocity; phys.previousElasticN += phys.knI * (geom.penetrationDepth - phys.previousun); phys.normalForce = Fn * geom.normal; phys.t = 0; phys.t2 = 0; //phys.previousElasticTensionN = 0; } else{ Fn = Mult * phys.kn * (geom.penetrationDepth - phys.previousun) + Mult * phys.previousElasticN + phys.cn * normalVelocity; phys.previousElasticN += Mult * phys.kn * (geom.penetrationDepth - phys.previousun); phys.finalElasticN += Mult * phys.kn * (geom.penetrationDepth - phys.previousun); phys.normalForce = Fn * geom.normal; phys.t = 0; phys.t2 = 0; //phys.previousElasticTensionN = 0; } } //Tension else if(normalVelocity<0){ if(phys.t == 0){ phys.maxNormalComp = - phys.finalElasticN; //printf("------> %E \n", phys.maxNormalComp); phys.RupOverlap = phys.previousun; //throw runtime_error("STOP"); phys.t = 1; } Real MaxTension = phys.maxNormalComp * 0.25; Real FnTension = phys.previousElasticTensionN + Mult * phys.kn * dt * normalVelocity; //printf("===:: %E \n", MaxTension); //printf("===:: %E \n", FnTension); if (FnTension > MaxTension && phys.t2 == 0){ Fn = phys.previousElasticTensionN + Mult * phys.kn * dt * normalVelocity; phys.previousElasticTensionN = Fn; phys.normalForce = Fn * geom.normal; phys.RupOverlap = geom.penetrationDepth; //phys.previousElasticN = 0; //printf("===-- %E \n", Fn); } else{ //phys.DamageTension = geom.penetrationDepth / (phys.maxPenetration - phys.maxPenetration * phys.RupTension); //phys.previousElasticTensionN -= phys.DamageTension * Mult * phys.kn * dt * normalVelocity; phys.t2 = 1; Fn = MaxTension * geom.penetrationDepth / phys.RupOverlap; phys.normalForce = Fn * geom.normal; //printf("===// %E \n", Fn); //throw runtime_error("STOP"); } } } else{ Fn = 0; phys.normalForce = Fn * geom.normal; phys.finalElasticN = 0; } phys.previousun = geom.penetrationDepth; //Shear Force shearForce += phys.ks*dt*shearVelocity; Vector3r shearForceVisc = Vector3r::Zero(); const Real maxFs = phys.normalForce.squaredNorm() * std::pow(phys.tangensOfFrictionAngle,2); if( shearForce.squaredNorm() > maxFs ){ // Then Mohr-Coulomb is violated (so, we slip), // we have the max value of the shear force, so // we consider only friction damping. const Real ratio = sqrt(maxFs) / shearForce.norm(); shearForce *= ratio; } else{ // Then no slip occurs we consider friction damping + viscous damping. shearForceVisc = phys.cs*shearVelocity; } if (I->isActive) { const Vector3r f = phys.normalForce + shearForce + shearForceVisc; addForce (id1,-f,scene); addForce (id2, f,scene); addTorque(id1,-c1x.cross(f),scene); addTorque(id2, c2x.cross(f),scene); } }
/* Generic function to compute L3Geom (with colinear points), used for {sphere,facet,wall}+sphere contacts now */ void Ig2_Sphere_Sphere_L3Geom::handleSpheresLikeContact(const shared_ptr<Interaction>& I, const State& state1, const State& state2, const Vector3r& shift2, bool is6Dof, const Vector3r& normal, const Vector3r& contPt, Real uN, Real r1, Real r2){ // create geometry if(!I->geom){ if(is6Dof) I->geom=shared_ptr<L6Geom>(new L6Geom); else I->geom=shared_ptr<L3Geom>(new L3Geom); L3Geom& g(I->geom->cast<L3Geom>()); g.contactPoint=contPt; g.refR1=r1; g.refR2=r2; g.normal=normal; // g.trsf.setFromTwoVectors(Vector3r::UnitX(),g.normal); // quaternion just from the X-axis; does not seem to work for some reason?! const Vector3r& locX(g.normal); // initial local y-axis orientation, in the xz or xy plane, depending on which component is larger to avoid singularities Vector3r locY=normal.cross(abs(normal[1])<abs(normal[2])?Vector3r::UnitY():Vector3r::UnitZ()); locY-=locX*locY.dot(locX); locY.normalize(); Vector3r locZ=normal.cross(locY); #ifdef L3_TRSF_QUATERNION Matrix3r trsf; trsf.row(0)=locX; trsf.row(1)=locY; trsf.row(2)=locZ; g.trsf=Quaternionr(trsf); // from transformation matrix #else g.trsf.row(0)=locX; g.trsf.row(1)=locY; g.trsf.row(2)=locZ; #endif g.u=Vector3r(uN,0,0); // zero shear displacement if(distFactor<0) g.u0[0]=uN; // L6Geom::phi is initialized to Vector3r::Zero() automatically //cerr<<"Init trsf=\n"<<g.trsf<<endl<<"locX="<<locX<<", locY="<<locY<<", locZ="<<locZ<<endl; return; } // update geometry /* motion of the conctact consists in rigid motion (normRotVec, normTwistVec) and mutual motion (relShearDu); they are used to update trsf and u */ L3Geom& g(I->geom->cast<L3Geom>()); const Vector3r& currNormal(normal); const Vector3r& prevNormal(g.normal); // normal rotation vector, between last steps Vector3r normRotVec=prevNormal.cross(currNormal); // contrary to what ScGeom::precompute does now (r2486), we take average normal, i.e. .5*(prevNormal+currNormal), // so that all terms in the equation are in the previous mid-step // the re-normalization might not be necessary for very small increments, but better do it Vector3r avgNormal=(approxMask&APPROX_NO_MID_NORMAL) ? prevNormal : .5*(prevNormal+currNormal); if(!(approxMask&APPROX_NO_RENORM_MID_NORMAL) && !(approxMask&APPROX_NO_MID_NORMAL)) avgNormal.normalize(); // normalize only if used and if requested via approxMask // twist vector of the normal from the last step Vector3r normTwistVec=avgNormal*scene->dt*.5*avgNormal.dot(state1.angVel+state2.angVel); // compute relative velocity // noRatch: take radius or current distance as the branch vector; see discussion in ScGeom::precompute (avoidGranularRatcheting) Vector3r c1x=((noRatch && !r1>0) ? ( r1*normal).eval() : (contPt-state1.pos).eval()); // used only for sphere-sphere Vector3r c2x=(noRatch ? (-r2*normal).eval() : (contPt-state2.pos+shift2).eval()); //Vector3r state2velCorrected=state2.vel+(scene->isPeriodic?scene->cell->intrShiftVel(I->cellDist):Vector3r::Zero()); // velocity of the second particle, corrected with meanfield velocity if necessary //cerr<<"correction "<<(scene->isPeriodic?scene->cell->intrShiftVel(I->cellDist):Vector3r::Zero())<<endl; Vector3r relShearVel=(state2.vel+state2.angVel.cross(c2x))-(state1.vel+state1.angVel.cross(c1x)); // account for relative velocity of particles in different cell periods if(scene->isPeriodic) relShearVel+=scene->cell->intrShiftVel(I->cellDist); relShearVel-=avgNormal.dot(relShearVel)*avgNormal; Vector3r relShearDu=relShearVel*scene->dt; /* Update of quantities in global coords consists in adding 3 increments we have computed; in global coords (a is any vector) 1. +relShearVel*scene->dt; // mutual motion of the contact 2. -a.cross(normRotVec); // rigid rotation perpendicular to the normal 3. -a.cross(normTwistVec); // rigid rotation parallel to the normal */ // compute current transformation, by updating previous axes // the X axis can be prescribed directly (copy of normal) // the mutual motion on the contact does not change transformation #ifdef L3_TRSF_QUATERNION const Matrix3r prevTrsf(g.trsf.toRotationMatrix()); Quaternionr prevTrsfQ(g.trsf); #else const Matrix3r prevTrsf(g.trsf); // could be reference perhaps, but we need it to compute midTrsf (if applicable) #endif Matrix3r currTrsf; currTrsf.row(0)=currNormal; for(int i=1; i<3; i++){ currTrsf.row(i)=prevTrsf.row(i)-prevTrsf.row(i).cross(normRotVec)-prevTrsf.row(i).cross(normTwistVec); } #ifdef L3_TRSF_QUATERNION Quaternionr currTrsfQ(currTrsf); if((scene->iter % trsfRenorm)==0 && trsfRenorm>0) currTrsfQ.normalize(); #else if((scene->iter % trsfRenorm)==0 && trsfRenorm>0){ #if 1 currTrsf.row(0).normalize(); currTrsf.row(1)-=currTrsf.row(0)*currTrsf.row(1).dot(currTrsf.row(0)); // take away y projected on x, to stabilize numerically currTrsf.row(1).normalize(); currTrsf.row(2)=currTrsf.row(0).cross(currTrsf.row(1)); currTrsf.row(2).normalize(); #else currTrsf=Matrix3r(Quaternionr(currTrsf).normalized()); #endif #ifdef YADE_DEBUG if(abs(currTrsf.determinant()-1)>.05){ LOG_ERROR("##"<<I->getId1()<<"+"<<I->getId2()<<", |trsf|="<<currTrsf.determinant()); g.trsf=currTrsf; throw runtime_error("Transformation matrix far from orthonormal."); } #endif } #endif /* Previous local trsf u'⁻ must be updated to current u'⁰. We have transformation T⁻ and T⁰, δ(a) denotes increment of a as defined above. Two possibilities: 1. convert to global, update, convert back: T⁰(T⁻*(u'⁻)+δ(T⁻*(u'⁻))). Quite heavy. 2. update u'⁻ straight, using relShearVel in local coords; since relShearVel is computed at (t-Δt/2), we would have to find intermediary transformation (same axis, half angle; the same as slerp at t=.5 between the two). This could be perhaps simplified by using T⁰ or T⁻ since they will not differ much, but it would have to be verified somehow. */ // if requested via approxMask, just use prevTrsf #ifdef L3_TRSF_QUATERNION Quaternionr midTrsf=(approxMask&APPROX_NO_MID_TRSF) ? prevTrsfQ : prevTrsfQ.slerp(.5,currTrsfQ); #else Quaternionr midTrsf=(approxMask&APPROX_NO_MID_TRSF) ? Quaternionr(prevTrsf) : Quaternionr(prevTrsf).slerp(.5,Quaternionr(currTrsf)); #endif //cerr<<"prevTrsf=\n"<<prevTrsf<<", currTrsf=\n"<<currTrsf<<", midTrsf=\n"<<Matrix3r(midTrsf)<<endl; // updates of geom here // midTrsf*relShearVel should have the 0-th component (approximately) zero -- to be checked g.u+=midTrsf*relShearDu; //cerr<<"midTrsf=\n"<<midTrsf<<",relShearDu="<<relShearDu<<", transformed "<<midTrsf*relShearDu<<endl; g.u[0]=uN; // this does not have to be computed incrementally #ifdef L3_TRSF_QUATERNION g.trsf=currTrsfQ; #else g.trsf=currTrsf; #endif // GenericSpheresContact g.refR1=r1; g.refR2=r2; g.normal=currNormal; g.contactPoint=contPt; if(is6Dof){ // update phi, from the difference of angular velocities // the difference is transformed to local coord using the midTrsf transformation // perhaps not consistent when spheres have different radii (depends how bending moment is computed) I->geom->cast<L6Geom>().phi+=midTrsf*(scene->dt*(state2.angVel-state1.angVel)); } };