lightweight_geom_data.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 |
   +------------------------------------------------------------------------+ */
#pragma once
 
#include <mrpt/core/bits_math.h>
#include <mrpt/typemeta/TTypeName.h>
#include <mrpt/math/math_frwds.h>  // forward declarations
#include <mrpt/math/lightweight_geom_data_frwds.h>  // forward declarations
#include <mrpt/math/eigen_frwds.h>  // forward declarations
#include <mrpt/math/wrap2pi.h>
#include <mrpt/core/format.h>
#include <mrpt/core/exceptions.h>
#include <vector>
#include <stdexcept>
#include <type_traits>
#include <mrpt/serialization/serialization_frwds.h>
 
namespace mrpt
{
namespace math
{
/** Base type of all TPoseXX and TPointXX classes in mrpt::math.
 * Useful for type traits. No virtual methods at all. */
struct TPoseOrPoint
{
};
 
/** \addtogroup geometry_grp
 * @{ */
 
// Set of typedefs for lightweight geometric items.
/**
 * Lightweight 2D point. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPoint2D
 */
struct TPoint2D : public TPoseOrPoint
{
    enum
    {
        static_size = 2
    };
    /** X,Y coordinates */
    double x, y;
    /** Constructor from TPose2D, discarding phi.
     * \sa TPose2D
     */
    explicit TPoint2D(const TPose2D& p);
    /**
     * Constructor from TPoint3D, discarding z.
     * \sa TPoint3D
     */
    explicit TPoint2D(const TPoint3D& p);
    /**
     * Constructor from TPose3D, discarding z and the angular coordinates.
     * \sa TPose3D
     */
    explicit TPoint2D(const TPose3D& p);
 
    /**
     * Constructor from coordinates.
     */
    constexpr TPoint2D(double xx, double yy) : x(xx), y(yy) {}
    /**
     * Default fast constructor. Initializes to garbage.
     */
    TPoint2D() {}
    /** Coordinate access using operator[]. Order: x,y */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: x,y */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            default:
                throw std::out_of_range("index out of range");
        }
    }
 
    /**
     * Transformation into vector.
     */
    void getAsVector(std::vector<double>& v) const
    {
        v.resize(2);
        v[0] = x;
        v[1] = y;
    }
 
    bool operator<(const TPoint2D& p) const;
 
    TPoint2D& operator+=(const TPoint2D& p)
    {
        x += p.x;
        y += p.y;
        return *this;
    }
 
    TPoint2D& operator-=(const TPoint2D& p)
    {
        x -= p.x;
        y -= p.y;
        return *this;
    }
 
    TPoint2D& operator*=(double d)
    {
        x *= d;
        y *= d;
        return *this;
    }
 
    TPoint2D& operator/=(double d)
    {
        x /= d;
        y /= d;
        return *this;
    }
 
    constexpr TPoint2D operator+(const TPoint2D& p) const
    {
        return {x + p.x, y + p.y};
    }
 
    constexpr TPoint2D operator-(const TPoint2D& p) const
    {
        return {x - p.x, y - p.y};
    }
 
    constexpr TPoint2D operator*(double d) const
    {
        return {d*x, d*y};
    }
 
    constexpr TPoint2D operator/(double d) const
    {
        return {x/d, y/d};
    }
    /** Returns a human-readable textual representation of the object (eg:
     * "[0.02 1.04]" )
     * \sa fromString
     */
    void asString(std::string& s) const { s = mrpt::format("[%f %f]", x, y); }
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[0.02 1.04]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
    static size_t size() { return 2; }
    /** Point norm. */
    double norm() const { return sqrt(square(x) + square(y)); }
};
 
/**
 * Lightweight 2D pose. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPose2D
 */
struct TPose2D : public TPoseOrPoint
{
    enum
    {
        static_size = 3
    };
    /** X,Y coordinates */
    double x, y;
    /** Orientation (rads) */
    double phi;
    /** Implicit constructor from TPoint2D. Zeroes the phi coordinate.
     * \sa TPoint2D
     */
    TPose2D(const TPoint2D& p);
    /**
     * Constructor from TPoint3D, losing information. Zeroes the phi
     * coordinate.
     * \sa TPoint3D
     */
    explicit TPose2D(const TPoint3D& p);
    /**
     * Constructor from TPose3D, losing information. The phi corresponds to the
     * original pose's yaw.
     * \sa TPose3D
     */
    explicit TPose2D(const TPose3D& p);
    /**
     * Constructor from coordinates.
     */
    TPose2D(double xx, double yy, double pphi) : x(xx), y(yy), phi(pphi)
    {
    }
    /**
     * Default fast constructor. Initializes to garbage.
     */
    TPose2D() {}
    /** Coordinate access using operator[]. Order: x,y,phi */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return phi;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: x,y,phi */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return phi;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /**
     * Transformation into vector.
     */
    void getAsVector(std::vector<double>& v) const
    {
        v.resize(3);
        v[0] = x;
        v[1] = y;
        v[2] = phi;
    }
    /** Returns a human-readable textual representation of the object (eg: "[x y
     * yaw]", yaw in degrees)
     * \sa fromString
     */
    void asString(std::string& s) const;
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Operator "oplus" pose composition: "ret=this \oplus b"  \sa CPose2D */
    mrpt::math::TPose2D operator+(const mrpt::math::TPose2D& b) const;
    /** Operator "ominus" pose composition: "ret=this \ominus b"  \sa CPose2D */
    mrpt::math::TPose2D operator-(const mrpt::math::TPose2D& b) const;
 
    mrpt::math::TPoint2D composePoint(const TPoint2D l) const
    {
        const double ccos = ::cos(phi), csin = ::sin(phi);
        return {x + l.x * ccos - l.y * csin, y + l.x * csin + l.y * ccos};
    }
 
    mrpt::math::TPoint2D operator+(const mrpt::math::TPoint2D& b) const
    {
        return this->composePoint(b);
    }
 
    mrpt::math::TPoint2D inverseComposePoint(const TPoint2D g) const
    {
        const double Ax = g.x - x, Ay = g.y - y, ccos = ::cos(phi),
                     csin = ::sin(phi);
        return { Ax * ccos + Ay * csin, -Ax * csin + Ay * ccos };
    }
 
    /** Returns the norm of the (x,y) vector (phi is not used) */
    double norm() const { return mrpt::hypot_fast(x, y); }
    /** Forces "phi" to be in the range [-pi,pi] */
    void normalizePhi() { phi = mrpt::math::wrapToPi(phi); }
 
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[0.02 1.04 -45.0]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
 
    constexpr static size_t size() { return 3; }
};
 
/** Lightweight 3D point (float version).
 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3D
 */
struct TPoint3Df : public TPoseOrPoint
{
    enum
    {
        static_size = 3
    };
    float x;
    float y;
    float z;
 
    TPoint3Df() {}
    constexpr TPoint3Df(const float xx, const float yy, const float zz)
        : x(xx), y(yy), z(zz)
    {
    }
    TPoint3Df& operator+=(const TPoint3Df& p)
    {
        x += p.x;
        y += p.y;
        z += p.z;
        return *this;
    }
    TPoint3Df operator*(const float s)
    {
        return TPoint3Df(x * s, y * s, z * s);
    }
    /** Coordinate access using operator[]. Order: x,y,z */
    float& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            default:
                throw std::out_of_range("index out of range");
        }
    }
 
    /** Coordinate access using operator[]. Order: x,y,z */
    constexpr const float& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            default:
                throw std::out_of_range("index out of range");
        }
    }
};
 
