void Graphics::HandleEventOnDisplay(void)
{
//draw bounding box
const double *bbox = m_graphics->m_simulator.m_bbox;
glColor3f(0, 0, 1);
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glBegin(GL_POLYGON);
glVertex2d(bbox[0], bbox[1]);
glVertex2d(bbox[2], bbox[1]);
glVertex2d(bbox[2], bbox[3]);
glVertex2d(bbox[0], bbox[3]);
glEnd();
//draw robot, goal, and obstacles
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glColor3f(1, 0, 0);
DrawCircle2D(m_simulator.GetRobotCenterX(), m_simulator.GetRobotCenterY(), m_simulator.GetRobotRadius());
glColor3f(0, 1, 0);
DrawCircle2D(m_simulator.GetGoalCenterX(), m_simulator.GetGoalCenterY(), m_simulator.GetGoalRadius());
glColor3f(0, 0, 1);
for(int i = 0; i < m_simulator.GetNrObstacles(); ++i)
DrawCircle2D(m_simulator.GetObstacleCenterX(i),
m_simulator.GetObstacleCenterY(i),
m_simulator.GetObstacleRadius(i));
//draw planner vertices
if(m_drawPlannerVertices)
{
glPointSize(4.0);
const int n = m_planner->m_vertices.size();
glColor3f(0.6, 0.8, 0.3);
glBegin(GL_POINTS);
for(int i = 0; i < n; ++i)
glVertex2dv(m_planner->m_vertices[i]->m_state);
glEnd();
glBegin(GL_LINES);
for(int i = 1; i < n; ++i)
{
glVertex2dv(m_planner->m_vertices[i]->m_state);
glVertex2dv(m_planner->m_vertices[m_planner->m_vertices[i]->m_parent]->m_state);
}
glEnd();
}
}
void Graphics::HandleEventOnDisplay(void)
{
char str[100];
if(m_motionPlannerDraw)
m_motionPlanner->Draw();
//draw robot
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glColor3f(1, 0, 0);
glBegin(GL_POLYGON);
glVertex2dv(&(m_motionPlanner->m_simulator->m_robotCurr[0]));
glVertex2dv(&(m_motionPlanner->m_simulator->m_robotCurr[2]));
glVertex2dv(&(m_motionPlanner->m_simulator->m_robotCurr[4]));
glVertex2dv(&(m_motionPlanner->m_simulator->m_robotCurr[6]));
glEnd();
glColor3f(0, 0, 0);
DrawPoint2D(m_motionPlanner->m_simulator->GetRobotCenterX(),
m_motionPlanner->m_simulator->GetRobotCenterY());
if(m_bestPathAnimate && m_bestPath.size() > 0)
{
sprintf(str, "%d", m_bestPathCurrReward);
glColor3f(0, 0, 0);
DrawString2D(m_motionPlanner->m_simulator->GetRobotCenterX(),
m_motionPlanner->m_simulator->GetRobotCenterY(), str);
}
//draw obstacles
glColor3f(0.45, 0.34, 0.76);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glBegin(GL_TRIANGLES);
const int no = m_motionPlanner->m_simulator->GetNrObstacles();
for(int i = 0; i < no; ++i)
{
Simulator::Obstacle *obst = m_motionPlanner->m_simulator->m_obstacles[i];
const int ntri = obst->m_triangles.size();
for(int j = 0; j < ntri; j += 3)
{
glVertex2dv(&obst->m_vertices[2 * obst->m_triangles[j + 0]]);
glVertex2dv(&obst->m_vertices[2 * obst->m_triangles[j + 1]]);
glVertex2dv(&obst->m_vertices[2 * obst->m_triangles[j + 2]]);
}
}
glEnd();
//draw reward regions
if(m_bestPathAnimate && m_bestPath.size() > 0)
{
for(int i = 0; i < m_bestPathCurrRemainingRewards.size(); ++i)
{
const int id = m_bestPathCurrRemainingRewards[i];
glColor3f(0, 1, 0);
DrawCircle2D(m_motionPlanner->m_simulator->GetRewardRegionCenterX(id),
m_motionPlanner->m_simulator->GetRewardRegionCenterY(id),
m_motionPlanner->m_simulator->GetRewardRegionRadius(id));
sprintf(str, "%d", m_motionPlanner->m_simulator->GetRewardRegionValue(id));
glColor3f(0, 0, 0);
DrawString2D(m_motionPlanner->m_simulator->GetRewardRegionCenterX(id),
m_motionPlanner->m_simulator->GetRewardRegionCenterY(id),
str);
}
}
else
{
for(int i = 0; i < m_motionPlanner->m_simulator->GetNrRewardRegions(); ++i)
{
const int id = i;
glColor3f(0, 1, 0);
DrawCircle2D(m_motionPlanner->m_simulator->GetRewardRegionCenterX(id),
m_motionPlanner->m_simulator->GetRewardRegionCenterY(id),
m_motionPlanner->m_simulator->GetRewardRegionRadius(id));
sprintf(str, "%d", m_motionPlanner->m_simulator->GetRewardRegionValue(id));
glColor3f(0, 0, 0);
DrawString2D(m_motionPlanner->m_simulator->GetRewardRegionCenterX(id),
m_motionPlanner->m_simulator->GetRewardRegionCenterY(id),
str);
}
}
}
void Graphics::DrawCircle2D( double x, double y, double r, int res, GLuint texture )
{
DrawCircle2D( x, y, r, res, texture, 1.f, 1.f, 1.f, 1.f );
}
void Graphics::HandleEventOnDisplay(void)
{
//draw bounding box
const double *bbox = m_graphics->m_simulator.m_bbox;
glColor3f(0, 0, 1);
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
glBegin(GL_POLYGON);
glVertex2d(bbox[0], bbox[1]);
glVertex2d(bbox[2], bbox[1]);
glVertex2d(bbox[2], bbox[3]);
glVertex2d(bbox[0], bbox[3]);
glEnd();
//draw robot, goal, and obstacles
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glColor3f(0, 1, 0);
DrawCircle2D(m_simulator.GetGoalCenterX(), m_simulator.GetGoalCenterY(), m_simulator.GetGoalRadius());
glColor3f(0, 0, 1);
for(int i = 0; i < m_simulator.GetNrObstacles(); ++i)
{
DrawCircle2D(m_simulator.GetObstacleCenterX(i),m_simulator.GetObstacleCenterY(i),m_simulator.GetObstacleRadius(i));
}
//draw planner vertices
if(m_drawPlannerVertices)
{
glPointSize(4.0);
const int n = m_planner->m_vertices.size();
glColor3f(0.6, 0.8, 0.3);
glBegin(GL_POINTS);
for(int i = 0; i < n; ++i)
{
glVertex2dv(m_planner->m_vertices[i]->m_state);
}
glEnd();
glBegin(GL_LINES);
for(int i = 1; i < n; ++i)
{
glVertex2dv(m_planner->m_vertices[i]->m_state);
glVertex2dv(m_planner->m_vertices[m_planner->m_vertices[i]->m_parent]->m_state);
}
glEnd();
}
if(m_planner->IsProblemSolved())
{
glColor3f(1.0,0.0,0.0);
glPointSize(2.0);
glBegin(GL_LINES);
for(int i=0;i<m_path.size();i++)
{
double * state = (m_planner->m_vertices[m_path[i]]->m_state);
glVertex2dv(state);
}
glVertex2d(m_simulator.GetGoalCenterX(), m_simulator.GetGoalCenterY());
glEnd();
}
glColor3f(1, 0, 0);
DrawCircle2D(m_simulator.GetRobotCenterX(), m_simulator.GetRobotCenterY(), m_simulator.GetRobotRadius());
double sampleX = m_simulator.getLastSampleX();
double sampleY = m_simulator.getLastSampleY();
if(!m_planner->IsProblemSolved() && sampleX != -1 && sampleY !=-1)
{
glColor3f(1.0,0.6,0.0);
DrawCircle2D(m_simulator.getLastSampleX(), m_simulator.getLastSampleY(),.5);
}
// Draw the directional pointer
glDepthMask(GL_TRUE);
glDepthFunc(GL_ALWAYS);
double startPoint[2];
double endPoint[2];
startPoint[0] = m_simulator.GetRobotCenterX();
startPoint[1] = m_simulator.GetRobotCenterY();
double theta = m_simulator.GetRobotTheta();
endPoint[0] = startPoint[0] + (m_simulator.GetRobotRadius() + 1) * cos(theta);
endPoint[1] = startPoint[1] + (m_simulator.GetRobotRadius() + 1) * sin(theta);
glColor3f(1,0,0);
glLineWidth(3);
glBegin(GL_LINES);
glVertex2dv(startPoint);
glVertex2dv(endPoint);
glEnd();
glLineWidth(1);
}
void Graphics::HandleEventOnDisplay(void)
{
//draw robot
if(m_setRobotAndGoalCenters >= 1)
{
glColor3f(1, 0, 0);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
DrawCircle2D(m_simulator.GetRobotCenterX(), m_simulator.GetRobotCenterY(), 0.1);
}
if(m_setRobotAndGoalCenters >= 2)
{
//draw goal
glColor3f(0, 1, 0);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
DrawCircle2D(m_simulator.GetGoalCenterX(), m_simulator.GetGoalCenterY(), 0.6);
}
//draw trajectory
glColor3f(0, 0, 1);
glLineWidth(3.0);
glBegin(GL_LINE_STRIP);
for(int i = 0; i < (int) m_simulator.m_path.size(); i += 2)
glVertex2dv(&m_simulator.m_path[i]);
glEnd();
glLineWidth(1.0);
//draw obstacles
glColor3f(0.45, 0.34, 0.76);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glBegin(GL_TRIANGLES);
for(int i = 0; i < (int) m_simulator.m_obstacles.size(); ++i)
{
Simulator::Obstacle *obst = m_simulator.m_obstacles[i];
const int ntri = obst->m_triangles.size();
for(int j = 0; j < ntri; j += 3)
{
glVertex2dv(&obst->m_vertices[2 * obst->m_triangles[j + 0]]);
glVertex2dv(&obst->m_vertices[2 * obst->m_triangles[j + 1]]);
glVertex2dv(&obst->m_vertices[2 * obst->m_triangles[j + 2]]);
}
}
glEnd();
//draw leave point
if(m_bugAlgorithms->m_distLeaveToGoal != HUGE_VAL)
{
glColor3f(0, 0, 0);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
DrawCircle2D(m_bugAlgorithms->m_leave[0], m_bugAlgorithms->m_leave[1], 0.5);
}
if(m_bugAlgorithms->m_hit[0] != HUGE_VAL)
{
glColor3f(0, 0.7, 0.7);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
DrawCircle2D(m_bugAlgorithms->m_hit[0], m_bugAlgorithms->m_hit[1], 0.5);
}
}
void iGraphics::draw_circle (int x, int y, int r)
{
DrawCircle2D (x, invert (y), r);
};
