Стрельба в SFML OpenGL

#include <SFML/Graphics.hpp>
#include <SFML/OpenGL.hpp>
////////////////////////////////////////////////////////////////////////////////
#include <cmath>
namespace math
{
    // тут минимально необходимая математика
    struct vec3; // 3-х компонентный вектор
    struct vec4; // 4-х компонентный вектор
    struct mat4; // матрица 4x4
 
    struct vec3
    {
        vec3() = default;
        vec3(float x, float y, float z) : data{ x, y, z } {}
 
        float data[3]{};
        float& operator[](size_t i) { return data[i]; }
        const float& operator[](size_t i) const { return data[i]; }
    };
 
    vec3 operator+(const vec3& lh, const vec3& rh);
    vec3 operator*(const vec3& lh, float rh);
 
    float dot(const vec3& lh, const vec3& rh);  // скалярное произведение
    vec3 cross(const vec3& lh, const vec3& rh); // векторное произведение
    vec3 normalize(const vec3& v);              // нормализация вектора
 
    struct vec4
    {
        vec4() = default;
        vec4(float x, float y, float z, float w) : data{ x, y, z, w } {}
        vec4(const vec3& v) : data{ v[0], v[1], v[2] } {}
 
        float data[4]{};
        float& operator[](size_t i) { return data[i]; }
        const float& operator[](size_t i) const { return data[i]; }
    };
 
    vec4 operator+(const vec4& lh, const vec4& rh);
    vec4 operator*(const vec4& lh, float rh);
 
    struct mat4
    {
        vec4 data[4]{};
        vec4& operator[](size_t i) { return data[i]; }
        const vec4& operator[](size_t i) const { return data[i]; }
    };
 
    mat4 identity();    // единичная матрица
 
    mat4 operator*(const mat4& lh, const mat4& rh);
    vec4 operator*(const mat4& lh, const vec4& rh);
 
    mat4 translate(const mat4& m, float x, float y, float z);               // перенос
    mat4 rotate(const mat4& m, float angle, float ax, float ay, float az);  // поворот
    mat4 scale(const mat4& m, float fx, float fy, float fz);                // масштабирование
 
    mat4 perspective(float fov, float ratio, float z_near, float z_far);    // матрица перспективной проекции
 
    constexpr float quat_pi();  // pi / 4
    constexpr float half_pi();  // pi / 2
    constexpr float pi();       // pi
    constexpr float two_pi();   // 2 * pi
 
    constexpr float dgs_rad();  // pi / 180 (1 градус в радианах)
    constexpr float radians(float deg); // перевод градусов в радианы
}
////////////////////////////////////////////////////////////////////////////////
// примитивная камера
struct Camera
{
    Camera(float aspect); // aspect - соотношение сторон экрана
 
    void yaw(float angle);      // рыскание
    void pitch(float angle);    // тангаж
 
    const math::mat4& get_view();   // матрица вида
    const math::mat4& get_proj();   // матрица проекции
 
private:
    // для оптимизации
    void update();  // обновление матрицы (та самая LookAt)
    void change();  // установка флага обновления
 
private:
 
    float m_yaw;        // угол рыскания
    float m_pitch;      // угол тангажа
    math::mat4 m_view;  // матрица вида
    math::mat4 m_proj;  // матрица проекции
    bool need_update;   // флаг обновления
};
////////////////////////////////////////////////////////////////////////////////
// Bounding box модели
struct Bbox
{
    math::vec4 min; // минимальные x,y,z координаты вершин модели
    math::vec4 max; // максимальные x,y,z координаты вершин модели
};
////////////////////////////////////////////////////////////////////////////////
// 3D куб
struct Cube
{
    Cube();
 
    void set_pos(const math::vec3& pos);
    void set_color(const math::vec3& color);
 
    void draw(sf::Shader& shader, Camera& cam);
 
    Bbox get_local_bounds();
    const math::mat4& get_transform();  // матрица модели
 
private:
 
    static void init(); // инициализация массива вершин
    static void draw(
        sf::Shader& shader,         // шейдер
        const math::mat4& mvp,      // итоговая матрица преобразований (ModelViewProjection)
        const math::vec3& color);   // цвет модели
 
private:
 
    math::mat4 m_model; // матрица модели
    math::vec3 m_color; // цвет куба
 
    static bool ready;  // статус инициализации
    static const float vertices[];  // массив вершин
};
////////////////////////////////////////////////////////////////////////////////
// вершинный шейдер
static const std::string vert = R"(
 
