geometry.h

Go to the documentation of this file.

 
 
#ifndef ASW_GEOMETRY_H
#define ASW_GEOMETRY_H
 
#include <algorithm>
#include <cmath>
#include <concepts>
#include <cstddef>
#include <optional>
#include <type_traits>
#include <vector>
 
namespace asw {
 
template <typename T> class Vec2 {
public:
    using Real = std::conditional_t<std::is_floating_point_v<T>, T, float>;
 
    Vec2() = default;
 
    Vec2(T x, T y)
        : x(x)
        , y(y)
    {
    }
 
    Real angle(const Vec2& other) const
    {
        return std::atan2(static_cast<Real>(y - other.y), static_cast<Real>(x - other.x));
    }
 
    Real angle() const
    {
        if (x == 0 && y == 0) {
            return 0;
        }
        return std::atan2(static_cast<Real>(y), static_cast<Real>(x));
    }
 
    static Vec2 from_angle(T angle, T length = T(1))
        requires std::floating_point<T>
    {
        return Vec2(std::cos(angle) * length, std::sin(angle) * length);
    }
 
    Real distance(const Vec2& other) const
    {
        return std::hypot(static_cast<Real>(x - other.x), static_cast<Real>(y - other.y));
    }
 
    Real distance_to_segment(const Vec2& start, const Vec2& end) const
    {
        const auto seg_x = static_cast<Real>(end.x - start.x);
        const auto seg_y = static_cast<Real>(end.y - start.y);
        const auto rel_x = static_cast<Real>(x - start.x);
        const auto rel_y = static_cast<Real>(y - start.y);
 
        const Real length_sq = (seg_x * seg_x) + (seg_y * seg_y);
        const Real t = length_sq > Real(0)
            ? std::clamp(((rel_x * seg_x) + (rel_y * seg_y)) / length_sq, Real(0), Real(1))
            : Real(0);
 
        return std::hypot(rel_x - (seg_x * t), rel_y - (seg_y * t));
    }
 
    T dot(const Vec2& other) const
    {
        return (x * other.x) + (y * other.y);
    }
 
    T cross(const Vec2& other) const
    {
        return (x * other.y) - (y * other.x);
    }
 
    Real magnitude() const
    {
        return std::sqrt(static_cast<Real>((x * x) + (y * y)));
    }
 
    Vec2 normalized() const
        requires std::floating_point<T>
    {
        const T length = magnitude();
        if (length == T(0)) {
            return Vec2(T(0), T(0));
        }
 
        return Vec2(x / length, y / length);
    }
 
    Vec2 operator+(const Vec2& other) const
    {
        return Vec2(x + other.x, y + other.y);
    }
 
    Vec2 operator-(const Vec2& other) const
    {
        return Vec2(x - other.x, y - other.y);
    }
 
    Vec2 operator*(const T scalar) const
    {
        return Vec2(x * scalar, y * scalar);
    }
 
    Vec2 operator/(const T scalar) const
    {
        return Vec2(x / scalar, y / scalar);
    }
 
    Vec2 operator%(const T scalar) const
    {
        return Vec2(x % scalar, y % scalar);
    }
 
    Vec2& operator+=(const Vec2& other)
    {
        x += other.x;
        y += other.y;
        return *this;
    }
 
    Vec2& operator-=(const Vec2& other)
    {
        x -= other.x;
        y -= other.y;
        return *this;
    }
 
    Vec2& operator*=(const T scalar)
    {
        x *= scalar;
        y *= scalar;
        return *this;
    }
 
    Vec2& operator/=(const T scalar)
    {
        x /= scalar;
        y /= scalar;
        return *this;
    }
 
    Vec2& operator%=(const T scalar)
    {
        x %= scalar;
        y %= scalar;
        return *this;
    }
 
    bool operator==(const Vec2& other) const = default;
 
    T x { 0 };
 
    T y { 0 };
};
 
template <typename T> class Vec3 {
public:
    using Real = std::conditional_t<std::is_floating_point_v<T>, T, float>;
 
    Vec3() = default;
 
    Vec3(T x, T y, T z)
        : x(x)
        , y(y)
        , z(z)
    {
    }
 
    Real angle(const Vec3& other) const
    {
        const auto magnitudes = magnitude() * other.magnitude();
        if (magnitudes == Real(0)) {
            return 0;
        }
 
        // Rounding can push the ratio just past 1, which acos turns into NaN
        const auto ratio = static_cast<Real>(dot(other)) / magnitudes;
        return std::acos(std::clamp(ratio, Real(-1), Real(1)));
    }
 
    Real distance(const Vec3& other) const
    {
        return std::sqrt(static_cast<Real>((x - other.x) * (x - other.x)
            + (y - other.y) * (y - other.y) + (z - other.z) * (z - other.z)));
    }
 
    T dot(const Vec3& other) const
    {
        return (x * other.x) + (y * other.y) + (z * other.z);
    }
 
    Vec3 cross(const Vec3& other) const
    {
        return Vec3((y * other.z) - (z * other.y), (z * other.x) - (x * other.z),
            (x * other.y) - (y * other.x));
    }
 
    Real magnitude() const
    {
        return std::sqrt(static_cast<Real>((x * x) + (y * y) + (z * z)));
    }
 
    Vec3 operator+(const Vec3& other) const
    {
        return Vec3(x + other.x, y + other.y, z + other.z);
    }
 
    Vec3 operator-(const Vec3& other) const
    {
        return Vec3(x - other.x, y - other.y, z - other.z);
    }
 
    Vec3 operator*(const T scalar) const
    {
        return Vec3(x * scalar, y * scalar, z * scalar);
    }
 
    Vec3 operator/(const T scalar) const
    {
        return Vec3(x / scalar, y / scalar, z / scalar);
    }
 
    Vec3 operator%(const T scalar) const
    {
        return Vec3(x % scalar, y % scalar, z % scalar);
    }
 
    Vec3& operator+=(const Vec3& other)
    {
        x += other.x;
        y += other.y;
        z += other.z;
        return *this;
    }
 
    Vec3& operator-=(const Vec3& other)
    {
        x -= other.x;
        y -= other.y;
        z -= other.z;
        return *this;
    }
 
    Vec3& operator*=(const T scalar)
    {
        x *= scalar;
        y *= scalar;
        z *= scalar;
        return *this;
    }
 
    Vec3& operator/=(const T scalar)
    {
        x /= scalar;
        y /= scalar;
        z /= scalar;
        return *this;
    }
 
    Vec3& operator%=(const T scalar)
    {
        x %= scalar;
        y %= scalar;
        z %= scalar;
        return *this;
    }
 
    bool operator==(const Vec3& other) const = default;
 
    T x { 0 };
 
    T y { 0 };
 
    T z { 0 };
};
 
template <typename T> class Quad {
public:
    Quad()
        : position(0, 0)
        , size(0, 0)
    {
    }
 
    Quad(const Vec2<T>& position, const Vec2<T>& size)
        : position(position)
        , size(size)
    {
    }
 
    Quad(T x, T y, T width, T height)
        : position(x, y)
        , size(width, height)
    {
    }
 
    void set_position(T x, T y)
    {
        position = Vec2<T>(x, y);
    }
 
    void set_size(T width, T height)
    {
        size = Vec2<T>(width, height);
    }
 
    Vec2<T> get_center() const
    {
        return Vec2<T>(position.x + (size.x / 2.0F), position.y + (size.y / 2.0F));
    }
 
    bool contains(const Vec2<T>& point) const
    {
        return contains(point.x, point.y);
    }
 
    bool contains(T x, T y) const
    {
        return x >= position.x && x < position.x + size.x && y >= position.y
            && y < position.y + size.y;
    }
 
    bool collides(const Quad& other) const
    {
        bool is_outside = position.x + size.x <= other.position.x || // a is left of b
            other.position.x + other.size.x <= position.x || // b is left of a
            position.y + size.y <= other.position.y || // a is above b
            other.position.y + other.size.y <= position.y; // b is above a
 
        return !is_outside;
    }
 
    Vec2<T> closest_point(const Vec2<T>& point) const
    {
        // The size can be negative, clamp between the edges in either order
        const T x1 = position.x + size.x;
        const T y1 = position.y + size.y;
        return Vec2<T>(std::clamp(point.x, std::min(position.x, x1), std::max(position.x, x1)),
            std::clamp(point.y, std::min(position.y, y1), std::max(position.y, y1)));
    }
 
    typename Vec2<T>::Real distance_to(const Vec2<T>& point) const
    {
        return closest_point(point).distance(point);
    }
 
    Vec2<T> get_push_out(const Quad& other) const
    {
        if (!collides(other)) {
            return Vec2<T>(T(0), T(0));
        }
 
        const T left = (position.x + size.x) - other.position.x;
        const T right = (other.position.x + other.size.x) - position.x;
        const T up = (position.y + size.y) - other.position.y;
        const T down = (other.position.y + other.size.y) - position.y;
 
        const T push_x = left < right ? -left : right;
        const T push_y = up < down ? -up : down;
 
        if (std::min(left, right) < std::min(up, down)) {
            return Vec2<T>(push_x, T(0));
        }
 
        return Vec2<T>(T(0), push_y);
    }
 
    // Collision
    bool collides_bottom(const Quad& other) const
    {
        return position.y < other.position.y + other.size.y
            && position.y + size.y > other.position.y + other.size.y;
    }
 
    bool collides_top(const Quad& other) const
    {
        return position.y + size.y > other.position.y && position.y < other.position.y;
    }
 
    bool collides_left(const Quad& other) const
    {
        return position.x + size.x > other.position.x && position.x < other.position.x;
    }
 
    bool collides_right(const Quad& other) const
    {
        return position.x < other.position.x + other.size.x
            && position.x + size.x > other.position.x + other.size.x;
    }
 
    Quad operator+(const Quad<T>& quad) const
    {
        return Quad(position + quad.position, size + quad.size);
    }
 
    Quad operator-(const Quad<T>& quad) const
    {
        return Quad(position - quad.position, size - quad.size);
    }
 
    Quad operator*(const T scalar) const
    {
        return Quad(position * scalar, size * scalar);
    }
 
    Quad operator/(const T scalar) const
    {
        return Quad(position / scalar, size / scalar);
    }
 
    Vec2<T> position;
 
    Vec2<T> size;
};
 
using Vec2f = Vec2<float>;
using Vec2i = Vec2<int>;
using Vec3f = Vec3<float>;
using Vec3i = Vec3<int>;
using Quadf = Quad<float>;
using Quadi = Quad<int>;
 
template <typename T> using Polygon = std::vector<Vec2<T>>;
using Polygonf = Polygon<float>;
 
namespace geometry {
 