/**
 * Lightweight 3D point. Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
 */
struct TPoint3D : public TPoseOrPoint
{
    enum
    {
        static_size = 3
    };
    /** X,Y,Z coordinates */
    double x, y, z;
 
    /** Constructor from coordinates.  */
    constexpr TPoint3D(double xx, double yy, double zz) : x(xx), y(yy), z(zz) {}
    /** Default fast constructor. Initializes to garbage. */
    TPoint3D() {}
    /** Explicit constructor from coordinates.  */
    explicit TPoint3D(const TPoint3Df& p) : x(p.x), y(p.y), z(p.z) {}
    /** Implicit constructor from TPoint2D. Zeroes the z.
     * \sa TPoint2D
     */
    TPoint3D(const TPoint2D& p);
    /**
     * Constructor from TPose2D, losing information. Zeroes the z.
     * \sa TPose2D
     */
    explicit TPoint3D(const TPose2D& p);
    /**
     * Constructor from TPose3D, losing information.
     * \sa TPose3D
     */
    explicit TPoint3D(const TPose3D& p);
    /** Coordinate access using operator[]. Order: x,y,z */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: x,y,z */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /**
     * Point-to-point distance.
     */
    double distanceTo(const TPoint3D& p) const
    {
        return sqrt(square(p.x - x) + square(p.y - y) + square(p.z - z));
    }
    /**
     * Point-to-point distance, squared.
     */
    double sqrDistanceTo(const TPoint3D& p) const
    {
        return square(p.x - x) + square(p.y - y) + square(p.z - z);
    }
    /**
     * Point norm.
     */
    double norm() const
    {
        return sqrt(square(x) + square(y) + square(z));
    }
    /**
     * Point scale.
     */
    TPoint3D& operator*=(const double f)
    {
        x *= f;
        y *= f;
        z *= f;
        return *this;
    }
    /**
     * Transformation into vector.
     */
    template <class VECTORLIKE>
    void getAsVector(VECTORLIKE& v) const
    {
        v.resize(3);
        v[0] = x;
        v[1] = y;
        v[2] = z;
    }
    /**
     * Translation.
     */
    TPoint3D& operator+=(const TPoint3D& p)
    {
        x += p.x;
        y += p.y;
        z += p.z;
        return *this;
    }
    /**
     * Difference between points.
     */
    TPoint3D& operator-=(const TPoint3D& p)
    {
        x -= p.x;
        y -= p.y;
        z -= p.z;
        return *this;
    }
    /**
     * Points addition.
     */
    constexpr TPoint3D operator+(const TPoint3D& p) const
    {
        return {x + p.x, y + p.y, z + p.z};
    }
    /**
     * Points substraction.
     */
    constexpr TPoint3D operator-(const TPoint3D& p) const
    {
        return {x - p.x, y - p.y, z - p.z};
    }
 
    constexpr TPoint3D operator*(double d) const
    {
        return {x * d, y * d, z * d};
    }
 
    constexpr TPoint3D operator/(double d) const
    {
        return {x / d, y / d, z / d};
    }
 
    bool operator<(const TPoint3D& p) const;
 
    /** Returns a human-readable textual representation of the object (eg:
     * "[0.02 1.04 -0.8]" )
     * \sa fromString
     */
    void asString(std::string& s) const
    {
        s = mrpt::format("[%f %f %f]", x, y, z);
    }
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[0.02 1.04 -0.8]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
    static constexpr size_t size() { return 3; }
};
 
/** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZIu8
{
    mrpt::math::TPoint3D pt;
    uint8_t intensity;
    TPointXYZIu8() : pt(), intensity(0) {}
    constexpr TPointXYZIu8(double x, double y, double z, uint8_t intensity_val)
        : pt(x, y, z), intensity(intensity_val)
    {
    }
};
/** XYZ point (double) + RGB(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZRGBu8
{
    mrpt::math::TPoint3D pt;
    uint8_t R, G, B;
    TPointXYZRGBu8() : pt(), R(0), G(0), B(0) {}
    constexpr TPointXYZRGBu8(
        double x, double y, double z, uint8_t R_val, uint8_t G_val,
        uint8_t B_val)
        : pt(x, y, z), R(R_val), G(G_val), B(B_val)
    {
    }
};
/** XYZ point (float) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfIu8
{
    mrpt::math::TPoint3Df pt;
    uint8_t intensity;
    TPointXYZfIu8() : pt(), intensity(0) {}
    constexpr TPointXYZfIu8(float x, float y, float z, uint8_t intensity_val)
        : pt(x, y, z), intensity(intensity_val)
    {
    }
};
/** XYZ point (float) + RGB(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfRGBu8
{
    mrpt::math::TPoint3Df pt;
    uint8_t R, G, B;
    TPointXYZfRGBu8() : pt(), R(0), G(0), B(0) {}
    constexpr TPointXYZfRGBu8(
        float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val)
        : pt(x, y, z), R(R_val), G(G_val), B(B_val)
    {
    }
};
 
/**
 * Lightweight 3D pose (three spatial coordinates, plus three angular
 * coordinates). Allows coordinate access using [] operator.
 * \sa mrpt::poses::CPose3D
 */
struct TPose3D : public TPoseOrPoint
{
    enum
    {
        static_size = 6
    };
    /** X,Y,Z, coords */
    double x, y, z;
    /** Yaw coordinate (rotation angle over Z axis). */
    double yaw;
    /** Pitch coordinate (rotation angle over Y axis). */
    double pitch;
    /** Roll coordinate (rotation angle over X coordinate). */
    double roll;
    /** Implicit constructor from TPoint2D. Zeroes all the unprovided
     * information.
     * \sa TPoint2D
     */
    TPose3D(const TPoint2D& p);
    /**
     * Implicit constructor from TPose2D. Gets the yaw from the 2D pose's phi,
     * zeroing all the unprovided information.
     * \sa TPose2D
     */
    TPose3D(const TPose2D& p);
    /**
     * Implicit constructor from TPoint3D. Zeroes angular information.
     * \sa TPoint3D
     */
    TPose3D(const TPoint3D& p);
    /**
     * Constructor from coordinates.
     */
    constexpr TPose3D(
        double _x, double _y, double _z, double _yaw, double _pitch,
        double _roll)
        : x(_x), y(_y), z(_z), yaw(_yaw), pitch(_pitch), roll(_roll)
    {
    }
    /**
     * Default fast constructor. Initializes to garbage.
     */
    TPose3D() {}
    /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            case 3:
                return yaw;
            case 4:
                return pitch;
            case 5:
                return roll;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            case 3:
                return yaw;
            case 4:
                return pitch;
            case 5:
                return roll;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /**
     * Pose's spatial coordinates norm.
     */
    double norm() const { return std::sqrt(square(x) + square(y) + square(z)); }
    /**
     * Gets the pose as a vector of doubles.
     */
    void getAsVector(std::vector<double>& v) const
    {
        v.resize(6);
        v[0] = x;
        v[1] = y;
        v[2] = z;
        v[3] = yaw;
        v[4] = pitch;
        v[5] = roll;
    }
    /** Returns a human-readable textual representation of the object (eg: "[x y
     * z yaw pitch roll]", angles in degrees.)
     * \sa fromString
     */
    void asString(std::string& s) const;
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Returns the quaternion associated to the rotation of this object (NOTE:
     * XYZ translation is ignored)
     * \f[ \mathbf{q} = \left( \begin{array}{c} \cos (\phi /2) \cos (\theta /2)
     * \cos (\psi /2) +  \sin (\phi /2) \sin (\theta /2) \sin (\psi /2) \\ \sin
     * (\phi /2) \cos (\theta /2) \cos (\psi /2) -  \cos (\phi /2) \sin (\theta
     * /2) \sin (\psi /2) \\ \cos (\phi /2) \sin (\theta /2) \cos (\psi /2) +
     * \sin (\phi /2) \cos (\theta /2) \sin (\psi /2) \\ \cos (\phi /2) \cos
     * (\theta /2) \sin (\psi /2) -  \sin (\phi /2) \sin (\theta /2) \cos (\psi
     * /2) \\ \end{array}\right) \f]
     * With : \f$ \phi = roll \f$,  \f$ \theta = pitch \f$ and \f$ \psi = yaw
     * \f$.
     * \param out_dq_dr  If provided, the 4x3 Jacobian of the transformation
     * will be computed and stored here. It's the Jacobian of the transformation
     * from (yaw pitch roll) to (qr qx qy qz).
     */
    void getAsQuaternion(
        mrpt::math::CQuaternion<double>& q,
        mrpt::math::CMatrixFixedNumeric<double, 4, 3>* out_dq_dr = NULL) const;
 
