CEllipsoid.h

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 |
   +------------------------------------------------------------------------+ */
#ifndef opengl_CEllipsoid_H
#define opengl_CEllipsoid_H

#include <mrpt/opengl/CRenderizableDisplayList.h>
#include <mrpt/math/CMatrixD.h>
#include <mrpt/math/CMatrixFixedNumeric.h>

namespace mrpt::opengl
{
/** A 2D ellipse or 3D ellipsoid, depending on the size of the m_cov matrix (2x2
  *or 3x3).
  *  The center of the ellipsoid is the "m_x,m_y,m_z" object's coordinates. In
  *the case of
  *   a 2D ellipse it will be drawn in the XY plane, for z=0.
  *  The color is determined by the RGBA fields in the class "CRenderizable".
  *Note that a
  *   transparent ellipsoid can be drawn for "0<alpha<1" values.
  *  If one of the eigen value of the covariance matrix of the ellipsoid is
  *null, ellipsoid will not be rendered.
  *  \sa opengl::COpenGLScene
  *
  *
  * Please read the documentation of
  *CGeneralizedEllipsoidTemplate::setQuantiles() for learning
  *  the mathematical details about setting the desired confidence interval.
  *
  *  <div align="center">
  *  <table border="0" cellspan="4" cellspacing="4" style="border-width: 1px;
  *border-style: solid;">
  *   <tr> <td> mrpt::opengl::CEllipsoid </td> <td> \image html
  *preview_CEllipsoid.png </td> </tr>
  *  </table>
  *  </div>
  *
  * \ingroup mrpt_opengl_grp
  */
class CEllipsoid : public CRenderizableDisplayList
{
    DEFINE_SERIALIZABLE(CEllipsoid)

   protected:
    /** Used to store computed values the first time this is rendered, and to
     * avoid recomputing them again.
     */
    math::CMatrixD m_eigVal, m_eigVec, m_prevComputedCov;

    /** The 2x2 or 3x3 covariance matrix that will determine the aspect of the
     * ellipsoid. */
    math::CMatrixD m_cov;
    /** If set to true (default), a whole ellipsoid surface will be drawn, or if
     * set to "false" it will be drawn as a "wireframe". */
    bool m_drawSolid3D;
    /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3) */
    float m_quantiles;
    /** The number of segments of a 2D ellipse (default=20) */
    unsigned int m_2D_segments;
    /** The number of segments of a 3D ellipse (in both "axis") (default=20) */
    unsigned int m_3D_segments;
    /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
    float m_lineWidth;
    mutable mrpt::math::TPoint3D m_bb_min, m_bb_max;

   public:
    /** Set the 2x2 or 3x3 covariance matrix that will determine the aspect of
     * the ellipsoid (if resizeToSize>0, the matrix will be cut to the square
     * matrix of the given size) */
    void setCovMatrix(
        const mrpt::math::CMatrixDouble& m, int resizeToSize = -1);
    /** Set the 2x2 or 3x3 covariance matrix that will determine the aspect of
     * the ellipsoid (if resizeToSize>0, the matrix will be cut to the square
     * matrix of the given size). */
    void setCovMatrix(const mrpt::math::CMatrixFloat& m, int resizeToSize = -1);

    /**  Set the 2x2 or 3x3 covariance matrix that will determine the aspect of
     * the ellipsoid (if resizeToSize>0, the matrix will be cut to the square
     * matrix of the given size)
     */
    template <typename T>
    void setCovMatrix(
        const mrpt::math::CMatrixFixedNumeric<T, 3, 3>& m,
        int resizeToSize = -1)
    {
        setCovMatrix(mrpt::math::CMatrixTemplateNumeric<T>(m), resizeToSize);
    }

    /**  Set the 2x2 or 3x3 covariance matrix that will determine the aspect of
     * the ellipsoid (if resizeToSize>0, the matrix will be cut to the square
     * matrix of the given size)
     */
    template <typename T>
    void setCovMatrix(const mrpt::math::CMatrixFixedNumeric<T, 2, 2>& m)
    {
        setCovMatrix(mrpt::math::CMatrixTemplateNumeric<T>(m));
    }

    mrpt::math::CMatrixDouble getCovMatrix() const
    {
        return mrpt::math::CMatrixDouble(m_cov);
    }

    /** If set to true (default), a whole ellipsoid surface will be drawn, or if
     * set to "false" it will be drawn as a "wireframe". */
    void enableDrawSolid3D(bool v)
    {
        m_drawSolid3D = v;
        CRenderizableDisplayList::notifyChange();
    }
    /** The number of "sigmas" for drawing the ellipse/ellipsoid (default=3) */
    void setQuantiles(float q)
    {
        m_quantiles = q;
        CRenderizableDisplayList::notifyChange();
    }
    float getQuantiles() const { return m_quantiles; }
    /** The number of segments of a 2D ellipse (default=20) */
    void set2DsegmentsCount(unsigned int N)
    {
        m_2D_segments = N;
        CRenderizableDisplayList::notifyChange();
    }
    /** The number of segments of a 3D ellipse (in both "axis") (default=20) */
    void set3DsegmentsCount(unsigned int N)
    {
        m_3D_segments = N;
        CRenderizableDisplayList::notifyChange();
    }

    /** The line width for 2D ellipses or 3D wireframe ellipsoids (default=1) */
    void setLineWidth(float w)
    {
        m_lineWidth = w;
        CRenderizableDisplayList::notifyChange();
    }
    float getLineWidth() const { return m_lineWidth; }
    /** Render
      * If one of the eigen value of the covariance matrix of the ellipsoid is
      *null, ellipsoid will not
      * be rendered to ensure stability in the rendering process.
      */
    void render_dl() const override;

    /** Evaluates the bounding box of this object (including possible children)
     * in the coordinate frame of the object parent. */
    void getBoundingBox(
        mrpt::math::TPoint3D& bb_min,
        mrpt::math::TPoint3D& bb_max) const override;

    /** Ray tracing
      */
    bool traceRay(const mrpt::poses::CPose3D& o, double& dist) const override;

    /** Constructor
      */
    CEllipsoid()
        : m_eigVal(),
          m_eigVec(),
          m_prevComputedCov(),
          m_cov(2, 2),
          m_drawSolid3D(true),
          m_quantiles(3),
          m_2D_segments(20),
          m_3D_segments(20),
          m_lineWidth(1.0),
          m_bb_min(0, 0, 0),
          m_bb_max(0, 0, 0)
    {
    }
    /** Private, virtual destructor: only can be deleted from smart pointers */
    virtual ~CEllipsoid() {}
};

}
#endif