void display()
{
if(camera_mode >= 1) camCent = robot;
if(camera_mode >= 2) camRotH = glpos.data.theta + M_PI;
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glEnable(GL_CULL_FACE);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
#ifdef MERGEDEBUG
if(testscreen)
{
const double md=100.0;
glViewport(0,0,wid1+wid2,hei);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(0, 2*M_PI, 0, md, -1.0, 1.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0,0,1, 0,0,0, 0,1,0);
displayT();
myString(0, 0, 0, strprintf("%d", mergedline.data.size()).c_str() );
glutSwapBuffers();
return;
}
#endif
glViewport(0,0,wid1,hei);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(45.0, (double)wid1/hei, 0.1, farDist);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
myLookAt();
display1();
glViewport(wid1,0,wid2,hei);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0,0,1, 0,0,0, 0,1,0);
display2();
glViewport(0,0,wid1+wid2,hei);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(0,0,1, 0,0,0, 0,1,0);
if(linfo.isOpen())
myString(-0.98, -0.98, 0, strprintf("%s [%d]", linfo.data.mapname, linfo.data.mapnum).c_str() );
if(einfo.isOpen())
myString(-0.98, -0.90, 0, strprintf("%s [%d]", einfo.data.mapname, einfo.data.mapnum).c_str() );
if(setgl)
myString(-0.98, 0.90, 0, "Restart Mode");
glutSwapBuffers();
}
void idle()
{
bool update = false;
if(setgl)
{
qout << "r";
}
#ifdef MERGEDEBUG
if(edata.isOpen()) if(edata.readNew())
{
double minX=1e10, maxX=-1e10;
double minY=1e10, maxY=-1e10;
for(int i=0; i<edata.data.numpoints; i++)
{
minX = min(minX, edata.data. start_x[i]);
minX = min(minX, edata.data. finish_x[i]);
minY = min(minY, edata.data. start_y[i]);
minY = min(minY, edata.data. finish_y[i]);
maxX = max(maxX, edata.data. start_x[i]);
maxX = max(maxX, edata.data. finish_x[i]);
maxY = max(maxY, edata.data. start_y[i]);
maxY = max(maxY, edata.data. finish_y[i]);
point s(edata.data. start_x[i], edata.data. start_y[i]);
point t(edata.data.finish_x[i], edata.data.finish_y[i]);
mergedline.push(line(s,t));
}
mergecenter = point( (minX+maxX)/2.0, (minY+maxY)/2.0 );
if(true) //mergecnt==0)
{
mergedline.shift(-mergecenter);
mergedline.prepare();
mergedline.clustering_line();
mergedline.clustering_segment();
mergedline.merge();
mergedline.shift(+mergecenter);
}
update=true;
}
#endif
if(ldata.isOpen()) if(ldata.readNew()) { update=true; }
if(edata.isOpen()) if(edata.readNew()) { update=true; }
if(lajst.isOpen()) if(lajst.readNew()) { update=true; }
if(eajst.isOpen()) if(eajst.readNew()) { update=true; }
if(pinfo.isOpen()) if(pinfo.readNew()) { update=true; }
if(glpos.isOpen()) if(glpos.readNew())
{
robot = triple(glpos.data.x, glpos.data.y, 0);
update=true;
}
triple t;
if(linfo.isOpen())
{
if(linfo.readNew())
{
linemap.clear();
ifstream fin(linfo.data.mapname);
if(fin) while(fin >> t.x >> t.y) { linemap.push_back(t); }
}
}
void display1()
{
vector<double> color;
double low = (vh[0]+0.9)/map_a/1.8 - map_b;
double high = (vh[1]+0.9)/map_a/1.8 - map_b;
// style
glLineWidth(1);
glPointSize(3);
// ground
const double gsize = 50.0;
double gr_x = floor(robot.x);
double gr_y = floor(robot.y);
setColor(config["ground-panel"]);
glBegin(GL_POLYGON);
glVertex3d(gr_x+gsize, gr_y+gsize, 0.0);
glVertex3d(gr_x-gsize, gr_y+gsize, 0.0);
glVertex3d(gr_x-gsize, gr_y-gsize, 0.0);
glVertex3d(gr_x+gsize, gr_y-gsize, 0.0);
glEnd();
setColor(config["ground-line"]);
glBegin(GL_LINES);
for(int i=-gsize; i<=gsize; i++)
{
glVertex3d(gr_x+i*1.0, gr_y-gsize, 0.0);
glVertex3d(gr_x+i*1.0, gr_y+gsize, 0.0);
glVertex3d(gr_x-gsize, gr_y+i*1.0, 0.0);
glVertex3d(gr_x+gsize, gr_y+i*1.0, 0.0);
}
glEnd();
// style
glLineWidth(3);
// robot
glColor3d(1.0, 1.0, 1.0);
drowCross(robot, glpos.data.theta, 1.0);
// adjust
if(disp_lajst)
{
glColor3d(0.0, 1.0, 1.0);
drowCross(triple(lajst.data.x, lajst.data.y, 0.0), lajst.data.theta, 0.5);
}
if(disp_eajst)
{
glColor3d(0.0, 1.0, 0.0);
drowCross(triple(eajst.data.x, eajst.data.y, 0.0), eajst.data.theta, 0.5);
}
#ifdef MERGEDEBUG
glColor3d(1.0, 0.0, 1.0);
drowCross(triple(mergecenter.real(), mergecenter.imag(), 0.0), eajst.data.theta, 0.5);
#endif
// gridmap
if(disp_map)
{
color = config["map-points"];
glBegin(GL_POINTS);
for(int i=0; i<gridmap.size(); i++)
{
if(low<=gridmap[i].z && gridmap[i].z<=high)
{
setColor(color, map_a*(map_b+gridmap[i].z));
if(view2d) glVertex2dv(gridmap[i].vec);
else glVertex3dv(gridmap[i].vec);
}
}
glEnd();
}
// scan data
for(int i=0; i<stream_num; i++)
{
// scan point
if(!disp_sdata[i]) continue;
glBegin(GL_POINTS);
color = config[strprintf("urg-points-%d",i)];
for(int j=0; j<sdata[i].property.numPoints; j++)
{
if(sdata[i].data[j].isError()) continue;
triple ref(sdata[i].data[j].reflect.vec);
if(low<=ref.z && ref.z<=high)
{
ref.rotZ(glpos.data.theta);
setColor(color, map_a*(map_b+ref.z));
glVertex3dv( (robot+ref).vec );
}
}
glEnd();
// scan laser
if(!disp_laser) continue;
glBegin(GL_LINES);
for(int i=0; i<stream_num; i++)
{
color = config[strprintf("urg-beams-%d",i)];
for(int j=0; j<sdata[i].property.numPoints; j++)
{
if(sdata[i].data[j].isError()) continue;
triple ref(sdata[i].data[j].reflect.vec);
triple ori(sdata[i].data[j].origin.vec);
if(low<=ref.z && ref.z<=high)
{
ref.rotZ(glpos.data.theta);
ori.rotZ(glpos.data.theta);
setColor(color, map_a*(map_b+ref.z));
glVertex3dv( (robot+ref).vec );
glVertex3dv( (robot+ori).vec );
}
}
}
glEnd();
}
// linemap
if( disp_lmap )
{
glColor3d(0.0, 0.4, 0.4);
glBegin(GL_LINES);
for(int i=0; i<linemap.size(); i++)
{
glVertex3dv( linemap[i].vec );
}
glEnd();
}
// edgemap
if( disp_emap )
{
glColor3d(0.0, 0.4, 0.0);
glBegin(GL_LINES);
for(int i=0; i<edgemap.size(); i++)
{
glVertex3dv( edgemap[i].vec );
}
glEnd();
}
// linedata
if( ldata.isOpen() && disp_ldata )
{
glColor3d(0.0, 1.0, 1.0);
glBegin(GL_LINES);
for(int i=0; i<ldata.data.numpoints; i++)
{
glVertex3d( ldata.data.start_x[i], ldata.data.start_y[i], 0.5 );
glVertex3d( ldata.data.finish_x[i], ldata.data.finish_y[i], 0.5 );
}
glEnd();
}
// edgedata
if( edata.isOpen() && disp_edata )
{
glColor3d(0.0, 1.0, 0.0);
glBegin(GL_LINES);
for(int i=0; i<edata.data.numpoints; i++)
{
glVertex3d( edata.data.start_x[i], edata.data.start_y[i], 0.5 );
glVertex3d( edata.data.finish_x[i], edata.data.finish_y[i], 0.5 );
}
glEnd();
}
#ifdef MERGEDEBUG
glColor3d(1.0, 1.0, 1.0);
glBegin(GL_LINES);
glVertex3d(0,0,0);
glVertex3d(10,0,0);
glVertex3d(0,0,0);
glVertex3d(0,10,0);
glVertex3d(0,0,0);
glVertex3d(0,0,10);
glEnd();
//glColor3d(1.0, 1.0, 1.0);
glBegin(GL_LINES);
/*
for(int i=0; i<mergedline.data.size(); i++)
{
point p1(0, 100), p2(0, -100);
p1 *= polar(1.0, mergedline.data[i].rad); p1 += polar(mergedline.data[i].dist, mergedline.data[i].rad);
p2 *= polar(1.0, mergedline.data[i].rad); p2 += polar(mergedline.data[i].dist, mergedline.data[i].rad);
glColor3dv( testcolor[mergedline.data[i].group%7]);
glVertex3d( p1.real(), p1.imag(), 1.5 );
glVertex3d( p2.real(), p2.imag(), 1.5 );
}
*/
for(int i=0; i<mergedline.data.size(); i++)
{
//glColor3dv( testcolor[mergedline.data[i].group%7]);
glColor4d(1, 0, 0, 0.5);
glVertex3d( mergedline.data[i][0].real(), mergedline.data[i][0].imag(), 0.5 );
glVertex3d( mergedline.data[i][1].real(), mergedline.data[i][1].imag(), 0.5 );
}
glEnd();
#endif
// style
glPointSize(7);
// route
if(disp_way)
{
// route line
glBegin(GL_LINES);
for(int i=1; i<route.size(); i++)
{
//glColor3d(1.0, 1.0, 1.0);
//if(pinfo.isOpen()) if(i==pinfo.data.waypoint) glColor3d(0.0, 1.0, 0.0);
glColor3dv(amodecolor[routeinfo[i-1]]);
glVertex3dv(route[i-1].vec);
glVertex3dv(route[i ].vec);
}
glEnd();
// route point
glColor3d(1.0, 1.0, 0.0);
glBegin(GL_POINTS);
for(int i=0; i<route.size(); i++)
{
glVertex3dv(route[i].vec);
}
glEnd();
// string
glColor3d(1.0, 1.0, 1.0);
for(int i=0; i<route.size(); i++)
{
//glColor3dv(amodecolor[routeinfo[i]]);
myString(route[i]+triple(0,0,0.2), strprintf("%d",i).c_str());
}
}
}