glWidget.cpp

Go to the documentation of this file.

 
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#include <SOIL/SOIL.h>
#include <cmath>
 
#include "glWidget.hh"
 
static GLuint textureWheel;
static GLuint textureRobot;
static float angle;
 
GLWidget::GLWidget(QWidget* parent) : QGLViewer(parent)
{
    _wheelTexturePath = "textures/wheel.png";
    _robotTexturePath = "textures/robot.png";
}
 
void GLWidget::obstacleDataReady(const std::map<float, int>& obstacleData)
{
    _obstacleCoordinates.clear();
    _obstacleColors.clear();
 
    std::vector<float> distances;
 
    for(const auto& [angle, distance] : obstacleData)
    {
        float radianAngle = ((angle + 1) * PI) / 180.0;
 
        float x = distance * _dimensions.scale * cos(radianAngle);
        float y = distance * _dimensions.scale * sin(radianAngle);
        float z = _dimensions.lidarHeight;
        _obstacleCoordinates.push_back({x, y, z});
        distances.push_back(distance);
    }
 
    float min = *std::min_element(std::begin(distances), std::end(distances));
    float max = *std::max_element(std::begin(distances), std::end(distances));
 
    for(const auto& distance : distances)
    {
        float color = (distance - min) / (max - min) * 255.0;
        _obstacleColors.push_back(color);
    }
 
    updateGL();
}
 
void GLWidget::rightWheelDirection(const int& direction)
{
    _rightWheelDirection = direction;
}
 
void GLWidget::leftWheelDirection(const int& direction)
{
    _leftWheelDirection = direction;
}
 
void GLWidget::draw()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glEnable(GL_TEXTURE_2D);
 
    // Background
    glClearColor(0.5, 0.5, 0.5, 1);
 
    drawRobot();
 
    for(unsigned long i = 0; i < _obstacleCoordinates.size(); i++)
    {
        drawOneObstacle(_obstacleCoordinates[i][0], _obstacleCoordinates[i][1], _obstacleCoordinates[i][2],
                        _obstacleColors[i]);
    }
 
    glFlush();
}
 
void GLWidget::init()
{
    restoreStateFromFile();
 
    GLfloat lightPosition[] = {1.0, 1.0, 1.0, 0.0};
    glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);
 
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glEnable(GL_DEPTH_TEST);
 
    glEnable(GL_TEXTURE_2D);
    loadTexture(&textureWheel, _wheelTexturePath.toStdString());
    loadTexture(&textureRobot, _robotTexturePath.toStdString());
    // help();
}
 
QString GLWidget::helpString() const
{
    QString text("<h2>S i m p l e V i e w e r</h2>");
    text += "Use the mouse to move the camera around the object. ";
    text += "You can respectively revolve around, zoom and translate with the three mouse buttons. ";
    text += "Left and middle buttons pressed together rotate around the camera view direction axis<br><br>";
    text += "Pressing <b>Alt</b> and one of the function keys (<b>F1</b>..<b>F12</b>) defines a camera keyFrame. ";
    text += "Simply press the function key again to restore it. Several keyFrames define a ";
    text += "camera path. Paths are saved when you quit the application and restored at next start.<br><br>";
    text += "Press <b>F</b> to display the frame rate, <b>A</b> for the world axis, ";
    text += "<b>Alt+Return</b> for full screen mode and <b>Control+S</b> to save a snapshot. ";
    text += "See the <b>Keyboard</b> tab in this window for a complete shortcut list.<br><br>";
    text += "Double clicks automates single click actions: A left button double click aligns the closer axis with the "
            "camera (if close enough). ";
    text +=
        "A middle button double click fits the zoom of the camera and the right button re-centers the scene.<br><br>";
    text += "A left button double click while holding right button pressed defines the camera <i>Revolve Around "
            "Point</i>. ";
    text += "See the <b>Mouse</b> tab and the documentation web pages for details.<br><br>";
    text += "Press <b>Escape</b> to exit the viewer.";
    return text;
}
 
void GLWidget::loadTexture(GLuint* texture, const std::string& path)
{
    glEnable(GL_TEXTURE_2D);
    glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
    glGenTextures(1, texture);
    glBindTexture(GL_TEXTURE_2D, *texture);
 
    glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
 
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
 
    int width, height;
    unsigned char* image = SOIL_load_image(path.c_str(), &width, &height, 0, SOIL_LOAD_RGB);
 
    glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, image);
}
 
