Polyhedra generate_polyhedra_from_points(const std::vector<std::array<double,3> >& vertices)
{
std::vector<Point_3> points(vertices.size());
for(int i=0;i<(int)vertices.size();i++)
points[i] = Point_3(vertices[i][0],vertices[i][1],vertices[i][2]);
Polyhedron_3 poly;
CGAL::convex_hull_3(points.begin(), points.end(), poly);
std::transform(poly.facets_begin(), poly.facets_end(), poly.planes_begin(), Plane_from_facet());
for(Halfedge_iterator half=poly.halfedges_begin();half!=poly.halfedges_end();++half)
{
Point_3 v1 = half->vertex()->point();
Point_3 v2 = half->next()->vertex()->point();
Point_3 v3 = half->next()->next()->vertex()->point();
Point_3 v4 = half->opposite()->vertex()->point();
Point_3 v5 = half->opposite()->next()->vertex()->point();
Point_3 v6 = half->opposite()->next()->next()->vertex()->point();
if(coplanar_handmade(v1,v2,v3,v4)&&coplanar_handmade(v1,v2,v3,v5)&&coplanar_handmade(v1,v2,v3,v6)&&
coplanar_handmade(v1,v2,v4,v5)&&coplanar_handmade(v1,v2,v4,v6)&&coplanar_handmade(v1,v2,v5,v6)&&
coplanar_handmade(v1,v3,v4,v5)&&coplanar_handmade(v1,v3,v4,v6)&&coplanar_handmade(v1,v4,v5,v6)&&
coplanar_handmade(v2,v3,v4,v5)&&coplanar_handmade(v2,v3,v4,v6)&&coplanar_handmade(v2,v3,v5,v6)&&
coplanar_handmade(v2,v4,v5,v6)&&coplanar_handmade(v1,v3,v5,v6)&&coplanar_handmade(v3,v4,v5,v6))
{
poly.join_facet(half);
half=poly.halfedges_begin();
}
}
int cur = 0;
for(auto it=poly.points_begin(); it!=poly.points_end(); it++, cur++)
points[cur] = *it;
cur = 0;
std::vector<std::vector<int> > faces;
for(Facet_iterator i=poly.facets_begin(); i!=poly.facets_end(); i++)
{
faces.push_back(std::vector<int>());
Halfedge_facet_circulator j = i->facet_begin();
do
{faces[cur].push_back(std::distance(poly.vertices_begin(), j->vertex()));} while (++j != i->facet_begin());
cur++;
}
return Polyhedra{points,faces};
}
void CGALConvexHull3D::run(const MatrixFr& points) {
std::list<Point_3> cgal_pts;
const size_t num_pts = points.rows();
const size_t dim = points.cols();
if (dim != 3) {
std::stringstream err_msg;
err_msg << "Invalid dim: " << dim << " Expect dim=3.";
throw RuntimeError(err_msg.str());
}
for (size_t i=0; i<num_pts; i++) {
const VectorF& p = points.row(i);
cgal_pts.push_back(Point_3(p[0], p[1], p[2]));
}
Polyhedron_3 hull;
CGAL::convex_hull_3(cgal_pts.begin(), cgal_pts.end(), hull);
assert(hull.is_closed());
assert(hull.is_pure_triangle());
const size_t num_vertices = hull.size_of_vertices();
const size_t num_faces = hull.size_of_facets();
m_vertices.resize(num_vertices, dim);
m_faces.resize(num_faces, 3);
size_t vertex_count=0;
for (auto itr=hull.vertices_begin(); itr!=hull.vertices_end(); itr++) {
const Point_3& p = itr->point();
m_vertices.coeffRef(vertex_count, 0) = p.x();
m_vertices.coeffRef(vertex_count, 1) = p.y();
m_vertices.coeffRef(vertex_count, 2) = p.z();
itr->id() = vertex_count;
vertex_count++;
}
size_t face_count=0;
for (auto f_itr=hull.facets_begin(); f_itr!=hull.facets_end(); f_itr++) {
size_t edge_count=0;
auto h_itr = f_itr->facet_begin();
do {
m_faces.coeffRef(face_count, edge_count) = h_itr->vertex()->id();
edge_count++;
h_itr++;
} while (h_itr != f_itr->facet_begin());
face_count++;
}
compute_index_map(points);
reorient_faces();
}
void CGALDelaunay::TriangulateUsingCGAL(vector<Vertex_handle> * DelaunayTriangulationVertices, vector<vector<float> > * PointsToBeInserted, Triangulation * T, vector<float> *bufferPointer, vector<float> *colorPointer, int*totalVertices)
{
//CHECK TO SEE IF THE POINT TO ADD NEEDS TO BE EITHER MODIFIED OR ADDED
//ROS_INFO("SIZE OF DELAUNAY VERTICES:%i", DelaunayTriangulationVertices->size());
//ROS_INFO("SIZE OF VERTICES to be added:%i", PointsToBeInserted->size());
for(int i = 0;i<PointsToBeInserted->size();i++)
{
vector<float> currentPoint = PointsToBeInserted->at(i);
T->insert(Point(currentPoint[0],currentPoint[1], currentPoint[2]));
}
//CONVERT DELAUNAY TRIANGULATION TO CONVEX HULL
Polyhedron_3 chull;
CGAL::convex_hull_3_to_polyhedron_3(*T, chull);
int count = 0;
int vec3Order[] = {0,1,2};
vector<float> emptyVector;
*bufferPointer = emptyVector;
*colorPointer = emptyVector;
for( Polyhedron_3::Facet_iterator fit = chull.facets_begin(); fit != chull.facets_end(); ++fit){
vector<Point> currentFacet;
HF_circulator h = fit->facet_begin();
size_t order = 0;
vector<vector<float> > TriangleVec;
do
{
Point currentPoint = h->vertex()->point();
vector<float> tempVec;
tempVec.push_back(currentPoint.x());
tempVec.push_back(currentPoint.y());
tempVec.push_back(currentPoint.z());
TriangleVec.push_back(tempVec);
}
while(++h != fit->facet_begin());
BufferActions::addVec3ToBuffer(vec3Order, bufferPointer, &TriangleVec, 3);
BufferActions::addVec3ToBuffer(vec3Order, colorPointer, &TriangleVec, 3);
count += 3;
}
*totalVertices= count;
}
std::vector<Plane_3> renumerateFacets(Polyhedron_3 polyhedron,
std::vector<SimpleEdge_3> &edges, std::map<int, int> &indices)
{
DEBUG_START;
std::vector<Plane_3> planes;
for (const SimpleEdge_3 &edge : edges)
{
indices.insert(std::pair<int, int>(edge.iForward,
UNINITIALIZED_MAP_VALUE));
indices.insert(std::pair<int, int>(edge.iBackward,
UNINITIALIZED_MAP_VALUE));
}
int i = 0;
for (auto &pair : indices)
{
pair.second = i++;
}
i = 0;
for (auto facet = polyhedron.facets_begin();
facet < polyhedron.facets_end(); ++facet)
{
ASSERT(facet->id == i);
if (indices.find(i) != indices.end())
{
planes.push_back(facet->plane());
}
++i;
}
for (auto &pair : indices)
{
std::cout << "map[" << pair.first << "] = " << pair.second
<< ", plane: " << planes[pair.second]
<< std::endl;
}
for (SimpleEdge_3 &edge : edges)
{
edge.iForward = indices[edge.iForward];
edge.iBackward = indices[edge.iBackward];
}
DEBUG_END;
return planes;
}
Polyhedron_3 obtainPolyhedron(Polyhedron_3 initialP, std::map<int, int> map,
IpoptTopologicalCorrector *FTNLP)
{
DEBUG_START;
std::vector<Vector_3> directions = FTNLP->getDirections();
std::vector<double> values = FTNLP->getValues();
std::vector<Plane_3> planes(initialP.size_of_facets());
unsigned iFacet = 0;
for (auto I = initialP.facets_begin(), E = initialP.facets_end();
I != E; ++I)
{
auto it = map.find(iFacet);
if (it != map.end())
{
int i = it->second;
Vector_3 u = directions[i];
double h = values[i];
ASSERT(h > 0);
planes[iFacet] = Plane_3(-u.x(), -u.y(), -u.z(), h);
std::cout << "Changing plane #" << iFacet << ": "
<< I->plane() << " |--> " << planes[iFacet]
<< std::endl;
}
else
{
planes[iFacet] = I->plane();
}
++iFacet;
}
Polyhedron_3 intersection(planes);
std::cout << "Change in facets number: " << initialP.size_of_facets()
<< " -> " << intersection.size_of_facets() << std::endl;
ASSERT(initialP.size_of_facets() - intersection.size_of_facets()
< map.size() &&
"It seems that all extracted facets have gone");
DEBUG_END;
return intersection;
}
int main()
{
CGAL::Random_points_in_sphere_3<Point_3, PointCreator> gen(100.0);
// generate 250 points randomly on a sphere of radius 100.0
// and copy them to a vector
std::vector<Point_3> points;
CGAL::cpp11::copy_n( gen, 250, std::back_inserter(points) );
// define polyhedron to hold convex hull
Polyhedron_3 poly;
// compute convex hull of non-collinear points
CGAL::convex_hull_3(points.begin(), points.end(), poly);
std::cout << "The convex hull contains " << poly.size_of_vertices() << " vertices" << std::endl;
// assign a plane equation to each polyhedron facet using functor Plane_from_facet
std::transform( poly.facets_begin(), poly.facets_end(), poly.planes_begin(),Plane_from_facet());
return 0;
}
/* FIXME: Copied from Polyhedron_io.cpp with slight modifications. */
static std::shared_ptr<Polyhedron> convertWithAssociation(Polyhedron_3 p,
const Point_3 &C, const std::vector<Plane_3> &initPlanes)
{
/* Check for non-emptiness. */
ASSERT(p.size_of_vertices());
ASSERT(p.size_of_facets());
int numVertices = p.size_of_vertices();
int numFacets = p.size_of_facets();
/* Allocate memory for arrays. */
Vector3d *vertices = new Vector3d[numVertices];
Facet *facets = new Facet[numFacets];
/* Transform vertexes. */
int iVertex = 0;
for (auto vertex = p.vertices_begin(); vertex != p.vertices_end(); ++vertex)
{
Point_3 point = C + vertex->point();
vertices[iVertex++] = Vector3d(point.x(), point.y(), point.z());
}
/*
* Transform facets.
* This algorithm is based on example kindly provided at CGAL online user
* manual. See example Polyhedron/polyhedron_prog_off.cpp
*/
int iFacet = 0;
auto plane = p.planes_begin();
auto facet = p.facets_begin();
/* Iterate through the std::lists of planes and facets. */
do
{
int id = p.indexPlanes_[iFacet];
facets[id].id = id;
/* Transform current plane. */
Plane_3 pi = centerizePlane(*plane,
Point_3(-C.x(), -C.y(), -C.z()),
signedDist(initPlanes[id], C));
facets[id].plane = Plane(Vector3d(pi.a(), pi.b(), pi.c()),
pi.d());
/*
* Iterate through the std::list of halfedges incident to the curent CGAL
* facet.
*/
auto halfedge = facet->facet_begin();
/* Facets in polyhedral surfaces are at least triangles. */
CGAL_assertion(CGAL::circulator_size(halfedge) >= 3);
facets[id].numVertices = CGAL::circulator_size(halfedge);
facets[id].indVertices =
new int[3 * facets[id].numVertices + 1];
/*
* TODO: It's too unsafe architecture if we do such things as setting
* the size of internal array outside the API functions. Moreover, it
* can cause us to write memory leaks.
* indFacets and numFacets should no be public members.
*/
int iFacetVertex = 0;
do
{
facets[id].indVertices[iFacetVertex++] =
std::distance(p.vertices_begin(), halfedge->vertex());
} while (++halfedge != facet->facet_begin());
/* Add cycling vertex to avoid assertion during printing. */
facets[id].indVertices[facets[id].numVertices] =
facets[id].indVertices[0];
ASSERT(facets[id].correctPlane());
/* Increment the ID of facet. */
++iFacet;
} while (++plane != p.planes_end() && ++facet != p.facets_end());
return std::make_shared<Polyhedron>(numVertices, numFacets, vertices,
facets);
}