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