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