C++ Vector3r::normalize方法代码示例

	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;
	}
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........