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;
}
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();
}
/* 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);
}