uniform mat4 u_MVP;
 
void main()
{
    gl_Position = u_MVP * gl_Vertex;
}
 
)";
// фрагментный шейдер
static const std::string frag = R"(
 
uniform vec3 u_color;
 
void main()
{
    gl_FragColor = vec4(u_color, 1);
}
 
)";
////////////////////////////////////////////////////////////////////////////////
std::vector<Cube> generate();   // генерация массива кубиков
 
// САМАЯ ВАЖНАЯ ФУНКЦИЯ - проверка пересечения луча взгляда с bounding box модели
bool test_ray_intersection(
    const Bbox& bb,             // bounding box в локальных координатах
    const math::mat4& view,     // матрица вида
    const math::mat4& model);   // матрица модели
////////////////////////////////////////////////////////////////////////////////
int main()
{
    //настраиваем контекст
    sf::ContextSettings ctx_sets;
    ctx_sets.depthBits = 24;
    // размеры экрана(окна)
    int width = 1600;
    int height = 900;
    // создаём окно
    sf::RenderWindow window(sf::VideoMode(width, height), "test", sf::Style::Close, ctx_sets);
    window.setVerticalSyncEnabled(true);    // vSync
    window.setMouseCursorGrabbed(true);     // лочим курсор в окне
    window.setMouseCursorVisible(false);    // прячем курсор
    // позиция курсора
    sf::Vector2i cursor_pos = sf::Mouse::getPosition();
    // маркер прицела
    sf::RectangleShape cross(sf::Vector2f(10, 10));
    cross.setOrigin(5, 5);
    cross.setPosition(width / 2.0f, height / 2.0f);
    // инициализируем OpenGL
    window.setActive(true);
    glClearColor(0.1f, 0.5f, 0.8f, 1.0f);
    glEnable(GL_DEPTH_TEST);
    glDisable(GL_LIGHTING);
    glViewport(0, 0, width, height);
    window.setActive(false);
 
    Camera cam(float(width) / height);  // камера
    constexpr float mouse_sense = 0.2f; // чувствительность мыши
    // массив объектов(кубов)
    std::vector<Cube> objects = generate();
    // инициализируем шейдер
    sf::Shader shader;
    if (!shader.loadFromMemory(vert, frag))
    {
        return 1;
    }
    // основной цикл
    while (window.isOpen())
    {
        sf::Event event;
        while (window.pollEvent(event))
        {
            if (event.type == sf::Event::Closed)
            {
                window.close();
            }
            else if (event.type == sf::Event::KeyPressed)
            {
                if (event.key.code == sf::Keyboard::Escape)
                {
                    window.close();
                }
            }
        }
        // обрабатывем управление камерой
        const sf::Vector2i pos = sf::Mouse::getPosition();
        cam.yaw(math::radians(mouse_sense * (pos.x - cursor_pos.x)));
        cam.pitch(math::radians(mouse_sense * (cursor_pos.y - pos.y)));
        cursor_pos = pos;
        // тестируем пересечение луча взгляда с объектами
        for (auto&& obj : objects)
        {
            if (test_ray_intersection(obj.get_local_bounds(), cam.get_view(), obj.get_transform()))
                obj.set_color(math::vec3(0, 1, 0)); // зелёный, если попали
            else
                obj.set_color(math::vec3(1, 0, 0)); // красный - мимо
        }
        // отрисовка кубов
        window.setActive(true);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        for (auto&& obj : objects)
        {   
            obj.draw(shader, cam);
        }
        window.setActive(false);
        // отрисовка перекрестия прицела
        window.pushGLStates();
        window.draw(cross);
        window.popGLStates();
 
        window.display();
    }
 
    return 0;
}
////////////////////////////////////////////////////////////////////////////////
namespace math
{
    vec3 operator+(const vec3 & lh, const vec3 & rh)
    {
        return
        {
            lh[0] + rh[0],
            lh[1] + rh[1],
            lh[2] + rh[2]
        };
    }
 
    vec3 operator*(const vec3 & lh, float rh)
    {
        return
        {
            lh[0] * rh,
            lh[1] * rh,
            lh[2] * rh
        };
    }
 
    float dot(const vec3& lh, const vec3& rh)
    {
        return lh[0] * rh[0] + lh[1] * rh[1] + lh[2] * rh[2];
    }
 
