CAxis.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "opengl-precomp.h"  // Precompiled header

#include <mrpt/opengl/CAxis.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/os.h>
#include <mrpt/opengl/gl_utils.h>

#include "opengl_internals.h"

using namespace mrpt;
using namespace mrpt::opengl;
using namespace mrpt::system;

using namespace std;

IMPLEMENTS_SERIALIZABLE(CAxis, CRenderizableDisplayList, mrpt::opengl)

CAxis::CAxis(
    float xmin, float ymin, float zmin, float xmax, float ymax, float zmax,
    float frecuency, float lineWidth, bool marks)
    : m_xmin(xmin),
      m_ymin(ymin),
      m_zmin(zmin),
      m_xmax(xmax),
      m_ymax(ymax),
      m_zmax(zmax),
      m_frequency(frecuency),
      m_lineWidth(lineWidth),
      m_textScale(0.25f)
{
    for (int i = 0; i < 3; i++) m_marks[i] = marks;

    // x:180, 0, 90
    m_textRot[0][0] = 180.f;
    m_textRot[0][1] = 0.f;
    m_textRot[0][2] = 90.f;
    // y:90, 0, 90
    m_textRot[1][0] = 90.f;
    m_textRot[1][1] = 0.f;
    m_textRot[1][2] = 90.f;
    // z:180, 0, 90
    m_textRot[2][0] = 180.f;
    m_textRot[2][1] = 0.f;
    m_textRot[2][2] = 90.f;
}

CAxis::Ptr CAxis::Create(
    float xmin, float ymin, float zmin, float xmax, float ymax, float zmax,
    float frecuency, float lineWidth, bool marks)
{
    return CAxis::Ptr(
        new CAxis(
            xmin, ymin, zmin, xmax, ymax, zmax, frecuency, lineWidth, marks));
}

void CAxis::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
    MRPT_START
    glDisable(GL_LIGHTING);

    glEnable(GL_BLEND);
    checkOpenGLError();
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    checkOpenGLError();

    ASSERT_(m_frequency >= 0);

    glLineWidth(m_lineWidth);
    checkOpenGLError();
    glBegin(GL_LINES);
    glColor4ub(m_color.R, m_color.G, m_color.B, m_color.A);
    // X axis
    glVertex3f(m_xmin, 0.0f, 0.0f);
    glVertex3f(m_xmax, 0.0f, 0.0f);
    // Y axis
    glVertex3f(0.0f, m_ymin, 0.0f);
    glVertex3f(0.0f, m_ymax, 0.0f);
    // Z axis
    glVertex3f(0.0f, 0.0f, m_zmin);
    glVertex3f(0.0f, 0.0f, m_zmax);

    glEnd();
    checkOpenGLError();

    glLineWidth(1.0f);
    checkOpenGLError();

    glDisable(GL_BLEND);
    checkOpenGLError();

    // Draw the "tick marks":
    char n[50];
    if (m_marks[0])
    {
        // X axis
        glPushMatrix();
        glTranslatef(m_xmin, .0f, .05f);
        for (float i = m_xmin; i <= m_xmax; i = i + m_frequency)
        {
            os::sprintf(n, 50, "%.02f", i);
            glPushMatrix();
            glRotatef(m_textRot[0][0], 0, 0, 1);
            glRotatef(m_textRot[0][1], 0, 1, 0);
            glRotatef(m_textRot[0][2], 1, 0, 0);
            gl_utils::glDrawText(n, m_textScale, mrpt::opengl::FILL);
            glPopMatrix();
            glTranslatef(m_frequency, 0, 0);
        }

        glPopMatrix();
        glPushMatrix();
        glTranslatef(m_xmax + 1.0f * m_frequency, 0, 0);
        glRotatef(m_textRot[0][0], 0, 0, 1);
        glRotatef(m_textRot[0][1], 0, 1, 0);
        glRotatef(m_textRot[0][2], 1, 0, 0);
        gl_utils::glDrawText("+X", m_textScale * 1.2, mrpt::opengl::NICE);
        glPopMatrix();
    }
    if (m_marks[1])
    {
        // Y axis
        glPushMatrix();
        glTranslatef(.0f, m_ymin, .05f);
        for (float i = m_ymin; i <= m_ymax; i = i + m_frequency)
        {
            if (std::abs(i) > 1e-4)
            {  // Dont draw the "0" more than once
                os::sprintf(n, 50, "%.02f", i);
                glPushMatrix();
                glRotatef(m_textRot[1][0], 0, 0, 1);
                glRotatef(m_textRot[1][1], 0, 1, 0);
                glRotatef(m_textRot[1][2], 1, 0, 0);
                gl_utils::glDrawText(n, m_textScale, mrpt::opengl::FILL);
                glPopMatrix();
            }
            glTranslatef(0, m_frequency, 0);
        }

        glPopMatrix();
        glPushMatrix();
        glTranslatef(0, m_ymax + .5f * m_frequency, 0);
        glRotatef(m_textRot[1][0], 0, 0, 1);
        glRotatef(m_textRot[1][1], 0, 1, 0);
        glRotatef(m_textRot[1][2], 1, 0, 0);
        gl_utils::glDrawText("+Y", m_textScale * 1.2, mrpt::opengl::NICE);
        glPopMatrix();
    }
    if (m_marks[2])
    {
        // Z axis
        glPushMatrix();
        glTranslatef(.0f, .0f, m_zmin);
        for (float i = m_zmin; i <= m_zmax; i = i + m_frequency)
        {
            if (std::abs(i) > 1e-4)
            {  // Dont draw the "0" more than once
                glPushMatrix();
                glRotatef(m_textRot[2][0], 0, 0, 1);
                glRotatef(m_textRot[2][1], 0, 1, 0);
                glRotatef(m_textRot[2][2], 1, 0, 0);
                os::sprintf(n, 50, "%.02f", i);
                gl_utils::glDrawText(n, m_textScale, mrpt::opengl::FILL);
                glPopMatrix();
            }
            glTranslatef(0, 0, m_frequency);
        }

        glPopMatrix();
        glPushMatrix();
        glTranslatef(0, 0, m_zmax + 0.5f * m_frequency);
        glRotatef(m_textRot[2][0], 0, 0, 1);
        glRotatef(m_textRot[2][1], 0, 1, 0);
        glRotatef(m_textRot[2][2], 1, 0, 0);
        gl_utils::glDrawText("+Z", m_textScale * 1.2, mrpt::opengl::NICE);
        glPopMatrix();
    }

    glEnable(GL_LIGHTING);
    MRPT_END
