bool operator()(const Vector3r& pt, Real pad=0.) const {
Real u=(pt.dot(c12)-c1.dot(c12))/(ht*ht); // normalized coordinate along the c1--c2 axis
if((u*ht<0+pad) || (u*ht>ht-pad)) return false; // out of cylinder along the axis
Real axisDist=((pt-c1).cross(pt-c2)).norm()/ht;
if(axisDist>radius-pad) return false;
return true;
}
// http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
Matrix3r woo::Volumetric::triangleInertia(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2){
Matrix3r V; V<<v0.transpose(),v1.transpose(),v2.transpose(); // rows!
Real a=((v1-v0).cross(v2-v0)).norm(); // twice the triangle area
Matrix3r S; S<<2,1,1, 1,2,1, 1,1,2; S*=(1/24.);
Matrix3r C=a*V.transpose()*S*V;
return Matrix3r::Identity()*C.trace()-C;
};
/*
The following code is based on GPL-licensed K-3d importer module from
https://github.com/K-3D/k3d/blob/master/modules/stl_io/mesh_reader.cpp
TODO: read color/material, convert to scalar color in Woo
*/
vector<shared_ptr<Particle>> DemFuncs::importSTL(const string& filename, const shared_ptr<Material>& mat, int mask, Real color, Real scale, const Vector3r& shift, const Quaternionr& ori, Real threshold, Real maxBox, bool readColors, bool flex, Real thickness){
vector<shared_ptr<Particle>> ret;
std::ifstream in(filename,std::ios::in|std::ios::binary);
if(!in) throw std::runtime_error("Error opening "+filename+" for reading (STL import).");
char buffer[80]; in.read(buffer, 80); in.seekg(0, std::ios::beg);
bool isAscii=boost::algorithm::starts_with(buffer,"solid");
// linear array of vertices, each triplet is one face
// this is filled from ASCII and binary formats as intermediary representation
// coordinate system change using (ori, scale, shift) is done already when reading vertices from the file
vector<Vector3r> vertices;
if(isAscii){
LOG_TRACE("STL: ascii format detected");
string lineBuf;
long lineNo=-1;
int fVertsNum=0; // number of vertices in this facet (for checking)
for(std::getline(in,lineBuf); in; getline(in,lineBuf)){
lineNo++;
string tok;
std::istringstream line(lineBuf);
line>>tok;
if(tok=="facet"){
string tok2; line>>tok2;
if(tok2!="normal") LOG_WARN("STL: 'normal' expected after 'facet' (line "+to_string(lineNo)+")");
// we ignore normal values:
// Vector3r normal; line>>normal.x(); line>>normal.y(); line>>normal.z();
} else if(tok=="vertex"){
Vector3r p; line>>p.x(); line>>p.y(); line>>p.z();
vertices.push_back(ori*(p*scale+shift));
fVertsNum++;
} else if(tok=="endfacet"){
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;
}
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);
}
// WARN: this is not accurate, since padding is taken as perpendicular to the axis, not the the surface
bool operator()(const Vector3r& pt, Real pad=0.) const {
Real v=(pt.dot(c12)-c1.dot(c12))/(ht*ht); // normalized coordinate along the c1--c2 axis
if((v*ht<0+pad) || (v*ht>ht-pad)) return false; // out of cylinder along the axis
Real u=(v-.5)*ht/c; // u from the wolfram parametrization; u is 0 in the center
Real rHere=a*sqrt(1+u*u); // pad is taken perpendicular to the axis, not to the surface (inaccurate)
Real axisDist=((pt-c1).cross(pt-c2)).norm()/ht;
if(axisDist>rHere-pad) return false;
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;
}
bool operator()(const Vector3r& pt, Real pad=0.) const {
Real distUp=normal.dot(pt-c)-aperture/2, distDown=-normal.dot(pt-c)-aperture/2, distInPlane=-inside.dot(pt-c);
// LOG_DEBUG("pt="<<pt<<", distUp="<<distUp<<", distDown="<<distDown<<", distInPlane="<<distInPlane);
if(distInPlane>=pad) return true;
if(distUp >=pad) return true;
if(distDown >=pad) return true;
if(distInPlane<0) return false;
if(distUp >0) return sqrt(pow(distInPlane,2)+pow(distUp,2))>=pad;
if(distDown>0) return sqrt(pow(distInPlane,2)+pow(distUp,2))>=pad;
// between both notch planes, closer to the edge than pad (distInPlane<pad)
return false;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::computePBFLagrangeMultiplier(
const unsigned int particleIndex,
const unsigned int numberOfParticles,
const Vector3r x[],
const Real mass[],
const Vector3r boundaryX[],
const Real boundaryPsi[],
const Real density,
const unsigned int numNeighbors,
const unsigned int neighbors[],
const Real density0,
const bool boundaryHandling,
Real &lambda)
{
const Real eps = 1.0e-6;
// Evaluate constraint function
const Real C = std::max(density / density0 - 1.0, 0.0); // clamp to prevent particle clumping at surface
if (C != 0.0)
{
// Compute gradients dC/dx_j
Real sum_grad_C2 = 0.0;
Vector3r gradC_i(0.0, 0.0, 0.0);
for (unsigned int j = 0; j < numNeighbors; j++)
{
const unsigned int neighborIndex = neighbors[j];
if (neighborIndex < numberOfParticles) // Test if fluid particle
{
const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
sum_grad_C2 += gradC_j.squaredNorm();
gradC_i -= gradC_j;
}
else if (boundaryHandling)
{
// Boundary: Akinci2012
const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
sum_grad_C2 += gradC_j.squaredNorm();
gradC_i -= gradC_j;
}
}
sum_grad_C2 += gradC_i.squaredNorm();
// Compute lambda
lambda = -C / (sum_grad_C2 + eps);
}
else
lambda = 0.0;
return true;
}
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
}
bool Law2_ScGeom_ElectrostaticPhys::go(shared_ptr<IGeom>& iGeom, shared_ptr<IPhys>& iPhys, Interaction* interaction)
{
// Physic
ElectrostaticPhys* phys=static_cast<ElectrostaticPhys*>(iPhys.get());
// Geometry
ScGeom* geom=static_cast<ScGeom*>(iGeom.get());
// Get bodies properties
Body::id_t id1 = interaction->getId1();
Body::id_t id2 = interaction->getId2();
const shared_ptr<Body> b1 = Body::byId(id1,scene);
const shared_ptr<Body> b2 = Body::byId(id2,scene);
State* s1 = b1->state.get();
State* s2 = b2->state.get();
// geometric parameters
Real a((geom->radius1+geom->radius2)/2.);
bool isNew=false;
// Speeds
Vector3r shiftVel=scene->isPeriodic ? Vector3r(scene->cell->velGrad*scene->cell->hSize*interaction->cellDist.cast<Real>()) : Vector3r::Zero();
Vector3r shift2 = scene->isPeriodic ? Vector3r(scene->cell->hSize*interaction->cellDist.cast<Real>()): Vector3r::Zero();
Vector3r relV = geom->getIncidentVel(s1, s2, scene->dt, shift2, shiftVel, false );
Real undot = relV.dot(geom->normal); // Normal velocity norm
if(-geom->penetrationDepth > a && -geom->penetrationDepth > 10.*phys->DebyeLength) { return false; }
// inititalization
if(phys->u == -1. ) {phys->u = -geom->penetrationDepth; isNew=true;}
// Solve normal
normalForce_DL_Adim(phys,geom, undot,isNew);
// Solve shear and torques
Vector3r C1 = Vector3r::Zero();
Vector3r C2 = Vector3r::Zero();
computeShearForceAndTorques_log(phys, geom, s1, s2, C1, C2);
// Apply!
scene->forces.addForce(id1,phys->normalForce+phys->shearForce);
scene->forces.addTorque(id1,C1);
scene->forces.addForce(id2,-(phys->normalForce+phys->shearForce));
scene->forces.addTorque(id2,C2);
return true;
}
inParallelepiped(const Vector3r& o, const Vector3r& a, const Vector3r& b, const Vector3r& c){
Vector3r A(o), B(a), C(a+(b-o)), D(b), E(c), F(c+(a-o)), G(c+(a-o)+(b-o)), H(c+(b-o));
Vector3r x(B-A), y(D-A), z(E-A);
n[0]=-y.cross(z).normalized(); n[1]=-n[0]; pts[0]=A; pts[1]=B;
n[2]=-z.cross(x).normalized(); n[3]=-n[2]; pts[2]=A; pts[3]=D;
n[4]=-x.cross(y).normalized(); n[5]=-n[4]; pts[4]=A; pts[5]=E;
// bounding box
Vector3r vertices[8]={A,B,C,D,E,F,G,H};
mn=mx=vertices[0];
for(int i=1; i<8; i++){
mn=mn.cwiseMin(vertices[i]);
mx=mx.cwiseMax(vertices[i]);
}
}
string GenerateCloud_water(vector<BasicSphere>& sphere_list, Vector3r lowerCorner, Vector3r upperCorner, long number, Real rad_std_dev, Real porosity)
{
typedef boost::minstd_rand StdGenerator;
static StdGenerator generator;
static boost::variate_generator<StdGenerator&, boost::uniform_real<> >
random1(generator, boost::uniform_real<>(0,1));
// static boost::variate_generator<StdGenerator&, boost::normal_distribution<> >
// randomN(generator, boost::normal_distribution<>(aggregateMeanRadius,aggregateSigmaRadius));
sphere_list.clear();
long tries = 1000; //nb of tries for positionning the next sphere
Vector3r dimensions = upperCorner - lowerCorner;
Real mean_radius = std::pow(dimensions.x()*dimensions.y()*dimensions.z()*(1-porosity)/(3.1416*1.3333*number),0.333333);
//cerr << mean_radius;
Real Rmin=mean_radius, Rmax=mean_radius;
std::cerr << "generating aggregates ... ";
long t, i;
for (i=0; i<number; ++i) {
BasicSphere s;
s.second = (random1()-0.5)*rad_std_dev*mean_radius+mean_radius;
for (t=0; t<tries; ++t) {
s.first.x() = lowerCorner.x()+s.second+(dimensions.x()-2*s.second)*random1();
s.first.y() = lowerCorner.y()+s.second+(dimensions.y()-2*s.second)*random1();
s.first.z() = lowerCorner.z()+s.second+(dimensions.z()-2*s.second)*random1();
bool overlap=false;
for (long j=0; (j<i && !overlap); j++)
if ( pow(sphere_list[j].second+s.second, 2) > (sphere_list[j].first-s.first).squaredNorm()) overlap=true;
if (!overlap){
sphere_list.push_back(s);
Rmin = std::min(Rmin,s.second);
Rmax = std::max(Rmax,s.second);
break;}
}
if (t==tries) return "More than " + lexical_cast<string>(tries) +
" tries while generating sphere number " +
lexical_cast<string>(i+1) + "/" + lexical_cast<string>(number) + ".";
}
return "Generated a sample with " + lexical_cast<string>(number) + "spheres inside box of dimensions: ("
+ lexical_cast<string>(dimensions[0]) + ","
+ lexical_cast<string>(dimensions[1]) + ","
+ lexical_cast<string>(dimensions[2]) + ")."
+ " mean radius=" + lexical_cast<string>(mean_radius) +
+ " Rmin =" + lexical_cast<string>(Rmin) +
+ " Rmax =" + lexical_cast<string>(Rmax) + ".";
}
void SimpleShear::createSphere(shared_ptr<Body>& body, Vector3r position, Real radius)
{
body = shared_ptr<Body>(new Body); body->groupMask=1;
shared_ptr<NormalInelasticMat> mat(new NormalInelasticMat);
shared_ptr<Aabb> aabb(new Aabb);
shared_ptr<Sphere> iSphere(new Sphere);
body->state->pos =position;
body->state->ori =Quaternionr::Identity();
body->state->vel =Vector3r(0,0,0);
body->state->angVel =Vector3r(0,0,0);
Real masse =4.0/3.0*Mathr::PI*radius*radius*radius*density;
body->state->mass =masse;
body->state->inertia = Vector3r(2.0/5.0*masse*radius*radius,2.0/5.0*masse*radius*radius,2.0/5.0*masse*radius*radius);
mat->young = sphereYoungModulus;
mat->poisson = spherePoissonRatio;
mat->frictionAngle = sphereFrictionDeg * Mathr::PI/180.0;
body->material = mat;
aabb->color = Vector3r(0,1,0);
iSphere->radius = radius;
iSphere->color = ((int)(floor(8*position.x()/length)))%2?Vector3r(0.7,0.7,0.7):Vector3r(0.45,0.45,0.45);// so that we have eight different colour bands
body->shape = iSphere;
body->bound = aabb;
}
// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::solveDensityConstraint(
const unsigned int particleIndex,
const unsigned int numberOfParticles,
const Vector3r x[],
const Real mass[],
const Vector3r boundaryX[],
const Real boundaryPsi[],
const unsigned int numNeighbors,
const unsigned int neighbors[],
const Real density0,
const bool boundaryHandling,
const Real lambda[],
Vector3r &corr)
{
// Compute position correction
corr.setZero();
for (unsigned int j = 0; j < numNeighbors; j++)
{
const unsigned int neighborIndex = neighbors[j];
if (neighborIndex < numberOfParticles) // Test if fluid particle
{
const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
corr -= (lambda[particleIndex] + lambda[neighborIndex]) * gradC_j;
}
else if (boundaryHandling)
{
// Boundary: Akinci2012
const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
corr -= (lambda[particleIndex]) * gradC_j;
}
}
return true;
}
string SimpleShear::GenerateCloud(vector<BasicSphere>& sphere_list,Vector3r lowerCorner,Vector3r upperCorner,long number,Real rad_std_dev, Real porosity)
{
sphere_list.clear();
long tries = 1000; //nb max of tries for positionning the next sphere
Vector3r dimensions = upperCorner - lowerCorner;
Real mean_radius = pow(dimensions.x()*dimensions.y()*dimensions.z()*(1-porosity)/(4.0/3.0*Mathr::PI*number),1.0/3.0);
cerr << " mean radius " << mean_radius << endl;;
// std::cerr << "generating aggregates ... ";
long t, i;
for (i=0; i<number; ++i)
{
BasicSphere s;
for (t=0; t<tries; ++t)
{
s.second = (Mathr::UnitRandom()-0.5)*rad_std_dev*mean_radius+mean_radius;
s.first.x() = lowerCorner.x()+s.second+(dimensions.x()-2*s.second)*Mathr::UnitRandom();
s.first.y() = lowerCorner.y()+s.second+(dimensions.y()-2*s.second)*Mathr::UnitRandom();
s.first.z() = lowerCorner.z()+s.second+(dimensions.z()-2*s.second)*Mathr::UnitRandom();
bool overlap=false;
for (long j=0; (j<i && !overlap); j++)
if ( pow(sphere_list[j].second+s.second, 2) > (sphere_list[j].first-s.first).squaredNorm()) overlap=true;
if (!overlap)
{
sphere_list.push_back(s);
// cout << "j'ai bien rajoute une sphere dans la liste" << endl;
break;
}
}
if (t==tries)
{
string str1="Generated a sample with " + boost::lexical_cast<string>(i) + " spheres inside box of dimensions: ("
+ boost::lexical_cast<string>(dimensions[0]) + ","
+ boost::lexical_cast<string>(dimensions[1]) + ","
+ boost::lexical_cast<string>(dimensions[2]) + ").\n";
return str1 + "More than " + boost::lexical_cast<string>(tries) + " tries while generating sphere number " +
boost::lexical_cast<string>(i+1) + "/" + boost::lexical_cast<string>(number) + ".";
}
}
return "Generated a sample with " + boost::lexical_cast<string>(number) + " spheres inside box of dimensions: ("
+ boost::lexical_cast<string>(dimensions[0]) + ","
+ boost::lexical_cast<string>(dimensions[1]) + ","
+ boost::lexical_cast<string>(dimensions[2]) + ").";
}
void NewtonIntegrator::updateEnergy(const shared_ptr<Body>& b, const State* state, const Vector3r& fluctVel, const Vector3r& f, const Vector3r& m){
assert(b->isStandalone() || b->isClump());
// always positive dissipation, by-component: |F_i|*|v_i|*damping*dt (|T_i|*|ω_i|*damping*dt for rotations)
if(damping!=0. && state->isDamped){
scene->energy->add(fluctVel.cwise().abs().dot(f.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,/*non-incremental*/false);
// when the aspherical integrator is used, torque is damped instead of ang acceleration; this code is only approximate
scene->energy->add(state->angVel.cwise().abs().dot(m.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,false);
}
// kinetic energy
Real Etrans=.5*state->mass*fluctVel.squaredNorm();
Real Erot;
// rotational terms
if(b->isAspherical()){
Matrix3r mI; mI<<state->inertia[0],0,0, 0,state->inertia[1],0, 0,0,state->inertia[2];
Matrix3r T(state->ori);
Erot=.5*b->state->angVel.transpose().dot((T.transpose()*mI*T)*b->state->angVel);
} else { Erot=0.5*state->angVel.dot(state->inertia.cwise()*state->angVel); }
if(!kinSplit) scene->energy->add(Etrans+Erot,"kinetic",kinEnergyIx,/*non-incremental*/true);
else{ scene->energy->add(Etrans,"kinTrans",kinEnergyTransIx,true); scene->energy->add(Erot,"kinRot",kinEnergyRotIx,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 _gts_face_to_stl(GtsTriangle* t,_gts_face_to_stl_data* data){
GtsVertex* v[3];
Vector3r n;
gts_triangle_vertices(t,&v[0],&v[1],&v[2]);
if(data->clipCell && data->scene->isPeriodic){
for(short ax:{0,1,2}){
Vector3r p(GTS_POINT(v[ax])->x,GTS_POINT(v[ax])->y,GTS_POINT(v[ax])->z);
if(!data->scene->cell->isCanonical(p)) return;
}
}
gts_triangle_normal(t,&n[0],&n[1],&n[2]);
n.normalize();
std::ofstream& stl(data->stl);
stl<<" facet normal "<<n[0]<<" "<<n[1]<<" "<<n[2]<<"\n";
stl<<" outer loop\n";
for(GtsVertex* _v: v){
stl<<" vertex "<<GTS_POINT(_v)->x<<" "<<GTS_POINT(_v)->y<<" "<<GTS_POINT(_v)->z<<"\n";
}
stl<<" endloop\n";
stl<<" endfacet\n";
data->numTri+=3;
}
std::tuple<vector<shared_ptr<Node>>,vector<shared_ptr<Particle>>> SphereClumpGeom::makeParticles(const shared_ptr<Material>& mat, const Vector3r& clumpPos, const Quaternionr& clumpOri, int mask, Real scale){
ensureOk();
assert(centers.size()==radii.size());
const auto N=centers.size();
if(N==1){ // fast path for a single sphere (do not clump at all)
auto s=DemFuncs::makeSphere(radii[0]*scale,mat);
s->mask=mask;
s->shape->nodes[0]->pos=(isnan(clumpPos.maxCoeff())?centers[0]:clumpPos); // natural or forced position
return std::make_tuple(vector<shared_ptr<Node>>({s->shape->nodes[0]}),vector<shared_ptr<Particle>>({s}));
}
vector<shared_ptr<Particle>> par(N);
auto n=make_shared<Node>();
auto cd=make_shared<ClumpData>();
n->setData<DemData>(cd);
n->pos=(isnan(clumpPos.maxCoeff())?pos:clumpPos);
n->ori=clumpOri;
cd->nodes.resize(N);
cd->relPos.resize(N);
cd->relOri.resize(N);
for(size_t i=0; i<N; i++){
par[i]=DemFuncs::makeSphere(radii[i]*scale,mat);
par[i]->mask=mask;
cd->nodes[i]=par[i]->shape->nodes[0];
cd->nodes[i]->getData<DemData>().setClumped(n); // sets flag and assigned master node
cd->relPos[i]=(centers[i]-pos)*scale;
cd->relOri[i]=ori.conjugate(); // nice to set, but not really important
}
// sets particles in global space based on relPos, relOri
ClumpData::applyToMembers(n);
// set clump properties
assert(!isnan(volume));
cd->setClump(); assert(cd->isClump());
// scale = length scale (but not density scale)
cd->mass=mat->density*volume*pow(scale,3);
cd->inertia=mat->density*inertia*pow(scale,5);
cd->equivRad=equivRad;
return std::make_tuple(vector<shared_ptr<Node>>({n}),par);
}
void Leapfrog::leapfrogAsphericalRotate(const shared_ptr<Node>& node, const Vector3r& M){
Quaternionr& ori(node->ori); DemData& dyn(node->getData<DemData>());
Vector3r& angMom(dyn.angMom); Vector3r& angVel(dyn.angVel); const Vector3r& inertia(dyn.inertia);
// initialize angular momentum; it is in local coordinates, therefore angVel must be rotated
if(isnan(angMom.minCoeff())){
angMom=dyn.inertia.asDiagonal()*node->ori.conjugate()*dyn.angVel;
}
Matrix3r A=ori.conjugate().toRotationMatrix(); // rotation matrix from global to local r.f.
const Vector3r l_n = angMom + dt/2 * M; // global angular momentum at time n
const Vector3r l_b_n = A*l_n; // local angular momentum at time n
const Vector3r angVel_b_n = (l_b_n.array()/inertia.array()).matrix(); // local angular velocity at time n
const Quaternionr dotQ_n=DotQ(angVel_b_n,ori); // dQ/dt at time n
const Quaternionr Q_half = ori + dt/2 * dotQ_n; // Q at time n+1/2
angMom+=dt*M; // global angular momentum at time n+1/2
const Vector3r l_b_half = A*angMom; // local angular momentum at time n+1/2
Vector3r angVel_b_half = (l_b_half.array()/inertia.array()).matrix(); // local angular velocity at time n+1/2
const Quaternionr dotQ_half=DotQ(angVel_b_half,Q_half); // dQ/dt at time n+1/2
ori=ori+dt*dotQ_half; // Q at time n+1
angVel=ori*angVel_b_half; // global angular velocity at time n+1/2
ori.normalize();
}
//float/vec parameters can have NaN which makes them optional
void SafetyEval::setSafety(SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_strategy,
const Vector3r& origin, float xy_length, float max_z, float min_z)
{
if (!origin.hasNaN() && !std::isnan(xy_length) && !std::isnan(max_z) && !std::isnan(min_z))
fence_ptr_->setBoundry(origin, xy_length, max_z, min_z);
if (!std::isnan(obs_clearance))
vehicle_params_.obs_clearance = obs_clearance;
enable_reasons_ = enable_reasons;
setObsAvoidanceStrategy(obs_strategy);
Utils::log(Utils::stringf("enable_reasons: %X", uint(enable_reasons)));
}
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();
}
void PolyhedraSplitter::action()
{
const shared_ptr<Scene> _rb=shared_ptr<Scene>();
shared_ptr<Scene> rb=(_rb?_rb:Omega::instance().getScene());
vector<PSplitT> splitsV;
vector<Matrix3r> bStresses (scene->bodies->size(), Matrix3r::Zero());
getStressForEachBody(bStresses);
FOREACH(const shared_ptr<Body>& b, *rb->bodies){
if(!b || !b->material || !b->shape) continue;
shared_ptr<Polyhedra> p=YADE_PTR_DYN_CAST<Polyhedra>(b->shape);
shared_ptr<PolyhedraMat> m=YADE_PTR_DYN_CAST<PolyhedraMat>(b->material);
if(p && m->IsSplitable){
//not real strees, to get real one, it has to be divided by body volume
Matrix3r stress = bStresses[b->id];
//get eigenstresses
Symmetrize(stress);
Matrix3r I_vect(Matrix3r::Zero()), I_valu(Matrix3r::Zero());
matrixEigenDecomposition(stress,I_vect,I_valu);
Eigen::Matrix3f::Index min_i, max_i;
I_valu.diagonal().minCoeff(&min_i);
I_valu.diagonal().maxCoeff(&max_i);
//division of stress by volume
const Vector3r dirC = I_vect.col(max_i);
const Vector3r dirT = I_vect.col(min_i);
const Vector3r dir1 = dirC.normalized() + dirT.normalized();
const Vector3r dir2 = dirC.normalized() - dirT.normalized();
//double sigma_t = -comp_stress/2.+ tens_stress;
const Real sigma_t = pow((
pow(I_valu(0,0)-I_valu(1,1),2)+
pow(I_valu(0,0)-I_valu(2,2),2)+
pow(I_valu(1,1)-I_valu(2,2),2))
/2.,0.5)/p->GetVolume();
if (sigma_t > getStrength(p->GetVolume(),m->GetStrength())) {
splitsV.push_back(std::make_tuple(b, dir1.normalized(), dir2.normalized()));
}
}
}
std::for_each(splitsV.begin(), splitsV.end(), &SplitPolyhedraDouble);
}
size_t DemFuncs::radialAxialForce(const Scene* scene, const DemField* dem, int mask, Vector3r axis, bool shear, Vector2r& radAxF){
size_t ret=0;
radAxF=Vector2r::Zero();
axis.normalize();
for(const shared_ptr<Particle>& p: *dem->particles){
if(!(p->mask & mask) || !p->shape) continue;
ret++;
for(const auto& idC: p->contacts){
const shared_ptr<Contact>& C(idC.second);
if(!C->isReal()) continue;
Vector3r F=C->geom->node->ori*((shear?C->phys->force:Vector3r(C->phys->force[0],0,0))*C->forceSign(p));
Vector3r axF=F.dot(axis);
radAxF+=Vector2r(axF,(F-axF).norm());
}
}
return ret;
}
msr::airlib::VehicleCameraBase::ImageResponse to() const
{
msr::airlib::VehicleCameraBase::ImageResponse d;
d.pixels_as_float = pixels_as_float;
if (! pixels_as_float)
d.image_data_uint8 = image_data_uint8;
else
d.image_data_float = image_data_float;
d.camera_position = camera_position.to();
d.camera_orientation = camera_orientation.to();
d.time_stamp = time_stamp;
d.message = message;
d.compress = compress;
d.width = width;
d.height = height;
d.image_type = image_type;
return d;
}
//! 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 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();
}
inline
real OctreeBound::getSize() const
{
return positionOfUpperCorner_m.getX() - positionOfLowerCorner_m.getX();
}
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;
}
};