    vec3 cross(const vec3 & lh, const vec3 & rh)
    {
        return
        {
            lh[1] * rh[2] - rh[1] * lh[2],
            lh[2] * rh[0] - rh[2] * lh[0],
            lh[0] * rh[1] - rh[0] * lh[1]
        };
    }
 
    vec3 normalize(const vec3 & v)
    {
        return v * (1 / std::sqrt(dot(v, v)));
    }
 
    vec4 operator+(const vec4& lh, const vec4& rh)
    {
        return
        {
            lh[0] + rh[0],
            lh[1] + rh[1],
            lh[2] + rh[2],
            lh[3] + rh[3]
        };
    }
 
    vec4 operator*(const vec4& lh, float rh)
    {
        return
        {
            lh[0] * rh,
            lh[1] * rh,
            lh[2] * rh,
            lh[3] * rh
        };
    }
 
    mat4 identity()
    {
        mat4 result;
        result[0][0] = result[1][1] = result[2][2] = result[3][3] = 1;
        return result;
    }
 
    mat4 operator*(const mat4& lh, const mat4& rh)
    {
        const vec4 A0 = lh[0];
        const vec4 A1 = lh[1];
        const vec4 A2 = lh[2];
        const vec4 A3 = lh[3];
 
        const vec4 B0 = rh[0];
        const vec4 B1 = rh[1];
        const vec4 B2 = rh[2];
        const vec4 B3 = rh[3];
 
        mat4 result;
        result[0] = A0 * B0[0] + A1 * B0[1] + A2 * B0[2] + A3 * B0[3];
        result[1] = A0 * B1[0] + A1 * B1[1] + A2 * B1[2] + A3 * B1[3];
        result[2] = A0 * B2[0] + A1 * B2[1] + A2 * B2[2] + A3 * B2[3];
        result[3] = A0 * B3[0] + A1 * B3[1] + A2 * B3[2] + A3 * B3[3];
        return result;
    }
 
    vec4 operator*(const mat4& lh, const vec4& rh)
    {
        const vec4 A = lh[0] * rh[0] + lh[1] * rh[1];
        const vec4 B = lh[2] * rh[2] + lh[3] * rh[3];
        return A + B;
    }
 
    mat4 translate(const mat4& m, float x, float y, float z)
    {
        mat4 result = m;
        result[3] = m[0] * x + m[1] * y + m[2] * z + m[3];
        return result;
    }
 
    mat4 rotate(const mat4& m, float angle, float ax, float ay, float az)
    {
        const float cosa = std::cos(angle);
        const float sina = std::sin(angle);
 
        const vec4 axis = normalize({ ax, ay, az });
        const vec4 temp = axis * (1 - cosa);
 
        mat4 rotate;
        rotate[0][0] = cosa + temp[0] * axis[0];
        rotate[0][1] = temp[0] * axis[1] + sina * axis[2];
        rotate[0][2] = temp[0] * axis[2] - sina * axis[1];
 
        rotate[1][0] = temp[1] * axis[0] - sina * axis[2];
        rotate[1][1] = cosa + temp[1] * axis[1];
        rotate[1][2] = temp[1] * axis[2] + sina * axis[0];
 
        rotate[2][0] = temp[2] * axis[0] + sina * axis[1];
        rotate[2][1] = temp[2] * axis[1] - sina * axis[0];
        rotate[2][2] = cosa + temp[2] * axis[2];
 
        mat4 result;
        result[0] = m[0] * rotate[0][0] + m[1] * rotate[0][1] + m[2] * rotate[0][2];
        result[1] = m[0] * rotate[1][0] + m[1] * rotate[1][1] + m[2] * rotate[1][2];
        result[2] = m[0] * rotate[2][0] + m[1] * rotate[2][1] + m[2] * rotate[2][2];
        result[3] = m[3];
        return result;
    }
 
    mat4 scale(const mat4& m, float fx, float fy, float fz)
    {
        mat4 result;
        result[0] = m[0] * fx;
        result[1] = m[1] * fy;
        result[2] = m[2] * fz;
        result[3] = m[3];
        return result;
    }
 
    mat4 perspective(float fov, float ratio, float z_near, float z_far)
    {
        const float tan_hf = std::tan(fov / 2);
 
        mat4 result;
        result[0][0] = 1 / (ratio * tan_hf);
        result[1][1] = 1 / tan_hf;
        result[2][2] = -(z_far + z_near) / (z_far - z_near);
        result[2][3] = -1;
        result[3][2] = -(2 *z_far * z_near) / (z_far - z_near);
        return result;
    }
 
