point.h

/*****************************************************************************
 * VCLib                                                                     *
 * Visual Computing Library                                                  *
 *                                                                           *
 * Copyright(C) 2021-2025                                                    *
 * Visual Computing Lab                                                      *
 * ISTI - Italian National Research Council                                  *
 *                                                                           *
 * All rights reserved.                                                      *
 *                                                                           *
 * This program is free software; you can redistribute it and/or modify      *
 * it under the terms of the Mozilla Public License Version 2.0 as published *
 * by the Mozilla Foundation; either version 2 of the License, or            *
 * (at your option) any later version.                                       *
 *                                                                           *
 * This program is distributed in the hope that it will be useful,           *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the              *
 * Mozilla Public License Version 2.0                                        *
 * (https://www.mozilla.org/en-US/MPL/2.0/) for more details.                *
 ****************************************************************************/
 
#ifndef VCL_SPACE_CORE_POINT_H
#define VCL_SPACE_CORE_POINT_H
 
#include <vclib/base.h>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <compare>
 
namespace vcl {
 
template<typename Scalar, uint N>
class Point : public Eigen::Matrix<Scalar, N, 1>
{
    using Base = Eigen::Matrix<Scalar, N, 1>;
 
public:
    using BaseMatrixType = Base;
 
    // inherit Base operators
    using Base::operator+;
    using Base::operator-;
    using Base::operator*;
    using Base::operator/;
    using Base::operator+=;
    using Base::operator-=;
    using Base::operator*=;
    using Base::operator/=;
 
    using ScalarType = Scalar;
 
    static const uint DIM = N;
 
    Point() { Base::setZero(); }
 
    template<typename OtherDerived>
    Point(const Eigen::MatrixBase<OtherDerived>& other) : Base(other)
    {
    }
 
    template<typename... Scalars>
    Point(Scalars... scalars) requires (sizeof...(scalars) == N)
    {
        set(scalars...);
    }
 
    ScalarType& x() requires (N >= 1) { return at(0); }
 
    const ScalarType& x() const requires (N >= 1) { return at(0); }
 
    ScalarType& y() requires (N >= 2) { return at(1); }
 
    const ScalarType& y() const requires (N >= 2) { return at(1); }
 
    ScalarType& z() requires (N >= 3) { return at(2); }
 
    const ScalarType& z() const requires (N >= 3) { return at(2); }
 
    ScalarType& w() requires (N >= 4) { return at(3); }
 
    const ScalarType& w() const requires (N >= 4) { return at(3); }
 
    ScalarType& at(uint i) { return Base::operator()(i); }
 
    const ScalarType& at(uint i) const { return Base::operator()(i); }
 
    template<typename S>
    auto cast() const
    {
        if constexpr (std::is_same_v<Scalar, S>) {
            return *this;
        }
        else {
            return Point<S, N>(Base::template cast<S>());
        }
    }
 
    bool isDegenerate() const
    {
        for (size_t i = 0; i < DIM; ++i)
            if (vcl::isDegenerate(at(i)))
                return true;
        return false;
    }
 
    bool epsilonEquals(
        const Point& p1,
        Scalar       epsilon = std::numeric_limits<Scalar>::epsilon()) const
    {
        bool b = true;
        for (size_t i = 0; i < DIM; ++i)
            b = b && vcl::epsilonEquals(at(i), p1(i), epsilon);
        return b;
    }
 
    Scalar angle(const Point& p1) const
    {
        Scalar w = Base::norm() * p1.norm();
        if (w == 0)
            return -1;
        Scalar t = Base::dot(p1) / w;
        if (t > 1)
            t = 1;
        else if (t < -1)
            t = -1;
        return (Scalar) acos(t);
    }
 
    Scalar dist(const Point& p1) const { return (*this - p1).norm(); }
 
    Scalar squaredDist(const Point& p1) const
    {
        return (*this - p1).squaredNorm();
    }
 
    Point mul(const Point& p1) const
    {
        Point<Scalar, N> tmp;
        for (size_t i = 0; i < DIM; ++i)
            tmp[i] = at(i) * p1[i];
        return tmp;
    }
 
