text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          https://www.mrpt.org/                         |
     |                                                                        |
     | Copyright (c) 2005-2021, 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
  