    void composePoint(const TPoint3D l, TPoint3D& g) const;
    void inverseComposePoint(const TPoint3D g, TPoint3D& l) const;
    void composePose(const TPose3D other, TPose3D& result) const;
    void getRotationMatrix(mrpt::math::CMatrixDouble33& R) const;
    void getHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const;
    void getInverseHomogeneousMatrix(mrpt::math::CMatrixDouble44& HG) const;
    void fromHomogeneousMatrix(const mrpt::math::CMatrixDouble44& HG);
    static void SO3_to_yaw_pitch_roll(
        const mrpt::math::CMatrixDouble33& R, double& yaw, double& pitch,
        double& roll);
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[0.02 1.04 -0.8]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
    static size_t size() { return 6; }
};
 
/** Unary $\ominus\$ operator: computes inverse SE(3) element */
TPose3D operator-(const TPose3D& p);
/** Binary $\ominus\$ operator: \$b \ominus a\$ computes the relative SE(3) pose
 * of `b` "as seen from" `a` */
TPose3D operator-(const TPose3D& b, const TPose3D& a);
 
/** Lightweight 3D pose (three spatial coordinates, plus a quaternion ). Allows
 * coordinate access using [] operator.
 * \sa mrpt::poses::CPose3DQuat
 */
struct TPose3DQuat : public TPoseOrPoint
{
    enum
    {
        static_size = 7
    };
    /** Translation in x,y,z */
    double x, y, z;
    /** Unit quaternion part, qr,qx,qy,qz */
    double qr, qx, qy, qz;
 
    /** Constructor from coordinates. */
    constexpr TPose3DQuat(
        double _x, double _y, double _z, double _qr, double _qx, double _qy,
        double _qz)
        : x(_x), y(_y), z(_z), qr(_qr), qx(_qx), qy(_qy), qz(_qz)
    {
    }
    /** Default fast constructor. Initializes to garbage. */
    TPose3DQuat() {}
    /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            case 3:
                return qr;
            case 4:
                return qx;
            case 5:
                return qy;
            case 6:
                return qz;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: x,y,z,qr,qx,qy,qz */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return x;
            case 1:
                return y;
            case 2:
                return z;
            case 3:
                return qr;
            case 4:
                return qx;
            case 5:
                return qy;
            case 6:
                return qz;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Pose's spatial coordinates norm. */
    double norm() const { return sqrt(square(x) + square(y) + square(z)); }
    /** Gets the pose as a vector of doubles. */
    void getAsVector(std::vector<double>& v) const
    {
        v.resize(7);
        for (size_t i = 0; i < 7; i++) v[i] = (*this)[i];
    }
    /** Returns a human-readable textual representation of the object as "[x y z
     * qr qx qy qz]"
     * \sa fromString
     */
    void asString(std::string& s) const
    {
        s = mrpt::format("[%f %f %f %f %f %f %f]", x, y, z, qr, qx, qy, qz);
    }
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[0.02 1.04 -0.8 1.0 0.0 0.0 0.0]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
    static size_t size() { return 7; }
};
 
// Text streaming functions:
std::ostream& operator<<(std::ostream& o, const TPoint2D& p);
std::ostream& operator<<(std::ostream& o, const TPoint3D& p);
std::ostream& operator<<(std::ostream& o, const TPose2D& p);
std::ostream& operator<<(std::ostream& o, const TPose3D& p);
std::ostream& operator<<(std::ostream& o, const TPose3DQuat& p);
 
/**
 * Unary minus operator for 3D points.
 */
constexpr TPoint3D operator-(const TPoint3D& p1)
{
    return {-p1.x, -p1.y, -p1.z};
}
/**
 * Exact comparison between 2D points.
 */
constexpr bool operator==(const TPoint2D& p1, const TPoint2D& p2)
{
    return (p1.x == p2.x) && (p1.y == p2.y);  //-V550
}
/**
 * Exact comparison between 2D points.
 */
constexpr bool operator!=(const TPoint2D& p1, const TPoint2D& p2)
{
    return (p1.x != p2.x) || (p1.y != p2.y);  //-V550
}
/**
 * Exact comparison between 3D points.
 */
constexpr bool operator==(const TPoint3D& p1, const TPoint3D& p2)
{
    return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z);  //-V550
}
/**
 * Exact comparison between 3D points.
 */
constexpr bool operator!=(const TPoint3D& p1, const TPoint3D& p2)
{
    return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z);  //-V550
}
/**
 * Exact comparison between 2D poses, taking possible cycles into account.
 */
inline bool operator==(const TPose2D& p1, const TPose2D& p2)
{
    return (p1.x == p2.x) && (p1.y == p2.y) &&
           (mrpt::math::wrapTo2Pi(p1.phi) ==
            mrpt::math::wrapTo2Pi(p2.phi));  //-V550
}
/**
 * Exact comparison between 2D poses, taking possible cycles into account.
 */
inline bool operator!=(const TPose2D& p1, const TPose2D& p2)
{
    return (p1.x != p2.x) || (p1.y != p2.y) ||
           (mrpt::math::wrapTo2Pi(p1.phi) !=
            mrpt::math::wrapTo2Pi(p2.phi));  //-V550
}
/**
 * Exact comparison between 3D poses, taking possible cycles into account.
 */
inline bool operator==(const TPose3D& p1, const TPose3D& p2)
{
    return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z) &&
           (mrpt::math::wrapTo2Pi(p1.yaw) == mrpt::math::wrapTo2Pi(p2.yaw)) &&
           (mrpt::math::wrapTo2Pi(p1.pitch) ==
            mrpt::math::wrapTo2Pi(p2.pitch)) &&
           (mrpt::math::wrapTo2Pi(p1.roll) ==
            mrpt::math::wrapTo2Pi(p2.roll));  //-V550
}
/**
 * Exact comparison between 3D poses, taking possible cycles into account.
 */
inline bool operator!=(const TPose3D& p1, const TPose3D& p2)
{
    return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z) ||
           (mrpt::math::wrapTo2Pi(p1.yaw) != mrpt::math::wrapTo2Pi(p2.yaw)) ||
           (mrpt::math::wrapTo2Pi(p1.pitch) !=
            mrpt::math::wrapTo2Pi(p2.pitch)) ||
           (mrpt::math::wrapTo2Pi(p1.roll) !=
            mrpt::math::wrapTo2Pi(p2.roll));  //-V550
}
// Forward declarations
struct TSegment3D;
struct TLine3D;
class TPolygon3D;
struct TObject3D;
 
