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