bool DihedralConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2, const unsigned int particle3, const unsigned int particle4) { m_bodies[0] = particle1; m_bodies[1] = particle2; m_bodies[2] = particle3; m_bodies[3] = particle4; ParticleData &pd = model.getParticles(); const Vector3r &p0 = pd.getPosition0(particle1); const Vector3r &p1 = pd.getPosition0(particle2); const Vector3r &p2 = pd.getPosition0(particle3); const Vector3r &p3 = pd.getPosition0(particle4); Vector3r e = p3 - p2; Real elen = e.norm(); if (elen < 1e-6) return false; Real invElen = 1.0 / elen; Vector3r n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.squaredNorm(); Vector3r n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.squaredNorm(); n1.normalize(); n2.normalize(); Real dot = n1.dot(n2); if (dot < -1.0) dot = -1.0; if (dot > 1.0) dot = 1.0; m_restAngle = acos(dot); return true; }
// function used to check if the newly broken bond belongs in a cluster or not, if so attach to proper cluster and set appropriate flags void Law2_ScGeom_JCFpmPhys_JointedCohesiveFrictionalPM::checkForCluster(JCFpmPhys* phys, ScGeom* geom, Body* b1, Body* b2, Interaction* contact){ const shared_ptr<Shape>& sphere1 = b1->shape; const shared_ptr<Shape>& sphere2 = b2->shape; const Real sphereRadius1 = static_cast<Sphere*>(sphere1.get())->radius; const Real sphereRadius2 = static_cast<Sphere*>(sphere2.get())->radius; const Real avgDiameter = (sphereRadius1+sphereRadius2); Vector3r& brokenInteractionLocation = geom->contactPoint; phys->nearbyFound=0; FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){ //#endif JCFpmPhys* nearbyPhys; const ScGeom* nearbyGeom; if (!I || !I->geom.get() || !I->phys.get()) continue; if (I && I->isReal() && JCFpmPhys::getClassIndexStatic()==I->phys->getClassIndex()){ nearbyPhys = YADE_CAST<JCFpmPhys*>(I->phys.get()); if (!nearbyPhys) continue; if (I->geom.get() /*&& !nearbyPhys->momentBroken*/){ nearbyGeom = YADE_CAST<ScGeom*> (I->geom.get()); if (!nearbyGeom) continue; Vector3r nearbyInteractionLocation = nearbyGeom->contactPoint; Vector3r proximityVector = nearbyInteractionLocation-brokenInteractionLocation; Real proximity = proximityVector.norm(); // this logic is finding interactions within a radius of the broken one, and identifiying if it is an original event or if it belongs in a cluster if (proximity < avgDiameter*momentRadiusFactor && proximity != 0){ if (nearbyPhys->originalClusterEvent && !nearbyPhys->momentCalculated && !phys->clusteredEvent && clusterMoments) { phys->eventNumber = nearbyPhys->eventNumber; phys->clusteredEvent = true; phys->originalEvent = I.get(); } else if (nearbyPhys->clusteredEvent && !phys->clusteredEvent && clusterMoments){ JCFpmPhys* originalPhys = YADE_CAST<JCFpmPhys*>(nearbyPhys->originalEvent->phys.get()); if (!originalPhys->momentCalculated){ phys->eventNumber = nearbyPhys->eventNumber; phys->clusteredEvent = true; phys->originalEvent = nearbyPhys->originalEvent; } } if (nearbyPhys->momentBroken) continue; phys->nearbyInts.push_back(I.get()); } } } } if (!phys->clusteredEvent) { phys->originalClusterEvent = true; // if not clustered, its an original event. We use this interaction as the master for the cluster. Its list of nearbyInts will expand if other ints break nearby. phys->eventBeginTime=scene->time; phys->originalEvent = contact; eventNumber += 1; phys->eventNumber = eventNumber; } phys->checkedForCluster = true; }
Vector3r SafetyEval::getDestination(const Vector3r& cur_pos, const Vector3r& velocity) const { //breaking distance at this velocity float velocity_mag = velocity.norm(); float dest_pos_dist = std::max(velocity_mag * vehicle_params_.vel_to_breaking_dist, vehicle_params_.min_breaking_dist); //calculate dest_pos cur_pos we will be if we had to break suddenly return velocity_mag >= vehicle_params_.distance_accuracy ? (cur_pos + (velocity / velocity_mag) * dest_pos_dist) : cur_pos; }
void SafetyEval::isSafeDestination(const Vector3r& dest_pos, const Vector3r& cur_pos, const Quaternionr& quaternion, SafetyEval::EvalResult& result) { //this function should work even when dest_pos == cur_pos result.dest_pos = dest_pos; result.cur_pos = cur_pos; //is this dest_pos cur_pos within the fence? checkFence(dest_pos, cur_pos, result); if (!(enable_reasons_ & SafetyViolationType_::Obstacle)) return; //transform dest_pos vector to body frame const Vector3r cur_dest = dest_pos - cur_pos; float cur_dest_norm = cur_dest.norm(); //check for approx zero vectors to avoid random yaw angles if (cur_dest_norm < vehicle_params_.distance_accuracy) { //we are hovering result.dest_risk_dist = Utils::nan<float>(); isCurrentSafer(result); } else { //see if we have obstacle in direction result.cur_dest_body = VectorMath::transformToBodyFrame(cur_dest, quaternion, true); //get yaw in body frame, ie, front is always 0 radians float point_angle = std::atan2(result.cur_dest_body[1], result.cur_dest_body[0]); //yaw to ticks int point_tick = obs_xy_ptr_->angleToTick(point_angle); //get obstacles in the window at the tick direction around the window result.dest_obs = obs_xy_ptr_->hasObstacle(point_tick - vehicle_params_.obs_window, point_tick + vehicle_params_.obs_window); //less risk distance is better result.dest_risk_dist = cur_dest_norm + adjustClearanceForPrStl(vehicle_params_.obs_clearance, result.dest_obs.confidence) - result.dest_obs.distance; if (result.dest_risk_dist >= 0) { //potential collision //check obstacles around current position and see if it has lower risk isCurrentSafer(result); } //else obstacle is too far } //if we detected unsafe condition due to obstacle, find direction to move away to if (!result.is_safe && result.reason & SafetyViolationType_::Obstacle) { //look for each surrounding tick to see if we have obstacle free angle setSuggestedVelocity(result, quaternion); Utils::log(Utils::stringf("isSafeDestination: cur_dest_norm=%f, result=%s, quaternion=%s", cur_dest_norm, result.toString().c_str(), VectorMath::toString(quaternion, true).c_str())); } //else no suggestions required }
//! 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; }
void Gl1_PolyhedraPhys::go(const shared_ptr<IPhys>& ip, const shared_ptr<Interaction>& i, const shared_ptr<Body>& b1, const shared_ptr<Body>& b2, bool wireFrame){ if(!gluQuadric){ gluQuadric=gluNewQuadric(); if(!gluQuadric) throw runtime_error("Gl1_PolyhedraPhys::go unable to allocate new GLUquadric object (out of memory?)."); } PolyhedraPhys* np=static_cast<PolyhedraPhys*>(ip.get()); shared_ptr<IGeom> ig(i->geom); if(!ig) return; // changed meanwhile? PolyhedraGeom* geom=YADE_CAST<PolyhedraGeom*>(ig.get()); Real fnNorm=np->normalForce.dot(geom->normal); if((signFilter>0 && fnNorm<0) || (signFilter<0 && fnNorm>0)) return; int fnSign=fnNorm>0?1:-1; fnNorm=std::abs(fnNorm); Real radiusScale=1.; maxFn=max(fnNorm,maxFn); Real realMaxRadius; if(maxRadius<0){ refRadius=min(0.03,refRadius); realMaxRadius=refRadius; } else realMaxRadius=maxRadius; Real radius=radiusScale*realMaxRadius*(fnNorm/maxFn); if (radius<=0.) radius = 1E-8; Vector3r color=Shop::scalarOnColorScale(fnNorm*fnSign,-maxFn,maxFn); Vector3r p1=b1->state->pos, p2=b2->state->pos; Vector3r relPos; relPos=p2-p1; Real dist=relPos.norm(); glDisable(GL_CULL_FACE); glPushMatrix(); glTranslatef(p1[0],p1[1],p1[2]); Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),relPos/dist /* normalized */)); // using Transform with OpenGL: http://eigen.tuxfamily.org/dox/TutorialGeometry.html //glMultMatrixd(Eigen::Affine3d(q).data()); glMultMatrix(Eigen::Transform<Real,3,Eigen::Affine>(q).data()); glColor3v(color); gluCylinder(gluQuadric,radius,radius,dist,slices,stacks); glPopMatrix(); }
bool Law2_ScGeom_MindlinPhys_Mindlin::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact){ const Real& dt = scene->dt; // get time step Body::id_t id1 = contact->getId1(); // get id body 1 Body::id_t id2 = contact->getId2(); // get id body 2 State* de1 = Body::byId(id1,scene)->state.get(); State* de2 = Body::byId(id2,scene)->state.get(); ScGeom* scg = static_cast<ScGeom*>(ig.get()); MindlinPhys* phys = static_cast<MindlinPhys*>(ip.get()); const shared_ptr<Body>& b1=Body::byId(id1,scene); const shared_ptr<Body>& b2=Body::byId(id2,scene); bool useDamping=(phys->betan!=0. || phys->betas!=0. || phys->alpha!=0.); bool LinDamp=true; if (phys->alpha!=0.) {LinDamp=false;} // use non linear damping // tangential and normal stiffness coefficients, recomputed from betan,betas at every step Real cn=0, cs=0; /****************/ /* NORMAL FORCE */ /****************/ Real uN = scg->penetrationDepth; // get overlapping if (uN<0) { if (neverErase) {phys->shearForce = phys->normalForce = Vector3r::Zero(); phys->kn=phys->ks=0; return true;} else return false; } /* Hertz-Mindlin's formulation (PFC) Note that the normal stiffness here is a secant value (so as it is cannot be used in the GSTS) In the first place we get the normal force and then we store kn to be passed to the GSTS */ Real Fn = phys->kno*std::pow(uN,1.5); // normal Force (scalar) if (includeAdhesion) { Fn -= phys->adhesionForce; // include adhesion force to account for the effect of Van der Waals interactions phys->isAdhesive = (Fn<0); // set true the bool to count the number of adhesive contacts } phys->normalForce = Fn*scg->normal; // normal Force (vector) if (calcEnergy){ Real R=scg->radius1*scg->radius2/(scg->radius1+scg->radius2); phys->radius=pow((Fn+(includeAdhesion?phys->adhesionForce:0.))*pow(R,3/2.)/phys->kno,1/3.); // attribute not used anywhere, we do not need it } /*******************************/ /* TANGENTIAL NORMAL STIFFNESS */ /*******************************/ phys->kn = 3./2.*phys->kno*std::pow(uN,0.5); // here we store the value of kn to compute the time step /******************************/ /* TANGENTIAL SHEAR STIFFNESS */ /******************************/ phys->ks = phys->kso*std::pow(uN,0.5); // get tangential stiffness (this is a tangent value, so we can pass it to the GSTS) /************************/ /* DAMPING COEFFICIENTS */ /************************/ // Inclusion of local damping if requested // viscous damping is defined for both linear and non-linear elastic case if (useDamping && LinDamp){ Real mbar = (!b1->isDynamic() && b2->isDynamic()) ? de2->mass : ((!b2->isDynamic() && b1->isDynamic()) ? de1->mass : (de1->mass*de2->mass / (de1->mass + de2->mass))); // get equivalent mass if both bodies are dynamic, if not set it equal to the one of the dynamic body //Real mbar = de1->mass*de2->mass / (de1->mass + de2->mass); // equivalent mass Real Cn_crit = 2.*sqrt(mbar*phys->kn); // Critical damping coefficient (normal direction) Real Cs_crit = 2.*sqrt(mbar*phys->ks); // Critical damping coefficient (shear direction) // Note: to compare with the analytical solution you provide cn and cs directly (since here we used a different method to define c_crit) cn = Cn_crit*phys->betan; // Damping normal coefficient cs = Cs_crit*phys->betas; // Damping tangential coefficient if(phys->kn<0 || phys->ks<0){ cerr<<"Negative stiffness kn="<<phys->kn<<" ks="<<phys->ks<<" for ##"<<b1->getId()<<"+"<<b2->getId()<<", step "<<scene->iter<<endl; } } else if (useDamping){ // (see Tsuji, 1992) Real mbar = (!b1->isDynamic() && b2->isDynamic()) ? de2->mass : ((!b2->isDynamic() && b1->isDynamic()) ? de1->mass : (de1->mass*de2->mass / (de1->mass + de2->mass))); // get equivalent mass if both bodies are dynamic, if not set it equal to the one of the dynamic body cn = phys->alpha*sqrt(mbar)*pow(uN,0.25); // normal viscous coefficient, see also [Antypov2011] eq. 10 cs = cn; // same value for shear viscous coefficient } /***************/ /* SHEAR FORCE */ /***************/ Vector3r& shearElastic = phys->shearElastic; // reference for shearElastic force // Define shifts to handle periodicity const Vector3r shift2 = scene->isPeriodic ? scene->cell->intrShiftPos(contact->cellDist): Vector3r::Zero(); const Vector3r shiftVel = scene->isPeriodic ? scene->cell->intrShiftVel(contact->cellDist): Vector3r::Zero(); // 1. Rotate shear force shearElastic = scg->rotate(shearElastic); Vector3r prev_FsElastic = shearElastic; // save shear force at previous time step // 2. Get incident velocity, get shear and normal components Vector3r incidentV = scg->getIncidentVel(de1, de2, dt, shift2, shiftVel, preventGranularRatcheting); Vector3r incidentVn = scg->normal.dot(incidentV)*scg->normal; // contact normal velocity Vector3r incidentVs = incidentV - incidentVn; // contact shear velocity // 3. Get shear force (incrementally) shearElastic = shearElastic - phys->ks*(incidentVs*dt); /**************************************/ /* VISCOUS DAMPING (Normal direction) */ /**************************************/ // normal force must be updated here before we apply the Mohr-Coulomb criterion if (useDamping){ // get normal viscous component phys->normalViscous = cn*incidentVn; Vector3r normTemp = phys->normalForce - phys->normalViscous; // temporary normal force // viscous force should not exceed the value of current normal force, i.e. no attraction force should be permitted if particles are non-adhesive // if particles are adhesive, then fixed the viscous force at maximum equal to the adhesion force // *** enforce normal force to zero if no adhesion is permitted *** if (phys->adhesionForce==0.0 || !includeAdhesion){ if (normTemp.dot(scg->normal)<0.0){ phys->normalForce = Vector3r::Zero(); phys->normalViscous = phys->normalViscous + normTemp; // normal viscous force is such that the total applied force is null - it is necessary to compute energy correctly! } else{phys->normalForce -= phys->normalViscous;} } else if (includeAdhesion && phys->adhesionForce!=0.0){ // *** limit viscous component to the max adhesive force *** if (normTemp.dot(scg->normal)<0.0 && (phys->normalViscous.norm() > phys->adhesionForce) ){ Real normVisc = phys->normalViscous.norm(); Vector3r normViscVector = phys->normalViscous/normVisc; phys->normalViscous = phys->adhesionForce*normViscVector; phys->normalForce -= phys->normalViscous; } // *** apply viscous component - in the presence of adhesion *** else {phys->normalForce -= phys->normalViscous;} } if (calcEnergy) {normDampDissip += phys->normalViscous.dot(incidentVn*dt);} // calc dissipation of energy due to normal damping } /*************************************/ /* SHEAR DISPLACEMENT (elastic only) */ /*************************************/ Vector3r& us_elastic = phys->usElastic; us_elastic = scg->rotate(us_elastic); // rotate vector Vector3r prevUs_el = us_elastic; // store previous elastic shear displacement (already rotated) us_elastic -= incidentVs*dt; // add shear increment /****************************************/ /* SHEAR DISPLACEMENT (elastic+plastic) */ /****************************************/ Vector3r& us_total = phys->usTotal; us_total = scg->rotate(us_total); // rotate vector Vector3r prevUs_tot = us_total; // store previous total shear displacement (already rotated) us_total -= incidentVs*dt; // add shear increment NOTE: this vector is not passed into the failure criterion, hence it holds also the plastic part of the shear displacement bool noShearDamp = false; // bool to decide whether we need to account for shear damping dissipation or not /********************/ /* MOHR-COULOMB law */ /********************/ phys->isSliding=false; phys->shearViscous=Vector3r::Zero(); // reset so that during sliding, the previous values is not there Fn = phys->normalForce.norm(); if (!includeAdhesion) { Real maxFs = Fn*phys->tangensOfFrictionAngle; if (shearElastic.squaredNorm() > maxFs*maxFs){ phys->isSliding=true; noShearDamp = true; // no damping is added in the shear direction, hence no need to account for shear damping dissipation Real ratio = maxFs/shearElastic.norm(); shearElastic *= ratio; phys->shearForce = shearElastic; /*store only elastic shear displacement*/ us_elastic*= ratio; if (calcEnergy) {frictionDissipation += (us_total-prevUs_tot).dot(shearElastic);} // calculate energy dissipation due to sliding behavior } else if (useDamping){ // add current contact damping if we do not slide and if damping is requested phys->shearViscous = cs*incidentVs; // get shear viscous component phys->shearForce = shearElastic - phys->shearViscous;} else if (!useDamping) {phys->shearForce = shearElastic;} // update the shear force at the elastic value if no damping is present and if we passed MC } else { // Mohr-Coulomb formulation adpated due to the presence of adhesion (see Thornton, 1991). Real maxFs = phys->tangensOfFrictionAngle*(phys->adhesionForce+Fn); // adhesionForce already included in normalForce (above) if (shearElastic.squaredNorm() > maxFs*maxFs){ phys->isSliding=true; noShearDamp = true; // no damping is added in the shear direction, hence no need to account for shear damping dissipation Real ratio = maxFs/shearElastic.norm(); shearElastic *= ratio; phys->shearForce = shearElastic; /*store only elastic shear displacement*/ us_elastic *= ratio; if (calcEnergy) {frictionDissipation += (us_total-prevUs_tot).dot(shearElastic);} // calculate energy dissipation due to sliding behavior } else if (useDamping){ // add current contact damping if we do not slide and if damping is requested phys->shearViscous = cs*incidentVs; // get shear viscous component phys->shearForce = shearElastic - phys->shearViscous;} else if (!useDamping) {phys->shearForce = shearElastic;} // update the shear force at the elastic value if no damping is present and if we passed MC } /************************/ /* SHEAR ELASTIC ENERGY */ /************************/ // NOTE: shear elastic energy calculation must come after the MC criterion, otherwise displacements and forces are not updated if (calcEnergy) { shearEnergy += (us_elastic-prevUs_el).dot((shearElastic+prev_FsElastic)/2.); // NOTE: no additional energy if we perform sliding since us_elastic and prevUs_el will hold the same value (in fact us_elastic is only keeping the elastic part). We work out the area of the trapezium. } /**************************************************/ /* VISCOUS DAMPING (energy term, shear direction) */ /**************************************************/ if (useDamping){ // get normal viscous component (the shear one is calculated inside Mohr-Coulomb criterion, see above) if (calcEnergy) {if (!noShearDamp) {shearDampDissip += phys->shearViscous.dot(incidentVs*dt);}} // calc energy dissipation due to viscous linear damping } /****************/ /* APPLY FORCES */ /****************/ if (!scene->isPeriodic) applyForceAtContactPoint(-phys->normalForce - phys->shearForce, scg->contactPoint , id1, de1->se3.position, id2, de2->se3.position); else { // in scg we do not wrap particles positions, hence "applyForceAtContactPoint" cannot be used Vector3r force = -phys->normalForce - phys->shearForce; scene->forces.addForce(id1,force); scene->forces.addForce(id2,-force); scene->forces.addTorque(id1,(scg->radius1-0.5*scg->penetrationDepth)* scg->normal.cross(force)); scene->forces.addTorque(id2,(scg->radius2-0.5*scg->penetrationDepth)* scg->normal.cross(force)); } /********************************************/ /* MOMENT CONTACT LAW */ /********************************************/ if (includeMoment){ // *** Bending ***// // new code to compute relative particle rotation (similar to the way the shear is computed) // use scg function to compute relAngVel Vector3r relAngVel = scg->getRelAngVel(de1,de2,dt); //Vector3r relAngVel = (b2->state->angVel-b1->state->angVel); Vector3r relAngVelBend = relAngVel - scg->normal.dot(relAngVel)*scg->normal; // keep only the bending part Vector3r relRot = relAngVelBend*dt; // relative rotation due to rolling behaviour // incremental formulation for the bending moment (as for the shear part) Vector3r& momentBend = phys->momentBend; momentBend = scg->rotate(momentBend); // rotate moment vector (updated) momentBend = momentBend-phys->kr*relRot; // add incremental rolling to the rolling vector // ---------------------------------------------------------------------------------------- // *** Torsion ***// Vector3r relAngVelTwist = scg->normal.dot(relAngVel)*scg->normal; Vector3r relRotTwist = relAngVelTwist*dt; // component of relative rotation along n // incremental formulation for the torsional moment Vector3r& momentTwist = phys->momentTwist; momentTwist = scg->rotate(momentTwist); // rotate moment vector (updated) momentTwist = momentTwist-phys->ktw*relRotTwist; #if 0 // code to compute the relative particle rotation if (includeMoment){ Real rMean = (scg->radius1+scg->radius2)/2.; // sliding motion Vector3r duS1 = scg->radius1*(phys->prevNormal-scg->normal); Vector3r duS2 = scg->radius2*(scg->normal-phys->prevNormal); // rolling motion Vector3r duR1 = scg->radius1*dt*b1->state->angVel.cross(scg->normal); Vector3r duR2 = -scg->radius2*dt*b2->state->angVel.cross(scg->normal); // relative position of the old contact point with respect to the new one Vector3r relPosC1 = duS1+duR1; Vector3r relPosC2 = duS2+duR2; Vector3r duR = (relPosC1+relPosC2)/2.; // incremental displacement vector (same radius is temporarily assumed) // check wheter rolling will be present, if not do nothing Vector3r x=scg->normal.cross(duR); Vector3r normdThetaR(Vector3r::Zero()); // initialize if(x.squaredNorm()==0) { /* no rolling */ } else { Vector3r normdThetaR = x/x.norm(); // moment unit vector phys->dThetaR = duR.norm()/rMean*normdThetaR;} // incremental rolling // incremental formulation for the bending moment (as for the shear part) Vector3r& momentBend = phys->momentBend; momentBend = scg->rotate(momentBend); // rotate moment vector momentBend = momentBend+phys->kr*phys->dThetaR; // add incremental rolling to the rolling vector FIXME: is the sign correct? #endif // check plasticity condition (only bending part for the moment) Real MomentMax = phys->maxBendPl*phys->normalForce.norm(); Real scalarMoment = phys->momentBend.norm(); if (MomentMax>0){ if(scalarMoment > MomentMax) { Real ratio = MomentMax/scalarMoment; // to fix the moment to its yielding value phys->momentBend *= ratio; } } // apply moments Vector3r moment = phys->momentTwist+phys->momentBend; scene->forces.addTorque(id1,-moment); scene->forces.addTorque(id2,moment); } return true; // update variables //phys->prevNormal = scg->normal; } // The following code was moved from Ip2_FrictMat_FrictMat_MindlinCapillaryPhys.cpp void Ip2_FrictMat_FrictMat_MindlinCapillaryPhys::go( const shared_ptr<Material>& b1 //FrictMat , const shared_ptr<Material>& b2 // FrictMat , const shared_ptr<Interaction>& interaction) { if(interaction->phys) return; // no updates of an already existing contact necessary shared_ptr<MindlinCapillaryPhys> contactPhysics(new MindlinCapillaryPhys()); interaction->phys = contactPhysics; FrictMat* mat1 = YADE_CAST<FrictMat*>(b1.get()); FrictMat* mat2 = YADE_CAST<FrictMat*>(b2.get()); /* from interaction physics */ Real Ea = mat1->young; Real Eb = mat2->young; Real Va = mat1->poisson; Real Vb = mat2->poisson; Real fa = mat1->frictionAngle; Real fb = mat2->frictionAngle; /* from interaction geometry */ GenericSpheresContact* scg = YADE_CAST<GenericSpheresContact*>(interaction->geom.get()); Real Da = scg->refR1>0 ? scg->refR1 : scg->refR2; Real Db = scg->refR2; //Vector3r normal=scg->normal; //The variable set but not used /* calculate stiffness coefficients */ Real Ga = Ea/(2*(1+Va)); Real Gb = Eb/(2*(1+Vb)); Real G = (Ga+Gb)/2; // average of shear modulus Real V = (Va+Vb)/2; // average of poisson's ratio Real E = Ea*Eb/((1.-std::pow(Va,2))*Eb+(1.-std::pow(Vb,2))*Ea); // Young modulus Real R = Da*Db/(Da+Db); // equivalent radius Real Rmean = (Da+Db)/2.; // mean radius Real Kno = 4./3.*E*sqrt(R); // coefficient for normal stiffness Real Kso = 2*sqrt(4*R)*G/(2-V); // coefficient for shear stiffness Real frictionAngle = std::min(fa,fb); Real Adhesion = 4.*Mathr::PI*R*gamma; // calculate adhesion force as predicted by DMT theory /* pass values calculated from above to MindlinCapillaryPhys */ contactPhysics->tangensOfFrictionAngle = std::tan(frictionAngle); //mindlinPhys->prevNormal = scg->normal; // used to compute relative rotation contactPhysics->kno = Kno; // this is just a coeff contactPhysics->kso = Kso; // this is just a coeff contactPhysics->adhesionForce = Adhesion; contactPhysics->kr = krot; contactPhysics->ktw = ktwist; contactPhysics->maxBendPl = eta*Rmean; // does this make sense? why do we take Rmean? /* compute viscous coefficients */ if(en && betan) throw std::invalid_argument("Ip2_FrictMat_FrictMat_MindlinCapillaryPhys: only one of en, betan can be specified."); if(es && betas) throw std::invalid_argument("Ip2_FrictMat_FrictMat_MindlinCapillaryPhys: only one of es, betas can be specified."); // en or es specified, just compute alpha, otherwise alpha remains 0 if(en || es){ Real logE = log((*en)(mat1->id,mat2->id)); contactPhysics->alpha = -sqrt(5/6.)*2*logE/sqrt(pow(logE,2)+pow(Mathr::PI,2))*sqrt(2*E*sqrt(R)); // (see Tsuji, 1992) } // betan specified, use that value directly; otherwise give zero else{ contactPhysics->betan=betan ? (*betan)(mat1->id,mat2->id) : 0; contactPhysics->betas=betas ? (*betas)(mat1->id,mat2->id) : contactPhysics->betan; } };
void Law2_ScGeom_CFpmPhys_CohesiveFrictionalPM::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact){ ScGeom* geom = static_cast<ScGeom*>(ig.get()); CFpmPhys* phys = static_cast<CFpmPhys*>(ip.get()); const int &id1 = contact->getId1(); const int &id2 = contact->getId2(); Body* b1 = Body::byId(id1,scene).get(); Body* b2 = Body::byId(id2,scene).get(); Real displN = geom->penetrationDepth; // NOTE: the sign for penetrationdepth is different from ScGeom and Dem3DofGeom: geom->penetrationDepth>0 when spheres interpenetrate Real Dtensile=phys->FnMax/phys->kn; Real Dsoftening = phys->strengthSoftening*Dtensile; /*to set the equilibrium distance between all cohesive elements when they first meet -> allows one to work with initial stress-free assembly*/ if ( contact->isFresh(scene) ) { phys->initD = displN; phys->normalForce = Vector3r::Zero(); phys->shearForce = Vector3r::Zero();} Real D = displN - phys->initD; // interparticular distance is computed depending on the equilibrium distance /* Determination of interaction */ if (D < 0){ //spheres do not touch if (!phys->isCohesive){ scene->interactions->requestErase(contact); return; } // destroy the interaction before calculation if ((phys->isCohesive) && (abs(D) > (Dtensile + Dsoftening))) { // spheres are bonded and the interacting distance is greater than the one allowed ny the defined cohesion phys->isCohesive=false; // update body state with the number of broken bonds CFpmState* st1=dynamic_cast<CFpmState*>(b1->state.get()); CFpmState* st2=dynamic_cast<CFpmState*>(b2->state.get()); st1->numBrokenCohesive+=1; st2->numBrokenCohesive+=1; //// the same thing but from ConcretePM //const shared_ptr<Body>& body1=Body::byId(contact->getId1(),scene), body2=Body::byId(contact->getId2(),scene); assert(body1); assert(body2); //const shared_ptr<CFpmState>& st1=YADE_PTR_CAST<CFpmState>(body1->state), st2=YADE_PTR_CAST<CFpmState>(body2->state); //{ boost::mutex::scoped_lock lock(st1->updateMutex); st1->numBrokenCohesive+=1; } //{ boost::mutex::scoped_lock lock(st2->updateMutex); st2->numBrokenCohesive+=1; } // end of update scene->interactions->requestErase(contact); return; } } /*NormalForce*/ Real Fn=0, Dsoft=0; if ((D < 0) && (abs(D) > Dtensile)) { //to take into account strength softening Dsoft = D+Dtensile; // Dsoft<0 for a negative value of Fn (attractive force) Fn = -(phys->FnMax+(phys->kn/phys->strengthSoftening)*Dsoft); // computes FnMax - FnSoftening } else { Fn = phys->kn*D; } phys->normalForce = Fn*geom->normal; // NOTE normal is position2-position1 - It is directed from particle1 to particle2 /*ShearForce*/ Vector3r& shearForce = phys->shearForce; // using scGeom function rotateAndGetShear State* st1 = Body::byId(id1,scene)->state.get(); State* st2 = Body::byId(id2,scene)->state.get(); geom->rotate(phys->shearForce); const Vector3r& dus = geom->shearIncrement(); //Linear elasticity giving "trial" shear force shearForce -= phys->ks*dus; // needed for the next timestep phys->prevNormal = geom->normal; /* Morh-Coulomb criterion */ Real maxFs = phys->FsMax + Fn*phys->tanFrictionAngle; if (shearForce.squaredNorm() > maxFs*maxFs){ shearForce*=maxFs/shearForce.norm(); // to fix the shear force to its yielding value } /* Apply forces */ Vector3r f = phys->normalForce + shearForce; // these lines to adapt to periodic boundary conditions (NOTE applyForceAtContactPoint computes torque induced by normal and shear force too) if (!scene->isPeriodic) applyForceAtContactPoint(f , geom->contactPoint , id2, st2->se3.position, id1, st1->se3.position); else { // in scg we do not wrap particles positions, hence "applyForceAtContactPoint" cannot be used when scene is periodic scene->forces.addForce(id1,-f); scene->forces.addForce(id2,f); scene->forces.addTorque(id1,(geom->radius1-0.5*geom->penetrationDepth)* geom->normal.cross(-f)); scene->forces.addTorque(id2,(geom->radius2-0.5*geom->penetrationDepth)* geom->normal.cross(-f)); } /* Moment Rotation Law */ // NOTE this part could probably be computed in ScGeom to avoid copy/paste multiplication !!! Quaternionr delta( b1->state->ori * phys->initialOrientation1.conjugate() *phys->initialOrientation2 * b2->state->ori.conjugate()); delta.normalize(); //relative orientation AngleAxisr aa(delta); // axis of rotation - this is the Moment direction UNIT vector; angle represents the power of resistant ELASTIC moment if(aa.angle() > Mathr::PI) aa.angle() -= Mathr::TWO_PI; // angle is between 0 and 2*pi, but should be between -pi and pi phys->cumulativeRotation = aa.angle(); //Find angle*axis. That's all. But first find angle about contact normal. Result is scalar. Axis is contact normal. Real angle_twist(aa.angle() * aa.axis().dot(geom->normal) ); //rotation about normal Vector3r axis_twist(angle_twist * geom->normal); Vector3r moment_twist(axis_twist * phys->kr); Vector3r axis_bending(aa.angle()*aa.axis() - axis_twist); //total rotation minus rotation about normal Vector3r moment_bending(axis_bending * phys->kr); Vector3r moment = moment_twist + moment_bending; Real MomentMax = phys->maxBend*std::fabs(phys->normalForce.norm()); Real scalarMoment = moment.norm(); /*Plastic moment */ if(scalarMoment > MomentMax) { Real ratio = 0; ratio *= MomentMax/scalarMoment; // to fix the moment to its yielding value moment *= ratio; moment_twist *= ratio; moment_bending *= ratio; } phys->moment_twist = moment_twist; phys->moment_bending = moment_bending; scene->forces.addTorque(id1,-moment); scene->forces.addTorque(id2, moment); }
bool Ig2_Facet_Sphere_ScGeom::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) { TIMING_DELTAS_START(); const Se3r& se31=state1.se3; const Se3r& se32=state2.se3; Facet* facet = static_cast<Facet*>(cm1.get()); /* could be written as (needs to be tested): * Vector3r cl=se31.orientation.Conjugate()*(se32.position-se31.position); */ Matrix3r facetAxisT=se31.orientation.toRotationMatrix(); Matrix3r facetAxis = facetAxisT.transpose(); // local orientation Vector3r cl = facetAxis*(se32.position + shift2 - se31.position); // "contact line" in facet-local coords // // BEGIN everything in facet-local coordinates // Vector3r normal = facet->normal; Real L = normal.dot(cl); if (L<0) {normal=-normal; L=-L; } Real sphereRadius = static_cast<Sphere*>(cm2.get())->radius; if (L>sphereRadius && !c->isReal() && !force) { // no contact, but only if there was no previous contact; ortherwise, the constitutive law is responsible for setting Interaction::isReal=false TIMING_DELTAS_CHECKPOINT("Ig2_Facet_Sphere_ScGeom"); return false; } Vector3r cp = cl - L*normal; const Vector3r* ne = facet->ne; Real penetrationDepth=0; Real bm = ne[0].dot(cp); int m=0; for (int i=1; i<3; ++i) { Real b=ne[i].dot(cp); if (bm<b) {bm=b; m=i;} } Real sh = sphereRadius*shrinkFactor; Real icr = facet->icr-sh; if (icr<0) { LOG_WARN("a radius of a facet's inscribed circle less than zero! So, shrinkFactor is too large and would be reduced to zero."); shrinkFactor=0; icr = facet->icr; sh = 0; } if (bm<icr) // contact with facet's surface { penetrationDepth = sphereRadius - L; normal.normalize(); } else { cp = cp + ne[m]*(icr-bm); if (cp.dot(ne[(m-1<0)?2:m-1])>icr) // contact with vertex m // cp = facet->vertices[m]; cp = facet->vu[m]*(facet->vl[m]-sh); else if (cp.dot(ne[m=(m+1>2)?0:m+1])>icr) // contact with vertex m+1 // cp = facet->vertices[(m+1>2)?0:m+1]; cp = facet->vu[m]*(facet->vl[m]-sh); normal = cl-cp; Real norm=normal.norm(); normal/=norm; penetrationDepth = sphereRadius - norm; } // // END everything in facet-local coordinates // if (penetrationDepth>0 || c->isReal()) { shared_ptr<ScGeom> scm; bool isNew = !c->geom; if (c->geom) scm = YADE_PTR_CAST<ScGeom>(c->geom); else scm = shared_ptr<ScGeom>(new ScGeom()); normal = facetAxisT*normal; // in global orientation scm->contactPoint = se32.position + shift2 - (sphereRadius-0.5*penetrationDepth)*normal; scm->penetrationDepth = penetrationDepth; scm->radius1 = 2*sphereRadius; scm->radius2 = sphereRadius; if (isNew) c->geom = scm; scm->precompute(state1,state2,scene,c,normal,isNew,shift2,false/*avoidGranularRatcheting only for sphere-sphere*/); TIMING_DELTAS_CHECKPOINT("Ig2_Facet_Sphere_ScGeom"); return true; } TIMING_DELTAS_CHECKPOINT("Ig2_Facet_Sphere_ScGeom"); return false; }
string FlowAnalysis::vtkExportFractions(const string& out, /*copy, since we may modify locally*/ vector<size_t> fractions){ if(timeSpan==0.) throw std::runtime_error("FlowAnalysis.timeSpan==0, no data was ever collected."); if(nFractions<=0) throw std::runtime_error("FlowAnalysis.nFractions<=0?!!"); auto grid=vtkMakeGrid(); auto flow=vtkMakeArray(grid,"flow (momentum density)",3); auto flowNorm=vtkMakeArray(grid,"|flow|",1); auto ek=vtkMakeArray(grid,"Ek density",1); auto hitRate=vtkMakeArray(grid,"hit rate",1); auto sumWeight=vtkMakeArray(grid,"sum weight",1); auto diam=vtkMakeArray(grid,"avg. diameter",1); auto solid=vtkMakeArray(grid,"avg. solid ratio",1); auto vel=vtkMakeArray(grid,"avg. velocity",3); // no fractions given, export all of them together if(fractions.empty()){ for(size_t i=0; i<(size_t)nFractions; i++) fractions.push_back(i); } for(size_t fraction: fractions){ if((int)fraction>=nFractions) throw std::runtime_error("FlowAnalysis.vtkExportFraction: fraction="+to_string(fraction)+" out of range 0.."+to_string(nFractions-1)); // traverse the grid now for(int i=0; i<boxCells[0]; i++){ for(int j=0; j<boxCells[1]; j++){ for(int k=0; k<boxCells[2]; k++){ int ijk[]={i,j,k}; vtkIdType dataId; if(cellData) dataId=grid->ComputeCellId(ijk); else dataId=grid->ComputePointId(ijk); const auto ptData(data[fraction][i][j][k]); /* quantities SUMMED over fractions */ // flow vector Vector3r f; flow->GetTupleValue(dataId,f.data()); flow->SetTupleValue(dataId,(f+Vector3r(ptData[PT_FLOW_X],ptData[PT_FLOW_Y],ptData[PT_FLOW_Z])/timeSpan).eval().data()); // scalars *(ek->GetPointer(dataId))+=ptData[PT_EK]/timeSpan; *(hitRate->GetPointer(dataId))+=ptData[PT_SUM_WEIGHT]/timeSpan; *(sumWeight->GetPointer(dataId))+=ptData[PT_SUM_WEIGHT]; /* quantities AVERAGED over fractions (summed here and divided by summary weight in the next loop) */ *(diam ->GetPointer(dataId))+=ptData[PT_SUM_DIAM]; *(solid->GetPointer(dataId))+=ptData[PT_SUM_SOLID_RATIO]; Vector3r v; vel->GetTupleValue(dataId,v.data()); vel->SetTupleValue(dataId,(v+Vector3r(ptData[PT_VEL_X],ptData[PT_VEL_Y],ptData[PT_VEL_Z])).eval().data()); } } } } // extra loop to fill the |flow| // (cannot be done incrementally above, as sum or norms does not equal norm of sums) for(int i=0; i<boxCells[0]; i++){ for(int j=0; j<boxCells[1]; j++){ for(int k=0; k<boxCells[2]; k++){ int ijk[]={i,j,k}; vtkIdType dataId; if(cellData) dataId=grid->ComputeCellId(ijk); else dataId=grid->ComputePointId(ijk); Vector3r f; flow->GetTupleValue(dataId,f.data()); flowNorm->SetValue(dataId,f.norm()); // averages over all fractions Real wAll=0.; for(size_t fraction: fractions) wAll+=data[fraction][i][j][k][PT_SUM_WEIGHT]; if(wAll!=0){ *(diam ->GetPointer(dataId))/=wAll; *(solid->GetPointer(dataId))/=wAll; Vector3r v; vel->GetTupleValue(dataId,v.data()); vel->SetTupleValue(dataId,(v/wAll).eval().data()); } else { *(diam ->GetPointer(dataId))=NaN; *(solid->GetPointer(dataId))=0; vel->SetTupleValue(dataId,Vector3r::Zero().eval().data()); } } } } return vtkWriteGrid(out,grid); }
bool Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D::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) { const ChainedState *pChain1, *pChain2; pChain1=YADE_CAST<const ChainedState*>(&state1); pChain2=YADE_CAST<const ChainedState*>(&state2); unsigned int sizeChain1=pChain1->chains[pChain1->chainNumber].size(); unsigned int sizeChain2=pChain2->chains[pChain2->chainNumber].size(); if (!pChain1 || !pChain2) { cerr <<"cast failed8567"<<endl; } const bool revert = ((int) pChain2->rank- (int) pChain1->rank == -1); const ChainedState& bchain1 = revert? *pChain2 : *YADE_CAST<const ChainedState*>(&state1); const ChainedState& bchain2 = revert? *pChain1 : *pChain2; ChainedCylinder *bs1=static_cast<ChainedCylinder*>(revert? cm2.get():cm1.get()); bool isLast = bchain1.chains[bchain1.chainNumber].size()==(bchain1.rank+1) || bchain2.chains[bchain2.chainNumber].size()==(bchain2.rank+1); bool isNew = !c->geom; if (pChain2->chainNumber!=pChain1->chainNumber) { shared_ptr<ChCylGeom6D> scm; if(isLast){ return false; } shared_ptr<Body> cylinderNext1=Body::byId(pChain1->chains[pChain1->chainNumber][pChain1->rank+1],scene); shared_ptr<Body> cylinderNext2=Body::byId(pChain2->chains[pChain2->chainNumber][pChain2->rank+1],scene); //cout<<c->id1<<" "<<c->id2<<endl; bool colinearVectors=0, insideCyl1=1, insideCyl2=1; //insideCyl1&2 are used to determine whether the contact is inside each cylinder's segment Real dist=NaN,k=0,m=0; //k and m are the parameters of the fictious states on the cylinders. Vector3r A=pChain1->pos, a=cylinderNext1->state->pos-A , B=pChain2->pos , b=cylinderNext2->state->pos-B; Vector3r N=a.cross(b); Vector3r normal; if(N.norm()>1e-14){ dist=std::abs(N.dot(B-A)/(N.norm())); //distance between the two LINES. //But we need to check that the intersection point is inside the two SEGMENTS ... //Projection of B to have a common plan between the two segments. Vector3r projB1=B+dist*(N/(N.norm())) , projB2=B-dist*(N/(N.norm())); Real distB1A=(projB1-A).norm() , distB2A=(projB2-A).norm() ; Vector3r projB=(distB1A<=distB2A)*projB1 + (distB1A>distB2A)*projB2; int b1=0, b2=1; //base vectors used to compute the segment intersection (if N is aligned with an axis, we can't use this axis) if(std::abs(N[1])<1e-14 && std::abs(N[2])<1e-14){b1=1;b2=2;} if(std::abs(N[0])<1e-14 && std::abs(N[2])<1e-14){b1=0;b2=2;} Real det=a[b1]*b[b2]-a[b2]*b[b1]; if(std::abs(det)>1e-14){ //Check if the two segments are intersected (using k and m) k = (b[b2]*(projB[b1]-A[b1])+b[b1]*(A[b2]-projB[b2]))/det; m = (a[b1]*(-projB[b2]+A[b2])+a[b2]*(projB[b1]-A[b1]))/det; if( k<0.0 || k>=1.0 || m<0.0 || m>=1.0 ) { //so they are not intersected dist=NaN; if(k>=1){k=1; if(!(pChain1->rank>=sizeChain1-2))insideCyl1=0; } if(k<0){k=0; if(!(pChain1->rank==0))insideCyl1=0; } if(m>=1){m=1; if(!(pChain2->rank>=sizeChain2-2))insideCyl2=0; } if(m<0){m=0; if(!(pChain2->rank==0))insideCyl2=0; } } } else cout<<"Error in Ig2_ChainedCylinder_ChainedCylinder_ScGeom6D : det==0 !!!"<<endl;//should not happen } else { //Special case for parallel cylinders. //FIXME : this is an approximation, but it seems very complicated to do something else. //FIXME : contact following have to be done for parallel cylinders. colinearVectors=1; insideCyl1=1 ; insideCyl2=1; Real PA=(A-B).dot(b)/(b.norm()*b.norm()); PA=min((Real)1.0,max((Real)0.0,PA)); Real Pa=(A+a-B).dot(b)/(b.norm()*b.norm()); Pa=min((Real)1.0,max((Real)0.0,Pa)); Real PB=(B-A).dot(a)/(a.norm()*a.norm()); PB=min((Real)1.0,max((Real)0.0,PB)); Real Pb=(B+b-A).dot(a)/(a.norm()*a.norm()); Pb=min((Real)1.0,max((Real)0.0,Pb)); Real h1=(A+0.5*a-B).dot(b)/(b.norm()*b.norm()); //Projection parameter of center of a on b Real h2=(B+0.5*b-A).dot(a)/(a.norm()*a.norm()); //Projection parameter of center of b on a k=(PB+Pb)/2. ; m=(PA+Pa)/2.; dist= (A+k*a - (B+m*b)).norm(); bool edgeEdgeContact = (h1>1 && pChain2->rank>=sizeChain2-2) || (h1<0 && pChain2->rank==0) || (h2>1 && pChain1->rank>=sizeChain1-2) || (h2<0 && pChain1->rank==0); if(edgeEdgeContact)colinearVectors=0; if( (0<=h1 and 1>h1) or (0<=h2 and 1>h2) or edgeEdgeContact){; //Do a perfectly flat contact } else return false;//Parallel lines but edge-edge contact } ChainedCylinder *cc1=static_cast<ChainedCylinder*>(cm1.get()); ChainedCylinder *cc2=static_cast<ChainedCylinder*>(cm2.get()); if(isnan(dist)){ //now if we didn't found a suitable distance because the segments don't cross each other, we try to find a sphere-cylinder distance. Vector3r pointsToCheck[4]={A,A+a,B,B+b}; Real resultDist=dist, resultProj=dist ; int whichCaseIsCloser=-1 ; for (int i=0;i<4;i++){ //loop on the 4 cylinder's extremities and look at the extremity-cylinder distance Vector3r S=pointsToCheck[i], C=(i<2)?B:A, vec=(i<2)?b:a; Vector3r CS=S-C; Real d=CS.dot(vec)/(vec.norm()); if(d<0.) resultDist=CS.norm(); else if(d>vec.norm()) resultDist=(C+vec-S).norm(); else resultDist=(CS.cross(vec)).norm()/(vec.norm()); if(dist>resultDist or isnan(dist)){dist=resultDist ; whichCaseIsCloser=i; resultProj=d;} } //we know which extremity may be in contact (i), so k and m are computed to generate the right fictiousStates. insideCyl1=1 ; insideCyl2=1; //FIXME:NATCHOS ! this should be reformulated if(whichCaseIsCloser==0 || whichCaseIsCloser==1){ k=whichCaseIsCloser==0?0:1; if(resultProj<=0){ m=0; if(!(pChain2->rank==0))insideCyl2=0;} else if(resultProj>b.norm()){ m=1; if(!(pChain2->rank>=sizeChain2-2))insideCyl2=0;} else m=resultProj/(b.norm()); if(isNew && whichCaseIsCloser==1 && !(pChain1->rank>=sizeChain1-2)) return false; } else{ m=whichCaseIsCloser==2?0:1; if(resultProj<=0){ k=0; if(!(pChain1->rank==0))insideCyl1=0;} else if(resultProj>a.norm()){ k=1; if(!(pChain1->rank>=sizeChain2-2))insideCyl1=0;} else k=resultProj/(a.norm()); if(isNew && whichCaseIsCloser==3 && !(pChain2->rank>=sizeChain2-2)) return false; } } if(isNew && dist>=cc1->radius + cc2->radius) return false; //if the contact had not yet occured, return false. //FIXME:the next line sometimes causes an error in the terminal, because instead of returning false here the contact should be correctly erased. if(insideCyl1==0 || insideCyl2==0) return false; //the contact may be duplicated ... else{ //else create the geometry. if(!isNew) scm=YADE_PTR_CAST<ChCylGeom6D>(c->geom); else { scm=shared_ptr<ChCylGeom6D>(new ChCylGeom6D()); c->geom=scm; } scm->relPos1=colinearVectors?0.5:k ; scm->relPos2=colinearVectors?0.5:m; scm->fictiousState1.pos=A + k*a; scm->fictiousState2.pos=B + m*b; scm->fictiousState1.vel = (1-k)*pChain1->vel + k*cylinderNext1->state->vel; scm->fictiousState2.vel = (1-m)*pChain2->vel + m*cylinderNext2->state->vel; Vector3r direction = a/(a.norm()); scm->fictiousState1.angVel = ((1-k)*pChain1->angVel + k*cylinderNext1->state->angVel).dot(direction)*direction //twist part : interpolated + a.cross(cylinderNext1->state->vel - pChain1->vel);// non-twist part : defined from nodes velocities direction = b/(b.norm()); scm->fictiousState2.angVel = ((1-m)*pChain2->angVel + m*cylinderNext2->state->angVel).dot(direction)*direction //twist part : interpolated + b.cross(cylinderNext2->state->vel - pChain2->vel);// non-twist part : defined from nodes velocities scm->contactPoint = 0.5*(scm->fictiousState1.pos+scm->fictiousState2.pos); normal= scm->fictiousState2.pos - scm->fictiousState1.pos; normal=normal/(normal.norm()); scm->penetrationDepth=cc1->radius+cc2->radius-dist; scm->radius1=cc1->radius; scm->radius2=cc2->radius; scm->precompute(scm->fictiousState1,scm->fictiousState2,scene,c,normal,isNew,shift2,true); return true; } } else if (bchain2.rank-bchain1.rank != 1) {/*cerr<<"Mutual contacts in same chain between not adjacent elements, not handled*/ return false;} else{ //contact between two Cylinders within the same chain. shared_ptr<ScGeom6D> scm; if(!isNew) scm=YADE_PTR_CAST<ScGeom6D>(c->geom); else { scm=shared_ptr<ScGeom6D>(new ScGeom6D()); c->geom=scm; } Real length=(bchain2.pos-bchain1.pos).norm(); Vector3r segt =pChain2->pos-pChain1->pos; if(isNew) {/*scm->normal=scm->prevNormal=segt/length;*/bs1->initLength=length;} if (!halfLengthContacts){ scm->radius1=revert ? 0:bs1->initLength; scm->radius2=revert ? bs1->initLength:0; scm->contactPoint=bchain2.pos; } else { scm->radius1=scm->radius2=0.5*bs1->initLength; scm->contactPoint=0.5*(bchain2.pos+bchain1.pos); } scm->penetrationDepth=bs1->initLength-length; //bs1->segment used for fast BBs and projections + display bs1->segment= bchain2.pos-bchain1.pos; #ifdef YADE_OPENGL bs1->length=length; bs1->chainedOrientation.setFromTwoVectors(Vector3r::UnitZ(),bchain1.ori.conjugate()*segt); #endif scm->precompute(state1,state2,scene,c,segt/length,isNew,shift2,true); scm->precomputeRotations(state1,state2,isNew,false); //Set values that will be considered in Ip2 functor, geometry (precomputed) is really defined with values above scm->radius1 = scm->radius2 = bs1->initLength*0.5; return true; } }
void Gl1_NormPhys::go(const shared_ptr<IPhys>& ip, const shared_ptr<Interaction>& i, const shared_ptr<Body>& b1, const shared_ptr<Body>& b2, bool wireFrame){ if(!gluQuadric){ gluQuadric=gluNewQuadric(); if(!gluQuadric) throw runtime_error("Gl1_NormPhys::go unable to allocate new GLUquadric object (out of memory?)."); } NormPhys* np=static_cast<NormPhys*>(ip.get()); shared_ptr<IGeom> ig(i->geom); if(!ig) return; // changed meanwhile? GenericSpheresContact* geom=YADE_CAST<GenericSpheresContact*>(ig.get()); //if(!geom) cerr<<"Gl1_NormPhys: IGeom is not a GenericSpheresContact, but a "<<ig->getClassName()<<endl; Real fnNorm=np->normalForce.dot(geom->normal); if((signFilter>0 && fnNorm<0) || (signFilter<0 && fnNorm>0)) return; int fnSign=fnNorm>0?1:-1; fnNorm=abs(fnNorm); Real radiusScale=1.; // weak/strong fabric, only used if maxWeakFn is set if(!isnan(maxWeakFn)){ if(fnNorm*fnSign<maxWeakFn){ // weak fabric if(weakFilter>0) return; radiusScale=weakScale; } else { // strong fabric if(weakFilter<0) return; } } maxFn=max(fnNorm,maxFn); Real realMaxRadius; if(maxRadius<0){ if(geom->refR1>0) refRadius=min(geom->refR1,refRadius); if(geom->refR2>0) refRadius=min(geom->refR2,refRadius); realMaxRadius=refRadius; } else realMaxRadius=maxRadius; Real radius=radiusScale*realMaxRadius*(fnNorm/maxFn); // use logarithmic scale here? Vector3r color=Shop::scalarOnColorScale(fnNorm*fnSign,-maxFn,maxFn); # if 0 // get endpoints from body positions Vector3r p1=b1->state->pos, p2=b2->state->pos; Vector3r relPos; if(scene->isPeriodic){ relPos=p2+scene->cell->Hsize*i->cellDist.cast<Real>()-p1; p1=scene->cell->wrapShearedPt(p1); p2=p1+relPos; } else { relPos=p2-p1; } Real dist=relPos.norm(); #else // get endpoints from geom // max(r,0) handles r<0 which is the case for "radius" of the facet in Dem3DofGeom_FacetSphere Vector3r cp=scene->isPeriodic? scene->cell->wrapShearedPt(geom->contactPoint) : geom->contactPoint; Vector3r p1=cp-max(geom->refR1,(Real)0.)*geom->normal; Vector3r p2=cp+max(geom->refR2,(Real)0.)*geom->normal; const Vector3r& dispScale=scene->renderer ? scene->renderer->dispScale : Vector3r::Ones(); if(dispScale!=Vector3r::Ones()){ // move p1 and p2 by the same amounts as particles themselves would be moved p1+=dispScale.cwiseProduct(Vector3r(b1->state->pos-b1->state->refPos)); p2+=dispScale.cwiseProduct(Vector3r(b2->state->pos-b2->state->refPos)); } Vector3r relPos=p2-p1; Real dist=relPos.norm(); //max(geom->refR1,0.)+max(geom->refR2,0.); #endif glDisable(GL_CULL_FACE); glPushMatrix(); glTranslatef(p1[0],p1[1],p1[2]); Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),relPos/dist /* normalized */)); // using Transform with OpenGL: http://eigen.tuxfamily.org/dox/TutorialGeometry.html //glMultMatrixd(Eigen::Affine3d(q).data()); glMultMatrix(Eigen::Transform<Real,3,Eigen::Affine>(q).data()); glColor3v(color); gluCylinder(gluQuadric,radius,radius,dist,slices,stacks); glPopMatrix(); }
void RockLiningGlobal::action(){ const double PI = 3.14159; if (openingCreated == true && installed == false){ double angleInterval = 2.0*PI/static_cast<double>(totalNodes); for (int n=0; n<totalNodes;n++){ double currentAngle = 0.0 + n*angleInterval; /* from 0 degrees east */ double unitX = cos(currentAngle); double unitY = sin(currentAngle); Vector3r searchDir(unitX,0,unitY); vector<double> distanceFrOpening; vector<int> IDs; double outerRadius = openingRad + 1.0; FOREACH(const shared_ptr<Body>& b, *scene->bodies){ if (!b) continue; if (b->isClump() == true) continue; PotentialBlock* pb=static_cast<PotentialBlock*>(b->shape.get()); if(!pb) continue; if(pb->isBoundary == true || pb->erase== true || pb->isLining==true){continue;} State* state1 = b->state.get(); Vector3r intersectionPt(0,0,0); if ( installLining(pb, state1, startingPoint, searchDir, outerRadius, intersectionPt )){ IDs.push_back(b->id); distanceFrOpening.push_back((intersectionPt-startingPoint).norm()); //std::cout<<"currentAngle: "<<currentAngle<<", b->id: "<<b->id<<", dist: "<<(intersectionPt-startingPoint).norm()<<endl; } } /* find closest block */ int totalBlocks = IDs.size(); double closestDistance = 100000.0; int closestID=0; for (int i=0; i<totalBlocks; i++){ if ( distanceFrOpening[i] < closestDistance){ closestID = IDs[i]; closestDistance = distanceFrOpening[i]; } } stickIDs.push_back(closestID); IDs.clear();distanceFrOpening.clear(); //std::cout<<"closestID: "<<closestID<<endl; /* find intersection with edges of polygon */ Vector3r jointIntersection (0,0,0); State* state1 = Body::byId(closestID,scene)->state.get(); Shape* shape1 = Body::byId(closestID,scene)->shape.get(); PotentialBlock *pb=static_cast<PotentialBlock*>(shape1); int totalPlanes = pb->a.size(); int intersectNo = 0; Vector3r nodeLocalPos(0,0,0); Vector3r nodeGlobalPos(0,0,0); //std::cout<<"totalPlanes: "<<totalPlanes<<endl; double closestPlaneDist = 1000000; for (int i=0; i<totalPlanes; i++){ Vector3r plane = state1->ori*Vector3r(pb->a[i], pb->b[i], pb->c[i]); double planeD = plane.dot(state1->pos) + pb->d[i] +pb->r; if ( intersectPlane(pb, state1,startingPoint,searchDir, outerRadius, jointIntersection, plane, planeD)){ double distance = jointIntersection.norm(); if (distance < closestPlaneDist){ closestPlaneDist = distance; nodeLocalPos = state1->ori.conjugate()*(jointIntersection-state1->pos); nodeGlobalPos=jointIntersection; } } } if(nodeGlobalPos.norm() > 1.03*openingRad){ nodeGlobalPos=1.03*openingRad*searchDir;} //if(nodeGlobalPos.norm() < 0.98*openingRad){ continue;} //initOverlap = interfaceTension/interfaceStiffness; nodeGlobalPos = nodeGlobalPos + searchDir*initOverlap; localCoordinates.push_back(nodeLocalPos); refPos.push_back(nodeGlobalPos); int nodeID = insertNode(nodeGlobalPos, lumpedMass, contactLength); blockIDs.push_back(nodeID); //(nodeID); //(closestID); refOri.push_back(Quaternionr::Identity()); //(state1->ori); installed = true; axialForces.push_back(0.0); shearForces.push_back(0.0); moment.push_back(0.0); sigmaMax.push_back(0.0); sigmaMin.push_back(0.0); displacement.push_back(0.0); radialDisplacement.push_back(0.0); } totalNodes=blockIDs.size(); /* Assembling global stiffness matrix */ for (int n=0; n<totalNodes;n++){ int nextID = n+1; if(nextID==totalNodes){nextID=0;} double Length = (refPos[nextID]-refPos[n]).norm(); lengthNode.push_back(Length); Vector3r localDir = refPos[nextID]-refPos[n]; localDir.normalize(); refDir.push_back(localDir); double angle = acos(localDir.dot(Vector3r(1,0,0))); Vector3r signAngle = Vector3r(1,0.0,0).cross(localDir); if (signAngle.dot(Vector3r(0,-1.0,0)) < 0.0){angle =2.0*PI - angle;} refAngle.push_back(angle); std::cout<<"angle "<<n<<" : "<<angle/PI*180.0<<endl; } }
inHyperboloid(const Vector3r& _c1, const Vector3r& _c2, Real _R, Real _r){ c1=_c1; c2=_c2; R=_R; a=_r; c12=c2-c1; ht=c12.norm(); Real uMax=sqrt(pow(R/a,2)-1); c=ht/(2*uMax); }
inCylinder(const Vector3r& _c1, const Vector3r& _c2, Real _radius){c1=_c1; c2=_c2; c12=c2-c1; radius=_radius; ht=c12.norm(); }
bool Law2_ScGeom_JCFpmPhys_JointedCohesiveFrictionalPM::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact){ const int &id1 = contact->getId1(); const int &id2 = contact->getId2(); ScGeom* geom = static_cast<ScGeom*>(ig.get()); JCFpmPhys* phys = static_cast<JCFpmPhys*>(ip.get()); Body* b1 = Body::byId(id1,scene).get(); Body* b2 = Body::byId(id2,scene).get(); Real Dtensile=phys->FnMax/phys->kn; string fileCracks = "cracks_"+Key+".txt"; string fileMoments = "moments_"+Key+".txt"; /// Defines the interparticular distance used for computation Real D = 0; /*this is for setting the equilibrium distance between all cohesive elements at the first contact detection*/ if ( contact->isFresh(scene) ) { phys->normalForce = Vector3r::Zero(); phys->shearForce = Vector3r::Zero(); if ((smoothJoint) && (phys->isOnJoint)) { phys->jointNormal = geom->normal.dot(phys->jointNormal)*phys->jointNormal; //to set the joint normal colinear with the interaction normal phys->jointNormal.normalize(); phys->initD = std::abs((b1->state->pos - b2->state->pos).dot(phys->jointNormal)); // to set the initial gap as the equilibrium gap } else { phys->initD = geom->penetrationDepth; } } if ( smoothJoint && phys->isOnJoint ) { if ( phys->more || ( phys-> jointCumulativeSliding > (2*min(geom->radius1,geom->radius2)) ) ) { if (!neverErase) return false; else { phys->shearForce = Vector3r::Zero(); phys->normalForce = Vector3r::Zero(); phys->isCohesive =0; phys->FnMax = 0; phys->FsMax = 0; return true; } } else { D = phys->initD - std::abs((b1->state->pos - b2->state->pos).dot(phys->jointNormal)); } } else { D = geom->penetrationDepth - phys->initD; } phys->crackJointAperture = D<0? -D : 0.; // for DFNFlow if (!phys->momentBroken && useStrainEnergy) phys->strainEnergy = 0.5*((pow(phys->normalForce.norm(),2)/phys->kn) + (pow(phys->shearForce.norm(),2)/phys->ks)); else if (!phys->momentBroken && !useStrainEnergy) computeKineticEnergy(phys, b1, b2); //Compute clustered acoustic emission events: if (recordMoments && !neverErase){ cerr << "Acoustic emissions algorithm requires neverErase=True, changing value from False to True" << endl; neverErase=true; } if (phys->momentBroken && recordMoments && !phys->momentCalculated){ if (phys->originalClusterEvent && !phys->computedCentroid) computeCentroid(phys); if (phys->originalClusterEvent) computeClusteredMoment(phys); if (phys->momentCalculated && phys->momentMagnitude!=0){ std::ofstream file (fileMoments.c_str(), !momentsFileExist ? std::ios::trunc : std::ios::app); if(file.tellp()==0){ file <<"i p0 p1 p2 moment numInts eventNum time beginTime"<<endl; } file << boost::lexical_cast<string> ( scene->iter )<<" "<< boost::lexical_cast<string> ( phys->momentCentroid[0] ) <<" "<< boost::lexical_cast<string> ( phys->momentCentroid[1] ) <<" "<< boost::lexical_cast<string> ( phys->momentCentroid[2] ) <<" "<< boost::lexical_cast<string> ( phys->momentMagnitude ) << " " << boost::lexical_cast<string> ( phys->clusterInts.size() ) << " " << boost::lexical_cast<string> ( phys->eventNumber ) << " " << boost::lexical_cast<string> (scene->time) << " " << boost::lexical_cast<string> (phys->eventBeginTime) << endl; momentsFileExist=true; } } /* Determination of interaction */ if (D < 0) { //tensile configuration if ( !phys->isCohesive) { if (!neverErase) return false; else { phys->shearForce = Vector3r::Zero(); phys->normalForce = Vector3r::Zero(); phys->isCohesive =0; phys->FnMax = 0; phys->FsMax = 0; return true; } } if ( phys->isCohesive && (phys->FnMax>0) && (std::abs(D)>Dtensile) ) { nbTensCracks++; phys->isCohesive = 0; phys->FnMax = 0; phys->FsMax = 0; /// Do we need both the following lines? phys->breakOccurred = true; // flag to trigger remesh for DFNFlowEngine phys->isBroken = true; // flag for DFNFlowEngine // update body state with the number of broken bonds -> do we really need that? JCFpmState* st1=dynamic_cast<JCFpmState*>(b1->state.get()); JCFpmState* st2=dynamic_cast<JCFpmState*>(b2->state.get()); st1->nbBrokenBonds++; st2->nbBrokenBonds++; st1->damageIndex+=1.0/st1->nbInitBonds; st2->damageIndex+=1.0/st2->nbInitBonds; phys->momentBroken = true; Real scalarNF=phys->normalForce.norm(); Real scalarSF=phys->shearForce.norm(); totalTensCracksE+=0.5*( ((scalarNF*scalarNF)/phys->kn) + ((scalarSF*scalarSF)/phys->ks) ); totalCracksSurface += phys->crossSection; if (recordCracks){ std::ofstream file (fileCracks.c_str(), !cracksFileExist ? std::ios::trunc : std::ios::app); if(file.tellp()==0){ file <<"iter time p0 p1 p2 type size norm0 norm1 norm2 nrg onJnt"<<endl; } Vector3r crackNormal=Vector3r::Zero(); if ((smoothJoint) && (phys->isOnJoint)) { crackNormal=phys->jointNormal; } else {crackNormal=geom->normal;} file << boost::lexical_cast<string> ( scene->iter ) << " " << boost::lexical_cast<string> ( scene->time ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[0] ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[1] ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[2] ) <<" "<< 1 <<" "<< boost::lexical_cast<string> ( 0.5*(geom->radius1+geom->radius2) ) <<" "<< boost::lexical_cast<string> ( crackNormal[0] ) <<" "<< boost::lexical_cast<string> ( crackNormal[1] ) <<" "<< boost::lexical_cast<string> ( crackNormal[2] ) <<" "<< boost::lexical_cast<string> ( 0.5*( ((scalarNF*scalarNF)/phys->kn) + ((scalarSF*scalarSF)/phys->ks) ) ) <<" "<< boost::lexical_cast<string> ( phys->isOnJoint ) << endl; } if (recordMoments && !phys->momentCalculated){ checkForCluster(phys, geom, b1, b2, contact); clusterInteractions(phys, contact); computeTemporalWindow(phys, b1, b2); } cracksFileExist=true; if (!neverErase) return false; else { phys->shearForce = Vector3r::Zero(); phys->normalForce = Vector3r::Zero(); return true; } } } /* NormalForce */ Real Fn = 0; Fn = phys->kn*D; /* ShearForce */ Vector3r& shearForce = phys->shearForce; Real jointSliding=0; if ((smoothJoint) && (phys->isOnJoint)) { /// incremental formulation (OK?) Vector3r relativeVelocity = (b2->state->vel - b1->state->vel); // angVel are not taken into account as particles on joint don't rotate ???? Vector3r slidingVelocity = relativeVelocity - phys->jointNormal.dot(relativeVelocity)*phys->jointNormal; Vector3r incrementalSliding = slidingVelocity*scene->dt; shearForce -= phys->ks*incrementalSliding; jointSliding = incrementalSliding.norm(); phys->jointCumulativeSliding += jointSliding; } else { shearForce = geom->rotate(phys->shearForce); const Vector3r& incrementalShear = geom->shearIncrement(); shearForce -= phys->ks*incrementalShear; } /* Mohr-Coulomb criterion */ Real maxFs = phys->FsMax + Fn*phys->tanFrictionAngle; Real scalarShearForce = shearForce.norm(); if (scalarShearForce > maxFs) { if (scalarShearForce != 0) shearForce*=maxFs/scalarShearForce; else shearForce=Vector3r::Zero(); if ((smoothJoint) && (phys->isOnJoint)) {phys->dilation=phys->jointCumulativeSliding*phys->tanDilationAngle-D; phys->initD+=(jointSliding*phys->tanDilationAngle);} // if (!phys->isCohesive) { // nbSlips++; // totalSlipE+=((1./phys->ks)*(trialForce-shearForce))/*plastic disp*/.dot(shearForce)/*active force*/; // // if ( (recordSlips) && (maxFs!=0) ) { // std::ofstream file (fileCracks.c_str(), !cracksFileExist ? std::ios::trunc : std::ios::app); // if(file.tellp()==0){ file <<"iter time p0 p1 p2 type size norm0 norm1 norm2 nrg"<<endl; } // Vector3r crackNormal=Vector3r::Zero(); // if ((smoothJoint) && (phys->isOnJoint)) { crackNormal=phys->jointNormal; } else {crackNormal=geom->normal;} // file << boost::lexical_cast<string> ( scene->iter ) <<" " << boost::lexical_cast<string> ( scene->time ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[0] ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[1] ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[2] ) <<" "<< 0 <<" "<< boost::lexical_cast<string> ( 0.5*(geom->radius1+geom->radius2) ) <<" "<< boost::lexical_cast<string> ( crackNormal[0] ) <<" "<< boost::lexical_cast<string> ( crackNormal[1] ) <<" "<< boost::lexical_cast<string> ( crackNormal[2] ) <<" "<< boost::lexical_cast<string> ( ((1./phys->ks)*(trialForce-shearForce)).dot(shearForce) ) << endl; // } // cracksFileExist=true; // } if ( phys->isCohesive ) { nbShearCracks++; phys->isCohesive = 0; phys->FnMax = 0; phys->FsMax = 0; /// Do we need both the following lines? phys->breakOccurred = true; // flag to trigger remesh for DFNFlowEngine phys->isBroken = true; // flag for DFNFlowEngine phys->momentBroken = true; // update body state with the number of broken bonds -> do we really need that? JCFpmState* st1=dynamic_cast<JCFpmState*>(b1->state.get()); JCFpmState* st2=dynamic_cast<JCFpmState*>(b2->state.get()); st1->nbBrokenBonds++; st2->nbBrokenBonds++; st1->damageIndex+=1.0/st1->nbInitBonds; st2->damageIndex+=1.0/st2->nbInitBonds; Real scalarNF=phys->normalForce.norm(); Real scalarSF=phys->shearForce.norm(); totalShearCracksE+=0.5*( ((scalarNF*scalarNF)/phys->kn) + ((scalarSF*scalarSF)/phys->ks) ); totalCracksSurface += phys->crossSection; if (recordCracks){ std::ofstream file (fileCracks.c_str(), !cracksFileExist ? std::ios::trunc : std::ios::app); if(file.tellp()==0){ file <<"iter time p0 p1 p2 type size norm0 norm1 norm2 nrg onJnt"<<endl; } Vector3r crackNormal=Vector3r::Zero(); if ((smoothJoint) && (phys->isOnJoint)) { crackNormal=phys->jointNormal; } else {crackNormal=geom->normal;} file << boost::lexical_cast<string> ( scene->iter ) << " " << boost::lexical_cast<string> ( scene->time ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[0] ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[1] ) <<" "<< boost::lexical_cast<string> ( geom->contactPoint[2] ) <<" "<< 2 <<" "<< boost::lexical_cast<string> ( 0.5*(geom->radius1+geom->radius2) ) <<" "<< boost::lexical_cast<string> ( crackNormal[0] ) <<" "<< boost::lexical_cast<string> ( crackNormal[1] ) <<" "<< boost::lexical_cast<string> ( crackNormal[2] ) <<" "<< boost::lexical_cast<string> ( 0.5*( ((scalarNF*scalarNF)/phys->kn) + ((scalarSF*scalarSF)/phys->ks) ) ) <<" "<< boost::lexical_cast<string> ( phys->isOnJoint ) << endl; } cracksFileExist=true; // // option 1: delete contact whatsoever (if in compression, it will be detected as a new contact at the next timestep -> actually, not necesarily because of the near neighbour interaction: there could be a gap between the bonded particles and thus a broken contact may not be frictional at the next timestep if the detection is done for strictly contacting particles...) -> to TEST // if (!neverErase) return false; // else { // phys->shearForce = Vector3r::Zero(); // phys->normalForce = Vector3r::Zero(); // return true; // } if (recordMoments && !phys->momentCalculated){ checkForCluster(phys, geom, b1, b2, contact); clusterInteractions(phys, contact); computeTemporalWindow(phys, b1, b2); } // option 2: delete contact if in tension // shearForce *= Fn*phys->tanFrictionAngle/scalarShearForce; // now or at the next timestep? should not be very different -> to TEST if ( D < 0 ) { // spheres do not touch if (!neverErase) return false; else { phys->shearForce = Vector3r::Zero(); phys->normalForce = Vector3r::Zero(); return true; } } } } /* Apply forces */ if ((smoothJoint) && (phys->isOnJoint)) { phys->normalForce = Fn*phys->jointNormal; } else { phys->normalForce = Fn*geom->normal; } Vector3r f = phys->normalForce + shearForce; /// applyForceAtContactPoint computes torque also and, for now, we don't want rotation for particles on joint (some errors in calculation due to specific geometry) //applyForceAtContactPoint(f, geom->contactPoint, I->getId2(), b2->state->pos, I->getId1(), b1->state->pos, scene); scene->forces.addForce (id1,-f); scene->forces.addForce (id2, f); // simple solution to avoid torque computation for particles interacting on a smooth joint if ( (phys->isOnJoint)&&(smoothJoint) ) return true; /// those lines are needed if rootBody->forces.addForce and rootBody->forces.addMoment are used instead of applyForceAtContactPoint -> NOTE need to check for accuracy!!! scene->forces.addTorque(id1,(geom->radius1-0.5*geom->penetrationDepth)* geom->normal.cross(-f)); scene->forces.addTorque(id2,(geom->radius2-0.5*geom->penetrationDepth)* geom->normal.cross(-f)); return true; }
//! 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; }
bool PositionBasedElasticRod::ProjectEdgeConstraints( const Vector3r& pA, const float wA, const Vector3r& pB, const float wB, const Vector3r& pG, const float wG, const float edgeKs, const float edgeRestLength, const float ghostEdgeRestLength, Vector3r& corrA, Vector3r& corrB, Vector3r& corrC) { corrA.setZero(); corrB.setZero(); corrC.setZero(); //Edge distance constraint Vector3r dir = pA - pB; float len = dir.norm(); float wSum = wA + wB; if (len > EPSILON && wSum > EPSILON) { Vector3r dP = (1.0f / wSum) * (len - edgeRestLength) * (dir / len) * edgeKs; corrA -= dP * wA; corrB += dP * wB; corrC = Vector3r(0, 0, 0); } //Bisector constraint Vector3r pm = 0.5f * (pA + pB); Vector3r p0p2 = pA - pG; Vector3r p2p1 = pG - pB; Vector3r p1p0 = pB - pA; Vector3r p2pm = pG - pm; float lambda; wSum = wA * p0p2.squaredNorm() + wB * p2p1.squaredNorm() + wG * p1p0.squaredNorm(); if (wSum > EPSILON) { lambda = p2pm.dot(p1p0) / wSum * edgeKs; corrA -= p0p2 * lambda * wA; corrB -= p2p1 * lambda * wB; corrC -= p1p0 * lambda * wG; } ////Ghost-Edge constraint wSum = 0.25f * wA + 0.25f * wB + 1.0f * wG; if (wSum > EPSILON) { //need to use updated positions pm = 0.5f * (pA + corrA + pB + corrB); p2pm = pG + corrC - pm; float p2pm_mag = p2pm.norm(); p2pm *= 1.0f / p2pm_mag; lambda = (p2pm_mag - ghostEdgeRestLength) / wSum * edgeKs; corrA += 0.5f * wA * lambda * p2pm; corrB += 0.5f * wB * lambda * p2pm; corrC -= 1.0f * wG * lambda * p2pm; } return true; }