TPoint3D.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/core/bits_math.h>
#include <mrpt/core/format.h>
#include <mrpt/math/TPoseOrPoint.h>
#include <mrpt/math/math_frwds.h>  // CMatrixFixed
#include <cmath>  // sqrt

namespace mrpt::math
{
// Ensure 1-byte memory alignment, no additional stride bytes.
#pragma pack(push, 1)

/** Trivially copiable underlying data for TPoint3D
 * 1-byte memory packed, no padding]
 */
template <typename T>
struct TPoint3D_data
{
    /** X,Y,Z coordinates */
    T x, y, z;
};

/** Base template for TPoint3D and TPoint3Df.
 * [1-byte memory packed, no padding]
 * \ingroup geometry_grp
 */
template <typename T>
struct TPoint3D_ : public TPoseOrPoint,
                   public TPoint3D_data<T>,
                   public internal::ProvideStaticResize<TPoint3D_<T>>
{
    enum
    {
        static_size = 3
    };

    /** Default constructor. Initializes to zeros. */
    constexpr TPoint3D_() : TPoint3D_data<T>{0, 0, 0} {}
    /** Constructor from coordinates.  */
    constexpr TPoint3D_(T xx, T yy, T zz) : TPoint3D_data<T>{xx, yy, zz} {}

    /** Constructor from coordinates. */
    template <typename U>
    TPoint3D_(const TPoint3D_data<U>& p)
    {
        TPoint3D_data<T>::x = static_cast<T>(p.x);
        TPoint3D_data<T>::y = static_cast<T>(p.y);
        TPoint3D_data<T>::z = static_cast<T>(p.z);
    }

    /** Constructor from column vector. */
    template <typename U>
    TPoint3D_(const mrpt::math::CMatrixFixed<U, 3, 1>& m)
    {
        TPoint3D_data<T>::x = static_cast<T>(m[0]);
        TPoint3D_data<T>::y = static_cast<T>(m[1]);
        TPoint3D_data<T>::z = static_cast<T>(m[2]);
    }

    /** Implicit constructor from TPoint2D. Zeroes the z.
     * \sa TPoint2D
     */
    TPoint3D_(const TPoint2D_<T>& 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);

    /** Return a copy of this object using type U for coordinates */
    template <typename U>
    TPoint3D_<U> cast() const
    {
        return TPoint3D_<U>(
            static_cast<U>(this->x), static_cast<U>(this->y),
            static_cast<U>(this->z));
    }

    /** Coordinate access using operator[]. Order: x,y,z */
    T& operator[](size_t i)
    {
        switch (i)
        {
            case 0:
                return TPoint3D_data<T>::x;
            case 1:
                return TPoint3D_data<T>::y;
            case 2:
                return TPoint3D_data<T>::z;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /** Coordinate access using operator[]. Order: x,y,z */
    constexpr T operator[](size_t i) const
    {
        switch (i)
        {
            case 0:
                return TPoint3D_data<T>::x;
            case 1:
                return TPoint3D_data<T>::y;
            case 2:
                return TPoint3D_data<T>::z;
            default:
                throw std::out_of_range("index out of range");
        }
    }
    /**
     * Point-to-point distance.
     */
    T distanceTo(const TPoint3D_<T>& p) const
    {
        return std::sqrt(
            square(p.x - TPoint3D_data<T>::x) +
            square(p.y - TPoint3D_data<T>::y) +
            square(p.z - TPoint3D_data<T>::z));
    }
    /**
     * Point-to-point distance, squared.
     */
    T sqrDistanceTo(const TPoint3D_<T>& p) const
    {
        return square(p.x - TPoint3D_data<T>::x) +
               square(p.y - TPoint3D_data<T>::y) +
               square(p.z - TPoint3D_data<T>::z);
    }
    /** Squared norm: |v|^2 = x^2+y^2+z^2 */
    T sqrNorm() const
    {
        return square(TPoint3D_data<T>::x) + square(TPoint3D_data<T>::y) +
               square(TPoint3D_data<T>::z);
    }

    /** Point norm: |v| = sqrt(x^2+y^2+z^2) */
    T norm() const { return std::sqrt(sqrNorm()); }

    /** Returns this vector with unit length: v/norm(v) */
    TPoint3D_<T> unitarize() const
    {
        const T n = norm();
        ASSERT_ABOVE_(n, 0);
        const T f = 1 / n;
        return {TPoint3D_data<T>::x * f, TPoint3D_data<T>::y * f,
                TPoint3D_data<T>::z * f};
    }

    /** Scale point/vector */
    TPoint3D_<T>& operator*=(const T f)
    {
        TPoint3D_data<T>::x *= f;
        TPoint3D_data<T>::y *= f;
        TPoint3D_data<T>::z *= f;
        return *this;
    }
    /**
     * Transformation into vector.
     */
    template <class VECTORLIKE>
    void asVector(VECTORLIKE& v) const
    {
        v.resize(3);
        v[0] = TPoint3D_data<T>::x;
        v[1] = TPoint3D_data<T>::y;
        v[2] = TPoint3D_data<T>::z;
    }
    /**
     * Translation.
     */
    TPoint3D_<T>& operator+=(const TPoint3D_<T>& p)
    {
        TPoint3D_data<T>::x += p.x;
        TPoint3D_data<T>::y += p.y;
        TPoint3D_data<T>::z += p.z;
        return *this;
    }
    /**
     * Difference between points.
     */
    TPoint3D_<T>& operator-=(const TPoint3D_<T>& p)
    {
        TPoint3D_data<T>::x -= p.x;
        TPoint3D_data<T>::y -= p.y;
        TPoint3D_data<T>::z -= p.z;
        return *this;
    }
    /**
     * Points addition.
     */
    constexpr TPoint3D_<T> operator+(const TPoint3D_<T>& p) const
    {
        return {TPoint3D_data<T>::x + p.x, TPoint3D_data<T>::y + p.y,
                TPoint3D_data<T>::z + p.z};
    }
    /**
     * Points substraction.
     */
    constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p) const
    {
        return {TPoint3D_data<T>::x - p.x, TPoint3D_data<T>::y - p.y,
                TPoint3D_data<T>::z - p.z};
    }

    constexpr TPoint3D_<T> operator*(T d) const
    {
        return {TPoint3D_data<T>::x * d, TPoint3D_data<T>::y * d,
                TPoint3D_data<T>::z * d};
    }

    constexpr TPoint3D_<T> operator/(T d) const
    {
        return {TPoint3D_data<T>::x / d, TPoint3D_data<T>::y / d,
                TPoint3D_data<T>::z / d};
    }

    bool operator<(const TPoint3D_<T>& 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]", TPoint3D_data<T>::x, TPoint3D_data<T>::y,
            TPoint3D_data<T>::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 TPoint3D_<T> FromString(const std::string& s)
    {
        TPoint3D_<T> o;
        o.fromString(s);
        return o;
    }
};

/** Lightweight 3D point. Allows coordinate access using [] operator.
 * (1-byte memory packed, no padding).
 * \sa mrpt::poses::CPoint3D, mrpt::math::TPoint3Df
 */
using TPoint3D = TPoint3D_<double>;
using TPoint3Df = TPoint3D_<float>;

/** Useful type alias for 3-vectors.
 * (1-byte memory packed, no padding) */
using TVector3D = TPoint3D;
using TVector3Df = TPoint3Df;

/** XYZ point (double) + Intensity(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZIu8
{
    mrpt::math::TPoint3D pt;
    uint8_t intensity{0};
    TPointXYZIu8() : pt() {}
    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{0}, g{0}, b{0};
    TPointXYZRGBu8() = default;
    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{0};
    TPointXYZfIu8() = default;
    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{0}, g{0}, b{0};
    TPointXYZfRGBu8() : pt() {}
    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)
    {
    }
};

mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TPointXYZfRGBu8& p);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TPointXYZfRGBu8& p);

/** XYZ point (float) + RGBA(u8) \sa mrpt::math::TPoint3D */
struct TPointXYZfRGBAu8
{
    mrpt::math::TPoint3Df pt;
    uint8_t r{0}, g{0}, b{0}, a{0xff};
    TPointXYZfRGBAu8() : pt() {}
    constexpr TPointXYZfRGBAu8(
        float x, float y, float z, uint8_t R_val, uint8_t G_val, uint8_t B_val,
        uint8_t A_val = 0xff)
        : pt(x, y, z), r(R_val), g(G_val), b(B_val), a(A_val)
    {
    }
};

mrpt::serialization::CArchive& operator>>(
    mrpt::serialization::CArchive& in, mrpt::math::TPointXYZfRGBAu8& p);
mrpt::serialization::CArchive& operator<<(
    mrpt::serialization::CArchive& out, const mrpt::math::TPointXYZfRGBAu8& p);

/** XYZ point (float) + RGBA(float) [1-byte memory packed, no padding]
 * \sa mrpt::math::TPoint3D */
struct TPointXYZRGBAf
{
    mrpt::math::TPoint3Df pt;
    float R{0}, G{0}, B{0}, A{0};
    TPointXYZRGBAf() = default;

    constexpr TPointXYZRGBAf(
        float x, float y, float z, float R_val, float G_val, float B_val,
        float A_val)
        : pt(x, y, z), R(R_val), G(G_val), B(B_val), A(A_val)
    {
    }
};
#pragma pack(pop)

/** Unary minus operator for 3D points. */
template <typename T>
constexpr TPoint3D_<T> operator-(const TPoint3D_<T>& p1)
{
    return {-p1.x, -p1.y, -p1.z};
}

/** Exact comparison between 3D points */
template <typename T>
constexpr bool operator==(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
{
    return (p1.x == p2.x) && (p1.y == p2.y) && (p1.z == p2.z);  //-V550
}
/** Exact comparison between 3D points */
template <typename T>
constexpr bool operator!=(const TPoint3D_<T>& p1, const TPoint3D_<T>& p2)
{
    return (p1.x != p2.x) || (p1.y != p2.y) || (p1.z != p2.z);  //-V550
}

}  // namespace mrpt::math

namespace mrpt::typemeta
{
// Specialization must occur in the same namespace
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint3D, mrpt::math)
MRPT_DECLARE_TTYPENAME_NO_NAMESPACE(TPoint3Df, mrpt::math)
}  // namespace mrpt::typemeta