// Pragma defined to ensure no structure packing
/**
 * 2D segment, consisting of two points.
 * \sa TSegment3D,TLine2D,TPolygon2D,TPoint2D
 */
struct TSegment2D
{
   public:
    /**
     * Origin point.
     */
    TPoint2D point1;
    /**
     * Destiny point.
     */
    TPoint2D point2;
    /**
     * Segment length.
     */
    double length() const;
    /**
     * Distance to point.
     */
    double distance(const TPoint2D& point) const;
    /**
     * Distance with sign to point (sign indicates which side the point is).
     */
    double signedDistance(const TPoint2D& point) const;
    /**
     * Check whether a point is inside a segment.
     */
    bool contains(const TPoint2D& point) const;
    /** Access to points using operator[0-1] */
    TPoint2D& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return point1;
            case 1:
                return point2;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Access to points using operator[0-1] */
    constexpr const TPoint2D& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return point1;
            case 1:
                return point2;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /**
     * Project into 3D space, setting the z to 0.
     */
    void generate3DObject(TSegment3D& s) const;
    /**
     * Segment's central point.
     */
    void getCenter(TPoint2D& p) const
    {
        p.x = (point1.x + point2.x) / 2;
        p.y = (point1.y + point2.y) / 2;
    }
    /**
     * Constructor from both points.
     */
    TSegment2D(const TPoint2D& p1, const TPoint2D& p2) : point1(p1), point2(p2)
    {
    }
    /**
     * Fast default constructor. Initializes to garbage.
     */
    TSegment2D() {}
    /**
     * Explicit constructor from 3D object, discarding the z.
     */
    explicit TSegment2D(const TSegment3D& s);
 
    bool operator<(const TSegment2D& s) const;
};
/**
 * 3D segment, consisting of two points.
 * \sa TSegment2D,TLine3D,TPlane,TPolygon3D,TPoint3D
 */
struct TSegment3D
{
   public:
    /**
     * Origin point.
     */
    TPoint3D point1;
    /**
     * Destiny point.
     */
    TPoint3D point2;
    /**
     * Segment length.
     */
    double length() const;
    /**
     * Distance to point.
     */
    double distance(const TPoint3D& point) const;
    /**
     * Distance to another segment.
     */
    double distance(const TSegment3D& segment) const;
    /**
     * Check whether a point is inside the segment.
     */
    bool contains(const TPoint3D& point) const;
    /** Access to points using operator[0-1] */
    TPoint3D& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return point1;
            case 1:
                return point2;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Access to points using operator[0-1] */
    const TPoint3D& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return point1;
            case 1:
                return point2;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /**
     * Projection into 2D space, discarding the z.
     */
    void generate2DObject(TSegment2D& s) const { s = TSegment2D(*this); }
    /**
     * Segment's central point.
     */
    void getCenter(TPoint3D& p) const
    {
        p.x = (point1.x + point2.x) / 2;
        p.y = (point1.y + point2.y) / 2;
        p.z = (point1.z + point2.z) / 2;
    }
    /**
     * Constructor from both points.
     */
    TSegment3D(const TPoint3D& p1, const TPoint3D& p2) : point1(p1), point2(p2)
    {
    }
    /**
     * Fast default constructor. Initializes to garbage.
     */
    TSegment3D() {}
    /**
     * Constructor from 2D object. Sets the z to zero.
     */
    explicit TSegment3D(const TSegment2D& s)
        : point1(s.point1), point2(s.point2)
    {
    }
 
    bool operator<(const TSegment3D& s) const;
};
 
inline bool operator==(const TSegment2D& s1, const TSegment2D& s2)
{
    return (s1.point1 == s2.point1) && (s1.point2 == s2.point2);
}
 
inline bool operator!=(const TSegment2D& s1, const TSegment2D& s2)
{
    return (s1.point1 != s2.point1) || (s1.point2 != s2.point2);
}
 
inline bool operator==(const TSegment3D& s1, const TSegment3D& s2)
{
    return (s1.point1 == s2.point1) && (s1.point2 == s2.point2);
}
 
inline bool operator!=(const TSegment3D& s1, const TSegment3D& s2)
{
    return (s1.point1 != s2.point1) || (s1.point2 != s2.point2);
}
 
/**
 * 2D line without bounds, represented by its equation \f$Ax+By+C=0\f$.
 * \sa TLine3D,TSegment2D,TPolygon2D,TPoint2D
 */
struct TLine2D
{
   public:
    /**
     * Line coefficients, stored as an array: \f$\left[A,B,C\right]\f$.
     */
    double coefs[3];
    /**
     * Evaluate point in the line's equation.
     */
    double evaluatePoint(const TPoint2D& point) const;
    /**
     * Check whether a point is inside the line.
     */
    bool contains(const TPoint2D& point) const;
    /**
     * Distance from a given point.
     */
    double distance(const TPoint2D& point) const;
    /**
     * Distance with sign from a given point (sign indicates side).
     */
    double signedDistance(const TPoint2D& point) const;
    /**
     * Get line's normal vector.
     */
    void getNormalVector(double (&vector)[2]) const;
    /**
     * Unitarize line's normal vector.
     */
    void unitarize();
    /**
     * Get line's normal vector after unitarizing line.
     */
    void getUnitaryNormalVector(double (&vector)[2])
    {
        unitarize();
        getNormalVector(vector);
    }
    /**
     * Get line's director vector.
     */
    void getDirectorVector(double (&vector)[2]) const;
    /**
     * Unitarize line and then get director vector.
     */
    void getUnitaryDirectorVector(double (&vector)[2])
    {
        unitarize();
        getDirectorVector(vector);
    }
    /**
     * Project into 3D space, setting the z to 0.
     */
    void generate3DObject(TLine3D& l) const;
    /**
     * Constructor from two points, through which the line will pass.
     * \throw logic_error if both points are the same
     */
    TLine2D(const TPoint2D& p1, const TPoint2D& p2);
    /**
     * Constructor from a segment.
     */
    explicit TLine2D(const TSegment2D& s);
    /**
     * Fast default constructor. Initializes to garbage.
     */
    TLine2D() {}
    /**
     * Constructor from line's coefficients.
     */
    constexpr TLine2D(double A, double B, double C) : 
        coefs{ A,B,C }
    {
    }
    /**
     * Construction from 3D object, discarding the Z.
     * \throw std::logic_error if the line is normal to the XY plane.
     */
    explicit TLine2D(const TLine3D& l);
    void getAsPose2D(TPose2D& outPose) const;
    void getAsPose2DForcingOrigin(
        const TPoint2D& origin, TPose2D& outPose) const;
};
 
/**
 * 3D line, represented by a base point and a director vector.
 * \sa TLine2D,TSegment3D,TPlane,TPolygon3D,TPoint3D
 */
