CPoint.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/poses/CPoseOrPoint.h>

namespace mrpt::poses
{
/** A base class for representing a point in 2D or 3D.
 *   For more information refer to the <a
 * href="http://www.mrpt.org/2D_3D_Geometry">2D/3D Geometry tutorial</a> online.
 * \note This class is based on the CRTP design pattern
 * \sa CPoseOrPoint, CPose
 * \ingroup poses_grp
 */
template <class DERIVEDCLASS, std::size_t DIM>
class CPoint : public CPoseOrPoint<DERIVEDCLASS, DIM>
{
    DERIVEDCLASS& derived() { return *static_cast<DERIVEDCLASS*>(this); }
    const DERIVEDCLASS& derived() const
    {
        return *static_cast<const DERIVEDCLASS*>(this);
    }

   public:
    /** @name Methods common to all 2D or 3D points
        @{ */

    /** Scalar addition of all coordinates.
     * This is diferent from poses/point composition, which is implemented as
     * "+" operators in classes derived from "CPose"
     */
    template <class OTHERCLASS>
    inline void AddComponents(const OTHERCLASS& b)
    {
        const int dims = std::min(
            size_t(DERIVEDCLASS::static_size),
            size_t(OTHERCLASS::is3DPoseOrPoint() ? 3 : 2));
        for (int i = 0; i < dims; i++)
            derived().m_coords[i] +=
                static_cast<const OTHERCLASS*>(&b)->m_coords[i];
    }

    /** Scalar multiplication. */
    inline void operator*=(const double s)
    {
        for (int i = 0; i < DERIVEDCLASS::static_size; i++)
            derived().m_coords[i] *= s;
    }

    /** Returns the corresponding 4x4 homogeneous transformation matrix for the
     * point(translation) or pose (translation+orientation).
     * \sa getInverseHomogeneousMatrix
     */
    template <class MATRIX44>
    void getHomogeneousMatrix(MATRIX44& out_HM) const
    {
        out_HM.setIdentity(4);
        out_HM(0, 3) = static_cast<const DERIVEDCLASS*>(this)->x();
        out_HM(1, 3) = static_cast<const DERIVEDCLASS*>(this)->y();
        if (DERIVEDCLASS::is3DPoseOrPoint())
            out_HM(2, 3) = static_cast<const DERIVEDCLASS*>(this)->m_coords[2];
    }

    /** Returns a human-readable textual representation of the object (eg:
     * "[0.02 1.04]" )
     * \sa fromString
     */
    void asString(std::string& s) const;
    inline 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);

    inline double operator[](unsigned int i) const
    {
        return static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
    }
    inline double& operator[](unsigned int i) { return derived().m_coords[i]; }
    /** @} */

};  // End of class def.

/** Used by STL algorithms */
template <class DERIVEDCLASS, std::size_t DIM>
bool operator<(
    const CPoint<DERIVEDCLASS, DIM>& a, const CPoint<DERIVEDCLASS, DIM>& b)
{
    if (a.x() < b.x())
        return true;
    else
    {
        if (!a.is3DPoseOrPoint())
            return a.y() < b.y();
        else if (a.y() < b.y())
            return true;
        else
            return a[2] < b[2];
    }
}

template <class DERIVEDCLASS, std::size_t DIM>
bool operator==(
    const CPoint<DERIVEDCLASS, DIM>& p1, const CPoint<DERIVEDCLASS, DIM>& p2)
{
    for (int i = 0; i < DERIVEDCLASS::static_size; i++)
        if (p1[i] != p2[i]) return false;  //-V550
    return true;
}

template <class DERIVEDCLASS, std::size_t DIM>
bool operator!=(
    const CPoint<DERIVEDCLASS, DIM>& p1, const CPoint<DERIVEDCLASS, DIM>& p2)
{
    for (int i = 0; i < DERIVEDCLASS::static_size; i++)
        if (p1[i] != p2[i]) return true;  //-V550
    return false;
}
}  // namespace mrpt::poses