Nef_polyhedron_3 build_polyhedron(Array points, Array faces){
Polyhedron_builder<HalfedgeDS> pb(points, faces);
Polyhedron_3 P;
P.delegate(pb);
Nef_polyhedron_3 N(P);
return N;
}
bool TOMPCFilter<T>::convexHull(VecCloud &clouds_in, VecCloud &clouds_out, VecHull &hulls)
{
// std::cout << "Convex Hull" << std::endl;
for(int i=0 ; i<clouds_in.size(); i++){
pcl::PointCloud<T>& point_cloud = *clouds_in.at(i);
// assign points
std::vector<Point_3> points;
for(int j=0;j<point_cloud.size();j++){
Point_3 point(point_cloud.points[j].x, point_cloud.points[j].y, point_cloud.points[j].z);
points.push_back(point);
}
// 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);
hulls.push_back(poly);
CloudPtr cloud_convex (new pcl::PointCloud<T>);
Polyhedron_3::Vertex_iterator it;
for(it = poly.vertices_begin(); it != poly.vertices_end(); ++it){
T point;
point.x = it->point()[0];
point.y = it->point()[1];
point.z = it->point()[2];
cloud_convex->points.push_back(point);
}
clouds_out.push_back(cloud_convex);
}
return 1;
}
int main()
{
std::vector<Point_3> points;
CGAL::Object ch_object;
Traits ch_traits;
std::cout << "Testing hull of no points " << std::endl;
CGAL::convex_hull_3(points.begin(), points.end(), ch_object,
ch_traits);
assert(ch_object.is_empty());
Point_3 p1(0, 0, 0);
points.push_back(p1);
std::cout << "Testing hull of one point " << std::endl;
CGAL::convex_hull_3(points.begin(), points.end(), ch_object, ch_traits);
Point_3 ch_point;
assert(CGAL::assign(ch_point, ch_object));
std::cout << "Testing hull of two points " << std::endl;
Point_3 p2(1, 0, 0);
points.push_back(p2);
CGAL::convex_hull_3(points.begin(), points.end(), ch_object, ch_traits);
Segment_3 ch_segment;
assert(CGAL::assign(ch_segment, ch_object));
std::cout << "Testing hull of three points " << std::endl;
Point_3 p3(1, 1, 0);
points.push_back(p3);
CGAL::convex_hull_3(points.begin(), points.end(), ch_object, ch_traits);
Triangle_3 ch_triangle;
assert(CGAL::assign(ch_triangle, ch_object));
std::cout << "Testing hull of four points " << std::endl;
Point_3 p4(1, 1, 1);
points.push_back(p4);
CGAL::convex_hull_3(points.begin(), points.end(), ch_object, ch_traits);
Polyhedron_3 poly;
assert(CGAL::assign(poly, ch_object));
assert(poly.size_of_vertices()==4);
std::cout << "Testing hull of collinear points " << std::endl;
test_collinear();
std::cout << "Testing hull of points in xy-plane " << std::endl;
test_coplanar_xy();
std::cout << "Testing hull of points in xz-plane " << std::endl;
test_coplanar_xz();
std::cout << "Testing hull of points in yz-plane " << std::endl;
test_coplanar_yz();
std::cout << "Testing hull of points in arbitrary plane " << std::endl;
test_coplanar_arbitrary();
std::cout << "Testing hull of points in arbitrary plane " << std::endl;
test_coplanar_arbitrary();
std::cout << "Testing hull of coplanar points which convex hull is a triangle " << std::endl;
test_coplanar_triangle();
return 0;
}
static Point_3 getCenter(const Polyhedron_3 &p)
{
Point_3 C(0., 0., 0.);
for (auto I = p.vertices_begin(), E = p.vertices_end(); I != E; ++I)
C = C + (I->point() - CGAL::Origin());
C = C * (1. / p.size_of_vertices());
return C;
}
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;
}
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();
}
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;
}
int main()
{
CGAL::Random_points_in_sphere_3<Point_3> gen(100.0);
std::list<Point_3> points;
// generate 250 points randomly on a sphere of radius 100.0
// and insert them into the triangulation
CGAL::cpp0x::copy_n(gen, 250, std::back_inserter(points) );
Delaunay T;
T.insert(points.begin(), points.end());
std::list<Vertex_handle> vertices;
T.incident_vertices(T.infinite_vertex(), std::back_inserter(vertices));
std::cout << "This convex hull of the 250 points has "
<< vertices.size() << " points on it." << std::endl;
// remove 25 of the input points
std::list<Vertex_handle>::iterator v_set_it = vertices.begin();
for (int i = 0; i < 25; i++)
{
T.remove(*v_set_it);
v_set_it++;
}
//copy the convex hull of points into a polyhedron and use it
//to get the number of points on the convex hull
Polyhedron_3 chull;
CGAL::convex_hull_3_to_polyhedron_3(T,chull);
std::cout << "After removal of 25 points, there are "
<< chull.size_of_vertices() << " points on the convex hull." << std::endl;
return 0;
}
SupportFunctionDataPtr SupportFunctionDataConstructor::run(
std::vector<Point_3> directions,
Polyhedron_3 polyhedron)
{
DEBUG_START;
std::vector<Vector_3> vertices = polyhedron.getVertices();
std::vector<SupportFunctionDataItem> items;
for (auto &direction: directions)
{
Vector_3 vDirection = direction - CGAL::ORIGIN;
double scalarProductMax = 0.;
Vector_3 supportPoint(0., 0., 0.);
int iVertexTangient = 0;
int iVertex = 0;
for (auto &vertex: vertices)
{
double scalarProduct = vDirection * vertex;
if (scalarProduct > scalarProductMax)
{
scalarProductMax = scalarProduct;
supportPoint = vertex;
iVertexTangient = iVertex;
}
++iVertex;
}
tangientIDs_.push_back(iVertexTangient);
ASSERT(scalarProductMax > 0);
SupportFunctionDataItem item(direction, scalarProductMax);
item.info = SupportFunctionDataItemInfoPtr(
new SupportFunctionDataItemInfo());
item.info->point = supportPoint;
items.push_back(item);
}
DEBUG_END;
return SupportFunctionDataPtr(new SupportFunctionData(items));
}
static std::vector<SimpleEdge_3> getEdges(Polyhedron_3 P,
std::map<int, int> &map)
{
DEBUG_START;
std::vector<bool> visited(P.size_of_halfedges());
for (unsigned i = 0 ; i < visited.size(); ++i)
visited[i] = false;
std::cout << "getEdges: number of halfedges: " << P.size_of_halfedges()
<< std::endl;
std::vector<SimpleEdge_3> edges;
P.initialize_indices();
CGAL::Origin origin;
for (auto &i : map)
i.second = UNINITIALIZED_MAP_VALUE;
for (auto I = P.halfedges_begin(), E = P.halfedges_end(); I != E; ++I)
{
if (visited[I->id])
continue;
SimpleEdge_3 edge;
edge.A = I->vertex()->point() - origin;
edge.B = I->opposite()->vertex()->point() - origin;
edge.iForward = I->facet()->id;
edge.iBackward = I->opposite()->facet()->id;
// FIXME: Remember indices of planes, not planes themselves,
// and fix structure for this.
map[I->id] = map[I->opposite()->id] = edges.size();
edges.push_back(edge);
visited[I->id] = visited[I->opposite()->id] = true;
}
std::cout << "getEdges: number of edges: " << edges.size() << std::endl;
ASSERT(edges.size() * 2 == P.size_of_halfedges());
DEBUG_END;
return edges;
}
/**
* Performs the testing of dynamic convex hull of CGAL.
*/
int main(int argc, char** argv)
{
DEBUG_START;
if (argc != 2)
{
print_usage(argc, argv);
DEBUG_END;
return EXIT_FAILURE;
}
int num_points = 0;
if (sscanf(argv[1], "%d", &num_points) != 1)
{
print_usage(argc, argv);
DEBUG_END;
return EXIT_FAILURE;
}
CGAL::Random_points_on_sphere_3<Point_3> gen(100.0);
std::list<Point_3> points;
/*
* generate <num_points> points randomly on a sphere of radius 100.0
* and copy them to a std::vector
*/
CGAL::cpp11::copy_n(gen, num_points, std::back_inserter(points) );
/* begin time measurement */
TimeMeasurer timer;
timer.pushTimer();
Delaunay T;
T.insert(points.begin(), points.end());
std::list<Vertex_handle> vertices;
T.incident_vertices(T.infinite_vertex(), std::back_inserter(vertices));
/*
* copy the convex hull of points into a polyhedron and use it
* to get the number of points on the convex hull
*/
Polyhedron_3 chull0;
CGAL::convex_hull_3_to_polyhedron_3(T,chull0);
std::cout << "The convex hull contains " << chull0.size_of_vertices()
<< " vertices" << std::endl;
/* end time measurement */
double timeFirst = timer.popTimer();
printf("Time for initial construction of convex hull: %lf\n", timeFirst);
/* begin time measurement */
timer.pushTimer();
/* Remove 90% of points. */
float nReduced = (1. - 1. / (float) FACTOR_OF_VERTICES_REDUCTION) *
chull0.size_of_vertices();
std::list<Vertex_handle>::iterator v_set_it = vertices.begin();
for (int i = 0; i < nReduced; i++)
{
T.remove(*v_set_it);
v_set_it++;
}
/*
* copy the convex hull of points into a polyhedron and use it
* to get the number of points on the convex hull
*/
Polyhedron_3 chull;
CGAL::convex_hull_3_to_polyhedron_3(T,chull);
/* end time measurement */
double timeSecond = timer.popTimer();
printf("Time for recalculating of convex hull: %lf\n", timeSecond);
std::cout << "The convex hull contains " << chull.size_of_vertices()
<< " vertices" << std::endl;
DEBUG_END;
return 0;
}
void PCViewerWidget::paintGL()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(xLook, yLook, zLook, 0,0,0, 0,1,0);
glRotatef(-90,1,0,0);
glRotatef(90,0,0,1);
glTranslatef(0.1,0.1,0);
// glTranslated(0,0,-1);
// glRotatef(-90,0,1,0);
// glRotatef(90,1,0,0);
// // Draw Axis
// float a = 100;
// glLineWidth(2);
// glBegin(GL_LINES);{
// glColor4f(1,0,0,1);
// glVertex3f(0,0,0);
// glVertex3f(.1,0,0);
// glColor4f(0,1,0,1);
// glVertex3f(0,0,0);
// glVertex3f(0,.1,0);
// glColor4f(0,0,1,1);
// glVertex3f(0,0,0);
// glVertex3f(0,0,.1);
// }
// glEnd();
// // Draw Grid
// glLineWidth(1);
// glColor4f(1,1,1,0.3);
// glBegin(GL_LINES);
// for(int i=0;i<20;i++){
// glVertex3f(i*.1-1, -1, 0);
// glVertex3f(i*.1-1, 1, 0);
// }
// for(int i=0;i<20;i++){
// glVertex3f(-1,i*.1-1,0);
// glVertex3f(1,i*.1-1,0);
// }
// glEnd();
// // Draw workspace
// if(m_ws.bottom != m_ws.top){
// glLineWidth(2);
// glColor4f(1,0,0,0.5);
// glBegin(GL_LINE_STRIP);{
// glVertex3f(m_ws.left, m_ws.top, 0);
// glVertex3f(m_ws.right, m_ws.top, 0);
// glVertex3f(m_ws.right, m_ws.bottom, 0);
// glVertex3f(m_ws.left, m_ws.bottom, 0);
// glVertex3f(m_ws.left, m_ws.top, 0);
// } glEnd();
// glBegin(GL_LINE_STRIP);{
// glVertex3f(m_ws.left+m_ws.margin, m_ws.top-m_ws.margin, 0);
// glVertex3f(m_ws.right-m_ws.margin, m_ws.top-m_ws.margin, 0);
// glVertex3f(m_ws.right-m_ws.margin, m_ws.bottom+m_ws.margin, 0);
// glVertex3f(m_ws.left+m_ws.margin, m_ws.bottom+m_ws.margin, 0);
// glVertex3f(m_ws.left+m_ws.margin, m_ws.top-m_ws.margin, 0);
// } glEnd();
// }
// Draw points
if(m_showMode == SHOW_RAWDATA){
if(m_cloud->size() != 0){
glPointSize(2);
glColor4f(0.,0.,0.,1.);
glBegin(GL_POINTS);
for(int i=0;i<m_cloud->size();i++){
glVertex3f(m_cloud->points[i].x,m_cloud->points[i].y,m_cloud->points[i].z);
}
glEnd();
}
}
// Draw points plane
if(m_showBGMode == SHOW_BACKGROUND_ON){
if(m_cloud_plane->size() != 0){
glPointSize(1.4);
glColor4f(0.,0.,0.,1.);
glBegin(GL_POINTS);
for(int i=0;i<m_cloud_plane->size();i++){
glVertex3f(m_cloud_plane->points[i].x,m_cloud_plane->points[i].y,m_cloud_plane->points[i].z);
}
glEnd();
}
}
// Draw objects
if(m_showMode == SHOW_SEGMENTS || m_showMode == SHOW_OBJECTS){
if(m_clouds_object.size()!=0){
for(int i=0;i<m_clouds_object.size();i++){
glPointSize(3.5);
glColor4f(randRGB[i%20][0],randRGB[i%20][1],randRGB[i%20][2],1.);
switch(i){
case 0: glColor4f(1,0,0,1.); break;
case 1: glColor4f(0,0,1,1.); break;
case 2: glColor4f(0,1,0,1.); break;
case 3: glColor4f(0,1,1,1.); break;
}
glBegin(GL_POINTS);
for(int j=0;j<m_clouds_object.at(i)->size();j++){
glVertex3f(m_clouds_object.at(i)->points[j].x,m_clouds_object.at(i)->points[j].y,m_clouds_object.at(i)->points[j].z);
}
glEnd();
}
}
}
if(m_showMode == SHOW_OBJECTS){
// Draw statistic
if(m_clouds_statistic.size() != 0){
for(int i=0;i<m_clouds_statistic.size();i++){
// mean value
TOM_OBJECT statistic = m_clouds_statistic.at(i);
int id = m_clouds_statistic.at(i).id;
glPointSize(7);
glColor4f(randRGB[i%20][0],randRGB[i%20][1],randRGB[i%20][2],1.);
switch(i){
case 0: glColor4f(1,0,0,1.); break;
case 1: glColor4f(0,0,1,1.); break;
case 2: glColor4f(0,1,0,1.); break;
case 3: glColor4f(0,1,1,1.); break;
}
glBegin(GL_POINTS);
glVertex3f(statistic.mean[0],statistic.mean[1],statistic.mean[2]);
glEnd();
// covariance -> ellipsoid
double scale = 1;
GLfloat mat[16];
GLUquadricObj *obj = gluNewQuadric();
gluQuadricDrawStyle( obj, GLU_LINE); // GLU_FILL
// A homogeneous transformation matrix, in this order:
//
// 0 4 8 12
// 1 5 9 13
// 2 6 10 14
// 3 7 11 15
//
mat[3] = mat[7] = mat[11] = 0;
mat[15] = 1;
mat[12] = mat[13] = mat[14] = 0;
mat[0] = statistic.eigenvectors.col(0)[0]; mat[1] = statistic.eigenvectors.col(0)[1]; mat[2] = statistic.eigenvectors.col(0)[2]; // New X-axis
mat[4] = statistic.eigenvectors.col(1)[0]; mat[5] = statistic.eigenvectors.col(1)[1]; mat[6] = statistic.eigenvectors.col(1)[2]; // New X-axis
mat[8] = statistic.eigenvectors.col(2)[0]; mat[9] = statistic.eigenvectors.col(2)[1]; mat[10] = statistic.eigenvectors.col(2)[2]; // New X-axis
glPushMatrix();
glTranslatef(statistic.mean[0], statistic.mean[1], statistic.mean[2]);
glMultMatrixf( mat );
glScalef(sqrt(statistic.eigenvalues[0])*scale, sqrt(statistic.eigenvalues[1])*scale, sqrt(statistic.eigenvalues[2])*scale);
gluSphere( obj, 1, 10, 10);
glPopMatrix();
gluDeleteQuadric(obj);
glColor4f(1,1,1,1);
QFont font("Times", 50, QFont::Bold);
// renderText(statistic.mean[0], statistic.mean[1], statistic.mean[2], QString::number(i), font);
}
}
}
// Draw tracks
if(m_showMode == SHOW_TRACKS || m_showMode == SHOW_TRACKCLOUDS){
if(m_tracks.size() != 0){
for(int i=0;i<m_tracks.size();i++){
TOM_OBJECT object = m_tracks.at(i).ptrLast();
int id = m_tracks.at(i).num();
if(m_showMode == SHOW_TRACKS){
// mean value
glPointSize(7);
glColor4f(randRGB[id%20][0],randRGB[id%20][1],randRGB[id%20][2],1.);
switch(id){
case 1: glColor4f(1,0,0,1.); break;
case 2: glColor4f(0,0,1,1.); break;
case 3: glColor4f(0,1,0,1.); break;
case 4: glColor4f(0,1,1,1.); break;
}
glBegin(GL_POINTS);
glVertex3f(object.mean[0],object.mean[1],object.mean[2]);
glEnd();
// covariance -> ellipsoid
double scale = 1;
GLfloat mat[16];
GLUquadricObj *obj = gluNewQuadric();
gluQuadricDrawStyle( obj, GLU_LINE); // GLU_FILL
// A homogeneous transformation matrix, in this order:
//
// 0 4 8 12
// 1 5 9 13
// 2 6 10 14
// 3 7 11 15
//
mat[3] = mat[7] = mat[11] = 0;
mat[15] = 1;
mat[12] = mat[13] = mat[14] = 0;
mat[0] = object.eigenvectors.col(0)[0]; mat[1] = object.eigenvectors.col(0)[1]; mat[2] = object.eigenvectors.col(0)[2]; // New X-axis
mat[4] = object.eigenvectors.col(1)[0]; mat[5] = object.eigenvectors.col(1)[1]; mat[6] = object.eigenvectors.col(1)[2]; // New X-axis
mat[8] = object.eigenvectors.col(2)[0]; mat[9] = object.eigenvectors.col(2)[1]; mat[10] = object.eigenvectors.col(2)[2]; // New X-axis
glPushMatrix();
glTranslatef(object.mean[0], object.mean[1], object.mean[2]);
glMultMatrixf( mat );
glScalef(sqrt(object.eigenvalues[0])*scale, sqrt(object.eigenvalues[1])*scale, sqrt(object.eigenvalues[2])*scale);
gluSphere( obj, 1, 10, 10);
glPopMatrix();
gluDeleteQuadric(obj);
glColor4f(0,0,0,1);
QFont font("Times", 30, QFont::Bold);
renderText(object.mean[0], object.mean[1], object.mean[2], QString::number(id), font);
}
// draw point cloud
glPointSize(3.5);
glColor4f(randRGB[id%20][0],randRGB[id%20][1],randRGB[id%20][2],1.);
switch(id){
case 1: glColor4f(1,0,0,1.); break;
case 2: glColor4f(0,0,1,1.); break;
case 3: glColor4f(0,1,0,1.); break;
case 4: glColor4f(0,1,1,1.); break;
}
glBegin(GL_POINTS);
for(int k=0;k<m_tracks.at(i).getCloud().size();k++){
glVertex3f(m_tracks.at(i).getCloud().points[k].x,m_tracks.at(i).getCloud().points[k].y,m_tracks.at(i).getCloud().points[k].z);
}
glEnd();
// draw points
// for(int j=0;j<m_tracks.at(i).size();j++){
// glPointSize(2);
// glColor4f(randRGB[i%20][0],randRGB[i%20][1],randRGB[i%20][2],1.);
// glBegin(GL_POINTS);
// for(int k=0;k<m_tracks.at(i).ptrAt(j).cloud.size();k++){
// glVertex3f(m_tracks.at(i).ptrAt(j).cloud.points[k].x,m_tracks.at(i).ptrAt(j).cloud.points[k].y,m_tracks.at(i).ptrAt(j).cloud.points[k].z);
// }
// glEnd();
// }
}
}
}
// Draw convex hull points
if(m_hulls.size() != 0){
for(int i=0;i<m_hulls.size();i++){
Polyhedron_3 poly = m_hulls.at(i);
// draw points
// Polyhedron_3::Vertex_iterator it;
// glPointSize(7);
// glColor4f(randRGB[i][0],randRGB[i][0],randRGB[i][0],0.5);
// glBegin(GL_POINTS);
// for(it = poly.vertices_begin(); it != poly.vertices_end(); ++it){
// glVertex3f(it->point()[0],it->point()[1],it->point()[2]);
// }
// glEnd();
// draw edges
glColor4f(1,1,1,0.3);
glBegin(GL_LINES);
Polyhedron_3::Halfedge_iterator it_edge;
for(it_edge = poly.halfedges_begin(); it_edge != poly.halfedges_end(); ++it_edge){
Polyhedron_3::Halfedge_handle h = it_edge->prev();
Polyhedron_3::Vertex_handle vs = h->vertex();
Polyhedron_3::Vertex_handle ve = it_edge->vertex();
glVertex3f(vs->point()[0],vs->point()[1],vs->point()[2]);
glVertex3f(ve->point()[0],ve->point()[1],ve->point()[2]);
}
glEnd();
}
}
if(m_tCloudA.size() != 0){
// draw point cloud
glPointSize(2);
glColor4f(1,0,0,1.);
glBegin(GL_POINTS);
for(int k=0;k<m_tCloudA.size();k++){
glVertex3f(m_tCloudA.points[k].x,m_tCloudA.points[k].y,m_tCloudA.points[k].z);
}
glEnd();
}
if(m_tCloudB.size() != 0){
// draw point cloud
glPointSize(2);
glColor4f(0,0,1,1.);
glBegin(GL_POINTS);
for(int k=0;k<m_tCloudB.size();k++){
glVertex3f(m_tCloudB.points[k].x,m_tCloudB.points[k].y,m_tCloudB.points[k].z);
}
glEnd();
}
}
Nef_polyhedron_3 to_nef(Iso_cuboid_3 cuboid){
Cube_builder<HalfedgeDS> builder(cuboid);
Polyhedron_3 P;
P.delegate(builder);
return Nef_polyhedron_3(P);
}
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};
}
/* 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);
}
