add vec

Quake/vec.cpp

@@ -0,0 +1,220 @@
+#include "vec.hpp"
+
+#include <array>
+#include <cmath>
+#include <utility>
+
+namespace math {
+    float vec3f::dot(const vec3f &rhs) const {
+        return (x * rhs.x) + (y * rhs.y) + (z * rhs.z);
+    }
+
+    double vec3f::ddot(const vec3f &rhs) const {
+        return (static_cast<double>(x) * rhs.x) + (static_cast<double>(y) * rhs.y) + (static_cast<double>(z) * rhs.z);
+    }
+
+    float vec3f::dot(const vec4f &rhs) const {
+        return (x * rhs.x) + (y * rhs.y) + (z * rhs.z);
+    }
+
+    double vec3f::ddot(const vec4f &rhs) const {
+        return (static_cast<double>(x) * rhs.x) + (static_cast<double>(y) * rhs.y) + (static_cast<double>(z) * rhs.z);
+    }
+
+    float vec3f::operator*(const vec3f &rhs) const {
+        return dot(rhs);
+    }
+
+    float vec3f::operator*(const vec4f &rhs) const {
+        return dot(rhs);
+    }
+
+    vec3f vec3f::operator*(const float rhs) const {
+        return {x * rhs, y * rhs, z * rhs};
+    }
+
+    vec3f vec3f::operator+(const vec3f &rhs) const {
+        return {x + rhs.x, y + rhs.y, z + rhs.z};
+    }
+
+    vec3f vec3f::operator-(const vec3f &rhs) const {
+        return {x - rhs.x, y - rhs.y, z - rhs.z};
+    }
+
+    bool vec3f::operator==(const vec3f &rhs) const {
+        return x == rhs.x && y == rhs.y && z == rhs.z;
+    }
+
+    float &vec3f::operator[](const std::size_t i) {
+        switch (i) {
+            case 0:
+                return x;
+            case 1:
+                return y;
+            case 2:
+                return z;
+            default:
+                throw std::out_of_range("vec3f::operator[]");
+        }
+    }
+
+    vec3f vec3f::normalize() const {
+        vec3f res{0.0f, 0.0f, 0.0f};
+        if (const auto d = dot(*this); d > 0.0f) {
+            res = *this * (1.0f / std::sqrt(d));
+        }
+
+        return res;
+    }
+
+    vec3f vec3f::multiply_add(const vec3f &rhs, const float scale) const {
+        return {
+            x + (scale * rhs.x),
+            y + (scale * rhs.y),
+            z + (scale * rhs.z)
+        };
+    }
+
+    vec3f vec3f::cross(const vec3f &rhs) const {
+        return {
+            (y * rhs.z) - (z * rhs.y),
+            (z * rhs.x) - (x * rhs.z),
+            (x * rhs.y) - (y * rhs.x)
+        };
+    }
+
+    float vec3f::magnitude() const {
+        return std::sqrt(dot(*this));
+    }
+
+    vec3f vec3f::operator-() const {
+        return {-x, -y, -z};
+    }
+
+    vec3f project_point_on_plane(const vec3f &point, const vec3f &normal) {
+        const auto inv_denom = 1.0f / normal.dot(normal);
+        const auto dist = normal.dot(point) * inv_denom;
+
+        const auto scaled_normal = normal * inv_denom;
+
+        return point - (scaled_normal * dist);
+    }
+
+    vec3f vec3f::perpendicular() const {
+        vec3f res{0.0f, 0.0f, 0.0f};
+        auto pos = 0;
+        auto min = 1.0f;
+
+        if (std::abs(x) < min) {
+            pos = 0;
+            min = std::abs(x);
+        }
+        if (std::abs(y) < min) {
+            pos = 1;
+            min = std::abs(y);
+        }
+        if (std::abs(z) < min) {
+            pos = 2;
+            min = std::abs(z);
+        }
+
+        switch (pos) {
+            case 0:
+                res.x = 1.0f;
+                break;
+            case 1:
+                res.y = 1.0f;
+                break;
+            case 2:
+                res.z = 1.0f;
+                break;
+            default:
+                std::unreachable();
+        }
+
+        res = project_point_on_plane(res, *this);
+
+        res = res.normalize();
+
+        return res;
+    }
+
+    std::array<float, 3> vec3f::into_array() const {
+        return std::array{x, y, z};
+    }
+
+    vec3f vec3f::to_angles(const vec3f forward) {
+        const vec3f temp{forward.x, forward.y, 0.0f};
+
+        auto pitch = -std::atan2(forward.z, temp.magnitude());
+        auto yaw = std::atan2(forward.y, forward.x);
+        auto roll = 0.0f;
+
+        return {pitch, yaw, roll};
+    }
+
+    std::tuple<vec3f, vec3f, vec3f> vec3f::from_angles(const vec3f &angles) {
+        vec3f forward{0.0f, 0.0f, 0.0f};
+        vec3f right{0.0f, 0.0f, 0.0f};
+        vec3f up{0.0f, 0.0f, 0.0f};
+
+        const auto pitch = angles.x * (std::numbers::pi * 2 / 360);
+        const auto pitch_sin = std::sin(pitch);
+        const auto pitch_cos = std::cos(pitch);
+        const auto yaw = angles.y * (std::numbers::pi * 2 / 360);
+        const auto yaw_sin = std::sin(yaw);
+        const auto yaw_cos = std::cos(yaw);
+        const auto roll = angles.z * (std::numbers::pi * 2 / 360);
+        const auto roll_sin = std::sin(roll);
+        const auto roll_cos = std::cos(roll);
+
+        forward = {
+            static_cast<float>(pitch_cos * yaw_cos), static_cast<float>(pitch_cos * yaw_sin),
+            static_cast<float>(-pitch_sin)
+        };
+        right = {
+            static_cast<float>(-1 * roll_sin * pitch_sin * yaw_cos + -1 * roll_cos * -yaw_sin),
+            static_cast<float>(-1 * roll_sin * pitch_sin * yaw_sin + -1 * roll_cos * yaw_cos),
+            static_cast<float>(-1 * roll_sin * pitch_cos),
+        };
+        up = {
+            static_cast<float>(roll_cos * pitch_sin * yaw_cos + -roll_sin * -yaw_sin),
+            static_cast<float>(roll_cos * pitch_sin * yaw_sin + -roll_sin * yaw_cos),
+            static_cast<float>(roll_cos * pitch_cos),
+        };
+        return {forward, right, up};
+    }
+
+    /**
+     * turn forward towards side on the plane defined by forward and side
+     * if angle = 90, the result will be equal to side
+     * assumes side and forward are perpendicular, and normalized
+     * to turn away from side, use a negative angle
+     *
+     * @param forward
+     * @param side
+     * @param angle
+     * @return
+     */
+    vec3f vec3f::turn(const vec3f &forward, const vec3f &side, const float angle) {
+        const float scale_forward = static_cast<float>(std::cos(degrees_to_radians(angle)));
+        const float scale_side = static_cast<float>(std::sin(degrees_to_radians(angle)));
+
+        return (forward * scale_forward) + (side * scale_side);
+    }
+
+    float & vec4f::operator[](std::size_t i) {
+        switch (i) {
+            case 0:
+                return x;
+            case 1:
+                return y;
+            case 2:
+                return z;
+            case 3:
+                return w;
+            default:
+                throw std::out_of_range("vec4f::operator[]");
+        }
+    }
+}

