Зрение NPC с препятствиями

#pragma once
 
#include <SFML/System/Vector2.hpp>
#include <cmath>
#include <utility>
#include <limits>
 
namespace sf
{
    /// скалярное произведение
    inline float dot(Vector2f lh, Vector2f rh) noexcept
    {
        return lh.x * rh.x + lh.y * rh.y;
    }
    
    /// псевдовекторное произведение
    inline float cross(Vector2f lh, Vector2f rh) noexcept
    {
        return lh.x * rh.y - lh.y * rh.x;
    }
} // namespace sf
 
namespace math
{
    /// epsilon
    constexpr float eps() noexcept { return 1.0e-6f; }
    
    /// два пи
    constexpr float twoPi() { return 6.283185307f; }
    
    /// тест пересечения луча ab и отрезка cd
    /// если пересекаются, возвращает параметр t
    /// точка пересечения P(t) = A + t * AB
    inline bool testRaySegment(::sf::Vector2f a, ::sf::Vector2f ab,
                               ::sf::Vector2f c, ::sf::Vector2f d,
                               float & t) noexcept
    {
        bool result = false;
        auto const cd = d - c;
        auto const denom = cross(cd, ab);
        if (::std::abs(denom) > eps()) // прямые не коллинеарны
        {
            auto const ac = c - a;
            auto const t2 = cross(ab, ac) / denom;
            if (0 - eps() < t2 && t2 < 1 + eps()) // точка на отрезке
            {
                auto const t1 = ::std::abs(ab.x) > eps()
                                ? (ac.x + cd.x * t2) / ab.x
                                : (ac.y + cd.y * t2) / ab.y; // если луч вертикальный
                if (t1 > 0) // точка впереди луча
                {
                    result = true;
                    t = t1;
                }
            }
        }
        return result;
    }
    
    /// отрезок
    typedef ::std::pair<::sf::Vector2f, ::sf::Vector2f> Segment2f;
    
    /// тест пересечения луча ray {начало, направление} с группой отрезков [first, last)
    /// возвращает итератор ближайшего отрезка и параметр t точки пересечения
    /// если пересечений не найдено, то result.first == last
    /// точка пересечения P(t) = ray.first + t * ray.second
    template <class It>
    ::std::pair<It, float> findNearestRaySegments(Segment2f ray, It first, It last)
    {
        auto result = ::std::make_pair(last, ::std::numeric_limits<float>::max());
        for (; first != last; ++first)
        {
            float t;
            if (testRaySegment(ray.first, ray.second, first->first, first->second, t) &&
                t < result.second)
            {
                result.first = first;
                result.second = t;
            }
        }
        return result;
    }
} // namespace math

#include <SFML/Graphics.hpp>
#include "math.hpp"
#include <vector>
#include <array>
#include <iterator>
 
// стена
class Wall : public sf::Drawable
{
public:
    Wall(sf::FloatRect bounds)
        : m_segs(sf::Lines, 4)
        , m_shape{{bounds.width, bounds.height}}
    {
        m_shape.setPosition(bounds.left, bounds.top);
        m_segs[0] = sf::Vertex({bounds.left, bounds.top}, sf::Color::Blue);
        m_segs[1] = sf::Vertex({bounds.left + bounds.width, bounds.top + bounds.height}, sf::Color::Blue);
        m_segs[2] = sf::Vertex({bounds.left, bounds.top + bounds.height}, sf::Color::Blue);
        m_segs[3] = sf::Vertex({bounds.left + bounds.width, bounds.top}, sf::Color::Blue);
    }
    
    // я просто стену в качестве цели использую
    sf::FloatRect getGlobalBounds() const
    {
        return m_shape.getGlobalBounds();
    }
    
    void setColor(sf::Color color)
    {
        m_shape.setFillColor(color);
    }
    
    std::array<math::Segment2f, 2> getSegments() const
    {
        return
        {
            math::Segment2f{m_segs[0].position, m_segs[1].position},
            math::Segment2f{m_segs[2].position, m_segs[3].position}
        };
    }
    
private:
    
    void draw(sf::RenderTarget & target, sf::RenderStates states) const override
    {
        target.draw(m_shape, states);
        target.draw(m_segs, states);
    }
    
    sf::VertexArray m_segs; // отрезки, для наглядности
    sf::RectangleShape m_shape;
};
 
