obb.hpp

 
#ifndef UFO_GEOMETRY_OBB_HPP
#define UFO_GEOMETRY_OBB_HPP
 
// UFO
#include <ufo/geometry/aabb.hpp>
#include <ufo/numeric/mat.hpp>
#include <ufo/numeric/quat.hpp>
#include <ufo/numeric/vec.hpp>
 
// STL
#include <cmath>
#include <concepts>
#include <cstddef>
#include <format>
#include <ostream>
 
namespace ufo
{
template <std::size_t Dim = 3, std::floating_point T = float>
struct OBB;
 
template <std::floating_point T>
struct OBB<2, T> {
    using value_type = T;
 
    Vec<2, T> center_;
 
    Vec<2, T> half_length;
 
    Mat<2, 2, T> rotation;
 
    constexpr OBB() noexcept = default;
 
    constexpr OBB(Vec<2, T> const& start, Vec<2, T> const& end,
                  Vec<1, T> const& half_length)
        : center_((start + end) * T(0.5))
    {
        auto dir = end - start;
 
        this->half_length = Vec<2, T>(norm(dir) * T(0.5), half_length[0]);
 
        auto theta     = std::atan2(dir.y, dir.x);
        auto cos_theta = std::cos(theta);
        auto sin_theta = std::sin(theta);
 
        rotation[0][0] = cos_theta;
        rotation[0][1] = sin_theta;
        rotation[1][0] = -sin_theta;
        rotation[1][1] = cos_theta;
    }
 
    constexpr OBB(Vec<2, T> const& start, Vec<2, T> const& end, T const& half_length)
        : OBB(start, end, Vec<1, T>(half_length))
    {
    }
 
    constexpr OBB(Vec<2, T> const& center, Vec<2, T> const& half_length) noexcept
        : center_(center), half_length(half_length)
    {
    }
 
    constexpr OBB(Vec<2, T> const& center, Vec<2, T> const& half_length,
                  Mat<2, 2, T> const& rotation) noexcept
        : center_(center), half_length(half_length), rotation(rotation)
    {
    }
 
    constexpr OBB(OBB const&) noexcept = default;
 
    template <std::convertible_to<T> U>
    constexpr explicit OBB(OBB<2, U> const& other) noexcept
        : center_(Vec<2, T>(other.center()))
        , half_length(Vec<2, T>(other.half_length))
        , rotation(Mat<2, 2, T>(other.rotation))
    {
    }
 
    [[nodiscard]] constexpr Vec<2, T>& center() noexcept { return center_; }
 
    [[nodiscard]] constexpr Vec<2, T> const& center() const noexcept { return center_; }
 
    [[nodiscard]] static constexpr std::size_t dimension() noexcept { return 2; }
 
    [[nodiscard]] constexpr T diagonal() const noexcept { return norm(half_length) * T(2); }
 
    [[nodiscard]] constexpr AABB<2, T> aabb() const noexcept
    {
        Vec<2, T> extent;
        for (std::size_t i = 0; i < 2; ++i) {
            extent[i] = std::abs(rotation[0][i]) * half_length[0] +
                        std::abs(rotation[1][i]) * half_length[1];
        }
        return AABB<2, T>(center_ - extent, center_ + extent);
    }
 
    [[nodiscard]] constexpr bool isDegenerate() const noexcept
    {
        return any(lessThan(half_length, Vec<2, T>()));
    }
 
    [[nodiscard]] constexpr T volume() const noexcept
    {
        return T(4) * half_length[0] * half_length[1];
    }
 
    [[nodiscard]] constexpr Vec<2, T> rotatedHalfLength() const
    {
        return half_length * rotation;
    }
 
    void setRotation(T angle)
    {
        auto cos_theta = std::cos(angle);
        auto sin_theta = std::sin(angle);
 
        rotation[0][0] = cos_theta;
        rotation[0][1] = sin_theta;
        rotation[1][0] = -sin_theta;
        rotation[1][1] = cos_theta;
    }
 
    [[nodiscard]] bool operator==(OBB const&) const = default;
};
 
template <std::floating_point T>
struct OBB<3, T> {
    using value_type = T;
 
    Vec<3, T> center_;
 