Quake/vec.hpp

@@ -0,0 +1,96 @@
+/*
+Copyright (C) 2026 iikorni
+
+This program is free software; you can redistribute it and/or
+modify it under the terms of the GNU General Public License
+as published by the Free Software Foundation; either version 2
+of the License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
+
+*/
+
+#pragma once
+#include <tuple>
+#include <GL/gl.h>
+
+namespace math {
+    constexpr std::floating_point auto degrees_to_radians(std::floating_point auto angle) {
+        return angle * (std::numbers::pi / 180.0);
+    }
+
+    struct vec3f;
+    struct vec4f;
+
+    struct vec3f {
+        float x, y, z;
+
+        constexpr vec3f() : x(0), y(0), z(0) {}
+        constexpr vec3f(const float x, const float y, const float z) : x(x), y(y), z(z) {
+        }
+
+        [[nodiscard]] float dot(const vec3f &rhs) const;
+
+        [[nodiscard]] double ddot(const vec3f &rhs) const;
+
+        [[nodiscard]] float dot(const vec4f &rhs) const;
+
+        [[nodiscard]] double ddot(const vec4f &rhs) const;
+
+        float operator*(const vec3f &rhs) const;
+
+        float operator*(const vec4f &rhs) const;
+
+        vec3f operator*(const float rhs) const;
+
+        vec3f operator+(const vec3f &rhs) const;
+
+        vec3f operator-(const vec3f &rhs) const;
+
+        bool operator==(const vec3f &rhs) const;
+
+        float& operator[](std::size_t i);
+
+        vec3f normalize() const;
+
+        vec3f multiply_add(const vec3f &rhs, float scale) const;
+
+        vec3f cross(const vec3f &rhs) const;
+
+        float magnitude() const;
+
+        vec3f operator-() const;
+
+        vec3f perpendicular() const;
+
+        std::array<float, 3> into_array() const;
+
+        static vec3f to_angles(vec3f forward);
+
+        static std::tuple<vec3f, vec3f, vec3f> from_angles(const vec3f &angles);
+
+        static vec3f turn(const vec3f &forward, const vec3f &side, float angle);
+    };
+
+
+    struct vec4f {
+        float x, y, z, w;
+
+        constexpr vec4f() : x(0), y(0), z(0), w(1.0) {}
+        constexpr vec4f(const float x, const float y, const float z, const float w) : x(x), y(y), z(z), w(w) {}
+        explicit constexpr vec4f(const vec3f &v) : x(v.x), y(v.y), z(v.z), w(1.0f) {}
+
+        float &operator[](std::size_t i);
+    };
+
+    constexpr vec3f ORIGIN = { 0.0f, 0.0f, 0.0f };
+
+}