CArrow.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/CArrow.h>
#include <mrpt/math/CMatrix.h>
#include <mrpt/math/geometry.h>
#include <mrpt/serialization/CArchive.h>
 
#include "opengl_internals.h"
 
using namespace mrpt;
using namespace mrpt::opengl;
 
using namespace mrpt::math;
using namespace std;
 
IMPLEMENTS_SERIALIZABLE(CArrow, CRenderizableDisplayList, mrpt::opengl)
 
/** Class factory  */
CArrow::Ptr CArrow::Create(
        float x0, float y0, float z0, float x1, float y1, float z1, float headRatio,
        float smallRadius, float largeRadius, float arrow_roll, float arrow_pitch,
        float arrow_yaw)
{
        return CArrow::Ptr(
                new CArrow(
                        x0, y0, z0, x1, y1, z1, headRatio, smallRadius, largeRadius,
                        arrow_roll, arrow_pitch, arrow_yaw));
}
/*---------------------------------------------------------------
                                                        render
  ---------------------------------------------------------------*/
void CArrow::render_dl() const
{
#if MRPT_HAS_OPENGL_GLUT
 
        GLUquadricObj* obj1 = gluNewQuadric();
        GLUquadricObj* obj2 = gluNewQuadric();
 
        GLfloat mat[16];
 
        // Compute the direction vector, which will become the transformed z-axis:
        float vx = m_x1 - m_x0;
        float vy = m_y1 - m_y0;
        float vz = m_z1 - m_z0;
        if ((m_arrow_roll != -1.0f) || (m_arrow_pitch != -1.0f) ||
                (m_arrow_yaw != -1.0f))
        {
                m_x0 = 0.0f;
                m_x1 = 0.0f;
                m_y0 = 0.0f;
                m_y1 = 0.1f;
                m_z0 = 0.0f;
                m_z1 = 0.0f;
 
                float cr = cos(m_arrow_roll);
                float sr = sin(m_arrow_roll);
                float cp = cos(m_arrow_pitch);
                float sp = sin(m_arrow_pitch);
                float cy = cos(m_arrow_yaw);
                float sy = sin(m_arrow_yaw);
 
                CMatrixFloat m(3, 3), xx(3, 1), out(1, 3);
                m(0, 0) = cr * cp;
                m(0, 1) = cr * sp * sy - sr * cy;
                m(0, 2) = sr * sy + cr * sp * cy;
                m(1, 0) = sr * cp;
                m(1, 1) = sr * sp * sy + cr * cy;
                m(1, 2) = sr * sp * cy - cr * sy;
                m(2, 0) = -sp;
                m(2, 1) = cp * sy;
                m(2, 2) = cp * cy;
                xx(0, 0) = 0.0f;
                xx(1, 0) = 1.0f;
                xx(2, 0) = 0.0f;
 
                out = m * xx;
                vx = out(0, 0);
                vy = out(1, 0);
                vz = out(2, 0);
        }
 
        // Normalize:
        const float v_mod = sqrt(square(vx) + square(vy) + square(vz));
        if (v_mod > 0)
        {
                vx /= v_mod;
                vy /= v_mod;
                vz /= v_mod;
        }
 
        //  A homogeneous transformation matrix, in this order:
        //
        //     0  4  8  12
        //     1  5  9  13
        //     2  6  10 14
        //     3  7  11 15
        //
 
        mat[3] = mat[7] = mat[11] = 0;
        mat[15] = 1;
        mat[12] = m_x0;
        mat[13] = m_y0;
        mat[14] = m_z0;
 
        // New Z-axis
        mat[8] = vx;
        mat[9] = vy;
        mat[10] = vz;
 
        // New X-axis: Perp. to Z
        if (vx != 0 || vy != 0)
        {
                mat[0] = -vy;
                mat[1] = vx;
                mat[2] = 0;
        }
        else
        {
                mat[0] = 0;
                mat[1] = vz;
                mat[2] = -vy;
        }
 
        // New Y-axis: Perp. to both: the cross product:
        //  | i  j  k |     | i  j  k |
        //  | x0 y0 z0| --> | 8  9  10|
        //  | x1 y1 z1|     | 0  1  2 |
        GLfloat* out_v3 = mat + 4;
        math::crossProduct3D(
                mat + 8,  // 1st vector
                mat + 0,  // 2nd vector
                out_v3  // Output cross product
                );
 
        glPushMatrix();
 
        glMultMatrixf(mat);
        // Scale Z to the size of the cylinder:
        glScalef(1.0f, 1.0f, v_mod * (1.0f - m_headRatio));
        gluCylinder(obj1, m_smallRadius, m_smallRadius, 1, 10, 1);
 
        glPopMatrix();
 
        // Draw the head of the arrow: a cone (built from a cylinder)
        //-------------------------------------------------------------
        mat[12] = m_x0 + vx * v_mod * (1.0f - m_headRatio);
        mat[13] = m_y0 + vy * v_mod * (1.0f - m_headRatio);
        mat[14] = m_z0 + vz * v_mod * (1.0f - m_headRatio);
 
        glPushMatrix();
 
        glMultMatrixf(mat);
        // Scale Z to the size of the cylinder:
        glScalef(1.0f, 1.0f, v_mod * m_headRatio);
 
        gluCylinder(obj2, m_largeRadius, 0, 1, 10, 10);
 
        glPopMatrix();
 
        gluDeleteQuadric(obj1);
        gluDeleteQuadric(obj2);
 
#endif
}
 
uint8_t CArrow::serializeGetVersion() const { return 1; }
void CArrow::serializeTo(mrpt::serialization::CArchive& out) const
{
        writeToStreamRender(out);
        out << m_x0 << m_y0 << m_z0;
        out << m_x1 << m_y1 << m_z1;
        out << m_headRatio << m_smallRadius << m_largeRadius;
        out << m_arrow_roll << m_arrow_pitch << m_arrow_yaw;
}
 
void CArrow::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
        switch (version)
        {
                case 0:
                {
                        readFromStreamRender(in);
                        in >> m_x0 >> m_y0 >> m_z0;
                        in >> m_x1 >> m_y1 >> m_z1;
                        in >> m_headRatio >> m_smallRadius >> m_largeRadius;
                }
                break;
                case 1:
                {
                        readFromStreamRender(in);
                        in >> m_x0 >> m_y0 >> m_z0;
                        in >> m_x1 >> m_y1 >> m_z1;
                        in >> m_headRatio >> m_smallRadius >> m_largeRadius;
                        in >> m_arrow_roll >> m_arrow_pitch >> m_arrow_yaw;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
        CRenderizableDisplayList::notifyChange();
}
 
void CArrow::getBoundingBox(
        mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
{
        bb_min.x = std::min(m_x0, m_x1);
        bb_min.y = std::min(m_y0, m_y1);
        bb_min.z = std::min(m_z0, m_z1);
 
        bb_max.x = std::max(m_x0, m_x1);
        bb_max.y = std::max(m_y0, m_y1);
        bb_max.z = std::max(m_z0, m_z1);
 
        // Convert to coordinates of my parent:
        m_pose.composePoint(bb_min, bb_min);
        m_pose.composePoint(bb_max, bb_max);
}