CPoint.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, 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 CPOINT_H
#define CPOINT_H

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

namespace mrpt
{
        namespace 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>
                class CPoint : public CPoseOrPoint<DERIVEDCLASS>
                {
                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++)
                                        static_cast<DERIVEDCLASS*>(this)->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++)
                                        static_cast<DERIVEDCLASS*>(this)->m_coords[i] *= s;
                        }

                        /** Return the pose or point as a 1x2 or 1x3 vector [x y] or [x y z] */
                        inline void getAsVector(mrpt::math::CVectorDouble &v) const
                        {
                                v.resize(DERIVEDCLASS::static_size);
                                for (int i=0;i<DERIVEDCLASS::static_size;i++)
                                        v[i] = static_cast<const DERIVEDCLASS*>(this)->m_coords[i];
                        }
                        //! \overload
                        inline mrpt::math::CVectorDouble getAsVector() const { mrpt::math::CVectorDouble v; getAsVector(v); return v; }

                        /** Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (translation+orientation).
                          * \sa getInverseHomogeneousMatrix
                          */
                        void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 & out_HM ) const
                        {
                                out_HM.unit(4,1.0);
                                out_HM.get_unsafe(0,3)= static_cast<const DERIVEDCLASS*>(this)->x();
                                out_HM.get_unsafe(1,3)= static_cast<const DERIVEDCLASS*>(this)->y();
                                if (DERIVEDCLASS::is3DPoseOrPoint())
                                        out_HM.get_unsafe(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
                        {
                                s = (!DERIVEDCLASS::is3DPoseOrPoint()) ?
                                        mrpt::format("[%f %f]", static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y()) :
                                        mrpt::format("[%f %f %f]",static_cast<const DERIVEDCLASS*>(this)->x(), static_cast<const DERIVEDCLASS*>(this)->y(), static_cast<const DERIVEDCLASS*>(this)->m_coords[2]);
                        }
                        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)
                        {
                                mrpt::math::CMatrixDouble  m;
                                if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString");
                                ASSERT_EQUAL_(mrpt::math::size(m,1),1)
                                ASSERT_EQUAL_(mrpt::math::size(m,2),DERIVEDCLASS::static_size)
                                for (int i=0;i<DERIVEDCLASS::static_size;i++)
                                        static_cast<DERIVEDCLASS*>(this)->m_coords[i] = m.get_unsafe(0,i);
                        }

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

                        /** @} */

                }; // End of class def.

        /** Dumps a point as a string [x,y] or [x,y,z]  */
        template <class DERIVEDCLASS>
        std::ostream &operator << (std::ostream& o, const CPoint<DERIVEDCLASS>& p)
        {
                o << "(" << p[0] << "," << p[1];
                if (p.is3DPoseOrPoint())        o << "," << p[2];
                o <<")";
                return o;
        }

        /** Used by STL algorithms */
        template <class DERIVEDCLASS>
        bool operator < (const CPoint<DERIVEDCLASS> &a, const CPoint<DERIVEDCLASS> &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>
        bool operator==(const CPoint<DERIVEDCLASS> &p1,const CPoint<DERIVEDCLASS> &p2)
        {
                for (int i=0;i<DERIVEDCLASS::static_size;i++)
                        if (p1[i]!=p2[i])       return false; //-V550
                return true;
        }

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



        } // End of namespace
} // End of namespace

#endif