    constexpr float quat_pi()
    {
        return 0.7853981634f;
    }
 
    constexpr float half_pi()
    {
        return 1.5707963268f;
    }
 
    constexpr float pi()
    {
        return 3.1415926536f;
    }
 
    constexpr float two_pi()
    {
        return 6.2831853072f;
    }
 
    constexpr float dgs_rad()
    {
        return 0.01745329252f;
    }
 
    constexpr float radians(float deg)
    {
        return deg * dgs_rad();
    }
}
////////////////////////////////////////////////////////////////////////////////
Camera::Camera(float ratio)
    : m_yaw(-math::half_pi())
    , m_pitch()
    , m_view(math::identity())
    , m_proj(math::perspective(math::radians(45), ratio, 0.1f, 100.0f))
    , need_update(true)
{
}
 
void Camera::yaw(float angle)
{
    static constexpr float max_yaw = math::two_pi();
    m_yaw += angle;
    if (m_yaw < -max_yaw)
        m_yaw += max_yaw;
    else if (m_yaw > max_yaw)
        m_yaw -= max_yaw;
    change();
}
 
void Camera::pitch(float angle)
{
    static constexpr float max_pitch = math::quat_pi() - math::dgs_rad();
    m_pitch += angle;
    if (m_pitch < -max_pitch)
        m_pitch = -max_pitch;
    else if (m_pitch > max_pitch)
        m_pitch = max_pitch;
    change();
}
 
const math::mat4 & Camera::get_view()
{
    update();
    return m_view;
}
 
const math::mat4& Camera::get_proj()
{
    return m_proj;
}
 
void Camera::update()
{
    if (need_update)
    {
        need_update = false;
 
        math::vec3 front
        {
            std::cos(m_pitch) * std::cos(m_yaw),
            std::sin(m_pitch),
            std::cos(m_pitch) * std::sin(m_yaw)
        };
        front = normalize(front);
 
        const math::vec3 side = normalize(cross(front, math::vec3{ 0, 1, 0 }));
        const math::vec3 up = normalize(cross(side, front));
 
        m_view[0][0] = side[0];
        m_view[1][0] = side[1];
        m_view[2][0] = side[2];
        m_view[0][1] = up[0];
        m_view[1][1] = up[1];
        m_view[2][1] = up[2];
        m_view[0][2] = -front[0];
        m_view[1][2] = -front[1];
        m_view[2][2] = -front[2];
    }
}
 
void Camera::change()
{
    if (!need_update)
        need_update = true;
}
////////////////////////////////////////////////////////////////////////////////
bool Cube::ready = false;
 
const float Cube::vertices[] = 
{
    -1, -1, -1,
    -1,  1, -1,
    -1, -1,  1,
    -1, -1,  1,
    -1,  1, -1,
    -1,  1,  1,
 
     1, -1, -1,
     1,  1, -1,
     1, -1,  1,
     1, -1,  1,
     1,  1, -1,
     1,  1,  1,
 
    -1, -1, -1,
     1, -1, -1,
    -1, -1,  1,
    -1, -1,  1,
     1, -1, -1,
     1, -1,  1,
 
    -1,  1, -1,
     1,  1, -1,
    -1,  1,  1,
    -1,  1,  1,
     1,  1, -1,
     1,  1,  1,
 
    -1, -1, -1,
     1, -1, -1,
    -1,  1, -1,
    -1,  1, -1,
     1, -1, -1,
     1,  1, -1,
 
    -1, -1,  1,
     1, -1,  1,
    -1,  1,  1,
    -1,  1,  1,
     1, -1,  1, 
     1,  1,  1
};
 
Cube::Cube()
    : m_model(math::identity())
    , m_color(1, 0, 0)
{
}
 
void Cube::set_pos(const math::vec3 & pos)
{
    m_model = math::translate(math::identity(), pos[0], pos[1], pos[2]);
}
 
void Cube::set_color(const math::vec3 & color)
{
    m_color = color;
}
 
void Cube::draw(sf::Shader & shader, Camera & cam)
{
    const math::mat4 mvp = cam.get_proj() * cam.get_view() * m_model;
    draw(shader, mvp, m_color);
}
 
