line.hpp

 
#ifndef UFO_GEOMETRY_LINE_HPP
#define UFO_GEOMETRY_LINE_HPP
 
// UFO
#include <ufo/geometry/aabb.hpp>
#include <ufo/numeric/vec.hpp>
 
// STL
#include <concepts>
#include <cstddef>
#include <format>
#include <limits>
#include <ostream>
 
namespace ufo
{
template <std::size_t Dim = 3, std::floating_point T = float>
struct Line {
    using value_type = T;
 
    Vec<Dim, T> origin;
 
    Vec<Dim, T> direction;
 
    constexpr Line() noexcept = default;
 
    constexpr Line(Line const&) noexcept = default;
 
    constexpr Line(Vec<Dim, T> const& origin, Vec<Dim, T> const& direction) noexcept
        : origin(origin), direction(normalize(direction))
    {
    }
 
    [[nodiscard]] static constexpr Line fromPoints(Vec<Dim, T> const& v_1,
                                                   Vec<Dim, T> const& v_2) noexcept
    {
        return Line(v_1, v_2 - v_1);
    }
 
    template <std::convertible_to<T> U>
    constexpr explicit Line(Line<Dim, U> const& other) noexcept
        : origin(Vec<Dim, T>(other.origin)), direction(Vec<Dim, T>(other.direction))
    {
    }
 
    [[nodiscard]] static constexpr std::size_t dimension() noexcept { return Dim; }
 
    [[nodiscard]] constexpr Line& operator=(Line const&) noexcept = default;
 
    [[nodiscard]] constexpr bool isDegenerate() const noexcept
    {
        return dot(direction, direction) < std::numeric_limits<T>::epsilon();
    }
 
    [[nodiscard]] constexpr Vec<Dim, T> center() const noexcept { return origin; }
 
    [[nodiscard]] constexpr Vec<Dim, T> at(T t) const noexcept
    {
        return origin + t * direction;
    }
 
    [[nodiscard]] constexpr T diameter() const noexcept
    {
        return std::numeric_limits<T>::infinity();
    }
 
    [[nodiscard]] constexpr T volume() const noexcept { return T(0); }
 
    [[nodiscard]] constexpr AABB<Dim, T> aabb() const noexcept
    {
        // A line is infinite
        return AABB<Dim, T>(Vec<Dim, T>(-std::numeric_limits<T>::infinity()),
                            Vec<Dim, T>(std::numeric_limits<T>::infinity()));
    }
};
 
template <std::size_t Dim, std::floating_point T>
[[nodiscard]] constexpr Vec<Dim, T> intersectionPoint(Line<Dim, T> const& a,
                                                      Line<Dim, T> const& b)
{
    if constexpr (2 == Dim) {
        T det = a.direction.x() * b.direction.y() - a.direction.y() * b.direction.x();
        if (std::abs(det) < std::numeric_limits<T>::epsilon()) {
            return Vec<Dim, T>(std::numeric_limits<T>::infinity());
        }
        T t = ((b.origin.x() - a.origin.x()) * b.direction.y() -
               (b.origin.y() - a.origin.y()) * b.direction.x()) /
              det;
        return a.origin + t * a.direction;
    } else if constexpr (3 == Dim) {
        Vec<3, T> w0     = a.origin - b.origin;
        T         a_dot  = T(1);  // a.direction is normalized
        T         b_dot  = T(1);  // b.direction is normalized
        T         ab_dot = dot(a.direction, b.direction);
        T         aw_dot = dot(a.direction, w0);
        T         bw_dot = dot(b.direction, w0);
        T         denom  = a_dot * b_dot - ab_dot * ab_dot;
        if (std::abs(denom) < std::numeric_limits<T>::epsilon()) {
            return Vec<Dim, T>(std::numeric_limits<T>::infinity());
        }
        T sc = (ab_dot * bw_dot - b_dot * aw_dot) / denom;
        return a.origin + sc * a.direction;
    } else {
        // TODO: Implement for 4D
        return Vec<Dim, T>(std::numeric_limits<T>::quiet_NaN());
    }
}
 
template <std::size_t Dim, std::floating_point T>
[[nodiscard]] bool operator==(Line<Dim, T> const& lhs, Line<Dim, T> const& rhs) noexcept
{
    // Lines are equal if they are collinear (same direction and origin lies on the other
    // line) Or same origin and direction/opposite direction
    T d = dot(lhs.direction, rhs.direction);
    if (std::abs(std::abs(d) - T(1)) > std::numeric_limits<T>::epsilon()) {
        return false;
    }
    Vec<Dim, T> diff = lhs.origin - rhs.origin;
    if (normSquared(diff) < std::numeric_limits<T>::epsilon()) {
        return true;
    }
    diff = normalize(diff);
    return std::abs(std::abs(dot(diff, lhs.direction)) - T(1)) <
           std::numeric_limits<T>::epsilon();
}
 
template <std::size_t Dim, std::floating_point T>
std::ostream& operator<<(std::ostream& out, Line<Dim, T> const& line)
{
    return out << "Origin: [" << line.origin << "], Direction: [" << line.direction << "]";
}
 
template <std::floating_point T>
using Line1 = Line<1, T>;
template <std::floating_point T>
using Line2 = Line<2, T>;
template <std::floating_point T>
using Line3 = Line<3, T>;
template <std::floating_point T>
using Line4 = Line<4, T>;
 
using Line1f = Line<1, float>;
using Line2f = Line<2, float>;
using Line3f = Line<3, float>;
using Line4f = Line<4, float>;
 
using Line1d = Line<1, double>;
using Line2d = Line<2, double>;
using Line3d = Line<3, double>;
using Line4d = Line<4, double>;
 
}  // namespace ufo
 
template <std::size_t Dim, std::floating_point T>
  requires std::formattable<T, char>
struct std::formatter<ufo::Line<Dim, T>> {
    constexpr auto parse(std::format_parse_context& ctx) { return ctx.begin(); }
 
    auto format(ufo::Line<Dim, T> const& line, std::format_context& ctx) const
    {
        return std::format_to(ctx.out(), "Origin: [{}], Direction: [{}]", line.origin,
                              line.direction);
    }
};
 
#endif  // UFO_GEOMETRY_LINE_HPP