From 05cc7f0bf2f6832f08e963a769e28e90be0936dc Mon Sep 17 00:00:00 2001 From: BlackMATov Date: Sat, 27 Feb 2021 08:03:31 +0700 Subject: [PATCH] add single header vmath variant --- CMakeLists.txt | 4 + singles/vmath.hpp/vmath.hpp | 4403 +++++++++++++++++++++++++++++++++++ untests/CMakeLists.txt | 2 +- 3 files changed, 4408 insertions(+), 1 deletion(-) create mode 100644 singles/vmath.hpp/vmath.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 542fef8..6d64e76 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,6 +11,10 @@ add_library(${PROJECT_NAME} INTERFACE) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) target_include_directories(${PROJECT_NAME} INTERFACE headers) +add_library(${PROJECT_NAME}.singles INTERFACE) +target_compile_features(${PROJECT_NAME}.singles INTERFACE cxx_std_17) +target_include_directories(${PROJECT_NAME}.singles INTERFACE singles) + if(BUILD_AS_STANDALONE) option(BUILD_WITH_UNTESTS "Build with unit tests" ON) if(BUILD_WITH_UNTESTS) diff --git a/singles/vmath.hpp/vmath.hpp b/singles/vmath.hpp/vmath.hpp new file mode 100644 index 0000000..b85f903 --- /dev/null +++ b/singles/vmath.hpp/vmath.hpp @@ -0,0 +1,4403 @@ +/******************************************************************************* + * This file is part of the "https://github.com/blackmatov/vmath.hpp" + * For conditions of distribution and use, see copyright notice in LICENSE.md + * Copyright (C) 2020-2021, by Matvey Cherevko (blackmatov@gmail.com) + ******************************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#if defined(_MSC_VER) +# define VMATH_HPP_FORCE_INLINE __forceinline +#elif defined(__clang__) || defined(__GNUC__) +# define VMATH_HPP_FORCE_INLINE inline __attribute__((__always_inline__)) +#else +# define VMATH_HPP_FORCE_INLINE inline +#endif + +#if !defined(__cpp_exceptions) && !defined(__EXCEPTIONS) && !defined(_CPPUNWIND) +# define VMATH_HPP_NO_EXCEPTIONS +#endif + +#ifdef VMATH_HPP_NO_EXCEPTIONS +# define VMATH_HPP_THROW(...) std::abort() +#else +# define VMATH_HPP_THROW(...) throw __VA_ARGS__ +#endif + +#define VMATH_HPP_THROW_IF(pred, ...)\ + ( (pred) ? VMATH_HPP_THROW(__VA_ARGS__) : (void)0 ) + +namespace vmath_hpp +{ + struct uninit_t { explicit uninit_t() = default; }; + inline constexpr uninit_t uninit{}; + + struct zero_init_t { explicit zero_init_t() = default; }; + inline constexpr zero_init_t zero_init{}; + + struct unit_init_t { explicit unit_init_t() = default; }; + inline constexpr unit_init_t unit_init{}; + + struct identity_init_t { explicit identity_init_t() = default; }; + inline constexpr identity_init_t identity_init{}; +} + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + class vec; + + using bvec2 = vec; + using bvec3 = vec; + using bvec4 = vec; + + using ivec2 = vec; + using ivec3 = vec; + using ivec4 = vec; + + using uvec2 = vec; + using uvec3 = vec; + using uvec4 = vec; + + using fvec2 = vec; + using fvec3 = vec; + using fvec4 = vec; + + using dvec2 = vec; + using dvec3 = vec; + using dvec4 = vec; +} + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + class mat; + + using bmat2 = mat; + using bmat3 = mat; + using bmat4 = mat; + + using imat2 = mat; + using imat3 = mat; + using imat4 = mat; + + using umat2 = mat; + using umat3 = mat; + using umat4 = mat; + + using fmat2 = mat; + using fmat3 = mat; + using fmat4 = mat; + + using dmat2 = mat; + using dmat3 = mat; + using dmat4 = mat; +} + +namespace vmath_hpp +{ + template < typename T > + class qua; + + using fqua = qua; + using dqua = qua; +} + +// +// Common Functions +// + +namespace vmath_hpp +{ + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr abs(T x) noexcept { + if constexpr ( std::is_signed_v ) { + return x < T{0} ? -x : x; + } else { + return x; + } + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr sqr(T x) noexcept { + return x * x; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr sign(T x) noexcept { + return static_cast((T{0} < x) - (x < T{0})); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr rcp(T x) noexcept { + return T{1} / x; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + floor(T x) noexcept { + return std::floor(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + trunc(T x) noexcept { + return std::trunc(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + round(T x) noexcept { + return std::round(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + ceil(T x) noexcept { + return std::ceil(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + fract(T x) noexcept { + return x - floor(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + fmod(T x, T y) noexcept { + return std::fmod(x, y); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + modf(T x, T* y) noexcept { + return std::modf(x, y); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + copysign(T x, T s) noexcept { + return std::copysign(x, s); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr min(T x, T y) noexcept { + return x < y ? x : y; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr max(T x, T y) noexcept { + return x < y ? y : x; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr clamp(T x, T min_x, T max_x) noexcept { + return min(max(x, min_x), max_x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr saturate(T x) noexcept { + return clamp(x, T{0}, T{1}); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr lerp(T x, T y, T a) noexcept { + return x * (T{1} - a) + y * a; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr lerp(T x, T y, T x_a, T y_a) noexcept { + return x * x_a + y * y_a; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr step(T edge, T x) noexcept { + return x < edge ? T{0} : T{1}; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr smoothstep(T edge0, T edge1, T x) noexcept { + const T t = clamp((x - edge0) * rcp(edge1 - edge0), T{0}, T{1}); + return t * t * (T{3} - T{2} * t); + } +} + +// +// Angle and Trigonometric Functions +// + +namespace vmath_hpp +{ + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr radians(T degrees) noexcept { + return degrees * T(0.01745329251994329576923690768489); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr degrees(T radians) noexcept { + return radians * T(57.295779513082320876798154814105); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + sin(T x) noexcept { + return std::sin(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + cos(T x) noexcept { + return std::cos(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + tan(T x) noexcept { + return std::tan(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + asin(T x) noexcept { + return std::asin(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + acos(T x) noexcept { + return std::acos(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + atan(T x) noexcept { + return std::atan(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + atan2(T y, T x) noexcept { + return std::atan2(y, x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + sinh(T x) noexcept { + return std::sinh(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + cosh(T x) noexcept { + return std::cosh(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + tanh(T x) noexcept { + return std::tanh(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + asinh(T x) noexcept { + return std::asinh(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + acosh(T x) noexcept { + return std::acosh(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + atanh(T x) noexcept { + return std::atanh(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, std::pair> + sincos(T x) noexcept { + return { sin(x), cos(x) }; + } + + template < typename T > + std::enable_if_t, void> + sincos(T x, T* s, T* c) noexcept { + *s = sin(x); + *c = cos(x); + } +} + +// +// Exponential Functions +// + +namespace vmath_hpp +{ + template < typename T > + [[nodiscard]] std::enable_if_t, T> + pow(T x, T y) noexcept { + return std::pow(x, y); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + exp(T x) noexcept { + return std::exp(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + log(T x) noexcept { + return std::log(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + exp2(T x) noexcept { + return std::exp2(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + log2(T x) noexcept { + return std::log2(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + sqrt(T x) noexcept { + return std::sqrt(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + rsqrt(T x) noexcept { + return rcp(sqrt(x)); + } +} + +// +// Geometric Functions +// + +namespace vmath_hpp +{ + template < typename T, typename U + , typename V = decltype(std::declval() * std::declval()) > + [[nodiscard]] std::enable_if_t, V> + constexpr dot(T x, U y) noexcept { + return { x * y }; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr length(T x) noexcept { + return abs(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr rlength(T x) noexcept { + return rcp(abs(x)); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr length2(T x) noexcept { + return dot(x, x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr rlength2(T x) noexcept { + return rcp(dot(x, x)); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr distance(T x, T y) noexcept { + return length(y - x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr distance2(T x, T y) noexcept { + return length2(y - x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr normalize(T x) noexcept { + return x * rlength(x); + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr faceforward(T n, T i, T nref) noexcept { + return dot(nref, i) < T{0} ? n : -n; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + constexpr reflect(T i, T n) noexcept { + return i - T{2} * dot(n, i) * n; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, T> + refract(T i, T n, T eta) noexcept { + const T d = dot(n, i); + const T k = T{1} - sqr(eta) * (T{1} - sqr(d)); + return k < T{0} ? T{0} : (eta * i - (eta * d + sqrt(k)) * n); + } +} + +// +// Relational Functions +// + +namespace vmath_hpp +{ + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr any(T x) noexcept { + return !!x; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr all(T x) noexcept { + return !!x; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr approx(T x, T y) noexcept { + if constexpr ( std::is_floating_point_v ) { + /// REFERENCE: + /// http://www.realtimecollisiondetection.net/pubs/Tolerances + const T epsilon = std::numeric_limits::epsilon(); + return abs(x - y) <= epsilon * max(max(T{1}, abs(x)), abs(y)); + } else { + return x == y; + } + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr approx(T x, T y, T epsilon) noexcept { + if constexpr ( std::is_floating_point_v ) { + /// REFERENCE: + /// http://www.realtimecollisiondetection.net/pubs/Tolerances + return abs(x - y) <= epsilon * max(max(T{1}, abs(x)), abs(y)); + } else { + return abs(x - y) <= epsilon; + } + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr less(T x, T y) noexcept { + return x < y; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr less_equal(T x, T y) noexcept { + return x <= y; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr greater(T x, T y) noexcept { + return x > y; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr greater_equal(T x, T y) noexcept { + return x >= y; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr equal_to(T x, T y) noexcept { + return x == y; + } + + template < typename T > + [[nodiscard]] std::enable_if_t, bool> + constexpr not_equal_to(T x, T y) noexcept { + return x != y; + } +} + +namespace vmath_hpp::detail +{ + template < typename T, std::size_t Size > + class vec_base; + + template < typename T > + class vec_base { + public: + T x, y; + public: + constexpr vec_base() + : vec_base{zero_init} {} + + constexpr vec_base(uninit_t) {} + constexpr vec_base(zero_init_t): vec_base{T{0}} {} + constexpr vec_base(unit_init_t): vec_base{T{1}} {} + + constexpr explicit vec_base(T v): x{v}, y{v} {} + constexpr vec_base(T x, T y): x{x}, y{y} {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr vec_base(const vec_base& other): vec_base(other[0], other[1]) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit vec_base(const vec_base& other): vec_base(other[0], other[1]) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit vec_base(const vec_base& other): vec_base(other[0], other[1]) {} + + [[nodiscard]] constexpr T& operator[](std::size_t index) noexcept { + switch ( index ) { + default: + case 0: return x; + case 1: return y; + } + } + + [[nodiscard]] constexpr const T& operator[](std::size_t index) const noexcept { + switch ( index ) { + default: + case 0: return x; + case 1: return y; + } + } + }; + + template < typename T > + class vec_base { + public: + T x, y, z; + public: + constexpr vec_base() + : vec_base{zero_init} {} + + constexpr vec_base(uninit_t) {} + constexpr vec_base(zero_init_t): vec_base{T{0}} {} + constexpr vec_base(unit_init_t): vec_base{T{1}} {} + + constexpr explicit vec_base(T v): x{v}, y{v}, z{v} {} + constexpr vec_base(T x, T y, T z): x{x}, y{y}, z{z} {} + + constexpr vec_base(const vec_base& xy, T z): vec_base(xy[0], xy[1], z) {} + constexpr vec_base(T x, const vec_base& yz): vec_base(x, yz[0], yz[1]) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr vec_base(const vec_base& other): vec_base(other[0], other[1], other[2]) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit vec_base(const vec_base& other): vec_base(other[0], other[1], other[2]) {} + + [[nodiscard]] constexpr T& operator[](std::size_t index) noexcept { + switch ( index ) { + default: + case 0: return x; + case 1: return y; + case 2: return z; + } + } + + [[nodiscard]] constexpr const T& operator[](std::size_t index) const noexcept { + switch ( index ) { + default: + case 0: return x; + case 1: return y; + case 2: return z; + } + } + }; + + template < typename T > + class vec_base { + public: + T x, y, z, w; + public: + constexpr vec_base() + : vec_base{zero_init} {} + + constexpr vec_base(uninit_t) {} + constexpr vec_base(zero_init_t) : vec_base{T{0}} {} + constexpr vec_base(unit_init_t) : vec_base{T{1}} {} + + constexpr explicit vec_base(T v): x{v}, y{v}, z{v}, w{v} {} + constexpr vec_base(T x, T y, T z, T w): x{x}, y{y}, z{z}, w{w} {} + + constexpr vec_base(const vec_base& xy, T z, T w): vec_base(xy[0], xy[1], z, w) {} + constexpr vec_base(T x, const vec_base& yz, T w): vec_base(x, yz[0], yz[1], w) {} + constexpr vec_base(T x, T y, const vec_base& zw): vec_base(x, y, zw[0], zw[1]) {} + constexpr vec_base(const vec_base& xy, const vec_base& zw): vec_base(xy[0], xy[1], zw[0], zw[1]) {} + + constexpr vec_base(const vec_base& xyz, T w): vec_base(xyz[0], xyz[1], xyz[2], w) {} + constexpr vec_base(T x, const vec_base& yzw): vec_base(x, yzw[0], yzw[1], yzw[2]) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr vec_base(const vec_base& other): vec_base(other[0], other[1], other[2], other[3]) {} + + [[nodiscard]] constexpr T& operator[](std::size_t index) noexcept { + switch ( index ) { + default: + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + } + + [[nodiscard]] constexpr const T& operator[](std::size_t index) const noexcept { + switch ( index ) { + default: + case 0: return x; + case 1: return y; + case 2: return z; + case 3: return w; + } + } + }; +} + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + class vec final : public detail::vec_base { + public: + using self_type = vec; + using base_type = detail::vec_base; + using component_type = T; + + using pointer = component_type*; + using const_pointer = const component_type*; + + using reference = component_type&; + using const_reference = const component_type&; + + using iterator = pointer; + using const_iterator = const_pointer; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + static inline constexpr std::size_t size = Size; + public: + using base_type::vec_base; + using base_type::operator[]; + + void swap(vec& other) noexcept(std::is_nothrow_swappable_v) { + for ( std::size_t i = 0; i < Size; ++i ) { + using std::swap; + swap((*this)[i], other[i]); + } + } + + [[nodiscard]] iterator begin() noexcept { return iterator(data()); } + [[nodiscard]] const_iterator begin() const noexcept { return const_iterator(data()); } + [[nodiscard]] iterator end() noexcept { return iterator(data() + Size); } + [[nodiscard]] const_iterator end() const noexcept { return const_iterator(data() + Size); } + + [[nodiscard]] reverse_iterator rbegin() noexcept { return reverse_iterator(end()); } + [[nodiscard]] const_reverse_iterator rbegin() const noexcept { return const_reverse_iterator(end()); } + [[nodiscard]] reverse_iterator rend() noexcept { return reverse_iterator(begin()); } + [[nodiscard]] const_reverse_iterator rend() const noexcept { return const_reverse_iterator(begin()); } + + [[nodiscard]] const_iterator cbegin() const noexcept { return begin(); } + [[nodiscard]] const_iterator cend() const noexcept { return end(); } + [[nodiscard]] const_reverse_iterator crbegin() const noexcept { return rbegin(); } + [[nodiscard]] const_reverse_iterator crend() const noexcept { return rend(); } + + [[nodiscard]] pointer data() noexcept { + return &(*this)[0]; + } + + [[nodiscard]] const_pointer data() const noexcept { + return &(*this)[0]; + } + + [[nodiscard]] constexpr reference at(std::size_t index) { + VMATH_HPP_THROW_IF(index >= size, std::out_of_range("vec::at")); + return (*this)[index]; + } + + [[nodiscard]] constexpr const_reference at(std::size_t index) const { + VMATH_HPP_THROW_IF(index >= size, std::out_of_range("vec::at")); + return (*this)[index]; + } + }; +} + +namespace vmath_hpp +{ + // vec2 + + template < typename T > + vec(T, T) -> vec; + + // vec3 + + template < typename T > + vec(T, T, T) -> vec; + + template < typename T > + vec(const vec&, T) -> vec; + + template < typename T > + vec(T, const vec&) -> vec; + + // vec4 + + template < typename T > + vec(T, T, T, T) -> vec; + + template < typename T > + vec(const vec&, T, T) -> vec; + + template < typename T > + vec(T, const vec&, T) -> vec; + + template < typename T > + vec(T, T, const vec&) -> vec; + + template < typename T > + vec(const vec&, const vec&) -> vec; + + template < typename T > + vec(const vec&, T) -> vec; + + template < typename T > + vec(T, const vec&) -> vec; + + // swap + + template < typename T, std::size_t Size > + void swap(vec& l, vec& r) noexcept(noexcept(l.swap(r))) { + l.swap(r); + } +} + +namespace vmath_hpp::detail::impl +{ + template < typename A, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join_impl( + F&& f, + const vec& a, + std::index_sequence) + { + return vec{ f(a[Is])... }; + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join_impl( + F&& f, + const vec& a, + const vec& b, + std::index_sequence) + { + return vec{ f(a[Is], b[Is])... }; + } + + template < typename A, typename B, typename C, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join_impl( + F&& f, + const vec& a, + const vec& b, + const vec& c, + std::index_sequence) + { + return vec{ f(a[Is], b[Is], c[Is])... }; + } + + template < typename A, typename B, typename C, typename D, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join_impl( + F&& f, + const vec& a, + const vec& b, + const vec& c, + const vec& d, + std::index_sequence) + { + return vec{ f(a[Is], b[Is], c[Is], d[Is])... }; + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold_join_impl( + F&& f, + A init, + const vec& b, + std::index_sequence) + { + return ((init = f(std::move(init), b[Is])), ...); + } + + template < typename A, std::size_t Size, typename F, std::size_t I, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_join_impl( + F&& f, + const vec& a, + std::index_sequence) + { + A init = a[I]; + return ((init = f(std::move(init), a[Is])), ...); + } + + template < typename A, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join_impl( + F&& f, + const vec& a, + std::index_sequence) + { + return (... && f(a[Is])); + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join_impl( + F&& f, + const vec& a, + const vec& b, + std::index_sequence) + { + return (... && f(a[Is], b[Is])); + } + + template < typename A, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join_impl( + F&& f, + const vec& a, + std::index_sequence) + { + return (... || f(a[Is])); + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join_impl( + F&& f, + const vec& a, + const vec& b, + std::index_sequence) + { + return (... || f(a[Is], b[Is])); + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_plus_join_impl( + F&& f, + const vec& a, + const vec& b, + std::index_sequence) + { + return (... + f(a[Is], b[Is])); + } +} + +namespace vmath_hpp::detail +{ + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const vec& a) { + return impl::map_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const vec& a, const vec& b) { + return impl::map_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } + + template < typename A, typename B, typename C, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const vec& a, const vec& b, const vec& c) { + return impl::map_join_impl(std::forward(f), a, b, c, std::make_index_sequence{}); + } + + template < typename A, typename B, typename C, typename D, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const vec& a, const vec& b, const vec& c, const vec& d) { + return impl::map_join_impl(std::forward(f), a, b, c, d, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold_join(F&& f, A init, const vec& b) { + return impl::fold_join_impl(std::forward(f), std::move(init), b, std::make_index_sequence{}); + } + + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_join(F&& f, const vec& a) { + return impl::fold1_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join(F&& f, const vec& a) { + return impl::fold1_and_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join(F&& f, const vec& a, const vec& b) { + return impl::fold1_and_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } + + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join(F&& f, const vec& a) { + return impl::fold1_or_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join(F&& f, const vec& a, const vec& b) { + return impl::fold1_or_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_plus_join(F&& f, const vec& a, const vec& b) { + return impl::fold1_plus_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } +} + +// +// Operators +// + +namespace vmath_hpp +{ + // +operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator+(const vec& xs) { + return map_join([](T x){ return +x; }, xs); + } + + // -operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator-(const vec& xs) { + return map_join([](T x){ return -x; }, xs); + } + + // ~operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator~(const vec& xs) { + return map_join([](T x){ return ~x; }, xs); + } + + // !operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator!(const vec& xs) { + return map_join([](T x){ return !x; }, xs); + } + + // ++operator + + template < typename T, std::size_t Size > + constexpr vec& operator++(vec& xs) { + return (xs = xs + T{1}); + } + + // --operator + + template < typename T, std::size_t Size > + constexpr vec& operator--(vec& xs) { + return (xs = xs - T{1}); + } + + // operator++ + + template < typename T, std::size_t Size > + constexpr vec operator++(vec& xs, int) { + vec ys = xs; + ++xs; + return ys; + } + + // operator-- + + template < typename T, std::size_t Size > + constexpr vec operator--(vec& xs, int) { + vec ys = xs; + --xs; + return ys; + } + + // operator+ + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator+(const vec& xs, U y) { + return map_join([y](T x){ return x + y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator+(T x, const vec& ys) { + return map_join([x](U y){ return x + y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator+(const vec& xs, const vec& ys) { + return map_join([](T x, U y){ return x + y; }, xs, ys); + } + + // operator+= + + template < typename T, std::size_t Size > + constexpr vec& operator+=(vec& xs, T y) { + return (xs = (xs + y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator+=(vec& xs, const vec& ys) { + return (xs = (xs + ys)); + } + + // operator- + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator-(const vec& xs, U y) { + return map_join([y](T x){ return x - y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator-(T x, const vec& ys) { + return map_join([x](U y){ return x - y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator-(const vec& xs, const vec& ys) { + return map_join([](T x, U y){ return x - y; }, xs, ys); + } + + // operator-= + + template < typename T, std::size_t Size > + constexpr vec& operator-=(vec& xs, T y) { + return (xs = (xs - y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator-=(vec& xs, const vec& ys) { + return (xs = (xs - ys)); + } + + // operator* + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(const vec& xs, U y) { + return map_join([y](T x){ return x * y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(T x, const vec& ys) { + return map_join([x](U y){ return x * y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(const vec& xs, const vec& ys) { + return map_join([](T x, U y){ return x * y; }, xs, ys); + } + + // operator*= + + template < typename T, std::size_t Size > + constexpr vec& operator*=(vec& xs, T y) { + return (xs = (xs * y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator*=(vec& xs, const vec& ys) { + return (xs = (xs * ys)); + } + + // operator/ + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator/(const vec& xs, U y) { + return map_join([y](T x){ return x / y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator/(T x, const vec& ys) { + return map_join([x](U y){ return x / y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator/(const vec& xs, const vec& ys) { + return map_join([](T x, U y){ return x / y; }, xs, ys); + } + + // operator/= + + template < typename T, std::size_t Size > + constexpr vec& operator/=(vec& xs, T y) { + return (xs = (xs / y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator/=(vec& xs, const vec& ys) { + return (xs = (xs / ys)); + } + + // operator& + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&(const vec& xs, T y) { + return map_join([y](T x){ return x & y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&(T x, const vec& ys) { + return map_join([x](T y){ return x & y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return x & y; }, xs, ys); + } + + // operator&= + + template < typename T, std::size_t Size > + constexpr vec& operator&=(vec& xs, T y) { + return (xs = (xs & y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator&=(vec& xs, const vec& ys) { + return (xs = (xs & ys)); + } + + // operator| + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator|(const vec& xs, T y) { + return map_join([y](T x){ return x | y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator|(T x, const vec& ys) { + return map_join([x](T y){ return x | y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator|(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return x | y; }, xs, ys); + } + + // operator|= + + template < typename T, std::size_t Size > + constexpr vec& operator|=(vec& xs, T y) { + return (xs = (xs | y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator|=(vec& xs, const vec& ys) { + return (xs = (xs | ys)); + } + + // operator^ + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator^(const vec& xs, T y) { + return map_join([y](T x){ return x ^ y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator^(T x, const vec& ys) { + return map_join([x](T y){ return x ^ y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator^(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return x ^ y; }, xs, ys); + } + + // operator^= + + template < typename T, std::size_t Size > + constexpr vec& operator^=(vec& xs, T y) { + return (xs = (xs ^ y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator^=(vec& xs, const vec& ys) { + return (xs = (xs ^ ys)); + } + + // operator&& + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&&(const vec& xs, T y) { + return map_join([y](T x){ return x && y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&&(T x, const vec& ys) { + return map_join([x](T y){ return x && y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&&(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return x && y; }, xs, ys); + } + + // operator|| + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator||(const vec& xs, T y) { + return map_join([y](T x){ return x || y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator||(T x, const vec& ys) { + return map_join([x](T y){ return x || y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator||(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return x || y; }, xs, ys); + } + + // operator== + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr bool operator==(const vec& xs, const vec& ys) { + return fold1_and_join([](T x, T y){ return x == y; }, xs, ys); + } + + // operator!= + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr bool operator!=(const vec& xs, const vec& ys) { + return fold1_or_join([](T x, T y){ return x != y; }, xs, ys); + } + + // operator< + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr bool operator<(const vec& xs, const vec& ys) { + for ( std::size_t i = 0; i < Size; ++i ) { + if ( xs[i] < ys[i] ) { + return true; + } + if ( ys[i] < xs[i] ) { + return false; + } + } + return false; + } +} + +// +// Common Functions +// + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec abs(const vec& xs) { + return map_join([](T x) { return abs(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec sqr(const vec& xs) { + return map_join([](T x) { return sqr(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec sign(const vec& xs) { + return map_join([](T x) { return sign(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec rcp(const vec& xs) { + return map_join([](T x) { return rcp(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec floor(const vec& xs) { + return map_join([](T x) { return floor(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec trunc(const vec& xs) { + return map_join([](T x) { return trunc(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec round(const vec& xs) { + return map_join([](T x) { return round(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec ceil(const vec& xs) { + return map_join([](T x) { return ceil(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec fract(const vec& xs) { + return map_join([](T x) { return fract(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec fmod(const vec& xs, T y) { + return map_join([y](T x) { return fmod(x, y); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec fmod(const vec& xs, const vec& ys) { + return map_join([](T x, T y) { return fmod(x, y); }, xs, ys); + } + + namespace impl + { + template < typename T, std::size_t Size, std::size_t... Is > + constexpr VMATH_HPP_FORCE_INLINE + vec modf_impl(const vec& xs, vec* is, std::index_sequence) { + return { modf(xs[Is], &(*is)[Is])... }; + } + } + + template < typename T, std::size_t Size > + constexpr vec modf(const vec& xs, vec* is) { + return impl::modf_impl(xs, is, std::make_index_sequence{}); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec copysign(const vec& xs, T s) { + return map_join([s](T x) { return copysign(x, s); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec copysign(const vec& xs, const vec& ss) { + return map_join([](T x, T s) { return copysign(x, s); }, xs, ss); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T min(const vec& xs) { + return fold1_join([](T acc, T x){ return min(acc, x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec min(const vec& xs, T y) { + return map_join([y](T x) { return min(x, y); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec min(T x, const vec& ys) { + return map_join([x](T y) { return min(x, y); }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec min(const vec& xs, const vec& ys) { + return map_join([](T x, T y) { return min(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T max(const vec& xs) { + return fold1_join([](T acc, T x){ return max(acc, x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec max(const vec& xs, T y) { + return map_join([y](T x) { return max(x, y); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec max(T x, const vec& ys) { + return map_join([x](T y) { return max(x, y); }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec max(const vec& xs, const vec& ys) { + return map_join([](T x, T y) { return max(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec clamp(const vec& xs, T min_x, T max_x) { + return map_join([min_x, max_x](T x) { return clamp(x, min_x, max_x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec clamp(const vec& xs, const vec& min_xs, const vec& max_xs) { + return map_join([](T x, T min_x, T max_x) { return clamp(x, min_x, max_x); }, xs, min_xs, max_xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec saturate(const vec& xs) { + return map_join([](T x) { return saturate(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec lerp(const vec& xs, const vec& ys, T a) { + return map_join([a](T x, T y) { return lerp(x, y, a); }, xs, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec lerp(const vec& xs, const vec& ys, T x_a, T y_a) { + return map_join([x_a, y_a](T x, T y) { return lerp(x, y, x_a, y_a); }, xs, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec lerp( + const vec& xs, + const vec& ys, + const vec& as) + { + return map_join([](T x, T y, T a) { return lerp(x, y, a); }, xs, ys, as); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec lerp( + const vec& xs, + const vec& ys, + const vec& xs_a, + const vec& ys_a) + { + return map_join([](T x, T y, T x_a, T y_a) { return lerp(x, y, x_a, y_a); }, xs, ys, xs_a, ys_a); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec step(T edge, const vec& xs) { + return map_join([edge](T x) { return step(edge, x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec step(const vec& edges, const vec& xs) { + return map_join([](T edge, T x) { return step(edge, x); }, edges, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec smoothstep(T edge0, T edge1, const vec& xs) { + return map_join([edge0, edge1](T x) { return smoothstep(edge0, edge1, x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec smoothstep(const vec& edges0, const vec& edges1, const vec& xs) { + return map_join([](T edge0, T edge1, T x) { return smoothstep(edge0, edge1, x); }, edges0, edges1, xs); + } +} + +// +// Angle and Trigonometric Functions +// + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec radians(const vec& degrees) { + return map_join([](T x) { return radians(x); }, degrees); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec degrees(const vec& radians) { + return map_join([](T x) { return degrees(x); }, radians); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec sin(const vec& xs) { + return map_join([](T x) { return sin(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec cos(const vec& xs) { + return map_join([](T x) { return cos(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec tan(const vec& xs) { + return map_join([](T x) { return tan(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec asin(const vec& xs) { + return map_join([](T x) { return asin(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec acos(const vec& xs) { + return map_join([](T x) { return acos(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec atan(const vec& xs) { + return map_join([](T x) { return atan(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec atan2(const vec& ys, const vec& xs) { + return map_join([](T y, T x) { return atan2(y, x); }, ys, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec sinh(const vec& xs) { + return map_join([](T x) { return sinh(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec cosh(const vec& xs) { + return map_join([](T x) { return cosh(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec tanh(const vec& xs) { + return map_join([](T x) { return tanh(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec asinh(const vec& xs) { + return map_join([](T x) { return asinh(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec acosh(const vec& xs) { + return map_join([](T x) { return acosh(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec atanh(const vec& xs) { + return map_join([](T x) { return atanh(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr std::pair, vec> sincos(const vec& xs) { + return { sin(xs), cos(xs) }; + } + + template < typename T, std::size_t Size > + constexpr void sincos(const vec& xs, vec* ss, vec* cs) { + *ss = sin(xs); + *cs = cos(xs); + } +} + +// +// Exponential Functions +// + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec pow(const vec& xs, const vec& ys) { + return map_join([](T x, T y) { return pow(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec exp(const vec& xs) { + return map_join([](T x) { return exp(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec log(const vec& xs) { + return map_join([](T x) { return log(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec exp2(const vec& xs) { + return map_join([](T x) { return exp2(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec log2(const vec& xs) { + return map_join([](T x) { return log2(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec sqrt(const vec& xs) { + return map_join([](T x) { return sqrt(x); }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec rsqrt(const vec& xs) { + return map_join([](T x) { return rsqrt(x); }, xs); + } +} + +// +// Geometric Functions +// + +namespace vmath_hpp +{ + template < typename T, typename U, std::size_t Size + , typename V = decltype(std::declval() * std::declval()) > + [[nodiscard]] constexpr V dot(const vec& xs, const vec& ys) { + return fold1_plus_join([](T x, U y){ return x * y; }, xs, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T length(const vec& xs) { + return sqrt(dot(xs, xs)); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T rlength(const vec& xs) { + return rsqrt(dot(xs, xs)); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T length2(const vec& xs) { + return dot(xs, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T rlength2(const vec& xs) { + return rcp(dot(xs, xs)); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T distance(const vec& xs, const vec& ys) { + return length(ys - xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T distance2(const vec& xs, const vec& ys) { + return length2(ys - xs); + } + + template < typename T, typename U + , typename V = decltype(std::declval() * std::declval()) > + [[nodiscard]] constexpr V cross(const vec& xs, const vec& ys) { + return { xs.x * ys.y - xs.y * ys.x }; + } + + template < typename T, typename U + , typename V = decltype(std::declval() * std::declval()) > + [[nodiscard]] constexpr vec cross(const vec& xs, const vec& ys) { + return { + xs.y * ys.z - xs.z * ys.y, + xs.z * ys.x - xs.x * ys.z, + xs.x * ys.y - xs.y * ys.x}; + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec normalize(const vec& xs) { + return xs * rlength(xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec faceforward(const vec& n, const vec& i, const vec& nref) { + return dot(nref, i) < T{0} ? n : -n; + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec reflect(const vec& i, const vec& n) { + return i - T{2} * dot(n, i) * n; + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec refract(const vec& i, const vec& n, T eta) { + const T d = dot(n, i); + const T k = T{1} - sqr(eta) * (T{1} - sqr(d)); + return k < T{0} ? vec{T{0}} : (eta * i - (eta * d + sqrt(k)) * n); + } +} + +// +// Relational Functions +// + +namespace vmath_hpp +{ + template < typename T, std::size_t Size + , typename U = decltype(any(std::declval())) > + [[nodiscard]] constexpr U any(const vec& xs) { + return fold1_or_join([](T x){ return any(x); }, xs); + } + + template < typename T, std::size_t Size + , typename U = decltype(all(std::declval())) > + [[nodiscard]] constexpr U all(const vec& xs) { + return fold1_and_join([](T x){ return all(x); }, xs); + } + + template < typename T, std::size_t Size + , typename U = decltype(approx( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec approx(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return approx(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(approx( + std::declval(), + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec approx(const vec& xs, const vec& ys, T epsilon) { + return map_join([epsilon](T x, T y){ return approx(x, y, epsilon); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(less( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec less(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return less(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(less_equal( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec less_equal(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return less_equal(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(greater( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec greater(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return greater(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(greater_equal( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec greater_equal(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return greater_equal(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(equal_to( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec equal_to(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return equal_to(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = decltype(not_equal_to( + std::declval(), + std::declval())) > + [[nodiscard]] constexpr vec not_equal_to(const vec& xs, const vec& ys) { + return map_join([](T x, T y){ return not_equal_to(x, y); }, xs, ys); + } +} + +namespace vmath_hpp::detail +{ + template < typename T, std::size_t Size > + class mat_base; + + template < typename T > + class mat_base { + public: + using row_type = vec; + row_type rows[2]; + public: + constexpr mat_base() + : mat_base(identity_init) {} + + constexpr mat_base(uninit_t) {} + constexpr mat_base(zero_init_t): mat_base{zero_init, zero_init} {} + constexpr mat_base(unit_init_t): mat_base{unit_init, unit_init} {} + constexpr mat_base(identity_init_t): mat_base{T{1}} {} + + constexpr explicit mat_base(T d) + : rows{ + {d, T{0}}, + {T{0}, d}} {} + + constexpr explicit mat_base(const row_type& d) + : rows{ + {d[0], T{0}}, + {T{0}, d[1]}} {} + + constexpr mat_base( + T m11, T m12, + T m21, T m22) + : rows{ + {m11, m12}, + {m21, m22}} {} + + constexpr mat_base( + const row_type& row0, + const row_type& row1) + : rows{row0, row1} {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr mat_base(const mat_base& other): mat_base( + row_type{other.rows[0]}, + row_type{other.rows[1]}) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit mat_base(const mat_base& other): mat_base( + row_type{other.rows[0]}, + row_type{other.rows[1]}) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit mat_base(const mat_base& other): mat_base( + row_type{other.rows[0]}, + row_type{other.rows[1]}) {} + }; + + template < typename T > + class mat_base { + public: + using row_type = vec; + row_type rows[3]; + public: + constexpr mat_base() + : mat_base(identity_init) {} + + constexpr mat_base(uninit_t) {} + constexpr mat_base(zero_init_t): mat_base{zero_init, zero_init, zero_init} {} + constexpr mat_base(unit_init_t): mat_base{unit_init, unit_init, unit_init} {} + constexpr mat_base(identity_init_t): mat_base{T{1}} {} + + constexpr explicit mat_base(T d) + : rows{ + {d, T{0}, T{0}}, + {T{0}, d, T{0}}, + {T{0}, T{0}, d}} {} + + constexpr explicit mat_base(const row_type& d) + : rows{ + {d[0], T{0}, T{0}}, + {T{0}, d[1], T{0}}, + {T{0}, T{0}, d[2]}} {} + + constexpr mat_base( + T m11, T m12, T m13, + T m21, T m22, T m23, + T m31, T m32, T m33) + : rows{ + {m11, m12, m13}, + {m21, m22, m23}, + {m31, m32, m33}} {} + + constexpr mat_base( + const row_type& row0, + const row_type& row1, + const row_type& row2) + : rows{row0, row1, row2} {} + + constexpr mat_base( + const mat_base& m, + const vec_base& v) + : rows{ + {m.rows[0], T{0}}, + {m.rows[1], T{0}}, + {v, T{1}}} {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr mat_base(const mat_base& other): mat_base( + row_type{other.rows[0]}, + row_type{other.rows[1]}, + row_type{other.rows[2]}) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit mat_base(const mat_base& other): mat_base( + row_type{other.rows[0], T{0}}, + row_type{other.rows[1], T{0}}, + row_type{T{0}, T{0}, T{1}}) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit mat_base(const mat_base& other): mat_base( + row_type{other.rows[0]}, + row_type{other.rows[1]}, + row_type{other.rows[2]}) {} + }; + + template < typename T > + class mat_base { + public: + using row_type = vec; + row_type rows[4]; + public: + constexpr mat_base() + : mat_base(identity_init) {} + + constexpr mat_base(uninit_t) {} + constexpr mat_base(zero_init_t): mat_base{zero_init, zero_init, zero_init, zero_init} {} + constexpr mat_base(unit_init_t): mat_base{unit_init, unit_init, unit_init, unit_init} {} + constexpr mat_base(identity_init_t): mat_base{T{1}} {} + + constexpr explicit mat_base(T d) + : rows{ + {d, T{0}, T{0}, T{0}}, + {T{0}, d, T{0}, T{0}}, + {T{0}, T{0}, d, T{0}}, + {T{0}, T{0}, T{0}, d}} {} + + constexpr explicit mat_base(const row_type& d) + : rows{ + {d[0], T{0}, T{0}, T{0}}, + {T{0}, d[1], T{0}, T{0}}, + {T{0}, T{0}, d[2], T{0}}, + {T{0}, T{0}, T{0}, d[3]}} {} + + constexpr mat_base( + T m11, T m12, T m13, T m14, + T m21, T m22, T m23, T m24, + T m31, T m32, T m33, T m34, + T m41, T m42, T m43, T m44) + : rows{ + {m11, m12, m13, m14}, + {m21, m22, m23, m24}, + {m31, m32, m33, m34}, + {m41, m42, m43, m44}} {} + + constexpr mat_base( + const row_type& row0, + const row_type& row1, + const row_type& row2, + const row_type& row3) + : rows{row0, row1, row2, row3} {} + + constexpr mat_base( + const mat_base& m, + const vec_base& v) + : rows{ + {m.rows[0], T{0}}, + {m.rows[1], T{0}}, + {m.rows[2], T{0}}, + {v, T{1}}} {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr mat_base(const mat_base& other): mat_base( + row_type{other.rows[0]}, + row_type{other.rows[1]}, + row_type{other.rows[2]}, + row_type{other.rows[3]}) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit mat_base(const mat_base& other): mat_base( + row_type{other.rows[0], T{0}, T{0}}, + row_type{other.rows[1], T{0}, T{0}}, + row_type{T{0}, T{0}, T{1}, T{0}}, + row_type{T{0}, T{0}, T{0}, T{1}}) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit mat_base(const mat_base& other): mat_base( + row_type{other.rows[0], T{0}}, + row_type{other.rows[1], T{0}}, + row_type{other.rows[2], T{0}}, + row_type{T{0}, T{0}, T{0}, T{1}}) {} + }; +} + +namespace vmath_hpp +{ + template < typename T, std::size_t Size > + class mat final : public detail::mat_base { + public: + using self_type = mat; + using base_type = detail::mat_base; + using component_type = T; + + using row_type = vec; + + using pointer = row_type*; + using const_pointer = const row_type*; + + using reference = row_type&; + using const_reference = const row_type&; + + using iterator = pointer; + using const_iterator = const_pointer; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + static inline constexpr std::size_t size = Size; + public: + using base_type::mat_base; + using base_type::rows; + + void swap(mat& other) noexcept(std::is_nothrow_swappable_v) { + for ( std::size_t i = 0; i < Size; ++i ) { + using std::swap; + swap(rows[i], other.rows[i]); + } + } + + [[nodiscard]] iterator begin() noexcept { return iterator(data()); } + [[nodiscard]] const_iterator begin() const noexcept { return const_iterator(data()); } + [[nodiscard]] iterator end() noexcept { return iterator(data() + Size); } + [[nodiscard]] const_iterator end() const noexcept { return const_iterator(data() + Size); } + + [[nodiscard]] reverse_iterator rbegin() noexcept { return reverse_iterator(end()); } + [[nodiscard]] const_reverse_iterator rbegin() const noexcept { return const_reverse_iterator(end()); } + [[nodiscard]] reverse_iterator rend() noexcept { return reverse_iterator(begin()); } + [[nodiscard]] const_reverse_iterator rend() const noexcept { return const_reverse_iterator(begin()); } + + [[nodiscard]] const_iterator cbegin() const noexcept { return begin(); } + [[nodiscard]] const_iterator cend() const noexcept { return end(); } + [[nodiscard]] const_reverse_iterator crbegin() const noexcept { return rbegin(); } + [[nodiscard]] const_reverse_iterator crend() const noexcept { return rend(); } + + [[nodiscard]] pointer data() noexcept { + return &rows[0]; + } + + [[nodiscard]] const_pointer data() const noexcept { + return &rows[0]; + } + + [[nodiscard]] constexpr reference at(std::size_t index) { + VMATH_HPP_THROW_IF(index >= size, std::out_of_range("mat::at")); + return rows[index]; + } + + [[nodiscard]] constexpr const_reference at(std::size_t index) const { + VMATH_HPP_THROW_IF(index >= size, std::out_of_range("mat::at")); + return rows[index]; + } + + [[nodiscard]] constexpr reference operator[](std::size_t index) noexcept { + return rows[index]; + } + + [[nodiscard]] constexpr const_reference operator[](std::size_t index) const noexcept { + return rows[index]; + } + }; +} + +namespace vmath_hpp +{ + // mat2 + + template < typename T > + mat(T, T, T, T) -> mat; + + template < typename T > + mat(const vec&, const vec&) -> mat; + + template < typename T > + mat(std::initializer_list, std::initializer_list) -> mat; + + // mat3 + + template < typename T > + mat(T, T, T, T, T, T, T, T, T) -> mat; + + template < typename T > + mat(const vec&, const vec&, const vec&) -> mat; + + template < typename T > + mat(const mat&, const vec&) -> mat; + + template < typename T > + mat(std::initializer_list, std::initializer_list, std::initializer_list) -> mat; + + // mat4 + + template < typename T > + mat(T, T, T, T, T, T, T, T, T, T, T, T, T, T, T, T) -> mat; + + template < typename T > + mat(const vec&, const vec&, const vec&, const vec&) -> mat; + + template < typename T > + mat(const mat&, const vec&) -> mat; + + template < typename T > + mat(std::initializer_list, std::initializer_list, std::initializer_list, std::initializer_list) -> mat; + + // swap + + template < typename T, std::size_t Size > + void swap(mat& l, mat& r) noexcept(noexcept(l.swap(r))) { + l.swap(r); + } +} + +namespace vmath_hpp::detail::impl +{ + template < typename A, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join_impl( + F&& f, + const mat& a, + std::index_sequence) + { + return mat{ f(a[Is])... }; + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join_impl( + F&& f, + const mat& a, + const mat& b, + std::index_sequence) + { + return mat{ f(a[Is], b[Is])... }; + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold_join_impl( + F&& f, + A init, + const mat& b, + std::index_sequence) + { + return ((init = f(std::move(init), b[Is])), ...); + } + + template < typename A, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join_impl( + F&& f, + const mat& a, + std::index_sequence) + { + return (... && f(a[Is])); + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join_impl( + F&& f, + const mat& a, + const mat& b, + std::index_sequence) + { + return (... && f(a[Is], b[Is])); + } + + template < typename A, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join_impl( + F&& f, + const mat& a, + std::index_sequence) + { + return (... || f(a[Is])); + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join_impl( + F&& f, + const mat& a, + const mat& b, + std::index_sequence) + { + return (... || f(a[Is], b[Is])); + } + + template < typename A, typename B, std::size_t Size, typename F, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_plus_join_impl( + F&& f, + const vec& a, + const mat& b, + std::index_sequence) + { + return (... + f(a[Is], b[Is])); + } +} + +namespace vmath_hpp::detail +{ + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const mat& a) { + return impl::map_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const mat& a, const mat& b) { + return impl::map_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold_join(F&& f, A init, const mat& b) { + return impl::fold_join_impl(std::forward(f), std::move(init), b, std::make_index_sequence{}); + } + + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join(F&& f, const mat& a) { + return impl::fold1_and_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_and_join(F&& f, const mat& a, const mat& b) { + return impl::fold1_and_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } + + template < typename A, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join(F&& f, const mat& a) { + return impl::fold1_or_join_impl(std::forward(f), a, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_or_join(F&& f, const mat& a, const mat& b) { + return impl::fold1_or_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } + + template < typename A, typename B, std::size_t Size, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold1_plus_join(F&& f, const vec& a, const mat& b) { + return impl::fold1_plus_join_impl(std::forward(f), a, b, std::make_index_sequence{}); + } +} + +// +// Operators +// + +namespace vmath_hpp +{ + // +operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator+(const mat& xs) { + return map_join([](const vec& x){ return +x; }, xs); + } + + // -operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator-(const mat& xs) { + return map_join([](const vec& x){ return -x; }, xs); + } + + // ~operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator~(const mat& xs) { + return map_join([](const vec& x){ return ~x; }, xs); + } + + // !operator + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator!(const mat& xs) { + return map_join([](const vec& x){ return !x; }, xs); + } + + // ++operator + + template < typename T, std::size_t Size > + constexpr mat& operator++(mat& xs) { + return (xs = xs + T{1}); + } + + // --operator + + template < typename T, std::size_t Size > + constexpr mat& operator--(mat& xs) { + return (xs = xs - T{1}); + } + + // operator++ + + template < typename T, std::size_t Size > + constexpr mat operator++(mat& xs, int) { + mat ys = xs; + ++xs; + return ys; + } + + // operator-- + + template < typename T, std::size_t Size > + constexpr mat operator--(mat& xs, int) { + mat ys = xs; + --xs; + return ys; + } + + // operator+ + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator+(const mat& xs, U y) { + return map_join([y](const vec& x){ return x + y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator+(T x, const mat& ys) { + return map_join([x](const vec& y){ return x + y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator+(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x + y; }, xs, ys); + } + + // operator+= + + template < typename T, std::size_t Size > + constexpr mat& operator+=(mat& xs, T y) { + return (xs = (xs + y)); + } + + template < typename T, std::size_t Size > + constexpr mat& operator+=(mat& xs, const mat& ys) { + return (xs = (xs + ys)); + } + + // operator- + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator-(const mat& xs, U y) { + return map_join([y](const vec& x){ return x - y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator-(T x, const mat& ys) { + return map_join([x](const vec& y){ return x - y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator-(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x - y; }, xs, ys); + } + + // operator-= + + template < typename T, std::size_t Size > + constexpr mat& operator-=(mat& xs, T y) { + return (xs = (xs - y)); + } + + template < typename T, std::size_t Size > + constexpr mat& operator-=(mat& xs, const mat& ys) { + return (xs = (xs - ys)); + } + + // operator* + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(const mat& xs, U y) { + return map_join([y](const vec& x){ return x * y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(T x, const mat& ys) { + return map_join([x](const vec& y){ return x * y; }, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(const vec& xs, const mat& ys) { + return fold1_plus_join([](T x, const vec& y){ return x * y; }, xs, ys); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator*(const mat& xs, const mat& ys) { + return map_join([&ys](const vec& x){ return x * ys; }, xs); + } + + // operator*= + + template < typename T, std::size_t Size > + constexpr mat& operator*=(mat& xs, T y) { + return (xs = (xs * y)); + } + + template < typename T, std::size_t Size > + constexpr vec& operator*=(vec& xs, const mat& ys) { + return (xs = (xs * ys)); + } + + template < typename T, std::size_t Size > + constexpr mat& operator*=(mat& xs, const mat& ys) { + return (xs = (xs * ys)); + } + + // operator/ + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator/(const mat& xs, U y) { + return map_join([y](const vec& x){ return x / y; }, xs); + } + + template < typename T, typename U, std::size_t Size > + [[nodiscard]] constexpr auto operator/(T x, const mat& ys) { + return map_join([x](const vec& y){ return x / y; }, ys); + } + + // operator/= + + template < typename T, std::size_t Size > + constexpr mat& operator/=(mat& xs, T y) { + return (xs = (xs / y)); + } + + // operator& + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&(const mat& xs, T y) { + return map_join([y](const vec& x){ return x & y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&(T x, const mat& ys) { + return map_join([x](const vec& y){ return x & y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x & y; }, xs, ys); + } + + // operator&= + + template < typename T, std::size_t Size > + constexpr mat& operator&=(mat& xs, T y) { + return (xs = (xs & y)); + } + + template < typename T, std::size_t Size > + constexpr mat& operator&=(mat& xs, const mat& ys) { + return (xs = (xs & ys)); + } + + // operator| + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator|(const mat& xs, T y) { + return map_join([y](const vec& x){ return x | y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator|(T x, const mat& ys) { + return map_join([x](const vec& y){ return x | y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator|(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x | y; }, xs, ys); + } + + // operator|= + + template < typename T, std::size_t Size > + constexpr mat& operator|=(mat& xs, T y) { + return (xs = (xs | y)); + } + + template < typename T, std::size_t Size > + constexpr mat& operator|=(mat& xs, const mat& ys) { + return (xs = (xs | ys)); + } + + // operator^ + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator^(const mat& xs, T y) { + return map_join([y](const vec& x){ return x ^ y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator^(T x, const mat& ys) { + return map_join([x](const vec& y){ return x ^ y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator^(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x ^ y; }, xs, ys); + } + + // operator^= + + template < typename T, std::size_t Size > + constexpr mat& operator^=(mat& xs, T y) { + return (xs = (xs ^ y)); + } + + template < typename T, std::size_t Size > + constexpr mat& operator^=(mat& xs, const mat& ys) { + return (xs = (xs ^ ys)); + } + + // operator&& + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&&(const mat& xs, T y) { + return map_join([y](const vec& x){ return x && y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&&(T x, const mat& ys) { + return map_join([x](const vec& y){ return x && y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator&&(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x && y; }, xs, ys); + } + + // operator|| + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator||(const mat& xs, T y) { + return map_join([y](const vec& x){ return x || y; }, xs); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator||(T x, const mat& ys) { + return map_join([x](const vec& y){ return x || y; }, ys); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr auto operator||(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return x || y; }, xs, ys); + } + + // operator== + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr bool operator==(const mat& xs, const mat& ys) { + return fold1_and_join([](const vec& x, const vec& y){ return x == y; }, xs, ys); + } + + // operator!= + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr bool operator!=(const mat& xs, const mat& ys) { + return fold1_or_join([](const vec& x, const vec& y){ return x != y; }, xs, ys); + } + + // operator< + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr bool operator<(const mat& xs, const mat& ys) { + for ( std::size_t i = 0; i < Size; ++i ) { + if ( xs[i] < ys[i] ) { + return true; + } + if ( ys[i] < xs[i] ) { + return false; + } + } + return false; + } +} + +// +// Relational Functions +// + +namespace vmath_hpp +{ + template < typename T, std::size_t Size + , typename U = decltype(any(std::declval>())) > + [[nodiscard]] constexpr U any(const mat& xs) { + return fold1_or_join([](const vec& x){ return any(x); }, xs); + } + + template < typename T, std::size_t Size + , typename U = decltype(all(std::declval>())) > + [[nodiscard]] constexpr U all(const mat& xs) { + return fold1_and_join([](const vec& x){ return all(x); }, xs); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(approx( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat approx(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return approx(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(approx( + std::declval>(), + std::declval>(), + std::declval()))::component_type > + [[nodiscard]] constexpr mat approx(const mat& xs, const mat& ys, T epsilon) { + return map_join([epsilon](const vec& x, const vec& y){ return approx(x, y, epsilon); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(less( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat less(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return less(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(less_equal( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat less_equal(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return less_equal(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(greater( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat greater(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return greater(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(greater_equal( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat greater_equal(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return greater_equal(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(equal_to( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat equal_to(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return equal_to(x, y); }, xs, ys); + } + + template < typename T, std::size_t Size + , typename U = typename decltype(not_equal_to( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr mat not_equal_to(const mat& xs, const mat& ys) { + return map_join([](const vec& x, const vec& y){ return not_equal_to(x, y); }, xs, ys); + } +} + +// +// Matrix Functions +// + +namespace vmath_hpp +{ + namespace impl + { + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat transpose_2x2_impl( + T a, T c, + T b, T d) + { + return { + a, b, + c, d}; + } + + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat transpose_3x3_impl( + T a, T d, T g, + T b, T e, T h, + T c, T f, T i) + { + return { + a, b, c, + d, e, f, + g, h, i}; + } + + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat transpose_4x4_impl( + T a, T e, T i, T m, + T b, T f, T j, T n, + T c, T g, T k, T o, + T d, T h, T l, T p) + { + return { + a, b, c, d, + e, f, g, h, + i, j, k, l, + m, n, o, p}; + } + } + + template < typename T > + [[nodiscard]] constexpr mat transpose(const mat& m) { + return impl::transpose_2x2_impl( + m[0][0], m[0][1], + m[1][0], m[1][1]); + } + + template < typename T > + [[nodiscard]] constexpr mat transpose(const mat& m) { + return impl::transpose_3x3_impl( + m[0][0], m[0][1], m[0][2], + m[1][0], m[1][1], m[1][2], + m[2][0], m[2][1], m[2][2]); + } + + template < typename T > + [[nodiscard]] constexpr mat transpose(const mat& m) { + return impl::transpose_4x4_impl( + m[0][0], m[0][1], m[0][2], m[0][3], + m[1][0], m[1][1], m[1][2], m[1][3], + m[2][0], m[2][1], m[2][2], m[2][3], + m[3][0], m[3][1], m[3][2], m[3][3]); + } + + namespace impl + { + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + T determinant_2x2_impl( + T a, T b, + T c, T d) + { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/twoD/ + + return + + a * d + - b * c; + } + + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + T determinant_3x3_impl( + T a, T b, T c, + T d, T e, T f, + T g, T h, T i) + { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/threeD/ + + return + + a * determinant_2x2_impl(e, f, h, i) + - b * determinant_2x2_impl(d, f, g, i) + + c * determinant_2x2_impl(d, e, g, h); + + } + + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + T determinant_4x4_impl( + T a, T b, T c, T d, + T e, T f, T g, T h, + T i, T j, T k, T l, + T m, T n, T o, T p) + { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/fourD/ + + return + + a * determinant_3x3_impl(f, g, h, j, k, l, n, o, p) + - b * determinant_3x3_impl(e, g, h, i, k, l, m, o, p) + + c * determinant_3x3_impl(e, f, h, i, j, l, m, n, p) + - d * determinant_3x3_impl(e, f, g, i, j, k, m, n, o); + } + } + + template < typename T > + [[nodiscard]] constexpr T determinant(const mat& m) { + return impl::determinant_2x2_impl( + m[0][0], m[0][1], + m[1][0], m[1][1]); + } + + template < typename T > + [[nodiscard]] constexpr T determinant(const mat& m) { + return impl::determinant_3x3_impl( + m[0][0], m[0][1], m[0][2], + m[1][0], m[1][1], m[1][2], + m[2][0], m[2][1], m[2][2]); + } + + template < typename T > + [[nodiscard]] constexpr T determinant(const mat& m) { + return impl::determinant_4x4_impl( + m[0][0], m[0][1], m[0][2], m[0][3], + m[1][0], m[1][1], m[1][2], m[1][3], + m[2][0], m[2][1], m[2][2], m[2][3], + m[3][0], m[3][1], m[3][2], m[3][3]); + } + + namespace impl + { + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat inverse_2x2_impl( + T a, T b, + T c, T d) + { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/twoD/ + + const T inv_det = rcp(determinant_2x2_impl( + a, b, + c, d)); + + const mat inv_m{ + d, -b, + -c, a}; + + return inv_m * inv_det; + } + + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat inverse_3x3_impl( + T a, T b, T c, + T d, T e, T f, + T g, T h, T i) + { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/threeD/ + + const T inv_det = rcp(determinant_3x3_impl( + a, b, c, + d, e, f, + g, h, i)); + + const mat inv_m{ + e * i - f * h, + c * h - b * i, + b * f - c * e, + f * g - d * i, + a * i - c * g, + c * d - a * f, + d * h - e * g, + b * g - a * h, + a * e - b * d}; + + return inv_m * inv_det; + } + + template < typename T > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat inverse_4x4_impl( + T a, T b, T c, T d, + T e, T f, T g, T h, + T i, T j, T k, T l, + T m, T n, T o, T p) + { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/ + + const T inv_det = rcp(determinant_4x4_impl( + a, b, c, d, + e, f, g, h, + i, j, k, l, + m, n, o, p)); + + const mat inv_m{ + (f * (k * p - l * o) + g * (l * n - j * p) + h * (j * o - k * n)), + (j * (c * p - d * o) + k * (d * n - b * p) + l * (b * o - c * n)), + (n * (c * h - d * g) + o * (d * f - b * h) + p * (b * g - c * f)), + (b * (h * k - g * l) + c * (f * l - h * j) + d * (g * j - f * k)), + (g * (i * p - l * m) + h * (k * m - i * o) + e * (l * o - k * p)), + (k * (a * p - d * m) + l * (c * m - a * o) + i * (d * o - c * p)), + (o * (a * h - d * e) + p * (c * e - a * g) + m * (d * g - c * h)), + (c * (h * i - e * l) + d * (e * k - g * i) + a * (g * l - h * k)), + (h * (i * n - j * m) + e * (j * p - l * n) + f * (l * m - i * p)), + (l * (a * n - b * m) + i * (b * p - d * n) + j * (d * m - a * p)), + (p * (a * f - b * e) + m * (b * h - d * f) + n * (d * e - a * h)), + (d * (f * i - e * j) + a * (h * j - f * l) + b * (e * l - h * i)), + (e * (k * n - j * o) + f * (i * o - k * m) + g * (j * m - i * n)), + (i * (c * n - b * o) + j * (a * o - c * m) + k * (b * m - a * n)), + (m * (c * f - b * g) + n * (a * g - c * e) + o * (b * e - a * f)), + (a * (f * k - g * j) + b * (g * i - e * k) + c * (e * j - f * i))}; + + return inv_m * inv_det; + } + } + + template < typename T > + [[nodiscard]] constexpr mat inverse(const mat& m) { + return impl::inverse_2x2_impl( + m[0][0], m[0][1], + m[1][0], m[1][1]); + } + + template < typename T > + [[nodiscard]] constexpr mat inverse(const mat& m) { + return impl::inverse_3x3_impl( + m[0][0], m[0][1], m[0][2], + m[1][0], m[1][1], m[1][2], + m[2][0], m[2][1], m[2][2]); + } + + template < typename T > + [[nodiscard]] constexpr mat inverse(const mat& m) { + return impl::inverse_4x4_impl( + m[0][0], m[0][1], m[0][2], m[0][3], + m[1][0], m[1][1], m[1][2], m[1][3], + m[2][0], m[2][1], m[2][2], m[2][3], + m[3][0], m[3][1], m[3][2], m[3][3]); + } +} + +namespace vmath_hpp::detail +{ + template < typename T > + class qua_base { + public: + vec v; + T s; + public: + constexpr qua_base() + : qua_base(identity_init) {} + + constexpr qua_base(uninit_t) {} + constexpr qua_base(zero_init_t) : qua_base{zero_init, T{0}} {} + constexpr qua_base(identity_init_t) : qua_base{zero_init, T{1}} {} + + constexpr qua_base(T vx, T vy, T vz, T s): v{vx, vy, vz}, s{s} {} + constexpr qua_base(const vec& v, T s): v{v}, s{s} {} + constexpr explicit qua_base(const vec& vs): v{vs[0], vs[1], vs[2]}, s{vs[3]} {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr qua_base(const qua_base& other): qua_base(other.v, other.s) {} + + template < typename U, std::enable_if_t, int> = 0 > + constexpr explicit operator vec() const { return vec(v, s); } + + [[nodiscard]] constexpr T& operator[](std::size_t index) noexcept { + switch ( index ) { + default: + case 0: return v.x; + case 1: return v.y; + case 2: return v.z; + case 3: return s; + } + } + + [[nodiscard]] constexpr const T& operator[](std::size_t index) const noexcept { + switch ( index ) { + default: + case 0: return v.x; + case 1: return v.y; + case 2: return v.z; + case 3: return s; + } + } + }; +} + +namespace vmath_hpp +{ + template < typename T > + class qua final : public detail::qua_base { + public: + using self_type = qua; + using base_type = detail::qua_base; + using component_type = T; + + using imag_type = vec; + using real_type = T; + + using pointer = component_type*; + using const_pointer = const component_type*; + + using reference = component_type&; + using const_reference = const component_type&; + + using iterator = pointer; + using const_iterator = const_pointer; + using reverse_iterator = std::reverse_iterator; + using const_reverse_iterator = std::reverse_iterator; + + static inline constexpr std::size_t size = 4; + public: + using base_type::qua_base; + using base_type::operator[]; + + void swap(qua& other) noexcept(std::is_nothrow_swappable_v) { + for ( std::size_t i = 0; i < size; ++i ) { + using std::swap; + swap((*this)[i], other[i]); + } + } + + [[nodiscard]] iterator begin() noexcept { return iterator(data()); } + [[nodiscard]] const_iterator begin() const noexcept { return const_iterator(data()); } + [[nodiscard]] iterator end() noexcept { return iterator(data() + size); } + [[nodiscard]] const_iterator end() const noexcept { return const_iterator(data() + size); } + + [[nodiscard]] reverse_iterator rbegin() noexcept { return reverse_iterator(end()); } + [[nodiscard]] const_reverse_iterator rbegin() const noexcept { return const_reverse_iterator(end()); } + [[nodiscard]] reverse_iterator rend() noexcept { return reverse_iterator(begin()); } + [[nodiscard]] const_reverse_iterator rend() const noexcept { return const_reverse_iterator(begin()); } + + [[nodiscard]] const_iterator cbegin() const noexcept { return begin(); } + [[nodiscard]] const_iterator cend() const noexcept { return end(); } + [[nodiscard]] const_reverse_iterator crbegin() const noexcept { return rbegin(); } + [[nodiscard]] const_reverse_iterator crend() const noexcept { return rend(); } + + [[nodiscard]] pointer data() noexcept { + return &(*this)[0]; + } + + [[nodiscard]] const_pointer data() const noexcept { + return &(*this)[0]; + } + + [[nodiscard]] constexpr reference at(std::size_t index) { + VMATH_HPP_THROW_IF(index >= size, std::out_of_range("qua::at")); + return (*this)[index]; + } + + [[nodiscard]] constexpr const_reference at(std::size_t index) const { + VMATH_HPP_THROW_IF(index >= size, std::out_of_range("qua::at")); + return (*this)[index]; + } + }; +} + +namespace vmath_hpp +{ + // vec + + template < typename T > + vec(const qua&) -> vec; + + // qua + + template < typename T > + qua(T, T, T, T) -> qua; + + template < typename T > + qua(const vec&, T) -> qua; + + template < typename T > + qua(const vec&) -> qua; + + template < typename T > + qua(std::initializer_list) -> qua; + + // swap + + template < typename T > + void swap(qua& l, qua& r) noexcept(noexcept(l.swap(r))) { + l.swap(r); + } +} + +namespace vmath_hpp::detail +{ + template < typename A, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto map_join(F&& f, const qua& a) { + return qua(map_join(std::forward(f), vec{a})); + } + + template < typename A, typename B, typename F > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + auto fold_join(F&& f, A init, const qua& b) { + return fold_join(std::forward(f), std::move(init), vec{b}); + } +} + +// +// Operators +// + +namespace vmath_hpp +{ + // +operator + + template < typename T > + [[nodiscard]] constexpr auto operator+(const qua& xs) { + return qua(+vec{xs}); + } + + // -operator + + template < typename T > + [[nodiscard]] constexpr auto operator-(const qua& xs) { + return qua(-vec{xs}); + } + + // operator+ + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator+(const qua& xs, const qua& ys) { + return qua(vec{xs} + vec{ys}); + } + + // operator+= + + template < typename T > + constexpr qua& operator+=(qua& xs, const qua& ys) { + return (xs = (xs + ys)); + } + + // operator- + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator-(const qua& xs, const qua& ys) { + return qua(vec{xs} - vec{ys}); + } + + // operator-= + + template < typename T > + constexpr qua& operator-=(qua& xs, const qua& ys) { + return (xs = (xs - ys)); + } + + // operator* + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator*(const qua& xs, U y) { + return qua(vec{xs} * y); + } + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator*(T x, const qua& ys) { + return qua(x * vec{ys}); + } + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator*(const vec& xs, const qua& ys) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/ + + const vec qv2 = cross(ys.v, xs) * T{2}; + return xs + qv2 * ys.s + cross(ys.v, qv2); + } + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator*(const qua& xs, const qua& ys) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/ + + return qua{ + cross(ys.v, xs.v) + ys.s * xs.v + xs.s * ys.v, + ys.s * xs.s - dot(ys.v, xs.v)}; + } + + // operator*= + + template < typename T > + constexpr qua& operator*=(qua& xs, T y) { + return (xs = (xs * y)); + } + + template < typename T > + constexpr vec& operator*=(vec& xs, const qua& ys) { + return (xs = (xs * ys)); + } + + template < typename T > + constexpr qua& operator*=(qua& xs, const qua& ys) { + return (xs = (xs * ys)); + } + + // operator/ + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator/(const qua& xs, U y) { + return qua(vec{xs} / y); + } + + template < typename T, typename U > + [[nodiscard]] constexpr auto operator/(T x, const qua& ys) { + return qua(x / vec{ys}); + } + + // operator/= + + template < typename T > + constexpr qua& operator/=(qua& xs, T y) { + return (xs = (xs / y)); + } + + // operator== + + template < typename T > + [[nodiscard]] constexpr bool operator==(const qua& xs, const qua& ys) { + return vec{xs} == vec{ys}; + } + + // operator!= + + template < typename T > + [[nodiscard]] constexpr bool operator!=(const qua& xs, const qua& ys) { + return vec{xs} != vec{ys}; + } + + // operator< + + template < typename T > + [[nodiscard]] constexpr bool operator<(const qua& xs, const qua& ys) { + return vec{xs} < vec{ys}; + } +} + +// +// Common Functions +// + +namespace vmath_hpp +{ + template < typename T > + [[nodiscard]] constexpr qua lerp(const qua& xs, const qua& ys, T a) { + return qua(lerp(vec{xs}, vec{ys}, a)); + } + + template < typename T > + [[nodiscard]] constexpr qua lerp(const qua& xs, const qua& ys, T x_a, T y_a) { + return qua(lerp(vec{xs}, vec{ys}, x_a, y_a)); + } + + template < typename T > + [[nodiscard]] constexpr qua nlerp(const qua& unit_xs, const qua& unit_ys, T a) { + const T xs_scale = T{1} - a; + const T ys_scale = a * sign(dot(unit_xs, unit_ys)); + return normalize(lerp(unit_xs, unit_ys, xs_scale, ys_scale)); + } + + template < typename T > + [[nodiscard]] constexpr qua slerp(const qua& unit_xs, const qua& unit_ys, T a) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/ + + const T raw_cos_theta = dot(unit_xs, unit_ys); + const T raw_cos_theta_sign = sign(raw_cos_theta); + + // half degree linear threshold: cos((pi / 180) * 0.25) + if ( const T cos_theta = raw_cos_theta * raw_cos_theta_sign; cos_theta < T{0.99999f} ) { + const T theta = acos(cos_theta); + const T rsin_theta = rsqrt(T{1} - sqr(cos_theta)); + const T xs_scale = sin((T{1} - a) * theta) * rsin_theta; + const T ys_scale = sin(a * theta) * raw_cos_theta_sign * rsin_theta; + return lerp(unit_xs, unit_ys, xs_scale, ys_scale); + } else { + // use linear interpolation for small angles + const T xs_scale = T{1} - a; + const T ys_scale = a * raw_cos_theta_sign; + return normalize(lerp(unit_xs, unit_ys, xs_scale, ys_scale)); + } + } +} + +// +// Geometric Functions +// + +namespace vmath_hpp +{ + template < typename T, typename U + , typename V = decltype(dot( + std::declval>(), + std::declval>())) > + [[nodiscard]] constexpr V dot(const qua& xs, const qua& ys) { + return dot(vec{xs}, vec{ys}); + } + + template < typename T > + [[nodiscard]] constexpr T length(const qua& xs) { + return length(vec{xs}); + } + + template < typename T > + [[nodiscard]] constexpr T rlength(const qua& xs) { + return rlength(vec{xs}); + } + + template < typename T > + [[nodiscard]] constexpr T length2(const qua& xs) { + return length2(vec{xs}); + } + + template < typename T > + [[nodiscard]] constexpr T rlength2(const qua& xs) { + return rlength2(vec{xs}); + } + + template < typename T > + [[nodiscard]] constexpr T distance(const qua& xs, const qua& ys) { + const qua zs = xs * conjugate(ys); + return T{2} * atan2(length(zs.v), abs(zs.s)); + } + + template < typename T > + [[nodiscard]] constexpr qua normalize(const qua& xs) { + return qua(normalize(vec{xs})); + } +} + +// +// Relational Functions +// + +namespace vmath_hpp +{ + template < typename T + , typename U = decltype(any(std::declval>())) > + [[nodiscard]] constexpr U any(const qua& xs) { + return any(vec{xs}); + } + + template < typename T + , typename U = decltype(all(std::declval>())) > + [[nodiscard]] constexpr U all(const qua& xs) { + return all(vec{xs}); + } + + template < typename T + , typename U = typename decltype(approx( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec approx(const qua& xs, const qua& ys) { + return approx(vec{xs}, vec{ys}); + } + + template < typename T + , typename U = typename decltype(approx( + std::declval>(), + std::declval>(), + std::declval()))::component_type > + [[nodiscard]] constexpr vec approx(const qua& xs, const qua& ys, T epsilon) { + return approx(vec{xs}, vec{ys}, epsilon); + } + + template < typename T + , typename U = typename decltype(less( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec less(const qua& xs, const qua& ys) { + return less(vec{xs}, vec{ys}); + } + + template < typename T + , typename U = typename decltype(less_equal( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec less_equal(const qua& xs, const qua& ys) { + return less_equal(vec{xs}, vec{ys}); + } + + template < typename T + , typename U = typename decltype(greater( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec greater(const qua& xs, const qua& ys) { + return greater(vec{xs}, vec{ys}); + } + + template < typename T + , typename U = typename decltype(greater_equal( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec greater_equal(const qua& xs, const qua& ys) { + return greater_equal(vec{xs}, vec{ys}); + } + + template < typename T + , typename U = typename decltype(equal_to( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec equal_to(const qua& xs, const qua& ys) { + return equal_to(vec{xs}, vec{ys}); + } + + template < typename T + , typename U = typename decltype(not_equal_to( + std::declval>(), + std::declval>()))::component_type > + [[nodiscard]] constexpr vec not_equal_to(const qua& xs, const qua& ys) { + return not_equal_to(vec{xs}, vec{ys}); + } +} + +// +// Quaternion Functions +// + +namespace vmath_hpp +{ + // conjugate + + template < typename T > + [[nodiscard]] constexpr qua conjugate(const qua& q) { + return {-q.v, q.s}; + } + + // inverse + + template < typename T > + [[nodiscard]] constexpr qua inverse(const qua& q) { + return conjugate(q) * rlength2(q); + } +} + +// +// Units +// + +namespace vmath_hpp +{ + template < typename T > inline constexpr vec zero2{zero_init}; + template < typename T > inline constexpr vec zero3{zero_init}; + template < typename T > inline constexpr vec zero4{zero_init}; + + template < typename T > inline constexpr vec unit2{unit_init}; + template < typename T > inline constexpr vec unit3{unit_init}; + template < typename T > inline constexpr vec unit4{unit_init}; + + template < typename T > inline constexpr vec unit2_x{T{1}, T{0}}; + template < typename T > inline constexpr vec unit2_y{T{0}, T{1}}; + + template < typename T > inline constexpr vec unit3_x{T{1}, T{0}, T{0}}; + template < typename T > inline constexpr vec unit3_y{T{0}, T{1}, T{0}}; + template < typename T > inline constexpr vec unit3_z{T{0}, T{0}, T{1}}; + + template < typename T > inline constexpr vec unit4_x{T{1}, T{0}, T{0}, T{0}}; + template < typename T > inline constexpr vec unit4_y{T{0}, T{1}, T{0}, T{0}}; + template < typename T > inline constexpr vec unit4_z{T{0}, T{0}, T{1}, T{0}}; + template < typename T > inline constexpr vec unit4_w{T{0}, T{0}, T{0}, T{1}}; + + template < typename T > inline constexpr mat mzero2{zero_init}; + template < typename T > inline constexpr mat mzero3{zero_init}; + template < typename T > inline constexpr mat mzero4{zero_init}; + + template < typename T > inline constexpr mat munit2{unit_init}; + template < typename T > inline constexpr mat munit3{unit_init}; + template < typename T > inline constexpr mat munit4{unit_init}; + + template < typename T > inline constexpr mat midentity2{identity_init}; + template < typename T > inline constexpr mat midentity3{identity_init}; + template < typename T > inline constexpr mat midentity4{identity_init}; + + template < typename T > inline constexpr qua qzero{zero_init}; + template < typename T > inline constexpr qua qidentity{identity_init}; +} + +// +// Hash +// + +namespace vmath_hpp::detail +{ + struct hash_combiner { + template < typename T > + [[nodiscard]] std::size_t operator()(std::size_t seed, const T& x) noexcept { + return (seed ^= std::hash{}(x) + 0x9e3779b9 + (seed << 6) + ( seed >> 2)); + } + }; + + template < typename T, size_t Size > + [[nodiscard]] std::size_t hash(const vec& v) noexcept { + return fold_join(hash_combiner{}, std::size_t{}, v); + } + + template < typename T, size_t Size > + [[nodiscard]] std::size_t hash(const mat& m) noexcept { + return fold_join(hash_combiner{}, std::size_t{}, m); + } + + template < typename T > + [[nodiscard]] std::size_t hash(const qua& q) noexcept { + return fold_join(hash_combiner{}, std::size_t{}, q); + } +} + +namespace std +{ + template < typename T, size_t Size > + struct hash> { + size_t operator()(const vmath_hpp::vec& v) const noexcept { + return vmath_hpp::detail::hash(v); + } + }; + + template < typename T, size_t Size > + struct hash> { + size_t operator()(const vmath_hpp::mat& m) const noexcept { + return vmath_hpp::detail::hash(m); + } + }; + + template < typename T > + struct hash> { + size_t operator()(const vmath_hpp::qua& q) const noexcept { + return vmath_hpp::detail::hash(q); + } + }; +} + +// +// Cast +// + +namespace vmath_hpp +{ + template < typename To, typename From > + [[nodiscard]] std::enable_if_t< + std::is_arithmetic_v && std::is_arithmetic_v + , To> + constexpr cast_to(From x) noexcept { + return static_cast(x); + } + + template < typename To, typename From, std::size_t Size > + [[nodiscard]] constexpr vec cast_to(const vec& v) { + return map_join([](From x){ return cast_to(x); }, v); + } + + template < typename To, typename From, std::size_t Size > + [[nodiscard]] constexpr mat cast_to(const mat& m) { + return map_join([](const vec& v){ return cast_to(v); }, m); + } + + template < typename To, typename From > + [[nodiscard]] constexpr qua cast_to(const qua& q) { + return map_join([](From x){ return cast_to(x); }, q); + } +} + +// +// Access +// + +namespace vmath_hpp +{ + // component + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T component(const vec& v, std::size_t index) { + return v[index]; + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec component(vec v, std::size_t index, T component) { + v[index] = component; + return v; + } + + // row + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec row(const mat& m, std::size_t index) { + return m.rows[index]; + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr mat row(mat m, std::size_t index, const vec& row) { + m.rows[index] = row; + return m; + } + + // column + + namespace impl + { + template < typename T, std::size_t Size, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + vec column_impl(const mat& m, std::size_t index, std::index_sequence) { + return { m[Is][index]... }; + } + + template < typename T, std::size_t Size, std::size_t... Is > + [[nodiscard]] constexpr VMATH_HPP_FORCE_INLINE + mat column_impl(const mat& m, std::size_t index, const vec& column, std::index_sequence) { + return { component(m[Is], index, column[Is])... }; + } + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec column(const mat& m, std::size_t index) { + return impl::column_impl(m, index, std::make_index_sequence{}); + } + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr mat column(const mat& m, std::size_t index, const vec& column) { + return impl::column_impl(m, index, column, std::make_index_sequence{}); + } + + // real + + template < typename T > + [[nodiscard]] constexpr T real(const qua& q) { + return q.s; + } + + template < typename T > + [[nodiscard]] constexpr qua real(qua q, T real) { + q.s = real; + return q; + } + + // imag + + template < typename T > + [[nodiscard]] constexpr vec imag(const qua& q) { + return q.v; + } + + template < typename T > + [[nodiscard]] constexpr qua imag(qua q, const vec& imag) { + q.v = imag; + return q; + } +} + +// +// Matrix Transform 3D +// + +namespace vmath_hpp +{ + // trs + + template < typename T > + [[nodiscard]] constexpr mat trs(const vec& t, const mat& r) { + return { + { r[0], T{0} }, + { r[1], T{0} }, + { r[2], T{0} }, + { t, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat trs(const vec& t, const mat& r, const vec& s) { + return { + { r[0] * s[0], T{0} }, + { r[1] * s[1], T{0} }, + { r[2] * s[2], T{0} }, + { t, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat trs(const vec& t, const qua& r) { + return trs(t, rotate(r)); + } + + template < typename T > + [[nodiscard]] constexpr mat trs(const vec& t, const qua& r, const vec& s) { + return trs(t, rotate(r), s); + } + + // translate + + template < typename T > + [[nodiscard]] constexpr mat translate(const vec& v) { + /// REFERENCE: + /// https://en.wikipedia.org/wiki/Translation_(geometry) + + return { + { T{1}, T{0}, T{0}, T{0} }, + { T{0}, T{1}, T{0}, T{0} }, + { T{0}, T{0}, T{1}, T{0} }, + { v.x, v.y, v.z, T{1} }}; + } + + // rotate + + template < typename T > + [[nodiscard]] constexpr mat rotate(const qua& q) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/ + + const auto [qv, qs] = normalize(q); + + const T x2 = qv.x * T{2}; + const T y2 = qv.y * T{2}; + const T z2 = qv.z * T{2}; + + const T sx2 = qs * x2; + const T sy2 = qs * y2; + const T sz2 = qs * z2; + + const T xx2 = qv.x * x2; + const T xy2 = qv.x * y2; + const T xz2 = qv.x * z2; + + const T yy2 = qv.y * y2; + const T yz2 = qv.y * z2; + const T zz2 = qv.z * z2; + + return { + { T{1} - (yy2 + zz2), (xy2 + sz2), (xz2 - sy2) }, + { (xy2 - sz2), T{1} - (xx2 + zz2), (yz2 + sx2) }, + { (xz2 + sy2), (yz2 - sx2), T{1} - (xx2 + yy2) }}; + } + + template < typename T > + [[nodiscard]] constexpr mat rotate4(const qua& q) { + return mat(rotate(q)); + } + + template < typename T > + [[nodiscard]] constexpr mat rotate(T angle, const vec& axis) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/ + + const auto [s, c] = sincos(angle); + const auto [x, y, z] = normalize(axis); + + const T xx = x * x; + const T yy = y * y; + const T zz = z * z; + + const T xs = x * s; + const T ys = y * s; + const T zs = z * s; + + const T ic = T{1} - c; + + const T xxm = xx * ic; + const T yym = yy * ic; + const T zzm = zz * ic; + + const T xym = x * y * ic; + const T xzm = x * z * ic; + const T yzm = y * z * ic; + + return { + { xxm + c, xym + zs, xzm - ys }, + { xym - zs, yym + c, yzm + xs }, + { xzm + ys, yzm - xs, zzm + c }}; + } + + template < typename T > + [[nodiscard]] constexpr mat rotate4(T angle, const vec& axis) { + return mat(rotate(angle, axis)); + } + + // rotate_x + + template < typename T > + [[nodiscard]] constexpr mat rotate_x(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ + + const auto [s, c] = sincos(angle); + + return { + { T{1}, T{0}, T{0} }, + { T{0}, c, s }, + { T{0}, -s, c }}; + } + + template < typename T > + [[nodiscard]] constexpr mat rotate4_x(T angle) { + return mat(rotate_x(angle)); + } + + // rotate_y + + template < typename T > + [[nodiscard]] constexpr mat rotate_y(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ + + const auto [s, c] = sincos(angle); + + return { + { c, T{0}, -s }, + { T{0}, T{1}, T{0} }, + { s, T{0}, c }}; + } + + template < typename T > + [[nodiscard]] constexpr mat rotate4_y(T angle) { + return mat(rotate_y(angle)); + } + + // rotate_z + + template < typename T > + [[nodiscard]] constexpr mat rotate_z(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ + + const auto [s, c] = sincos(angle); + + return { + { c, s, T{0} }, + { -s, c, T{0} }, + { T{0}, T{0}, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat rotate4_z(T angle) { + return mat(rotate_z(angle)); + } + + // scale + + template < typename T > + [[nodiscard]] constexpr mat scale(const vec& v) { + /// REFERENCE: + /// https://en.wikipedia.org/wiki/Scaling_(geometry) + + return { + { v.x, T{0}, T{0} }, + { T{0}, v.y, T{0} }, + { T{0}, T{0}, v.z }}; + } + + template < typename T > + [[nodiscard]] constexpr mat scale4(const vec& v) { + return mat(scale(v)); + } + + // look_at + + template < typename T > + [[nodiscard]] constexpr mat look_at_lh( + const vec& dir, + const vec& up) + { + /// REFERENCE: + /// https://www.euclideanspace.com/maths/algebra/vectors/lookat/ + + const vec az = normalize(dir); + const vec ax = normalize(cross(up, az)); + const vec ay = cross(az, ax); + + return { + { ax.x, ay.x, az.x }, + { ax.y, ay.y, az.y }, + { ax.z, ay.z, az.z }}; + } + + template < typename T > + [[nodiscard]] constexpr mat look_at_lh( + const vec& eye, + const vec& at, + const vec& up) + { + /// REFERENCE: + /// https://www.euclideanspace.com/maths/algebra/vectors/lookat/ + + const vec az = normalize(at - eye); + const vec ax = normalize(cross(up, az)); + const vec ay = cross(az, ax); + + const T dx = dot(ax, eye); + const T dy = dot(ay, eye); + const T dz = dot(az, eye); + + return { + { ax.x, ay.x, az.x, T{0} }, + { ax.y, ay.y, az.y, T{0} }, + { ax.z, ay.z, az.z, T{0} }, + { -dx, -dy, -dz, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat look_at_rh( + const vec& dir, + const vec& up) + { + /// REFERENCE: + /// https://www.euclideanspace.com/maths/algebra/vectors/lookat/ + + const vec az = normalize(-dir); + const vec ax = normalize(cross(up, az)); + const vec ay = cross(az, ax); + + return { + { ax.x, ay.x, az.x }, + { ax.y, ay.y, az.y }, + { ax.z, ay.z, az.z }}; + } + + template < typename T > + [[nodiscard]] constexpr mat look_at_rh( + const vec& eye, + const vec& at, + const vec& up) + { + /// REFERENCE: + /// https://www.euclideanspace.com/maths/algebra/vectors/lookat/ + + const vec az = normalize(eye - at); + const vec ax = normalize(cross(up, az)); + const vec ay = cross(az, ax); + + const T dx = dot(ax, eye); + const T dy = dot(ay, eye); + const T dz = dot(az, eye); + + return { + { ax.x, ay.x, az.x, T{0} }, + { ax.y, ay.y, az.y, T{0} }, + { ax.z, ay.z, az.z, T{0} }, + { -dx, -dy, -dz, T{1} }}; + } +} + +// +// Matrix Transform 2D +// + +namespace vmath_hpp +{ + // trs + + template < typename T > + [[nodiscard]] constexpr mat trs(const vec& t, const mat& r) { + return { + { r[0], T{0} }, + { r[1], T{0} }, + { t, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat trs(const vec& t, const mat& r, const vec& s) { + return { + { r[0] * s[0], T{0} }, + { r[1] * s[1], T{0} }, + { t, T{1} }}; + } + + // translate + + template < typename T > + [[nodiscard]] constexpr mat translate(const vec& v) { + /// REFERENCE: + /// https://en.wikipedia.org/wiki/Translation_(geometry) + + return { + { T{1}, T{0}, T{0} }, + { T{0}, T{1}, T{0} }, + { v.x, v.y, T{1} }}; + } + + // rotate + + template < typename T > + [[nodiscard]] constexpr mat rotate(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/ + + const auto [s, c] = sincos(angle); + + return { + { c, s }, + { -s, c }}; + } + + template < typename T > + [[nodiscard]] constexpr mat rotate3(T angle) { + return mat(rotate(angle)); + } + + // scale + + template < typename T > + [[nodiscard]] constexpr mat scale(const vec& v) { + /// REFERENCE: + /// https://en.wikipedia.org/wiki/Scaling_(geometry) + + return { + { v.x, T{0} }, + { T{0}, v.y }}; + } + + template < typename T > + [[nodiscard]] constexpr mat scale3(const vec& v) { + return mat(scale(v)); + } + + // shear + + template < typename T > + [[nodiscard]] constexpr mat shear(const vec& v) { + /// REFERENCE: + /// https://en.wikipedia.org/wiki/Shear_matrix + + return { + { T{1}, v.y }, + { v.x, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat shear3(const vec& v) { + return mat(shear(v)); + } +} + +// +// Matrix Projections +// + +namespace vmath_hpp +{ + // orthographic + + template < typename T > + [[nodiscard]] constexpr mat orthographic_lh(T width, T height, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixortholh + + const T rwidth = rcp(width); + const T rheight = rcp(height); + const T frange = rcp(zfar - znear); + + const T sx = T{2} * rwidth; + const T sy = T{2} * rheight; + const T sz = frange; + const T tz = -frange * znear; + + return { + { sx, T{0}, T{0}, T{0} }, + { T{0}, sy, T{0}, T{0} }, + { T{0}, T{0}, sz, T{0} }, + { T{0}, T{0}, tz, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat orthographic_rh(T width, T height, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixorthorh + + const T rwidth = rcp(width); + const T rheight = rcp(height); + const T frange = rcp(znear - zfar); + + const T sx = T{2} * rwidth; + const T sy = T{2} * rheight; + const T sz = frange; + const T tz = frange * znear; + + return { + { sx, T{0}, T{0}, T{0} }, + { T{0}, sy, T{0}, T{0} }, + { T{0}, T{0}, sz, T{0} }, + { T{0}, T{0}, tz, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat orthographic_lh(T left, T right, T bottom, T top, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixorthooffcenterlh + + const T rwidth = rcp(right - left); + const T rheight = rcp(top - bottom); + const T frange = rcp(zfar - znear); + + return { + { T{2} * rwidth, T{0}, T{0}, T{0} }, + { T{0}, T{2} * rheight, T{0}, T{0} }, + { T{0}, T{0}, frange, T{0} }, + { -(left + right) * rwidth, -(top + bottom) * rheight, -frange * znear, T{1} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat orthographic_rh(T left, T right, T bottom, T top, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixorthooffcenterrh + + const T rwidth = rcp(right - left); + const T rheight = rcp(top - bottom); + const T frange = rcp(znear - zfar); + + return { + { T{2} * rwidth, T{0}, T{0}, T{0} }, + { T{0}, T{2} * rheight, T{0}, T{0} }, + { T{0}, T{0}, frange, T{0} }, + { -(left + right) * rwidth, -(top + bottom) * rheight, frange * znear, T{1} }}; + } + + // perspective + + template < typename T > + [[nodiscard]] constexpr mat perspective_lh(T width, T height, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixperspectivelh + + const T sx = T{2} * znear * rcp(width); + const T sy = T{2} * znear * rcp(height); + const T sz = zfar * rcp(zfar - znear); + const T tz = (znear * zfar) * rcp(znear - zfar); + + return { + { T{sx}, T{0}, T{0}, T{0} }, + { T{0}, T{sy}, T{0}, T{0} }, + { T{0}, T{0}, T{sz}, T{1} }, + { T{0}, T{0}, T{tz}, T{0} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat perspective_rh(T width, T height, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixperspectiverh + + const T sx = T{2} * znear * rcp(width); + const T sy = T{2} * znear * rcp(height); + const T sz = zfar * rcp(znear - zfar); + const T tz = (znear * zfar) * rcp(znear - zfar); + + return { + { sx, T{0}, T{0}, T{0} }, + { T{0}, sy, T{0}, T{0} }, + { T{0}, T{0}, sz, -T{1} }, + { T{0}, T{0}, tz, T{0} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat perspective_lh(T left, T right, T bottom, T top, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixperspectiveoffcenterlh + + const T znear2 = T{2} * znear; + const T rwidth = rcp(right - left); + const T rheight = rcp(top - bottom); + const T frange = zfar * rcp(zfar - znear); + + return { + { znear2 * rwidth, T{0}, T{0}, T{0} }, + { T{0}, znear2 * rheight, T{0}, T{0} }, + { -(left + right) * rwidth, -(top + bottom) * rheight, frange, T{1} }, + { T{0}, T{0}, -frange * znear, T{0} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat perspective_rh(T left, T right, T bottom, T top, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixperspectiveoffcenterrh + + const T znear2 = T{2} * znear; + const T rwidth = rcp(right - left); + const T rheight = rcp(top - bottom); + const T frange = zfar * rcp(znear - zfar); + + return { + { znear2 * rwidth, T{0}, T{0}, T{0} }, + { T{0}, znear2 * rheight, T{0}, T{0} }, + { (left + right) * rwidth, (top + bottom) * rheight, frange, -T{1} }, + { T{0}, T{0}, frange * znear, T{0} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat perspective_fov_lh(T fovy, T aspect, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixperspectivefovlh + + const T sy = rcp(tan(fovy * T{0.5f})); + const T sx = sy * rcp(aspect); + const T sz = zfar * rcp(zfar - znear); + const T tz = (znear * zfar) * rcp(znear - zfar); + + return { + { sx, T{0}, T{0}, T{0} }, + { T{0}, sy, T{0}, T{0} }, + { T{0}, T{0}, sz, T{1} }, + { T{0}, T{0}, tz, T{0} }}; + } + + template < typename T > + [[nodiscard]] constexpr mat perspective_fov_rh(T fovy, T aspect, T znear, T zfar) { + /// REFERENCE: + /// https://docs.microsoft.com/en-us/windows/win32/direct3d9/d3dxmatrixperspectivefovrh + + const T sy = rcp(tan(fovy * T{0.5f})); + const T sx = sy * rcp(aspect); + const T sz = zfar * rcp(znear - zfar); + const T tz = (znear * zfar) * rcp(znear - zfar); + return { + { sx, T{0}, T{0}, T{0} }, + { T{0}, sy, T{0}, T{0} }, + { T{0}, T{0}, sz, -T{1} }, + { T{0}, T{0}, tz, T{0} }}; + } +} + +// +// Vector Transform +// + +namespace vmath_hpp +{ + // angle + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr T angle(const vec& x, const vec& y) { + const T rs = rsqrt(length2(x) * length2(y)); + return acos(clamp(dot(x, y) * rs, -T{1}, T{1})); + } + + // rotate + + template < typename T > + [[nodiscard]] constexpr vec rotate(const vec& v, T angle) { + return v * rotate(angle); + } + + template < typename T > + [[nodiscard]] constexpr vec rotate_x(const vec& v, T angle) { + return v * qrotate(angle, unit3_x); + } + + template < typename T > + [[nodiscard]] constexpr vec rotate_y(const vec& v, T angle) { + return v * qrotate(angle, unit3_y); + } + + template < typename T > + [[nodiscard]] constexpr vec rotate_z(const vec& v, T angle) { + return v * qrotate(angle, unit3_z); + } + + template < typename T > + [[nodiscard]] constexpr vec rotate(const vec& v, T angle, const vec& axis) { + return v * qrotate(angle, axis); + } + + // project + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec project(const vec& v, const vec& normal) { + return dot(v, normal) * rlength2(normal) * normal; + } + + // perpendicular + + template < typename T, std::size_t Size > + [[nodiscard]] constexpr vec perpendicular(const vec& v, const vec& normal) { + return v - project(v, normal); + } +} + +// +// Quaternion Transform +// + +namespace vmath_hpp +{ + // qrotate + + template < typename T > + [[nodiscard]] constexpr qua qrotate(const mat& m) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + + auto xyzw = T{0.5f} * sqrt(max(T{0}, vec{ + T{1} + m[0][0] - m[1][1] - m[2][2], + T{1} - m[0][0] + m[1][1] - m[2][2], + T{1} - m[0][0] - m[1][1] + m[2][2], + T{1} + m[0][0] + m[1][1] + m[2][2]})); + + return qua(copysign(xyzw, { + m[1][2] - m[2][1], + m[2][0] - m[0][2], + m[0][1] - m[1][0], + T{1}})); + } + + template < typename T > + [[nodiscard]] constexpr qua qrotate(const vec& from, const vec& to) { + /// REFERENCE: + /// http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final + + const T n = sqrt(length2(from) * length2(to)); + const T s = dot(from, to) + n; + + if ( s < T{0.000001f} * n ) { + return abs(from.z) < abs(from.x) + ? normalize(qua{vec{-from.y, from.x, T{0}}, T{0}}) + : normalize(qua{vec{T{0}, -from.z, from.y}, T{0}}); + } + + return normalize(qua{cross(from, to), s}); + } + + template < typename T > + [[nodiscard]] constexpr qua qrotate(T angle, const vec& axis) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/ + + const auto [s, c] = sincos(angle * T{0.5f}); + const auto [x, y, z] = normalize(axis); + + return {vec{x, y, z} * s, c}; + } + + template < typename T > + [[nodiscard]] constexpr qua qrotate_x(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/ + + const auto [s, c] = sincos(angle * T{0.5f}); + + return {vec{s, T{0}, T{0}}, c}; + } + + template < typename T > + [[nodiscard]] constexpr qua qrotate_y(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/ + + const auto [s, c] = sincos(angle * T{0.5f}); + + return {vec{T{0}, s, T{0}}, c}; + } + + template < typename T > + [[nodiscard]] constexpr qua qrotate_z(T angle) { + /// REFERENCE: + /// http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/ + + const auto [s, c] = sincos(angle * T{0.5f}); + + return {vec{T{0}, T{0}, s}, c}; + } + + // qlook_at + + template < typename T > + [[nodiscard]] constexpr qua qlook_at_lh(const vec& dir, const vec& up) { + return qrotate(look_at_lh(dir, up)); + } + + template < typename T > + [[nodiscard]] constexpr qua qlook_at_rh(const vec& dir, const vec& up) { + return qrotate(look_at_rh(dir, up)); + } +} diff --git a/untests/CMakeLists.txt b/untests/CMakeLists.txt index edc5959..0220203 100644 --- a/untests/CMakeLists.txt +++ b/untests/CMakeLists.txt @@ -18,7 +18,7 @@ endif() file(GLOB_RECURSE UNTESTS_SOURCES "*.cpp" "*.hpp") add_executable(${PROJECT_NAME} ${UNTESTS_SOURCES}) -target_link_libraries(${PROJECT_NAME} vmath.hpp) +target_link_libraries(${PROJECT_NAME} vmath.hpp.singles) target_compile_options(${PROJECT_NAME} PRIVATE