    Point div(const Point& p1) const
    {
        Point<Scalar, N> tmp;
        for (size_t i = 0; i < DIM; ++i) {
            if (p1[i] == 0)
                throw std::runtime_error(
                    "Math error: Attempted to divide by Zero\n");
            tmp[i] = at(i) / p1[i];
        }
        return tmp;
    }
 
    constexpr uint size() const { return N; }
 
    template<typename... Scalars>
    void set(Scalars... scalars) requires (sizeof...(scalars) == N)
    {
        Scalar args[N] = {static_cast<Scalar>(scalars)...};
        for (uint i = 0; i < N; i++)
            at(i) = args[i];
    }
 
    auto outerProduct(const Point& p1) const
    {
        Eigen::Matrix<ScalarType, DIM, DIM> res;
        for (uint i = 0; i < DIM; i++) {
            for (uint j = 0; j < DIM; j++) {
                res(i, j) = at(i) * p1(j);
            }
        }
        return res;
    }
 
    // TODO: this function should return a pair
    void orthoBase(Point& u, Point& v) const requires (N == 3)
    {
        const double locEps = double(1e-7);
 
        Point<Scalar, 3> up(0, 1, 0);
        u = Base::cross(up);
 
        double len = u.norm();
        if (len < locEps) {
            if (std::abs(at(0)) < std::abs(at(1))) {
                if (std::abs(at(0)) < std::abs(at(2)))
                    up = Point<Scalar, 3>(1, 0, 0); // x is the min
                else
                    up = Point<Scalar, 3>(0, 0, 1); // z is the min
            }
            else {
                if (std::abs(at(1)) < std::abs(at(2)))
                    up = Point<Scalar, 3>(0, 1, 0); // y is the min
                else
                    up = Point<Scalar, 3>(0, 0, 1); // z is the min
            }
            u = Base::cross(up);
        }
        v = Base::cross(u);
    }
 
    void serialize(std::ostream& os) const
    {
        vcl::serializeN(os, Base::data(), N);
    }
 
    void deserialize(std::istream& is)
    {
        vcl::deserializeN(is, Base::data(), N);
    }
 
    std::size_t hash() const
    {
        std::size_t h = 0;
        for (size_t i = 0; i < DIM; ++i)
            hashCombine(h, at(i));
        return h;
    }
 
    template<typename OtherDerived>
    Point& operator=(const Eigen::MatrixBase<OtherDerived>& other)
    {
        this->Base::operator=(other);
        return *this;
    }
 
    auto operator<=>(const Point& p1) const
    {
        uint i = 0;
        while (i < DIM && at(i) == p1[i]) {
            ++i;
        }
        return i == DIM ? std::strong_ordering::equal : at(i) <=> p1[i];
    }
 
    Point operator+(const Scalar& s) const { return Base(Base::array() + s); }
 
    Point operator-(const Scalar& s) const { return Base(Base::array() - s); }
 
    Scalar operator*(const Point& p1) const { return Base::dot(p1); }
 
    Point operator*(const Eigen::Matrix<Scalar, N + 1, N + 1>& m) const
        requires (N == 3)
    {
        Eigen::Matrix<Scalar, 1, N> s;
 
        s(0) = m(0, 0) * at(0) + m(0, 1) * at(1) + m(0, 2) * at(2) + m(0, 3);
        s(1) = m(1, 0) * at(0) + m(1, 1) * at(1) + m(1, 2) * at(2) + m(1, 3);
        s(2) = m(2, 0) * at(0) + m(2, 1) * at(1) + m(2, 2) * at(2) + m(2, 3);
 
        Scalar w =
            at(0) * m(3, 0) + at(1) * m(3, 1) + at(2) * m(3, 2) + m(3, 3);
        if (w != 0) [[likely]]
            s = s / w;
        return Point(s);
    }
 
    Point& operator+=(const Scalar& s)
    {
        *this = *this + s;
        return *this;
    }
 
    Point& operator-=(const Scalar& s)
    {
        *this = *this - s;
        return *this;
    }
 