    template <typename T> typename Vec2<T>::Real signed_area(const Polygon<T>& polygon)
    {
        using Real = typename Vec2<T>::Real;
        Real area = 0;
        for (std::size_t i = 0; i < polygon.size(); ++i) {
            area += static_cast<Real>(polygon[i].cross(polygon[(i + 1) % polygon.size()]));
        }
        return area / Real(2);
    }
 
    template <typename T> Quad<T> bounds(const Polygon<T>& polygon)
    {
        if (polygon.empty()) {
            return { };
        }
 
        Vec2<T> lo = polygon.front();
        Vec2<T> hi = polygon.front();
        for (const auto& p : polygon) {
            lo = { std::min(lo.x, p.x), std::min(lo.y, p.y) };
            hi = { std::max(hi.x, p.x), std::max(hi.y, p.y) };
        }
        return { lo, hi - lo };
    }
 
    template <typename T>
    bool point_in_triangle(
        const Vec2<T>& point, const Vec2<T>& a, const Vec2<T>& b, const Vec2<T>& c)
    {
        const T d1 = (b - a).cross(point - a);
        const T d2 = (c - b).cross(point - b);
        const T d3 = (a - c).cross(point - c);
        const bool has_neg = d1 < 0 || d2 < 0 || d3 < 0;
        const bool has_pos = d1 > 0 || d2 > 0 || d3 > 0;
        return !(has_neg && has_pos);
    }
 
    std::optional<float> ray_hit(
        const Vec2f& origin, const Vec2f& direction, const Vec2f& a, const Vec2f& b);
 
    Polygonf visibility(const Vec2f& from, float radius, const std::vector<Polygonf>& occluders,
        float direction = 0.0F, float cone = 0.0F);
 
    void visibility(Polygonf& result, const Vec2f& from, float radius,
        const std::vector<Polygonf>& occluders, float direction = 0.0F, float cone = 0.0F);
 
    void visibility(Polygonf& result, const Vec2f& from, float radius,
        const std::vector<Polygonf>& occluders, const std::vector<Quadf>& occluder_bounds,
        float direction = 0.0F, float cone = 0.0F);
 
} // namespace geometry
 
} // namespace asw
 
#endif // ASW_GEOMETRY_H