From f1c99067d8acfe73198c2c79b50752f906ec041f Mon Sep 17 00:00:00 2001 From: iikorni Date: Mon, 31 Aug 2026 19:00:04 -0500 Subject: [PATCH] add vec --- Quake/vec.cpp | 220 ++++++++++++++++++++++++++++++++++++++++++++++++++ Quake/vec.hpp | 96 ++++++++++++++++++++++ 2 files changed, 316 insertions(+) create mode 100644 Quake/vec.cpp create mode 100644 Quake/vec.hpp diff --git a/Quake/vec.cpp b/Quake/vec.cpp new file mode 100644 index 0000000..9e53f63 --- /dev/null +++ b/Quake/vec.cpp @@ -0,0 +1,220 @@ +#include "vec.hpp" + +#include +#include +#include + +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(x) * rhs.x) + (static_cast(y) * rhs.y) + (static_cast(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(x) * rhs.x) + (static_cast(y) * rhs.y) + (static_cast(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 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::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(pitch_cos * yaw_cos), static_cast(pitch_cos * yaw_sin), + static_cast(-pitch_sin) + }; + right = { + static_cast(-1 * roll_sin * pitch_sin * yaw_cos + -1 * roll_cos * -yaw_sin), + static_cast(-1 * roll_sin * pitch_sin * yaw_sin + -1 * roll_cos * yaw_cos), + static_cast(-1 * roll_sin * pitch_cos), + }; + up = { + static_cast(roll_cos * pitch_sin * yaw_cos + -roll_sin * -yaw_sin), + static_cast(roll_cos * pitch_sin * yaw_sin + -roll_sin * yaw_cos), + static_cast(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(std::cos(degrees_to_radians(angle))); + const float scale_side = static_cast(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[]"); + } + } +} diff --git a/Quake/vec.hpp b/Quake/vec.hpp new file mode 100644 index 0000000..d6c422b --- /dev/null +++ b/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 +#include + +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 into_array() const; + + static vec3f to_angles(vec3f forward); + + static std::tuple 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 }; + +}