    Point& operator*=(const Eigen::Matrix<Scalar, N + 1, N + 1>& m)
        requires (N == 3)
    {
        *this = *this * m;
        return *this;
    }
 
private:
    // hide Base begin and end members
    using Base::begin;
    using Base::end;
};
 
/* Specialization Aliases */
 
template<typename Scalar>
using Point2 = Point<Scalar, 2>;
 
using Point2i = Point2<int>;
 
using Point2f = Point2<float>;
 
using Point2d = Point2<double>;
 
template<typename Scalar>
using Point3 = Point<Scalar, 3>;
 
using Point3i = Point3<int>;
 
using Point3f = Point3<float>;
 
using Point3d = Point3<double>;
 
template<typename Scalar>
using Point4 = Point<Scalar, 4>;
 
using Point4i = Point4<int>;
 
using Point4f = Point4<float>;
 
using Point4d = Point4<double>;
 
/* Concepts */
 
template<typename T>
concept PointConcept = std::derived_from< // same type or derived type
    std::remove_cvref_t<T>,
    Point<typename RemoveRef<T>::ScalarType, RemoveRef<T>::DIM>>;
 
template<typename T>
concept Point2Concept = PointConcept<T> && RemoveRef<T>::DIM == 2;
 
template<typename T>
concept Point3Concept = PointConcept<T> && RemoveRef<T>::DIM == 3;
 
template<typename T>
concept Point4Concept = PointConcept<T> && RemoveRef<T>::DIM == 4;
 
template<typename It>
concept PointIteratorConcept =
    IteratorConcept<It> && PointConcept<typename It::value_type>;
 
template<typename It>
concept Point2IteratorConcept =
    IteratorConcept<It> && Point2Concept<typename It::value_type>;
 
template<typename It>
concept Point3IteratorConcept =
    IteratorConcept<It> && Point3Concept<typename It::value_type>;
 
template<typename It>
concept Point4IteratorConcept =
    IteratorConcept<It> && Point4Concept<typename It::value_type>;
 
/* Utility functions */
 
template<typename Scalar, uint N>
bool epsilonEquals(
    const Point<Scalar, N>& p1,
    const Point<Scalar, N>& p2,
    const Scalar&           epsilon = std::numeric_limits<Scalar>::epsilon())
{
    return p1.epsilonEquals(p2, epsilon);
}
 
template<typename Scalar, uint N>
constexpr Point<Scalar, N> min(
    const Point<Scalar, N>& p1,
    const Point<Scalar, N>& p2)
{
    Point<Scalar, N> p;
    for (size_t i = 0; i < p.DIM; i++) {
        p[i] = std::min(p1[i], p2[i]);
    }
    return p;
}
 
template<typename Scalar, uint N>
constexpr Point<Scalar, N> max(
    const Point<Scalar, N>& p1,
    const Point<Scalar, N>& p2)
{
    Point<Scalar, N> p;
    for (size_t i = 0; i < p.DIM; i++) {
        p[i] = std::max(p1[i], p2[i]);
    }
    return p;
}
 
template<typename Scalar, uint N>
std::ostream& operator<<(std::ostream& os, const Point<Scalar, N>& p)
{
    // Create a custom IOFormat for single row output
    // Parameters: precision, flags, coeffSeparator, rowSeparator, rowPrefix,
    // rowSuffix, matPrefix, matSuffix
    static const Eigen::IOFormat rowFormat(
        Eigen::StreamPrecision, // precision (use stream's precision)
        Eigen::DontAlignCols,   // flags
        ", ",                   // coeffSeparator (between elements)
        ", ",                   // rowSeparator (not used for vectors)
        "",                     // rowPrefix
        "",                     // rowSuffix
        "[",                    // matPrefix
        "]"                     // matSuffix
    );
 
    return os << static_cast<const typename Point<Scalar, N>::BaseMatrixType&>(
                     p)
                     .format(rowFormat);
}
 
/* Deduction guides */
 
template<typename S, typename... Scalars>
Point(S, Scalars... scalars) -> Point<S, sizeof...(Scalars) + 1>;
 
} // namespace vcl
 
// inject vcl::Point hash function in std namespace
namespace std {
 
template<typename Scalar, unsigned int N>
struct hash<vcl::Point<Scalar, N>>
{
    size_t operator()(const vcl::Point<Scalar, N>& id) const noexcept
    {
        return id.hash();
    }
};
 
} // namespace std
 
#endif // VCL_SPACE_CORE_POINT_H