    Vec<3, T> half_length;
 
    Mat<3, 3, T> rotation;
 
    constexpr OBB() noexcept = default;
 
    constexpr OBB(Vec<3, T> const& start, Vec<3, T> const& end,
                  Vec<2, T> const& half_length,
                  Vec<3, T> const& up = Vec<3, T>(T(0), T(0), T(1)))
        : center_((start + end) * T(0.5))
    {
        auto dir = end - start;
 
        this->half_length = Vec<3, T>(norm(dir) * T(0.5), half_length);
 
        // Similar to right handed lookAt
        Vec<3, T> const f(normalize(dir));
        Vec<3, T> const s(normalize(cross(f, up)));
        Vec<3, T> const u(cross(s, f));
 
        rotation[0][0] = s.x;
        rotation[1][0] = s.y;
        rotation[2][0] = s.z;
        rotation[0][1] = u.x;
        rotation[1][1] = u.y;
        rotation[2][1] = u.z;
        rotation[0][2] = -f.x;
        rotation[1][2] = -f.y;
        rotation[2][2] = -f.z;
    }
 
    constexpr OBB(Vec<3, T> const& center, Vec<3, T> const& half_length) noexcept
        : center_(center), half_length(half_length)
    {
    }
 
    constexpr OBB(Vec<3, T> const& center, Vec<3, T> const& half_length,
                  Mat<3, 3, T> const& rotation) noexcept
        : center_(center), half_length(half_length), rotation(rotation)
    {
    }
 
    constexpr OBB(Vec<3, T> const& center, Vec<3, T> const& half_length,
                  Quat<T> const& rotation) noexcept
        : center_(center), half_length(half_length), rotation(rotation)
    {
    }
 
    constexpr OBB(OBB const&) noexcept = default;
 
    template <std::convertible_to<T> U>
    constexpr explicit OBB(OBB<3, U> const& other) noexcept
        : center_(Vec<3, T>(other.center()))
        , half_length(Vec<3, T>(other.half_length))
        , rotation(Mat<3, 3, T>(other.rotation))
    {
    }
 
    [[nodiscard]] constexpr Vec<3, T>& center() noexcept { return center_; }
 
    [[nodiscard]] constexpr Vec<3, T> const& center() const noexcept { return center_; }
 
    [[nodiscard]] static constexpr std::size_t dimension() noexcept { return 3; }
 
    [[nodiscard]] constexpr T diagonal() const noexcept { return norm(half_length) * T(2); }
 
    [[nodiscard]] constexpr T volume() const noexcept
    {
        return T(8) * half_length[0] * half_length[1] * half_length[2];
    }
 
    [[nodiscard]] constexpr AABB<3, T> aabb() const noexcept
    {
        Vec<3, T> extent;
        for (std::size_t i = 0; i < 3; ++i) {
            extent[i] = std::abs(rotation[0][i]) * half_length[0] +
                        std::abs(rotation[1][i]) * half_length[1] +
                        std::abs(rotation[2][i]) * half_length[2];
        }
        return AABB<3, T>(center_ - extent, center_ + extent);
    }
 
    [[nodiscard]] constexpr bool isDegenerate() const noexcept
    {
        return any(lessThan(half_length, Vec<3, T>()));
    }
 
    [[nodiscard]] constexpr Vec<3, T> rotatedHalfLength() const
    {
        return half_length * rotation;
    }
 
    void setRotation(Quat<T> const& rotation) { this->rotation = rotation; }
 
    [[nodiscard]] bool operator==(OBB const&) const = default;
};
 
template <std::floating_point T>
struct OBB<4, T> {
    using value_type = T;
 
    Vec<4, T>    center_;
    Vec<4, T>    half_length;
    Mat<4, 4, T> rotation;
 
    constexpr OBB() noexcept = default;
 
    constexpr OBB(Vec<4, T> const& start, Vec<4, T> const& end,
                  Vec<3, T> const& half_length)
    {
        auto dir          = end - start;
        center_           = (start + end) * T(0.5);
        this->half_length = Vec<4, T>(norm(dir) * T(0.5), half_length);
        // TODO: Implement rotation
    }
 
