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