void display(void)
{
if(boolpause==0){
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
robotOrientation();
gluLookAt(deltaX+betaX,deltaY+betaY,deltaZ+betaZ,0.0+deltaX,0.0,0.0+deltaZ, 0.0,1.0,0.0);
//Draw ground
glBegin(GL_QUADS);
glColor4f(0.0f,1.0f,0.0f,0.0f); //Green
glVertex3f(-5.0f, 0.0f, -5.0f);
glVertex3f(-5.0f, 0.0f, (float)(MaxDistance)+5.0f);
glVertex3f((float)(MaxDistance)+5.0f,0.0f,(float)(MaxDistance)+5.0f);
glVertex3f((float)(MaxDistance)+5.0f,0.0f,-5.0f);
glEnd();
//Draw Robot
drawRobot();
//Draw Buildings
drawBuildings(GL_RENDER);
glutSwapBuffers();
}
}
/*!*****************************************************************************
*******************************************************************************
\note display
\date August 7, 1992
\remarks
this function updates the OpenGL graphics
*******************************************************************************
Function Parameters: [in]=input,[out]=output
******************************************************************************/
void
display(void)
{
int i;
static SL_Jstate *state = joint_sim_state;
static SL_endeff *eff = endeff;
static SL_Cstate *basec = &base_state;
static SL_quat *baseo = &base_orient;
GLfloat objscolor[4]={(float)0.2,(float)0.2,(float)0.2,(float)1.0};
static int firsttime = TRUE;
if (firsttime) {
firsttime = FALSE;
double w;
if (read_parameter_pool_double(config_files[PARAMETERPOOL],"contact_force_scale",&w))
fscale = w;
}
drawRobot(state,basec,baseo);
// the standard display functions for openGL
#include "SL_user_display_core.h"
drawCenterOfPressure();
drawCOG();
drawIMU();
drawFootSensor();
if (comsDisplay)
drawCOMs(state, eff, basec, baseo);
}
/*!*****************************************************************************
*******************************************************************************
\note displayPhantom
\date April 2013
\remarks
creates a translucent phantom copy of the robot at the given position
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] b : the general array of bytes
******************************************************************************/
static void
displayPhantom(void *b)
{
int i,n;
float data[N_CART+N_QUAT+n_dofs];
SL_Cstate basec;
SL_quat baseo;
SL_Jstate state[n_dofs+1];
SL_endeff *eff=endeff;
// assign the data to apprpriate variables
n = 0;
for (i=1; i<=N_CART; ++i)
basec.x[i] = (double) ((float *)b)[n++];
for (i=1; i<=N_QUAT; ++i)
baseo.q[i] = (double) ((float *)b)[n++];
for (i=1; i<=n_dofs; ++i)
state[i].th = (double) ((float *)b)[n++];
/* here is the drawing rountines */
glPushMatrix();
glEnable(GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
//glDisable(GL_LIGHTING);
drawRobot(state,&basec,&baseo);
glEnable(GL_LIGHTING);
glDisable(GL_BLEND);
glPopMatrix();
}
void ViewSFML::drawPlayer(sf::RenderTarget &target, const std::string &name,
const Robot &robot) const {
// drawRobot
drawRobot(target, robot);
// drawLable
drawLable(target, name, robot.getPosition());
// drawArc(target, robot.getPosition(),
// robot.getRotation() + robot.getTurretAngle());
}
void locWmGlDisplay::DrawModel(const KF& model)
{
drawRobot(QColor(255,255,255,model.alpha*255), model.getState(KF::selfX), model.getState(KF::selfY), model.getState(KF::selfTheta));
if(drawSigmaPoints)
{
Matrix sigmaPoints = model.CalculateSigmaPoints();
for (int i=1; i < sigmaPoints.getn(); i++)
{
DrawSigmaPoint(QColor(255,255,255,model.alpha*255), sigmaPoints[KF::selfX][i], sigmaPoints[KF::selfY][i], sigmaPoints[KF::selfTheta][i]);
}
}
drawBall(QColor(255,165,0,255), model.getState(KF::ballX), model.getState(KF::ballY));
}
void Mapping::render()
{
cout << "Rendering o map" << endl;
glBegin(GL_QUADS);
// //cout << "\n\n\n\n" << "Printando visão do sensor: " << endl;
for(int y=0; y<MAP_LENGTH_WORLD;y++)
{
for(int x=0; x<MAP_LENGTH_WORLD;x++)
{
//cout << map[x][MAP_LENGTH-1-y] << " ";
double value;
switch (technique) {
case MappingTechnique::BAYES:
value = 1.0 - mapCell[x][y].probabilidadeOcupada();
//value = 1.0 - mapCell[x][y].isBayesProbability();
break;
case MappingTechnique::HIMM:
value = mapCell[x][y].himmProbability();
break;
case MappingTechnique::DeadReckoning:
default:
value = mapCell[x][y].cellValue();
break;
}
//qDebug() << value << endl;
if(value != 0.7 && value >= 0 && value != 0.5)
{
value *= 255;
drawBox(
(x-MAP_LENGTH_WORLD/2)*celRange-1,
(MAP_LENGTH_WORLD/2 - y)*celRange-1,
celRange,
celRange,
QColor(value,value,value)
);
}
}
}
glEnd();
drawRobot();
}
//robot's position will change depend on camera's position
void Display(void)
{
glLoadIdentity();
SetCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
drawScene();
glPushMatrix();
glTranslatef(camX, camY-0.5, camZ-0.8);
glRotatef(angleX, 0,1,0);
glTranslatef(-camX, -(camY + 0.5), -(camZ -0.7));
glTranslatef(camX, camY-0.5, camZ-0.8);
glRotatef(angleX, 0,1,0);
drawRobot();
glPopMatrix();
glutSwapBuffers();
}
/**
Function to be used with calibrate node for tracking objects
@param threshold cv::Mat
@param HSV cv::Mat
@param &cameraFeed cv::Mat
@return void
*/
void trackRobot(TrackedObject& robot, Mat threshold, Mat HSV, Mat &cameraFeed)
{
cv::Mat temp;
threshold.copyTo(temp);
std::vector< std::vector<Point> > contours;
std::vector<Vec4i> hierarchy;
cv::findContours(temp, contours, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
double refArea = 0;
bool objectFound = false;
if (hierarchy.size() > 0)
{
for (int index = 0; index >= 0; index = hierarchy[index][0])
{
Moments moment = moments((cv::Mat)contours[index]);
double area = moment.m00;
if(area > MIN_OBJECT_AREA)
{
robot.setXPos(moment.m10/area);
robot.setYPos(moment.m01/area);
objectFound = true;
}
else
{
objectFound = false;
}
}
if(objectFound ==true)
{
drawRobot(robot,cameraFeed);
}
else
{
putText(cameraFeed,"TOO MUCH NOISE! ADJUST FILTER", Point(0, 50), 1, 1, Scalar(0, 0, 255), 2);
}
}
}
void robotField::drawRobot(robotParameters *robotParameter)
{
// robotCenter=b->robotCenter;
robotCenter = new QPointF(robotParameter->getRobotCenter());
robotRect = new QRectF(robotParameter->getRobotModel());
isRed=robotParameter->isRed();
if(!isRed)
{
robot=(scene->addRect(*robotRect,QPen(Qt::blue),QBrush(Qt::white)));
robotRect->setX(277);
robotRect->setY(0);
robotRect->setWidth(23);
robotRect->setHeight(34);
}
else
{
robot=(scene->addRect(*robotRect,QPen(Qt::red),QBrush(Qt::white)));
}
// robotRect->setX(0);
robotRect->setY(0);
drawRobot();
}
// Show 3D model
void COpenGlDisplay::showModel(CSLAM* SLAM)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix();
glTranslatef (xTrans, yTrans, zTrans); // OpenGL施加操作的平移、旋转顺序与程序的先后顺序相反
glRotatef(yRot, 0, 1, 0);
glCallList(groundList);
glPopMatrix();
glPushMatrix();
glTranslatef (xTrans, yTrans, zTrans);
glRotatef(xRot, 1, 0, 0);
glRotatef(zRot, 0, 0, 1);
glCallList(worldFrameList);
drawRobot(SLAM);
drawPathAndOdometry(SLAM);
drawCurrentFeatures(SLAM);
drawHistoryFeatures(SLAM);
glPopMatrix();
SwapBuffers(hrenderDC);
}
/*** Simulation loop ***/
void simLoop(int pause)
{
if (!pause and !pauseGlobal) {
if(shouldIKeepRunningTheSimulation()){
walk(); // control of walking
dSpaceCollide(space,0,&nearCallback); // collision detection
#ifndef USE_NLEG_ROBOT
dWorldStep(world, 0.001); // jmc: original was 0.01 but it was crashing
#else
dWorldStep(world, 0.001);
// dWorldQuickStep(world, 0.001); // jmc: original was 0.01 but it was crashing
#endif
dJointGroupEmpty(contactgroup); // Point of contact group sky
}
#ifndef NOVIZ
else dsStop(); //end the simulation
#endif
}
#ifdef USE_NLEG_ROBOT
drawRobot_Nleg();
#else
drawRobot();
#endif
}
void GLWidget::paintGL()
{
glDrawBuffer(GL_BACK);
glClearColor(0.6f, 0.8f, 1.0f, 0.0f);
glClearDepth(1.0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_DEPTH_TEST);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
/*Angulo ratio znear, zfar*/
gluPerspective(50.0, width / height, 1.0, 50000.0);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/*pos cam pto central vector up*/
gluLookAt(this->glcam_pos_X, this->glcam_pos_Y, this->glcam_pos_Z,
this->glcam_foa_X, this->glcam_foa_Y, this->glcam_foa_Z,
0., 0., 1.);
// absolute axis
glLineWidth(3.0f);
glColor3f(0.7, 0., 0.);
glBegin(GL_LINES);
v3f(0.0, 0.0, 0.0);
v3f(10.0, 0.0, 0.0);
glEnd();
glColor3f(0., 0.7, 0.);
glBegin(GL_LINES);
v3f(0.0, 0.0, 0.0);
v3f(0.0, 10.0, 0.0);
glEnd();
glColor3f(0., 0., 0.7);
glBegin(GL_LINES);
v3f(0.0, 0.0, 0.0);
v3f(0.0, 0.0, 10.0);
glEnd();
glLineWidth(1.0f);
//SUELO
glColor3f(0.6, 0.6, 0.6);
glLineWidth(2.0f);
glBegin(GL_LINES);
for (int i = 0; i < ((int) MAXWORLD + 1); i++) {
v3f(-(int) MAXWORLD * 10 / 2. + (float) i * 10, -(int) MAXWORLD * 10 / 2., 0.);
v3f(-(int) MAXWORLD * 10 / 2. + (float) i * 10, (int) MAXWORLD * 10 / 2., 0.);
v3f(-(int) MAXWORLD * 10 / 2., -(int) MAXWORLD * 10 / 2. + (float) i * 10, 0.);
v3f((int) MAXWORLD * 10 / 2., -(int) MAXWORLD * 10 / 2. + (float) i * 10, 0.);
}
glEnd();
UpdateRobot();
//robot
drawRobot();
}