    constexpr OBB(Vec<4, T> const& center, Vec<4, T> const& half_length) noexcept
        : center_(center), half_length(half_length)
    {
    }
 
    constexpr OBB(Vec<4, T> const& center, Vec<4, T> const& half_length,
                  Mat<4, 4, T> const& rotation) noexcept
        : center_(center), half_length(half_length), rotation(rotation)
    {
    }
 
    constexpr OBB(OBB const&) noexcept = default;
 
    template <std::convertible_to<T> U>
    constexpr explicit OBB(OBB<4, U> const& other) noexcept
        : center_(Vec<4, T>(other.center()))
        , half_length(Vec<4, T>(other.half_length))
        , rotation(Mat<4, 4, T>(other.rotation))
    {
    }
 
    [[nodiscard]] constexpr Vec<4, T>& center() noexcept { return center_; }
 
    [[nodiscard]] constexpr Vec<4, T> const& center() const noexcept { return center_; }
 
    [[nodiscard]] static constexpr std::size_t dimension() noexcept { return 4; }
 
    [[nodiscard]] constexpr T diagonal() const noexcept { return norm(half_length) * T(2); }
 
    [[nodiscard]] constexpr T volume() const noexcept
    {
        return T(16) * half_length[0] * half_length[1] * half_length[2] * half_length[3];
    }
 
    [[nodiscard]] constexpr AABB<4, T> aabb() const noexcept
    {
        Vec<4, T> extent;
        for (std::size_t i = 0; i < 4; ++i) {
            extent[i] = std::abs(rotation[0][i]) * half_length[0] +
                        std::abs(rotation[1][i]) * half_length[1] +
                        std::abs(rotation[2][i]) * half_length[2] +
                        std::abs(rotation[3][i]) * half_length[3];
        }
        return AABB<4, T>(center_ - extent, center_ + extent);
    }
 
    [[nodiscard]] constexpr bool isDegenerate() const noexcept
    {
        return any(lessThan(half_length, Vec<4, T>()));
    }
 
    [[nodiscard]] constexpr Vec<4, T> rotatedHalfLength() const
    {
        return half_length * rotation;
    }
 
    [[nodiscard]] bool operator==(OBB const&) const = default;
};
 
template <std::floating_point T>
std::ostream& operator<<(std::ostream& out, OBB<2, T> const& obb)
{
    return out << "Center: [" << obb.center() << "], Half length: [" << obb.half_length
               << "], Rotation: [" << obb.rotation << "]";
}
 
template <std::floating_point T>
std::ostream& operator<<(std::ostream& out, OBB<3, T> const& obb)
{
    return out << "Center: [" << obb.center() << "], Half length: [" << obb.half_length
               << "], Rotation: [" << obb.rotation << "]";
}
 
template <std::floating_point T>
std::ostream& operator<<(std::ostream& out, OBB<4, T> const& obb)
{
    return out << "Center: [" << obb.center() << "], Half length: [" << obb.half_length
               << "], Rotation: [" << obb.rotation << "]";
}
 
template <class T>
using OBB1 = OBB<1, T>;
template <class T>
using OBB2 = OBB<2, T>;
template <class T>
using OBB3 = OBB<3, T>;
template <class T>
using OBB4 = OBB<4, T>;
 
using OBB1f = OBB<1, float>;
using OBB2f = OBB<2, float>;
using OBB3f = OBB<3, float>;
using OBB4f = OBB<4, float>;
 
using OBB1d = OBB<1, double>;
using OBB2d = OBB<2, double>;
using OBB3d = OBB<3, double>;
using OBB4d = OBB<4, double>;
 
}  // namespace ufo
 
template <std::size_t Dim, std::floating_point T>
  requires std::formattable<T, char>
struct std::formatter<ufo::OBB<Dim, T>> {
    constexpr auto parse(std::format_parse_context& ctx) { return ctx.begin(); }
 
    auto format(ufo::OBB<Dim, T> const& obb, std::format_context& ctx) const
    {
        return std::format_to(ctx.out(), "Center: [{}], Half length: [{}], Rotation: [{}]",
                              obb.center(), obb.half_length, obb.rotation);
    }
};
 
#endif  // UFO_GEOMETRY_OBB_HPP