/*******************************************************/
#endif
}

uint8_t CAxis::serializeGetVersion() const { return 1; }
void CAxis::serializeTo(mrpt::serialization::CArchive& out) const
{
    writeToStreamRender(out);
    out << m_xmin << m_ymin << m_zmin;
    out << m_xmax << m_ymax << m_zmax;
    out << m_frequency << m_lineWidth;
    // v1:
    out << m_marks[0] << m_marks[1] << m_marks[2] << m_textScale;
    for (int i = 0; i < 3; i++)
        for (int j = 0; j < 3; j++) out << m_textRot[i][j];
}

void CAxis::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
    switch (version)
    {
        case 0:
        case 1:
        {
            readFromStreamRender(in);
            in >> m_xmin >> m_ymin >> m_zmin;
            in >> m_xmax >> m_ymax >> m_zmax;
            in >> m_frequency >> m_lineWidth;
            if (version >= 1)
            {
                in >> m_marks[0] >> m_marks[1] >> m_marks[2] >> m_textScale;
                for (int i = 0; i < 3; i++)
                    for (int j = 0; j < 3; j++) in >> m_textRot[i][j];
            }
            else
            {
                bool v;
                in >> v;
                for (int i = 0; i < 3; i++) m_marks[i] = v;
                m_textScale = 0.25f;
            }
        }
        break;
        default:
            MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
    };
    CRenderizableDisplayList::notifyChange();
}

void CAxis::getBoundingBox(
    mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
    bb_min.x = m_xmin;
    bb_min.y = m_ymin;
    bb_min.z = m_zmin;

    bb_max.x = m_xmax;
    bb_max.y = m_ymax;
    bb_max.z = m_zmax;

    // Convert to coordinates of my parent:
    m_pose.composePoint(bb_min, bb_min);
    m_pose.composePoint(bb_max, bb_max);
}

void CAxis::setFrequency(float f)
{
    ASSERT_(f > 0);
    m_frequency = f;
    CRenderizableDisplayList::notifyChange();
}
float CAxis::getFrequency() const { return m_frequency; }
void CAxis::setLineWidth(float w)
{
    m_lineWidth = w;
    CRenderizableDisplayList::notifyChange();
}
float CAxis::getLineWidth() const { return m_lineWidth; }
void CAxis::enableTickMarks(bool v)
{
    for (int i = 0; i < 3; i++) m_marks[i] = v;
    CRenderizableDisplayList::notifyChange();
}
void CAxis::enableTickMarks(bool show_x, bool show_y, bool show_z)
{
    m_marks[0] = show_x;
    m_marks[1] = show_y;
    m_marks[2] = show_z;
    CRenderizableDisplayList::notifyChange();
}
void CAxis::setTextScale(float f)
{
    ASSERT_(f > 0);
    m_textScale = f;
    CRenderizableDisplayList::notifyChange();
}
float CAxis::getTextScale() const { return m_textScale; }
void CAxis::setAxisLimits(
    float xmin, float ymin, float zmin, float xmax, float ymax, float zmax)
{
    m_xmin = xmin;
    m_ymin = ymin;
    m_zmin = zmin;
    m_xmax = xmax;
    m_ymax = ymax;
    m_zmax = zmax;
    CRenderizableDisplayList::notifyChange();
}
void CAxis::setTextLabelOrientation(
    int axis, float yaw_deg, float pitch_deg, float roll_deg)
{
    ASSERT_(axis >= 0 && axis < 3);
    m_textRot[axis][0] = yaw_deg;
    m_textRot[axis][1] = pitch_deg;
    m_textRot[axis][2] = roll_deg;
}
void CAxis::getTextLabelOrientation(
    int axis, float& yaw_deg, float& pitch_deg, float& roll_deg) const
{
    ASSERT_(axis >= 0 && axis < 3);
    yaw_deg = m_textRot[axis][0];
    pitch_deg = m_textRot[axis][1];
    roll_deg = m_textRot[axis][2];
}