struct TLine3D
{
   public:
    /**
     * Base point.
     */
    TPoint3D pBase;
    /**
     * Director vector.
     */
    double director[3];
    /**
     * Check whether a point is inside the line.
     */
    bool contains(const TPoint3D& point) const;
    /**
     * Distance between the line and a point.
     */
    double distance(const TPoint3D& point) const;
    /**
     * Unitarize director vector.
     */
    void unitarize();
    /**
     * Get director vector.
     */
    void getDirectorVector(double (&vector)[3]) const
    {
        for (size_t i = 0; i < 3; i++) vector[i] = director[i];
    }
    /**
     * Unitarize and then get director vector.
     */
    void getUnitaryDirectorVector(double (&vector)[3])
    {
        unitarize();
        getDirectorVector(vector);
    }
    /**
     * Project into 2D space, discarding the Z coordinate.
     * \throw std::logic_error if the line's director vector is orthogonal to
     * the XY plane.
     */
    void generate2DObject(TLine2D& l) const { l = TLine2D(*this); }
    /**
     * Constructor from two points, through which the line will pass.
     * \throw std::logic_error if both points are the same.
     */
    TLine3D(const TPoint3D& p1, const TPoint3D& p2);
    /**
     * Constructor from 3D segment.
     */
    explicit TLine3D(const TSegment3D& s);
    /**
     * Fast default constructor. Initializes to garbage.
     */
    TLine3D() {}
    /** Constructor from 2D object. Zeroes the z. */
    explicit TLine3D(const TLine2D& l);
};
 
/**
 * 3D Plane, represented by its equation \f$Ax+By+Cz+D=0\f$
 * \sa TSegment3D,TLine3D,TPolygon3D,TPoint3D
 */
struct TPlane
{
   public:
    /**
     * Plane coefficients, stored as an array: \f$\left[A,B,C,D\right]\f$
     */
    double coefs[4];
    /**
     * Evaluate a point in the plane's equation.
     */
    double evaluatePoint(const TPoint3D& point) const;
    /**
     * Check whether a point is contained into the plane.
     */
    bool contains(const TPoint3D& point) const;
    /**
     * Check whether a segment is fully contained into the plane.
     */
    bool contains(const TSegment3D& segment) const
    {
        return contains(segment.point1) && contains(segment.point2);
    }
    /**
     * Check whether a line is fully contained into the plane.
     */
    bool contains(const TLine3D& line) const;
    /**
     * Distance to 3D point.
     */
    double distance(const TPoint3D& point) const;
    /**
     * Distance to 3D line. Will be zero if the line is not parallel to the
     * plane.
     */
    double distance(const TLine3D& line) const;
    /**
     * Get plane's normal vector.
     */
    void getNormalVector(double (&vec)[3]) const;
    /**
     * Unitarize normal vector.
     */
    void unitarize();
    void getAsPose3D(mrpt::math::TPose3D& outPose);
    /**
     * Unitarize, then get normal vector.
     */
    void getUnitaryNormalVector(double (&vec)[3])
    {
        unitarize();
        getNormalVector(vec);
    }
    /**
     * Gets a plane which contains these three points.
     * \throw std::logic_error if the points are linearly dependants.
     */
    TPlane(const TPoint3D& p1, const TPoint3D& p2, const TPoint3D& p3);
    /**
     * Gets a plane which contains this point and this line.
     * \throw std::logic_error if the point is inside the line.
     */
    TPlane(const TPoint3D& p1, const TLine3D& r2);
    /**
     * Gets a plane which contains the two lines.
     * \throw std::logic_error if the lines do not cross.
     */
    TPlane(const TLine3D& r1, const TLine3D& r2);
    /**
     * Fast default constructor. Initializes to garbage.
     */
    TPlane() {}
    /**
     * Constructor from plane coefficients.
     */
    constexpr TPlane(double A, double B, double C, double D) :
        coefs{A,B,C,D}
    {
    }
    /**
     * Constructor from an array of coefficients.
     */
    TPlane(const double (&vec)[4])
    {
        for (size_t i = 0; i < 4; i++) coefs[i] = vec[i];
    }
    void getAsPose3DForcingOrigin(const TPoint3D& newOrigin, TPose3D& pose);
};
 
using TPlane3D = TPlane;
 
/**
 * 2D polygon, inheriting from std::vector<TPoint2D>.
 * \sa TPolygon3D,TSegment2D,TLine2D,TPoint2D, CPolygon
 */
class TPolygon2D : public std::vector<TPoint2D>
{
   public:
    /** Distance to a point (always >=0) */
    double distance(const TPoint2D& point) const;
    /** Check whether a point is inside (or within geometryEpsilon of a polygon
     * edge). This works for concave or convex polygons. */
    bool contains(const TPoint2D& point) const;
    /** Gets as set of segments, instead of points. */
    void getAsSegmentList(std::vector<TSegment2D>& v) const;
    /** Projects into 3D space, zeroing the z. */
    void generate3DObject(TPolygon3D& p) const;
    /** Polygon's central point. */
    void getCenter(TPoint2D& p) const;
    /** Checks whether is convex. */
    bool isConvex() const;
    /** Erase repeated vertices.  \sa removeRedundantVertices */
    void removeRepeatedVertices();
    /** Erase every redundant vertex from the polygon, saving space. \sa
     * removeRepeatedVertices */
    void removeRedundantVertices();
    /** Gets plot data, ready to use on a 2D plot. \sa
     * mrpt::gui::CDisplayWindowPlots */
    void getPlotData(std::vector<double>& x, std::vector<double>& y) const;
    /** Get polygon bounding box. \exception On empty polygon */
    void getBoundingBox(TPoint2D& min_coords, TPoint2D& max_coords) const;
    /** Default constructor  */
    TPolygon2D() : std::vector<TPoint2D>() {}
    /** Constructor for a given number of vertices, intializing them as garbage.
     */
    explicit TPolygon2D(size_t N) : std::vector<TPoint2D>(N) {}
    /** Implicit constructor from a vector of 2D points */
    TPolygon2D(const std::vector<TPoint2D>& v) : std::vector<TPoint2D>(v) {}
    /** Constructor from a 3D object. */
    explicit TPolygon2D(const TPolygon3D& p);
    /** Static method to create a regular polygon, given its size and radius.
     * \throw std::logic_error if radius is near zero or the number of edges is
     * less than three.
     */
    static void createRegularPolygon(
        size_t numEdges, double radius, TPolygon2D& poly);
    /** Static method to create a regular polygon from its size and radius. The
     * center will correspond to the given pose.
     * \throw std::logic_error if radius is near zero or the number of edges is
     * less than three.
     */
    static void createRegularPolygon(
        size_t numEdges, double radius, TPolygon2D& poly,
        const mrpt::math::TPose2D& pose);
};
 
/**
 * 3D polygon, inheriting from std::vector<TPoint3D>
 * \sa TPolygon2D,TSegment3D,TLine3D,TPlane,TPoint3D
 */
