C++ Polyhedron::planes_begin方法代码示例

//**********************************************************************************
//test of polyhedron intersection callable from python shell
bool do_Polyhedras_Intersect(const shared_ptr<Shape>& cm1,const shared_ptr<Shape>& cm2,const State& state1,const State& state2){

	const Se3r& se31=state1.se3; 
	const Se3r& se32=state2.se3;
	Polyhedra* A = static_cast<Polyhedra*>(cm1.get());		
	Polyhedra* B = static_cast<Polyhedra*>(cm2.get());

	//move and rotate 1st the CGAL structure Polyhedron
	Matrix3r rot_mat = (se31.orientation).toRotationMatrix();
	Vector3r trans_vec = se31.position;
	Transformation t_rot_trans(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
	Polyhedron PA = A->GetPolyhedron();
	std::transform( PA.points_begin(), PA.points_end(), PA.points_begin(), t_rot_trans);

	//move and rotate 2st the CGAL structure Polyhedron
	rot_mat = (se32.orientation).toRotationMatrix();
	trans_vec = se32.position;
	t_rot_trans = Transformation(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
	Polyhedron PB = B->GetPolyhedron();
	std::transform( PB.points_begin(), PB.points_end(), PB.points_begin(), t_rot_trans);

	//calculate plane equations
	std::transform( PA.facets_begin(), PA.facets_end(), PA.planes_begin(),Plane_equation());
	std::transform( PB.facets_begin(), PB.facets_end(), PB.planes_begin(),Plane_equation());


	//call test
	return do_intersect(PA,PB);
}

int main() {
    Point_3 p( 1, 0, 0);
    Point_3 q( 0, 1, 0);
    Point_3 r( 0, 0, 1);
    Point_3 s( 0, 0, 0);
    Polyhedron P;
    P.make_tetrahedron( p, q, r, s);
    std::transform( P.facets_begin(), P.facets_end(), P.planes_begin(),
                    Plane_equation());
    CGAL::set_pretty_mode( std::cout);
    std::copy( P.planes_begin(), P.planes_end(),
               std::ostream_iterator<Plane_3>( std::cout, "\n"));
    return 0;
}

// Add semantics to Interior room polyhedra
void set_semantic_InteriorLoD4(Polyhedron& polyhe) {
	std::transform( polyhe.facets_begin(), polyhe.facets_end(),polyhe.planes_begin(),Plane_Newel_equation());		
	for (Polyhedron::Facet_iterator fIt = polyhe.facets_begin(); fIt != polyhe.facets_end(); ++fIt) {	// Iterate over faces
		Kernel::FT z = fIt->plane().orthogonal_vector().z();
		if		(z <= -HORIZONTAL_ANGLE_RANGE)	fIt->semanticBLA = "FloorSurface";
		else if (z >=  HORIZONTAL_ANGLE_RANGE)	fIt->semanticBLA = "CeilingSurface";
		else									fIt->semanticBLA = "InteriorWallSurface";
}	}	

void mergeCoplanar(Polyhedron& p,bool step2) {
	int facetsBefore =  p.size_of_facets();
	
	p.normalize_border();
	if(!p.size_of_border_halfedges()) {
		// Calculate normals only once in advace! so all tris should be coplanar with the original
		std::transform( p.facets_begin(), p.facets_end(),p.planes_begin(),Plane_equation());			// Calculate plane equations (only works on tri<- = bs)=true
		bool coplanarFound = true;
		std::vector<Polyhedron::Halfedge_handle> skipHEs;
		while (coplanarFound) {			
			coplanarFound = false;																		// Set coplanarFound false
			int percCount = 1;
			for (Polyhedron::Halfedge_iterator hit = p.halfedges_begin(); hit != p.halfedges_end(); ++hit,++percCount){ // Loop through all halfedges
				if (is_coplanar(hit,true)){																// If coplanar and equals semantics
					Polyhedron::Halfedge_handle removeMe = hit;
					while (CGAL::circulator_size(removeMe->vertex_begin()) < 3)							// Mover handle to beginning of linestring
						removeMe = removeMe->next();
					
					bool jcnh = false;
					if (!step2) jcnh = joinCreatesNoHole (hit);
					else		jcnh = joinCreatesNoHole2(hit);
					if (jcnh){														// If no holes will be created

						std::cout << "\rFacets   before/after: "<<facetsBefore<<" -> "<< p.size_of_facets()<<". ("<<100*percCount/p.size_of_halfedges()<<"%)";

						while (CGAL::circulator_size(removeMe->opposite()->vertex_begin()) < 3)			// Join vertexes until at the other end of linestring
							if (removeMe->facet_degree()>3 && removeMe->opposite()->facet_degree()>3)
								removeMe = (p.join_vertex(removeMe))->next()->opposite();
							else																		// One of the faces turned into a triangle ->remove center vertex
								break;																	
						if (CGAL::circulator_size(removeMe->opposite()->vertex_begin()) < 3)			// Remove remained of the border
							p.erase_center_vertex(removeMe->opposite());								// if two segments remain remove center point
						else
							p.join_facet(removeMe);														// if one segment remains join facets
						coplanarFound = true;
						break;
					} else { // simplify border, but how to do this safely? not optimal solution implemented. Should do: add inward offseted point of intersection etc.
						if (std::find(skipHEs.begin(), skipHEs.end(),hit)!=skipHEs.end()) {					// Skip if hit in skipList
							while (CGAL::circulator_size(removeMe->opposite()->vertex_begin()) < 3)	{		// Join vertexes until at the other end of linestring
								if (removeMe->facet_degree()>3 && removeMe->opposite()->facet_degree()>3)
									if (triDoesNotIntersectFacet(removeMe))									// if tri reMe,reME->prev does not intersect left or right facet
										removeMe = (p.join_vertex(removeMe))->next()->opposite();			// remove removeME
									else {
										skipHEs.push_back(removeMe);
										skipHEs.push_back(removeMe->opposite());
										removeMe = removeMe->prev();										// move removeME one halfedge back
									}
								else break;																	// stop if only a triangle remains or at other end																
							}	
							skipHEs.push_back(removeMe);
							skipHEs.push_back(removeMe->opposite());
	}	}	}	}	}	}
	//if (!step2) mergeCoplanar(p,true);
	//else 
		std::cout << "\rFacets   before/after: "<<facetsBefore<<" -> "<< p.size_of_facets()<<". (100%)"<<std::endl;
}

void set_semantic_AABB_C2V(Polyhedron& exteriorPolyhe,PolVector& polyVec) {

	if (exteriorPolyhe.is_pure_triangle()) {
		std::transform( exteriorPolyhe.facets_begin(), exteriorPolyhe.facets_end(),exteriorPolyhe.planes_begin(),Plane_equation());
		std::vector<std::string>	semList;
		std::vector<std::shared_ptr<AAbbTree>>		treeList;

		// Build Trees. One for each semantic
		for(PolVector::iterator pvIt = polyVec.begin();pvIt!=polyVec.end();++pvIt) {// Get AABB trees of all semantics
			if (pvIt->is_pure_triangle()) {
				std::string semP = pvIt->facets_begin()->semanticBLA;
				std::vector<std::string>::iterator  strIt = std::find(semList.begin(), semList.end(),semP);
				if (strIt==semList.end()) {											// If new sematic
					semList.push_back(semP);										// Add sem
					std::shared_ptr<AAbbTree> tree = std::make_shared<AAbbTree>(pvIt->facets_begin(),pvIt->facets_end());		// Create tree
					tree->accelerate_distance_queries();							// accelerate
					treeList.push_back(tree);										// Add tree
				} else																					// If not new
					treeList[strIt-semList.begin()]->insert(pvIt->facets_begin(),pvIt->facets_end());	// Append to tree
			} else std::cerr << "ERROR: Not pure triangle (set_semantic_AABB2C2V)" << std::endl;
		}
		

		// For each facet calculate the least distance to each tree
		std::string semListStr = boost::algorithm::join((semList), " ");
		int percCount = 1;
		Polyhedron::Facet_iterator exfIt;						// Iterate over exterior faces
		for (exfIt = exteriorPolyhe.facets_begin(); exfIt != exteriorPolyhe.facets_end(); ++exfIt,++percCount) {	
			
			std::cout << "\r"<<semListStr<<". ("<<100*percCount/exteriorPolyhe.size_of_facets()<<"%)";

			Vector_3 orthVec = exfIt->plane().orthogonal_vector();
			normalizeVector(orthVec);	//if (!normalizeVector(ortVec)) continue;

			std::vector<distSemFace> dsfList(semList.size());										
			Point_3 centerPoint = comp_facetCentroid(exfIt);						// Compute centroid
			std::vector<Kernel::FT> leastSemDistances;
			for (int intIt=0;intIt<(int)treeList.size();++intIt) {					// Loop over all trees
				AAbbTree::Point_and_primitive_id pp = treeList[intIt]->closest_point_and_primitive(centerPoint);
				dsfList[intIt].dist	= CGAL::squared_distance(centerPoint,pp.first);	// Store distance semantic and facet for each tree
				dsfList[intIt].sem	= semList[intIt];
				dsfList[intIt].fh	= pp.second;
			}

			std::sort(dsfList.begin(),dsfList.end(),by_dist());

			exfIt->leastSqDistance = dsfList[0].dist;					// least sqrt distance
			if (exfIt->isMinkFacet = dsfList[0].dist > SEMANTIC_DISTANCE_THRESHOLD) {
				exfIt->semanticBLA = TO_DIST_SEMANTIC;						// Default semantic if too distant
				continue;
			} else
				exfIt->semanticBLA = dsfList[0].sem;					// Semantics of closest

			Vector_3 faceNormal;
			Kernel::FT faceSqArea;
			double minAngle = 10;
			Kernel::FT maxArea= 0;

			for (std::vector<distSemFace>::iterator slIt = dsfList.begin();slIt != dsfList.end();++slIt)// HANDLE ANYTHING AS LESS IMPORTANT
				if (slIt->dist < dsfList[0].dist+OVERLAP_DIST_THRESHOLD) {	// Check if Equidistant
					pointVector facetPoints = comp_facetPoints(exfIt);
					CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),faceNormal); // Calculate normal vector, ortVec set to zero in newell
					double angle = comp_angle(orthVec,faceNormal);
					if (angle!=-1 && angle < minAngle+OVERLAP_ANGLE_THRESHOLD) {
						if (minAngle >= angle+OVERLAP_ANGLE_THRESHOLD)		exfIt->equidistSems.clear();
						if (angle < minAngle)								minAngle = angle;
						faceSqArea = comp_facetSquaredArea(facetPoints);
						if (faceSqArea>maxArea-OVERLAP_AREA_THRESHOLD) {
							if (maxArea<=faceSqArea-OVERLAP_AREA_THRESHOLD)	exfIt->equidistSems.clear();
							if (faceSqArea>maxArea)							maxArea = faceSqArea;
							exfIt->equidistSems.push_back(slIt->sem);				// Add equidist semantics
						}
					}
				}
		}
		std::cout << "\r"<<semListStr<<". (100%)" << std::endl;
	}else std::cerr << "ERROR: Not pure triangle (set_semantic_AABB2C2V)" << std::endl;
}