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;
}
Ejemplo n.º 3
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();
    }

}