Bbox Cube::get_local_bounds()
{
    const math::vec4 min(-1, -1, -1, 1);
    const math::vec4 max( 1,  1,  1, 1);
    return  { min, max };
}
 
const math::mat4 & Cube::get_transform()
{
    return m_model;
}
 
void Cube::init()
{
    glEnableClientState(GL_VERTEX_ARRAY);
    glVertexPointer(3, GL_FLOAT, 3 * sizeof(float), vertices);
 
    glDisableClientState(GL_NORMAL_ARRAY);
    glDisableClientState(GL_COLOR_ARRAY);
    glDisableClientState(GL_TEXTURE_COORD_ARRAY);
 
    ready = true;
}
 
void Cube::draw(sf::Shader& shader, const math::mat4& mvp, const math::vec3& color)
{
    if (!ready) init();
 
    sf::Shader::bind(&shader);
    shader.setUniform("u_MVP", sf::Glsl::Mat4(&mvp[0][0]));
    shader.setUniform("u_color", sf::Glsl::Vec3(color[0], color[1], color[2]));
 
    glDrawArrays(GL_TRIANGLES, 0, 36);
    sf::Shader::bind(nullptr);
}
////////////////////////////////////////////////////////////////////////////////
std::vector<Cube> generate()
{
    std::vector<Cube> result;
    const float offset = 6;
    for (int x = -1; x <= 1; ++x)
    {
        for (int y = -1; y <= 1; ++y)
        {
            for (int z = -1; z <= 1; ++z)
            {
                if (x == 0 && z == 0) continue;
                result.push_back(Cube());
                result.back().set_pos(math::vec3(offset * x, offset * y, offset * z));
            }
        }
    }
    return result;
}
 
bool test_ray_intersection(const Bbox & bb, const math::mat4 & view, const math::mat4 & model)
{
    float t_min = 0.0f;
    float t_max = 100000.0f;
    const math::mat4 model_view = view * model;
    const math::vec3 pos(model_view[3][0], model_view[3][1], model_view[3][2]); // направление на объект
    const math::vec3 ray(0, 0, -1); // луч взгляда
    {
        const math::vec3 xaxis(model_view[0][0], model_view[0][1], model_view[0][2]); // ось X модели
        const float e = math::dot(xaxis, pos);
        const float f = -xaxis[2];
 
        if (std::abs(f) > 0.001f)
        {
            float t1 = (e + bb.min[0]) / f; // пересечение с левой плоскостью
            float t2 = (e + bb.max[0]) / f; // пересечение с правой плоскостью
 
            if (t1 > t2) std::swap(t1, t2);
 
            if (t2 < t_max)
                t_max = t2;
            if (t1 > t_min)
                t_min = t1;
            if (t_max < t_min)
                return false; // луч не пересекает модель, если ближнее пересечение дальше дальнего
        }
        else
        {   // лучь параллелен плоскостям
            if (-e + bb.min[0] > 0.0f || -e + bb.max[0] < 0.0f)
                return false;
        }
    }
    {   // аналогично для оси Y
        const math::vec3 yaxis(model_view[1][0], model_view[1][1], model_view[1][2]);
        const float e = math::dot(yaxis, pos);
        const float f = -yaxis[2];
 
        if (std::abs(f) > 0.001f)
        {
            float t1 = (e + bb.min[1]) / f;
            float t2 = (e + bb.max[1]) / f;
 
            if (t1 > t2) std::swap(t1, t2);
 
            if (t2 < t_max)
                t_max = t2;
            if (t1 > t_min)
                t_min = t1;
            if (t_min > t_max)
                return false;
        }
        else
        {
            if (-e + bb.min[1] > 0.0f || -e + bb.max[1] < 0.0f)
                return false;
        }
    }
    {   // аналогично для оси Z
        const math::vec3 zaxis(model_view[2][0], model_view[2][1], model_view[2][2]);
        const float e = math::dot(zaxis, pos);
        const float f = -zaxis[2];
 
        if (std::abs(f) > 0.001f)
        {
            float t1 = (e + bb.min[2]) / f;
            float t2 = (e + bb.max[2]) / f;
 
            if (t1 > t2)
                std::swap(t1, t2);
 
            if (t2 < t_max)
                t_max = t2;
            if (t1 > t_min)
                t_min = t1;
            if (t_min > t_max)
                return false;
        }
        else
        {
            if (-e + bb.min[2] > 0.0f || -e + bb.max[2] < 0.0f)
                return false;
        }
    }
    return true;
}