class TPolygon3D : public std::vector<TPoint3D>
{
   public:
    /** Distance to point (always >=0) */
    double distance(const TPoint3D& point) const;
    /** Check whether a point is inside (or within geometryEpsilon of a polygon
     * edge). This works for concave or convex polygons. */
    bool contains(const TPoint3D& point) const;
    /** Gets as set of segments, instead of set of points. */
    void getAsSegmentList(std::vector<TSegment3D>& v) const;
    /** Gets a plane which contains the polygon. Returns false if the polygon is
     * skew and cannot be fit inside a plane. */
    bool getPlane(TPlane& p) const;
    /** Gets the best fitting plane, disregarding whether the polygon actually
     * fits inside or not. \sa getBestFittingPlane */
    void getBestFittingPlane(TPlane& p) const;
    /** Projects into a 2D space, discarding the z. \sa getPlane,isSkew */
    void generate2DObject(TPolygon2D& p) const { p = TPolygon2D(*this); }
    /** Get polygon's central point. */
    void getCenter(TPoint3D& p) const;
    /** Check whether the polygon is skew. Returns true if there doesn't exist a
     * plane in which the polygon can fit. \sa getBestFittingPlane */
    bool isSkew() const;
    /** Remove polygon's repeated vertices. */
    void removeRepeatedVertices();
    /** Erase every redundant vertex, thus saving space. */
    void removeRedundantVertices();
    /** Default constructor. Creates a polygon with no vertices.  */
    TPolygon3D() : std::vector<TPoint3D>() {}
    /** Constructor for a given size. Creates a polygon with a fixed number of
     * vertices, which are initialized to garbage. */
    explicit TPolygon3D(size_t N) : std::vector<TPoint3D>(N) {}
    /** Implicit constructor from a 3D points vector. */
    TPolygon3D(const std::vector<TPoint3D>& v) : std::vector<TPoint3D>(v) {}
    /** Constructor from a 2D object. Zeroes the z. */
    explicit TPolygon3D(const TPolygon2D& p);
    /** Static method to create a regular polygon, given its size and radius.
     * \throw std::logic_error if number of edges is less than three, or radius
     * is near zero. */
    static void createRegularPolygon(
        size_t numEdges, double radius, TPolygon3D& poly);
    /** Static method to create a regular polygon, given its size and radius.
     * The center will be located on the given pose.
     * \throw std::logic_error if number of edges is less than three, or radius
     * is near zero.
     */
    static void createRegularPolygon(
        size_t numEdges, double radius, TPolygon3D& poly,
        const mrpt::math::TPose3D& pose);
};
 
/**
 * Object type identifier for TPoint2D or TPoint3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_POINT = 0;
/**
 * Object type identifier for TSegment2D or TSegment3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_SEGMENT = 1;
/**
 * Object type identifier for TLine2D or TLine3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_LINE = 2;
/**
 * Object type identifier for TPolygon2D or TPolygon3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_POLYGON = 3;
/**
 * Object type identifier for TPlane.
 * \sa TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_PLANE = 4;
/**
 * Object type identifier for empty TObject2D or TObject3D.
 * \sa TObject2D,TObject3D
 */
static constexpr unsigned char GEOMETRIC_TYPE_UNDEFINED = 255;
 
/**
 * Standard type for storing any lightweight 2D type. Do not inherit from this
 * class.
 * \sa TPoint2D,TSegment2D,TLine2D,TPolygon2D
 */
struct TObject2D
{
   private:
    /**
     * Object type identifier.
     */
    unsigned char type;
    /**
     * Union type storing pointers to every allowed type.
     */
    struct tobject2d_data_t
    {
        TPoint2D point;
        TSegment2D segment;
        TLine2D line;
        TPolygon2D* polygon;
 
        tobject2d_data_t() : polygon(nullptr) {}
    } data;
    /**
     * Destroys the object, releasing the pointer to the content (if any).
     */
    void destroy()
    {
        if (type == GEOMETRIC_TYPE_POLYGON) delete data.polygon;
        type = GEOMETRIC_TYPE_UNDEFINED;
    }
 