// наблюдатель
class Watcher
    : public sf::Drawable
{
public:
    Watcher(float angle, float distance) // угол сектора в радианах
        : m_sectorRay(std::cos(angle * 0.5f), std::sin(angle * 0.5f))
        , m_cone(sf::Triangles, 3)
        , m_sqrDist(distance * distance)
    {
        const sf::Vector2f r = m_sectorRay;
        const sf::Vector2f l = {m_sectorRay.x, - m_sectorRay.y};
        m_cone[1].position = l * distance;
        m_cone[2].position = r * distance;
    }
    
    // проверка, нужно ли кастить луч в точку to
    // если нужно, результат записывается в ray
    bool getRay(sf::Vector2f to, math::Segment2f & ray) const
    {
        auto const delta = to - m_position;
        auto const sqrDist = dot(delta, delta);
        if (sqrDist <= m_sqrDist) // на расстоянии видимости
        {
            // крайние лучи сектора обзора
            // их лучше хранить в классе и считать при изменении напрявления взгляда,
            // для примера не принципиально
            auto const l = rotate(m_sectorRay);
            auto const r = rotate({m_sectorRay.x, -m_sectorRay.y});
            // в секторе обзора
            bool const inSector = cross(delta, l) > 0 &&
                                  cross(delta, r) < 0;
            if (inSector)
            {
                ray = {m_position, delta};
                return true;
            }
        }
        return false;
    }
    
    void setPosition(sf::Vector2f position)
    {
        m_position = position;
    }
    
    // направление взгляда на target
    void setDirection(sf::Vector2f target)
    {
        auto const dir = target - m_position;
        auto const sqrLen = dot(dir, dir);
        if (sqrLen > math::eps())
            m_rotation = dir / std::sqrt(sqrLen);
    }
    
    void setColor(sf::Color color)
    {
        if (m_cone[0].color != color)
        {
            m_cone[0].color =
            m_cone[1].color =
            m_cone[2].color = color;
        }
    }
    
private:
    
    void draw(sf::RenderTarget & target, sf::RenderStates states) const override
    {
        states.transform *= sf::Transform(m_rotation.x, -m_rotation.y, m_position.x,
                                          m_rotation.y,  m_rotation.x, m_position.y,
                                                  0.0f,          0.0f,         1.0f);
        target.draw(m_cone, states);
    }
    
    // хэлпер для поворота направления
    sf::Vector2f rotate(sf::Vector2f vec) const
    {
        return
        {
            dot(vec, sf::Vector2f(m_rotation.x, -m_rotation.y)),
            dot(vec, sf::Vector2f(m_rotation.y,  m_rotation.x))
        };
    }
    
    sf::Vector2f m_position{0.0f, 0.0f};
    sf::Vector2f m_rotation{1.0f, 0.0f};
    sf::Vector2f m_sectorRay; // "луч" сектора обзора
    sf::VertexArray m_cone;
    float m_sqrDist; // квадрат дистанции обзора
};
 
// находится ли объект aabb в поле обзора наблюдателя watch
// преграды для обзора - [first, last)
// для наглядности, все проверяемые лучи отправляем в dest для последубщей отрисовки
template <class It, /*DEBUG ONLY-->>*/class OutIt>
bool isVisible(const Watcher & watch, It first, It last, sf::FloatRect aabb, /*DEBUG ONLY-->>*/OutIt dest)
{
    sf::Vector2f const points[] =
    {
        {aabb.left, aabb.top},
        {aabb.left + aabb.width, aabb.top + aabb.height},
        {aabb.left, aabb.top + aabb.height},
        {aabb.left + aabb.width, aabb.top}
    };
    for (auto p : points)
    {
        math::Segment2f ray;
        if (watch.getRay(p, ray))
        {
            /*DEBUG ONLY-->>*/*dest++ = ray;
            auto res = math::findNearestRaySegments(ray, first, last);
            if (res.first == last || res.second > 1 + math::eps())
                return true;
        }
    }
    return false;
}
 
int main()
{
    sf::RenderWindow window{ sf::VideoMode{ 800, 600 }, L"RayCast", sf::Style::Close };
    window.setFramerateLimit(120);
    
    std::vector<Wall> walls // стены
    {
        sf::FloatRect(100, 100, 20, 300),
        sf::FloatRect(600, 200, 20, 200),
        sf::FloatRect(200, 200, 200, 20),
        sf::FloatRect(100, 500, 500, 20)
    };
    
    Wall target(sf::FloatRect(400, 300, 50, 50)); // цель
    target.setColor(sf::Color::Yellow);
    
    Watcher watch(30 * math::twoPi() / 360, 500); //наблюдатель
    
    bool needUpdate = true;
    
    std::vector<math::Segment2f> segments; // препятствия
    for (auto const & wall : walls)
    {
        auto const segs = wall.getSegments();
        segments.insert(segments.end(), segs.begin(), segs.end());
    }
    
    std::vector<math::Segment2f> rays; // лучи
    
    while (window.isOpen())
    {
        sf::Event event;
        while (window.pollEvent(event))
        {
            if (event.type == sf::Event::Closed)
                window.close();
            else if (event.type == sf::Event::MouseMoved)
            {
                auto const & m = event.mouseMove;
                watch.setDirection(window.mapPixelToCoords({m.x, m.y}));
                needUpdate = true;
            }
            else if (event.type == sf::Event::MouseButtonPressed &&
                     event.mouseButton.button == sf::Mouse::Left)
            {
                auto const & m = event.mouseButton;
                watch.setPosition(window.mapPixelToCoords({m.x, m.y}));
                needUpdate = true;
            }
        }
        
        if (needUpdate)
        {
            needUpdate = false;
            rays.clear();
            if (isVisible(watch, segments.begin(), segments.end(), target.getGlobalBounds(), std::back_inserter(rays)))
                watch.setColor(sf::Color(0x7f00007f));
            else
                watch.setColor(sf::Color(0x7f007f));
        }
        
        window.clear();
        
        for (auto const & wall : walls)
            window.draw(wall);
        window.draw(watch);
        window.draw(target);
        for (auto ray : rays)
        {
            sf::Vertex line[2] = {
                {ray.first, sf::Color::Green},
                {ray.first + ray.second, sf::Color::Green}
            };
            window.draw(line, 2, sf::PrimitiveType::Lines);
        }
        window.display();
    }
}