void GLWidget::drawRobot()
{
    _drawCylinder(_dimensions.robotRadius, _dimensions.robotHeight, 10, 10, 10, textureRobot);
 
    glPushMatrix();
    glPushMatrix();
 
    glTranslatef(_dimensions.robotRadius, 0, _dimensions.robotWheelRadius);
    glRotatef(90, 0, 1, 0);
    glRotatef(_rightWheelDirection * angle, 0, 0, 1);
    _drawCylinder(_dimensions.robotWheelRadius, _dimensions.robotWheelThickness, 50, 50, 50, textureWheel);
 
    glPopMatrix();
 
    glTranslatef(-_dimensions.robotRadius, 0, _dimensions.robotWheelRadius);
    glRotatef(-90, 0, 1, 0);
    glRotatef(_leftWheelDirection * angle, 0, 0, 1);
    _drawCylinder(_dimensions.robotWheelRadius, _dimensions.robotWheelThickness, 50, 50, 50, textureWheel);
 
    angle += 10.0;
    angle = std::fmod(angle, 360.0);
 
    glPopMatrix();
}
 
void GLWidget::drawOneObstacle(GLfloat x, GLfloat y, GLfloat z, GLfloat color)
{
    glPushMatrix();
    glTranslatef(x, y, z);
 
    _drawCube(_dimensions.obstacleSize, 0, color, 0);
    glPopMatrix();
}
 
void GLWidget::_drawCube(GLfloat size, GLubyte R, GLubyte G, GLubyte B)
{
    glPushMatrix();
 
    glColor3ub(R, G, B);
    glBegin(GL_POLYGON);
    glVertex3f(size, -size, size);
    glVertex3f(size, size, size);
    glVertex3f(-size, size, size);
    glVertex3f(-size, -size, size);
    glEnd();
 
    glColor3ub(R, G, B);
    glBegin(GL_POLYGON);
    glVertex3f(size, -size, -size);
    glVertex3f(size, size, -size);
    glVertex3f(size, size, size);
    glVertex3f(size, -size, size);
    glEnd();
 
    glColor3ub(R, G, B);
    glBegin(GL_POLYGON);
    glVertex3f(-size, -size, size);
    glVertex3f(-size, size, size);
    glVertex3f(-size, size, -size);
    glVertex3f(-size, -size, -size);
    glEnd();
 
    glColor3ub(R, G, B);
    glBegin(GL_POLYGON);
    glVertex3f(size, size, size);
    glVertex3f(size, size, -size);
    glVertex3f(-size, size, -size);
    glVertex3f(-size, size, size);
    glEnd();
 
    glColor3ub(R, G, B);
    glBegin(GL_POLYGON);
    glVertex3f(size, -size, -size);
    glVertex3f(size, -size, size);
    glVertex3f(-size, -size, size);
    glVertex3f(-size, -size, -size);
    glEnd();
 
    glPopMatrix();
}
 
void GLWidget::_drawCylinder(GLfloat radius, GLfloat height, GLubyte R, GLubyte G, GLubyte B, GLuint texture)
{
    glEnable(GL_TEXTURE_2D);
    glBindTexture(GL_TEXTURE_2D, texture);
 
    double i, resolution = 0.001;
 
    glPushMatrix();
    glRotatef(90, 1, 0, 0);
 
    /* Top triangle */
    glBegin(GL_TRIANGLE_FAN);
    glTexCoord2f(0.5, 0.5);
    glVertex3f(0, height, 0);
    i = 2 * PI;
    while(i >= 0)
    {
        glTexCoord2f(0.5f * cos(i) + 0.5f, 0.5f * sin(i) + 0.5f);
        glVertex3f(radius * cos(i), height, radius * sin(i));
        i -= resolution;
    }
    glTexCoord2f(0.5, 0.5);
    glVertex3f(radius, height, 0);
    glEnd();
 
    glDisable(GL_TEXTURE_2D);
 
    /* Bottom triangle */
    glColor3ub(R, G, B);
    glBegin(GL_TRIANGLE_FAN);
    // glTexCoord2f(0.5, 0.5);
    glVertex3f(0, 0, 0);
    i = 0;
    while(i <= 2 * PI)
    {
        // glTexCoord2f(0.5f * cos(i) + 0.5f, 0.5f * sin(i) + 0.5f);
        glVertex3f(radius * cos(i), 0, radius * sin(i));
        i += resolution;
    }
    glEnd();
 
    /* Middle tube */
    glColor3ub(R, G, B);
    glBegin(GL_QUAD_STRIP);
    i = 0;
    while(i <= 2 * PI)
    {
        // const float tc = (i / (float)(2 * PI));
        // glTexCoord2f(tc, 0.0);
        glVertex3f(radius * cos(i), 0, radius * sin(i));
        // glTexCoord2f(tc, 1.0);
        glVertex3f(radius * cos(i), height, radius * sin(i));
        i += resolution;
    }
    // glTexCoord2f(0.0, 0.0);
    glVertex3f(radius, 0, 0);
    // glTexCoord2f(0.0, 1.0);
    glVertex3f(radius, height, 0);
    glEnd();
 
    glPopMatrix();
}