Polyhedron_3::Plane_3 operator()(Polyhedron_3::Facet& f)
{
Polyhedron_3::Halfedge_handle h = f.halfedge();
return Polyhedron_3::Plane_3(h->vertex()->point(),
h->next()->vertex()->point(),
h->opposite()->vertex()->point());
}
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();
}
}