   public:
    /**
     * Implicit constructor from point.
     */
    TObject2D(const TPoint2D& p) : type(GEOMETRIC_TYPE_POINT)
    {
        data.point = p;
    }
    /**
     * Implicit constructor from segment.
     */
    TObject2D(const TSegment2D& s) : type(GEOMETRIC_TYPE_SEGMENT)
    {
        data.segment = s;
    }
    /**
     * Implicit constructor from line.
     */
    TObject2D(const TLine2D& r) : type(GEOMETRIC_TYPE_LINE)
    {
        data.line = r;
    }
    /**
     * Implicit constructor from polygon.
     */
    TObject2D(const TPolygon2D& p) : type(GEOMETRIC_TYPE_POLYGON)
    {
        data.polygon = new TPolygon2D(p);
    }
    /**
     * Implicit constructor from polygon.
     */
    TObject2D() : type(GEOMETRIC_TYPE_UNDEFINED) {}
    /**
     * Object destruction.
     */
    ~TObject2D() { destroy(); }
    /**
     * Checks whether content is a point.
     */
    bool isPoint() const { return type == GEOMETRIC_TYPE_POINT; }
    /**
     * Checks whether content is a segment.
     */
    bool isSegment() const { return type == GEOMETRIC_TYPE_SEGMENT; }
    /**
     * Checks whether content is a line.
     */
    bool isLine() const { return type == GEOMETRIC_TYPE_LINE; }
    /**
     * Checks whether content is a polygon.
     */
    bool isPolygon() const { return type == GEOMETRIC_TYPE_POLYGON; }
    /**
     * Gets content type.
     */
    unsigned char getType() const { return type; }
    /**
     * Gets the content as a point, returning false if the type is inadequate.
     */
    bool getPoint(TPoint2D& p) const
    {
        if (isPoint())
        {
            p = data.point;
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a segment, returning false if the type is
     * inadequate.
     */
    bool getSegment(TSegment2D& s) const
    {
        if (isSegment())
        {
            s = data.segment;
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a line, returning false if the type is inadequate.
     */
    bool getLine(TLine2D& r) const
    {
        if (isLine())
        {
            r = data.line;
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a polygon, returning false if the type is
     * inadequate.
     */
    bool getPolygon(TPolygon2D& p) const
    {
        if (isPolygon())
        {
            p = *(data.polygon);
            return true;
        }
        else
            return false;
    }
    /**
     * Assign another TObject2D. Pointers are not shared.
     */
    TObject2D& operator=(const TObject2D& obj)
    {
        if (this == &obj) return *this;
        destroy();
        switch (type = obj.type)
        {
            case GEOMETRIC_TYPE_POINT:
                data.point = obj.data.point;
                break;
            case GEOMETRIC_TYPE_SEGMENT:
                data.segment = obj.data.segment;
                break;
            case GEOMETRIC_TYPE_LINE:
                data.line = obj.data.line;
                break;
            case GEOMETRIC_TYPE_POLYGON:
                data.polygon = new TPolygon2D(*(obj.data.polygon));
                break;
        }
        return *this;
    }
    /**
     * Assign a point to this object.
     */
    void operator=(const TPoint2D& p)
    {
        destroy();
        type = GEOMETRIC_TYPE_POINT;
        data.point = p;
    }
    /**
     * Assign a segment to this object.
     */
    void operator=(const TSegment2D& s)
    {
        destroy();
        type = GEOMETRIC_TYPE_SEGMENT;
        data.segment = s;
    }
    /**
     * Assign a line to this object.
     */
    void operator=(const TLine2D& l)
    {
        destroy();
        type = GEOMETRIC_TYPE_LINE;
        data.line = l;
    }
    /**
     * Assign a polygon to this object.
     */
    void operator=(const TPolygon2D& p)
    {
        destroy();
        type = GEOMETRIC_TYPE_POLYGON;
        data.polygon = new TPolygon2D(p);
    }
    /**
     * Project into 3D space.
     */
    void generate3DObject(TObject3D& obj) const;
    /**
     * Constructor from another TObject2D.
     */
    TObject2D(const TObject2D& obj) : type(GEOMETRIC_TYPE_UNDEFINED)
    {
        operator=(obj);
    }
    /**
     * Static method to retrieve all the points in a vector of TObject2D.
     */
    static void getPoints(
        const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts);
    /**
     * Static method to retrieve all the segments in a vector of TObject2D.
     */
    static void getSegments(
        const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms);
    /**
     * Static method to retrieve all the lines in a vector of TObject2D.
     */
    static void getLines(
        const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins);
    /**
     * Static method to retrieve all the polygons in a vector of TObject2D.
     */
    static void getPolygons(
        const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys);
    /**
     * Static method to retrieve all the points in a vector of TObject2D,
     * returning the remainder objects in another parameter.
     */
    static void getPoints(
        const std::vector<TObject2D>& objs, std::vector<TPoint2D>& pnts,
        std::vector<TObject2D>& remainder);
    /**
     * Static method to retrieve all the segments in a vector of TObject2D,
     * returning the remainder objects in another parameter.
     */
    static void getSegments(
        const std::vector<TObject2D>& objs, std::vector<TSegment2D>& sgms,
        std::vector<TObject2D>& remainder);
    /**
     * Static method to retrieve all the lines in a vector of TObject2D,
     * returning the remainder objects in another parameter.
     */
    static void getLines(
        const std::vector<TObject2D>& objs, std::vector<TLine2D>& lins,
        std::vector<TObject2D>& remainder);
    /**
     * Static method to retrieve all the polygons in a vector of TObject2D,
     * returning the remainder objects in another parameter.
     */
    static void getPolygons(
        const std::vector<TObject2D>& objs, std::vector<TPolygon2D>& polys,
        std::vector<TObject2D>& remainder);
};
/**
 * Standard object for storing any 3D lightweight object. Do not inherit from
 * this class.
 * \sa TPoint3D,TSegment3D,TLine3D,TPlane,TPolygon3D
 */
struct TObject3D
{
   private:
    /**
     * Object type identifier.
     */
    unsigned char type;
    /**
     * Union containing pointer to actual data.
     */
    struct tobject3d_data_t
    {
        TPoint3D point;
        TSegment3D segment;
        TLine3D line;
        TPolygon3D* polygon;
        TPlane plane;
 
        tobject3d_data_t() : polygon(nullptr) {}
    } data;
    /**
     * Destroys the object and releases the pointer, if any.
     */
    void destroy()
    {
        if (type == GEOMETRIC_TYPE_POLYGON) delete data.polygon;
        type = GEOMETRIC_TYPE_UNDEFINED;
    }
 
   public:
    /**
     * Constructor from point.
     */
    TObject3D(const TPoint3D& p) : type(GEOMETRIC_TYPE_POINT)
    {
        data.point = p;
    }
    /**
     * Constructor from segment.
     */
    TObject3D(const TSegment3D& s) : type(GEOMETRIC_TYPE_SEGMENT)
    {
        data.segment = s;
    }
    /**
     * Constructor from line.
     */
    TObject3D(const TLine3D& r) : type(GEOMETRIC_TYPE_LINE) { data.line = r; }
    /**
     * Constructor from polygon.
     */
    TObject3D(const TPolygon3D& p) : type(GEOMETRIC_TYPE_POLYGON)
    {
        data.polygon = new TPolygon3D(p);
    }
    /**
     * Constructor from plane.
     */
    TObject3D(const TPlane& p) : type(GEOMETRIC_TYPE_PLANE) { data.plane = p; }
    /**
     * Empty constructor.
     */
    TObject3D() : type(GEOMETRIC_TYPE_UNDEFINED) {}
    /**
     * Destructor.
     */
    ~TObject3D() { destroy(); }
    /**
     * Checks whether content is a point.
     */
    bool isPoint() const { return type == GEOMETRIC_TYPE_POINT; }
    /**
     * Checks whether content is a segment.
     */
    bool isSegment() const { return type == GEOMETRIC_TYPE_SEGMENT; }
    /**
     * Checks whether content is a line.
     */
    bool isLine() const { return type == GEOMETRIC_TYPE_LINE; }
    /**
     * Checks whether content is a polygon.
     */
    bool isPolygon() const { return type == GEOMETRIC_TYPE_POLYGON; }
    /**
     * Checks whether content is a plane.
     */
    bool isPlane() const { return type == GEOMETRIC_TYPE_PLANE; }
    /**
     * Gets object type.
     */
    unsigned char getType() const { return type; }
    /**
     * Gets the content as a point, returning false if the type is not
     * adequate.
     */
    bool getPoint(TPoint3D& p) const
    {
        if (isPoint())
        {
            p = data.point;
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a segment, returning false if the type is not
     * adequate.
     */
    bool getSegment(TSegment3D& s) const
    {
        if (isSegment())
        {
            s = data.segment;
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a line, returning false if the type is not adequate.
     */
    bool getLine(TLine3D& r) const
    {
        if (isLine())
        {
            r = data.line;
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a polygon, returning false if the type is not
     * adequate.
     */
    bool getPolygon(TPolygon3D& p) const
    {
        if (isPolygon())
        {
            p = *(data.polygon);
            return true;
        }
        else
            return false;
    }
    /**
     * Gets the content as a plane, returning false if the type is not
     * adequate.
     */
    bool getPlane(TPlane& p) const
    {
        if (isPlane())
        {
            p = data.plane;
            return true;
        }
        else
            return false;
    }
    /**
     * Assigns another object, creating a new pointer if needed.
     */
    TObject3D& operator=(const TObject3D& obj)
    {
        if (this == &obj) return *this;
        destroy();
        switch (type = obj.type)
        {
            case GEOMETRIC_TYPE_POINT:
                data.point = obj.data.point;
                break;
            case GEOMETRIC_TYPE_SEGMENT:
                data.segment = obj.data.segment;
                break;
            case GEOMETRIC_TYPE_LINE:
                data.line = obj.data.line;
                break;
            case GEOMETRIC_TYPE_POLYGON:
                data.polygon = new TPolygon3D(*(obj.data.polygon));
                break;
            case GEOMETRIC_TYPE_PLANE:
                data.plane = obj.data.plane;
                break;
            case GEOMETRIC_TYPE_UNDEFINED:
                break;
            default:
                THROW_EXCEPTION("Invalid TObject3D object");
        }
        return *this;
    }
    /**
     * Assigns a point to this object.
     */
    void operator=(const TPoint3D& p)
    {
        destroy();
        type = GEOMETRIC_TYPE_POINT;
        data.point = p;
    }
    /**
     * Assigns a segment to this object.
     */
    void operator=(const TSegment3D& s)
    {
        destroy();
        type = GEOMETRIC_TYPE_SEGMENT;
        data.segment = s;
    }
    /**
     * Assigns a line to this object.
     */
    void operator=(const TLine3D& l)
    {
        destroy();
        type = GEOMETRIC_TYPE_LINE;
        data.line = l;
    }
    /**
     * Assigns a polygon to this object.
     */
    void operator=(const TPolygon3D& p)
    {
        destroy();
        type = GEOMETRIC_TYPE_POLYGON;
        data.polygon = new TPolygon3D(p);
    }
    /**
     * Assigns a plane to this object.
     */
    void operator=(const TPlane& p)
    {
        destroy();
        type = GEOMETRIC_TYPE_PLANE;
        data.plane = p;
    }
    /**
     * Projects into 2D space.
     * \throw std::logic_error if the 3D object loses its properties when
     * projecting into 2D space (for example, it's a plane or a vertical line).
     */
    void generate2DObject(TObject2D& obj) const
    {
        switch (type)
        {
            case GEOMETRIC_TYPE_POINT:
                obj = TPoint2D(data.point);
                break;
            case GEOMETRIC_TYPE_SEGMENT:
                obj = TSegment2D(data.segment);
                break;
            case GEOMETRIC_TYPE_LINE:
                obj = TLine2D(data.line);
                break;
            case GEOMETRIC_TYPE_POLYGON:
                obj = TPolygon2D(*(data.polygon));
                break;
            case GEOMETRIC_TYPE_PLANE:
                throw std::logic_error("Too many dimensions");
            default:
                obj = TObject2D();
                break;
        }
    }
    /**
     * Constructs from another object.
     */
    TObject3D(const TObject3D& obj) : type(GEOMETRIC_TYPE_UNDEFINED)
    {
        operator=(obj);
    }
    /**
     * Static method to retrieve every point included in a vector of objects.
     */
    static void getPoints(
        const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts);
    /**
     * Static method to retrieve every segment included in a vector of objects.
     */
    static void getSegments(
        const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms);
    /**
     * Static method to retrieve every line included in a vector of objects.
     */
    static void getLines(
        const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins);
    /**
     * Static method to retrieve every plane included in a vector of objects.
     */
    static void getPlanes(
        const std::vector<TObject3D>& objs, std::vector<TPlane>& plns);
    /**
     * Static method to retrieve every polygon included in a vector of objects.
     */
    static void getPolygons(
        const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys);
    /**
     * Static method to retrieve every point included in a vector of objects,
     * returning the remaining objects in another argument.
     */
    static void getPoints(
        const std::vector<TObject3D>& objs, std::vector<TPoint3D>& pnts,
        std::vector<TObject3D>& remainder);
    /**
     * Static method to retrieve every segment included in a vector of objects,
     * returning the remaining objects in another argument.
     */
    static void getSegments(
        const std::vector<TObject3D>& objs, std::vector<TSegment3D>& sgms,
        std::vector<TObject3D>& remainder);
    /**
     * Static method to retrieve every line included in a vector of objects,
     * returning the remaining objects in another argument.
     */
    static void getLines(
        const std::vector<TObject3D>& objs, std::vector<TLine3D>& lins,
        std::vector<TObject3D>& remainder);
    /**
     * Static method to retrieve every plane included in a vector of objects,
     * returning the remaining objects in another argument.
     */
    static void getPlanes(
        const std::vector<TObject3D>& objs, std::vector<TPlane>& plns,
        std::vector<TObject3D>& remainder);
    /**
     * Static method to retrieve every polygon included in a vector of objects,
     * returning the remaining objects in another argument.
     */
    static void getPolygons(
        const std::vector<TObject3D>& objs, std::vector<TPolygon3D>& polys,
        std::vector<TObject3D>& remainder);
};
 
/** 2D twist: 2D velocity vector (vx,vy) + planar angular velocity (omega)
 * \sa mrpt::math::TTwist3D, mrpt::math::TPose2D
 */
struct TTwist2D
{
    enum
    {
        static_size = 3
    };
    /** Velocity components: X,Y (m/s) */
    double vx, vy;
    /** Angular velocity (rad/s) */
    double omega;
 
    /** Constructor from components */
    constexpr TTwist2D(double vx_, double vy_, double omega_)
        : vx(vx_), vy(vy_), omega(omega_)
    {
    }
    /** Default fast constructor. Initializes to garbage  */
    TTwist2D() {}
    /** Coordinate access using operator[]. Order: vx,vy,vphi */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return vx;
            case 1:
                return vy;
            case 2:
                return omega;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: vx,vy,vphi */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return vx;
            case 1:
                return vy;
            case 2:
                return omega;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Transformation into vector */
    void getAsVector(std::vector<double>& v) const
    {
        v.resize(3);
        v[0] = vx;
        v[1] = vy;
        v[2] = omega;
    }
    /** Transform the (vx,vy) components for a counterclockwise rotation of
     * `ang` radians. */
    void rotate(const double ang);
    bool operator==(const TTwist2D& o) const;
    bool operator!=(const TTwist2D& o) const;
    /** Returns the pose increment of multiplying each twist component times
     * "dt" seconds. */
    mrpt::math::TPose2D operator*(const double dt) const;
    /** Returns a human-readable textual representation of the object (eg: "[vx
     * vy omega]", omega in deg/s)
     * \sa fromString
     */
    void asString(std::string& s) const;
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[0.02 1.04 -45.0]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
    static size_t size() { return 3; }
};
 
/** 3D twist: 3D velocity vector (vx,vy,vz) + angular velocity (wx,wy,wz)
 * \sa mrpt::math::TTwist2D, mrpt::math::TPose3D
 */
struct TTwist3D
{
    enum
    {
        static_size = 6
    };
    /** Velocity components: X,Y (m/s) */
    double vx, vy, vz;
    /** Angular velocity (rad/s) */
    double wx, wy, wz;
 
    /** Constructor from components */
    constexpr TTwist3D(
        double vx_, double vy_, double vz_, double wx_, double wy_, double wz_)
        : vx(vx_), vy(vy_), vz(vz_), wx(wx_), wy(wy_), wz(wz_)
    {
    }
    /** Default fast constructor. Initializes to garbage  */
    TTwist3D() {}
    /** Coordinate access using operator[]. Order: vx,vy,vphi */
    double& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return vx;
            case 1:
                return vy;
            case 2:
                return vz;
            case 3:
                return wx;
            case 4:
                return wy;
            case 5:
                return wz;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: vx,vy,vphi */
    constexpr const double& operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return vx;
            case 1:
                return vy;
            case 2:
                return vz;
            case 3:
                return wx;
            case 4:
                return wy;
            case 5:
                return wz;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Transformation into vector */
    void getAsVector(std::vector<double>& v) const
    {
        v.resize(6);
        for (int i = 0; i < 6; i++) v[i] = (*this)[i];
    }
    bool operator==(const TTwist3D& o) const;
    bool operator!=(const TTwist3D& o) const;
    /** Returns a human-readable textual representation of the object (eg: "[vx
     * vy vz wx wy wz]", omegas in deg/s)
     * \sa fromString
     */
    void asString(std::string& s) const;
    std::string asString() const
    {
        std::string s;
        asString(s);
        return s;
    }
 
    /** Transform all 6 components for a change of reference frame from "A" to
     * another frame "B" whose rotation with respect to "A" is given by `rot`.
     * The translational part of the pose is ignored */
    void rotate(const mrpt::math::TPose3D& rot);
 
    /** Set the current object value from a string generated by 'asString' (eg:
     * "[vx vy vz wx wy wz]" )
     * \sa asString
     * \exception std::exception On invalid format
     */
    void fromString(const std::string& s);
    static size_t size() { return 3; }
};
 
// Binary streaming functions
template <class PoseOrPoint, typename = std::enable_if_t<std::is_base_of<
                                 mrpt::math::TPoseOrPoint, PoseOrPoint>::value>>
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, PoseOrPoint& o)
{
    for (int i = 0; i < o.static_size; i++) in >> o[i];
    return in;
}
template <class PoseOrPoint, typename = std::enable_if_t<std::is_base_of<
                                 mrpt::math::TPoseOrPoint, PoseOrPoint>::value>>
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const PoseOrPoint& o)
{
    for (int i = 0; i < o.static_size; i++) out << o[i];
    return out;
}
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TSegment2D& s);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TSegment2D& s);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TLine2D& l);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TLine2D& l);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TObject2D& o);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TObject2D& o);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TSegment3D& s);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TSegment3D& s);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TLine3D& l);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TLine3D& l);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TPlane& p);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TPlane& p);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TObject3D& o);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TObject3D& o);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TTwist2D& o);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TTwist2D& o);
 
mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TTwist3D& o);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TTwist3D& o);
 
/** @} */  // end of grouping
 
}  // end of namespace math
 
namespace typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPose2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPose3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TSegment2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TLine2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPolygon2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TObject2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TSegment3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TLine3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPlane, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPolygon3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TObject3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TTwist2D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TTwist3D, mrpt::math)
 
}  // namespace typemeta
}  // namespace mrpt