commit bc037aa2bd9fd2b8916c9cf783096c7c8941b77b Author: jeanlemotan Date: Tue Jul 2 18:27:05 2024 +0200 First diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c585e19 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +out \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..98ab633 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.16.5) + +project(VMath) + +include(${PROJECT_SOURCE_DIR}/../build-utils/cmake/compiler_settings.cmake) + +if(NOT TARGET TL) + add_subdirectory("${PROJECT_SOURCE_DIR}/../TL" "./TL") +endif() + +file(GLOB_RECURSE SRC "src/*.cpp") +file(GLOB_RECURSE HEADERS "include/*.h" "include/*.hpp" "include/*.inl") + +foreach(_source IN ITEMS ${HEADERS}) +get_filename_component(_source_path "${_source}" PATH) +file(RELATIVE_PATH _source_path_rel "${PROJECT_SOURCE_DIR}" "${_source_path}") +string(REPLACE "/" "\\" _group_path "${_source_path_rel}") +source_group("${_group_path}" FILES "${_source}") +endforeach() + +foreach(_source IN ITEMS ${SRC}) +get_filename_component(_source_path "${_source}" PATH) +file(RELATIVE_PATH _source_path_rel "${PROJECT_SOURCE_DIR}" "${_source_path}") +string(REPLACE "/" "\\" _group_path "${_source_path_rel}") +source_group("${_group_path}" FILES "${_source}") +endforeach() + +add_library(VMath STATIC ${SRC} ${HEADERS}) + +target_include_directories(VMath PUBLIC "include") +target_link_libraries(VMath TL) + + +if (0) +#////////////////////////////////// + + set(Boost_INCLUDE_DIR "${PROJECT_SOURCE_DIR}/../boost") + set(Boost_LIBRARY_DIR "${PROJECT_SOURCE_DIR}/../boost/stage/lib") + set(Boost_USE_STATIC_LIBS ON) + set(Boost_USE_MULTITHREADED ON) + set(Boost_USE_STATIC_RUNTIME OFF) + find_package(Boost 1.69 COMPONENTS unit_test_framework) + + file(GLOB_RECURSE TEST_SRC "test/*.cpp" "test/*.h") + add_executable(VMath_Test ${TEST_SRC}) + target_link_libraries(VMath_Test VMath) + target_link_libraries(VMath_Test ${Boost_LIBRARIES}) + target_include_directories(VMath_Test PUBLIC ${Boost_INCLUDE_DIRS}) +endif() \ No newline at end of file diff --git a/include/VMath.h b/include/VMath.h new file mode 100644 index 0000000..fbd21e2 --- /dev/null +++ b/include/VMath.h @@ -0,0 +1,269 @@ +#pragma once + +#include +#include +#include +#include + +#if defined(_MSC_VER) && _MSC_VER >= 1900 +# define MATH_RESTRICT __restrict +#elif (defined(__clang__) || defined(__GNUC__)) +# define MATH_RESTRICT __restrict__ +#else +# define MATH_RESTRICT +#endif + +namespace math +{ +static struct ZUninitialized {} uninitialized; + +//standard precision +struct standard {}; +//fast but less precise +struct fast {}; +//standard precision but safe assumptions +struct safe {}; +} + +#include "VMath/angle.h" +#include "VMath/vec2.h" +#include "VMath/vec3.h" +#include "VMath/vec4.h" +#include "VMath/rect.h" +#include "VMath/mat2.h" +#include "VMath/mat3.h" +#include "VMath/mat4.h" +#include "VMath/trans2.h" +#include "VMath/trans3.h" +#include "VMath/rigid2.h" +#include "VMath/rigid3.h" +#include "VMath/quat.h" +#include "VMath/plane.h" +#include "VMath/line2.h" +#include "VMath/line3.h" +#include "VMath/ray2.h" +#include "VMath/ray3.h" +#include "VMath/segment2.h" +#include "VMath/segment3.h" +#include "VMath/circle2.h" +#include "VMath/triangle3.h" +#include "VMath/aabb3.h" +#include "VMath/aabb2.h" +#include "VMath/range.h" +#include "VMath/clamped_value.h" + +namespace math +{ +typedef angle anglef; +typedef angle angled; + +typedef vec2 byte2; +typedef vec2 ubyte2; +typedef vec2 short2; +typedef vec2 ushort2; +typedef vec2 int2; +typedef vec2 uint2; +typedef vec2 float2; +typedef vec2 double2; + +typedef vec2 vec2b; +typedef vec2 vec2ub; +typedef vec2 vec2s; +typedef vec2 vec2us; +typedef vec2 vec2i; +typedef vec2 vec2u; +typedef vec2 vec2f; +typedef vec2 vec2d; + +typedef vec3 byte3; +typedef vec3 ubyte3; +typedef vec3 short3; +typedef vec3 ushort3; +typedef vec3 int3; +typedef vec3 uint3; +typedef vec3 float3; +typedef vec3 double3; + +typedef vec3 vec3b; +typedef vec3 vec3ub; +typedef vec3 vec3s; +typedef vec3 vec3us; +typedef vec3 vec3i; +typedef vec3 vec3u; +typedef vec3 vec3f; +typedef vec3 vec3d; + +typedef vec4 byte4; +typedef vec4 ubyte4; +typedef vec4 short4; +typedef vec4 ushort4; +typedef vec4 int4; +typedef vec4 uint4; +typedef vec4 float4; +typedef vec4 double4; + +typedef vec4 vec4b; +typedef vec4 vec4ub; +typedef vec4 vec4s; +typedef vec4 vec4us; +typedef vec4 vec4i; +typedef vec4 vec4u; +typedef vec4 vec4f; +typedef vec4 vec4d; + +typedef quat quatf; +typedef quat quatd; + +typedef mat2 mat2f; +typedef mat2 mat2d; +typedef mat3 mat3f; +typedef mat3 mat3d; +typedef mat4 mat4f; +typedef mat4 mat4d; + +typedef trans2 trans2f; +typedef trans2 trans2d; +typedef trans3 trans3f; +typedef trans3 trans3d; + +typedef rigid2 rigid2f; +typedef rigid2 rigid2d; +typedef rigid3 rigid3f; +typedef rigid3 rigid3d; + +typedef line2 line2f; +typedef line2 line2d; +typedef line3 line3f; +typedef line3 line3d; + +typedef ray2 ray2f; +typedef ray2 ray2d; +typedef ray3 ray3f; +typedef ray3 ray3d; + +typedef segment2 segment2f; +typedef segment2 segment2d; +typedef segment3 segment3f; +typedef segment3 segment3d; + +typedef rect recti; +typedef rect rectu; +typedef rect rectf; +typedef rect rectd; + +typedef aabb2 aabb2f; +typedef aabb2 aabb2d; +typedef aabb3 aabb3f; +typedef aabb3 aabb3d; + +typedef triangle3 triangle3f; +typedef triangle3 triangle3d; + +typedef circle2 circle2f; +typedef circle2 circle2d; + +typedef plane planef; +typedef plane planed; + +typedef range rangef; +typedef range range2f; +typedef range range3f; +typedef range range4f; + +typedef range ranged; +typedef range range2d; +typedef range range3d; +typedef range range4d; + +typedef range rangei; +typedef range range2i; +typedef range range3i; +typedef range range4i; + +namespace detail +{ +template +struct UValue_Traits +{ + static constexpr T max = tl::numeric_limits::max(); + static constexpr T min = T(0); +}; +template +struct Mu_Traits +{ + static constexpr T min = T(0); + static constexpr T max = T(1); +}; +template +struct Smu_Traits +{ + static constexpr T min = T(-1); + static constexpr T max = T(1); +}; +} + +typedef clamped_value> ufloat; +typedef clamped_value> udouble; + +typedef clamped_value> muf; +typedef clamped_value> mud; + +typedef clamped_value> smuf; +typedef clamped_value> smud; + +}//namespace math + +#include "VMath/func_test.h" +#include "VMath/func_common.h" +#include "VMath/func_range.h" +#include "VMath/func_trig.h" +#include "VMath/func_interp.h" +#include "VMath/func_projection.h" +#include "VMath/func_transform.h" +#include "VMath/func_binary.h" +#include "VMath/func_string.h" +#include "VMath/func_cast.h" +#include "VMath/func_interpolation.h" +#include "VMath/func_intersect.h" + +#include "VMath/angle.inl" +#include "VMath/circle2.inl" +#include "VMath/vec2.inl" +#include "VMath/vec3.inl" +#include "VMath/vec4.inl" +#include "VMath/rect.inl" +#include "VMath/mat2.inl" +#include "VMath/mat3.inl" +#include "VMath/mat4.inl" +#include "VMath/trans2.inl" +#include "VMath/trans3.inl" +#include "VMath/rigid2.inl" +#include "VMath/rigid3.inl" +#include "VMath/quat.inl" +#include "VMath/plane.inl" +#include "VMath/line2.inl" +#include "VMath/line3.inl" +#include "VMath/ray2.inl" +#include "VMath/ray3.inl" +#include "VMath/segment2.inl" +#include "VMath/segment3.inl" +#include "VMath/triangle3.inl" +#include "VMath/aabb3.inl" +#include "VMath/aabb2.inl" +#include "VMath/range.inl" +#include "VMath/clamped_value.inl" + +#include "VMath/func_cast.inl" +#include "VMath/func_range.inl" +#include "VMath/func_test.inl" +#include "VMath/func_common.inl" +#include "VMath/func_trig.inl" +#include "VMath/func_interp.inl" +#include "VMath/func_projection.inl" +#include "VMath/func_transform.inl" +#include "VMath/func_binary.inl" +#include "VMath/func_interpolation.inl" +#include "VMath/func_intersect.inl" + + +#undef MATH_RESTRICT diff --git a/include/VMath/aabb2.h b/include/VMath/aabb2.h new file mode 100644 index 0000000..6ac7fd8 --- /dev/null +++ b/include/VMath/aabb2.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +namespace math +{ + +template +struct aabb2 +{ + typedef T value_t; + typedef aabb2 this_t; + + constexpr aabb2() noexcept = default; + constexpr aabb2(const vec2& min, const vec2& max) noexcept; + constexpr explicit aabb2(const vec2& init) noexcept; + + constexpr vec2 get_center() const noexcept; + constexpr vec2 get_extent() const noexcept; + constexpr vec2 get_half_extent() const noexcept; + + constexpr bool is_valid() const noexcept; + constexpr bool is_empty() const noexcept; + + constexpr T get_area() const noexcept; + + ///@param corners: an array of 4 corners. + /* Edges are stored in this way (assuming Y up): + 1---------2 +Y + | | ^ + | | | + | | +-----> +X + 0---------3 + */ + constexpr void get_corners(tl::span> corners) const noexcept; + + constexpr void add(const vec2& p) noexcept; + constexpr void add(const this_t& b) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + vec2 min = vec2(tl::numeric_limits::max()); + vec2 max = vec2(tl::numeric_limits::lowest()); +}; + +} diff --git a/include/VMath/aabb2.inl b/include/VMath/aabb2.inl new file mode 100644 index 0000000..7f20e9e --- /dev/null +++ b/include/VMath/aabb2.inl @@ -0,0 +1,119 @@ + +#include + +namespace math +{ + +template +inline constexpr aabb2::aabb2(const vec2& minp, const vec2& maxp) noexcept + : min(minp) + , max(maxp) +{ + TL_PLAIN_ASSERT(is_lequal(minp, maxp)); +} + +template +inline constexpr aabb2::aabb2(const vec2& p) noexcept + : min(p) + , max(p) +{ +} + +template +inline constexpr vec2 aabb2::get_center() const noexcept +{ + if constexpr (std::is_floating_point_v) + return (min + max) * T(0.5); + else + return (min + max) / T(2); +} + +template +inline constexpr vec2 aabb2::get_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return max - min; +} + +template +inline constexpr vec2 aabb2::get_half_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + if constexpr (std::is_floating_point_v) + return (max - min) * T(0.5); + else + return (max - min) / T(2); +} + +template +inline constexpr bool aabb2::is_valid() const noexcept +{ + return max.x >= min.x; +} + +template +inline constexpr bool aabb2::is_empty() const noexcept +{ + return !is_valid() || min == max; +} + +template +inline constexpr T aabb2::get_area() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + vec2 e = get_extent(); + return e.x * e.y; +} + +template +inline constexpr void aabb2::get_corners(tl::span> corners) const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + + /* + Edges are stored in this way (assuming Y up): + Hey, am I an ascii artist, or what? :) niko. + 1---------2 +Y + | | ^ + | | | + | | +-----> +X + 0---------3 + */ + const vec2 middle = get_center(); + const vec2 diag = get_half_extent(); + corners[0] = vec2(middle.x - diag.x, middle.y - diag.y); + corners[1] = vec2(middle.x - diag.x, middle.y + diag.y); + corners[2] = vec2(middle.x + diag.x, middle.y + diag.y); + corners[3] = vec2(middle.x + diag.x, middle.y - diag.y); +} + +template +inline constexpr void aabb2::add(const vec2& p) noexcept +{ + this->min = math::min(this->min, p); + this->max = math::max(this->max, p); +} + +template +inline constexpr void aabb2::add(const this_t& b) noexcept +{ + add(b.min); + add(b.max); +} + +}//namespace math + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::aabb2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.min); + tl::hash_and_combine(seed, p.max); + return seed; + } +}; diff --git a/include/VMath/aabb3.h b/include/VMath/aabb3.h new file mode 100644 index 0000000..eea3fa0 --- /dev/null +++ b/include/VMath/aabb3.h @@ -0,0 +1,53 @@ +#pragma once + +#include +#include +#include + +namespace math +{ + +template +struct aabb3 +{ + using value_t = T; + using this_t = aabb3; + + constexpr aabb3() = default; + constexpr aabb3(const vec3& min, const vec3& max) noexcept; + constexpr explicit aabb3(const vec3& init) noexcept; + + constexpr vec3 get_center() const noexcept; + constexpr vec3 get_extent() const noexcept; + constexpr vec3 get_half_extent() const noexcept; + + constexpr bool is_valid() const noexcept; + constexpr bool is_empty() const noexcept; + + constexpr T get_volume() const noexcept; + constexpr T get_area() const noexcept; + + /*! Returns box corners into corners[] + /3--------/7 + / | / | +Y + / | / | ^ + 1---------5 | | + | 2- - -| -6 +-------> +X + | / | / / + |/ | / / + 0---------4/ +Z + */ + constexpr void get_corners(tl::span> corners) const noexcept; + + constexpr void add(const vec3& p) noexcept; + constexpr void add(const this_t& b) noexcept; + constexpr void add_checked(const this_t& b) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + vec3 min = vec3(tl::numeric_limits::max()); + vec3 max = vec3(tl::numeric_limits::lowest()); +}; + +} diff --git a/include/VMath/aabb3.inl b/include/VMath/aabb3.inl new file mode 100644 index 0000000..7ee6510 --- /dev/null +++ b/include/VMath/aabb3.inl @@ -0,0 +1,141 @@ +namespace math +{ + +template +inline constexpr aabb3::aabb3(const vec3& minp, const vec3& maxp) noexcept + : min(minp) + , max(maxp) +{ + TL_PLAIN_ASSERT(is_lequal(minp, maxp)); +} + +template +inline constexpr aabb3::aabb3(const vec3& init) noexcept + : min(init) + , max(init) +{ +} + +template +inline constexpr vec3 aabb3::get_center() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + if constexpr (std::is_floating_point_v) + return (min + max) * T(0.5); + else + return (min + max) / T(2); +} + +template +inline constexpr vec3 aabb3::get_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return max - min; +} + +template +inline constexpr vec3 aabb3::get_half_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + if constexpr (std::is_floating_point_v) + return (max - min) * T(0.5); + else + return (max - min) / T(2); +} + +template +inline constexpr bool aabb3::is_valid() const noexcept +{ + return is_lequal(min, max); +} + +template +inline constexpr bool aabb3::is_empty() const noexcept +{ + return !is_valid() || min == max; +} + +template +inline constexpr T aabb3::get_volume() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + vec3 e = get_extent(); + return e.x * e.y * e.z; +} + +template +inline constexpr T aabb3::get_area() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + vec3 e = get_extent(); + return T(2) * (e.x*e.y + e.x*e.z + e.y*e.z); +} + +template +inline constexpr void aabb3::get_corners(tl::span> corners) const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + /* + Edges are stored in this way(assuming Y up): + Hey, am I an ascii artist, or what? :) niko. + /3--------/7 + / | / | +Y + / | / | ^ + 1---------5 | | + | 2- - -| -6 +-------> +X + | / | / / + |/ | / / + 0---------4/ +Z + */ + const vec3& middle = get_center(); + const vec3& half_diag = get_half_extent(); + corners[0] = vec3(middle.x - half_diag.x, middle.y - half_diag.y, middle.z + half_diag.z); + corners[1] = vec3(middle.x - half_diag.x, middle.y + half_diag.y, middle.z + half_diag.z); + corners[2] = vec3(middle.x - half_diag.x, middle.y - half_diag.y, middle.z - half_diag.z); + corners[3] = vec3(middle.x - half_diag.x, middle.y + half_diag.y, middle.z - half_diag.z); + corners[4] = vec3(middle.x + half_diag.x, middle.y - half_diag.y, middle.z + half_diag.z); + corners[5] = vec3(middle.x + half_diag.x, middle.y + half_diag.y, middle.z + half_diag.z); + corners[6] = vec3(middle.x + half_diag.x, middle.y - half_diag.y, middle.z - half_diag.z); + corners[7] = vec3(middle.x + half_diag.x, middle.y + half_diag.y, middle.z - half_diag.z); +} + +template +inline constexpr void aabb3::add(const vec3& p) noexcept +{ + this->min = math::min(this->min, p); + this->max = math::max(this->max, p); +} + +template +inline constexpr void aabb3::add(const aabb3& b) noexcept +{ + TL_PLAIN_ASSERT(b.is_valid()); + add(b.min); + add(b.max); +} +template +inline constexpr void aabb3::add_checked(const aabb3& b) noexcept +{ + if (b.is_valid()) + { + add(b.min); + add(b.max); + } +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::aabb3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.min); + tl::hash_and_combine(seed, p.max); + return seed; + } +}; diff --git a/include/VMath/angle.h b/include/VMath/angle.h new file mode 100644 index 0000000..bb9d33e --- /dev/null +++ b/include/VMath/angle.h @@ -0,0 +1,61 @@ +#pragma once + +#include + +namespace math +{ + +template +struct angle +{ + typedef T value_t; + using this_t = angle; + + static inline T const pi = T(3.1415926535897932384626433832795028841971); + static inline T const pi2 = T(3.1415926535897932384626433832795028841971) / T(2); + static inline T const _2pi = T(3.1415926535897932384626433832795028841971) * T(2); + static inline T const _3pi2 = T(3.1415926535897932384626433832795028841971) * T(3) / T(2); + + + constexpr angle() noexcept = default; + constexpr explicit angle(T radians) noexcept; + constexpr angle(angle const& a) = default; + + constexpr explicit operator T() const { return radians; } + + // normalizes an angle to -pi .. pi + constexpr angle& normalize() noexcept; + + // normalizes an angle to 0 .. 2pi + constexpr angle& normalize_from_0_to_2pi() noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr angle operator-() const noexcept; + + constexpr angle operator+(angle const& v) const noexcept; + constexpr angle operator-(angle const& v) const noexcept; + constexpr angle operator*(angle const& v) const noexcept; + constexpr angle operator/(angle const& v) const noexcept; + constexpr angle& operator=(angle const& v) noexcept; + constexpr angle& operator+=(angle const& v) noexcept; + constexpr angle& operator-=(angle const& v) noexcept; + constexpr angle& operator*=(angle const& v) noexcept; + constexpr angle& operator/=(angle const& v) noexcept; + + constexpr angle operator+(T v) const noexcept; + constexpr angle operator-(T v) const noexcept; + constexpr angle operator*(T v) const noexcept; + constexpr angle operator/(T v) const noexcept; + constexpr angle& operator=(T v) noexcept; + constexpr angle& operator+=(T v) noexcept; + constexpr angle& operator-=(T v) noexcept; + constexpr angle& operator*=(T v) noexcept; + constexpr angle& operator/=(T v) noexcept; + + + T radians = T{}; +}; + +} diff --git a/include/VMath/angle.inl b/include/VMath/angle.inl new file mode 100644 index 0000000..4a67f42 --- /dev/null +++ b/include/VMath/angle.inl @@ -0,0 +1,173 @@ +#pragma once + +namespace math +{ +////////////////////////////////////////////////////////////////////////// + +template +constexpr inline angle::angle(T radians) noexcept : radians(radians) { normalize(); } + +template +constexpr inline angle& angle::normalize() noexcept +{ + T a = radians; + while (a > pi) a -= _2pi; + while (a < -pi) a += _2pi; + + TL_PLAIN_ASSERT(-pi <= a && a <= pi); + radians = a; + return *this; +} + +template +constexpr inline angle& angle::normalize_from_0_to_2pi() noexcept +{ + T a = radians; + while (a > _2pi) a -= _2pi; + while (a < 0) a += _2pi; + + TL_PLAIN_ASSERT(0 <= a && a <= _2pi); + radians = a; + return *this; +} + +template +constexpr inline angle angle::operator-() const noexcept +{ + T rad = (radians != -pi) ? -radians : pi; + TL_PLAIN_ASSERT(-pi < rad&& rad <= pi); + return angle(rad); +} +template +constexpr inline angle angle::operator+(angle const& v) const noexcept +{ + return *this + v.radians; +} +template +constexpr inline angle angle::operator-(angle const& v) const noexcept +{ + return *this - v.radians; +} +template +constexpr inline angle angle::operator*(angle const& v) const noexcept +{ + return *this * v.radians; +} +template +constexpr inline angle angle::operator/(angle const& v) const noexcept +{ + TL_PLAIN_ASSERT(!is_zero(v.radians)); + return *this / v.radians; +} +template +constexpr inline angle& angle::operator=(angle const& v) noexcept +{ + TL_PLAIN_ASSERT(-pi <= T(v) && T(v) <= pi); + radians = v.radians; + return *this; +} +template +constexpr inline angle& angle::operator+=(angle const& v) noexcept +{ + *this += v.radians; + return *this; +} +template +constexpr inline angle& angle::operator-=(angle const& v) noexcept +{ + *this -= v.radians; + return *this; +} +template +constexpr inline angle& angle::operator*=(angle const& v) noexcept +{ + *this *= v.radians; + return *this; +} +template +constexpr inline angle& angle::operator/=(angle const& v) noexcept +{ + TL_PLAIN_ASSERT(!is_zero(v.radians)); + *this /= v.radians; + return *this; +} + +////////////////////////////////////////////////////////////////////////// +template +constexpr inline angle angle::operator+(T v) const noexcept +{ + const T sum = radians + v; + return angle(sum); +} +template +constexpr inline angle angle::operator-(T v) const noexcept +{ + const T diff = radians - v; + return angle(diff); +} +template +constexpr inline angle angle::operator*(T v) const noexcept +{ + const T a = radians * v; + return angle(a); +} +template +constexpr inline angle angle::operator/(T v) const noexcept +{ + TL_PLAIN_ASSERT(!is_zero(v)); + const T a = radians / v; + return angle(a); +} +template +constexpr inline angle& angle::operator=(T v) noexcept +{ + radians = v; + normalize(); + return *this; +} +template +constexpr inline angle& angle::operator+=(T v) noexcept +{ + radians += v; + normalize(); + return *this; +} +template +constexpr inline angle& angle::operator-=(T v) noexcept +{ + radians -= v; + normalize(); + return *this; +} +template +constexpr inline angle& angle::operator*=(T v) noexcept +{ + radians *= v; + normalize(); + return *this; +} +template +constexpr inline angle& angle::operator/=(T v) noexcept +{ + TL_PLAIN_ASSERT(!is_zero(v)); + radians /= v; + normalize(); + return *this; +} + +////////////////////////////////////////////////////////////////////////// +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::angle& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.radians); + return seed; + } +}; diff --git a/include/VMath/circle2.h b/include/VMath/circle2.h new file mode 100644 index 0000000..62b23c6 --- /dev/null +++ b/include/VMath/circle2.h @@ -0,0 +1,31 @@ +#pragma once + +#include + +namespace math +{ + +template +struct circle2 +{ + typedef T value_t; + typedef circle2 this_t; + + constexpr circle2() noexcept = default; + constexpr circle2(const vec2& center, T radius) noexcept; + constexpr circle2(const circle2&) noexcept = default; + + constexpr circle2& operator=(const circle2&) noexcept = default; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr bool is_valid() const noexcept; + constexpr bool is_empty() const noexcept; + + + vec2 center; + T radius = T(-1); +}; + +} diff --git a/include/VMath/circle2.inl b/include/VMath/circle2.inl new file mode 100644 index 0000000..b52fc73 --- /dev/null +++ b/include/VMath/circle2.inl @@ -0,0 +1,39 @@ +namespace math +{ + +template +inline constexpr circle2::circle2(const vec2& center, T radius) noexcept + : center(center) + , radius(radius) +{ + TL_PLAIN_ASSERT(radius >= 0); +} + +template +inline constexpr bool circle2::is_valid() const noexcept +{ + return !is_negative(radius); +} + +template +inline constexpr bool circle2::is_empty() const noexcept +{ + return is_zero(radius); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::circle2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.center); + tl::hash_and_combine(seed, p.radius); + return seed; + } +}; diff --git a/include/VMath/clamped_value.h b/include/VMath/clamped_value.h new file mode 100644 index 0000000..d8bf48d --- /dev/null +++ b/include/VMath/clamped_value.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +namespace math +{ + +template +struct clamped_value +{ + using this_t = clamped_value; + constexpr clamped_value() noexcept = default; + constexpr clamped_value(T value) noexcept; + constexpr clamped_value(const this_t& other) noexcept; + constexpr this_t& operator=(T value) noexcept; + constexpr this_t& operator=(const this_t& other) noexcept; + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + constexpr bool operator!=(const this_t&) const noexcept = default; + + constexpr this_t operator+(T value) const noexcept; + constexpr this_t operator-(T value) const noexcept; + constexpr this_t operator*(T value) const noexcept; + constexpr this_t operator/(T value) const noexcept; + constexpr this_t& operator+=(T value) noexcept; + constexpr this_t& operator-=(T value) noexcept; + constexpr this_t& operator*=(T value) noexcept; + constexpr this_t& operator/=(T value) noexcept; + constexpr this_t& operator+=(const this_t& value) noexcept; + constexpr this_t& operator-=(const this_t& value) noexcept; + constexpr this_t& operator*=(const this_t& value) noexcept; + constexpr this_t& operator/=(const this_t& value) noexcept; + + constexpr operator T() const noexcept; + + constexpr T value() const noexcept; + +private: + T m_value; +}; + +} + diff --git a/include/VMath/clamped_value.inl b/include/VMath/clamped_value.inl new file mode 100644 index 0000000..6f4c844 --- /dev/null +++ b/include/VMath/clamped_value.inl @@ -0,0 +1,95 @@ +#include + +namespace math +{ + +template +constexpr clamped_value::clamped_value(T value) noexcept : m_value(math::clamp(value, Traits::min, Traits::max)) {} +template +constexpr clamped_value::clamped_value(const this_t& other) noexcept : m_value(other.m_value) {} +template +constexpr clamped_value& clamped_value::operator=(T value) noexcept + { + m_value = math::clamp(value, Traits::min, Traits::max); + return *this; + } +template +constexpr clamped_value& clamped_value::operator=(const this_t& other) noexcept + { + m_value = other.m_value; + return *this; + } + +template +constexpr clamped_value clamped_value::operator+(T value) const noexcept { return this_t(m_value + value); } +template +constexpr clamped_value clamped_value::operator-(T value) const noexcept { return this_t(m_value - value); } +template +constexpr clamped_value clamped_value::operator*(T value) const noexcept { return this_t(m_value * value); } +template +constexpr clamped_value clamped_value::operator/(T value) const noexcept { return this_t(m_value / value); } +template +constexpr clamped_value& clamped_value::operator+=(T value) noexcept { m_value = math::clamp(m_value + value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator-=(T value) noexcept { m_value = math::clamp(m_value - value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator*=(T value) noexcept { m_value = math::clamp(m_value * value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator/=(T value) noexcept { m_value = math::clamp(m_value / value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator+=(const this_t& value) noexcept { m_value = math::clamp(m_value + value.m_value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator-=(const this_t& value) noexcept { m_value = math::clamp(m_value - value.m_value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator*=(const this_t& value) noexcept { m_value = math::clamp(m_value * value.m_value, Traits::min, Traits::max); return *this; } +template +constexpr clamped_value& clamped_value::operator/=(const this_t& value) noexcept { m_value = math::clamp(m_value / value.m_value, Traits::min, Traits::max); return *this; } + +template +constexpr clamped_value::operator T() const noexcept { return m_value; } + +template +constexpr T clamped_value::value() const noexcept { return m_value; } + +} + +template +class eastl::numeric_limits> +{ +public: + typedef math::clamped_value value_type; + + static EA_CONSTEXPR_OR_CONST bool is_specialized = true; + static EA_CONSTEXPR_OR_CONST int digits = tl::numeric_limits::digits; + static EA_CONSTEXPR_OR_CONST int digits10 = tl::numeric_limits::digits10; + static EA_CONSTEXPR_OR_CONST int max_digits10 = tl::numeric_limits::max_digits10; + static EA_CONSTEXPR_OR_CONST bool is_signed = Traits::min < 0; + static EA_CONSTEXPR_OR_CONST bool is_integer = tl::numeric_limits::is_integer; + static EA_CONSTEXPR_OR_CONST bool is_exact = tl::numeric_limits::is_exact; + static EA_CONSTEXPR_OR_CONST int radix = tl::numeric_limits::radix; + static EA_CONSTEXPR_OR_CONST int min_exponent = tl::numeric_limits::min_exponent; + static EA_CONSTEXPR_OR_CONST int min_exponent10 = tl::numeric_limits::min_exponent10; + static EA_CONSTEXPR_OR_CONST int max_exponent = tl::numeric_limits::max_exponent; + static EA_CONSTEXPR_OR_CONST int max_exponent10 = tl::numeric_limits::max_exponent10; + static EA_CONSTEXPR_OR_CONST bool is_bounded = tl::numeric_limits::is_bounded; + static EA_CONSTEXPR_OR_CONST bool is_modulo = tl::numeric_limits::is_modulo; + static EA_CONSTEXPR_OR_CONST bool traps = tl::numeric_limits::traps; + static EA_CONSTEXPR_OR_CONST bool tinyness_before = tl::numeric_limits::tinyness_before; + static EA_CONSTEXPR_OR_CONST float_round_style round_style = tl::numeric_limits::round_style; + static EA_CONSTEXPR_OR_CONST bool has_infinity = tl::numeric_limits::has_infinity; + static EA_CONSTEXPR_OR_CONST bool has_quiet_NaN = tl::numeric_limits::has_quiet_NaN; + static EA_CONSTEXPR_OR_CONST bool has_signaling_NaN = tl::numeric_limits::has_signaling_NaN; + static EA_CONSTEXPR_OR_CONST float_denorm_style has_denorm = tl::numeric_limits::has_denorm; + static EA_CONSTEXPR_OR_CONST bool has_denorm_loss = tl::numeric_limits::has_denorm_loss; + static EA_CONSTEXPR_OR_CONST bool is_iec559 = tl::numeric_limits::is_iec559; + + static value_type min() { return value_type(tl::numeric_limits::min()); } + static value_type max() { return value_type(Traits::max); } + static value_type lowest() { return value_type(T(Traits::min)); } + static value_type epsilon() { return value_type(tl::numeric_limits::epsilon()); } + static value_type round_error() { return value_type(tl::numeric_limits::round_error()); } + static value_type infinity() { return value_type(tl::numeric_limits::infinity()); } + static value_type quiet_NaN() { return value_type(tl::numeric_limits::quiet_NaN()); } + static value_type signaling_NaN() { return value_type(tl::numeric_limits::signaling_NaN()); } + static value_type denorm_min() { return value_type(tl::numeric_limits::denorm_min()); } +}; diff --git a/include/VMath/func_binary.h b/include/VMath/func_binary.h new file mode 100644 index 0000000..c6bfdc6 --- /dev/null +++ b/include/VMath/func_binary.h @@ -0,0 +1,37 @@ +#pragma once + +namespace math +{ + //returns true if the argument is a power of 2. + // isPow2(2) == true + // isPow2(3) == false + // isPow2(16) == true + template constexpr bool is_pot(T v) noexcept; + + //returns the smallest power of 2 that is greater or equal than the argument + // getSmallestPow2(127) == 128 + // getSmallestPow2(128) == 128 + // getSmallestPow2(129) == 256 + template constexpr T get_min_pot(T v) noexcept; + + //returns the biggest power of 2 that is smaller or equal than the argument + // getSmallestPow2(127) == 64 + // getSmallestPow2(128) == 128 + // getSmallestPow2(129) == 128 + template constexpr T get_max_pot(T v) noexcept; + + //returns the log2 of the argument + // getPow2(4) == 2 + // getPow2(5) == 2 + // getPow2(7) == 2 + // getPow2(8) == 3 + // getPow2(0) == undefined + template constexpr int8_t get_pot(T v) noexcept; + + //returns the number of 1 bits in the argument + // getOnBitsCount(4) == 1 + // getOnBitsCount(5) == 2 + // getOnBitsCount(7) == 3 + template constexpr int8_t get_on_bits_count(T v) noexcept; + +} diff --git a/include/VMath/func_binary.inl b/include/VMath/func_binary.inl new file mode 100644 index 0000000..7da5607 --- /dev/null +++ b/include/VMath/func_binary.inl @@ -0,0 +1,75 @@ +namespace math +{ + + +////////////////////////////////////////////////////////////////////////// + +template +constexpr inline bool is_pot(T v) noexcept +{ + return (!is_zero(v)) && ((v & (v - 1)) == 0); +} + +template +constexpr inline T get_min_pot(T v) noexcept +{ + TL_PLAIN_ASSERT(v < ~0u); + T result = 1; + while (result < v) + result <<= 1; + + return result; +} +template +constexpr inline T get_max_pot(T v) noexcept +{ + TL_PLAIN_ASSERT(v > 0); + T result = 1; + T crt = result; + while (crt <= v) + { + result = crt; + crt <<= 1; + } + return result; +} + +template +constexpr inline int8_t get_pot(T v) noexcept +{ + TL_PLAIN_ASSERT(v > 0); + int8_t shift = -1; + while (v) + { + shift++; + v >>= 1; + } + return shift; +} + +template<> +constexpr inline int8_t get_on_bits_count(uint32_t v) noexcept +{ + // From Hacker's Delight (http://books.google.com/books?id=iBNKMspIlqEC) + v = v - ((v >> 1) & 0x55555555); + v = (v & 0x33333333) + ((v >> 2) & 0x33333333); + v = (v + (v >> 4)) & 0x0F0F0F0F; + v = v + (v >> 8); + v = v + (v >> 16); + return static_cast(v & 0x0000003F); +} + +template +constexpr inline int8_t get_on_bits_count(T v) noexcept +{ + uint8_t count = 0; + const uint8_t size = sizeof(T) * 8; + for (uint8_t i = 0; i < size && v; i++) + { + count += (v & 0x1); + v >>= 1; + } + return count; +} + +} diff --git a/include/VMath/func_cast.h b/include/VMath/func_cast.h new file mode 100644 index 0000000..c80ca6c --- /dev/null +++ b/include/VMath/func_cast.h @@ -0,0 +1,194 @@ +#pragma once + +namespace math +{ + +template +constexpr vec2 to_vec2(const vec2& v) noexcept; + +template +constexpr vec2 to_vec2(const vec3& v) noexcept; +template +constexpr vec2 to_vec2(const vec3& v) noexcept; + +template +constexpr vec2 to_vec2(const vec4& v) noexcept; +template +constexpr vec2 to_vec2(const vec4& v) noexcept; + + + + +template +constexpr vec3 to_vec3(const vec3& v) noexcept; + +template +constexpr vec3 to_vec3(const vec2& v) noexcept; +template +constexpr vec3 to_vec3(const vec2& v) noexcept; + +template +constexpr vec3 to_vec3(const vec2& v, T z) noexcept; +template +constexpr vec3 to_vec3(const vec2& v, T z) noexcept; + +template +constexpr vec3 to_vec3(const vec4& v) noexcept; +template +constexpr vec3 to_vec3(const vec4& v) noexcept; + + + + +template +constexpr vec4 to_vec4(const vec4& v) noexcept; + +template +constexpr vec4 to_vec4(const vec2& v) noexcept; +template +constexpr vec4 to_vec4(const vec2& v) noexcept; + +template +constexpr vec4 to_vec4(const vec2& v, T z, T w) noexcept; +template +constexpr vec4 to_vec4(const vec2& v, T z, T w) noexcept; + +template +constexpr vec4 to_vec4(const vec3& v) noexcept; +template +constexpr vec4 to_vec4(const vec3& v) noexcept; + +template +constexpr vec4 to_vec4(const vec3& v, T w) noexcept; +template +constexpr vec4 to_vec4(const vec3& v, T w) noexcept; + + + + +template +constexpr mat2 to_mat2(const mat2& v) noexcept; + +template +constexpr mat2 to_mat2(const mat3& v) noexcept; +template +constexpr mat2 to_mat2(const mat3& v) noexcept; + +template +constexpr mat2 to_mat2(const mat4& v) noexcept; +template +constexpr mat2 to_mat2(const mat4& v) noexcept; + + + + +template +constexpr mat3 to_mat3(const mat3& v) noexcept; + +template +constexpr mat3 to_mat3(const mat4& v) noexcept; +template +constexpr mat3 to_mat3(const mat4& v) noexcept; + +template +constexpr mat3 to_mat3(const mat2& v) noexcept; +template +constexpr mat3 to_mat3(const mat2& v) noexcept; + +template +constexpr mat3 to_mat3(const trans2& v) noexcept; +template +constexpr mat3 to_mat3(const trans2& v) noexcept; + +template +constexpr mat3 to_mat3(const quat& v) noexcept; +template +constexpr mat3 to_mat3(const quat& v) noexcept; + + + +template +constexpr mat4 to_mat4(const mat4& v) noexcept; + +template +constexpr mat4 to_mat4(const mat2& v) noexcept; +template +constexpr mat4 to_mat4(const mat2& v) noexcept; + +template +constexpr mat4 to_mat4(const mat3& v) noexcept; +template +constexpr mat4 to_mat4(const mat3& v) noexcept; + +template +constexpr mat4 to_mat4(const trans3& v) noexcept; +template +constexpr mat4 to_mat4(const trans3& v) noexcept; + + + + +template +constexpr quat to_quat(const mat3& v) noexcept; +template +constexpr quat to_quat(const mat3& v) noexcept; + + + + +template +constexpr trans2 to_trans2(const mat3& v) noexcept; +template +constexpr trans2 to_trans2(const mat3& v) noexcept; + +template +constexpr trans2 to_trans2(const trans3& v) noexcept; +template +constexpr trans2 to_trans2(const trans3& v) noexcept; + +template +constexpr trans2 to_trans2(const rigid2& v) noexcept; +template +constexpr trans2 to_trans2(const rigid2& v, const math::vec2& scale) noexcept; +template +constexpr trans2 to_trans2(const rigid2& v) noexcept; +template +constexpr trans2 to_trans2(const rigid2& v, const math::vec2& scale) noexcept; + + + + +template +constexpr trans3 to_trans3(const mat4& v) noexcept; +template +constexpr trans3 to_trans3(const mat4& v) noexcept; + +template +constexpr trans3 to_trans3(const trans2& v) noexcept; +template +constexpr trans3 to_trans3(const trans2& v) noexcept; + +template +constexpr trans3 to_trans3(const rigid3& v) noexcept; +template +constexpr trans3 to_trans3(const rigid3& v, const math::vec3& scale) noexcept; +template +constexpr trans3 to_trans3(const rigid3& v) noexcept; +template +constexpr trans3 to_trans3(const rigid3& v, const math::vec3& scale) noexcept; + + + + +template +constexpr line3 to_line3(const segment3& v) noexcept; +template +constexpr line3 to_line3(const segment3& v) noexcept; + + + +//templatic cast function, using the functions above +template +constexpr To cast(const From& v) noexcept; + +} \ No newline at end of file diff --git a/include/VMath/func_cast.inl b/include/VMath/func_cast.inl new file mode 100644 index 0000000..daafe53 --- /dev/null +++ b/include/VMath/func_cast.inl @@ -0,0 +1,481 @@ +namespace math +{ + +template inline constexpr vec2 to_vec2(const vec2& v) noexcept { return vec2(T(v.x), T(v.y)); } +template inline constexpr vec2 to_vec2(const vec3& v) noexcept { return to_vec2(v); } +template inline constexpr vec2 to_vec2(const vec3& v) noexcept { return vec2(T(v.x), T(v.y)); } +template inline constexpr vec2 to_vec2(const vec4& v) noexcept { return to_vec2(v); } +template inline constexpr vec2 to_vec2(const vec4& v) noexcept { return vec2(T(v.x), T(v.y)); } + +////////////////////////////////////////////////////////////////////////// + +template inline constexpr vec3 to_vec3(const vec3& v) noexcept { return vec3(T(v.x), T(v.y), T(v.z)); } +template inline constexpr vec3 to_vec3(const vec2& v) noexcept { return to_vec3(v); } +template inline constexpr vec3 to_vec3(const vec2& v) noexcept { return vec3(T(v.x), T(v.y), 0); } +template inline constexpr vec3 to_vec3(const vec2& v, T z) noexcept { return to_vec3(v, z); } +template inline constexpr vec3 to_vec3(const vec2& v, T z) noexcept { return vec3(T(v.x), T(v.y), T(z)); } +template inline constexpr vec3 to_vec3(const vec4& v) noexcept { return to_vec3(v); } +template inline constexpr vec3 to_vec3(const vec4& v) noexcept { return vec3(T(v.x), T(v.y), T(v.z)); } + +////////////////////////////////////////////////////////////////////////// + +template inline constexpr vec4 to_vec4(const vec4& v) noexcept { return vec4(T(v.x), T(v.y), T(v.z), T(v.w)); } +template inline constexpr vec4 to_vec4(const vec2& v) noexcept { return to_vec4(v); } +template inline constexpr vec4 to_vec4(const vec2& v) noexcept { return vec4(T(v.x), T(v.y), T(0), T(0)); } +template inline constexpr vec4 to_vec4(const vec2& v, T z, T w) noexcept { return to_vec4(v, z, w); } +template inline constexpr vec4 to_vec4(const vec2& v, T z, T w) noexcept { return vec4(T(v.x), T(v.y), z, w); } +template inline constexpr vec4 to_vec4(const vec3& v) noexcept { return to_vec4(v); } +template inline constexpr vec4 to_vec4(const vec3& v) noexcept { return vec4(T(v.x), T(v.y), T(v.z), T(0)); } +template inline constexpr vec4 to_vec4(const vec3& v, T w) noexcept { return to_vec4(v, w); } +template inline constexpr vec4 to_vec4(const vec3& v, T w) noexcept { return vec4(T(v.x), T(v.y), T(v.z), w); } + +////////////////////////////////////////////////////////////////////////// + +template +inline constexpr mat2 to_mat2(const mat2& v) noexcept +{ + return mat2(to_vec2(v.column0), + to_vec2(v.column1)); +} + +template +inline constexpr mat2 to_mat2(const mat3& v) noexcept +{ + const T* m = v.data(); + return + { + T(m[0]), T(m[1]), + T(m[3]), T(m[4]), + }; +} +template inline constexpr mat2 to_mat2(const mat3& v) noexcept { return to_mat2(v); } + +template +inline constexpr mat2 to_mat2(const mat4& v) noexcept +{ + return to_mat2(to_mat3(v)); +} +template inline constexpr mat2 to_mat2(const mat4& v) noexcept { return to_mat2(v); } + +////////////////////////////////////////////////////////////////////////// + +template +inline constexpr mat3 to_mat3(const mat3& v) noexcept +{ + return mat3(to_vec3(v.column0), + to_vec3(v.column1), + to_vec3(v.column2)); +} + +template +inline constexpr mat3 to_mat3(const mat4& v) noexcept +{ + const T* m = v.data(); + return + { + T(m[0]), T(m[1]), T(m[2]), + T(m[4]), T(m[5]), T(m[6]), + T(m[8]), T(m[9]), T(m[10]) + }; +} +template inline constexpr mat3 to_mat3(const mat4& v) noexcept { return to_mat3(v); } + +template +inline constexpr mat3 to_mat3(const mat2& v) noexcept +{ + const U* m = v.data(); + return + { + T(m[2]), T(m[3]), T(0), + T(m[0]), T(m[1]), T(0), + T(0), T(0), T(1), + }; +} +template inline constexpr mat3 to_mat3(const mat2& v) noexcept { return to_mat3(v); } + +template +inline constexpr mat3 to_mat3(const trans2& trans) noexcept +{ + const U* m = trans.rotation_scale.data(); + return + { + T(m[0]), T(m[1]), T(0), + T(m[2]), T(m[3]), T(0), + T(trans.translation.x), T(trans.translation.y), T(1) + }; +} +template inline constexpr mat3 to_mat3(const trans2& trans) noexcept { return to_mat3(trans); } + +template +inline constexpr mat3 to_mat3(const quat& rotation) noexcept +{ + T x = T(rotation.x); + T y = T(rotation.y); + T z = T(rotation.z); + T w = T(rotation.w); + + T const x2 = x * T(2); + T const y2 = y * T(2); + T const z2 = z * T(2); + T const xx2 = x2 * x; + T const yy2 = y2 * y; + T const zz2 = z2 * z; + T const x2y = x2 * y; + T const x2z = x2 * z; + T const x2w = x2 * w; + T const y2z = y2 * z; + T const y2w = y2 * w; + T const z2w = z2 * w; + + return + { + 1 - yy2 - zz2, x2y + z2w, x2z - y2w, + x2y - z2w, 1 - xx2 - zz2, y2z + x2w, + x2z + y2w, y2z - x2w, 1 - yy2 - xx2, + }; +} +template inline constexpr mat3 to_mat3(const quat& rotation) noexcept { return to_mat3(rotation); } + +template +inline constexpr trans2 to_trans2(const mat3& mat) noexcept +{ + trans2 result(uninitialized); + T* tr = result.rotation_scale.data(); + const U* m = mat.data(); + + tr[0] = T(m[0]); + tr[1] = T(m[1]); + TL_PLAIN_ASSERT(is_zero(m[2], U(0.00001))); + + tr[2] = T(m[3]); + tr[3] = T(m[4]); + TL_PLAIN_ASSERT(is_zero(m[5], U(0.00001))); + + result.translation = { T(m[6]), T(m[7]) }; + TL_PLAIN_ASSERT(is_one(m[8], U(0.00001))); + + return result; +} +template inline constexpr trans2 to_trans2(const mat3& mat) noexcept { return to_trans2(mat); } + +template +inline constexpr trans2 to_trans2(const trans3& v) noexcept +{ + trans2 result(uninitialized); + T* tr = result.rotation_scale.data(); + const U* m = v.rotation_scale.data(); + + tr[0] = T(m[0]); + tr[1] = T(m[1]); + TL_PLAIN_ASSERT(is_zero(m[2], U(0.00001))); + + tr[2] = T(m[3]); + tr[3] = T(m[4]); + TL_PLAIN_ASSERT(is_zero(m[5], U(0.00001))); + + TL_PLAIN_ASSERT(is_zero(m[6], U(0.00001))); + TL_PLAIN_ASSERT(is_zero(m[7], U(0.00001))); + TL_PLAIN_ASSERT(is_one(m[8], U(0.00001))); + + result.translation = { T(v.translation.x), T(v.translation.y) }; + TL_PLAIN_ASSERT(is_zero(v.translation.z, U(0.00001))); + + return result; +} +template inline constexpr trans2 to_trans2(const trans3& v) noexcept { return to_trans2(v); } + + +template +constexpr trans2 to_trans2(const rigid2& v) noexcept +{ + return trans2(to_vec2(v.translation), static_cast(v.rotation)); +} +template +constexpr trans2 to_trans2(const rigid2& v, const math::vec2& scale) noexcept +{ + return trans2(to_vec2(v.translation), static_cast(v.rotation), to_vec2(scale)); +} +template constexpr trans2 to_trans2(const rigid2& v) noexcept +{ + return trans2(v.translation, v.rotation); +} +template constexpr trans2 to_trans2(const rigid2& v, const math::vec2& scale) noexcept +{ + return trans2(v.translation, v.rotation, scale); +} + +////////////////////////////////////////////////////////////////////////// + +template +inline constexpr mat4 to_mat4(const mat4& v) noexcept +{ + return mat4(to_vec4(v.column0), + to_vec4(v.column1), + to_vec4(v.column2), + to_vec4(v.column3)); +} + +template +inline constexpr mat4 to_mat4(const mat3& v) noexcept +{ + const U* m = v.data(); + return + { + T(m[0]), T(m[1]), T(m[2]), T(0), + T(m[3]), T(m[4]), T(m[5]), T(0), + T(m[6]), T(m[7]), T(m[8]), T(0), + T(0), T(0), T(0), T(1), + }; +} +template inline constexpr mat4 to_mat4(const mat3& v) noexcept { return to_mat4(v); } + +template +inline constexpr mat4 to_mat4(const mat2& v) noexcept +{ + return to_mat4(to_mat3(v)); +} +template inline constexpr mat4 to_mat4(const mat2& v) noexcept { return to_mat4(v); } + + +template +constexpr inline quat to_quat(mat3 const& mat) noexcept +{ + //from: https://en.wikipedia.org/wiki/Rotation_matrix + // "an M with negative determinant has no uniquely defined closest rotation matrix." + TL_PLAIN_ASSERT(mat.determinant() >= 0); + + T x = 0; + T y = 0; + T z = 0; + T w = 0; + T trace = T(mat.element(0, 0) + mat.element(1, 1) + mat.element(2, 2)); + if (trace > 0) + { + // |w| > 1/2, may as well choose w > 1/2 + T fRoot = sqrt(trace + 1); // 2w + w = T(0.5) * fRoot; + fRoot = T(0.5) / fRoot; // 1/(4w) + x = T(mat.element(1, 2) - mat.element(2, 1)) * fRoot; + y = T(mat.element(2, 0) - mat.element(0, 2)) * fRoot; + z = T(mat.element(0, 1) - mat.element(1, 0)) * fRoot; + } + else + { + // |w| <= 1/2 + int i = 0; + if (mat.element(2, 2) > mat.element(i, i)) + i = 2; + else if (mat.element(1, 1) > mat.element(0, 0)) + i = 1; + + int j = (i + 1) % 3; + int k = (j + 1) % 3; + + T fRoot = sqrt(T(mat.element(i, i) - mat.element(j, j) - mat.element(k, k) + T(1))); + T* apfQuat[3] = { &x, &y, &z }; + *apfQuat[i] = T(0.5) * fRoot; + fRoot = T(0.5) / fRoot; + w = T(mat.element(j, k) - mat.element(k, j)) * fRoot; + *apfQuat[j] = T(mat.element(i, j) + mat.element(j, i)) * fRoot; + *apfQuat[k] = T(mat.element(i, k) + mat.element(k, i)) * fRoot; + } + return { x, y, z, w }; +} +template constexpr inline quat to_quat(mat3 const& mat) noexcept { return to_quat(mat); } + + +template +inline constexpr mat4 to_mat4(const trans3& trans) noexcept +{ + const U* m = trans.rotation_scale.data(); + return + { + T(m[0]), T(m[1]), T(m[2]), T(0), + T(m[3]), T(m[4]), T(m[5]), T(0), + T(m[6]), T(m[7]), T(m[8]), T(0), + trans.translation.x, trans.translation.y, trans.translation.z, T(1), + }; +} +template inline constexpr mat4 to_mat4(const trans3& trans) noexcept { return to_mat4(trans); } + +template +inline constexpr trans3 to_trans3(const mat4& mat) noexcept +{ + trans3 result(uninitialized); + T* tr = result.rotation_scale.data(); + const U* m = mat.data(); + + tr[0] = T(m[0]); + tr[1] = T(m[1]); + tr[2] = T(m[2]); + TL_PLAIN_ASSERT(is_zero(m[3], U(0.00001))); + + tr[3] = T(m[4]); + tr[4] = T(m[5]); + tr[5] = T(m[6]); + TL_PLAIN_ASSERT(is_zero(m[7], U(0.00001))); + + tr[6] = T(m[8]); + tr[7] = T(m[9]); + tr[8] = T(m[10]); + TL_PLAIN_ASSERT(is_zero(m[11], U(0.00001))); + + result.translation = { T(m[12]), T(m[13]), T(m[14]) }; + TL_PLAIN_ASSERT(is_one(m[15], U(0.00001))); + + return result; +} +template inline constexpr trans3 to_trans3(const mat4& mat) noexcept { return to_trans3(mat); } + +template +inline constexpr trans3 to_trans3(const trans2& v) noexcept +{ + trans3 result(uninitialized); + T* tr = result.rotation_scale.data(); + const U* m = v.rotation_scale.data(); + + tr[0] = T(m[0]); + tr[1] = T(m[1]); + tr[2] = T(0); + + tr[3] = T(m[2]); + tr[4] = T(m[3]); + tr[5] = T(0); + + tr[6] = T(0); + tr[7] = T(0); + tr[8] = T(1); + + result.translation = { T(v.translation.x), T(v.translation.y), 0 }; + return result; +} +template inline constexpr trans3 to_trans3(const trans2& v) noexcept { return to_trans3(v); } + +template +constexpr trans3 to_trans3(const rigid3& v) noexcept +{ + return trans3(to_vec3(v.translation), static_cast(v.rotation)); +} +template +constexpr trans3 to_trans3(const rigid3& v, const math::vec3& scale) noexcept +{ + return trans3(to_vec3(v.translation), static_cast(v.rotation), to_vec3(scale)); +} +template constexpr trans3 to_trans3(const rigid3& v) noexcept +{ + return trans3(v.translation, v.rotation); +} +template constexpr trans3 to_trans3(const rigid3& v, const math::vec3& scale) noexcept +{ + return trans3(v.translation, v.rotation, scale); +} + +////////////////////////////////////////////////////////////////////////// + +template +inline constexpr line3 to_line3(const segment3& v) noexcept +{ + return { to_vec3(v.start), to_vec3(v.get_direction()) }; +} +template inline constexpr line3 to_line3(const segment3& v) noexcept { return to_line3(v); } + + +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////// + +namespace detail +{ + +template +struct caster +{ + To operator()(const From& v) const { return static_cast(v); } +}; + + +template +struct caster> +{ + T operator()(const vec2& v) const { return v[0]; } +}; +template +struct caster> +{ + T operator()(const vec3& v) const { return v[0]; } +}; +template +struct caster> +{ + T operator()(const vec4& v) const { return v[0]; } +}; + + +template +struct caster, From> +{ + vec2 operator()(const From& v) const { return to_vec2(v); } +}; +template +struct caster, From> +{ + vec3 operator()(const From& v) const { return to_vec3(v); } +}; +template +struct caster, From> +{ + vec4 operator()(const From& v) const { return to_vec4(v); } +}; +template +struct caster, From> +{ + quat operator()(const From& v) const { return to_quat(v); } +}; + + +template +struct caster, From> +{ + mat2 operator()(const From& v) const { return to_mat2(v); } +}; +template +struct caster, From> +{ + mat3 operator()(const From& v) const { return to_mat3(v); } +}; +template +struct caster, From> +{ + mat4 operator()(const From& v) const { return to_mat4(v); } +}; + + +template +struct caster, From> +{ + trans2 operator()(const From& v) const { return to_trans2(v); } +}; +template +struct caster, From> +{ + trans3 operator()(const From& v) const { return to_trans3(v); } +}; + + +template +struct caster, From> +{ + line3 operator()(const From& v) const { return to_line3(v); } +}; + +} + + +template inline constexpr To cast(const From& v) noexcept +{ + return detail::caster()(v); +} +//template inline constexpr T cast(const T& v) noexcept { return v; } + +} \ No newline at end of file diff --git a/include/VMath/func_common.h b/include/VMath/func_common.h new file mode 100644 index 0000000..6eb5272 --- /dev/null +++ b/include/VMath/func_common.h @@ -0,0 +1,167 @@ +#pragma once + +#include + +namespace math +{ +////////////////////////////////////////////////////////////////////////// + +template constexpr T epsilon() noexcept; + +template inline T abs(T v) noexcept; +template<> inline uint8_t abs(uint8_t v) noexcept; +template<> inline uint16_t abs(uint16_t v) noexcept; +template<> inline uint32_t abs(uint32_t v) noexcept; +template<> inline uint64_t abs(uint64_t v) noexcept; +template<> inline int64_t abs(int64_t a) noexcept; +template<> inline float abs(float v) noexcept; +template angle abs(angle const& v) noexcept; +template vec2 abs(vec2 const& v) noexcept; +template vec3 abs(vec3 const& v) noexcept; +template vec4 abs(vec4 const& v) noexcept; + +template constexpr T sgn(T const& v) noexcept; +template constexpr T sgn(angle const& v) noexcept; +template constexpr vec2 sgn(vec2 const& v) noexcept; +template constexpr vec3 sgn(vec3 const& v) noexcept; +template constexpr vec4 sgn(vec4 const& v) noexcept; + +template inline T sqrt(T const& v) noexcept; +template<> inline float sqrt(float const& v) noexcept; +template<> inline float sqrt(float const& v) noexcept; +template<> inline float sqrt(float const& v) noexcept; +template<> inline double sqrt(double const& v) noexcept; +template<> inline double sqrt(double const& v) noexcept; +template<> inline double sqrt(double const& v) noexcept; +template inline vec2 sqrt(vec2 const& v) noexcept; +template inline vec3 sqrt(vec3 const& v) noexcept; +template inline vec4 sqrt(vec4 const& v) noexcept; + +template T inv_sqrt(T const& v) noexcept; +template<> inline float inv_sqrt(float const& v) noexcept; +template<> inline float inv_sqrt(float const& v) noexcept; +template<> inline double inv_sqrt(double const& v) noexcept; +template vec2 inv_sqrt(vec2 const& v) noexcept; +template vec3 inv_sqrt(vec3 const& v) noexcept; +template vec4 inv_sqrt(vec4 const& v) noexcept; + +template constexpr bool inverse(T const& v, T& res) noexcept; +template constexpr rigid2 inverse(const rigid2& v) noexcept; //cannot fail +template constexpr rigid3 inverse(const rigid3& v) noexcept; //cannot fail +template constexpr quat inverse(const quat& v) noexcept; //cannot fail +template constexpr bool invert(T& v) noexcept; +template constexpr bool invert(mat2& v) noexcept; +template constexpr bool invert(mat3& v) noexcept; +template constexpr bool invert32(mat3& v) noexcept; //suitable for when the last row is 0 0 1 +template constexpr bool invert(mat4& v) noexcept; +template constexpr bool invert43(mat4& v) noexcept; //suitable for when the last row is 0 0 0 1 +template constexpr bool invert(trans2& v) noexcept; +template constexpr bool invert(trans3& v) noexcept; +template constexpr bool invert(rigid2& v) noexcept; +template constexpr bool invert(rigid3& v) noexcept; +template constexpr bool invert(quat& v) noexcept; + +template constexpr T transposed(T const& v) noexcept; +template constexpr void transpose(mat2& v) noexcept; +template constexpr void transpose(mat3& v) noexcept; +template constexpr void transpose(mat4& v) noexcept; + +template inline T pow(T const& a, T const& b) noexcept; +template<> inline float pow(float const& a, float const& b) noexcept; +template<> inline float pow(float const& a, float const& b) noexcept; +template<> inline float pow(float const& a, float const& b) noexcept; +template<> inline double pow(double const& a, double const& b) noexcept; +template<> inline double pow(double const& a, double const& b) noexcept; +template<> inline double pow(double const& a, double const& b) noexcept; +template inline vec2 pow(vec2 const& a, vec2 const& b) noexcept; +template inline vec3 pow(vec3 const& a, vec3 const& b) noexcept; +template inline vec4 pow(vec4 const& a, vec4 const& b) noexcept; + +template constexpr T positive_zero(T const& v) noexcept; +template<> constexpr float positive_zero(float const& v) noexcept; +template constexpr angle positive_zero(angle const& v) noexcept; +template constexpr vec2 positive_zero(vec2 const& v) noexcept; +template constexpr vec3 positive_zero(vec3 const& v) noexcept; +template constexpr vec4 positive_zero(vec4 const& v) noexcept; +template constexpr quat positive_zero(quat const& v) noexcept; + +template constexpr T dot(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr T dot(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr T dot(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr T dot(quat const& v1, quat const& v2) noexcept; + +template constexpr T cross(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr vec3 cross(vec3 const& v1, vec3 const& v2) noexcept; +template vec4 cross(vec4 const& x1, vec4 const& x2, vec4 const& x3) noexcept; + +template constexpr T length_sq(vec2 const& v) noexcept; +template constexpr T length_sq(vec3 const& v) noexcept; +template constexpr T length_sq(vec4 const& v) noexcept; +template constexpr T length_sq(quat const& v) noexcept; + +template constexpr T length(vec2 const& v) noexcept; +template constexpr T length(vec3 const& v) noexcept; +template constexpr T length(vec4 const& v) noexcept; +template constexpr T length(quat const& v) noexcept; + +template constexpr void set_length(vec2& v, T length) noexcept; +template constexpr void set_length(vec3& v, T length) noexcept; +template constexpr void set_length(vec4& v, T length) noexcept; + +template constexpr angle normalized(angle const& v) noexcept; +template constexpr vec2 normalized(vec2 const& v) noexcept; +template constexpr vec3 normalized(vec3 const& v) noexcept; +template constexpr vec4 normalized(vec4 const& v) noexcept; +template constexpr quat normalized(quat const& v) noexcept; + +template constexpr T square(T const& v) noexcept; + +template constexpr T distance(T const& v1, T const& v2) noexcept; +template constexpr angle distance(angle const& v1, angle const& v2) noexcept; +template constexpr T distance(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr T distance(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr T distance(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr T distance(line3 const& l1, line3 const& l2) noexcept; + +template constexpr T signed_distance(T const& v1, T const& v2) noexcept; +template constexpr angle signed_distance(angle const& v1, angle const& v2) noexcept; +template constexpr T signed_distance(plane const& p, vec3 const& v) noexcept; +template constexpr T signed_distance(vec3 const& v, plane const& p) noexcept; + +template constexpr T distance_sq(T const& v1, T const& v2) noexcept; +template constexpr T distance_sq(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr T distance_sq(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr T distance_sq(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr T distance_sq(line3 const& l1, line3 const& l2) noexcept; + +namespace cwise +{ + +template constexpr T min_component(vec2 const& v) noexcept; +template constexpr T min_component(vec3 const& v) noexcept; +template constexpr T min_component(vec4 const& v) noexcept; +template constexpr size_t min_component_index(vec2 const& v) noexcept; +template constexpr size_t min_component_index(vec3 const& v) noexcept; +template constexpr size_t min_component_index(vec4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr T max_component(vec2 const& v) noexcept; +template constexpr T max_component(vec3 const& v) noexcept; +template constexpr T max_component(vec4 const& v) noexcept; +template constexpr size_t max_component_index(vec2 const& v) noexcept; +template constexpr size_t max_component_index(vec3 const& v) noexcept; +template constexpr size_t max_component_index(vec4 const& v) noexcept; + +} + +namespace batch +{ +template void dot(T* dst, size_t dstStride, vec3 const* src1, size_t src1Stride, vec3 const* src2, size_t src2Stride, size_t count) noexcept; +template void dot(T* dst, size_t dstStride, vec4 const* src1, size_t src1Stride, vec4 const* src2, size_t src2Stride, size_t count) noexcept; +} + +} + +//#include "func_common.hpp" + diff --git a/include/VMath/func_common.inl b/include/VMath/func_common.inl new file mode 100644 index 0000000..63a4b4a --- /dev/null +++ b/include/VMath/func_common.inl @@ -0,0 +1,1067 @@ +#pragma once + +#include + +//#define CHECK_BAD_ARGS + +#ifdef CHECK_BAD_ARGS +# define ASSERT_BAD_ARGS(x) TL_PLAIN_ASSERT(x) +#else +# define ASSERT_BAD_ARGS(x) +#endif +namespace math +{ + +////////////////////////////////////////////////////////////////////////// + +template constexpr T epsilon() noexcept +{ + return tl::numeric_limits::epsilon(); +} + +////////////////////////////////////////////////////////////////////////// + +template T abs(T v) noexcept +{ + ASSERT_BAD_ARGS(!is_nan(v)); + return v < 0 ? -v : v; +} +template<> inline uint8_t abs(uint8_t v) noexcept +{ + return v; +} +template<> inline uint16_t abs(uint16_t v) noexcept +{ + return v; +} +template<> inline uint32_t abs(uint32_t v) noexcept +{ + return v; +} +template<> inline uint64_t abs(uint64_t v) noexcept +{ + return v; +} +template<> inline int64_t abs(int64_t a) noexcept +{ + return a < 0 ? -a : a; +} +template<> inline float abs(float v) noexcept +{ + ASSERT_BAD_ARGS(!is_nan(v)); + const uint32_t v2 = reinterpret_cast(v); + const uint32_t a = v2 & 0x7FFFFFFF; + return reinterpret_cast(a); +} +template angle abs(angle const& v) noexcept +{ + return angle(abs(v.radians)); +} +template vec2 abs(vec2 const& v) noexcept +{ + return vec2(abs(v.x), abs(v.y)); +} +template vec3 abs(vec3 const& v) noexcept +{ + return vec3(abs(v.x), abs(v.y), abs(v.z)); +} +template vec4 abs(vec4 const& v) noexcept +{ + return vec4(abs(v.x), abs(v.y), abs(v.z), abs(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T sgn(T const& v) noexcept +{ + ASSERT_BAD_ARGS(!is_nan(v)); + return v < T(0) ? T(-1) : v == T(0) ? T(0) : T(1); +} +template constexpr T sgn(angle const& v) noexcept +{ + return sgn(v.radians); +} +template constexpr vec2 sgn(vec2 const& v) noexcept +{ + return vec2(sgn(v.x), sgn(v.y)); +} +template constexpr vec3 sgn(vec3 const& v) noexcept +{ + return vec3(sgn(v.x), sgn(v.y), sgn(v.z)); +} +template constexpr vec4 sgn(vec4 const& v) noexcept +{ + return vec4(sgn(v.x), sgn(v.y), sgn(v.z), sgn(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template +inline T sqrt(T const& v) noexcept +{ + return (T)std::sqrt(v); +} + +template<> inline float sqrt(float const& v) noexcept +{ + ASSERT_BAD_ARGS(!(is_negative(v) || !is_finite(v))); + return ::sqrtf(v); +} +template<> inline float sqrt(float const& v) noexcept +{ + ASSERT_BAD_ARGS(!(is_negative(v) || !is_finite(v))); + return ::sqrtf(is_negative(v) || !is_finite(v) ? 0.f : v); +} +template<> inline float sqrt(float const& v) noexcept +{ + ASSERT_BAD_ARGS(!(is_negative(v) || !is_finite(v))); + float const x = v; + float const xhalf = 0.5f * x; + union // get bits for floating value + { + float x; + int i; + } u; + u.x = x; + u.i = 0x5f3759df - (u.i >> 1); // gives initial guess y0 + return x * u.x * (1.5f - xhalf * u.x * u.x);// Newton step, repeating increases accuracy +} + +template<> inline double sqrt(double const& v) noexcept +{ + ASSERT_BAD_ARGS(!(is_negative(v) || !is_finite(v))); return ::sqrt(v); +} +template<> inline double sqrt(double const& v) noexcept +{ + ASSERT_BAD_ARGS(!(is_negative(v) || !is_finite(v))); + return ::sqrt(v); +} +template<> inline double sqrt(double const& v) noexcept +{ + ASSERT_BAD_ARGS(!(is_negative(v) || !is_finite(v))); + return ::sqrt(is_negative(v) || !is_finite(v) ? 0 : v); +} + +template inline vec2 sqrt(vec2 const& v) noexcept +{ + return vec2(sqrt(v.x), sqrt(v.y)); +} +template inline vec3 sqrt(vec3 const& v) noexcept +{ + return vec3(sqrt(v.x), sqrt(v.y), sqrt(v.z)); +} +template inline vec4 sqrt(vec4 const& v) noexcept +{ + return vec4(sqrt(v.x), sqrt(v.y), sqrt(v.z), sqrt(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template T inv_sqrt(T const& v) noexcept +{ + ASSERT_BAD_ARGS(!is_zero(v) || !is_infinite(v)); + return T(1) / sqrt(v); +} +template<> inline float inv_sqrt(float const& v) noexcept +{ + ASSERT_BAD_ARGS(!is_zero(v) || !is_infinite(v)); + const float xhalf = 0.5f * v; + int i = *(int*)&v; + i = 0x5f3759df - (i >> 1); + float r = reinterpret_cast(i); + r = r * (1.5f - xhalf * r * r); + return r; +} +template<> inline float inv_sqrt(float const& v) noexcept +{ + ASSERT_BAD_ARGS(!is_zero(v) || !is_infinite(v)); + return is_zero(v) || !is_finite(v) ? tl::numeric_limits::max() : 1.f / sqrt(v); +} +template<> inline double inv_sqrt(double const& v) noexcept +{ + ASSERT_BAD_ARGS(!is_zero(v) || !is_infinite(v)); + return is_zero(v) || !is_finite(v) ? tl::numeric_limits::max() : 1.0 / sqrt(v); +} +template vec2 inv_sqrt(vec2 const& v) noexcept +{ + return vec2(inv_sqrt(v.x), inv_sqrt(v.y)); +} +template vec3 inv_sqrt(vec3 const& v) noexcept +{ + return vec3(inv_sqrt(v.x), inv_sqrt(v.y), inv_sqrt(v.z)); +} +template vec4 inv_sqrt(vec4 const& v) noexcept +{ + return vec4(inv_sqrt(v.x), inv_sqrt(v.y), inv_sqrt(v.z), inv_sqrt(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool inverse(T const& v, T& res) noexcept +{ + res = v; + return invert(res); +} +template constexpr rigid2 inverse(const rigid2& v) noexcept +{ + auto res = v; + (void)invert(res); //cannot fail + return res; +} +template constexpr rigid3 inverse(const rigid3& v) noexcept +{ + auto res = v; + (void)invert(res); //cannot fail + return res; +} +template constexpr quat inverse(const quat& v) noexcept +{ + auto res = v; + (void)invert(res); //cannot fail + return res; +} +template constexpr bool invert(T& v) noexcept +{ + if (is_zero(v) || !is_finite(v)) + return false; + v = T(1) / v; + return true; +} +template constexpr bool invert(mat2& v) noexcept +{ + const T* m = v.data(); + const T sav = m[0]; + T div = (m[0] * m[3] - m[1] * m[2]); + if (!invert(div)) + return false; + + v = mat2(div * m[3], -div * m[1], -div * m[2], div * sav); + return true; +} +template constexpr bool invert(mat3& v) noexcept +{ + // http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl + // Invert a 3x3 using cofactors. This is faster than using a generic + // Gaussian elimination because of the loop overhead of such a method. + + const T* m = v.data(); + const T inv[9] = + { + m[4] * m[8] - m[5] * m[7], + m[2] * m[7] - m[1] * m[8], + m[1] * m[5] - m[2] * m[4], + m[5] * m[6] - m[3] * m[8], + m[0] * m[8] - m[2] * m[6], + m[2] * m[3] - m[0] * m[5], + m[3] * m[7] - m[4] * m[6], + m[1] * m[6] - m[0] * m[7], + m[0] * m[4] - m[1] * m[3], + }; + + const T det = + m[0] * inv[0] + + m[1] * inv[3] + + m[2] * inv[6]; + + T invDet = {}; + if (!inverse(det, invDet)) + return false; + + v = + { + inv[0] * invDet, + inv[1] * invDet, + inv[2] * invDet, + inv[3] * invDet, + inv[4] * invDet, + inv[5] * invDet, + inv[6] * invDet, + inv[7] * invDet, + inv[8] * invDet, + }; + + return true; +} +template constexpr bool invert32(mat3& v) noexcept +{ + // http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl + // Invert a 3x3 using cofactors. This is faster than using a generic + // Gaussian elimination because of the loop overhead of such a method. + + const T* m = v.data(); + const T inv[9] = + { + m[4], + -m[1], + 0, + -m[3], + m[0], + 0, + m[3] * m[7] - m[4] * m[6], + m[1] * m[6] - m[0] * m[7], + m[0] * m[4] - m[1] * m[3], + }; + + const T det = + m[0] * inv[0] + + m[1] * inv[3]; + + T invDet = {}; + if (!inverse(det, invDet)) + return false; + + v = + { + inv[0] * invDet, + inv[1] * invDet, + 0, + inv[3] * invDet, + inv[4] * invDet, + 0, + inv[6] * invDet, + inv[7] * invDet, + inv[8] * invDet, + }; + + return true; +} +template constexpr bool invert(mat4& v) noexcept +{ + const T* m = v.data(); + const T t0 = m[10] * m[15] - m[11] * m[14]; + const T t1 = m[6] * m[15] - m[7] * m[14]; + const T t2 = m[6] * m[11] - m[7] * m[10]; + const T t3 = m[2] * m[15] - m[3] * m[14]; + const T t4 = m[2] * m[11] - m[3] * m[10]; + const T t5 = m[2] * m[7] - m[3] * m[6]; + + const T t6 = m[8] * m[13] - m[9] * m[12]; + const T t7 = m[4] * m[13] - m[5] * m[12]; + const T t8 = m[4] * m[9] - m[5] * m[8]; + const T t9 = m[0] * m[13] - m[1] * m[12]; + const T t10 = m[0] * m[9] - m[1] * m[8]; + const T t11 = m[0] * m[5] - m[1] * m[4]; + + const T det = t0 * t11 - t1 * t10 + t2 * t9 + t3 * t8 - t4 * t7 + t5 * t6; + T invDet = {}; + if (!inverse(det, invDet)) + return false; + + const T ft0 = (T)(t0 * invDet); + const T ft1 = (T)(t1 * invDet); + const T ft2 = (T)(t2 * invDet); + const T ft3 = (T)(t3 * invDet); + const T ft4 = (T)(t4 * invDet); + const T ft5 = (T)(t5 * invDet); + const T ft6 = (T)(t6 * invDet); + const T ft7 = (T)(t7 * invDet); + const T ft8 = (T)(t8 * invDet); + const T ft9 = (T)(t9 * invDet); + const T ft10 = (T)(t10 * invDet); + const T ft11 = (T)(t11 * invDet); + + v = + { + m[5] * ft0 - m[9] * ft1 + m[13] * ft2, + m[9] * ft3 - m[1] * ft0 - m[13] * ft4, + m[1] * ft1 - m[5] * ft3 + m[13] * ft5, + m[5] * ft4 - m[1] * ft2 - m[9] * ft5, + + m[8] * ft1 - m[4] * ft0 - m[12] * ft2, + m[0] * ft0 - m[8] * ft3 + m[12] * ft4, + m[4] * ft3 - m[0] * ft1 - m[12] * ft5, + m[0] * ft2 - m[4] * ft4 + m[8] * ft5, + + m[7] * ft6 - m[11] * ft7 + m[15] * ft8, + m[11] * ft9 - m[3] * ft6 - m[15] * ft10, + m[3] * ft7 - m[7] * ft9 + m[15] * ft11, + m[7] * ft10 - m[3] * ft8 - m[11] * ft11, + + m[10] * ft7 - m[6] * ft6 - m[14] * ft8, + m[2] * ft6 - m[10] * ft9 + m[14] * ft10, + m[6] * ft9 - m[2] * ft7 - m[14] * ft11, + m[2] * ft8 - m[6] * ft10 + m[10] * ft11, + }; + return true; +} +template constexpr bool invert43(mat4& v) noexcept +{ + const T* m = v.data(); + const T t0 = m[10]; + const T t1 = m[6]; + const T t3 = m[2]; + const T t6 = m[8] * m[13] - m[9] * m[12]; + const T t7 = m[4] * m[13] - m[5] * m[12]; + const T t8 = m[4] * m[9] - m[5] * m[8]; + const T t9 = m[0] * m[13] - m[1] * m[12]; + const T t10 = m[0] * m[9] - m[1] * m[8]; + const T t11 = m[0] * m[5] - m[1] * m[4]; + + const T det = t0 * t11 - t1 * t10 + t3 * t8; + T invDet = {}; + if (!inverse(det, invDet)) + return false; + + const T ft0 = (T)(t0 * invDet); + const T ft1 = (T)(t1 * invDet); + const T ft3 = (T)(t3 * invDet); + const T ft6 = (T)(t6 * invDet); + const T ft7 = (T)(t7 * invDet); + const T ft8 = (T)(t8 * invDet); + const T ft9 = (T)(t9 * invDet); + const T ft10 = (T)(t10 * invDet); + const T ft11 = (T)(t11 * invDet); + + v = + { + m[5] * ft0 - m[9] * ft1, + m[9] * ft3 - m[1] * ft0, + m[1] * ft1 - m[5] * ft3, + 0, + + m[8] * ft1 - m[4] * ft0, + m[0] * ft0 - m[8] * ft3, + m[4] * ft3 - m[0] * ft1, + 0, + + ft8, + -ft10, + ft11, + 0, + + m[10] * ft7 - m[6] * ft6 - m[14] * ft8, + m[2] * ft6 - m[10] * ft9 + m[14] * ft10, + m[6] * ft9 - m[2] * ft7 - m[14] * ft11, + m[2] * ft8 - m[6] * ft10 + m[10] * ft11, + }; + return true; +} +template constexpr bool invert(trans2& v) noexcept +{ + mat3 m = to_mat3(v); + if (!invert32(m)) + return false; + v = to_trans2(m); + return true; +} +template constexpr bool invert(trans3& v) noexcept +{ + mat4 m = to_mat4(v); + if (!invert43(m)) + return false; + v = to_trans3(m); + return true; +} +template constexpr bool invert(rigid2& v) noexcept +{ + const T inv_angle = -v.rotation; + const vec2 inv_translation = -math::rotate(inv_angle, v.translation); + v = rigid2(inv_translation, inv_angle); + return true; +} +template constexpr bool invert(rigid3& v) noexcept +{ + quat inv_rotation = v.rotation; + invert(inv_rotation); + const vec3 inv_translation = -math::rotate(inv_rotation, v.translation); + v = rigid3(inv_translation, inv_rotation); + return true; +} +template constexpr bool invert(quat& q) noexcept +{ + q.x = -q.x; + q.y = -q.y; + q.z = -q.z; + return true; +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T transposed(T const& v) noexcept +{ + T tmp(v); + transpose(tmp); + return tmp; +} +template constexpr void transpose(mat2& v) noexcept +{ + T* m = v.data(); + std::swap(m[1], m[2]); +} +template constexpr void transpose(mat3& v) noexcept +{ + T* m = v.data(); + std::swap(m[1], m[3]); + std::swap(m[2], m[6]); + std::swap(m[5], m[7]); +} +template constexpr void transpose(mat4& v) noexcept +{ + T* m = v.data(); + std::swap(m[1], m[4]); + std::swap(m[2], m[8]); + std::swap(m[3], m[12]); + std::swap(m[6], m[9]); + std::swap(m[7], m[13]); + std::swap(m[11], m[14]); +} + +////////////////////////////////////////////////////////////////////////// + +template<> inline float pow(float const& a, float const& b) noexcept +{ + ASSERT_BAD_ARGS(is_finite(a) && is_finite(b)); + return ::powf(a, b); +} +template<> inline float pow(float const& a, float const& b) noexcept +{ + ASSERT_BAD_ARGS(is_finite(a) && is_finite(b)); + return ::powf(a, b); +} +template<> inline float pow(float const& a, float const& b) noexcept +{ + ASSERT_BAD_ARGS(is_finite(a) && is_finite(b)); + return (!is_finite(a) || !is_finite(b)) ? 0 : ::powf(a, b); +} + +template<> inline double pow(double const& a, double const& b) noexcept +{ + ASSERT_BAD_ARGS(is_finite(a) && is_finite(b)); + return ::pow(a, b); +} +template<> inline double pow(double const& a, double const& b) noexcept +{ + ASSERT_BAD_ARGS(is_finite(a) && is_finite(b)); + return ::pow(a, b); +} +template<> inline double pow(double const& a, double const& b) noexcept +{ + ASSERT_BAD_ARGS(is_finite(a) && is_finite(b)); + return (!is_finite(a) || !is_finite(b)) ? 0 : ::pow(a, b); +} + +template inline vec2 pow(vec2 const& a, vec2 const& b) noexcept +{ + return vec2(pow(a.x, b.x), pow(a.y, b.y)); +} +template inline vec3 pow(vec3 const& a, vec3 const& b) noexcept +{ + return vec3(pow(a.x, b.x), pow(a.y, b.y), pow(a.z, b.z)); +} +template inline vec4 pow(vec4 const& a, vec4 const& b) noexcept +{ + return vec4(pow(a.x, b.x), pow(a.y, b.y), pow(a.z, b.z), pow(a.w, b.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template<> constexpr float positive_zero(float const& v) noexcept +{ + //ASSERT_BAD_ARGS(is_finite(v)); + return v == -0.f ? 0.f : v; +} +template constexpr T positive_zero(T const& v) noexcept +{ + //ASSERT_BAD_ARGS(is_finite(v)); + return v; +} +template constexpr angle positive_zero(angle const& v) noexcept +{ + return angle(positive_zero(v.x)); +} +template constexpr vec2 positive_zero(vec2 const& v) noexcept +{ + return vec2(positive_zero(v.x), positive_zero(v.y)); +} +template constexpr vec3 positive_zero(vec3 const& v) noexcept +{ + return vec3(positive_zero(v.x), positive_zero(v.y), positive_zero(v.z)); +} +template constexpr vec4 positive_zero(vec4 const& v) noexcept +{ + return vec4(positive_zero(v.x), positive_zero(v.y), positive_zero(v.z), positive_zero(v.w)); +} +template constexpr quat positive_zero(quat const& v) noexcept +{ + return quat(positive_zero(v.x), positive_zero(v.y), positive_zero(v.z), positive_zero(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T dot(vec2 const& v1, vec2 const& v2) noexcept +{ + ASSERT_BAD_ARGS(is_finite(v1) && is_finite(v2)); + return v1.x * v2.x + v1.y * v2.y; +} +template constexpr T dot(vec3 const& v1, vec3 const& v2) noexcept +{ + ASSERT_BAD_ARGS(is_finite(v1) && is_finite(v2)); + return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; +} +template constexpr T dot(vec4 const& v1, vec4 const& v2) noexcept +{ + ASSERT_BAD_ARGS(is_finite(v1) && is_finite(v2)); + return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z + v1.w * v2.w; +} +template +constexpr T dot(quat const& q1, quat const& q2) noexcept +{ + return (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) + (q1.w * q2.w); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T cross(vec2 const& v1, vec2 const& v2) noexcept +{ + ASSERT_BAD_ARGS(is_finite(v1) && is_finite(v2)); + return (v1.x * v2.y - v1.y * v2.x); +} +template constexpr vec3 cross(vec3 const& v1, vec3 const& v2) noexcept +{ + ASSERT_BAD_ARGS(is_finite(v1) && is_finite(v2)); + return vec3(v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x); +} +template vec4 cross(vec4 const& x1, vec4 const& x2, vec4 const& x3) noexcept +{ + ASSERT_BAD_ARGS(is_finite(x1) && is_finite(x2) && is_finite(x3)); + // 4d Cross product (GPG6, section 2.2) +#define DOT(AX,AY,AZ,BX,BY,BZ) AX*BX+AY*BY+AZ*BZ +#define CROSS(AX,AY,AZ,BX,BY,BZ,CX,CY,CZ) CX = AY*BZ - AZ*BY; CY = AZ*BX - AX*BZ; CZ = AX*BY - AY*BX; + vec4 c1; + vec4 c2; + vec4 c3; + vec4 c4; + CROSS(x2.y, x2.z, x2.w, x3.y, x3.z, x3.w, c1.x, c1.y, c1.z); + CROSS(x2.x, x2.z, x2.w, x3.x, x3.z, x3.w, c2.x, c2.y, c2.z); + CROSS(x2.x, x2.y, x2.w, x3.x, x3.y, x3.w, c3.x, c3.y, c3.z); + CROSS(x2.x, x2.y, x2.z, x3.x, x3.y, x3.z, c4.x, c4.y, c4.z); + + return vec4(DOT(x1.y, x1.z, x1.w, c1.x, c1.y, c1.z), + -DOT(x1.x, x1.z, x1.w, c2.x, c2.y, c2.z), + DOT(x1.x, x1.y, x1.w, c3.x, c3.y, c3.z), + -DOT(x1.x, x1.y, x1.z, c4.x, c4.y, c4.z)); +#undef DOT +#undef CROSS +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T length_sq(vec2 const& v) noexcept +{ + return v.x*v.x + v.y*v.y; //this is a dot product, inlined for faster debug perf +} +template constexpr T length_sq(vec3 const& v) noexcept +{ + return v.x*v.x + v.y*v.y + v.z*v.z; //this is a dot product, inlined for faster debug perf +} +template constexpr T length_sq(vec4 const& v) noexcept +{ + return v.x*v.x + v.y*v.y + v.z*v.z + v.w*v.w; //this is a dot product, inlined for faster debug perf +} +template constexpr T length_sq(quat const& v) noexcept +{ + return dot(v, v); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T length(vec2 const& v) noexcept +{ + return sqrt(length_sq(v)); +} +template constexpr T length(vec3 const& v) noexcept +{ + return sqrt(length_sq(v)); +} +template constexpr T length(vec4 const& v) noexcept +{ + return sqrt(length_sq(v)); +} + +template constexpr T length(quat const& v) noexcept +{ + return sqrt(length_sq(v)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr void set_length(vec2& v, T length) noexcept +{ + const T old_len = math::length(v); + if (!is_zero(old_len)) + v *= (length / old_len); +} +template constexpr void set_length(vec3& v, T length) noexcept +{ + const T old_len = math::length(v); + if (!is_zero(old_len)) + v *= (length / old_len); +} +template constexpr void set_length(vec4& v, T length) noexcept +{ + const T old_len = math::length(v); + if (!is_zero(old_len)) + v *= (length / old_len); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr angle normalized(angle const& v) noexcept +{ + angle x(v); + x.normalize(); + return x; +} +template constexpr vec2 normalized(vec2 const& v) noexcept +{ + const T len_sq = length_sq(v); + const T len_inv = inv_sqrt(len_sq); + return v * len_inv; +} +template constexpr vec3 normalized(vec3 const& v) noexcept +{ + const T len_sq = length_sq(v); + const T len_inv = inv_sqrt(len_sq); + return v * len_inv; +} +template constexpr vec4 normalized(vec4 const& v) noexcept +{ + const T len_sq = length_sq(v); + const T len_inv = inv_sqrt(len_sq); + return v * len_inv; +} +template constexpr quat normalized(quat const& q) noexcept +{ + const T n = inv_sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); + return + { + q.x * n, + q.y * n, + q.z * n, + q.w * n, + }; +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T square(T const& v) noexcept +{ + ASSERT_BAD_ARGS(is_finite(v)); + return v * v; +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T distance(T const& v1, T const& v2) noexcept +{ + return abs(v2 - v1); +} +template constexpr angle distance(angle const& v1, angle const& v2) noexcept +{ + return abs(signed_distance(v1, v2)); +} +template constexpr T distance(vec2 const& v1, vec2 const& v2) noexcept +{ + return sqrt(distance_sq(v1, v2)); +} +template constexpr T distance(vec3 const& v1, vec3 const& v2) noexcept +{ + return sqrt(distance_sq(v1, v2)); +} +template constexpr T distance(vec4 const& v1, vec4 const& v2) noexcept +{ + return sqrt(distance_sq(v1, v2)); +} + +template constexpr T distance(line3 const& l1, line3 const& l2) noexcept +{ + return sqrt(distance_sq(l1, l2)); +} + +////////////////////////////////////////////////////////////////////////// + +template +constexpr T signed_distance(const T& v1, const T& v2) noexcept +{ + return v2 - v1; +} +template +constexpr angle signed_distance(const angle& v1, const angle& v2) noexcept +{ + T start = v1.radians; + T end = v2.radians; + const T difference = abs(end - start); + if (difference > angle::pi) + { + if (end > start) + start += angle::_2pi; + else + end += angle::_2pi; + } + return (angle(end - start)); +} +template +constexpr T signed_distance(const vec3& point, const plane& plane) noexcept +{ + return dot(plane.normal, point) + plane.offset; +} +template +constexpr T signed_distance(const plane& plane, const vec3& point) noexcept +{ + return signed_distance(point, plane); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr T distance_sq(T const& v1, T const& v2) noexcept +{ + return abs(v2 - v1); +} +template constexpr T distance_sq(vec2 const& v1, vec2 const& v2) noexcept +{ + //these represent the following math, but inlined for faster debug performance + // auto v = v2 - v1; + // return dot(v, v); + const T dx = v2.x - v1.x; + const T dy = v2.y - v1.y; + return dx*dx + dy*dy; +} +template constexpr T distance_sq(vec3 const& v1, vec3 const& v2) noexcept +{ + //these represent the following math, but inlined for faster debug performance + // auto v = v2 - v1; + // return dot(v, v); + const T dx = v2.x - v1.x; + const T dy = v2.y - v1.y; + const T dz = v2.z - v1.z; + return dx*dx + dy*dy + dz*dz; +} +template constexpr T distance_sq(vec4 const& v1, vec4 const& v2) noexcept +{ + //these represent the following math, but inlined for faster debug performance + // auto v = v2 - v1; + // return dot(v, v); + const T dx = v2.x - v1.x; + const T dy = v2.y - v1.y; + const T dz = v2.z - v1.z; + const T dw = v2.w - v1.w; + return dx*dx + dy*dy + dz*dz + dw*dw; +} +template constexpr T distance_sq(line3 const& l1, line3 const& l2) noexcept +{ + const vec3 u(l1.end - l1.start); + const vec3 v(l2.end - l2.start); + const vec3 w(l1.start - l2.start); + const T a = dot(u, u); // always >= 0 + const T b = dot(u, v); + const T c = dot(v, v); // always >= 0 + const T d = dot(u, w); + const T e = dot(v, w); + const T D = a * c - b * b; // always >= 0 + T sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 + T tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 + + // compute the line parameters of the two closest points + if (D < tl::numeric_limits::epsilon()) + { // the lines are almost parallel + sN = T(0); // force using point P0 on segment S1 + sD = T(1); // to prevent possible division by 0.0 later + tN = e; + tD = c; + } + else + { // get the closest points on the infinite lines + sN = (b * e - c * d); + tN = (a * e - b * d); + if (sN < T(0)) + { // sc < 0 => the s=0 edge is visible + sN = T(0); + tN = e; + tD = c; + } + else if (sN > sD) + { // sc > 1 => the s=1 edge is visible + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < T(0)) + { // tc < 0 => the t=0 edge is visible + tN = T(0); + // recompute sc for this edge + if (-d < T(0)) + sN = T(0); + else if (-d > a) + sN = sD; + else + { + sN = -d; + sD = a; + } + } + else if (tN > tD) + { // tc > 1 => the t=1 edge is visible + tN = tD; + // recompute sc for this edge + if ((-d + b) < T(0)) + sN = 0; + else if ((-d + b) > a) + sN = sD; + else + { + sN = (-d + b); + sD = a; + } + } + // finally do the division to get sc and tc + sc = (abs(sN) < tl::numeric_limits::epsilon() ? T(0) : sN / sD); + tc = (abs(tN) < tl::numeric_limits::epsilon() ? T(0) : tN / tD); + + // get the difference of the two closest points + const vec3 dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc) + return length_sq(dP); // return the closest distance +} + +////////////////////////////////////////////////////////////////////////// + +namespace cwise +{ +////////////////////////////////////////////////////////////////////////// + +template +constexpr T min_component(vec2 const& v) noexcept +{ + return v[min_component_index(v)]; +} +template +constexpr T min_component(vec3 const& v) noexcept +{ + return v[min_component_index(v)]; +} +template +constexpr T min_component(vec4 const& v) noexcept +{ + return v[min_component_index(v)]; +} +template +constexpr size_t min_component_index(vec2 const& v) noexcept +{ + return (v.x < v.y) ? 0 : 1; +} +template +constexpr size_t min_component_index(vec3 const& v) noexcept +{ + if (v.x < v.y) + return (v.z < v.x) ? 2 : 0; + else + return (v.z < v.y) ? 2 : 1; +} +template +constexpr size_t min_component_index(vec4 const& v) noexcept +{ + if (v.x < v.y) + { + if (v.z < v.w) + return (v.z < v.x) ? 2 : 0; + else + return (v.w < v.x) ? 3 : 0; + } + else + { + if (v.y < v.z) + return (v.w < v.y) ? 3 : 1; + else + return (v.z < v.w) ? 2 : 3; + } +} + +////////////////////////////////////////////////////////////////////////// + +template +constexpr T max_component(vec2 const& v) noexcept +{ + return v[max_component_index(v)]; +} +template +constexpr T max_component(vec3 const& v) noexcept +{ + return v[max_component_index(v)]; +} +template +constexpr T max_component(vec4 const& v) noexcept +{ + return v[max_component_index(v)]; +} +template +constexpr size_t max_component_index(vec2 const& v) noexcept +{ + return (v.x > v.y) ? 0 : 1; +} +template +constexpr size_t max_component_index(vec3 const& v) noexcept +{ + if (v.x > v.y) + return (v.z > v.x) ? 2 : 0; + else + return (v.z > v.y) ? 2 : 1; +} +template +constexpr size_t max_component_index(vec4 const& v) noexcept +{ + if (v.x > v.y) + { + if (v.z > v.w) + return (v.z > v.x) ? 2 : 0; + else + return (v.w > v.x) ? 3 : 0; + } + else + { + if (v.y > v.z) + return (v.w > v.y) ? 3 : 1; + else + return (v.z > v.w) ? 2 : 3; + } +} + +} //cwise + +////////////////////////////////////////////////////////////////////////// + +namespace batch +{ +template void dot(T* dst, size_t dstStride, vec3 const* src1, size_t src1Stride, vec3 const* src2, size_t src2Stride, size_t count) noexcept +{ + auto* __restrict d = dst; + auto const* __restrict s1 = src1; + auto const* __restrict s2 = src2; + + ASSERT_BAD_ARGS(src1 && src2 && dst && count); + for (size_t i = 0; i < count; ++i) + { + *d = dot(*s1, *s2); + s1 = (vec3 const* __restrict)(((char const* __restrict)s1) + src1Stride); + s2 = (vec3 const* __restrict)(((char const* __restrict)s2) + src2Stride); + d = (T * __restrict)(((char* __restrict)d) + dstStride); + } +} +template void dot(T* dst, size_t dstStride, vec4 const* src1, size_t src1Stride, vec4 const* src2, size_t src2Stride, size_t count) noexcept +{ + auto* __restrict d = dst; + auto const* __restrict s1 = src1; + auto const* __restrict s2 = src2; + + ASSERT_BAD_ARGS(src1 && src2 && dst && count); + for (size_t i = 0; i < count; ++i) + { + *d = dot(*s1, *s2); + s1 = (vec4 const* __restrict)(((char const* __restrict)s1) + src1Stride); + s2 = (vec4 const* __restrict)(((char const* __restrict)s2) + src2Stride); + d = (T * __restrict)(((char* __restrict)d) + dstStride); + } +} +} + +} diff --git a/include/VMath/func_interp.h b/include/VMath/func_interp.h new file mode 100644 index 0000000..ada11df --- /dev/null +++ b/include/VMath/func_interp.h @@ -0,0 +1,30 @@ +#pragma once + +namespace math +{ + +template +constexpr float lerp(float a, float b, float t) noexcept; + +template +constexpr T lerp(T const& a, T const& b, float t) noexcept; + +template +constexpr angle lerp(angle const& a, angle const& b, float t) noexcept; + +template +constexpr vec2 lerp(vec2 const& a, vec2 const& b, float t) noexcept; + +template +constexpr vec3 lerp(vec3 const& a, vec3 const& b, float t) noexcept; + +template +constexpr vec4 lerp(vec4 const& a, vec4 const& b, float t) noexcept; + +template +constexpr quat nlerp(quat const& a, quat const& b, float t) noexcept; + +template +constexpr quat slerp(quat const& a, quat const& b, float t) noexcept; + +} diff --git a/include/VMath/func_interp.inl b/include/VMath/func_interp.inl new file mode 100644 index 0000000..940435e --- /dev/null +++ b/include/VMath/func_interp.inl @@ -0,0 +1,123 @@ +namespace math +{ +namespace detail +{ +template +constexpr float lerp_check_mu(float t) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(t)); + TL_PLAIN_ASSERT(t >= 0 && t <= 1); + return t; +} +template<> +constexpr float lerp_check_mu(float t) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(t)); + return clamp(t, 0.f, 1.f); +} +} + +template +constexpr float lerp(float a, float b, float t) noexcept +{ + TL_PLAIN_ASSERT(is_finite(a) && is_finite(b)/* && is_finite(t)*/); + t = detail::lerp_check_mu(t); + const float x = b - a; + return a + x*t; +} + +template +constexpr T lerp(T const& a, T const& b, float t) noexcept +{ + TL_PLAIN_ASSERT(is_finite(a) && is_finite(b)/* && is_finite(t)*/); + t = detail::lerp_check_mu(t); + return (T)(a*(1.f - t) + b*t); +} + +template +constexpr angle lerp(angle const& a, angle const& b, float t) noexcept +{ + t = detail::lerp_check_mu(t); + T start = a.radians; + T end = b.radians; + T difference = abs(end - start); + if (difference > angle::pi) // We need to add on to one of the values. + { + if (end > start) // We'll add it on to start... + start += angle::_2pi; + else // Add it on to end. + end += angle::_2pi; + } + return angle(start + ((end - start) * t)); +} + +template +constexpr vec2 lerp(vec2 const& a, vec2 const& b, float t) noexcept +{ + t = detail::lerp_check_mu(t); + vec2 x = b - a; + return vec2(a.x + x.x*T(t), a.y + x.y*T(t)); +} + +template +constexpr vec3 lerp(vec3 const& a, vec3 const& b, float t) noexcept +{ + t = detail::lerp_check_mu(t); + vec3 x = b - a; + return vec3(a.x + x.x*T(t), a.y + x.y*T(t), a.z + x.z*T(t)); +} + +template +constexpr vec4 lerp(vec4 const& a, vec4 const& b, float t) noexcept +{ + t = detail::lerp_check_mu(t); + vec4 x = b - a; + return vec4(a.x + x.x*T(t), a.y + x.y*T(t), a.z + x.z*T(t), a.w + x.w*T(t)); +} + +template +constexpr quat nlerp(quat const& a, quat const& b, float t) noexcept +{ + t = detail::lerp_check_mu(t); + T angle = dot(a, b); + if (angle >= 0) + return normalized(a + (b - a) * t); + else if (angle <= -0.9999) + return normalized(t < T(0.5) ? a : b); + else + return normalized(a + (-b - a) * t); +} + +template +constexpr quat slerp(quat const& a, quat const& b, float t) noexcept +{ + t = detail::lerp_check_mu(t); + + const T one = T(1) - tl::numeric_limits::epsilon() * T(2); + + T d = dot(a, b); + T absD = math::abs(d); + T scale0; + T scale1; + + if (absD >= one) + { + scale0 = T(1) - t; + scale1 = t; + } + else + { + // theta is the angle between the 2 quaternions + T theta = acos(absD); + T sinTheta = sin(theta); + + scale0 = sin((T(1) - t) * theta) / sinTheta; + scale1 = sin((t * theta)) / sinTheta; + } + if (d < T(0)) + scale1 = -scale1; + + TL_PLAIN_ASSERT(!is_nan(scale0) && !is_nan(scale1)); + return normalized(quat(a * scale0 + b * scale1)); +} +} diff --git a/include/VMath/func_interpolation.h b/include/VMath/func_interpolation.h new file mode 100644 index 0000000..510b713 --- /dev/null +++ b/include/VMath/func_interpolation.h @@ -0,0 +1,67 @@ +#pragma once + +namespace math +{ +// Spherical linear interpolation No-Invert. +// Knowing that dot(q1,q2) = cos(theta) where theta is the angle between q1 and q2. +// This version of slerp, used by squad, does not check for theta > 90. +template constexpr quat slerp_no_invert(quat const& a, quat const& b, float mu) noexcept; + +//! Spherical Quadrangle Interpolation +// It constructs a spherical cubic interpolation as a series of three spherical +// linear interpolations of a quadrangle of unit quaternions. +// qspline() uses this method to perform a spline interpolation. +template constexpr quat squad(quat const& q1, quat const& qa, quat const& qb, quat const& q2, float mu) noexcept; + +namespace tween +{ + +// Linear interpolation LinearStep(t) = t +template constexpr T linear(T t) noexcept; + +// Cubic Hermite interpolation; between [0 and 1]; see http://en.wikipedia.org/wiki/Smoothstep +// SmoothStep(t) = 3t^2 - 2t^3 +template constexpr T smooth(T t) noexcept; + +// Variation of Smooth Step; between [0 and 1]; see http://en.wikipedia.org/wiki/Smoothstep +// SmootherStep(t) = 6t^5 - 15t^4 + 10t^3 +template constexpr T smoother(T t) noexcept; + +// quadratic transition out [0 to 1] +template constexpr T quadratic_ease_out(T t) noexcept; + +// quadratic transition in [0 to 1] +template constexpr T quadratic_ease_in(T t) noexcept; + +// quadratic smooth transition [0 to 1] +template constexpr T quadratic_ease_inout(T t) noexcept; + +// quartic transition out [0 to 1] +template constexpr T quartic_ease_out(T t) noexcept; + +// quartic transition in [0 to 1] +template constexpr T quartic_ease_in(T t) noexcept; + +// quartic smooth transition [0 to 1] +template constexpr T quartic_ease_inout(T t) noexcept; + +// octic transition out [0 to 1] +template constexpr T octic_ease_out(T t) noexcept; + +// octic transition in [0 to 1] +template constexpr T octic_ease_in(T t) noexcept; + +// octic smooth transition inout [0 to 1] +templateconstexpr T octic_ease_inout(T t) noexcept; + +// circular transition out [0 to 1] +template constexpr T circular_ease_out(T t) noexcept; + +// circular transition in [0 to 1] +template constexpr T circular_ease_in(T t) noexcept; + +// circular transition inout [0 to 1] +template constexpr T circular_ease_inout(T t) noexcept; + +} +} diff --git a/include/VMath/func_interpolation.inl b/include/VMath/func_interpolation.inl new file mode 100644 index 0000000..44368f9 --- /dev/null +++ b/include/VMath/func_interpolation.inl @@ -0,0 +1,164 @@ +namespace math +{ + +template constexpr quat slerp_no_invert(quat const& a, quat const& b, float t) noexcept +{ + const T one = T(1); + + T angle = dot(a, b); + + T scale; + T invscale; + + if (angle > T(0.999) || angle < T(-0.999)) + { + scale = one - t; + invscale = t; + return math::normalized(quat((a * scale) + (b * invscale))); + } + else + { + T const theta = acos(angle); + T const invsintheta = one / sin(theta); + scale = sin(theta * (one - t)) * invsintheta; + invscale = sin(theta * t) * invsintheta; + return quat((a * scale) + (b * invscale)); + } +} + +template constexpr quat squad(quat const& q1, quat const& q_tangent_a, quat const& q_tangent_b, quat const& q2, float t) noexcept +{ + const math::quat r1 = slerp_no_invert(q1, q2, t); + const math::quat r2 = slerp_no_invert(q_tangent_a, q_tangent_b, t); + return slerp_no_invert(r1, r2, 2 * t * (1 - t)); +} + +namespace tween +{ + +template constexpr T linear(T t) noexcept +{ + return t; +} + +template constexpr T smooth(T t) noexcept +{ + return clamp(t * t * (T(3) - T(2) * t), T(0), T(1)); +} + +template constexpr T smoother(T t) noexcept +{ + return clamp(t * t * t * (t * (t * T(6) - T(15)) + T(10)), T(0), T(1)); +} + +template constexpr T quadratic_ease_out(T t) noexcept +{ + T invT = T(1) - t; + return clamp(T(1) - (invT * invT), T(0), T(1)); +} + +template constexpr T quadratic_ease_in(T t) noexcept +{ + return clamp(t * t, T(0), T(1)); +} + +template constexpr T quadratic_ease_inout(T t) noexcept +{ + t *= T(2); + if (t < T(1)) + return clamp(T(0.5) * t * t, T(0), T(1)); + else + { + t -= T(2); + return clamp(T(-0.5) * (t * t - T(2)), T(0), T(1)); + } +} + +template constexpr T quartic_ease_out(T t) noexcept +{ + T invT = T(1) - t; + invT *= invT; + return clamp(T(1) - (invT * invT), T(0), T(1)); +} + +template constexpr T quartic_ease_in(T t) noexcept +{ + T t2 = t * t; + return t2 * t2; +} + +template constexpr T quartic_ease_inout(T t) noexcept +{ + t *= T(2); + if (t < T(1)) + { + T t2 = t * t; + return T(0.5) * t2 * t2; + } + else + { + t -= T(2); + T t2 = t * t; + return clamp(T(-0.5) * (t2 * t2 - T(2)), T(0), T(1)); + } +} + +template constexpr T octic_ease_out(T t) noexcept +{ + T invT = T(1) - t; + invT *= invT; + invT *= invT; + invT *= invT; + return clamp(T(1) - invT, T(0), T(1)); +} + +template constexpr T octic_ease_in(T t) noexcept +{ + T t2 = t * t; + t2 *= t2; + return t2 * t2; +} + +template constexpr T octic_ease_inout(T t) noexcept +{ + t *= T(2); + if (t < T(1)) + { + T t2 = t * t; + t2 *= t2; + return T(0.5) * t2 * t2; + } + else + { + t -= T(2); + T t2 = t * t; + t2 *= t2; + return clamp(T(-0.5) * (t2 * t2 - T(2)), T(0), T(1)); + } +} + +template constexpr T circular_ease_out(T t) noexcept +{ + T t1 = t - 1; + return math::sqrt(T(1) - t1 * t1); +} + +template constexpr T circular_ease_in(T t) noexcept +{ + return clamp(T(-1) * (math::sqrt(T(1) - t * t) - T(1)), T(0), T(1)); +} + +template constexpr T circular_ease_inout(T t) noexcept +{ + t *= T(2); + if (t < T(1)) + return clamp(T(-0.5) * (math::sqrt(T(1) - t * t) - T(1)), T(0), T(1)); + else + { + t -= T(2); + return clamp(T(0.5) * (math::sqrt(T(1) - t * t) + T(1)), T(0), T(1)); + } +} + +} +} \ No newline at end of file diff --git a/include/VMath/func_intersect.h b/include/VMath/func_intersect.h new file mode 100644 index 0000000..f389ed6 --- /dev/null +++ b/include/VMath/func_intersect.h @@ -0,0 +1,53 @@ +#pragma once + +namespace math +{ + +/* + * d_near and d_far specifies the distance from the origin where the primitive enter and exits the aabb + * +*/ +template +constexpr bool intersect_line_plane(const line3& line, const plane& plane, float& d) noexcept; +template +constexpr bool intersect_line_aabb(const line3& line, const aabb3& aabb, float& d_near, float& d_far) noexcept; +template +constexpr bool intersect_line_aabb(const line2& line, const aabb2& aabb, float& d_near, float& d_far) noexcept; + +template +constexpr bool intersect_ray_plane(const ray3& line, const plane& plane, float& d) noexcept; +template +constexpr bool intersect_ray_aabb(const ray2& ray, const aabb2& aabb, float& d_near, float& d_far) noexcept; +template +constexpr bool intersect_ray_aabb(const ray3& ray, const aabb3& aabb, float& d_near, float& d_far) noexcept; + +template +constexpr bool intersect_segment_aabb(const segment2& segment, const aabb2& aabb, float& d_near, float& d_far) noexcept; +template +constexpr bool intersect_segment_aabb(const segment3& segment, const aabb3& aabb, float& d_near, float& d_far) noexcept; + +template +constexpr bool intersect_point_segment(const math::vec3& point, const segment3& segment) noexcept; +template +constexpr bool intersect_point_segment(const math::vec3& point, const segment3& segment, float& d) noexcept; + +template +constexpr bool intersect_line_triangle(const line3& line, const triangle3& triangle, float& line_d, vec2& bari_coords) noexcept; +template +constexpr bool intersect_ray_triangle(const ray3& ray, const triangle3& triangle, float& ray_d, vec2& bari_coords) noexcept; +template +constexpr bool intersect_segment_triangle(const segment3& segment, const triangle3& triangle, float& segment_d, vec2& bari_coords) noexcept; + +template +constexpr math::segment3 closest_segment_between_lines(const math::line3& a, const math::line3& b) noexcept; + +template +constexpr math::vec3 closest_point_to_line(const vec3& point, const line3& line) noexcept; + +template +constexpr math::vec3 closest_point_to_segment(const vec3& point, const segment3& segment) noexcept; + +template +constexpr math::vec3 closest_point_to_ray(const vec3& point, const ray3& ray) noexcept; + +} diff --git a/include/VMath/func_intersect.inl b/include/VMath/func_intersect.inl new file mode 100644 index 0000000..90ca944 --- /dev/null +++ b/include/VMath/func_intersect.inl @@ -0,0 +1,380 @@ +#pragma once + +namespace math +{ + +/* +* Reference +* https://github.com/juj/MathGeoLib/blob/master/src/Geometry/Line.cpp +*/ + +namespace detail +{ +template +constexpr bool do_intersect_line_aabb(const vec3& line_pos, const vec3& line_dir, const aabb3& aabb, float& d_near, float& d_far) noexcept +{ + // The user should have inputted values for d_near and d_far to specify the desired subrange [d_near, d_far] of the line + // for this intersection test. + // For a Line-AABB test, pass in + // d_near = -FLOAT_INF; + // d_far = FLOAT_INF; + // For a Ray-AABB test, pass in + // d_near = 0.f; + // d_far = FLOAT_INF; + // For a LineSegment-AABB test, pass in + // d_near = 0.f; + // d_far = LineSegment.Length(); + + // Test each cardinal plane (X, Y and Z) in turn. + if (!is_zero(line_dir.x)) + { + const float rcp_dir = T(1) / line_dir.x; + const float t1 = (aabb.min.x - line_pos.x) * rcp_dir; + const float t2 = (aabb.max.x - line_pos.x) * rcp_dir; + + // d_near tracks distance to intersect (enter) the AABB. + // d_far tracks the distance to exit the AABB. + if (t1 < t2) + d_near = math::max(t1, d_near), d_far = math::min(t2, d_far); + else // Swap t1 and t2. + d_near = math::max(t2, d_near), d_far = math::min(t1, d_far); + + if (d_near > d_far) + return false; // Box is missed since we "exit" before entering it. + } + else if (line_pos.x < aabb.min.x || line_pos.x > aabb.max.x) + return false; // The ray can't possibly enter the box, abort. + + if (!is_zero(line_dir.y)) + { + const float rcp_dir = T(1) / line_dir.y; + const float t1 = (aabb.min.y - line_pos.y) * rcp_dir; + const float t2 = (aabb.max.y - line_pos.y) * rcp_dir; + + if (t1 < t2) + d_near = math::max(t1, d_near), d_far = math::min(t2, d_far); + else // Swap t1 and t2. + d_near = math::max(t2, d_near), d_far = math::min(t1, d_far); + + if (d_near > d_far) + return false; // Box is missed since we "exit" before entering it. + } + else if (line_pos.y < aabb.min.y || line_pos.y > aabb.max.y) + return false; // The ray can't possibly enter the box, abort. + + if (!is_zero(line_dir.z)) // ray is parallel to plane in question + { + const float rcp_dir = T(1) / line_dir.z; + const float t1 = (aabb.min.z - line_pos.z) * rcp_dir; + const float t2 = (aabb.max.z - line_pos.z) * rcp_dir; + + if (t1 < t2) + d_near = math::max(t1, d_near), d_far = math::min(t2, d_far); + else // Swap t1 and t2. + d_near = math::max(t2, d_near), d_far = math::min(t1, d_far); + } + else if (line_pos.z < aabb.min.z || line_pos.z > aabb.max.z) + return false; // The ray can't possibly enter the box, abort. + + return d_near <= d_far; +} +template +constexpr bool do_intersect_line_aabb(const vec3& line_pos, const vec3& line_dir, const aabb2& aabb, float& d_near, float& d_far) noexcept +{ + // The user should have inputted values for d_near and d_far to specify the desired subrange [d_near, d_far] of the line + // for this intersection test. + // For a Line-AABB test, pass in + // d_near = -FLOAT_INF; + // d_far = FLOAT_INF; + // For a Ray-AABB test, pass in + // d_near = 0.f; + // d_far = FLOAT_INF; + // For a LineSegment-AABB test, pass in + // d_near = 0.f; + // d_far = LineSegment.Length(); + + // Test each cardinal plane (X, Y and Z) in turn. + if (!is_zero(line_dir.x)) + { + const float rcp_dir = T(1) / line_dir.x; + const float t1 = (aabb.min.x - line_pos.x) * rcp_dir; + const float t2 = (aabb.max.x - line_pos.x) * rcp_dir; + + // d_near tracks distance to intersect (enter) the AABB. + // d_far tracks the distance to exit the AABB. + if (t1 < t2) + d_near = math::max(t1, d_near), d_far = math::min(t2, d_far); + else // Swap t1 and t2. + d_near = math::max(t2, d_near), d_far = math::min(t1, d_far); + + if (d_near > d_far) + return false; // Box is missed since we "exit" before entering it. + } + else if (line_pos.x < aabb.min.x || line_pos.x > aabb.max.x) + return false; // The ray can't possibly enter the box, abort. + + if (!is_zero(line_dir.y)) + { + const float rcp_dir = T(1) / line_dir.y; + const float t1 = (aabb.min.y - line_pos.y) * rcp_dir; + const float t2 = (aabb.max.y - line_pos.y) * rcp_dir; + + if (t1 < t2) + d_near = math::max(t1, d_near), d_far = math::min(t2, d_far); + else // Swap t1 and t2. + d_near = math::max(t2, d_near), d_far = math::min(t1, d_far); + + if (d_near > d_far) + return false; // Box is missed since we "exit" before entering it. + } + else if (line_pos.y < aabb.min.y || line_pos.y > aabb.max.y) + return false; // The ray can't possibly enter the box, abort. + + return d_near <= d_far; +} + +/// Computes the closest point pair on two lines. +/** The first line is specified by two points start0 and end0. The second line is specified by +two points start1 and end1. +The implementation of this function follows http://paulbourke.net/geometry/lineline3d/ . +@param v0 The starting point of the first line. +@param v10 The direction vector of the first line. This can be unnormalized. +@param v2 The starting point of the second line. +@param v32 The direction vector of the second line. This can be unnormalized. +@param d [out] Receives the normalized distance of the closest point along the first line. +@param d2 [out] Receives the normalized distance of the closest point along the second line. +@return Returns the closest point on line start0<->end0 to the second line. +@note This is a low-level utility function. You probably want to use ClosestPoint() or Distance() instead. +@see ClosestPoint(), Distance(). */ +template +constexpr void closest_point_line_line(const vec3& a_pos, const vec3& a_dir, const vec3& b_pos, const vec3& b_dir, float& a_d, float& b_d) noexcept +{ + TL_PLAIN_ASSERT(!is_zero(a_dir)); + TL_PLAIN_ASSERT(!is_zero(b_dir)); + const vec3 v02 = a_pos - b_pos; + const T d0232 = dot(v02, b_dir); + const T d3210 = dot(b_dir, a_dir); + const T d3232 = dot(b_dir, b_dir); + TL_PLAIN_ASSERT(!is_zero(d3232)); // Don't call with a zero direction vector. + const T d0210 = dot(v02, a_dir); + const T d1010 = dot(a_dir, a_dir); + const T denom = d1010 * d3232 - d3210 * d3210; + if (denom != T(0)) + a_d = (d0232 * d3210 - d0210 * d3232) / denom; + else + a_d = T(0); + b_d = (d0232 + a_d * d3210) / d3232; +} + +/** Calculates the intersection between a line and a triangle. The facing is not accounted for, so +rays are reported to intersect triangles that are both front and backfacing. +According to "T. Möller, B. Trumbore. Fast, Minimum Storage Ray/Triangle Intersection. 2005." +http://jgt.akpeters.com/papers/MollerTrumbore97/ +@param linePos The starting point of the line. +@param lineDir The direction vector of the line. This does not need to be normalized. +@param v0 Vertex 0 of the triangle. +@param v1 Vertex 1 of the triangle. +@param v2 Vertex 2 of the triangle. +@param u [out] The barycentric u coordinate is returned here if an intersection occurred. +@param v [out] The barycentric v coordinate is returned here if an intersection occurred. +@return The distance along the ray to the point of intersection, or +inf if no intersection occurred. +If no intersection, then u and v and t will contain undefined values. If lineDir was not normalized, then to get the +real world-space distance, one must scale the returned value with lineDir.Length(). If the returned value is negative, +then the intersection occurs 'behind' the line starting position, with respect to the direction vector lineDir. */ +template +constexpr bool do_intersect_line_tri(const vec3& line_pos, const vec3& line_dir, + const vec3& v0, const vec3& v1, const vec3& v2, + T& line_d, T& tri_u, T& tri_v) noexcept +{ + const T epsilon = tl::numeric_limits::epsilon(); + + // Edge vectors + const vec3 vE1 = v1 - v0; + const vec3 vE2 = v2 - v0; + + // begin calculating determinant - also used to calculate U parameter + const vec3 vP = cross(line_dir, vE2); + + // If det < 0, intersecting backfacing, tri > 0, intersecting frontfacing, tri == 0, parallel to plane. + const T det = dot(vE1, vP); + + // If determinant is near zero, ray lies in plane of triangle. + if (abs(det) <= epsilon) [[unlikely]] + return false; + + const T inv_det = T(1) / det; + + // Calculate distance from v0 to ray origin + const vec3 vT = line_pos - v0; + + // Output barycentric tri_u + tri_u = dot(vT, vP) * inv_det; + if (tri_u < -epsilon || tri_u > T(1) + epsilon) + return false; // Barycentric U is outside the triangle - early out. + + // Prepare to test V parameter + const vec3 vQ = cross(vT, vE1); + + // Output barycentric v + tri_v = dot(line_dir, vQ) * inv_det; + if (tri_v < -epsilon || tri_u + tri_v > T(1) + epsilon) // Barycentric V or the combination of U and V are outside the triangle - no intersection. + return false; + + // Barycentric u and v are in limits, the ray intersects the triangle. + + // Output signed distance from ray to triangle. + line_d = dot(vE2, vQ) * inv_det; + return true; +} + +} + +template +constexpr bool intersect_line_plane(const line3& line, const plane& plane, float& d) noexcept +{ + const T b = math::dot(plane.normal, line.direction); + if (math::is_zero(b)) + return false; + + const math::vec3 v = line.origin - plane.get_member_point(); + const T a = -math::dot(plane.normal, v); + d = a / b; + return true; +} + +template +constexpr bool intersect_line_aabb(const line2& line, const aabb2& aabb, float& d_near, float& d_far) noexcept +{ + d_near = tl::numeric_limits::lowest(); + d_far = tl::numeric_limits::max(); + return detail::do_intersect_line_aabb(line.origin, line.direction, aabb, d_near, d_far); +} +template +constexpr bool intersect_line_aabb(const line3& line, const aabb3& aabb, float& d_near, float& d_far) noexcept +{ + d_near = tl::numeric_limits::lowest(); + d_far = tl::numeric_limits::max(); + return detail::do_intersect_line_aabb(line.origin, line.direction, aabb, d_near, d_far); +} + +template +constexpr bool intersect_ray_plane(const ray3& line, const plane& plane, float& d) noexcept +{ + const T b = math::dot(plane.normal, line.direction); + if (math::is_zero(b)) + return false; + + const math::vec3 v = line.origin - plane.get_member_point(); + const T a = -math::dot(plane.normal, v); + d = a / b; + return d >= T(0); +} +template +constexpr bool intersect_ray_aabb(const ray2& ray, const aabb2& aabb, float& d_near, float& d_far) noexcept +{ + d_near = 0; + d_far = tl::numeric_limits::max(); + return detail::do_intersect_line_aabb(ray.origin, ray.direction, aabb, d_near, d_far); +} +template +constexpr bool intersect_ray_aabb(const ray3& ray, const aabb3& aabb, float& d_near, float& d_far) noexcept +{ + d_near = 0; + d_far = tl::numeric_limits::max(); + return detail::do_intersect_line_aabb(ray.origin, ray.direction, aabb, d_near, d_far); +} + +template +constexpr bool intersect_segment_aabb(const segment2& segment, const aabb2& aabb, float& d_near, float& d_far) noexcept +{ + d_near = 0; + d_far = segment.length(); + const vec2 dir = (segment.end - segment.start) / d_far; + return detail::do_intersect_line_aabb(segment.start, dir, aabb, d_near, d_far); +} +template +constexpr bool intersect_segment_aabb(const segment3& segment, const aabb3& aabb, float& d_near, float& d_far) noexcept +{ + d_near = 0; + d_far = segment.length(); + const vec3 dir = (segment.end - segment.start) / d_far; + return detail::do_intersect_line_aabb(segment.start, dir, aabb, d_near, d_far); +} +template +constexpr bool intersect_point_segment(const math::vec3& point, const segment3& segment) noexcept +{ + T d; + return intersect_point_segment(point, segment, d); +} +template +constexpr bool intersect_point_segment(const math::vec3& point, const segment3& segment, float& d) noexcept +{ + const T length = segment.length(); + const vec3 dir = (segment.end - segment.start) / length; + d = dot(point - segment.start, dir); + return d >= 0 && d < length; +} + +template +constexpr bool intersect_line_triangle(const line3& line, const triangle3& triangle, float& line_d, vec2& bari_coords) noexcept +{ + return detail::do_intersect_line_tri(line.origin, line.direction, triangle.a, triangle.b, triangle.c, line_d, bari_coords.x, bari_coords.y); +} +template +constexpr bool intersect_ray_triangle(const ray3& ray, const triangle3& triangle, float& ray_d, vec2& bari_coords) noexcept +{ + if (!detail::do_intersect_line_tri(ray.origin, ray.direction, triangle.a, triangle.b, triangle.c, ray_d, bari_coords.x, bari_coords.y)) + return false; + if (ray_d < T(0)) + return false; + return true; +} +template +constexpr bool intersect_segment_triangle(const segment3& segment, const triangle3& triangle, float& segment_d, vec2& bari_coords) noexcept +{ + const T length = segment.length(); + const vec3 dir = (segment.end - segment.start) / length; + + if (!detail::do_intersect_line_tri(segment.start, dir, triangle.a, triangle.b, triangle.c, segment_d, bari_coords.x, bari_coords.y)) + return false; + if (segment_d < T(0) || segment_d > length) + return false; + return true; +} + + +template +constexpr math::segment3 closest_segment_between_lines(const math::line3& a, const math::line3& b) noexcept +{ + T ad, bd; + detail::closest_point_line_line(a.origin, a.direction, b.origin, b.direction, ad, bd); + return { a.origin + a.direction * ad, b.origin + b.direction * bd }; +} + +template +constexpr math::vec3 closest_point_to_line(const vec3& point, const line3& line) noexcept +{ + const T d = dot(point - line.origin.start, line.direction); + return line.start + line.direction * d; +} + +template +constexpr math::vec3 closest_point_to_segment(const vec3& point, const segment3& segment) noexcept +{ + const T length = segment.length(); + const vec3 dir = (segment.end - segment.start) / length; + T d = dot(point - segment.start, dir); + d = clamp(d, T(0), length); + return segment.start + dir * d; +} + +template +constexpr math::vec3 closest_point_to_ray(const vec3& point, const ray3& ray) noexcept +{ + T d = dot(point - ray.origin, ray.direction); + d = max(d, T(0)); + return ray.origin + ray.direction * d; +} + +} + diff --git a/include/VMath/func_projection.h b/include/VMath/func_projection.h new file mode 100644 index 0000000..60d2244 --- /dev/null +++ b/include/VMath/func_projection.h @@ -0,0 +1,14 @@ +#pragma once + +namespace math +{ + ///! Creates a RH offseted orthographic projection matrix + template constexpr mat4 ortho(T left, T right, T bottom, T top, T z_near, T z_far) noexcept; + + ///! Creates a RH perspective projection matrix + template constexpr mat4 perspective(T fov, T aspect_ratio, T z_near, T z_far) noexcept; + + ///! Creates a RH asymmetric frustum projection matrix + template constexpr mat4 perspective(T left, T right, T bottom, T top, T z_near, T z_far) noexcept; + +}//namespace math diff --git a/include/VMath/func_projection.inl b/include/VMath/func_projection.inl new file mode 100644 index 0000000..56f10a8 --- /dev/null +++ b/include/VMath/func_projection.inl @@ -0,0 +1,105 @@ +namespace math +{ + + +///! Creates a RH orthographic projection matrix +template +constexpr mat4 ortho(T left, T right, T bottom, T top, T z_near, T z_far) noexcept +{ + mat4 mat; + T* M = mat.data(); + M[0] = 2 / (right - left); + M[1] = 0; + M[2] = 0; + M[3] = 0; + + M[4] = 0; + M[5] = 2 / (top - bottom); + M[6] = 0; + M[7] = 0; + + M[8] = 0; + M[9] = 0; + M[10] = (T)(-2) / (z_far - z_near); + M[11] = 0; + + M[12] = - (right + left) / (right - left); + M[13] = - (top + bottom) / (top - bottom); + M[14] = - (z_far + z_near) / (z_far - z_near); + M[15] = 1; + return mat; +} + +///! Creates a RH perspective projection matrix +template +constexpr mat4 perspective(T fov, T aspect_ratio, T z_near, T z_far) noexcept +{ + T const y_fac = tan(fov / 2); + T const x_fac = y_fac*aspect_ratio; + mat4 mat; + T* M = mat.data(); + + M[0] = 1/x_fac; + M[1] = 0; + M[2] = 0; + M[3] = 0; + + M[4] = 0; + M[5] = 1/y_fac; + M[6] = 0; + M[7] = 0; + + M[8] = 0; + M[9] = 0; + M[10] = -(z_far+z_near)/(z_far-z_near); + M[11] = -1; + + M[12] = 0; + M[13] = 0; + M[14] = -(2*z_far*z_near)/(z_far-z_near); + M[15] = 0; + return mat; +} + +///! Creates a RH asymmetric frustum projection matrix +template +constexpr mat4 perspective(T left, T right, T bottom, T top, T z_near, T z_far) noexcept +{ + // glFrustum call (column order) + // + // | 2n/(r-l) 0 (r+l)/(r-l) 0 | + // | 0 2n/(t-b) (t+b)/(t-b) 0 | + // | 0 0 (n+f)/(n-f) (2nf)/(n-f) | + // | 0 0 -1 0 | + // + T diff_x = 1/(right-left); + T diff_y = 1/(top-bottom); + T diff_z = 1/(z_near-z_far); + + mat4 mat; + T* m = mat.data(); + m[ 0] = 2*z_near*diff_x; + m[ 1] = 0; + m[ 2] = 0; + m[ 3] = 0; + + m[ 4] = 0; + m[ 5] = 2*z_near*diff_y; + m[ 6] = 0; + m[ 7] = 0; + + m[ 8] = (right+left)*diff_x; + m[ 9] = (top+bottom)*diff_y; + m[10] = (z_far+z_near)*diff_z; + m[11] = -1; + + m[12] = 0; + m[13] = 0; + m[14] = 2*z_near*z_far*diff_z; + m[15] = 0; + + return mat; +} + + +} diff --git a/include/VMath/func_range.h b/include/VMath/func_range.h new file mode 100644 index 0000000..1921998 --- /dev/null +++ b/include/VMath/func_range.h @@ -0,0 +1,56 @@ +#pragma once + +namespace math +{ + //min + template constexpr vec2 min(vec2 const& a, vec2 const& b) noexcept; + template constexpr vec3 min(vec3 const& a, vec3 const& b) noexcept; + template constexpr vec4 min(vec4 const& a, vec4 const& b) noexcept; + template constexpr clamped_value min(clamped_value const& a, clamped_value const& b) noexcept; + template constexpr T min(T a, T b) noexcept; + + //max + template constexpr vec2 max(vec2 const& a, vec2 const& b) noexcept; + template constexpr vec3 max(vec3 const& a, vec3 const& b) noexcept; + template constexpr vec4 max(vec4 const& a, vec4 const& b) noexcept; + template constexpr clamped_value max(clamped_value const& a, clamped_value const& b) noexcept; + template constexpr T max(T a, T b) noexcept; + + //clamping + template constexpr vec2 clamp(vec2 const& x, vec2 const& min, vec2 const& max) noexcept; + template constexpr vec3 clamp(vec3 const& x, vec3 const& min, vec3 const& max) noexcept; + template constexpr vec4 clamp(vec4 const& x, vec4 const& min, vec4 const& max) noexcept; + template constexpr clamped_value clamp(clamped_value const& x, clamped_value const& min, clamped_value const& max) noexcept; + template constexpr T clamp(T x, T min, T max) noexcept; + + //horizontal tests + namespace horiz + { + template constexpr T min(vec2 const& a) noexcept; + template constexpr T max(vec2 const& a) noexcept; + template constexpr T min(vec3 const& a) noexcept; + template constexpr T max(vec3 const& a) noexcept; + template constexpr T min(vec4 const& a) noexcept; + template constexpr T max(vec4 const& a) noexcept; + template constexpr T min(quat const& a) noexcept; + template constexpr T max(quat const& a) noexcept; + template constexpr T min(mat2 const& a) noexcept; + template constexpr T max(mat2 const& a) noexcept; + template constexpr T min(mat3 const& a) noexcept; + template constexpr T max(mat3 const& a) noexcept; + template constexpr T min(mat4 const& a) noexcept; + template constexpr T max(mat4 const& a) noexcept; + } + + //rounding + template T floor(T x) noexcept; + template T ceil(T x) noexcept; + template T fract(T x) noexcept; + + //rounds to the closest integer + template T round(T x) noexcept; + //round to decimal + template T round(T x, uint8_t decimals) noexcept; +} + +//#include "func_range.hpp" diff --git a/include/VMath/func_range.inl b/include/VMath/func_range.inl new file mode 100644 index 0000000..53cdcdd --- /dev/null +++ b/include/VMath/func_range.inl @@ -0,0 +1,345 @@ +#pragma once + +namespace math +{ + +////////////////////////////////////////////////////////////////////////// +// scalars + +namespace detail //unchecked min/max to use in clamp to avoid double checking +{ + template constexpr inline T min(T a, T b) noexcept + { + return a <= b ? a : b; + } + template constexpr inline T max(T a, T b) noexcept + { + return a >= b ? a : b; + } +} + +template constexpr inline T min(T a, T b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return a <= b ? a : b; +} +template constexpr inline T max(T a, T b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return a >= b ? a : b; +} +template constexpr inline vec2 min(vec2 const& a, vec2 const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return vec2(detail::min(a.x, b.x), detail::min(a.y, b.y)); +} +template constexpr inline vec3 min(vec3 const& a, vec3 const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return vec3(detail::min(a.x, b.x), detail::min(a.y, b.y), detail::min(a.z, b.z)); +} +template constexpr inline vec4 min(vec4 const& a, vec4 const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return vec4(detail::min(a.x, b.x), detail::min(a.y, b.y), detail::min(a.z, b.z), min(a.w, b.w)); +} +template constexpr inline clamped_value min(clamped_value const& a, clamped_value const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return clamped_value(detail::min(a.value(), b.value())); +} +template constexpr inline vec2 max(vec2 const& a, vec2 const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return vec2(detail::max(a.x, b.x), detail::max(a.y, b.y)); +} +template constexpr inline vec3 max(vec3 const& a, vec3 const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return vec3(detail::max(a.x, b.x), detail::max(a.y, b.y), detail::max(a.z, b.z)); +} +template constexpr inline vec4 max(vec4 const& a, vec4 const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return vec4(detail::max(a.x, b.x), detail::max(a.y, b.y), detail::max(a.z, b.z), detail::max(a.w, b.w)); +} +template constexpr inline clamped_value max(clamped_value const& a, clamped_value const& b) noexcept +{ + /*TL_PLAIN_ASSERT(is_finite(a) && is_finite(b));*/ + return clamped_value(detail::max(a.value(), b.value())); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr inline T clamp(T x, T min, T max) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)/* && is_finite(min) && is_finite(max)*/); + return detail::min(detail::max(x, min), max); +} +template constexpr inline vec2 clamp(vec2 const& x, vec2 const& min, vec2 const& max) noexcept +{ + return vec2(clamp(x.x, min.x, max.x), clamp(x.y, min.y, max.y)); +} +template constexpr inline vec3 clamp(vec3 const& x, vec3 const& min, vec3 const& max) noexcept +{ + return vec3(clamp(x.x, min.x, max.x), clamp(x.y, min.y, max.y), clamp(x.z, min.z, max.z)); +} +template constexpr inline vec4 clamp(vec4 const& x, vec4 const& min, vec4 const& max) noexcept +{ + return vec4(clamp(x.x, min.x, max.x), clamp(x.y, min.y, max.y), clamp(x.z, min.z, max.z), clamp(x.w, min.w, max.w)); +} +template constexpr inline clamped_value clamp(clamped_value const& x, clamped_value const& min, clamped_value const& max) noexcept +{ + return clamped_value(clamp(x.value(), min.value(), max.value())); +} + +////////////////////////////////////////////////////////////////////////// + +namespace horiz +{ +template constexpr inline T min(vec2 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::min(a.x, a.y); +} +template constexpr inline T max(vec2 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::max(a.x, a.y); +} +template constexpr inline T min(vec3 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::min(detail::min(a.x, a.y), a.z); +} +template constexpr inline T max(vec3 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::max(detail::max(a.x, a.y), a.z); +} +template constexpr inline T min(vec4 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::min(detail::min(detail::min(a.x, a.y), a.z), a.w); +} +template constexpr inline T max(vec4 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::max(detail::max(detail::max(a.x, a.y), a.z), a.w); +} +template constexpr inline T min(quat const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::min(detail::min(detail::min(a.x, a.y), a.z), a.w); +} +template constexpr inline T max(quat const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + return detail::max(detail::max(detail::max(a.x, a.y), a.z), a.w); +} +template constexpr inline T min(mat2 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + T x = detail::min(a.m[0], a.m[1]); + x = detail::min(x, a.m[2]); + x = detail::min(x, a.m[3]); + return x; +} +template constexpr inline T max(mat2 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + T x = detail::max(a.m[0], a.m[1]); + x = detail::max(x, a.m[2]); + x = detail::max(x, a.m[3]); + return x; +} +template constexpr inline T min(mat3 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + T x = detail::min(a.m[0], a.m[1]); + for (size_t i = 2; i < a.element_count; i++) + x = detail::min(x, a.m[i]); + return x; +} +template constexpr inline T max(mat3 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + T x = detail::max(a.m[0], a.m[1]); + for (size_t i = 2; i < a.element_count; i++) + x = detail::max(x, a.m[i]); + return x; +} +template constexpr inline T min(mat4 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + T x = detail::min(a.m[0], a.m[1]); + for (size_t i = 2; i < a.element_count; i++) + x = detail::min(x, a.m[i]); + return x; +} +template constexpr inline T max(mat4 const& a) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(a)); + T x = detail::max(a.m[0], a.m[1]); + for (size_t i = 2; i < a.element_count; i++) + x = detail::max(x, a.m[i]); + return x; +} +} + +////////////////////////////////////////////////////////////////////////// + +template inline T floor(T x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return std::floor(x); +} +template<> inline float floor(float x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return ::floorf(x); +} +template constexpr inline vec2 floor(vec2 const& x) noexcept +{ + return vec2(floor(x.x), floor(x.y)); +} +template constexpr inline vec3 floor(vec3 const& x) noexcept +{ + return vec3(floor(x.x), floor(x.y), floor(x.z)); +} +template constexpr inline vec4 floor(vec4 const& x) noexcept +{ + return vec4(floor(x.x), floor(x.y), floor(x.z), floor(x.w)); +} +template constexpr inline clamped_value floor(clamped_value const& x) noexcept +{ + return clamped_value(floor(x.value())); +} + +template inline T ceil(T x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return std::ceil(x); +} +template<> inline float ceil(float x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return ceilf(x); +} +template constexpr inline vec2 ceil(vec2 const& x) noexcept +{ + return vec2(ceil(x.x), ceil(x.y)); +} +template constexpr inline vec3 ceil(vec3 const& x) noexcept +{ + return vec3(ceil(x.x), ceil(x.y), ceil(x.z)); +} +template constexpr inline vec4 ceil(vec4 const& x) noexcept +{ + return vec4(ceil(x.x), ceil(x.y), ceil(x.z), ceil(x.w)); +} +template constexpr inline clamped_value ceil(clamped_value const& x) noexcept +{ + return clamped_value(ceil(x.value())); +} + + +template inline T fract(T x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return x - floor(x); +} +template constexpr inline vec2 fract(vec2 const& x) noexcept +{ + return vec2(fract(x.x), fract(x.y)); +} +template constexpr inline vec3 fract(vec3 const& x) noexcept +{ + return vec3(fract(x.x), fract(x.y), fract(x.z)); +} +template constexpr inline vec4 fract(vec4 const& x) noexcept +{ + return vec4(fract(x.x), fract(x.y), fract(x.z), fract(x.w)); +} +template constexpr inline clamped_value fract(clamped_value const& x) noexcept +{ + return clamped_value(fract(x.value)); +} + +//rounds to the closest integer +template inline T round(T x) noexcept; //no implementation + +//rounds to the closest integer +template<> inline float round(float x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return floor(x + 0.5f); +} +template<> inline double round(double x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + return floor(x + 0.5); +} + +template constexpr inline vec2 round(vec2 const& x) noexcept +{ + return vec2(round(x.x), round(x.y)); +} +template constexpr inline vec3 round(vec3 const& x) noexcept +{ + return vec3(round(x.x), round(x.y), round(x.z)); +} +template constexpr inline vec4 round(vec4 const& x) noexcept +{ + return vec4(round(x.x), round(x.y), round(x.z), round(x.w)); +} +template constexpr inline clamped_value round(clamped_value const& x) noexcept +{ + return clamped_value(round(x.value())); +} + + +//rounds to the closest integer +template inline T round(T x, uint8_t decimals) noexcept; //no implementation + +//rounds to the closest integer +template<> inline float round(float x, uint8_t decimals) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + const float ix = floor(x); + const float frac = x - ix; + const float p = ::powf(10.f, (float)decimals); + float f = floor(round(frac * p)); + f /= p; + return ix + f; +} +template<> inline double round(double x, uint8_t decimals) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x)); + const double ix = floor(x); + const double frac = x - ix; + const double p = ::pow(10.0, (double)decimals); + double f = floor(round(frac * p)); + f /= p; + return ix + f; +} + +template constexpr inline vec2 round(vec2 const& x, uint8_t decimals) noexcept +{ + return vec2(round(x.x, decimals), round(x.y, decimals)); +} +template constexpr inline vec3 round(vec3 const& x, uint8_t decimals) noexcept +{ + return vec3(round(x.x, decimals), round(x.y, decimals), round(x.z, decimals)); +} +template constexpr inline vec4 round(vec4 const& x, uint8_t decimals) noexcept +{ + return vec4(round(x.x, decimals), round(x.y, decimals), round(x.z, decimals), round(x.w, decimals)); +} +template constexpr inline clamped_value round(clamped_value const& x, uint8_t decimals) noexcept +{ + return clamped_value(round(x.value(), decimals)); +} + + +} diff --git a/include/VMath/func_string.h b/include/VMath/func_string.h new file mode 100644 index 0000000..abac198 --- /dev/null +++ b/include/VMath/func_string.h @@ -0,0 +1,85 @@ +#pragma once + +#include "tl/format.h" + +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::vec2& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "{},{}", v.x, v.y); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::vec3& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "{},{},{}", v.x, v.y, v.z); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::vec4& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "{},{},{},{}", v.x, v.y, v.z, v.w); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::quat& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "{},{},{},{}", v.x, v.y, v.z, v.w); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::mat2& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "[{}][{}]", v.get_column(0), v.get_column(1)); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::mat3& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "[{}][{}][{}]", v.get_column(0), v.get_column(1), v.get_column(2)); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::mat4& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "[{}][{}][{}][{}]", v.get_column(0), v.get_column(1), v.get_column(2), v.get_column(3)); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::trans2& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "[{}][{}][{}]", v.get_axis_x(), v.get_axis_y(), v.get_translation()); + } +}; +template +struct std::formatter> +{ + constexpr auto parse(format_parse_context& ctx) noexcept { return ctx.begin(); } + auto format(const math::trans3& v, std::format_context& ctx) const noexcept + { + return format_to(ctx.out(), "[{}][{}][{}][{}]", v.get_axis_x(), v.get_axis_y(), v.get_axis_z(), v.get_translation()); + } +}; diff --git a/include/VMath/func_test.h b/include/VMath/func_test.h new file mode 100644 index 0000000..9b0bc60 --- /dev/null +++ b/include/VMath/func_test.h @@ -0,0 +1,322 @@ +#pragma once + +#include + +namespace math +{ + +template inline bool is_equal(T v1, T v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template<> inline bool is_equal(float v1, float v2, float tolerance) noexcept; +template inline bool is_equal(angle const& v1, angle const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(vec2 const& v1, vec2 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(vec3 const& v1, vec3 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(vec4 const& v1, vec4 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(quat const& v1, quat const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(mat2 const& v1, mat2 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(mat3 const& v1, mat3 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(mat4 const& v1, mat4 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(trans3 const& v1, trans3 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(trans2 const& v1, trans2 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(rigid3 const& v1, rigid3 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline bool is_equal(rigid2 const& v1, rigid2 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_less(T v1, T v2) noexcept; +template constexpr bool is_less(angle const& v1, angle const& v2) noexcept; +template constexpr bool is_less(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr bool is_less(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr bool is_less(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr bool is_less(quat const& v1, quat const& v2) noexcept; +template constexpr bool is_less(mat2 const& v1, mat2 const& v2) noexcept; +template constexpr bool is_less(mat3 const& v1, mat3 const& v2) noexcept; +template constexpr bool is_less(mat4 const& v1, mat4 const& v2) noexcept; +template constexpr bool is_less(trans3 const& v1, trans3 const& v2) noexcept; +template constexpr bool is_less(trans2 const& v1, trans2 const& v2) noexcept; +template constexpr bool is_less(rigid3 const& v1, rigid3 const& v2) noexcept; +template constexpr bool is_less(rigid2 const& v1, rigid2 const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_lequal(T v1, T v2) noexcept; +template constexpr bool is_lequal(angle const& v1, angle const& v2) noexcept; +template constexpr bool is_lequal(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr bool is_lequal(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr bool is_lequal(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr bool is_lequal(quat const& v1, quat const& v2) noexcept; +template constexpr bool is_lequal(mat2 const& v1, mat2 const& v2) noexcept; +template constexpr bool is_lequal(mat3 const& v1, mat3 const& v2) noexcept; +template constexpr bool is_lequal(mat4 const& v1, mat4 const& v2) noexcept; +template constexpr bool is_lequal(trans3 const& v1, trans3 const& v2) noexcept; +template constexpr bool is_lequal(trans2 const& v1, trans2 const& v2) noexcept; +template constexpr bool is_lequal(rigid3 const& v1, rigid3 const& v2) noexcept; +template constexpr bool is_lequal(rigid2 const& v1, rigid2 const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_greater(T v1, T v2) noexcept; +template constexpr bool is_greater(angle const& v1, angle const& v2) noexcept; +template constexpr bool is_greater(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr bool is_greater(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr bool is_greater(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr bool is_greater(quat const& v1, quat const& v2) noexcept; +template constexpr bool is_greater(mat2 const& v1, mat2 const& v2) noexcept; +template constexpr bool is_greater(mat3 const& v1, mat3 const& v2) noexcept; +template constexpr bool is_greater(mat4 const& v1, mat4 const& v2) noexcept; +template constexpr bool is_greater(trans3 const& v1, trans3 const& v2) noexcept; +template constexpr bool is_greater(trans2 const& v1, trans2 const& v2) noexcept; +template constexpr bool is_greater(rigid3 const& v1, rigid3 const& v2) noexcept; +template constexpr bool is_greater(rigid2 const& v1, rigid2 const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_gequal(T v1, T v2) noexcept; +template constexpr bool is_gequal(angle const& v1, angle const& v2) noexcept; +template constexpr bool is_gequal(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr bool is_gequal(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr bool is_gequal(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr bool is_gequal(quat const& v1, quat const& v2) noexcept; +template constexpr bool is_gequal(mat2 const& v1, mat2 const& v2) noexcept; +template constexpr bool is_gequal(mat3 const& v1, mat3 const& v2) noexcept; +template constexpr bool is_gequal(mat4 const& v1, mat4 const& v2) noexcept; +template constexpr bool is_gequal(trans3 const& v1, trans3 const& v2) noexcept; +template constexpr bool is_gequal(trans2 const& v1, trans2 const& v2) noexcept; +template constexpr bool is_gequal(rigid3 const& v1, rigid3 const& v2) noexcept; +template constexpr bool is_gequal(rigid2 const& v1, rigid2 const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_zero(T v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(angle const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(vec2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(vec3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(vec4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(quat const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(mat2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(mat3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(mat4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(trans3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(trans2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(rigid3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_zero(rigid2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_one(T v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(vec2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(vec3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(vec4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(quat const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(mat2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(mat3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(mat4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(trans3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(trans2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(rigid3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_one(rigid2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template inline bool is_positive(T v) noexcept; +template<> inline bool is_positive(float v) noexcept; +template<> inline bool is_positive(uint8_t v) noexcept; +template<> inline bool is_positive(uint16_t v) noexcept; +template<> inline bool is_positive(uint32_t v) noexcept; +template<> inline bool is_positive(uint64_t v) noexcept; +template inline bool is_positive(angle const& v) noexcept; +template inline bool is_positive(vec2 const& v) noexcept; +template inline bool is_positive(vec3 const& v) noexcept; +template inline bool is_positive(vec4 const& v) noexcept; +template inline bool is_positive(quat const& v) noexcept; +template inline bool is_positive(mat2 const& v) noexcept; +template inline bool is_positive(mat3 const& v) noexcept; +template inline bool is_positive(mat4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template inline bool is_negative(T v) noexcept; +template<> inline bool is_negative(float v) noexcept; +template<> inline bool is_negative(float v) noexcept; +template<> inline bool is_negative(uint8_t v) noexcept; +template<> inline bool is_negative(uint16_t v) noexcept; +template<> inline bool is_negative(uint32_t v) noexcept; +template<> inline bool is_negative(uint64_t v) noexcept; +template inline bool is_negative(angle const& v) noexcept; +template inline bool is_negative(vec2 const& v) noexcept; +template inline bool is_negative(vec3 const& v) noexcept; +template inline bool is_negative(vec4 const& v) noexcept; +template inline bool is_negative(quat const& v) noexcept; +template inline bool is_negative(mat2 const& v) noexcept; +template inline bool is_negative(mat3 const& v) noexcept; +template inline bool is_negative(mat4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +constexpr bool is_nan(float f) noexcept; +constexpr bool is_nan(double d) noexcept; +template constexpr bool is_nan(T) noexcept; +template constexpr bool is_nan(angle const& v) noexcept; +template constexpr bool is_nan(vec2 const& v) noexcept; +template constexpr bool is_nan(vec3 const& v) noexcept; +template constexpr bool is_nan(vec4 const& v) noexcept; +template constexpr bool is_nan(quat const& v) noexcept; +template constexpr bool is_nan(mat2 const& v) noexcept; +template constexpr bool is_nan(mat3 const& v) noexcept; +template constexpr bool is_nan(mat4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +constexpr bool is_infinite(float f) noexcept; +constexpr bool is_infinite(double d) noexcept; +template constexpr bool is_infinite(T) noexcept; +template constexpr bool is_infinite(angle const& v) noexcept; +template constexpr bool is_infinite(vec2 const& v) noexcept; +template constexpr bool is_infinite(vec3 const& v) noexcept; +template constexpr bool is_infinite(vec4 const& v) noexcept; +template constexpr bool is_infinite(quat const& v) noexcept; +template constexpr bool is_infinite(mat2 const& v) noexcept; +template constexpr bool is_infinite(mat3 const& v) noexcept; +template constexpr bool is_infinite(mat4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +constexpr bool is_finite(float f) noexcept; +constexpr bool is_finite(double d) noexcept; +template constexpr bool is_finite(T) noexcept; +template constexpr bool is_finite(angle const& v) noexcept; +template constexpr bool is_finite(vec2 const& v) noexcept; +template constexpr bool is_finite(vec3 const& v) noexcept; +template constexpr bool is_finite(vec4 const& v) noexcept; +template constexpr bool is_finite(quat const& v) noexcept; +template constexpr bool is_finite(mat2 const& v) noexcept; +template constexpr bool is_finite(mat3 const& v) noexcept; +template constexpr bool is_finite(mat4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_identity(mat2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(mat3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(mat4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(quat const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(trans3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(trans2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(rigid3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_identity(rigid2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_normalized(vec2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_normalized(vec3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_normalized(vec4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr bool is_normalized(quat const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +namespace cwise +{ +template inline vec2 is_equal(vec2 const& v1, vec2 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline vec3 is_equal(vec3 const& v1, vec3 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline vec4 is_equal(vec4 const& v1, vec4 const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template inline vec4 is_equal(quat const& v1, quat const& v2, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +template<> inline vec2 is_equal(vec2 const& v1, vec2 const& v2, float tolerance) noexcept; +template<> inline vec3 is_equal(vec3 const& v1, vec3 const& v2, float tolerance) noexcept; +template<> inline vec4 is_equal(vec4 const& v1, vec4 const& v2, float tolerance) noexcept; +template<> inline vec4 is_equal(quat const& v1, quat const& v2, float tolerance) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_less(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr vec3 is_less(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr vec4 is_less(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr vec4 is_less(quat const& v1, quat const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_lequal(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr vec3 is_lequal(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr vec4 is_lequal(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr vec4 is_lequal(quat const& v1, quat const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_greater(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr vec3 is_greater(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr vec4 is_greater(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr vec4 is_greater(quat const& v1, quat const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_gequal(vec2 const& v1, vec2 const& v2) noexcept; +template constexpr vec3 is_gequal(vec3 const& v1, vec3 const& v2) noexcept; +template constexpr vec4 is_gequal(vec4 const& v1, vec4 const& v2) noexcept; +template constexpr vec4 is_gequal(quat const& v1, quat const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_zero(vec2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr vec3 is_zero(vec3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr vec4 is_zero(vec4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr vec4 is_zero(quat const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_one(vec2 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr vec3 is_one(vec3 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr vec4 is_one(vec4 const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; +template constexpr vec4 is_one(quat const& v, T tolerance = tl::numeric_limits::epsilon()) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_positive(vec2 const& v) noexcept; +template constexpr vec3 is_positive(vec3 const& v) noexcept; +template constexpr vec4 is_positive(vec4 const& v) noexcept; +template constexpr vec4 is_positive(quat const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_negative(vec2 const& v) noexcept; +template constexpr vec3 is_negative(vec3 const& v) noexcept; +template constexpr vec4 is_negative(vec4 const& v) noexcept; +template constexpr vec4 is_negative(quat const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_nan(vec2 const& v) noexcept; +template constexpr vec3 is_nan(vec3 const& v) noexcept; +template constexpr vec4 is_nan(vec4 const& v) noexcept; +template constexpr vec4 is_nan(quat const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_infinite(vec2 const& v) noexcept; +template constexpr vec3 is_infinite(vec3 const& v) noexcept; +template constexpr vec4 is_infinite(vec4 const& v) noexcept; +template constexpr vec4 is_infinite(quat const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_finite(vec2 const& v) noexcept; +template constexpr vec3 is_finite(vec3 const& v) noexcept; +template constexpr vec4 is_finite(vec4 const& v) noexcept; +template constexpr vec4 is_finite(quat const& v) noexcept; + + +////////////////////////////////////////////////////////////////////////// + + +constexpr bool all(vec2 const& v) noexcept; +constexpr bool all(vec3 const& v) noexcept; +constexpr bool all(vec4 const& v) noexcept; +constexpr bool any(vec2 const& v) noexcept; +constexpr bool any(vec3 const& v) noexcept; +constexpr bool any(vec4 const& v) noexcept; +constexpr bool none(vec2 const& v) noexcept; +constexpr bool none(vec3 const& v) noexcept; +constexpr bool none(vec4 const& v) noexcept; + +} //cwise + +} + + diff --git a/include/VMath/func_test.inl b/include/VMath/func_test.inl new file mode 100644 index 0000000..626b73e --- /dev/null +++ b/include/VMath/func_test.inl @@ -0,0 +1,1282 @@ +namespace math +{ +////////////////////////////////////////////////////////////////////////// + +template inline bool is_equal(T v1, T v2, T tolerance) noexcept +{ + TL_PLAIN_ASSERT(/*is_finite(v1) && is_finite(v2) && */is_finite(tolerance)); + TL_PLAIN_ASSERT(tolerance >= T(0)); + return abs(v1 - v2) <= tolerance; +} +template<> inline bool is_equal(float v1, float v2, float tolerance) noexcept +{ + TL_PLAIN_ASSERT(/*is_finite(v1) && is_finite(v2) && */is_finite(tolerance)); + TL_PLAIN_ASSERT(tolerance >= 0); + // http://realtimecollisiondetection.net/pubs/Tolerances/ + // Abs(x - y) <= Max(absTol, relTol * Max(Abs(x), Abs(y))) + // we assume absTol=relTol, this leaves + // Abs(x - y) <= absTol * Max(1.0f, Abs(x), Abs(y)) + return abs(v1 - v2) <= tolerance * max(max(1.0f, abs(v1)), abs(v2)); +} +template inline bool is_equal(angle const& v1, angle const& v2, T tolerance) noexcept +{ + return is_equal(v1.radians, v2.radians, tolerance); +} +template inline bool is_equal(vec2 const& v1, vec2 const& v2, T tolerance) noexcept +{ + return math::is_equal(v1.x, v2.x, tolerance) && + math::is_equal(v1.y, v2.y, tolerance); +} +template inline bool is_equal(vec3 const& v1, vec3 const& v2, T tolerance) noexcept +{ + return math::is_equal(v1.x, v2.x, tolerance) && + math::is_equal(v1.y, v2.y, tolerance) && + math::is_equal(v1.z, v2.z, tolerance); +} +template inline bool is_equal(vec4 const& v1, vec4 const& v2, T tolerance) noexcept +{ + return math::is_equal(v1.x, v2.x, tolerance) && + math::is_equal(v1.y, v2.y, tolerance) && + math::is_equal(v1.z, v2.z, tolerance) && + math::is_equal(v1.w, v2.w, tolerance); +} +template inline bool is_equal(quat const& v1, quat const& v2, T tolerance) noexcept +{ + return math::is_equal(v1.x, v2.x, tolerance) && + math::is_equal(v1.y, v2.y, tolerance) && + math::is_equal(v1.z, v2.z, tolerance) && + math::is_equal(v1.w, v2.w, tolerance); +} +template inline bool is_equal(mat2 const& v1, mat2 const& v2, T tolerance) noexcept +{ + return is_equal(v1.column0, v2.column0, tolerance) && + is_equal(v1.column1, v2.column1, tolerance); +} +template inline bool is_equal(mat3 const& v1, mat3 const& v2, T tolerance) noexcept +{ + return is_equal(v1.column0, v2.column0, tolerance) && + is_equal(v1.column1, v2.column1, tolerance) && + is_equal(v1.column2, v2.column2, tolerance); +} +template inline bool is_equal(mat4 const& v1, mat4 const& v2, T tolerance) noexcept +{ + return is_equal(v1.column0, v2.column0, tolerance) && + is_equal(v1.column1, v2.column1, tolerance) && + is_equal(v1.column2, v2.column2, tolerance) && + is_equal(v1.column3, v2.column3, tolerance); +} +template inline bool is_equal(trans3 const& v1, trans3 const& v2, T tolerance) noexcept +{ + return is_equal(v1.rotation_scale, v2.rotation_scale, tolerance) && is_equal(v1.translation, v2.translation, tolerance); +} +template inline bool is_equal(trans2 const& v1, trans2 const& v2, T tolerance) noexcept +{ + return is_equal(v1.rotation_scale, v2.rotation_scale, tolerance) && is_equal(v1.translation, v2.translation, tolerance); +} +template inline bool is_equal(rigid3 const& v1, rigid3 const& v2, T tolerance) noexcept +{ + return is_equal(v1.translation, v2.translation, tolerance) && is_equal(v1.rotation, v2.rotation, tolerance); +} +template inline bool is_equal(rigid2 const& v1, rigid2 const& v2, T tolerance) noexcept +{ + return is_equal(v1.translation, v2.translation, tolerance) && is_equal(v1.rotation, v2.rotation, tolerance); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_less(T v1, T v2) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v1) && is_finite(v2)); + return v1 < v2; +} +template constexpr bool is_less(angle const& v1, angle const& v2) noexcept +{ + return is_less(v1.radians, v2.radians); +} +template constexpr bool is_less(vec2 const& v1, vec2 const& v2) noexcept +{ + return math::is_less(v1.x, v2.x) && + math::is_less(v1.y, v2.y); +} +template constexpr bool is_less(vec3 const& v1, vec3 const& v2) noexcept +{ + return math::is_less(v1.x, v2.x) && + math::is_less(v1.y, v2.y) && + math::is_less(v1.z, v2.z); +} +template constexpr bool is_less(vec4 const& v1, vec4 const& v2) noexcept +{ + return math::is_less(v1.x, v2.x) && + math::is_less(v1.y, v2.y) && + math::is_less(v1.z, v2.z) && + math::is_less(v1.w, v2.w); +} +template constexpr bool is_less(quat const& v1, quat const& v2) noexcept +{ + return math::is_less(v1.x, v2.x) && + math::is_less(v1.y, v2.y) && + math::is_less(v1.z, v2.z) && + math::is_less(v1.w, v2.w); +} +template constexpr bool is_less(mat2 const& v1, mat2 const& v2) noexcept +{ + return is_less(v1.column0, v2.column0) && + is_less(v1.column1, v2.column1); +} +template constexpr bool is_less(mat3 const& v1, mat3 const& v2) noexcept +{ + return is_less(v1.column0, v2.column0) && + is_less(v1.column1, v2.column1) && + is_less(v1.column2, v2.column2); +} +template constexpr bool is_less(mat4 const& v1, mat4 const& v2) noexcept +{ + return is_less(v1.column0, v2.column0) && + is_less(v1.column1, v2.column1) && + is_less(v1.column2, v2.column2) && + is_less(v1.column3, v2.column3); +} +template constexpr bool is_less(trans3 const& v1, trans3 const& v2) noexcept +{ + return is_less(v1.mat, v2.mat); +} +template constexpr bool is_less(trans2 const& v1, trans2 const& v2) noexcept +{ + return is_less(v1.mat, v2.mat); +} +template constexpr bool is_less(rigid3 const& v1, rigid3 const& v2) noexcept +{ + return is_less(v1.translation, v2.translation) && is_less(v1.rotation, v2.rotation); +} +template constexpr bool is_less(rigid2 const& v1, rigid2 const& v2) noexcept +{ + return is_less(v1.translation, v2.translation) && is_less(v1.rotation, v2.rotation); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_lequal(T v1, T v2) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v1) && is_finite(v2)); + return v1 <= v2; +} +template constexpr bool is_lequal(angle const& v1, angle const& v2) noexcept +{ + return is_lequal(v1.radians, v2.radians); +} +template constexpr bool is_lequal(vec2 const& v1, vec2 const& v2) noexcept +{ + return math::is_lequal(v1.x, v2.x) && + math::is_lequal(v1.y, v2.y); +} +template constexpr bool is_lequal(vec3 const& v1, vec3 const& v2) noexcept +{ + return math::is_lequal(v1.x, v2.x) && + math::is_lequal(v1.y, v2.y) && + math::is_lequal(v1.z, v2.z); +} +template constexpr bool is_lequal(vec4 const& v1, vec4 const& v2) noexcept +{ + return math::is_lequal(v1.x, v2.x) && + math::is_lequal(v1.y, v2.y) && + math::is_lequal(v1.z, v2.z) && + math::is_lequal(v1.w, v2.w); +} +template constexpr bool is_lequal(quat const& v1, quat const& v2) noexcept +{ + return math::is_lequal(v1.x, v2.x) && + math::is_lequal(v1.y, v2.y) && + math::is_lequal(v1.z, v2.z) && + math::is_lequal(v1.w, v2.w); +} +template constexpr bool is_lequal(mat2 const& v1, mat2 const& v2) noexcept +{ + return is_lequal(v1.column0, v2.column0) && + is_lequal(v1.column1, v2.column1); +} +template constexpr bool is_lequal(mat3 const& v1, mat3 const& v2) noexcept +{ + return is_lequal(v1.column0, v2.column0) && + is_lequal(v1.column1, v2.column1) && + is_lequal(v1.column2, v2.column2); +} +template constexpr bool is_lequal(mat4 const& v1, mat4 const& v2) noexcept +{ + return is_lequal(v1.column0, v2.column0) && + is_lequal(v1.column1, v2.column1) && + is_lequal(v1.column2, v2.column2) && + is_lequal(v1.column3, v2.column3); +} +template constexpr bool is_lequal(trans3 const& v1, trans3 const& v2) noexcept +{ + return is_lequal(v1.mat, v2.mat); +} +template constexpr bool is_lequal(trans2 const& v1, trans2 const& v2) noexcept +{ + return is_lequal(v1.mat, v2.mat); +} +template constexpr bool is_lequal(rigid3 const& v1, rigid3 const& v2) noexcept +{ + return is_lequal(v1.translation, v2.translation) && is_lequal(v1.rotation, v2.rotation); +} +template constexpr bool is_lequal(rigid2 const& v1, rigid2 const& v2) noexcept +{ + return is_lequal(v1.translation, v2.translation) && is_lequal(v1.rotation, v2.rotation); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_greater(T v1, T v2) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v1) && is_finite(v2)); + return v1 > v2; +} +template constexpr bool is_greater(angle const& v1, angle const& v2) noexcept +{ + return is_greater(v1.radians, v2.radians); +} +template constexpr bool is_greater(vec2 const& v1, vec2 const& v2) noexcept +{ + return math::is_greater(v1.x, v2.x) && + math::is_greater(v1.y, v2.y); +} +template constexpr bool is_greater(vec3 const& v1, vec3 const& v2) noexcept +{ + return math::is_greater(v1.x, v2.x) && + math::is_greater(v1.y, v2.y) && + math::is_greater(v1.z, v2.z); +} +template constexpr bool is_greater(vec4 const& v1, vec4 const& v2) noexcept +{ + return math::is_greater(v1.x, v2.x) && + math::is_greater(v1.y, v2.y) && + math::is_greater(v1.z, v2.z) && + math::is_greater(v1.w, v2.w); +} +template constexpr bool is_greater(quat const& v1, quat const& v2) noexcept +{ + return math::is_greater(v1.x, v2.x) && + math::is_greater(v1.y, v2.y) && + math::is_greater(v1.z, v2.z) && + math::is_greater(v1.w, v2.w); +} +template constexpr bool is_greater(mat2 const& v1, mat2 const& v2) noexcept +{ + return is_greater(v1.column0, v2.column0) && + is_greater(v1.column1, v2.column1); +} +template constexpr bool is_greater(mat3 const& v1, mat3 const& v2) noexcept +{ + return is_greater(v1.column0, v2.column0) && + is_greater(v1.column1, v2.column1) && + is_greater(v1.column2, v2.column2); +} +template constexpr bool is_greater(mat4 const& v1, mat4 const& v2) noexcept +{ + return is_greater(v1.column0, v2.column0) && + is_greater(v1.column1, v2.column1) && + is_greater(v1.column2, v2.column2) && + is_greater(v1.column3, v2.column3); +} +template constexpr bool is_greater(trans3 const& v1, trans3 const& v2) noexcept +{ + return is_greater(v1.mat, v2.mat); +} +template constexpr bool is_greater(trans2 const& v1, trans2 const& v2) noexcept +{ + return is_greater(v1.mat, v2.mat); +} +template constexpr bool is_greater(rigid3 const& v1, rigid3 const& v2) noexcept +{ + return is_greater(v1.translation, v2.translation) && is_greater(v1.rotation, v2.rotation); +} +template constexpr bool is_greater(rigid2 const& v1, rigid2 const& v2) noexcept +{ + return is_greater(v1.translation, v2.translation) && is_greater(v1.rotation, v2.rotation); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr bool is_gequal(T v1, T v2) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v1) && is_finite(v2)); + return v1 >= v2; +} +template constexpr bool is_gequal(angle const& v1, angle const& v2) noexcept +{ + return is_gequal(v1.radians, v2.radians); +} +template constexpr bool is_gequal(vec2 const& v1, vec2 const& v2) noexcept +{ + return math::is_gequal(v1.x, v2.x) && + math::is_gequal(v1.y, v2.y); +} +template constexpr bool is_gequal(vec3 const& v1, vec3 const& v2) noexcept +{ + return math::is_gequal(v1.x, v2.x) && + math::is_gequal(v1.y, v2.y) && + math::is_gequal(v1.z, v2.z); +} +template constexpr bool is_gequal(vec4 const& v1, vec4 const& v2) noexcept +{ + return math::is_gequal(v1.x, v2.x) && + math::is_gequal(v1.y, v2.y) && + math::is_gequal(v1.z, v2.z) && + math::is_gequal(v1.w, v2.w); +} +template constexpr bool is_gequal(quat const& v1, quat const& v2) noexcept +{ + return math::is_gequal(v1.x, v2.x) && + math::is_gequal(v1.y, v2.y) && + math::is_gequal(v1.z, v2.z) && + math::is_gequal(v1.w, v2.w); +} +template constexpr bool is_gequal(mat2 const& v1, mat2 const& v2) noexcept +{ + return is_gequal(v1.column0, v2.column0) && + is_gequal(v1.column1, v2.column1); +} +template constexpr bool is_gequal(mat3 const& v1, mat3 const& v2) noexcept +{ + return is_gequal(v1.column0, v2.column0) && + is_gequal(v1.column1, v2.column1) && + is_gequal(v1.column2, v2.column2); +} +template constexpr bool is_gequal(mat4 const& v1, mat4 const& v2) noexcept +{ + return is_gequal(v1.column0, v2.column0) && + is_gequal(v1.column1, v2.column1) && + is_gequal(v1.column2, v2.column2) && + is_gequal(v1.column3, v2.column3); +} +template constexpr bool is_gequal(trans3 const& v1, trans3 const& v2) noexcept +{ + return is_gequal(v1.mat, v2.mat); +} +template constexpr bool is_gequal(trans2 const& v1, trans2 const& v2) noexcept +{ + return is_gequal(v1.mat, v2.mat); +} +template constexpr bool is_gequal(rigid3 const& v1, rigid3 const& v2) noexcept +{ + return is_gequal(v1.translation, v2.translation) && is_gequal(v1.rotation, v2.rotation); +} +template constexpr bool is_gequal(rigid2 const& v1, rigid2 const& v2) noexcept +{ + return is_gequal(v1.translation, v2.translation) && is_gequal(v1.rotation, v2.rotation); +} + +////////////////////////////////////////////////////////////////////////// +// zero with tolerance + +template constexpr bool is_zero(T v, T tolerance) noexcept +{ + TL_PLAIN_ASSERT(/*is_finite(v) && */is_finite(tolerance)); + TL_PLAIN_ASSERT(tolerance >= 0); + return abs(v) <= tolerance; +} +template constexpr bool is_zero(angle const& v, T tolerance) noexcept +{ + return is_zero(v.radians, tolerance); +} +template constexpr bool is_zero(vec2 const& v, T tolerance) noexcept +{ + return math::is_zero(v.x, tolerance) && + math::is_zero(v.y, tolerance); +} +template constexpr bool is_zero(vec3 const& v, T tolerance) noexcept +{ + return math::is_zero(v.x, tolerance) && + math::is_zero(v.y, tolerance) && + math::is_zero(v.z, tolerance); +} +template constexpr bool is_zero(vec4 const& v, T tolerance) noexcept +{ + return math::is_zero(v.x, tolerance) && + math::is_zero(v.y, tolerance) && + math::is_zero(v.z, tolerance) && + math::is_zero(v.w, tolerance); +} +template constexpr bool is_zero(quat const& v, T tolerance) noexcept +{ + return math::is_zero(v.x, tolerance) && + math::is_zero(v.y, tolerance) && + math::is_zero(v.z, tolerance) && + math::is_zero(v.w, tolerance); +} +template constexpr bool is_zero(mat2 const& v, T tolerance) noexcept +{ + return is_zero(v.column0, tolerance) && + is_zero(v.column1, tolerance); +} +template constexpr bool is_zero(mat3 const& v, T tolerance) noexcept +{ + return is_zero(v.column0, tolerance) && + is_zero(v.column1, tolerance) && + is_zero(v.column2, tolerance); +} +template constexpr bool is_zero(mat4 const& v, T tolerance) noexcept +{ + return is_zero(v.column0, tolerance) && + is_zero(v.column1, tolerance) && + is_zero(v.column2, tolerance) && + is_zero(v.column3, tolerance); +} +template constexpr bool is_zero(trans3 const& v, T tolerance) noexcept +{ + return is_zero(v.mat, tolerance); +} +template constexpr bool is_zero(trans2 const& v, T tolerance) noexcept +{ + return is_zero(v.mat, tolerance); +} +template constexpr bool is_zero(rigid3 const& v, T tolerance) noexcept +{ + return is_zero(v.translation, tolerance) && is_zero(v.rotation, tolerance); +} +template constexpr bool is_equal(rigid2 const& v, T tolerance) noexcept +{ + return is_zero(v.translation, tolerance) && is_zero(v.rotation, tolerance); +} + +////////////////////////////////////////////////////////////////////////// +// one with tolerance + +template constexpr bool is_one(T v, T tolerance) noexcept +{ + TL_PLAIN_ASSERT(/*is_finite(v) && */is_finite(tolerance)); + TL_PLAIN_ASSERT(tolerance >= 0); + return abs(v - T(1)) <= tolerance; +} +template constexpr bool is_one(vec2 const& v, T tolerance) noexcept +{ + return math::is_one(v.x, tolerance) && + math::is_one(v.y, tolerance); +} +template constexpr bool is_one(vec3 const& v, T tolerance) noexcept +{ + return math::is_one(v.x, tolerance) && + math::is_one(v.y, tolerance) && + math::is_one(v.z, tolerance); +} +template constexpr bool is_one(vec4 const& v, T tolerance) noexcept +{ + return math::is_one(v.x, tolerance) && + math::is_one(v.y, tolerance) && + math::is_one(v.z, tolerance) && + math::is_one(v.w, tolerance); +} +template constexpr bool is_one(quat const& v, T tolerance) noexcept +{ + return math::is_one(v.x, tolerance) && + math::is_one(v.y, tolerance) && + math::is_one(v.z, tolerance) && + math::is_one(v.w, tolerance); +} +template constexpr bool is_one(mat2 const& v, T tolerance) noexcept +{ + return is_one(v.column0, tolerance) && + is_one(v.column1, tolerance); +} +template constexpr bool is_one(mat3 const& v, T tolerance) noexcept +{ + return is_one(v.column0, tolerance) && + is_one(v.column1, tolerance) && + is_one(v.column2, tolerance); +} +template constexpr bool is_one(mat4 const& v, T tolerance) noexcept +{ + return is_one(v.column0, tolerance) && + is_one(v.column1, tolerance) && + is_one(v.column2, tolerance) && + is_one(v.column3, tolerance); +} +template constexpr bool is_one(trans3 const& v, T tolerance) noexcept +{ + return is_one(v.mat, tolerance); +} +template constexpr bool is_one(trans2 const& v, T tolerance) noexcept +{ + return is_one(v.mat, tolerance); +} +template constexpr bool is_one(rigid3 const& v, T tolerance) noexcept +{ + return is_one(v.translation, tolerance) && is_one(v.rotation, tolerance); +} +template constexpr bool is_one(rigid2 const& v, T tolerance) noexcept +{ + return is_one(v.translation, tolerance) && is_one(v.rotation, tolerance); +} + +////////////////////////////////////////////////////////////////////////// +// positive + +template inline bool is_positive(T v) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v)); + return v > T(0); +} +template<> inline bool is_positive(float v) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v)); + uint8_t const* __restrict lb = (reinterpret_cast(&v) + 3); //msb in little endian + return (*lb & 128) == 0; +} +template<> inline bool is_positive(uint8_t v) noexcept +{ + return true; +} +template<> inline bool is_positive(uint16_t v) noexcept +{ + return true; +} +template<> inline bool is_positive(uint32_t v) noexcept +{ + return true; +} +template<> inline bool is_positive(uint64_t v) noexcept +{ + return true; +} +template inline bool is_positive(angle const& v) noexcept +{ + return is_positive(v.radians); +} +template inline bool is_positive(vec2 const& v) noexcept +{ + return math::is_positive(v.x) && + math::is_positive(v.y); +} +template inline bool is_positive(vec3 const& v) noexcept +{ + return math::is_positive(v.x) && + math::is_positive(v.y) && + math::is_positive(v.z); +} +template inline bool is_positive(vec4 const& v) noexcept +{ + return math::is_positive(v.x) && + math::is_positive(v.y) && + math::is_positive(v.z) && + math::is_positive(v.w); +} +template inline bool is_positive(quat const& v) noexcept +{ + return math::is_positive(v.x) && + math::is_positive(v.y) && + math::is_positive(v.z) && + math::is_positive(v.w); +} +template inline bool is_positive(mat2 const& v) noexcept +{ + return is_positive(v.column0) && + is_positive(v.column1); +} +template inline bool is_positive(mat3 const& v) noexcept +{ + return is_positive(v.column0) && + is_positive(v.column1) && + is_positive(v.column2); +} +template inline bool is_positive(mat4 const& v) noexcept +{ + return is_positive(v.column0) && + is_positive(v.column1) && + is_positive(v.column2) && + is_positive(v.column3); +} + +////////////////////////////////////////////////////////////////////////// +// negative + +template inline bool is_negative(T v) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v)); + return v < T(0); +} +template<> inline bool is_negative(float v) noexcept +{ + //TL_PLAIN_ASSERT(is_finite(v)); + uint8_t const* __restrict lb = (reinterpret_cast(&v) + 3); //msb in little endian + return (*lb & 128) != 0; +} +template<> inline bool is_negative(uint8_t v) noexcept +{ + return false; +} +template<> inline bool is_negative(uint16_t v) noexcept +{ + return false; +} +template<> inline bool is_negative(uint32_t v) noexcept +{ + return false; +} +template<> inline bool is_negative(uint64_t v) noexcept +{ + return false; +} +template inline bool is_negative(angle const& v) noexcept +{ + return is_negative(v.radians); +} +template inline bool is_negative(vec2 const& v) noexcept +{ + return math::is_negative(v.x) && + math::is_negative(v.y); +} +template inline bool is_negative(vec3 const& v) noexcept +{ + return math::is_negative(v.x) && + math::is_negative(v.y) && + math::is_negative(v.z); +} +template inline bool is_negative(vec4 const& v) noexcept +{ + return math::is_negative(v.x) && + math::is_negative(v.y) && + math::is_negative(v.z) && + math::is_negative(v.w); +} +template inline bool is_negative(quat const& v) noexcept +{ + return math::is_negative(v.x) && + math::is_negative(v.y) && + math::is_negative(v.z) && + math::is_negative(v.w); +} +template inline bool is_negative(mat2 const& v) noexcept +{ + return is_negative(v.column0) && + is_negative(v.column1); +} +template inline bool is_negative(mat3 const& v) noexcept +{ + return is_negative(v.column0) && + is_negative(v.column1) && + is_negative(v.column2); +} +template inline bool is_negative(mat4 const& v) noexcept +{ + return is_negative(v.column0) && + is_negative(v.column1) && + is_negative(v.column2) && + is_negative(v.column3); +} + +////////////////////////////////////////////////////////////////////////// +// nan + +constexpr bool is_nan(float f) noexcept +{ + return f != f; +} +constexpr bool is_nan(double d) noexcept +{ + return d != d; +} +template constexpr bool is_nan(T) noexcept +{ + return false; +} +template constexpr bool is_nan(angle const& v) noexcept +{ + return is_nan(v.radians); +} +template constexpr bool is_nan(vec2 const& v) noexcept +{ + return is_nan(v.x) || is_nan(v.y); +} +template constexpr bool is_nan(vec3 const& v) noexcept +{ + return is_nan(v.x) || is_nan(v.y) || is_nan(v.z); +} +template constexpr bool is_nan(vec4 const& v) noexcept +{ + return is_nan(v.x) || is_nan(v.y) || is_nan(v.z) || is_nan(v.w); +} +template constexpr bool is_nan(quat const& v) noexcept +{ + return is_nan(v.x) || is_nan(v.y) || is_nan(v.z) || is_nan(v.w); +} +template constexpr bool is_nan(mat2 const& v) noexcept +{ + return is_nan(v.column0) || + is_nan(v.column1); +} +template constexpr bool is_nan(mat3 const& v) noexcept +{ + return is_nan(v.column0) || + is_nan(v.column1) || + is_nan(v.column2); +} +template constexpr bool is_nan(mat4 const& v) noexcept +{ + return is_nan(v.column0) || + is_nan(v.column1) || + is_nan(v.column2) || + is_nan(v.column3); +} + +////////////////////////////////////////////////////////////////////////// +// inf + +constexpr int __fpclassifyf(float x) noexcept +{ + union {float f; uint32_t i;} u = {x}; + int e = u.i>>23 & 0xff; + if (!e) return u.i<<1 ? FP_SUBNORMAL : FP_ZERO; + if (e==0xff) return u.i<<9 ? FP_NAN : FP_INFINITE; + return FP_NORMAL; +} +constexpr int __fpclassify(double x) noexcept +{ + union {double f; uint64_t i;} u = {x}; + int e = u.i>>52 & 0x7ff; + if (!e) return u.i<<1 ? FP_SUBNORMAL : FP_ZERO; + if (e==0x7ff) return u.i<<12 ? FP_NAN : FP_INFINITE; + return FP_NORMAL; +} + +constexpr bool is_infinite(float f) noexcept +{ + return __fpclassifyf(f) == FP_INFINITE; +} +constexpr bool is_infinite(double d) noexcept +{ + return __fpclassify(d) == FP_INFINITE; +} +template constexpr bool is_infinite(T) noexcept +{ + return false; +} +template constexpr bool is_infinite(angle const& v) noexcept +{ + return is_infinite(v.radians); +} +template constexpr bool is_infinite(vec2 const& v) noexcept +{ + return is_infinite(v.x) || is_infinite(v.y); +} +template constexpr bool is_infinite(vec3 const& v) noexcept +{ + return is_infinite(v.x) || is_infinite(v.y) || is_infinite(v.z); +} +template constexpr bool is_infinite(vec4 const& v) noexcept +{ + return is_infinite(v.x) || is_infinite(v.y) || is_infinite(v.z) || is_infinite(v.w); +} +template constexpr bool is_infinite(quat const& v) noexcept +{ + return is_infinite(v.x) || is_infinite(v.y) || is_infinite(v.z) || is_infinite(v.w); +} +template constexpr bool is_infinite(mat2 const& v) noexcept +{ + return is_infinite(v.column0) || + is_infinite(v.column1); +} +template constexpr bool is_infinite(mat3 const& v) noexcept +{ + return is_infinite(v.column0) || + is_infinite(v.column1) || + is_infinite(v.column2); +} +template constexpr bool is_infinite(mat4 const& v) noexcept +{ + return is_infinite(v.column0) || + is_infinite(v.column1) || + is_infinite(v.column2) || + is_infinite(v.column3); +} + +////////////////////////////////////////////////////////////////////////// +// finite + +constexpr bool is_finite(float f) noexcept +{ + return __fpclassifyf(f) != FP_INFINITE; +} +constexpr bool is_finite(double d) noexcept +{ + return __fpclassify(d) != FP_INFINITE; +} +template constexpr bool is_finite(T) noexcept +{ + return true; +} +template constexpr bool is_finite(angle const& v) noexcept +{ + return is_finite(v.radians); +} +template constexpr bool is_finite(vec2 const& v) noexcept +{ + return is_finite(v.x) && is_finite(v.y); +} +template constexpr bool is_finite(vec3 const& v) noexcept +{ + return is_finite(v.x) && is_finite(v.y) && is_finite(v.z); +} +template constexpr bool is_finite(vec4 const& v) noexcept +{ + return is_finite(v.x) && is_finite(v.y) && is_finite(v.z) && is_finite(v.w); +} +template constexpr bool is_finite(quat const& v) noexcept +{ + return is_finite(v.x) && is_finite(v.y) && is_finite(v.z) && is_finite(v.w); +} +template constexpr bool is_finite(mat2 const& v) noexcept +{ + return is_finite(v.column0) && + is_finite(v.column1); +} +template constexpr bool is_finite(mat3 const& v) noexcept +{ + return is_finite(v.column0) && + is_finite(v.column1) && + is_finite(v.column2); +} +template constexpr bool is_finite(mat4 const& v) noexcept +{ + return is_finite(v.column0) && + is_finite(v.column1) && + is_finite(v.column2) && + is_finite(v.column3); +} + +////////////////////////////////////////////////////////////////////////// +// identity with tolerance + +template constexpr bool is_identity(quat const& v, T tolerance) noexcept +{ + return is_equal(v, quat(), tolerance); +} +template constexpr bool is_identity(mat2 const& v, T tolerance) noexcept +{ + return is_equal(v, mat2(), tolerance); +} +template constexpr bool is_identity(mat3 const& v, T tolerance) noexcept +{ + return is_equal(v, mat3(), tolerance); +} +template constexpr bool is_identity(mat4 const& v, T tolerance) noexcept +{ + return is_equal(v, mat4(), tolerance); +} +template constexpr bool is_identity(trans3 const& v, T tolerance) noexcept +{ + return is_equal(v, trans3(), tolerance); +} +template constexpr bool is_identity(trans2 const& v, T tolerance) noexcept +{ + return is_equal(v, trans2(), tolerance); +} +template constexpr bool is_identity(rigid3 const& v, T tolerance) noexcept +{ + return is_equal(v, rigid3(), tolerance); +} +template constexpr bool is_identity(rigid2 const& v, T tolerance) noexcept +{ + return is_equal(v, rigid2(), tolerance); +} + +////////////////////////////////////////////////////////////////////////// +// normalized with tolerance + +template constexpr bool is_normalized(quat const& v, T tolerance) noexcept +{ + return is_one(v.length_sq(), tolerance); +} +template constexpr bool is_normalized(vec2 const& v, T tolerance) noexcept +{ + return is_one(v.length_sq(), tolerance); +} +template constexpr bool is_normalized(vec3 const& v, T tolerance) noexcept +{ + return is_one(v.length_sq(), tolerance); +} +template constexpr bool is_normalized(vec4 const& v, T tolerance) noexcept +{ + return is_one(v.length_sq(), tolerance); +} + + +////////////////////////////////////////////////////////////////////////// +// bool tests + +namespace cwise +{ +template inline vec2 is_equal(vec2 const& v1, vec2 const& v2, T tolerance) noexcept +{ + return vec2(math::is_equal(v1.x, v2.x, tolerance), + math::is_equal(v1.y, v2.y, tolerance)); +} +template inline vec3 is_equal(vec3 const& v1, vec3 const& v2, T tolerance) noexcept +{ + return vec3(math::is_equal(v1.x, v2.x, tolerance), + math::is_equal(v1.y, v2.y, tolerance), + math::is_equal(v1.z, v2.z, tolerance)); +} +template inline vec4 is_equal(vec4 const& v1, vec4 const& v2, T tolerance) noexcept +{ + return vec4(math::is_equal(v1.x, v2.x, tolerance), + math::is_equal(v1.y, v2.y, tolerance), + math::is_equal(v1.z, v2.z, tolerance), + math::is_equal(v1.w, v2.w, tolerance)); +} +template inline vec4 is_equal(quat const& v1, quat const& v2, T tolerance) noexcept +{ + return cwise::is_equal(reinterpret_cast const&>(v1), reinterpret_cast const&>(v2), tolerance); +} + + +// http://realtimecollisiondetection.net/pubs/Tolerances/ +// Abs(x - y) <= Max(absTol, relTol * Max(Abs(x), Abs(y))) +// we assume absTol=relTol, this leaves +// Abs(x - y) <= absTol * Max(1.0f, Abs(x), Abs(y)) + +template<> inline vec2 is_equal(vec2 const& v1, vec2 const& v2, float tolerance) noexcept +{ + //TL_PLAIN_ASSERT(math::is_finite(v1) && math::is_finite(v2)); + const vec2 dif = abs(v1 - v2); + const vec2 e = vec2(tolerance) * max(max(vec2(1.0f), abs(v1)), abs(v2)); + return vec2(dif.x <= e.x, dif.y <= e.y); +} +template<> inline vec3 is_equal(vec3 const& v1, vec3 const& v2, float tolerance) noexcept +{ + //TL_PLAIN_ASSERT(math::is_finite(v1) && math::is_finite(v2)); + const vec3 dif = abs(v1 - v2); + const vec3 e = vec3(tolerance) * max(max(vec3(1.0f), abs(v1)), abs(v2)); + return vec3(dif.x <= e.x, dif.y <= e.y, dif.z <= e.z); +} +template<> inline vec4 is_equal(vec4 const& v1, vec4 const& v2, float tolerance) noexcept +{ + //TL_PLAIN_ASSERT(math::is_finite(v1) && math::is_finite(v2)); + const vec4 dif = abs(v1 - v2); + const vec4 e = vec4(tolerance) * max(max(vec4(1.0f), abs(v1)), abs(v2)); + return vec4(dif.x <= e.x, dif.y <= e.y, dif.z <= e.z, dif.w <= e.w); +} +template<> inline vec4 is_equal(quat const& v1, quat const& v2, float tolerance) noexcept +{ + return cwise::is_equal(reinterpret_cast const&>(v1), reinterpret_cast const&>(v2), tolerance); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_less(vec2 const& v1, vec2 const& v2) noexcept +{ + return vec2(math::is_less(v1.x, v2.x), + math::is_less(v1.y, v2.y)); +} +template constexpr vec3 is_less(vec3 const& v1, vec3 const& v2) noexcept +{ + return vec3(math::is_less(v1.x, v2.x), + math::is_less(v1.y, v2.y), + math::is_less(v1.z, v2.z)); +} +template constexpr vec4 is_less(vec4 const& v1, vec4 const& v2) noexcept +{ + return vec4(math::is_less(v1.x, v2.x), + math::is_less(v1.y, v2.y), + math::is_less(v1.z, v2.z), + math::is_less(v1.w, v2.w)); +} +template constexpr vec4 is_less(quat const& v1, quat const& v2) noexcept +{ + return cwise::is_less(reinterpret_cast const&>(v1), reinterpret_cast const&>(v2)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_lequal(vec2 const& v1, vec2 const& v2) noexcept +{ + return vec2(math::is_lequal(v1.x, v2.x), + math::is_lequal(v1.y, v2.y)); +} +template constexpr vec3 is_lequal(vec3 const& v1, vec3 const& v2) noexcept +{ + return vec3(math::is_lequal(v1.x, v2.x), + math::is_lequal(v1.y, v2.y), + math::is_lequal(v1.z, v2.z)); +} +template constexpr vec4 is_lequal(vec4 const& v1, vec4 const& v2) noexcept +{ + return vec4(math::is_lequal(v1.x, v2.x), + math::is_lequal(v1.y, v2.y), + math::is_lequal(v1.z, v2.z), + math::is_lequal(v1.w, v2.w)); +} +template constexpr vec4 is_lequal(quat const& v1, quat const& v2) noexcept +{ + return cwise::is_lequal(reinterpret_cast const&>(v1), reinterpret_cast const&>(v2)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_greater(vec2 const& v1, vec2 const& v2) noexcept +{ + return vec2(math::is_greater(v1.x, v2.x), + math::is_greater(v1.y, v2.y)); +} +template constexpr vec3 is_greater(vec3 const& v1, vec3 const& v2) noexcept +{ + return vec3(math::is_greater(v1.x, v2.x), + math::is_greater(v1.y, v2.y), + math::is_greater(v1.z, v2.z)); +} +template constexpr vec4 is_greater(vec4 const& v1, vec4 const& v2) noexcept +{ + return vec4(math::is_greater(v1.x, v2.x), + math::is_greater(v1.y, v2.y), + math::is_greater(v1.z, v2.z), + math::is_greater(v1.w, v2.w)); +} +template constexpr vec4 is_greater(quat const& v1, quat const& v2) noexcept +{ + return cwise::is_greater(reinterpret_cast const&>(v1), reinterpret_cast const&>(v2)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_gequal(vec2 const& v1, vec2 const& v2) noexcept +{ + return vec2(math::is_gequal(v1.x, v2.x), + math::is_gequal(v1.y, v2.y)); +} +template constexpr vec3 is_gequal(vec3 const& v1, vec3 const& v2) noexcept +{ + return vec3(math::is_gequal(v1.x, v2.x), + math::is_gequal(v1.y, v2.y), + math::is_gequal(v1.z, v2.z)); +} +template constexpr vec4 is_gequal(vec4 const& v1, vec4 const& v2) noexcept +{ + return vec4(math::is_gequal(v1.x, v2.x), + math::is_gequal(v1.y, v2.y), + math::is_gequal(v1.z, v2.z), + math::is_gequal(v1.w, v2.w)); +} +template constexpr vec4 is_gequal(quat const& v1, quat const& v2) noexcept +{ + return cwise::is_gequal(reinterpret_cast const&>(v1), reinterpret_cast const&>(v2)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_zero(vec2 const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, vec2(0), tolerance); +} +template constexpr vec3 is_zero(vec3 const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, vec3(0), tolerance); +} +template constexpr vec4 is_zero(vec4 const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, vec4(0), tolerance); +} +template constexpr vec4 is_zero(quat const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, quat::zero, tolerance); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_one(vec2 const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, vec2(1), tolerance); +} +template constexpr vec3 is_one(vec3 const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, vec3(1), tolerance); +} +template constexpr vec4 is_one(vec4 const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, vec4(1), tolerance); +} +template constexpr vec4 is_one(quat const& v, T tolerance) noexcept +{ + return cwise::is_equal(v, quat::one, tolerance); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_positive(vec2 const& v) noexcept +{ + return vec2(math::is_positive(v.x), + math::is_positive(v.y)); +} +template constexpr vec3 is_positive(vec3 const& v) noexcept +{ + return vec3(math::is_positive(v.x), + math::is_positive(v.y), + math::is_positive(v.z)); +} +template constexpr vec4 is_positive(vec4 const& v) noexcept +{ + return vec4(math::is_positive(v.x), + math::is_positive(v.y), + math::is_positive(v.z), + math::is_positive(v.w)); +} +template constexpr vec4 is_positive(quat const& v) noexcept +{ + return vec4(math::is_positive(v.x), + math::is_positive(v.y), + math::is_positive(v.z), + math::is_positive(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_negative(vec2 const& v) noexcept +{ + return vec2(math::is_negative(v.x), + math::is_negative(v.y)); +} +template constexpr vec3 is_negative(vec3 const& v) noexcept +{ + return vec3(math::is_negative(v.x), + math::is_negative(v.y), + math::is_negative(v.z)); +} +template constexpr vec4 is_negative(vec4 const& v) noexcept +{ + return vec4(math::is_negative(v.x), + math::is_negative(v.y), + math::is_negative(v.z), + math::is_negative(v.w)); +} +template constexpr vec4 is_negative(quat const& v) noexcept +{ + return vec4(math::is_negative(v.x), + math::is_negative(v.y), + math::is_negative(v.z), + math::is_negative(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_nan(vec2 const& v) noexcept +{ + return vec2(math::is_nan(v.x), + math::is_nan(v.y)); +} +template constexpr vec3 is_nan(vec3 const& v) noexcept +{ + return vec3(math::is_nan(v.x), + math::is_nan(v.y), + math::is_nan(v.z)); +} +template constexpr vec4 is_nan(vec4 const& v) noexcept +{ + return vec4(math::is_nan(v.x), + math::is_nan(v.y), + math::is_nan(v.z), + math::is_nan(v.w)); +} +template constexpr vec4 is_nan(quat const& v) noexcept +{ + return vec4(math::is_nan(v.x), + math::is_nan(v.y), + math::is_nan(v.z), + math::is_nan(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_infinite(vec2 const& v) noexcept +{ + return vec2(math::is_infinite(v.x), + math::is_infinite(v.y)); +} +template constexpr vec3 is_infinite(vec3 const& v) noexcept +{ + return vec3(math::is_infinite(v.x), + math::is_infinite(v.y), + math::is_infinite(v.z)); +} +template constexpr vec4 is_infinite(vec4 const& v) noexcept +{ + return vec4(math::is_infinite(v.x), + math::is_infinite(v.y), + math::is_infinite(v.z), + math::is_infinite(v.w)); +} +template constexpr vec4 is_infinite(quat const& v) noexcept +{ + return vec4(math::is_infinite(v.x), + math::is_infinite(v.y), + math::is_infinite(v.z), + math::is_infinite(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 is_finite(vec2 const& v) noexcept +{ + return vec2(math::is_finite(v.x), + math::is_finite(v.y)); +} +template constexpr vec3 is_finite(vec3 const& v) noexcept +{ + return vec3(math::is_finite(v.x), + math::is_finite(v.y), + math::is_finite(v.z)); +} +template constexpr vec4 is_finite(vec4 const& v) noexcept +{ + return vec4(math::is_finite(v.x), + math::is_finite(v.y), + math::is_finite(v.z), + math::is_finite(v.w)); +} +template constexpr vec4 is_finite(quat const& v) noexcept +{ + return vec4(math::is_finite(v.x), + math::is_finite(v.y), + math::is_finite(v.z), + math::is_finite(v.w)); +} + +////////////////////////////////////////////////////////////////////////// + + +constexpr bool all(vec2 const& v) noexcept +{ + return v.x & v.y; +} +constexpr bool all(vec3 const& v) noexcept +{ + return v.x & v.y & v.z; +} +constexpr bool all(vec4 const& v) noexcept +{ + return v.x & v.y & v.z & v.w; +} +constexpr bool any(vec2 const& v) noexcept +{ + return v.x | v.y; +} +constexpr bool any(vec3 const& v) noexcept +{ + return v.x | v.y | v.z; +} +constexpr bool any(vec4 const& v) noexcept +{ + return v.x | v.y | v.z | v.w; +} +constexpr bool none(vec2 const& v) noexcept +{ + return !v.x & !v.y; +} +constexpr bool none(vec3 const& v) noexcept +{ + return !v.x & !v.y & !v.z; +} +constexpr bool none(vec4 const& v) noexcept +{ + return !v.x & !v.y & !v.z & !v.w; +} + +} +} \ No newline at end of file diff --git a/include/VMath/func_transform.h b/include/VMath/func_transform.h new file mode 100644 index 0000000..a4acf13 --- /dev/null +++ b/include/VMath/func_transform.h @@ -0,0 +1,173 @@ +namespace math +{ +////////////////////////////////////////////////////////////////////////// + +template constexpr mat2& multiply(mat2& result, mat2 const& a, mat2 const& b) noexcept; +template constexpr mat3& multiply(mat3& result, mat3 const& a, mat3 const& b) noexcept; +template constexpr mat4& multiply(mat4& result, mat4 const& a, mat4 const& b) noexcept; +#if defined(MATH_USE_SIMD) +template<> mat4& multiply(mat4& result, mat4 const& a, mat4 const& b) noexcept; +#endif +template constexpr trans2& multiply(trans2& result, trans2 const& a, trans2 const& b) noexcept; +template constexpr trans3& multiply(trans3& result, trans3 const& a, trans3 const& b) noexcept; +template constexpr rigid2& multiply(rigid2& result, rigid2 const& a, rigid2 const& b) noexcept; +template constexpr rigid3& multiply(rigid3& result, rigid3 const& a, rigid3 const& b) noexcept; + +////////////////////////////////////////////////////////////////////////// + +namespace cwise +{ +template constexpr mat4& multiply(mat4& result, mat4 const& a, mat4 const& b) noexcept; +template constexpr mat4& add(mat4& result, mat4 const& a, mat4 const& b) noexcept; +template constexpr mat4& substract(mat4& result, mat4 const& a, mat4 const& b) noexcept; +template constexpr mat4& multiply(mat4& result, mat4 const& a, T b) noexcept; +template constexpr mat4& add(mat4& result, mat4 const& a, T b) noexcept; +template constexpr mat4& substract(mat4& result, mat4 const& a, T b) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr mat3& multiply(mat3& result, mat3 const& a, mat3 const& b) noexcept; +template constexpr mat3& add(mat3& result, mat3 const& a, mat3 const& b) noexcept; +template constexpr mat3& substract(mat3& result, mat3 const& a, mat3 const& b) noexcept; +template constexpr mat3& multiply(mat3& result, mat3 const& a, T b) noexcept; +template constexpr mat3& add(mat3& result, mat3 const& a, T b) noexcept; +template constexpr mat3& substract(mat3& result, mat3 const& a, T b) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr mat2& multiply(mat2& result, mat2 const& a, mat2 const& b) noexcept; +template constexpr mat2& add(mat2& result, mat2 const& a, mat2 const& b) noexcept; +template constexpr mat2& substract(mat2& result, mat2 const& a, mat2 const& b) noexcept; +template constexpr mat2& multiply(mat2& result, mat2 const& a, T b) noexcept; +template constexpr mat2& add(mat2& result, mat2 const& a, T b) noexcept; +template constexpr mat2& substract(mat2& result, mat2 const& a, T b) noexcept; + +} //cwise + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec2 transform(mat2 const& mat, vec2 const& v) noexcept; +template constexpr vec2 transform(mat3 const& mat, vec2 const& v) noexcept; +template constexpr vec2 transform(rigid2 const& trans, vec2 const& v) noexcept; +template constexpr vec2 transform(trans2 const& t, vec2 const& v) noexcept; +template constexpr vec2 rotate(mat2 const& mat, vec2 const& v) noexcept; +template constexpr vec2 rotate(mat3 const& mat, vec2 const& v) noexcept; +template constexpr vec2 rotate(rigid2 const& trans, vec2 const& v) noexcept; +template constexpr vec2 rotate(T rotation, vec2 const& v) noexcept; +template constexpr vec2 rotate(trans2 const& t, vec2 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec3 transform(mat3 const& mat, vec3 const& v) noexcept; +#if defined(MATH_USE_SIMD) +template<> vec3 transform(const mat4& mat, const vec3& vect) noexcept; +#endif +template constexpr vec3 transform(mat4 const& mat, vec3 const& v) noexcept; +template constexpr vec3 transform(rigid3 const& trans, vec3 const& v) noexcept; +template constexpr vec3 transform(trans3 const& trans, vec3 const& v) noexcept; +template constexpr vec3 rotate(mat3 const& mat, vec3 const& v) noexcept; +template constexpr vec3 rotate(mat4 const& mat, vec3 const& v) noexcept; +template constexpr vec3 rotate(rigid3 const& trans, vec3 const& v) noexcept; +template constexpr vec3 rotate(trans3 const& m, vec3 const& v) noexcept; +template constexpr vec3 rotate(quat const& q, vec3 const& v) noexcept; +template constexpr vec3 project(mat4 const& m, vec3 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr vec4 transform(mat4 const& mat, vec4 const& v) noexcept; +#if defined(MATH_USE_SIMD) +template<> vec4 transform(const mat4& mat, const vec4& vect) noexcept; +#endif +template constexpr vec4 project(mat4 const& m, vec4 const& v) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template constexpr ray2 transform(mat2 const& mat, ray2 const& ray) noexcept; +template constexpr ray2 transform(mat3 const& mat, ray2 const& ray) noexcept; +template constexpr ray2 transform(rigid2 const& trans, ray2 const& ray) noexcept; +template constexpr ray2 transform(trans2 const& trans, ray2 const& ray) noexcept; + +template constexpr segment2 transform(mat2 const& mat, segment2 const& segment) noexcept; +template constexpr segment2 transform(mat3 const& mat, segment2 const& segment) noexcept; +template constexpr segment2 transform(rigid2 const& trans, segment2 const& segment) noexcept; +template constexpr segment2 transform(trans2 const& trans, segment2 const& segment) noexcept; + +template constexpr ray2 rotate(mat2 const& mat, ray2 const& ray) noexcept; +template constexpr ray2 rotate(mat3 const& mat, ray2 const& ray) noexcept; +template constexpr ray2 rotate(rigid2 const& trans, ray2 const& ray) noexcept; +template constexpr ray2 rotate(trans2 const& trans, ray2 const& ray) noexcept; + +template constexpr ray3 transform(mat3 const& mat, ray3 const& ray) noexcept; +template constexpr ray3 transform(mat4 const& mat, ray3 const& ray) noexcept; +template constexpr ray3 transform(rigid3 const& trans, ray3 const& ray) noexcept; +template constexpr ray3 transform(trans3 const& trans, ray3 const& ray) noexcept; + +template constexpr segment3 transform(mat3 const& mat, segment3 const& segment) noexcept; +template constexpr segment3 transform(mat4 const& mat, segment3 const& segment) noexcept; +template constexpr segment3 transform(rigid3 const& trans, segment3 const& segment) noexcept; +template constexpr segment3 transform(trans3 const& trans, segment3 const& segment) noexcept; + +template constexpr ray3 rotate(mat3 const& mat, ray3 const& ray) noexcept; +template constexpr ray3 rotate(mat4 const& mat, ray3 const& ray) noexcept; +template constexpr ray3 rotate(trans3 const& trans, ray3 const& ray) noexcept; +template constexpr ray3 rotate(rigid3 const& trans, ray3 const& ray) noexcept; + +////////////////////////////////////////////////////////////////////////// +/// aabbN transforms +////////////////////////////////////////////////////////////////////////// + +template constexpr aabb2 transform(mat3 const& trans, aabb2 const& box) noexcept; +template constexpr aabb2 transform(trans2 const& trans, aabb2 const& box) noexcept; +template constexpr aabb2 transform(rigid2 const& trans, aabb2 const& box) noexcept; +template constexpr rect transform(mat3 const& trans, rect const& rect) noexcept; +template constexpr rect transform(trans2 const& trans, rect const& rect) noexcept; +template constexpr rect transform(rigid2 const& trans, rect const& rect) noexcept; + +template constexpr aabb2 rotate(mat2 const& m, aabb2 const& box) noexcept; +template constexpr aabb2 rotate(mat3 const& m, aabb2 const& box) noexcept; +template constexpr aabb2 rotate(rigid2 const& trans, aabb2 const& box) noexcept; +template constexpr aabb2 rotate(trans2 const& trans, aabb2 const& box) noexcept; +template constexpr rect rotate(mat2 const& m, rect const& box) noexcept; +template constexpr rect rotate(mat3 const& m, rect const& box) noexcept; +template constexpr rect rotate(rigid2 const& trans, rect const& box) noexcept; +template constexpr rect rotate(trans2 const& trans, rect const& box) noexcept; + +template constexpr aabb3 transform(mat4 const& m, aabb3 const& box) noexcept; +template constexpr aabb3 transform(rigid3 const& trans, aabb3 const& box) noexcept; +template constexpr aabb3 transform(trans3 const& trans, aabb3 const& box) noexcept; +template constexpr aabb3 rotate(mat3 const& m, aabb3 const& box) noexcept; +template constexpr aabb3 rotate(mat4 const& m, aabb3 const& box) noexcept; +template constexpr aabb3 rotate(rigid3 const& trans, aabb3 const& box) noexcept; +template constexpr aabb3 rotate(trans3 const& trans, aabb3 const& box) noexcept; + +namespace batch +{ +template void transform(trans2 const& t, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept; +template void rotate(trans2 const& t, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept; + +template void transform(trans3 const& t, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept; +template void rotate(trans3 const& t, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept; + +template void transform(mat3 const& m, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept; +template void rotate(mat3 const& m, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept; + +template void transform(mat3 const& m, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept; +template void rotate(mat3 const& m, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept; + +template void transform(mat4 const& m, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept; +template void rotate(mat4 const& m, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept; + +template void transform(mat4 const& m, vec4* dst, size_t dst_stride, vec4 const* src, size_t src_stride, size_t count) noexcept; +template void rotate(mat4 const& m, vec4* dst, size_t dst_stride, vec4 const* src, size_t src_stride, size_t count) noexcept; + +////////////////////////////////////////////////////////////////////////// + +template void multiply(mat4 const& m, mat4* dst, size_t dst_stride, mat4 const* src, size_t src_stride, size_t count) noexcept; +template void multiply(trans3 const& m, trans3* dst, size_t dst_stride, trans3 const* src, size_t src_stride, size_t count) noexcept; +template void multiply(mat3 const& m, mat3* dst, size_t dst_stride, mat3 const* src, size_t src_stride, size_t count) noexcept; +template void multiply(trans2 const& m, trans2* dst, size_t dst_stride, trans2 const* src, size_t src_stride, size_t count) noexcept; + +} + +////////////////////////////////////////////////////////////////////////// +} diff --git a/include/VMath/func_transform.inl b/include/VMath/func_transform.inl new file mode 100644 index 0000000..700c684 --- /dev/null +++ b/include/VMath/func_transform.inl @@ -0,0 +1,1250 @@ +namespace math +{ +////////////////////////////////////////////////////////////////////////// + +template +constexpr mat2& multiply(mat2& result, mat2 const& a, mat2 const& b) noexcept +{ + T const* MATH_RESTRICT m1 = a.data(); + T const* MATH_RESTRICT m2 = b.data(); + T* MATH_RESTRICT m3 = result.data(); + TL_PLAIN_ASSERT(&result != &a && &result != &b); + + m3[0] = m1[0] * m2[0] + m1[2] * m2[1]; + m3[1] = m1[1] * m2[0] + m1[3] * m2[1]; + + m3[2] = m1[0] * m2[2] + m1[2] * m2[3]; + m3[3] = m1[1] * m2[2] + m1[3] * m2[3]; + + return result; +} + +template +constexpr mat3& multiply(mat3& result, mat3 const& a, mat3 const& b) noexcept +{ + T const* MATH_RESTRICT m1 = a.data(); + T const* MATH_RESTRICT m2 = b.data(); + T* MATH_RESTRICT m3 = result.data(); + TL_PLAIN_ASSERT(&result != &a && &result != &b); + + m3[0] = m1[0] * m2[0] + m1[3] * m2[1] + m1[6] * m2[2]; + m3[1] = m1[1] * m2[0] + m1[4] * m2[1] + m1[7] * m2[2]; + m3[2] = m1[2] * m2[0] + m1[5] * m2[1] + m1[8] * m2[2]; + + m3[3] = m1[0] * m2[3] + m1[3] * m2[4] + m1[6] * m2[5]; + m3[4] = m1[1] * m2[3] + m1[4] * m2[4] + m1[7] * m2[5]; + m3[5] = m1[2] * m2[3] + m1[5] * m2[4] + m1[8] * m2[5]; + + m3[6] = m1[0] * m2[6] + m1[3] * m2[7] + m1[6] * m2[8]; + m3[7] = m1[1] * m2[6] + m1[4] * m2[7] + m1[7] * m2[8]; + m3[8] = m1[2] * m2[6] + m1[5] * m2[7] + m1[8] * m2[8]; + + return result; +} + +template +constexpr mat4& multiply(mat4& result, mat4 const& a, mat4 const& b) noexcept +{ + T const* MATH_RESTRICT m1 = a.data(); + T const* MATH_RESTRICT m2 = b.data(); + T* MATH_RESTRICT m3 = result.data(); + TL_PLAIN_ASSERT(&result != &a && &result != &b); + + m3[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3]; + m3[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3]; + m3[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3]; + m3[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3]; + + m3[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7]; + m3[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7]; + m3[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7]; + m3[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7]; + + m3[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11]; + m3[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11]; + m3[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11]; + m3[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11]; + + m3[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15]; + m3[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15]; + m3[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15]; + m3[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15]; + + return result; +} + +#if defined(MATH_USE_SIMD) +template<> +mat4& multiply(mat4& result, mat4 const& a, mat4 const& b) noexcept; +#endif + +template +constexpr rigid2& multiply(rigid2& result, rigid2 const& a, rigid2 const& b) noexcept +{ + const vec2 translation = a.translation + math::rotate(a.rotation, b.translation); + const T rotation = a.rotation + b.rotation; + result = rigid2(translation, rotation); + return result; +} + +template +constexpr rigid3& multiply(rigid3& result, rigid3 const& a, rigid3 const& b) noexcept +{ + const vec3 translation = a.translation + math::rotate(a.rotation, b.translation); + const quat rotation = a.rotation * b.rotation; + result = rigid3(translation, rotation); + return result; +} + +template +constexpr trans2& multiply(trans2& result, trans2 const& a, trans2 const& b) noexcept +{ + T const* MATH_RESTRICT m1 = a.rotation_scale.data(); + T const* MATH_RESTRICT m2 = b.rotation_scale.data(); + T* MATH_RESTRICT m3 = result.rotation_scale.data(); + TL_PLAIN_ASSERT(m3 != m1 && m3 != m2); + + //created from the mat3 * mat3 code using the mat3->trans2 conversion table in trans2.h + m3[0] = m1[0] * m2[0] + m1[2] * m2[1]; + m3[1] = m1[1] * m2[0] + m1[3] * m2[1]; + + m3[2] = m1[0] * m2[2] + m1[2] * m2[3]; + m3[3] = m1[1] * m2[2] + m1[3] * m2[3]; + + result.translation.x = m1[0] * b.translation.x + m1[2] * b.translation.y + a.translation.x; + result.translation.y = m1[1] * b.translation.x + m1[3] * b.translation.y + a.translation.y; + + return result; +} + +template +constexpr trans3& multiply(trans3& result, trans3 const& a, trans3 const& b) noexcept +{ + T const* MATH_RESTRICT am = a.rotation_scale.data(); + T const* MATH_RESTRICT bm = b.rotation_scale.data(); + T* MATH_RESTRICT rm = result.rotation_scale.data(); + TL_PLAIN_ASSERT(rm != am && rm != bm); + + //created from the mat4 * mat4 code using the mat4->trans3 conversion table in trans3.h + rm[0] = am[0] * bm[0] + am[3] * bm[1] + am[6] * bm[2]; + rm[1] = am[1] * bm[0] + am[4] * bm[1] + am[7] * bm[2]; + rm[2] = am[2] * bm[0] + am[5] * bm[1] + am[8] * bm[2]; + + rm[3] = am[0] * bm[3] + am[3] * bm[4] + am[6] * bm[5]; + rm[4] = am[1] * bm[3] + am[4] * bm[4] + am[7] * bm[5]; + rm[5] = am[2] * bm[3] + am[5] * bm[4] + am[8] * bm[5]; + + rm[6] = am[0] * bm[6] + am[3] * bm[7] + am[6] * bm[8]; + rm[7] = am[1] * bm[6] + am[4] * bm[7] + am[7] * bm[8]; + rm[8] = am[2] * bm[6] + am[5] * bm[7] + am[8] * bm[8]; + + T ax = a.translation.x; + T ay = a.translation.y; + T az = a.translation.z; + T bx = b.translation.x; + T by = b.translation.y; + T bz = b.translation.z; + result.translation.x = am[0] * bx + am[3] * by + am[6] * bz + ax; + result.translation.y = am[1] * bx + am[4] * by + am[7] * bz + ay; + result.translation.z = am[2] * bx + am[5] * by + am[8] * bz + az; + + return result; +} + +////////////////////////////////////////////////////////////////////////// + +namespace cwise +{ +template +constexpr mat4& multiply(mat4& result, mat4 const& a, mat4 const& b) noexcept +{ + result.column0 = a.column0 * b.column0; + result.column1 = a.column1 * b.column1; + result.column2 = a.column2 * b.column2; + result.column3 = a.column3 * b.column3; + return result; +} + +template +constexpr mat4& add(mat4& result, mat4 const& a, mat4 const& b) noexcept +{ + result.column0 = a.column0 + b.column0; + result.column1 = a.column1 + b.column1; + result.column2 = a.column2 + b.column2; + result.column3 = a.column3 + b.column3; + return result; +} + +template +constexpr mat4& substract(mat4& result, mat4 const& a, mat4 const& b) noexcept +{ + result.column0 = a.column0 - b.column0; + result.column1 = a.column1 - b.column1; + result.column2 = a.column2 - b.column2; + result.column3 = a.column3 - b.column3; + return result; +} + +template +constexpr mat4& multiply(mat4& result, mat4 const& a, T b) noexcept +{ + result.column0 = a.column0 * b; + result.column1 = a.column1 * b; + result.column2 = a.column2 * b; + result.column3 = a.column3 * b; + return result; +} + +template +constexpr mat4& add(mat4& result, mat4 const& a, T b) noexcept +{ + result.column0 = a.column0 + b; + result.column1 = a.column1 + b; + result.column2 = a.column2 + b; + result.column3 = a.column3 + b; + return result; +} + +template +constexpr mat4& substract(mat4& result, mat4 const& a, T b) noexcept +{ + result.column0 = a.column0 - b; + result.column1 = a.column1 - b; + result.column2 = a.column2 - b; + result.column3 = a.column3 - b; + return result; +} + +////////////////////////////////////////////////////////////////////////// + +template +constexpr mat3& multiply(mat3& result, mat3 const& a, mat3 const& b) noexcept +{ + result.column0 = a.column0 * b.column0; + result.column1 = a.column1 * b.column1; + result.column2 = a.column2 * b.column2; + return result; +} + +template +constexpr mat3& add(mat3& result, mat3 const& a, mat3 const& b) noexcept +{ + result.column0 = a.column0 + b.column0; + result.column1 = a.column1 + b.column1; + result.column2 = a.column2 + b.column2; + return result; +} + +template +constexpr mat3& substract(mat3& result, mat3 const& a, mat3 const& b) noexcept +{ + result.column0 = a.column0 - b.column0; + result.column1 = a.column1 - b.column1; + result.column2 = a.column2 - b.column2; + return result; +} + +template +constexpr mat3& multiply(mat3& result, mat3 const& a, T b) noexcept +{ + result.column0 = a.column0 * b; + result.column1 = a.column1 * b; + result.column2 = a.column2 * b; + return result; +} + +template +constexpr mat3& add(mat3& result, mat3 const& a, T b) noexcept +{ + result.column0 = a.column0 + b; + result.column1 = a.column1 + b; + result.column2 = a.column2 + b; + return result; +} + +template +constexpr mat3& substract(mat3& result, mat3 const& a, T b) noexcept +{ + result.column0 = a.column0 - b; + result.column1 = a.column1 - b; + result.column2 = a.column2 - b; + return result; +} + +////////////////////////////////////////////////////////////////////////// + +template +constexpr mat2& multiply(mat2& result, mat2 const& a, mat2 const& b) noexcept +{ + result.column0 = a.column0 * b.column0; + result.column1 = a.column1 * b.column1; + return result; +} + +template +constexpr mat2& add(mat2& result, mat2 const& a, mat2 const& b) noexcept +{ + result.column0 = a.column0 + b.column0; + result.column1 = a.column1 + b.column1; + return result; +} + +template +constexpr mat2& substract(mat2& result, mat2 const& a, mat2 const& b) noexcept +{ + result.column0 = a.column0 - b.column0; + result.column1 = a.column1 - b.column1; + return result; +} + +template +constexpr mat2& multiply(mat2& result, mat2 const& a, T b) noexcept +{ + result.column0 = a.column0 * b; + result.column1 = a.column1 * b; + return result; +} + +template +constexpr mat2& add(mat2& result, mat2 const& a, T b) noexcept +{ + result.column0 = a.column0 + b; + result.column1 = a.column1 + b; + return result; +} + +template +constexpr mat2& substract(mat2& result, mat2 const& a, T b) noexcept +{ + result.column0 = a.column0 - b; + result.column1 = a.column1 - b; + return result; +} +} + +////////////////////////////////////////////////////////////////////////// +// Vec2 transforms +////////////////////////////////////////////////////////////////////////// + +template +constexpr vec2 transform(mat2 const& mat, vec2 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec2(v.x * m[0] + v.y * m[2], + v.x * m[1] + v.y * m[3]); +} + +template +constexpr vec2 transform(mat3 const& mat, vec2 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec2(v.x * m[0] + v.y * m[3] + m[6], + v.x * m[1] + v.y * m[4] + m[7]); +} + +template +constexpr vec2 transform(rigid2 const& trans, vec2 const& v) noexcept +{ + return rotate(trans.rotation, v) + trans.translation; +} + +template +constexpr vec2 transform(trans2 const& t, vec2 const& v) noexcept +{ + return rotate(t.rotation_scale, v) + t.translation; +} + +template +constexpr vec2 rotate(mat2 const& mat, vec2 const& v) noexcept +{ + return transform(mat, v); +} + +template +constexpr vec2 rotate(mat3 const& mat, vec2 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec2(v.x * m[0] + v.y * m[3], + v.x * m[1] + v.y * m[4]); +} + +template +constexpr vec2 rotate(rigid2 const& trans, vec2 const& v) noexcept +{ + return rotate(trans.rotation, v); +} + +template +constexpr vec2 rotate(T rotation, vec2 const& v) noexcept +{ + T s, c; + sin_cos(rotation, s, c); + return vec2(v.x * c + v.y * -s, + v.x * s + v.y * c); +} + +template +constexpr vec2 rotate(trans2 const& t, vec2 const& v) noexcept +{ + return rotate(t.rotation_scale, v); +} + +////////////////////////////////////////////////////////////////////////// +// Vec3 transforms +////////////////////////////////////////////////////////////////////////// + +template +constexpr vec3 transform(mat3 const& mat, vec3 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec3(v.x * m[0] + v.y * m[3] + v.z * m[6], + v.x * m[1] + v.y * m[4] + v.z * m[7], + v.x * m[2] + v.y * m[5] + v.z * m[8]); +} + +#if defined(MATH_USE_SIMD) +template<> +vec3 transform(const mat4& mat, const vec3& vect) noexcept; +#endif + +template +constexpr vec3 transform(mat4 const& mat, vec3 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec3(v.x * m[0] + v.y * m[4] + v.z * m[8] + m[12], + v.x * m[1] + v.y * m[5] + v.z * m[9] + m[13], + v.x * m[2] + v.y * m[6] + v.z * m[10] + m[14]); +} + +template +constexpr vec3 transform(rigid3 const& trans, vec3 const& v) noexcept +{ + return rotate(trans.rotation, v) + trans.translation; +} + +template +constexpr vec3 transform(trans3 const& trans, vec3 const& v) noexcept +{ + return rotate(trans.rotation_scale, v) + trans.translation; +} + +template +constexpr vec3 rotate(mat3 const& mat, vec3 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec3(v.x * m[0] + v.y * m[3] + v.z * m[6], + v.x * m[1] + v.y * m[4] + v.z * m[7], + v.x * m[2] + v.y * m[5] + v.z * m[8]); +} + +template +constexpr vec3 rotate(mat4 const& mat, vec3 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec3(v.x * m[0] + v.y * m[4] + v.z * m[8], + v.x * m[1] + v.y * m[5] + v.z * m[9], + v.x * m[2] + v.y * m[6] + v.z * m[10]); +} + +template +constexpr vec3 rotate(rigid3 const& trans, vec3 const& v) noexcept +{ + return rotate(trans.rotation, v); +} + +template +constexpr vec3 rotate(trans3 const& trans, vec3 const& v) noexcept +{ + return rotate(trans.rotation_scale, v); +} + +template +constexpr vec3 rotate(quat const& q, vec3 const& v) noexcept +{ + // Eigen implementation + const vec3 qvec(q.x, q.y, q.z); + vec3 uv = cross(qvec, v); + uv += uv; + const vec3 uuv = cross(qvec, uv); + + return v + q.w * uv + uuv; +} + +template +constexpr vec3 project(mat4 const& m, vec3 const& v) noexcept +{ + const vec4 tmp = transform(m, to_vec4(v, T(1))); + if (!is_zero(tmp.w)) + { + T invW = T(1) / tmp.w; + return to_vec3(tmp) * invW; + } + else + return to_vec3(tmp); +} + +////////////////////////////////////////////////////////////////////////// +// Vec4 transforms +////////////////////////////////////////////////////////////////////////// + +template +constexpr vec4 transform(mat4 const& mat, vec4 const& v) noexcept +{ + T const* MATH_RESTRICT m = mat.data(); + return vec4(v.x * m[0] + v.y * m[4] + v.z * m[8] + v.w * m[12], + v.x * m[1] + v.y * m[5] + v.z * m[9] + v.w * m[13], + v.x * m[2] + v.y * m[6] + v.z * m[10] + v.w * m[14], + v.x * m[3] + v.y * m[7] + v.z * m[11] + v.w * m[15]); +} + +#if defined TL_PLATFORM_WINDOWS_FAMILY +template<> +inline vec4 transform(const mat4& mat, const vec4& vect) noexcept +{ + float const* MATH_RESTRICT m = mat.data(); + + const __m128 col1 = _mm_loadu_ps(m); + const __m128 col2 = _mm_loadu_ps(m + 4); + const __m128 col3 = _mm_loadu_ps(m + 8); + const __m128 col4 = _mm_loadu_ps(m + 12); + + const __m128 v = _mm_loadu_ps(&vect.x); + + const __m128 u1 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(0, 0, 0, 0)); + const __m128 u2 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(1, 1, 1, 1)); + const __m128 u3 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(2, 2, 2, 2)); + const __m128 u4 = _mm_shuffle_ps(v, v, _MM_SHUFFLE(3, 3, 3, 3)); + + const __m128 prod1 = _mm_mul_ps(u1, col1); + const __m128 prod2 = _mm_mul_ps(u2, col2); + const __m128 prod3 = _mm_mul_ps(u3, col3); + const __m128 prod4 = _mm_mul_ps(u4, col4); + + const __m128 result = _mm_add_ps(_mm_add_ps(prod1, prod2), _mm_add_ps(prod3, prod4)); + + vec4 output; + _mm_storeu_ps(&output.x, result); + return output; +} + +//#elif defined NEON +//template<> +//vec4 transform(const mat4& mat, const vec4& vect) noexcept +//{ +// float const* MATH_RESTRICT m = mat.data(); +// float32x4_t m_vec = vld1q_f32(m); +// float32x4_t v_vec = vld1q_f32(&v.x); +// float32x4_t result = vmulq_f32(m_vec, v_vec); +// float32x4_t out = result[0] + result[1] + result[2] + result[3]); +// return vec4f(out[0], out[1], out[2], out[3]); +//} +#endif + +template +constexpr vec4 project(mat4 const& m, vec4 const& v) noexcept +{ + const vec4 tmp = transform(m, v); + if (!is_zero(tmp.w)) + { + T invW = T(1) / tmp.w; + return to_vec4(to_vec3(tmp) * invW, tmp.w); + } + else + return tmp; +} + +////////////////////////////////////////////////////////////////////////// +/// rayN transforms +////////////////////////////////////////////////////////////////////////// + +template +constexpr ray2 transform(mat2 const& mat, ray2 const& ray) noexcept +{ + return ray2(transform(mat, ray.origin), normalized(rotate(mat, ray.direction))); +} +template +constexpr ray2 transform(mat3 const& mat, ray2 const& ray) noexcept +{ + return ray2(transform(mat, ray.origin), normalized(rotate(mat, ray.direction))); +} +template +constexpr ray2 transform(trans2 const& trans, ray2 const& ray) noexcept +{ + return ray2(transform(trans, ray.origin), normalized(rotate(trans, ray.direction))); +} +template +constexpr ray2 transform(rigid2 const& trans, ray2 const& ray) noexcept +{ + return ray2(transform(trans, ray.origin), rotate(trans, ray.direction)); +} + +template +constexpr segment2 transform(mat2 const& mat, segment2 const& segment) noexcept +{ + return segment2(transform(mat, segment.start), transform(mat, segment.end)); +} +template +constexpr segment2 transform(mat3 const& mat, segment2 const& segment) noexcept +{ + return segment2(transform(mat, segment.start), transform(mat, segment.end)); +} +template +constexpr segment2 transform(trans2 const& trans, segment2 const& segment) noexcept +{ + return segment2(transform(trans, segment.start), transform(trans, segment.end)); +} +template +constexpr segment2 transform(rigid2 const& trans, segment2 const& segment) noexcept +{ + return segment2(transform(trans, segment.start), transform(trans, segment.end)); +} + +template +constexpr ray2 rotate(mat2 const& mat, ray2 const& ray) noexcept +{ + return ray2(ray.origin, normalized(rotate(mat, ray.direction))); +} +template +constexpr ray2 rotate(mat3 const& mat, ray2 const& ray) noexcept +{ + return ray2(ray.origin, normalized(rotate(mat, ray.direction))); +} +template +constexpr ray2 rotate(trans2 const& trans, ray2 const& ray) noexcept +{ + return ray2(ray.origin, normalized(rotate(trans, ray.direction))); +} +template +constexpr ray2 rotate(rigid2 const& trans, ray2 const& ray) noexcept +{ + return ray2(ray.origin, rotate(trans, ray.direction)); +} + +template +constexpr ray3 transform(mat3 const& mat, ray3 const& ray) noexcept +{ + return ray3(transform(mat, ray.origin), normalized(rotate(mat, ray.direction))); +} +template +constexpr ray3 transform(mat4 const& mat, ray3 const& ray) noexcept +{ + return ray3(transform(mat, ray.origin), normalized(rotate(mat, ray.direction))); +} +template +constexpr ray3 transform(rigid3 const& trans, ray3 const& ray) noexcept +{ + return ray3(transform(trans, ray.origin), rotate(trans, ray.direction)); +} +template +constexpr ray3 transform(trans3 const& trans, ray3 const& ray) noexcept +{ + return ray3(transform(trans, ray.origin), normalized(rotate(trans, ray.direction))); +} + +template +constexpr segment3 transform(mat3 const& mat, segment3 const& segment) noexcept +{ + return segment3(transform(mat, segment.start), transform(mat, segment.end)); +} +template +constexpr segment3 transform(mat4 const& mat, segment3 const& segment) noexcept +{ + return segment3(transform(mat, segment.start), transform(mat, segment.end)); +} +template +constexpr segment3 transform(rigid3 const& trans, segment3 const& segment) noexcept +{ + return segment3(transform(trans, segment.start), transform(trans, segment.end)); +} +template +constexpr segment3 transform(trans3 const& trans, segment3 const& segment) noexcept +{ + return segment3(transform(trans, segment.start), transform(trans, segment.end)); +} + +template +constexpr ray3 rotate(mat3 const& mat, ray3 const& ray) noexcept +{ + return ray3(ray.origin, normalized(rotate(mat, ray.direction))); +} +template +constexpr ray3 rotate(mat4 const& mat, ray3 const& ray) noexcept +{ + return ray3(ray.origin, normalized(rotate(mat, ray.direction))); +} +template +constexpr ray3 rotate(trans3 const& trans, ray3 const& ray) noexcept +{ + return ray3(ray.origin, normalized(rotate(trans, ray.direction))); +} +template +constexpr ray3 rotate(rigid3 const& trans, ray3 const& ray) noexcept +{ + return ray3(ray.origin, rotate(trans, ray.direction)); +} + +////////////////////////////////////////////////////////////////////////// +/// aabbN transforms +////////////////////////////////////////////////////////////////////////// + +namespace detail +{ +template +constexpr T _transform(Mat const& m, Vec const& new_center, T const& box) noexcept +{ + const Vec c0 = abs(m.column0); + const Vec c1 = abs(m.column1); + const Vec hs = box.get_half_extent(); + const Vec new_hs = + { + c0.x * hs.x + c1.x * hs.y, + c0.y * hs.x + c1.y * hs.y, + }; + return { new_center - new_hs, new_center + new_hs }; +} +} + +template +constexpr aabb2 transform(mat3 const& trans, aabb2 const& box) noexcept +{ + const vec2 hs = box.get_half_extent(); + const vec2 new_hs = + { + abs(trans.m[0]) * hs.x + abs(trans.m[3]) * hs.y, + abs(trans.m[1]) * hs.x + abs(trans.m[4]) * hs.y, + }; + const vec2 new_center = transform(trans, box.get_center()); + return { new_center - new_hs, new_center + new_hs }; +} +template +constexpr aabb2 transform(trans2 const& trans, aabb2 const& box) noexcept +{ + return detail::_transform(trans.rotation_scale, transform(trans, box.get_center()), box); +} +template +constexpr aabb2 transform(rigid2 const& trans, aabb2 const& box) noexcept +{ + const mat2 m = to_mat2(trans.rotation); //expensive - does sin/cos + return detail::_transform(m, transform(trans, box.get_center()), box); +} +template +constexpr rect transform(mat3 const& trans, rect const& rect) noexcept +{ + const vec2 hs = rect.get_extent() * T(0.5); + const vec2 new_hs = + { + abs(trans.m[0]) * hs.x + abs(trans.m[3]) * hs.y, + abs(trans.m[1]) * hs.x + abs(trans.m[4]) * hs.y, + }; + const vec2 new_center = transform(trans, rect.get_center()); + return { new_center - new_hs, new_center + new_hs }; +} +template +constexpr rect transform(trans2 const& trans, rect const& rect) noexcept +{ + return detail::_transform(trans.rotation_scale, transform(trans, rect.get_center()), rect); +} +template +constexpr rect transform(rigid2 const& trans, rect const& rect) noexcept +{ + const mat2 m = to_mat2(trans.rotation); //expensive - does sin/cos + return detail::_transform(m, transform(trans, rect.get_center()), rect); +} + +template +constexpr aabb2 rotate(mat3 const& trans, aabb2 const& box) noexcept +{ + const vec2 hs = box.get_half_extent(); + const vec2 new_hs = + { + abs(trans.m[0]) * hs.x + abs(trans.m[3]) * hs.y, + abs(trans.m[1]) * hs.x + abs(trans.m[4]) * hs.y, + }; + const vec2 center = box.get_center(); + return { center - new_hs, center + new_hs }; +} +template +constexpr aabb2 rotate(mat2 const& trans, aabb2 const& box) noexcept +{ + const vec2 hs = box.get_half_extent(); + const vec2 new_hs = + { + abs(trans.m[0]) * hs.x + abs(trans.m[2]) * hs.y, + abs(trans.m[1]) * hs.x + abs(trans.m[3]) * hs.y, + }; + + const vec2 center = box.get_center(); + return { center - new_hs, center + new_hs }; +} +template +constexpr aabb2 rotate(trans2 const& trans, aabb2 const& box) noexcept +{ + return rotate(trans.rotation_scale, box); +} +template +constexpr aabb2 rotate(rigid2 const& trans, aabb2 const& box) noexcept +{ + const mat2 m = to_mat2(trans.rotation); + return rotate(m, box); +} +template +constexpr rect rotate(mat3 const& trans, rect const& rect) noexcept +{ + const vec2 hs = rect.get_extent() * T(0.5); + const vec2 new_hs = + { + abs(trans.m[0]) * hs.x + abs(trans.m[3]) * hs.y, + abs(trans.m[1]) * hs.x + abs(trans.m[4]) * hs.y, + }; + + const vec2 center = rect.get_center(); + return { center - new_hs, center + new_hs }; +} +template +constexpr rect rotate(mat2 const& trans, rect const& rect) noexcept +{ + const vec2 hs = rect.get_extent() * T(0.5); + const vec2 new_hs = + { + abs(trans.m[0]) * hs.x + abs(trans.m[2]) * hs.y, + abs(trans.m[1]) * hs.x + abs(trans.m[3]) * hs.y, + }; + const vec2 center = rect.get_center(); + return { center - new_hs, center + new_hs }; +} +template +constexpr rect rotate(trans2 const& trans, rect const& rect) noexcept +{ + return rotate(trans.rotation_scale, rect); +} +template +constexpr rect rotate(rigid2 const& trans, rect const& rect) noexcept +{ + const mat2 m = to_mat2(trans.rotation); + return rotate(m, rect); +} + +namespace +{ +template +constexpr aabb3 _transform(mat3 const& m, vec3 const& new_center, aabb3 const& box) noexcept +{ + const vec3 c0 = abs(m.column0); + const vec3 c1 = abs(m.column1); + const vec3 c2 = abs(m.column2); + const vec3 hs = box.get_half_extent(); + const vec3 new_hs = + { + c0.x * hs.x + c1.x * hs.y + c2.x * hs.z, + c0.y * hs.x + c1.y * hs.y + c2.y * hs.z, + c0.z * hs.x + c1.z * hs.y + c2.z * hs.z, + }; + return { new_center - new_hs, new_center + new_hs }; +} +} +template +constexpr aabb3 transform(mat4 const& m, aabb3 const& box) noexcept +{ + const vec3 hs = box.get_half_extent(); + const vec3 new_hs = + { + abs(m.m[0]) * hs.x + abs(m.m[4]) * hs.y + abs(m.m[8]) * hs.z, + abs(m.m[1]) * hs.x + abs(m.m[5]) * hs.y + abs(m.m[9]) * hs.z, + abs(m.m[2]) * hs.x + abs(m.m[6]) * hs.y + abs(m.m[10]) * hs.z, + }; + const vec3 center = transform(m, box.get_center()); + return { center - new_hs, center + new_hs }; +} +template +constexpr aabb3 transform(rigid3 const& trans, aabb3 const& box) noexcept +{ + const mat3 m = to_mat3(trans.rotation); + return _transform(m, transform(trans, box.get_center()), box); +} +template +constexpr aabb3 transform(trans3 const& trans, aabb3 const& box) noexcept +{ + return _transform(trans.rotation_scale, transform(trans, box.get_center()), box); +} +template +constexpr aabb3 rotate(mat3 const& m, aabb3 const& box) noexcept +{ + const vec3 c0 = abs(m.column0); + const vec3 c1 = abs(m.column1); + const vec3 c2 = abs(m.column2); + const vec3 hs = box.get_half_extent(); + const vec3 tmp = + { + c0.x* hs.x + c1.x * hs.y + c2.x * hs.z, + c0.y* hs.x + c1.y * hs.y + c2.y * hs.z, + c0.z* hs.x + c1.z * hs.y + c2.z * hs.z, + }; + const vec3 center = box.get_center(); + return { center - tmp, center + tmp }; +} +template +constexpr aabb3 rotate(mat4 const& m, aabb3 const& box) noexcept +{ + const vec3 hs = box.get_half_extent(); + const vec3 tmp = + { + abs(m.m[0]) * hs.x + abs(m.m[4]) * hs.y + abs(m.m[8]) * hs.z, + abs(m.m[1]) * hs.x + abs(m.m[5]) * hs.y + abs(m.m[9]) * hs.z, + abs(m.m[2]) * hs.x + abs(m.m[6]) * hs.y + abs(m.m[10]) * hs.z, + }; + const vec3 center = box.get_center(); + return { center - tmp, center + tmp }; +} +template +constexpr aabb3 rotate(rigid3 const& trans, aabb3 const& box) noexcept +{ + const mat3 m = to_mat3(trans.rotation); + return rotate(m, box); +} +template +constexpr aabb3 rotate(trans3 const& trans, aabb3 const& box) noexcept +{ + return rotate(trans.rotation_scale, box); +} + +namespace batch +{ +template +void transform(trans2 const& t, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept +{ + batch::transform(to_mat3(t), dst, dst_stride, src, src_stride, count); +} +template +void rotate(trans2 const& t, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept +{ + batch::rotate(to_mat3(t), dst, dst_stride, src, src_stride, count); +} +template +void transform(trans3 const& t, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + batch::transform(to_mat4(t), dst, dst_stride, src, src_stride, count); +} +template +void rotate(trans3 const& t, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + batch::rotate(to_mat4(t), dst, dst_stride, src, src_stride, count); +} +template +void transform(mat3 const& m, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec2) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec2) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = transform(m, *src); + src = (vec2 const*)(((char const*)src) + src_stride); + dst = (vec2*)(((char*)dst) + dst_stride); + } +} +template +void rotate(mat3 const& m, vec2* dst, size_t dst_stride, vec2 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec2) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec2) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = rotate(m, *src); + src = (vec2 const*)(((char const*)src) + src_stride); + dst = (vec2*)(((char*)dst) + dst_stride); + } +} +template +void transform(mat3 const& m, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = transform(m, *src); + src = (vec3 const*)(((char const*)src) + src_stride); + dst = (vec3*)(((char*)dst) + dst_stride); + } +} +template +void rotate(mat3 const& m, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = rotate(m, *src); + src = (vec3 const*)(((char const*)src) + src_stride); + dst = (vec3*)(((char*)dst) + dst_stride); + } +} +template +void transform(mat4 const& mat, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = transform(mat, *src); + src = (vec3 const*)(((char const*)src) + src_stride); + dst = (vec3*)(((char*)dst) + dst_stride); + } +} +#if defined TL_PLATFORM_WINDOWS_FAMILY +template<> +inline void transform(mat4 const& mat, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + + float const* MATH_RESTRICT m = mat.data(); + const __m128 col0 = _mm_loadu_ps(m); + const __m128 col1 = _mm_loadu_ps(m + 4); + const __m128 col2 = _mm_loadu_ps(m + 8); + const __m128 col3 = _mm_loadu_ps(m + 12); + + const __m128 u3 = _mm_set_ps1(1.f); + + for (size_t i = 0; i < count; ++i) + { + float const* MATH_RESTRICT srcf = (const float*)src; + + const __m128 u0 = _mm_set_ps1(srcf[0]); + const __m128 u1 = _mm_set_ps1(srcf[1]); + const __m128 u2 = _mm_set_ps1(srcf[2]); + + const __m128 prod0 = _mm_mul_ps(u0, col0); + const __m128 prod1 = _mm_mul_ps(u1, col1); + const __m128 prod2 = _mm_mul_ps(u2, col2); + const __m128 prod3 = _mm_mul_ps(u3, col3); + + const __m128 result = _mm_add_ps(_mm_add_ps(prod0, prod1), _mm_add_ps(prod2, prod3)); + + float* MATH_RESTRICT dstf = (float*)src; + dstf[0] = result.m128_f32[0]; + dstf[1] = result.m128_f32[1]; + dstf[2] = result.m128_f32[2]; + + src = (vec3 const*)(((char const*)src) + src_stride); + dst = (vec3*)(((char*)dst) + dst_stride); + } +} +#endif +template +void rotate(mat4 const& mat, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = rotate(mat, *src); + src = (vec3 const*)(((char const*)src) + src_stride); + dst = (vec3*)(((char*)dst) + dst_stride); + } +} +#if defined TL_PLATFORM_WINDOWS_FAMILY +template<> +inline void rotate(mat4 const& mat, vec3* dst, size_t dst_stride, vec3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + + float const* MATH_RESTRICT m = mat.data(); + const __m128 col0 = _mm_loadu_ps(m); + const __m128 col1 = _mm_loadu_ps(m + 4); + const __m128 col2 = _mm_loadu_ps(m + 8); + const __m128 col3 = _mm_loadu_ps(m + 12); + + const __m128 u3 = _mm_set_ps1(0.f); + + for (size_t i = 0; i < count; ++i) + { + float const* MATH_RESTRICT srcf = (const float*)src; + + const __m128 u0 = _mm_set_ps1(srcf[0]); + const __m128 u1 = _mm_set_ps1(srcf[1]); + const __m128 u2 = _mm_set_ps1(srcf[2]); + + const __m128 prod0 = _mm_mul_ps(u0, col0); + const __m128 prod1 = _mm_mul_ps(u1, col1); + const __m128 prod2 = _mm_mul_ps(u2, col2); + const __m128 prod3 = _mm_mul_ps(u3, col3); + + const __m128 result = _mm_add_ps(_mm_add_ps(prod0, prod1), _mm_add_ps(prod2, prod3)); + + float* MATH_RESTRICT dstf = (float*)src; + dstf[0] = result.m128_f32[0]; + dstf[1] = result.m128_f32[1]; + dstf[2] = result.m128_f32[2]; + + src = (vec3 const*)(((char const*)src) + src_stride); + dst = (vec3*)(((char*)dst) + dst_stride); + } +} +#endif + +template +void transform(mat4 const& m, vec4* dst, size_t dst_stride, vec4 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec4) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec4) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = transform(m, *src); + src = (vec4 const*)(((char const*)src) + src_stride); + dst = (vec4*)(((char*)dst) + dst_stride); + } +} +#if defined TL_PLATFORM_WINDOWS_FAMILY +template<> +inline void transform(mat4 const& mat, vec4* dst, size_t dst_stride, vec4 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec4) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec4) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + + float const* MATH_RESTRICT m = mat.data(); + const __m128 col0 = _mm_loadu_ps(m); + const __m128 col1 = _mm_loadu_ps(m + 4); + const __m128 col2 = _mm_loadu_ps(m + 8); + const __m128 col3 = _mm_loadu_ps(m + 12); + + for (size_t i = 0; i < count; ++i) + { + float const* MATH_RESTRICT srcf = (const float*)src; + const __m128 u0 = _mm_set_ps1(srcf[0]); + const __m128 u1 = _mm_set_ps1(srcf[1]); + const __m128 u2 = _mm_set_ps1(srcf[2]); + const __m128 u3 = _mm_set_ps1(srcf[3]); + + const __m128 prod0 = _mm_mul_ps(u0, col0); + const __m128 prod1 = _mm_mul_ps(u1, col1); + const __m128 prod2 = _mm_mul_ps(u2, col2); + const __m128 prod3 = _mm_mul_ps(u3, col3); + + const __m128 result = _mm_add_ps(_mm_add_ps(prod0, prod1), _mm_add_ps(prod2, prod3)); + + _mm_storeu_ps((float*)dst, result); + + src = (vec4 const*)(((char const*)src) + src_stride); + dst = (vec4*)(((char*)dst) + dst_stride); + } +} +#endif +template +void rotate(mat4 const& m, vec4* dst, size_t dst_stride, vec4 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec4) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec4) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + *dst = rotate(m, *src); + src = (vec4 const*)(((char const*)src) + src_stride); + dst = (vec4*)(((char*)dst) + dst_stride); + } +} +#if defined TL_PLATFORM_WINDOWS_FAMILY +template<> +inline void rotate(mat4 const& mat, vec4* dst, size_t dst_stride, vec4 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(vec3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(vec3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + + float const* MATH_RESTRICT m = mat.data(); + const __m128 col0 = _mm_loadu_ps(m); + const __m128 col1 = _mm_loadu_ps(m + 4); + const __m128 col2 = _mm_loadu_ps(m + 8); + const __m128 col3 = _mm_loadu_ps(m + 12); + + const __m128 u3 = _mm_set_ps1(0.f); + + for (size_t i = 0; i < count; ++i) + { + float const* MATH_RESTRICT srcf = (const float*)src; + const __m128 u0 = _mm_set_ps1(srcf[0]); + const __m128 u1 = _mm_set_ps1(srcf[1]); + const __m128 u2 = _mm_set_ps1(srcf[2]); + + const __m128 prod0 = _mm_mul_ps(u0, col0); + const __m128 prod1 = _mm_mul_ps(u1, col1); + const __m128 prod2 = _mm_mul_ps(u2, col2); + const __m128 prod3 = _mm_mul_ps(u3, col3); + + const __m128 result = _mm_add_ps(_mm_add_ps(prod0, prod1), _mm_add_ps(prod2, prod3)); + + float* MATH_RESTRICT dstf = (float*)src; + dstf[0] = result.m128_f32[0]; + dstf[1] = result.m128_f32[1]; + dstf[2] = result.m128_f32[2]; + dstf[3] = srcf[3]; + + src = (vec4 const*)(((char const*)src) + src_stride); + dst = (vec4*)(((char*)dst) + dst_stride); + } +} +#endif +////////////////////////////////////////////////////////////////////////// + +template void multiply(mat4 const& m, mat4* dst, size_t dst_stride, mat4 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(mat4) : dst_stride; + src_stride = src_stride == 0 ? sizeof(mat4) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + multiply(*dst, m, *src); + src = (mat4 const*)(((char const*)src) + src_stride); + dst = (mat4*)(((char*)dst) + dst_stride); + } +} +template void multiply(trans3 const& m, trans3* dst, size_t dst_stride, trans3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(trans3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(trans3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + multiply(*dst, m, *src); + src = (trans3 const*)(((char const*)src) + src_stride); + dst = (trans3*)(((char*)dst) + dst_stride); + } +} +template void multiply(mat3 const& m, mat3* dst, size_t dst_stride, mat3 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(mat3) : dst_stride; + src_stride = src_stride == 0 ? sizeof(mat3) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + multiply(*dst, m, *src); + src = (mat3 const*)(((char const*)src) + src_stride); + dst = (mat3*)(((char*)dst) + dst_stride); + } +} +template void multiply(trans2 const& m, trans2* dst, size_t dst_stride, trans2 const* src, size_t src_stride, size_t count) noexcept +{ + dst_stride = dst_stride == 0 ? sizeof(trans2) : dst_stride; + src_stride = src_stride == 0 ? sizeof(trans2) : src_stride; + TL_PLAIN_ASSERT(src && dst || !count); + for (size_t i = 0; i < count; ++i) + { + multiply(*dst, m, *src); + src = (trans2 const*)(((char const*)src) + src_stride); + dst = (trans2*)(((char*)dst) + dst_stride); + } +} + +} + +////////////////////////////////////////////////////////////////////////// +} diff --git a/include/VMath/func_trig.h b/include/VMath/func_trig.h new file mode 100644 index 0000000..d98aa58 --- /dev/null +++ b/include/VMath/func_trig.h @@ -0,0 +1,54 @@ +#pragma once + +namespace math +{ +template constexpr T to_radians(T const& degrees) noexcept; +template constexpr T to_degrees(T const& radians) noexcept; + +////////////////////////////////////////////////////////////////////////// + +//component wise trig for vectors. +//vec3 val = sin(angles) is equivalent to +// val.x = sin(angles.x) +// val.y = sin(angles.y) +// val.z = sin(angles.z) +template /*constexpr*/ T cos(T const& angle) noexcept; +template /*constexpr*/ vec2 cos(vec2 const& angle) noexcept; +template /*constexpr*/ vec3 cos(vec3 const& angle) noexcept; +template /*constexpr*/ vec4 cos(vec4 const& angle) noexcept; + +template /*constexpr*/ T sin(T const& angle) noexcept; +template /*constexpr*/ vec2 sin(vec2 const& angle) noexcept; +template /*constexpr*/ vec3 sin(vec3 const& angle) noexcept; +template /*constexpr*/ vec4 sin(vec4 const& angle) noexcept; + +template /*constexpr*/ T tan(T const& angle) noexcept; +template /*constexpr*/ vec2 tan(vec2 const& angle) noexcept; +template /*constexpr*/ vec3 tan(vec3 const& angle) noexcept; +template /*constexpr*/ vec4 tan(vec4 const& angle) noexcept; + +template /*constexpr*/ void sin_cos(T const& angle, T& sin, T& cos) noexcept; +template /*constexpr*/ void sin_cos(vec2 const& angle, vec2& sin, vec2& cos) noexcept; +template /*constexpr*/ void sin_cos(vec3 const& angle, vec3& sin, vec3& cos) noexcept; +template /*constexpr*/ void sin_cos(vec4 const& angle, vec4& sin, vec4& cos) noexcept; + +template /*constexpr*/ T acos(T const& v) noexcept; +template /*constexpr*/ vec2 acos(vec2 const& v) noexcept; +template /*constexpr*/ vec3 acos(vec3 const& v) noexcept; +template /*constexpr*/ vec4 acos(vec4 const& v) noexcept; + +template /*constexpr*/ T asin(T const& v) noexcept; +template /*constexpr*/ vec2 asin(vec2 const& v) noexcept; +template /*constexpr*/ vec3 asin(vec3 const& v) noexcept; +template /*constexpr*/ vec4 asin(vec4 const& v) noexcept; + +template /*constexpr*/ T atan(T const& v) noexcept; +template /*constexpr*/ vec2 atan(vec2 const& v) noexcept; +template /*constexpr*/ vec3 atan(vec3 const& v) noexcept; +template /*constexpr*/ vec4 atan(vec4 const& v) noexcept; + +template /*constexpr*/ T atan2(T const& y, T const& x) noexcept; +template /*constexpr*/ vec2 atan2(vec2 const& y, vec2 const& x) noexcept; +template /*constexpr*/ vec3 atan2(vec3 const& y, vec3 const& x) noexcept; +template /*constexpr*/ vec4 atan2(vec4 const& y, vec4 const& x) noexcept; +} diff --git a/include/VMath/func_trig.inl b/include/VMath/func_trig.inl new file mode 100644 index 0000000..707bf74 --- /dev/null +++ b/include/VMath/func_trig.inl @@ -0,0 +1,635 @@ +namespace math +{ + + +////////////////////////////////////////////////////////////////////////// +// scalars + +template +constexpr T to_radians(T const& degrees) noexcept +{ + static_assert(std::is_floating_point_v); + TL_PLAIN_ASSERT(is_finite(degrees)); + if constexpr (std::is_floating_point_v) + return degrees * angle::pi * T(0.00555555555555555555555555555556); + else + return degrees * angle::pi / T(180); +} + +template +constexpr T to_degrees(T const& a) noexcept +{ + static_assert(std::is_floating_point_v); + TL_PLAIN_ASSERT(is_finite(a)); + return a * T(180) / angle::pi; +} + +////////////////////////////////////////////////////////////////////////// + +namespace detail +{ +//one version of fast_sin - around 1100 cycles on atmega2560 +//max_error = 0.001091, avg_error = 0.000505 +// constexpr float _fast_sin(float x) +// { +// float res = x * (1.27323954f - 0.405284735f * math::abs(x)); +// return res * (.225f * (math::abs(res) - 1.f) + 1.f); +// } +// +// constexpr float fast_sin(float x) +// { +// //always wrap input angle to -PI..PI +// if (is_negative(x) && x < -3.14159266f) +// { +// do { x += 6.28318531f; } while (x < -3.14159266f); +// } +// else if (x > 3.14159266f) +// { +// do { x -= 6.28318531f; } while (x > 3.14159266f); +// } +// return _fast_sin(x); +// } +// constexpr float fast_cos(float x) +// { +// return fast_sin(x + 1.5707963268f); +// } +// constexpr void fast_sin_cos(float x, float& s, float& c) +// { +// //always wrap input angle to -PI..PI +// if (is_negative(x) && x < -3.14159266f) +// { +// do { x += 6.28318531f; } while (x < -3.14159266f); +// +// s = _fast_sin(x); +// c = _fast_sin(x + 1.5707963268f); //no need to check for overflow +// } +// else if (x > 3.14159266f) +// { +// do { x -= 6.28318531f; } while (x > 3.14159266f); +// +// s = _fast_sin(x); +// c = _fast_sin(x + 1.5707963268f); //no need to check for overflow +// } +// else +// { +// s = _fast_sin(x); +// x += 1.5707963268f; +// if (is_positive(x) && x > 3.14159266f) +// { +// x -= 6.28318531f; +// } +// c = _fast_sin(x); +// } +// } + +////////////////////////////////////////////////////////////////////////// +// lookup table version: around 600 cycles + //max_error = 0.000538 avg_error = 0.000347 + + //mantissa of (sin(x = 0 - pi/2) + 1) + 1 + //the last element is to allow interpolating without checking for out of bounds + extern const uint32_t k_sin_cos[806]; + + inline float fast_sin_pi2(float x) noexcept //x = 64 .. 64 + pi/2 + { + TL_PLAIN_ASSERT(x >= 64 && x <= 64 + anglef::pi2); + //0 1234567 0 1234567 01234567 01234567 + //s eeeeeee e ------t tttttttt iiiiiiii + + const uint8_t* __restrict x1_ptr = reinterpret_cast(&x); + const uint8_t it = *x1_ptr; //low 8 bits are for interpolating between table entries + const uint16_t fx = *reinterpret_cast(x1_ptr + 1)&0x7FFF; //high 9 bits are for table lookup + + //now fx should be between 0 and 804 + + //compose the float from a precomputed exponent (1.0) by appending the decimals + const uint16_t t1 = k_sin_cos[fx] >> 16; + const uint16_t t2 = k_sin_cos[fx + 1] >> 16; + //v = a + (b - a)*t; + const uint32_t v = (uint32_t(t1) << 7) + ((uint32_t(t2 - t1) * it) >> 1); + + const uint32_t f = 0x3F800000 | v; + const float* __restrict ret = reinterpret_cast(&f); //1 .. 2 + + return *ret; + } + inline float fast_sin(float x) noexcept + { + while (is_positive(x) && x >= anglef::_2pi) x -= anglef::_2pi; + while (is_negative(x)) x += anglef::_2pi; + + if (x >= anglef::pi) + { + if (x >= 3.f * anglef::pi2) //IV + return 1.f - fast_sin_pi2(64.f + (anglef::_2pi - x)); + else //III + return 1.f - fast_sin_pi2(64.f + (x - anglef::pi)); + } + else + { + if (x >= anglef::pi2) //II + return fast_sin_pi2(64.f + (anglef::pi - x)) - 1.f; + else //I + return fast_sin_pi2(64.f + x) - 1.f; + } + + } + + inline float fast_cos(float x) noexcept + { + return fast_sin(x + anglef::pi2); + } + + inline void fast_sin_cos_pi2(float x, float& sres, float& cres) noexcept //x = 64 .. 64 + pi/2 + { + TL_PLAIN_ASSERT(x >= 64 && x <= 64 + anglef::pi2); + //0 1234567 0 1234567 01234567 01234567 + //s eeeeeee e ------t tttttttt iiiiiiii + + const uint8_t* __restrict x1_ptr = reinterpret_cast(&x); + const uint8_t it = *x1_ptr; //low 8 bits are for interpolating between table entries + const uint16_t fx = *reinterpret_cast(x1_ptr + 1) & 0x7FFF; //high 9 bits are for table lookup + + //now fx should be between 0 and 804 + + //compose the float from a precomputed exponent (1.0) by appending the decimals + const uint32_t sc1 = k_sin_cos[fx]; + const uint32_t sc2 = k_sin_cos[fx + 1]; + + { + const uint16_t v1 = sc1 >> 16; + const uint16_t v2 = sc2 >> 16; + //v = a + (b - a)*t; + const uint32_t v = (uint32_t(v1) << 7) + ((uint32_t(v2 - v1) * it) >> 1); + + const uint32_t f = 0x3F800000 | v; + sres = *reinterpret_cast(&f); //1 .. 2 + } + { + const uint16_t v1 = sc1 & 0xFFFF; + const uint16_t v2 = sc2 & 0xFFFF; + //v = a + (b - a)*t; + const uint32_t v = (uint32_t(v1) << 7) - ((uint32_t(v1 - v2) * it) >> 1); + + const uint32_t f = 0x3F800000 | v; + cres = *reinterpret_cast(&f); //1 .. 2 + } + } + + inline void fast_sin_cos(float x, float& s, float& c) noexcept + { + while (is_positive(x) && x >= anglef::_2pi) x -= anglef::_2pi; + while (is_negative(x)) x += anglef::_2pi; + + if (x >= anglef::pi) + { + if (x >= 3.f * anglef::pi2) + { + //IV + fast_sin_cos_pi2(64.f + (anglef::_2pi - x), s, c); + s = 1.f - s; + c = c - 1.f; + } + else + { + //III + fast_sin_cos_pi2(64.f + (x - anglef::pi), s, c); + s = 1.f - s; + c = 1.f - c; + } + } + else + { + if (x >= anglef::pi2) + { + //II + fast_sin_cos_pi2(64.f + (anglef::pi - x), s, c); + s = s - 1.f; + c = 1.f - c; + } + else + { + //I + fast_sin_cos_pi2(64.f + x, s, c); + s = s - 1.f; + c = c - 1.f; + } + } + } +} + +////////////////////////////////////////////////////////////////////////// +//standard + +template<> /*constexpr*/ inline float cos(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::cosf(s); +} +template<> /*constexpr*/ inline float sin(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::sinf(s); +} +template<> /*constexpr*/ inline float tan(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::tanf(s); +} +template<> /*constexpr*/ inline void sin_cos(float const& angle, float& s, float& c) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + s = ::sinf(angle); + c = ::cosf(angle); +} + +template<> /*constexpr*/ inline double cos(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::cos(s); +} +template<> /*constexpr*/ inline double sin(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::sin(s); +} +template<> /*constexpr*/ inline double tan(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::tan(s); +} +template<> /*constexpr*/ inline void sin_cos(double const& angle, double& s, double& c) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + s = ::sin(angle); + c = ::cos(angle); +} + +////////////////////////////////////////////////////////////////////////// +//fast + +template<> /*constexpr*/ inline float cos(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return detail::fast_cos(s); +} +template<> /*constexpr*/ inline float sin(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return detail::fast_sin(s); +} +template<> /*constexpr*/ inline float tan(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::tanf(s); +} +template<> /*constexpr*/ inline void sin_cos(float const& angle, float& s, float& c) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + detail::fast_sin_cos(angle, s, c); +} + +template<> /*constexpr*/ inline double cos(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::cos(s); +} +template<> /*constexpr*/ inline double sin(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::sin(s); +} +template<> /*constexpr*/ inline double tan(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return ::tan(s); +} +template<> /*constexpr*/ inline void sin_cos(double const& angle, double& s, double& c) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + s = ::sin(angle); + c = ::cos(angle); +} + +////////////////////////////////////////////////////////////////////////// +//safe + +template<> /*constexpr*/ inline float cos(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 1 : ::cosf(s); +} +template<> /*constexpr*/ inline float sin(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : ::sinf(s); +} +template<> /*constexpr*/ inline float tan(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : ::tanf(s); +} +template<> /*constexpr*/ inline void sin_cos(float const& angle, float& s, float& c) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + if (is_finite(angle)) + { + s = ::sinf(angle); + c = ::cosf(angle); + } + else + { + s = 0; + c = 1; + } +} + +template<> /*constexpr*/ inline double cos(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 1 : ::cos(s); +} +template<> /*constexpr*/ inline double sin(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : ::sin(s); +} +template<> /*constexpr*/ inline double tan(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : ::tan(s); +} +template<> /*constexpr*/ inline void sin_cos(double const& angle, double& s, double& c) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + if (!is_finite(angle)) + { + s = ::sin(angle); + c = ::cos(angle); + } + else + { + s = 0; + c = 1; + } +} + +template /*constexpr*/ inline void sin_cos(T const& angle, T& s, T& c) noexcept +{ + s = sin(angle); + c = cos(angle); +} + +////////////////////////////////////////////////////////////////////////// + +template +/*constexpr*/ inline T cos(T const& angle) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + return std::cos(angle); +} +template +/*constexpr*/ inline T sin(T const& angle) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + return std::sin(angle); +} +template +/*constexpr*/ inline T tan(T const& angle) noexcept +{ + TL_PLAIN_ASSERT(is_finite(angle)); + return std::tan(angle); +} + +////////////////////////////////////////////////////////////////////////// + +template<> /*constexpr*/ inline float acos(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return float(::acosf(s)); +} +template<> /*constexpr*/ inline float asin(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return float(::asinf(s)); +} +template<> /*constexpr*/ inline float atan(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return float(::atanf(s)); +} +template<> /*constexpr*/ inline float atan2(float const& y, float const& x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x) && is_finite(y)); + return float(::atan2f(y, x)); +} + +template<> /*constexpr*/ inline double acos(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return double(::acos(s)); +} +template<> /*constexpr*/ inline double asin(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return double(::asin(s)); +} +template<> /*constexpr*/ inline double atan(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return double(::atan(s)); +} +template<> /*constexpr*/ inline double atan2(double const& y, double const& x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x) && is_finite(y)); + return double(::atan2(y, x)); +} + +template<> /*constexpr*/ inline float acos(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return float(::acosf(s)); +} +template<> /*constexpr*/ inline float asin(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return float(::asinf(s)); +} +template<> /*constexpr*/ inline float atan(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return float(::atanf(s)); +} +template<> /*constexpr*/ inline float atan2(float const& y, float const& x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x) && is_finite(y)); + return float(::atan2f(y, x)); +} + +template<> /*constexpr*/ inline double acos(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return double(::acos(s)); +} +template<> /*constexpr*/ inline double asin(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return double(::asin(s)); +} +template<> /*constexpr*/ inline double atan(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return double(::atan(s)); +} +template<> /*constexpr*/ inline double atan2(double const& y, double const& x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x) && is_finite(y)); + return double(::atan2(y, x)); +} + +template<> /*constexpr*/ inline float acos(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? angle::pi2 : float(::acosf(clamp(s, 0.f, 1.f))); +} +template<> /*constexpr*/ inline float asin(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : float(::asinf(s)); +} +template<> /*constexpr*/ inline float atan(float const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : float(::atanf(s)); +} +template<> /*constexpr*/ inline float atan2(float const& y, float const& x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x) && is_finite(y) && !is_zero(y)); + return !is_finite(x) || !is_finite(y) || is_zero(y) ? 0 : float(::atan2f(y, x)); +} + +template<> /*constexpr*/ inline double acos(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? angle::pi2 : double(::acos(s)); +} +template<> /*constexpr*/ inline double asin(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : double(::asin(s)); +} +template<> /*constexpr*/ inline double atan(double const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + return !is_finite(s) ? 0 : double(::atan(s)); +} +template<> /*constexpr*/ inline double atan2(double const& y, double const& x) noexcept +{ + TL_PLAIN_ASSERT(is_finite(x) && is_finite(y) && !is_zero(y)); + return !is_finite(x) || !is_finite(y) || is_zero(y) ? 0 : double(::atan2(y, x)); +} + +////////////////////////////////////////////////////////////////////////// +// vec2 +template +constexpr vec2 to_degrees(vec2 const& v) noexcept +{ + return vec2(to_degrees(v.x), to_degrees(v.y)); +} +template +constexpr vec2 to_radians(vec2 const& v) noexcept +{ + return vec2(to_radians(v.x), to_radians(v.y)); +} + +template +/*constexpr*/ inline vec2 cos(vec2 const& v) noexcept +{ + return vec2(cos(v.x), cos(v.y)); +} +template +/*constexpr*/ inline vec2 sin(vec2 const& v) noexcept +{ + return vec2(sin(v.x), sin(v.y)); +} +template +/*constexpr*/ inline vec2 tan(vec2 const& v) noexcept +{ + return vec2(tan(v.x), tan(v.y)); +} +template +/*constexpr*/ inline void sin_cos(vec2 const& angle, vec2& s, vec2& c) noexcept +{ + s = sin(angle); + c = cos(angle); +} + +////////////////////////////////////////////////////////////////////////// +// vec3 +template +constexpr vec3 to_degrees(vec3 const& v) noexcept +{ + return vec3(to_degrees(v.x), to_degrees(v.y), to_degrees(v.z)); +} +template +constexpr vec3 to_radians(vec3 const& v) noexcept +{ + return vec3(to_radians(v.x), to_radians(v.y), to_radians(v.z)); +} +template +/*constexpr*/ inline vec3 cos(vec3 const& v) noexcept +{ + return vec3(cos(v.x), cos(v.y), cos(v.z)); +} +template +/*constexpr*/ inline vec3 sin(vec3 const& v) noexcept +{ + return vec3(sin(v.x), sin(v.y), sin(v.z)); +} +template +/*constexpr*/ inline vec3 tan(vec3 const& v) noexcept +{ + return vec3(tan(v.x), tan(v.y), tan(v.z)); +} +template +/*constexpr*/ inline void sin_cos(vec3 const& angle, vec3& s, vec3& c) noexcept +{ + s = sin(angle); + c = cos(angle); +} + +////////////////////////////////////////////////////////////////////////// +// vec4 +template +constexpr vec4 to_degrees(vec4 const& v) noexcept +{ + return vec4(to_degrees(v.x), to_degrees(v.y), to_degrees(v.z), to_degrees(v.w)); +} +template +constexpr vec4 to_radians(vec4 const& v) noexcept +{ + return vec4(to_radians(v.x), to_radians(v.y), to_radians(v.z), to_radians(v.w)); +} +template +/*constexpr*/ inline vec4 cos(vec4 const& v) noexcept +{ + return vec4(cos(v.x), cos(v.y), cos(v.z), cos(v.w)); +} +template +/*constexpr*/ inline vec4 sin(vec4 const& v) noexcept +{ + return vec4(sin(v.x), sin(v.y), sin(v.z), sin(v.w)); +} +template +/*constexpr*/ inline vec4 tan(vec4 const& v) noexcept +{ + return vec4(tan(v.x), tan(v.y), tan(v.z), tan(v.w)); +} +template +/*constexpr*/ inline void sin_cos(vec4 const& angle, vec4& s, vec4& c) noexcept +{ + s = sin(angle); + c = cos(angle); +} + +} diff --git a/include/VMath/line2.h b/include/VMath/line2.h new file mode 100644 index 0000000..cdcfe0c --- /dev/null +++ b/include/VMath/line2.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace math +{ + +template +struct line2 +{ + typedef T value_t; + typedef line2 this_t; + + constexpr line2() noexcept = default; + constexpr line2(const vec2& start, const vec2& direction) noexcept; + constexpr line2(const line2&) noexcept = default; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr line2& operator=(const line2&) noexcept = default; + + constexpr bool is_valid() const noexcept; + constexpr vec2 get_point(T distance) const noexcept; + constexpr void translate(const vec2& offset) noexcept; + + + vec2 origin; + vec2 direction; +}; + +} diff --git a/include/VMath/line2.inl b/include/VMath/line2.inl new file mode 100644 index 0000000..63a6181 --- /dev/null +++ b/include/VMath/line2.inl @@ -0,0 +1,45 @@ +namespace math +{ + +template +constexpr line2::line2(const vec2& start, const vec2& direction) noexcept + : origin(start) + , direction(direction) +{ +} + +template +constexpr bool line2::is_valid() const noexcept +{ + return !is_zero(direction); +} +template +constexpr vec2 line2::get_point(T distance) const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return origin + direction * distance; +} +template +constexpr void line2::translate(const vec2& offset) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + origin += offset; +} + + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::line2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.origin); + tl::hash_and_combine(seed, p.direction); + return seed; + } +}; diff --git a/include/VMath/line3.h b/include/VMath/line3.h new file mode 100644 index 0000000..531c0c9 --- /dev/null +++ b/include/VMath/line3.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace math +{ + +template +struct line3 +{ + typedef T value_t; + typedef line3 this_t; + + constexpr line3() noexcept = default; + constexpr line3(const vec3& start, const vec3& direction) noexcept; + constexpr line3(const line3&) noexcept = default; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr line3& operator=(const line3&) noexcept = default; + + constexpr bool is_valid() const noexcept; + constexpr vec3 get_point(T distance) const noexcept; + constexpr void translate(const vec3& offset) noexcept; + + + vec3 origin; + vec3 direction; +}; + +} diff --git a/include/VMath/line3.inl b/include/VMath/line3.inl new file mode 100644 index 0000000..37e8557 --- /dev/null +++ b/include/VMath/line3.inl @@ -0,0 +1,45 @@ +namespace math +{ + +template +constexpr line3::line3(const vec3& start, const vec3& direction) noexcept + : origin(start) + , direction(direction) +{ +} + +template +constexpr bool line3::is_valid() const noexcept +{ + return !is_zero(direction); +} +template +constexpr vec3 line3::get_point(T distance) const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return origin + direction * distance; +} +template +constexpr void line3::translate(const vec3& offset) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + origin += offset; +} + + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::line3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.origin); + tl::hash_and_combine(seed, p.direction); + return seed; + } +}; diff --git a/include/VMath/mat2.h b/include/VMath/mat2.h new file mode 100644 index 0000000..b9ed28f --- /dev/null +++ b/include/VMath/mat2.h @@ -0,0 +1,92 @@ +#pragma once + +#include + +namespace math +{ +template +struct vec2; + +//column-major (OpenGL) layout + +// .----------- packet0 (column0) - x axis +// | .------- packet1 (column1) - y axis +// 0 2 +// 1 3 + + +template +struct mat2 +{ + typedef T value_t; + using this_t = mat2; + static constexpr size_t row_count = 2; + static constexpr size_t column_count = 2; + static constexpr size_t element_count = 4; + + template + constexpr static mat2 from_rotation(T rotation) noexcept; + // template + // static mat2 from_rotation(math::angle angle) noexcept; + + constexpr mat2() noexcept; + constexpr explicit mat2(math::ZUninitialized) noexcept; + constexpr explicit mat2(angle const& rotation) noexcept; + constexpr explicit mat2(T value) noexcept; + constexpr mat2(mat2 const&) noexcept = default; + + //column major - column0 is initialized from v0, v1 + constexpr mat2(T v0, T v1, + T v2, T v3) noexcept; + + constexpr mat2(vec2 const& column0, + vec2 const& column1) noexcept; + + constexpr mat2& operator=(mat2 const&) noexcept = default; + + //Rows are NOT linearly in memory. First row is m[0], m[2] + constexpr vec2 get_row(size_t row) const noexcept; + constexpr void set_row(size_t row, vec2 const& v) noexcept; + + //Columns are linearly in memory. First row is m[0], m[1], and is the X axis of the matrix + constexpr vec2 const& get_column(size_t column) const noexcept; + constexpr void set_column(size_t column, vec2 const& v) noexcept; + + constexpr vec2 const& get_axis_x() const noexcept; + constexpr void set_axis_x(vec2 const& axis) noexcept; + + constexpr vec2 const& get_axis_y() const noexcept; + constexpr void set_axis_y(vec2 const& axis) noexcept; + + template + constexpr vec2 get_scale() const noexcept; + constexpr void set_scale(vec2 const& scale) noexcept; + + constexpr T determinant() const noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr T* data() noexcept; + constexpr T const* data() const noexcept; + + constexpr T& element(size_t column, size_t row) noexcept; + constexpr T const& element(size_t column, size_t row) const noexcept; + + constexpr mat2 operator*(mat2 const& other) const noexcept; + constexpr mat2 operator+(mat2 const& other) const noexcept; + constexpr mat2 operator-(mat2 const& other) const noexcept; + constexpr mat2 operator*(T scalar) const noexcept; + constexpr mat2 operator+(T scalar) const noexcept; + constexpr mat2 operator-(T scalar) const noexcept; + constexpr mat2& operator*=(mat2 const& other) noexcept; + constexpr mat2& operator+=(mat2 const& other) noexcept; + constexpr mat2& operator-=(mat2 const& other) noexcept; + constexpr mat2& operator*=(T scalar) noexcept; + constexpr mat2& operator+=(T scalar) noexcept; + constexpr mat2& operator-=(T scalar) noexcept; + + vec2 column0; + vec2 column1; +}; +} diff --git a/include/VMath/mat2.inl b/include/VMath/mat2.inl new file mode 100644 index 0000000..597b638 --- /dev/null +++ b/include/VMath/mat2.inl @@ -0,0 +1,285 @@ +namespace math +{ + +template +template +constexpr mat2 mat2::from_rotation(T rotation) noexcept +{ + if (rotation == T(0)) + return {}; + else + { + T s, c; + sin_cos(rotation, s, c); + return { c, s, -s, c }; + } +} + +// template +// template +// constexpr mat2 mat2::from_rotation(math::angle angle) noexcept +// { +// return from_rotation(angle.radians); +// } + +template +constexpr mat2::mat2() noexcept + : column0(T(1), T(0)) + , column1(T(0), T(1)) +{ +} + +template +constexpr mat2::mat2(math::ZUninitialized) noexcept +{ +} + +template +constexpr mat2::mat2(angle const& rotation) noexcept + : column0(math::uninitialized) + , column1(math::uninitialized) +{ + T s, c; + sin_cos(rotation.radians, s, c); + T* m = data(); + m[0] = c; + m[1] = s; + m[2] = -s; + m[3] = c; +} + +template +constexpr mat2::mat2(T value) noexcept + : column0(value, value) + , column1(value, value) +{ + TL_PLAIN_ASSERT(is_finite(value)); +} + +template +constexpr mat2::mat2(T v0, T v1, T v2, T v3) noexcept + : column0(v0, v1) + , column1(v2, v3) +{ + TL_PLAIN_ASSERT(is_finite(column0) && + is_finite(column1)); +} + +template +constexpr mat2::mat2(vec2 const& column0, vec2 const& column1) noexcept + : column0(column0) + , column1(column1) +{ + TL_PLAIN_ASSERT(is_finite(column0) && + is_finite(column1)); +} + +/////////////////////////////////////////////////////////////////////////////// +//methods +/////////////////////////////////////////////////////////////////////////////// + +template +constexpr vec2 mat2::get_row(size_t row) const noexcept +{ + TL_PLAIN_ASSERT(row < row_count); + const T* m = data(); + return vec2(m[row], m[row + 2]); +} +template +constexpr void mat2::set_row(size_t row, vec2 const& v) noexcept +{ + TL_PLAIN_ASSERT(is_finite(v)); + TL_PLAIN_ASSERT(row < row_count); + T* m = data(); + m[row] = v.x; + m[row + 2] = v.y; +} + +template +constexpr vec2 const& mat2::get_column(size_t column) const noexcept +{ + TL_PLAIN_ASSERT(column < column_count); + const T* m = data(); + size_t idx = column * row_count; + return *(vec2 const*)(&m[idx]); +} +template +constexpr void mat2::set_column(size_t column, vec2 const& v) noexcept +{ + TL_PLAIN_ASSERT(is_finite(v)); + TL_PLAIN_ASSERT(column < column_count); + T* m = data(); + size_t idx = column * row_count; + *(vec2*)(&m[idx]) = v; +} + +template +constexpr vec2 const& mat2::get_axis_x() const noexcept +{ + return get_column(0); +} + +template +constexpr vec2 const& mat2::get_axis_y() const noexcept +{ + return get_column(1); +} + +template +template +constexpr vec2 mat2::get_scale() const noexcept +{ + return vec2(get_axis_x().template length(), get_axis_y().template length()); +} + +template +constexpr void mat2::set_axis_x(vec2 const& axis) noexcept +{ + set_column(0, axis); +} + +template +constexpr void mat2::set_axis_y(vec2 const& axis) noexcept +{ + set_column(1, axis); +} + +template +constexpr void mat2::set_scale(vec2 const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + T* m = data(); + m[0] = s.x; + m[3] = s.y; +} + +template +constexpr T mat2::determinant() const noexcept +{ + const T* m = data(); + return m[0] * m[3] + - m[1] * m[2]; +} + +/////////////////////////////////////////////////////////////////////////////// +// indexing operators +/////////////////////////////////////////////////////////////////////////////// + +template +constexpr T* mat2::data() noexcept +{ + return (T*)&column0; +} +template +constexpr T const* mat2::data() const noexcept +{ + return (T const*)&column0; +} + +template +constexpr T& mat2::element(size_t column, size_t row) noexcept +{ + TL_PLAIN_ASSERT(column < column_count&& row < row_count); + T* m = data(); + return m[column * row_count + row]; +} + +template +constexpr T const& mat2::element(size_t column, size_t row) const noexcept +{ + TL_PLAIN_ASSERT(column < column_count&& row < row_count); + const T* m = data(); + return m[column * row_count + row]; +} + +template +constexpr mat2 mat2::operator*(mat2 const& other) const noexcept +{ + mat2 ret; + return multiply(ret, *this, other); +} +template +constexpr mat2 mat2::operator+(mat2 const& other) const noexcept +{ + mat2 ret; + return cwise::add(ret, *this, other); +} +template +constexpr mat2 mat2::operator-(mat2 const& other) const noexcept +{ + mat2 ret; + return cwise::substract(ret, *this, other); +} +template +constexpr mat2& mat2::operator*=(mat2 const& other) noexcept +{ + mat2 a(*this); + return multiply(*this, a, other); +} +template +constexpr mat2& mat2::operator+=(mat2 const& other) noexcept +{ + return cwise::add(*this, *this, other); +} +template +constexpr mat2& mat2::operator-=(mat2 const& other) noexcept +{ + return cwise::substract(*this, *this, other); +} + +template +constexpr mat2 mat2::operator*(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat2 ret; + return cwise::multiply(ret, *this, scalar); +} +template +constexpr mat2 mat2::operator+(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat2 ret; + return cwise::add(ret, *this, scalar); +} +template +constexpr mat2 mat2::operator-(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat2 ret; + return cwise::substract(ret, *this, scalar); +} +template +constexpr mat2& mat2::operator*=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::multiply(*this, *this, scalar); +} +template +constexpr mat2& mat2::operator+=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::add(*this, *this, scalar); +} +template +constexpr mat2& mat2::operator-=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::substract(*this, *this, scalar); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::mat2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.column0); + tl::hash_and_combine(seed, p.column1); + return seed; + } +}; diff --git a/include/VMath/mat3.h b/include/VMath/mat3.h new file mode 100644 index 0000000..4adb2bd --- /dev/null +++ b/include/VMath/mat3.h @@ -0,0 +1,101 @@ +#pragma once + +#include + +namespace math +{ +template +struct vec3; + +//column-major (OpenGL) layout + +// .--------------- column0 - x axis +// | .----------- column1 - y axis +// | | .------- column2 - translation +// 0 3 6 +// 1 4 7 +// 2 5 8 + +template +struct mat3 +{ + typedef T value_t; + using this_t = mat3; + static constexpr size_t row_count = 3; + static constexpr size_t column_count = 3; + static constexpr size_t element_count = 9; + + ///! Creates a world space lookat matrix (front axis is -yaxis, up axis is zaxis) + /// This lookAt method computes a look at matrix in jet coordinate system (3dmax biped). + /// This means that when you send a front of (0,1,0) and an up of (0,0,1) the resulting matrix is identity. + template + constexpr static mat3 from_look_at(vec3 const& front, vec3 const& up) noexcept; + + constexpr mat3() noexcept; + constexpr explicit mat3(math::ZUninitialized) noexcept; + constexpr explicit mat3(T value) noexcept; + constexpr mat3(mat3 const&) noexcept = default; + + //column major (column 0 is initialized from v0, v1, v2) + constexpr mat3(T v0, T v1, T v2, + T v3, T v4, T v5, + T v6, T v7, T v8) noexcept; + + constexpr mat3(vec3 const& column0, + vec3 const& column1, + vec3 const& column2) noexcept; + + constexpr mat3& operator=(mat3 const&) noexcept = default; + + //Rows are NOT linearly in memory. First row is m[0], m[3], m[6] + constexpr vec3 get_row(size_t row) const noexcept; + constexpr void set_row(size_t row, vec3 const& v) noexcept; + + //Columns are linearly in memory. First row is m[0], m[1], m[2] and is the X axis of the matrix + constexpr vec3 const& get_column(size_t column) const noexcept; + constexpr void set_column(size_t column, vec3 const& v) noexcept; + + constexpr vec3 const& get_axis_x() const noexcept; + constexpr void set_axis_x(vec3 const& axis) noexcept; + + constexpr vec3 const& get_axis_y() const noexcept; + constexpr void set_axis_y(vec3 const& axis) noexcept; + + constexpr vec3 const& get_axis_z() const noexcept; + constexpr void set_axis_z(vec3 const& axis) noexcept; + + template + constexpr vec3 get_scale() const noexcept; + constexpr void set_scale(vec3 const& scale) noexcept; + + constexpr void post_scale(vec3 const& scale) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr T determinant() const noexcept; + + constexpr T* data() noexcept; + constexpr T const* data() const noexcept; + + constexpr T& element(size_t column, size_t row) noexcept; + constexpr T const& element(size_t column, size_t row) const noexcept; + + constexpr mat3 operator*(mat3 const& other) const noexcept; + constexpr mat3 operator+(mat3 const& other) const noexcept; + constexpr mat3 operator-(mat3 const& other) const noexcept; + constexpr mat3 operator*(T scalar) const noexcept; + constexpr mat3 operator+(T scalar) const noexcept; + constexpr mat3 operator-(T scalar) const noexcept; + constexpr mat3& operator*=(mat3 const& other) noexcept; + constexpr mat3& operator+=(mat3 const& other) noexcept; + constexpr mat3& operator-=(mat3 const& other) noexcept; + constexpr mat3& operator*=(T scalar) noexcept; + constexpr mat3& operator+=(T scalar) noexcept; + constexpr mat3& operator-=(T scalar) noexcept; + + vec3 column0; + vec3 column1; + vec3 column2; +}; +} diff --git a/include/VMath/mat3.inl b/include/VMath/mat3.inl new file mode 100644 index 0000000..b474dfe --- /dev/null +++ b/include/VMath/mat3.inl @@ -0,0 +1,289 @@ +namespace math +{ + +template +template +constexpr mat3 mat3::from_look_at(vec3 const& front, vec3 const& up) noexcept +{ + vec3 axisY = normalized(front); + vec3 axisX = normalized(cross(axisY, normalized(up)));//this normalize is mandatory because axisY and up may be unitary but they hardly are orthogonal + vec3 axisZ = cross(axisX, axisY); + return mat3(axisX, axisY, axisZ); +} + +template +constexpr mat3::mat3() noexcept + : column0(T(1), T(0), T(0)) + , column1(T(0), T(1), T(0)) + , column2(T(0), T(0), T(1)) +{ +} + +template +constexpr mat3::mat3(ZUninitialized) noexcept +{ +} + +template +constexpr mat3::mat3(T value) noexcept + : column0(value, value, value) + , column1(value, value, value) + , column2(value, value, value) +{ + TL_PLAIN_ASSERT(is_finite(value)); +} + +template +constexpr mat3::mat3(T v0, T v1, T v2, T v3, T v4, T v5, T v6, T v7, T v8) noexcept + : column0(v0, v1, v2) + , column1(v3, v4, v5) + , column2(v6, v7, v8) +{ + TL_PLAIN_ASSERT(is_finite(column0) && + is_finite(column1) && + is_finite(column2)); +} + +template +constexpr mat3::mat3(vec3 const& column0, vec3 const& column1, vec3 const& column2) noexcept + : column0(column0) + , column1(column1) + , column2(column2) +{ + TL_PLAIN_ASSERT(is_finite(column0) && + is_finite(column1) && + is_finite(column2)); +} + +template +constexpr vec3 mat3::get_row(size_t row) const noexcept +{ + TL_PLAIN_ASSERT(row < row_count); + const T* m = data(); + return vec3(m[row + 0], m[row + 3], m[row + 6]); +} +template +constexpr void mat3::set_row(size_t row, vec3 const& v) noexcept +{ + TL_PLAIN_ASSERT(is_finite(v)); + TL_PLAIN_ASSERT(row < row_count); + T* m = data(); + m[row + 0] = v.x; + m[row + 3] = v.y; + m[row + 6] = v.z; +} + +template +constexpr vec3 const& mat3::get_column(size_t column) const noexcept +{ + TL_PLAIN_ASSERT(column < column_count); + const T* m = data(); + size_t idx = column * row_count; + return *reinterpret_cast const*>(&m[idx]); +} +template +constexpr void mat3::set_column(size_t column, vec3 const& v) noexcept +{ + TL_PLAIN_ASSERT(is_finite(v)); + TL_PLAIN_ASSERT(column < column_count); + T* m = data(); + size_t idx = column * row_count; + *reinterpret_cast*>(&m[idx]) = v; +} + +template +constexpr vec3 const& mat3::get_axis_x() const noexcept +{ + return get_column(0); +} + +template +constexpr vec3 const& mat3::get_axis_y() const noexcept +{ + return get_column(1); +} + +template +constexpr vec3 const& mat3::get_axis_z() const noexcept +{ + return get_column(2); +} + +template +template +constexpr vec3 mat3::get_scale() const noexcept +{ + return vec3(get_axis_x().template length(), get_axis_y().template length(), get_axis_z().template length()); +} + +template +constexpr void mat3::set_axis_x(vec3 const& axis) noexcept +{ + set_column(0, axis); +} + +template +constexpr void mat3::set_axis_y(vec3 const& axis) noexcept +{ + set_column(1, axis); +} + +template +constexpr void mat3::set_axis_z(vec3 const& axis) noexcept +{ + set_column(2, axis); +} + +template +constexpr void mat3::set_scale(vec3 const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + T* m = data(); + m[0] = s.x; + m[4] = s.y; + m[8] = s.z; +} + +template +constexpr void mat3::post_scale(vec3 const& s) noexcept +{ + TL_PLAIN_ASSERT(is_finite(s)); + T* m = data(); + m[0] *= s.x; + m[4] *= s.y; + m[8] *= s.z; +} + +template +constexpr T mat3::determinant() const noexcept +{ + const T* m = data(); + return m[0] * (m[4] * m[8] - m[5] * m[7]) + - m[1] * (m[3] * m[8] - m[5] * m[6]) + + m[2] * (m[3] * m[7] - m[4] * m[6]); +} + +/////////////////////////////////////////////////////////////////////////////// +// indexing operators +/////////////////////////////////////////////////////////////////////////////// + +template +constexpr T* mat3::data() noexcept +{ + return (T*)&column0; +} +template +constexpr T const* mat3::data() const noexcept +{ + return (T const*)&column0; +} + +template +constexpr T& mat3::element(size_t column, size_t row) noexcept +{ + TL_PLAIN_ASSERT(column < column_count&& row < row_count); + return data()[column * row_count + row]; +} + +template +constexpr T const& mat3::element(size_t column, size_t row) const noexcept +{ + TL_PLAIN_ASSERT(column < column_count&& row < row_count); + return data()[column * row_count + row]; +} + +template +constexpr mat3 mat3::operator*(mat3 const& other) const noexcept +{ + mat3 ret(math::uninitialized); + return multiply(ret, *this, other); +} +template +constexpr mat3 mat3::operator+(mat3 const& other) const noexcept +{ + mat3 ret(math::uninitialized); + return cwise::add(ret, *this, other); +} +template +constexpr mat3 mat3::operator-(mat3 const& other) const noexcept +{ + mat3 ret(math::uninitialized); + return cwise::substract(ret, *this, other); +} +template +constexpr mat3& mat3::operator*=(mat3 const& other) noexcept +{ + mat3 a(*this); + return multiply(*this, a, other); +} +template +constexpr mat3& mat3::operator+=(mat3 const& other) noexcept +{ + return cwise::add(*this, *this, other); +} +template +constexpr mat3& mat3::operator-=(mat3 const& other) noexcept +{ + return cwise::substract(*this, *this, other); +} + +template +constexpr mat3 mat3::operator*(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat3 ret(math::uninitialized); + return cwise::multiply(ret, *this, scalar); +} +template +constexpr mat3 mat3::operator+(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat3 ret(math::uninitialized); + return cwise::add(ret, *this, scalar); +} +template +constexpr mat3 mat3::operator-(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat3 ret(math::uninitialized); + return cwise::substract(ret, *this, scalar); +} +template +constexpr mat3& mat3::operator*=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::multiply(*this, *this, scalar); +} +template +constexpr mat3& mat3::operator+=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::add(*this, *this, scalar); +} +template +constexpr mat3& mat3::operator-=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::substract(*this, *this, scalar); +} + + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::mat3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.column0); + tl::hash_and_combine(seed, p.column1); + tl::hash_and_combine(seed, p.column2); + return seed; + } +}; + + diff --git a/include/VMath/mat4.h b/include/VMath/mat4.h new file mode 100644 index 0000000..b9a07ce --- /dev/null +++ b/include/VMath/mat4.h @@ -0,0 +1,98 @@ +#pragma once + +#include + +namespace math +{ +template +struct vec4; + +//column-major (OpenGL) layout + +// .--------------- column0 - x axis +// | .----------- column1 - y axis +// | | .------- column2 - z axis +// | | | .--- column3 - w axis +// 0 4 8 12 +// 1 5 9 13 +// 2 6 10 14 +// 3 7 11 15 + +template +struct mat4 +{ + typedef T value_t; + using this_t = mat4; + + static constexpr size_t row_count = 4; + static constexpr size_t column_count = 4; + static constexpr size_t element_count = 16; + + constexpr mat4() noexcept; + constexpr explicit mat4(math::ZUninitialized) noexcept; + constexpr explicit mat4(T value) noexcept; + constexpr mat4(mat4 const&) noexcept = default; + + //column major - column0 is initialized from v0, v1, v2, v3 + constexpr mat4(T v0, T v1, T v2, T v3, + T v4, T v5, T v6, T v7, + T v8, T v9, T v10, T v11, + T v12, T v13, T v14, T v15) noexcept; + + constexpr mat4(vec4 const& column0, + vec4 const& column1, + vec4 const& column2, + vec4 const& column3) noexcept; + + constexpr mat4& operator=(mat4 const&) noexcept = default; + + constexpr mat4 operator*(mat4 const& other) const noexcept; + constexpr mat4 operator+(mat4 const& other) const noexcept; + constexpr mat4 operator-(mat4 const& other) const noexcept; + constexpr mat4 operator*(T scalar) const noexcept; + constexpr mat4 operator+(T scalar) const noexcept; + constexpr mat4 operator-(T scalar) const noexcept; + constexpr mat4& operator*=(mat4 const& other) noexcept; + constexpr mat4& operator+=(mat4 const& other) noexcept; + constexpr mat4& operator-=(mat4 const& other) noexcept; + constexpr mat4& operator*=(T scalar) noexcept; + constexpr mat4& operator+=(T scalar) noexcept; + constexpr mat4& operator-=(T scalar) noexcept; + + //Rows are NOT linearly in memory. First row is m[0], m[4], m[8], m[12] + constexpr vec4 get_row(size_t row) const noexcept; + constexpr void set_row(size_t row, vec4 const& v) noexcept; + + //Columns are linearly in memory. First row is m[0], m[1], m[2], m[3] and is the X axis of the matrix + constexpr vec4 const& get_column(size_t column) const noexcept; + constexpr void set_column(size_t column, vec4 const& v) noexcept; + + constexpr vec4 const& get_axis_x() const noexcept; + constexpr void set_axis_x(vec4 const& axis) noexcept; + + constexpr vec4 const& get_axis_y() const noexcept; + constexpr void set_axis_y(vec4 const& axis) noexcept; + + constexpr vec4 const& get_axis_z() const noexcept; + constexpr void set_axis_z(vec4 const& axis) noexcept; + + constexpr vec4 const& get_axis_w() const noexcept; + constexpr void set_axis_w(vec4 const& axis) noexcept; + + constexpr T determinant() const noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr T* data() noexcept; + constexpr T const* data() const noexcept; + + constexpr T& element(size_t column, size_t row) noexcept; + constexpr T const& element(size_t column, size_t row) const noexcept; + + vec4 column0; + vec4 column1; + vec4 column2; + vec4 column3; +}; +} diff --git a/include/VMath/mat4.inl b/include/VMath/mat4.inl new file mode 100644 index 0000000..7d9ac63 --- /dev/null +++ b/include/VMath/mat4.inl @@ -0,0 +1,293 @@ +namespace math +{ + +template +constexpr mat4::mat4() noexcept + : column0(T(1), T(0), T(0), T(0)) + , column1(T(0), T(1), T(0), T(0)) + , column2(T(0), T(0), T(1), T(0)) + , column3(T(0), T(0), T(0), T(1)) +{ +} + +template +constexpr mat4::mat4(math::ZUninitialized) noexcept +{ +} + +template +constexpr mat4::mat4(T value) noexcept + : column0(value, value, value, value) + , column1(value, value, value, value) + , column2(value, value, value, value) + , column3(value, value, value, value) +{ + TL_PLAIN_ASSERT(is_finite(value)); +} + +template +constexpr mat4::mat4(T v0, T v1, T v2, T v3, + T v4, T v5, T v6, T v7, + T v8, T v9, T v10, T v11, + T v12, T v13, T v14, T v15) noexcept + : column0(v0, v1, v2, v3) + , column1(v4, v5, v6, v7) + , column2(v8, v9, v10, v11) + , column3(v12, v13, v14, v15) +{ + TL_PLAIN_ASSERT(is_finite(column0) && + is_finite(column1) && + is_finite(column2) && + is_finite(column3)); +} + +template +constexpr mat4::mat4(vec4 const& column0, vec4 const& column1, vec4 const& column2, vec4 const& column3) noexcept + : column0(column0) + , column1(column1) + , column2(column2) + , column3(column3) +{ + TL_PLAIN_ASSERT(is_finite(column0) && + is_finite(column1) && + is_finite(column2) && + is_finite(column3)); +} + +/////////////////////////////////////////////////////////////////////////////// +//methods +/////////////////////////////////////////////////////////////////////////////// + +template +constexpr vec4 mat4::get_row(size_t row) const noexcept +{ + TL_PLAIN_ASSERT(row < 4); + const T* m = data(); + return vec4(m[row + 0], m[row + 4], m[row + 8], m[row + 12]); +} + +template +constexpr void mat4::set_row(size_t row, vec4 const& v) noexcept +{ + TL_PLAIN_ASSERT(is_finite(v)); + TL_PLAIN_ASSERT(row < 4); + T* m = data(); + m[row + 0] = v.x; + m[row + 4] = v.y; + m[row + 8] = v.z; + m[row + 12] = v.w; +} + +template +constexpr vec4 const& mat4::get_column(size_t column) const noexcept +{ + TL_PLAIN_ASSERT(column < column_count); + size_t idx = column * row_count; + const T* m = data(); + return *reinterpret_cast const*>(&m[idx]); +} + +template +constexpr void mat4::set_column(size_t column, vec4 const& v) noexcept +{ + TL_PLAIN_ASSERT(is_finite(v)); + TL_PLAIN_ASSERT(column < column_count); + size_t idx = column * row_count; + T* m = data(); + *reinterpret_cast*>(&m[idx]) = v; +} + +template +constexpr vec4 const& mat4::get_axis_x() const noexcept +{ + return get_column(0); +} + +template +constexpr vec4 const& mat4::get_axis_y() const noexcept +{ + return get_column(1); +} + +template +constexpr vec4 const& mat4::get_axis_z() const noexcept +{ + return get_column(2); +} + +template +constexpr vec4 const& mat4::get_axis_w() const noexcept +{ + return get_column(3); +} + +template +constexpr void mat4::set_axis_x(vec4 const& axis) noexcept +{ + set_column(0, axis); +} + +template +constexpr void mat4::set_axis_y(vec4 const& axis) noexcept +{ + set_column(1, axis); +} + +template +constexpr void mat4::set_axis_z(vec4 const& axis) noexcept +{ + set_column(2, axis); +} + +template +constexpr void mat4::set_axis_w(vec4 const& axis) noexcept +{ + set_column(3, axis); +} + +template +constexpr T mat4::determinant() const noexcept +{ + const T* m = data(); + T t0 = m[10] * m[15] - m[11] * m[14]; + T t1 = m[6] * m[15] - m[7] * m[14]; + T t2 = m[6] * m[11] - m[7] * m[10]; + T t3 = m[2] * m[15] - m[3] * m[14]; + T t4 = m[2] * m[11] - m[3] * m[10]; + T t5 = m[2] * m[7] - m[3] * m[6]; + T t6 = m[8] * m[13] - m[9] * m[12]; + T t7 = m[4] * m[13] - m[5] * m[12]; + T t8 = m[4] * m[9] - m[5] * m[8]; + T t9 = m[0] * m[13] - m[1] * m[12]; + T t10 = m[0] * m[9] - m[1] * m[8]; + T t11 = m[0] * m[5] - m[1] * m[4]; + + return t0 * t11 - t1 * t10 + t2 * t9 + t3 * t8 - t4 * t7 + t5 * t6; +} + +/////////////////////////////////////////////////////////////////////////////// +// indexing operators +/////////////////////////////////////////////////////////////////////////////// + +template +constexpr T* mat4::data() noexcept +{ + return (T*)&column0; +} +template +constexpr T const* mat4::data() const noexcept +{ + return (T const*)&column0; +} + +template +constexpr T& mat4::element(size_t column, size_t row) noexcept +{ + TL_PLAIN_ASSERT(column < column_count&& row < row_count); + T* m = data(); + return m[column * row_count + row]; +} + +template +constexpr T const& mat4::element(size_t column, size_t row) const noexcept +{ + TL_PLAIN_ASSERT(column < column_count&& row < row_count); + const T* m = data(); + return m[column * row_count + row]; +} + +template +constexpr mat4 mat4::operator*(mat4 const& other) const noexcept +{ + mat4 ret; + return multiply(ret, *this, other); +} +template +constexpr mat4 mat4::operator+(mat4 const& other) const noexcept +{ + mat4 ret; + return cwise::add(ret, *this, other); +} +template +constexpr mat4 mat4::operator-(mat4 const& other) const noexcept +{ + mat4 ret; + return cwise::substract(ret, *this, other); +} +template +constexpr mat4& mat4::operator*=(mat4 const& other) noexcept +{ + mat4 a(*this); + return multiply(*this, a, other); +} +template +constexpr mat4& mat4::operator+=(mat4 const& other) noexcept +{ + return cwise::add(*this, *this, other); +} +template +constexpr mat4& mat4::operator-=(mat4 const& other) noexcept +{ + return cwise::substract(*this, *this, other); +} + +template +constexpr mat4 mat4::operator*(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat4 ret; + return cwise::multiply(ret, *this, scalar); +} +template +constexpr mat4 mat4::operator+(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat4 ret; + return cwise::add(ret, *this, scalar); +} +template +constexpr mat4 mat4::operator-(T scalar) const noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + mat4 ret; + return cwise::substract(ret, *this, scalar); +} +template +constexpr mat4& mat4::operator*=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::multiply(*this, *this, scalar); +} +template +constexpr mat4& mat4::operator+=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::add(*this, *this, scalar); +} +template +constexpr mat4& mat4::operator-=(T scalar) noexcept +{ + TL_PLAIN_ASSERT(is_finite(scalar)); + return cwise::substract(*this, *this, scalar); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::mat4& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.column0); + tl::hash_and_combine(seed, p.column1); + tl::hash_and_combine(seed, p.column2); + tl::hash_and_combine(seed, p.column3); + return seed; + } +}; + + diff --git a/include/VMath/obb3.h b/include/VMath/obb3.h new file mode 100644 index 0000000..cc87e78 --- /dev/null +++ b/include/VMath/obb3.h @@ -0,0 +1,31 @@ +#pragma once + +namespace math +{ +/////////////////////////////////////////////////////////////////////////////// + +//! Oriented bounding box in R^3 space +template +struct obb3 +{ + using vec3_t = vec3; + + /// Fast initializer, members will contain garbage + explicit obb3(ZUninitialized); + + obb3(); + obb3(const vec3_t& i_center, const vec3_t& i_half_extent, const vec3_t(&i_axes)[3]); + + bool is_valid() const; + void get_vertices(vec3_t(&o_vertices)[8]) const; + + ////////////////////////////////////////////////////////////////////////// + // members + ////////////////////////////////////////////////////////////////////////// + + vec3_t center; + vec3_t half_extent; + vec3_t axes[3]; +}; + +}//namespace diff --git a/include/VMath/obb3.inl b/include/VMath/obb3.inl new file mode 100644 index 0000000..52be92a --- /dev/null +++ b/include/VMath/obb3.inl @@ -0,0 +1,61 @@ +namespace math +{ +/////////////////////////////////////////////////////////////////////////////// + +template inline +obb3::obb3(ZUninitialized tag) + : center(tag), half_extent(tag), axes({tag,tag,tag}) +{} + +template inline +obb3::obb3() +{ + center = vec3_t::zero(); + half_extent = vec3_t(1.f); + axes[0] = vec3_t(1.f); + axes[1] = vec3_t(1.f); + axes[2] = vec3_t(1.f); +} + +template inline +obb3::obb3(const vec3_t& center, const vec3_t& half_extent, const vec3_t(&axes)[3]) + : center(center) + , half_extent(half_extent) + , axes{axes[0], axes[1], axes[2]} +{ + JTL_ASSERT(is_valid()); +} + +template inline +bool obb3::is_valid() const +{ + return has_positive_sign(half_extent) + && is_normalized(axes[0]) + && is_normalized(axes[1]) + && is_normalized(axes[2]); +} + +template inline +void obb3::get_vertices(vec3_t(&o_vertices)[8]) const +{ + const int numAxes = 3; + std::array half_side; + for (unsigned int d = 0; d < numAxes; ++d) + { + half_side[d] = half_extent[d] * axes[d]; + } + + const int numVertices = 8; + for (int i = 0; i < numVertices; ++i) + { + o_vertices[i] = center; + for (unsigned int d = 0, mask = 1; d < numAxes; ++d, mask <<= 1) + { + T sign = (i & mask ? (T)1 : (T)-1); + o_vertices[i] += sign * half_side[d]; + } + } +} + +/////////////////////////////////////////////////////////////////////////////// +}//namespace diff --git a/include/VMath/plane.h b/include/VMath/plane.h new file mode 100644 index 0000000..6f37d31 --- /dev/null +++ b/include/VMath/plane.h @@ -0,0 +1,103 @@ +#pragma once + +namespace math +{ + +//! Plane class with explicit formulation ax + by + cz + d =0 +template +struct plane +{ + typedef T value_t; + typedef plane this_t; + + /////////////////////////////////////////////////////////////////////////////// + // constructors + /////////////////////////////////////////////////////////////////////////////// + /// Constructs a plane with normal (0,0,1) and crossing origin + plane(); + /// Warning - fast constructor, members will contain garbage + explicit plane(ZUninitialized); + plane(const vec3& point, const vec3& normal); + plane(const vec3& point1, const vec3& point2, const vec3& point3); + + /////////////////////////////////////////////////////////////////////////////// + // operators + /////////////////////////////////////////////////////////////////////////////// + bool operator==(const this_t& other) const; + bool operator!=(const this_t& other) const; + + /////////////////////////////////////////////////////////////////////////////// + // methods + /////////////////////////////////////////////////////////////////////////////// + + //! Gets a member point of the plane. + vec3 get_member_point() const; + + ///! Computes the inverse half space + this_t& invert(); + + //! Computes the signed distance to the point + T signed_distance(const vec3& point) const; + +#if JMATH_USE_DEPRECATED + //! Recalculates the distance from origin by applying a new member point to the plane. + void recalculate_d(const vec3& MPoint); + + //! Test if the triangle would be front or backfacing from any point. + /** Thus, this method assumes a camera position from + which the triangle is definitely visible when looking into + the given direction. + Note that this only works if the normal is Normalized. + Do not use this method with points as it will give wrong results! + \param lookDirection: Look direction. + \return True if the plane is front facing and + false if it is backfacing. */ + bool is_front_facing(const vec3& lookDirection) const; + + //! Get the distance to a point. + /** Note that this only works if the normal is normalized. */ + T get_distance_to(const vec3& point) const; + + //! Project a point on the plane. + vec3 project_point(const vec3& point) const; + + //! Get percentage of line between two points where an intersection with this plane happens. + /** Only useful if known that there is an intersection. + \param linePoint1 Point1 of the line to intersect with. + \param linePoint2 Point2 of the line to intersect with. + \return Where on a line between two points an intersection with this plane happened. + For example, 0.5 is returned if the intersection happened exactly in the middle of the two points. + */ + float get_known_intersection_with_line(const vec3& linePoint1,const vec3& linePoint2) const; + + //! Enumeration for intersection relations of 3d objects + enum class RelationEnum + { + FRONT, + BACK, + ON_PLANE + }; + + //! Classifies the relation of a point to this plane. + /** \param point Point to classify its relation. + \return FRONT if the point is in front of the plane, + BACK if the point is behind of the plane, and + ON_PLANE if the point is within the plane. */ + RelationEnum classify_point_relation(const vec3& point) const; + + ///! Checks for plane AABB overlap + // Return values: + // 0: box is totally outside of the plane + // 1: box is totally inside of the plane + // 2: box is partially inside of the plane + RelationEnum overlap(const vec3& boxCenter, vec3& boxHalfsize) const; +#endif//JMATH_USE_DEPRECATED + + ///Plane equation Ax + By + Cz + D = 0 + //! Normal of the plane. i.e., (A,B,C) + vec3 normal; + //! Distance from origin negated. i.e., D = -(A*x0 + B*y0 + C*z0) + T offset; +}; + +} diff --git a/include/VMath/plane.inl b/include/VMath/plane.inl new file mode 100644 index 0000000..4c2157c --- /dev/null +++ b/include/VMath/plane.inl @@ -0,0 +1,66 @@ +namespace math +{ + +template +inline plane::plane() + : normal(0, 0, 1) + , offset(0) +{ +} + +template +inline plane::plane(ZUninitialized tag) : normal(tag) +{ +} + +template +inline plane::plane(const vec3& ipoint, const vec3& inormal) + : normal(inormal) + , offset(-dot(ipoint, normal)) +{ + TL_PLAIN_ASSERT(is_normalized(inormal)); +} + +template +inline plane::plane(const vec3& point1, const vec3& point2, const vec3& point3) +{ + normal = normalized(cross(point2 - point1, point3 - point1)); + TL_PLAIN_ASSERT(is_normalized(normal)); + offset = -dot(point1, normal); +} + +template +inline bool plane::operator==(const plane& other) const +{ + return offset == other.offset && normal == other.normal; +} + +template +inline bool plane::operator!=(const plane& other) const +{ + return !(*this == other); +} + +template +inline vec3 plane::get_member_point() const +{ + return normal * -offset; +} + +template +plane& plane::invert() +{ + this_t tmp = *this; + tmp.normal = -normal; + tmp.offset = -dot(tmp.normal, get_member_point()); + *this = tmp; + return *this; +} + +template +T plane::signed_distance(const vec3& point) const +{ + return dot(normal, point) + offset; +} + +} diff --git a/include/VMath/quat.h b/include/VMath/quat.h new file mode 100644 index 0000000..8a5d29d --- /dev/null +++ b/include/VMath/quat.h @@ -0,0 +1,102 @@ +#pragma once + +#include + +namespace math +{ + +template +struct quat +{ + typedef T value_t; + typedef quat this_t; + + static constexpr size_t element_count = 4; + + template + constexpr static quat from_axis_x(T rotation) noexcept; + template + constexpr static quat from_axis_y(T rotation) noexcept; + template + constexpr static quat from_axis_z(T rotation) noexcept; + template + constexpr static quat from_angle_axis(T rotation, vec3 const& axis) noexcept; + template + constexpr static quat from_euler_xyz(vec3 const& euler) noexcept; + template + constexpr static quat from_euler_xzy(vec3 const& euler) noexcept; + template + constexpr static quat from_euler_yxz(vec3 const& euler) noexcept; + template + constexpr static quat from_euler_yzx(vec3 const& euler) noexcept; + template + constexpr static quat from_euler_zxy(vec3 const& euler) noexcept; + template + constexpr static quat from_euler_zyx(vec3 const& euler) noexcept; + template + constexpr static quat from_a_to_b(vec3 const& a, vec3 const& b) noexcept; + template + constexpr static quat from_a_to_b(quat const& a, quat const& b) noexcept; + + constexpr quat() noexcept = default; + constexpr explicit quat(T v) noexcept; + constexpr quat(T x, T y, T z, T w) noexcept; + constexpr quat(quat const&) noexcept = default; + constexpr quat& operator=(quat const& other) noexcept = default; + + constexpr T& operator[](size_t i) noexcept; + constexpr T const& operator[](size_t i) const noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr quat operator+(quat const& other) const noexcept; + constexpr quat& operator+=(quat const& other) noexcept; + + constexpr quat operator-(quat const& other) const noexcept; + constexpr quat& operator-=(quat const& other) noexcept; + + constexpr quat operator*(quat const& other) const noexcept; + constexpr quat& operator*=(quat const& other) noexcept; + + constexpr this_t operator*(T s) const noexcept; + constexpr quat& operator*=(T s) noexcept; + + //! Negation (additive inverse) (-x,-y,-z,-w). + // A + (-A) == A - A == identity + constexpr quat operator-() const noexcept; + + //! Conjugation (spatial inverse) (-x,-y,-z,+w). + // A * (~A) == identity + constexpr quat operator~() const noexcept; + + template + constexpr vec3 get_euler_xyz() const noexcept; + template + constexpr vec3 get_euler_xzy() const noexcept; + template + constexpr vec3 get_euler_yxz() const noexcept; + template + constexpr vec3 get_euler_yzx() const noexcept; + template + constexpr vec3 get_euler_zxy() const noexcept; + template + constexpr vec3 get_euler_zyx() const noexcept; + + template + constexpr tl::pair> get_angle_axis() const noexcept; + + template + constexpr quat& normalize() noexcept; + template + constexpr T length() const noexcept; + constexpr T length_sq() const noexcept; + + + T x = T(0); + T y = T(0); + T z = T(0); + T w = T(1); +}; + +} diff --git a/include/VMath/quat.inl b/include/VMath/quat.inl new file mode 100644 index 0000000..141a2ce --- /dev/null +++ b/include/VMath/quat.inl @@ -0,0 +1,529 @@ +namespace math +{ + +template +template +constexpr quat quat::from_axis_x(T rotation) noexcept +{ + T a, b; + math::sin_cos(rotation * T(0.5), a, b); + return { a, 0, 0, b }; +} +template +template +constexpr quat quat::from_axis_y(T rotation) noexcept +{ + T a, b; + math::sin_cos(rotation * T(0.5), a, b); + return { 0, a, 0, b }; +} +template +template +constexpr quat quat::from_axis_z(T rotation) noexcept +{ + T a, b; + math::sin_cos(rotation * T(0.5), a, b); + return { 0, 0, a, b }; +} +template +template +constexpr quat quat::from_angle_axis(T rotation, vec3 const& axis) noexcept +{ + TL_PLAIN_ASSERT(is_equal(axis.length(), T(1), T(0.001)) || is_zero(rotation)); + T s, w; + sin_cos(rotation * T(0.5), s, w); + return { s * axis.x, s * axis.y, s * axis.z, w }; +} +template +template +constexpr quat quat::from_euler_xyz(vec3 const& euler) noexcept +{ + vec3 a = euler * T(0.5); + vec3 s, c; + sin_cos(a, s, c); + return + { + c.z * s.x * c.y + s.z * c.x * s.y, + c.z * c.x * s.y - s.z * s.x * c.y, + s.z * c.x * c.y + c.z * s.x * s.y, + c.z * c.x * c.y - s.z * s.x * s.y + }; +} +template +template +constexpr quat quat::from_euler_xzy(vec3 const& euler) noexcept +{ + vec3 a = euler * T(0.5); + vec3 s, c; + sin_cos(a, s, c); + return + { + c.z * s.x * c.y - s.z * c.x * s.y, + c.z * c.x * s.y - s.z * s.x * c.y, + s.z * c.x * c.y + c.z * s.x * s.y, + c.z * c.x * c.y + s.z * s.x * s.y, + }; +} +template +template +constexpr quat quat::from_euler_yxz(vec3 const& euler) noexcept +{ + vec3 a = euler * T(0.5); + vec3 s, c; + sin_cos(a, s, c); + return + { + c.z * s.x * c.y + s.z * c.x * s.y, + c.z * c.x * s.y - s.z * s.x * c.y, + s.z * c.x * c.y - c.z * s.x * s.y, + c.z * c.x * c.y + s.z * s.x * s.y, + }; +} +template +template +constexpr quat quat::from_euler_yzx(vec3 const& euler) noexcept +{ + vec3 a = euler * T(0.5); + vec3 s, c; + sin_cos(a, s, c); + return + { + c.z * s.x * c.y + s.z * c.x * s.y, + c.z * c.x * s.y + s.z * s.x * c.y, + s.z * c.x * c.y - c.z * s.x * s.y, + c.z * c.x * c.y - s.z * s.x * s.y, + }; +} +template +template +constexpr quat quat::from_euler_zxy(vec3 const& euler) noexcept +{ + vec3 a = euler * T(0.5); + vec3 s, c; + sin_cos(a, s, c); + return + { + c.z * s.x * c.y - s.z * c.x * s.y, + c.z * c.x * s.y + s.z * s.x * c.y, + s.z * c.x * c.y + c.z * s.x * s.y, + c.z * c.x * c.y - s.z * s.x * s.y, + }; +} +template +template +constexpr quat quat::from_euler_zyx(vec3 const& euler) noexcept +{ + vec3 a = euler * T(0.5); + vec3 s, c; + sin_cos(a, s, c); + return + { + c.z * s.x * c.y - s.z * c.x * s.y, + c.z * c.x * s.y + s.z * s.x * c.y, + s.z * c.x * c.y - c.z * s.x * s.y, + c.z * c.x * c.y + s.z * s.x * s.y, + }; +} +template +template +constexpr quat quat::from_a_to_b(vec3 const& a, vec3 const& b) noexcept +{ + //see: http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors + TL_PLAIN_ASSERT(is_one(a.length())); + TL_PLAIN_ASSERT(is_one(b.length())); + const T cos_theta = dot(a, b); + const vec3 axis = cross(a, b); + if (dot(cos_theta) >= T(1) - tl::numeric_limits::epsilon() * T(4)) + { + //handle parallel vectors - cos ~ 1 + const T value = cos_theta > (T)0 ? (T)0 : (T)1; + vec3 axis; + axis[cwise::min_component_index(abs(a))] = value; // coordinate axis most nearly orthogonal to "a" + return { axis.x, axis.y, axis.z, (T)1 - value }; + } + else + { + //common case - non parallel - cos < 1 + TL_PLAIN_ASSERT(math::length_sq(axis) > square((T)0.0002)); + return normalized(quat(axis.x, axis.y, axis.z, (T)1 + cos_theta)); + } +} +template +template +constexpr quat quat::from_a_to_b(quat const& a, quat const& b) noexcept +{ + quat inv_a; + math::inverse(a, inv_a); //this should never fail, quats are always invertible + return inv_a * b; +} + +template +constexpr quat::quat(T v) noexcept : x(v), y(v), z(v), w(v) {} + +template +constexpr quat::quat(T _x, T _y, T _z, T _w) noexcept : x(_x), y(_y), z(_z), w(_w) {} + +template +constexpr T& quat::operator[](size_t i) noexcept +{ + TL_PLAIN_ASSERT(i < element_count); + TL_PLAIN_ASSERT(&((T*)this)[0] == &x && &((T*)this)[1] == &y); + return ((T*)this)[i]; +} + +template +constexpr T const& quat::operator[](size_t i) const noexcept +{ + TL_PLAIN_ASSERT(i < element_count); + TL_PLAIN_ASSERT(&((T*)this)[0] == &x && &((T*)this)[1] == &y); + return ((T*)this)[i]; +} + +template +constexpr quat quat::operator+(quat const& b) const noexcept +{ + return { x + b.x, y + b.y, z + b.z, w + b.w }; +} +template +constexpr quat& quat::operator+=(quat const& b) noexcept +{ + x += b.x; + y += b.y; + z += b.z; + w += b.w; + return *this; +} + +template +constexpr quat quat::operator-(quat const& b) const noexcept +{ + return { x - b.x, y - b.y, z - b.z, w - b.w }; +} +template +constexpr quat& quat::operator-=(quat const& b) noexcept +{ + x -= b.x; + y -= b.y; + z -= b.z; + w -= b.w; + return *this; +} + +template +constexpr quat quat::operator*(quat const& other) const noexcept +{ + return + { + (other.x * w) + (other.w * x) + (other.z * y) - (other.y * z), + (other.y * w) + (other.w * y) + (other.x * z) - (other.z * x), + (other.z * w) + (other.w * z) + (other.y * x) - (other.x * y), + (other.w * w) - (other.x * x) - (other.y * y) - (other.z * z), + }; +} +template +constexpr quat& quat::operator*=(quat const& other) noexcept +{ + return (*this = (*this) * other); +} + +template +constexpr quat quat::operator*(T s) const noexcept +{ + return { s * x, s * y, s * z, s * w }; +} + +template +constexpr quat& quat::operator*=(T s) noexcept +{ + x *= s; + y *= s; + z *= s; + w *= s; + return *this; +} + +template +constexpr quat quat::operator-() const noexcept +{ + return { -x, -y, -z, -w }; +} + +template +constexpr quat quat::operator~() const noexcept +{ + return { -x, -y, -z, w }; +} + +template +template +constexpr vec3 quat::get_euler_xyz() const noexcept +{ + T const s = T(2) * (z * x + w * y); + if (s < T(0.9999)) + { + if (s > -T(0.9999)) + { + return + { + atan2(-T(2) * (z * y - w * x), T(1) - T(2) * (x * x + y * y)), + asin(s), + atan2(-T(2) * (x * y - w * z), T(1) - T(2) * (z * z + y * y)), + }; + } + else + { + return + { + -atan2(T(2) * (x * y + w * z), T(1) - T(2) * (z * z + x * x)), + -angle::pi2, + 0, + }; + } + } + else + { + return + { + atan2(T(2) * (x * y + w * z), T(1) - T(2) * (z * z + x * x)), + angle::pi2, + 0 + }; + } +} +template +template +constexpr vec3 quat::get_euler_xzy() const noexcept +{ + T const s = -T(2) * (x * y - w * z); + if (s < T(0.9999)) + { + if (s > -T(0.9999)) + { + return + { + atan2(T(2) * (z * y + w * x), T(1) - T(2) * (z * z + x * x)), + atan2(T(2) * (z * x + w * y), T(1) - T(2) * (z * z + y * y)), + asin(s), + }; + } + else + { + return + { + 0, + -atan2(T(2) * (z * x - w * y), T(1) - T(2) * (x * x + y * y)), + -angle::pi2, + }; + } + } + else + { + return + { + 0, + -atan2(T(2) * (z * x - w * y), T(1) - T(2) * (x * x + y * y)), + angle::pi2, + }; + } +} +template +template +constexpr vec3 quat::get_euler_yxz() const noexcept +{ + T const s = -T(2) * (z * y - w * x); + if (s < T(0.9999)) + { + if (s > -T(0.9999)) + { + return + { + asin(s), + atan2(T(2) * (z * x + w * y), T(1) - T(2) * (x * x + y * y)), + atan2(T(2) * (x * y + w * z), T(1) - T(2) * (z * z + x * x)), + }; + } + else + { + return + { + -angle::pi2, + 0, + atan2(-T(2) * (y * x - w * z), T(1) - T(2) * (y * y + z * z)), + }; + } + } + else + { + return + { + angle::pi2, + 0, + atan2(-T(2) * (y * x - w * z), T(1) - T(2) * (y * y + z * z)), + }; + } +} +template +template +constexpr vec3 quat::get_euler_yzx() const noexcept +{ + T const s = T(2) * (x * y + w * z); + if (s < T(0.9999)) + { + if (s > -T(0.9999)) + { + return + { + atan2(-T(2) * (z * y - w * x), T(1) - T(2) * (z * z + x * x)), + atan2(-T(2) * (z * x - w * y), T(1) - T(2) * (z * z + y * y)), + asin(s), + }; + } + else + { + return + { + 0, + -atan2(T(2) * (z * y + w * x), T(1) - T(2) * (x * x + y * y)), + -angle::pi2, + }; + } + } + else + { + return + { + 0, + atan2(T(2) * (z * y + w * x), T(1) - T(2) * (x * x + y * y)), + angle::pi2, + }; + } +} +template +template +constexpr vec3 quat::get_euler_zxy() const noexcept +{ + T const s = T(2) * (z * y + w * x); + if (s < T(0.9999)) + { + if (s > -T(0.9999)) + { + return + { + asin(s), + atan2(-T(2) * (z * x - w * y), T(1) - T(2) * (x * x + y * y)), + atan2(-T(2) * (x * y - w * z), T(1) - T(2) * (z * z + x * x)), + }; + } + else + { + return + { + -angle::pi2, + 0, + -atan2(T(2) * (z * x + w * y), T(1) - T(2) * (z * z + y * y)), + }; + } + } + else + { + return + { + angle::pi2, + 0, + atan2(T(2) * (z * x + w * y), T(1) - T(2) * (z * z + y * y)), + }; + } +} +template +template +constexpr vec3 quat::get_euler_zyx() const noexcept +{ + T const s = -T(2) * (z * x - w * y); + if (s < T(0.9999)) + { + if (s > -T(0.9999)) + { + return + { + atan2(T(2) * (z * y + w * x), T(1) - T(2) * (x * x + y * y)), + asin(s), + atan2(T(2) * (x * y + w * z), T(1) - T(2) * (z * z + y * y)), + }; + } + else + { + return + { + 0, + -angle::pi2, + atan2(-T(2) * (z * y - w * x), T(1) - T(2) * (z * z + x * x)), + }; + } + } + else + { + return + { + 0, + angle::pi2, + -atan2(-T(2) * (z * y - w * x), T(1) - T(2) * (z * z + x * x)), + }; + } +} + +template +template +constexpr tl::pair> quat::get_angle_axis() const noexcept +{ + T const w = clamp(w, T(-1), T(1)); + T const scale = sqrt(T(1) - w * w); + T angle = acos(w) * T(2); + if (scale < tl::numeric_limits::epsilon()) + return tl::make_pair(angle, vec3(x, y, z)); + else + return tl::make_pair(angle, vec3(x, y, z) / scale); +} + +template +template +constexpr quat& quat::normalize() noexcept +{ + const T len_sq = x * x + y * y + z * z + w * w; + const T len_inv = inv_sqrt(len_sq); + x *= len_inv; + y *= len_inv; + z *= len_inv; + w *= len_inv; + return *this; +} +template +template +constexpr T quat::length() const noexcept +{ + return sqrt(x * x + y * y + z * z + w * w); +} +template +constexpr T quat::length_sq() const noexcept +{ + return x * x + y * y + z * z + w * w; +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::quat& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.x); + tl::hash_and_combine(seed, p.y); + tl::hash_and_combine(seed, p.z); + tl::hash_and_combine(seed, p.w); + return seed; + } +}; + diff --git a/include/VMath/range.h b/include/VMath/range.h new file mode 100644 index 0000000..98f4c02 --- /dev/null +++ b/include/VMath/range.h @@ -0,0 +1,33 @@ +#pragma once + +#include +#include + +namespace math +{ + +template +struct range +{ + typedef T value_t; + typedef range this_t; + + constexpr range() noexcept = default; + constexpr range(const T& min, const T& max) noexcept; + constexpr range(const range&) noexcept = default; + + auto operator<=>(const this_t&) const noexcept = default; + bool operator==(const this_t&) const noexcept = default; + + constexpr range& operator=(const range&) noexcept = default; + + constexpr bool is_valid() const noexcept; + constexpr T get_center() const noexcept; + constexpr T get_extent() const noexcept; + constexpr T get_half_extent() const noexcept; + + T min = T(tl::numeric_limits::lowest()); + T max = T(tl::numeric_limits::max()); +}; + +} diff --git a/include/VMath/range.inl b/include/VMath/range.inl new file mode 100644 index 0000000..9aa3b7f --- /dev/null +++ b/include/VMath/range.inl @@ -0,0 +1,58 @@ +namespace math +{ + +template +constexpr range::range(const T& min, const T& max) noexcept + : min(min) + , max(max) +{ + TL_PLAIN_ASSERT(is_gequal(max, min)); +} + +template +constexpr bool range::is_valid() const noexcept +{ + return is_gequal(max, min); +} + +template +constexpr T range::get_center() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + if constexpr (std::is_floating_point_v) + return (min + max) * T(0.5); + else + return (min + max) / T(2); +} +template +constexpr T range::get_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return max - min; +} +template +constexpr T range::get_half_extent() const noexcept +{ + if constexpr (std::is_floating_point_v) + return get_extent() * T(0.5); + else + return get_extent() / T(2); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::range& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.min); + tl::hash_and_combine(seed, p.max); + return seed; + } +}; + diff --git a/include/VMath/ray2.h b/include/VMath/ray2.h new file mode 100644 index 0000000..a16144b --- /dev/null +++ b/include/VMath/ray2.h @@ -0,0 +1,34 @@ +#pragma once + +#include + +namespace math +{ + +template +struct ray2 +{ + typedef T value_t; + typedef ray2 this_t; + + constexpr ray2() noexcept = default; + constexpr ray2(const vec2&start, const vec2&direction) noexcept; + constexpr ray2(const ray2&) noexcept = default; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr ray2& operator=(const ray2&) noexcept = default; + + constexpr bool is_valid() const noexcept; + + //distance has to be positive + constexpr vec2 get_point(T distance) const noexcept; + constexpr void translate(const vec2&offset) noexcept; + + + vec2 origin; + vec2 direction; +}; + +} diff --git a/include/VMath/ray2.inl b/include/VMath/ray2.inl new file mode 100644 index 0000000..55a15c2 --- /dev/null +++ b/include/VMath/ray2.inl @@ -0,0 +1,48 @@ +namespace math +{ + +template +constexpr ray2::ray2(const vec2& start, const vec2& direction) noexcept + : origin(start) + , direction(direction) +{ +} + +template +constexpr bool ray2::is_valid() const noexcept +{ + return !is_zero(direction); +} +template +constexpr vec2 ray2::get_point(T distance) const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + TL_PLAIN_ASSERT(is_positive(distance)); + return origin + direction * distance; +} +template +constexpr void ray2::translate(const vec2& offset) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + origin += offset; +} + +} + + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::ray2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.origin); + tl::hash_and_combine(seed, p.direction); + return seed; + } +}; + + diff --git a/include/VMath/ray3.h b/include/VMath/ray3.h new file mode 100644 index 0000000..fb57ba9 --- /dev/null +++ b/include/VMath/ray3.h @@ -0,0 +1,32 @@ +#pragma once + +#include + +namespace math +{ + +template +struct ray3 +{ + typedef T value_t; + typedef ray3 this_t; + + constexpr ray3() noexcept = default; + constexpr ray3(const vec3& start, const vec3& direction) noexcept; + constexpr ray3(const ray3&) noexcept = default; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + constexpr ray3& operator=(const ray3&) noexcept = default; + + constexpr bool is_valid() const noexcept; + constexpr vec3 get_point(T distance) const noexcept; + constexpr void translate(const vec3& offset) noexcept; + + + vec3 origin; + vec3 direction; +}; + +} diff --git a/include/VMath/ray3.inl b/include/VMath/ray3.inl new file mode 100644 index 0000000..eb35794 --- /dev/null +++ b/include/VMath/ray3.inl @@ -0,0 +1,46 @@ +namespace math +{ + +template +constexpr ray3::ray3(const vec3& start, const vec3& direction) noexcept + : origin(start) + , direction(direction) +{ +} + +template +constexpr bool ray3::is_valid() const noexcept +{ + return !is_zero(direction); +} +template +constexpr vec3 ray3::get_point(T distance) const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return origin + direction * distance; +} +template +constexpr void ray3::translate(const vec3& offset) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + origin += offset; +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::ray3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.origin); + tl::hash_and_combine(seed, p.direction); + return seed; + } +}; + + diff --git a/include/VMath/rect.h b/include/VMath/rect.h new file mode 100644 index 0000000..cd0e55b --- /dev/null +++ b/include/VMath/rect.h @@ -0,0 +1,67 @@ +#pragma once + +#include +#include + +namespace math +{ + +/* +* Rectangle class to be used in 2D where Y goes down. +* Due to this (Y goes down), the bottom_right corner has a higher Y than the top Y for a valid rect +*/ +template +struct rect +{ + typedef T value_t; + typedef rect this_t; + + constexpr rect() noexcept = default; + constexpr rect(const vec2& top_left, const vec2& bottom_right) noexcept; + constexpr rect(T x1, T y1, T x2, T y2) noexcept; + constexpr rect(const rect&) noexcept = default; + constexpr rect& operator=(const rect&) noexcept = default; + + constexpr bool is_valid() const noexcept; + constexpr bool is_empty() const noexcept; + + constexpr T get_area() const noexcept; + constexpr T get_width() const noexcept; + constexpr T get_height() const noexcept; + constexpr vec2 get_center() const noexcept; + constexpr vec2 get_top_right_corner() const noexcept; + constexpr vec2 get_bottom_left_corner() const noexcept; + constexpr vec2 get_extent() const noexcept; + constexpr vec2 get_half_extent() const noexcept; + + constexpr void translate(const vec2& offset) noexcept; + + //size must be positive + constexpr void grow(const T& size) noexcept; + constexpr void grow(const vec2& size) noexcept; + + //size must be positive + constexpr void shrink(const T& size) noexcept; + constexpr void shrink(const vec2& size) noexcept; + + //size can be positive or negative to grow or shrink + //Only useful for integral / float types. Unsigned types will fail with this + constexpr void resize(const T& size) noexcept; + constexpr void resize(const vec2& size) noexcept; + + //only valid for floating point types + constexpr void scale(T scale) noexcept; + constexpr void scale(const vec2& size) noexcept; + + constexpr void add(const vec2& p) noexcept; + constexpr void add(const rect& other) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + vec2 top_left = vec2(tl::numeric_limits::max()); + vec2 bottom_right = vec2(tl::numeric_limits::lowest()); +}; + +} diff --git a/include/VMath/rect.inl b/include/VMath/rect.inl new file mode 100644 index 0000000..baf3876 --- /dev/null +++ b/include/VMath/rect.inl @@ -0,0 +1,243 @@ +namespace math +{ + +template +constexpr rect::rect(const vec2& top_left, const vec2& bottom_right) noexcept + : top_left(top_left) + , bottom_right(bottom_right) +{ + TL_PLAIN_ASSERT(is_lequal(top_left, bottom_right)); +} +template +constexpr rect::rect(T x1, T y1, T x2, T y2) noexcept + : top_left(x1, y1) + , bottom_right(x2, y2) +{ + TL_PLAIN_ASSERT(is_lequal(top_left, bottom_right)); +} + +template +constexpr bool rect::is_valid() const noexcept +{ + return is_gequal(bottom_right, top_left); +} +template +constexpr bool rect::is_empty() const noexcept +{ + return is_equal(top_left, bottom_right); +} + +template +constexpr T rect::get_area() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + vec2 ex = get_extent(); + return ex.x * ex.y; +} +template +constexpr T rect::get_width() const noexcept +{ + return get_extent().x; +} +template +constexpr T rect::get_height() const noexcept +{ + return get_extent().y; +} +template +constexpr vec2 rect::get_center() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + if constexpr (std::is_floating_point_v) + return (top_left + bottom_right) * T(0.5); + else + return (top_left + bottom_right) / T(2); +} +template +constexpr vec2 rect::get_top_right_corner() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return { bottom_right.x, top_left.y }; +} +template +constexpr vec2 rect::get_bottom_left_corner() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return { top_left.x, bottom_right.y }; +} +template +constexpr vec2 rect::get_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + return bottom_right - top_left; +} +template +constexpr vec2 rect::get_half_extent() const noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + if constexpr (std::is_floating_point_v) + return (bottom_right - top_left) * T(0.5); + else + return (bottom_right - top_left) / T(2); +} + +template +constexpr void rect::translate(const vec2& offset) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + top_left += offset; + bottom_right += offset; +} +template +constexpr void rect::grow(const T& size) noexcept +{ + TL_PLAIN_ASSERT(is_positive(size)); + TL_PLAIN_ASSERT(is_valid()); + vec2 s1; + if constexpr (std::is_floating_point_v) + s1 = size * T(0.5); + else + s1 = size / T(2); + + vec2 s2 = size - s1; + top_left -= s1; + bottom_right += s2; +} +template +constexpr void rect::grow(const vec2& size) noexcept +{ + TL_PLAIN_ASSERT(is_positive(size)); + TL_PLAIN_ASSERT(is_valid()); + vec2 s1; + if constexpr (std::is_floating_point_v) + s1 = size * T(0.5); + else + s1 = size / T(2); + + vec2 s2 = size - s1; + top_left -= s1; + bottom_right += s2; +} +template +constexpr void rect::shrink(const T& size) noexcept +{ + TL_PLAIN_ASSERT(is_positive(size)); + TL_PLAIN_ASSERT(is_valid()); + vec2 s1; + if constexpr (std::is_floating_point_v) + s1 = size * T(0.5); + else + s1 = size / T(2); + + vec2 s2 = size - s1; + top_left += s1; + bottom_right -= s2; +} +template +constexpr void rect::shrink(const vec2& size) noexcept +{ + TL_PLAIN_ASSERT(is_positive(size)); + vec2 s1; + if constexpr (std::is_floating_point_v) + s1 = size * T(0.5); + else + s1 = size / T(2); + + vec2 s2 = size - s1; + top_left += s1; + bottom_right -= s2; +} +template +constexpr void rect::resize(const T& size) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + vec2 s1; + if constexpr (std::is_floating_point_v) + s1 = size * T(0.5); + else + s1 = size / T(2); + + vec2 s2 = size - s1; + top_left -= s1; + bottom_right += s2; +} +template +constexpr void rect::resize(const vec2& size) noexcept +{ + TL_PLAIN_ASSERT(is_valid()); + vec2 s1; + if constexpr (std::is_floating_point_v) + s1 = size * T(0.5); + else + s1 = size / T(2); + + vec2 s2 = size - s1; + top_left -= s1; + bottom_right += s2; +} + +template +constexpr void rect::scale(T scale) noexcept +{ + TL_PLAIN_ASSERT(is_positive(scale)); + TL_PLAIN_ASSERT(is_valid()); + const vec2 center = get_center(); + T half_scale; + if constexpr (std::is_floating_point_v) + half_scale = scale * T(0.5); + else + half_scale = scale / T(2); + + vec2 ex = get_extent(); + vec2 half_ex = ex * half_scale; + top_left = center - half_ex; + bottom_right = center + half_ex; +} +template +constexpr void rect::scale(const vec2& size) noexcept +{ + TL_PLAIN_ASSERT(is_positive(size)); + TL_PLAIN_ASSERT(is_valid()); + const vec2 center = get_center(); + T half_scale; + if constexpr (std::is_floating_point_v) + half_scale = size * T(0.5); + else + half_scale = size / T(2); + + vec2 ex = get_extent(); + vec2 half_ex = ex * half_scale; + top_left = center - half_ex; + bottom_right = center + half_ex; +} + +template +constexpr void rect::add(const vec2& p) noexcept +{ + top_left = math::min(top_left, p); + bottom_right = math::max(bottom_right, p); +} +template +constexpr void rect::add(const rect& other) noexcept +{ + add(other.top_left); + add(other.bottom_right); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::rect& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.top_left); + tl::hash_and_combine(seed, p.bottom_right); + return seed; + } +}; + diff --git a/include/VMath/rigid2.h b/include/VMath/rigid2.h new file mode 100644 index 0000000..94a2d4b --- /dev/null +++ b/include/VMath/rigid2.h @@ -0,0 +1,35 @@ +#pragma once + +#include + +namespace math +{ + +template struct vec2; + +template +struct rigid2 +{ + typedef T value_t; + typedef rigid2 this_t; + + constexpr rigid2() noexcept = default; + constexpr rigid2(rigid2 const& other) noexcept = default; + constexpr explicit rigid2(vec2 const& translation) noexcept; + constexpr rigid2(vec2 const& translation, T rotation) noexcept; + + constexpr rigid2 operator*(rigid2 const& other) const noexcept; + constexpr rigid2& operator*=(rigid2 const& other) noexcept; + + constexpr rigid2& post_translate(vec2 const& trans) noexcept; + constexpr rigid2& post_rotate(T rotation) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + vec2 translation; + T rotation = T(0); /// rotation angle in radians +}; + +} diff --git a/include/VMath/rigid2.inl b/include/VMath/rigid2.inl new file mode 100644 index 0000000..5366ad4 --- /dev/null +++ b/include/VMath/rigid2.inl @@ -0,0 +1,63 @@ +namespace math +{ + +template +constexpr rigid2::rigid2(vec2 const& translation) noexcept + : translation(translation) +{ +} + +template +constexpr rigid2::rigid2(vec2 const& translation, T rotation) noexcept + : translation(translation) + , rotation(rotation) +{ +} + +template +constexpr rigid2& rigid2::post_translate(vec2 const& trans) noexcept +{ + translation += rotate(rotation, trans); + return *this; +} + +template +constexpr rigid2& rigid2::post_rotate(T rotation) noexcept +{ + rotation += rotation; + return *this; +} + +template +constexpr rigid2 rigid2::operator*(rigid2 const& other) const noexcept +{ + rigid2 ret; + multiply(ret, *this, other); + return ret; +} + +template +constexpr rigid2& rigid2::operator*=(rigid2 const& other) noexcept +{ + rigid2 a(*this); + return multiply(*this, a, other); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::rigid2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.translation); + tl::hash_and_combine(seed, p.rotation); + return seed; + } +}; + + diff --git a/include/VMath/rigid3.h b/include/VMath/rigid3.h new file mode 100644 index 0000000..78fd708 --- /dev/null +++ b/include/VMath/rigid3.h @@ -0,0 +1,36 @@ +#pragma once + +#include + +namespace math +{ + +template struct vec3; +template struct quat; + +template +struct rigid3 +{ + typedef T value_t; + typedef rigid3 this_t; + + constexpr rigid3() noexcept = default; + constexpr rigid3(rigid3 const& other) noexcept = default; + constexpr explicit rigid3(vec3 const& translation) noexcept; + constexpr rigid3(vec3 const& translation, quat const& rotation) noexcept; + + constexpr rigid3 operator*(rigid3 const& other) const noexcept; + constexpr rigid3& operator*=(rigid3 const& other) noexcept; + + constexpr rigid3& post_translate(vec3 const& trans) noexcept; + constexpr rigid3& post_rotate(quat const& rotation) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + vec3 translation; + quat rotation; +}; + +} diff --git a/include/VMath/rigid3.inl b/include/VMath/rigid3.inl new file mode 100644 index 0000000..c29e39e --- /dev/null +++ b/include/VMath/rigid3.inl @@ -0,0 +1,62 @@ +namespace math +{ + +template +constexpr rigid3::rigid3(vec3 const& translation) noexcept + : translation(translation) +{ +} + +template +constexpr rigid3::rigid3(vec3 const& translation, quat const& rotation) noexcept + : translation(translation) + , rotation(rotation) +{ +} + +template +constexpr rigid3& rigid3::post_translate(vec3 const& trans) noexcept +{ + translation += rotate(rotation, trans); + return *this; +} + +template +constexpr rigid3& rigid3::post_rotate(quat const& rotation) noexcept +{ + rotation *= rotation; + return *this; +} + +template +constexpr rigid3 rigid3::operator*(rigid3 const& other) const noexcept +{ + rigid3 ret; + multiply(ret, *this, other); + return ret; +} + +template +constexpr rigid3& rigid3::operator*=(rigid3 const& other) noexcept +{ + rigid3 a(*this); + return multiply(*this, a, other); +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::rigid3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.translation); + tl::hash_and_combine(seed, p.rotation); + return seed; + } +}; + diff --git a/include/VMath/segment2.h b/include/VMath/segment2.h new file mode 100644 index 0000000..6889fa5 --- /dev/null +++ b/include/VMath/segment2.h @@ -0,0 +1,40 @@ +#pragma once + +#include + +namespace math +{ + +template +struct segment2 +{ + typedef T value_t; + typedef segment2 this_t; + + constexpr segment2() noexcept = default; + constexpr segment2(T x1, T y1, T x2, T y2) noexcept; + constexpr segment2(const vec2& start, const vec2& end) noexcept; + + constexpr segment2(const segment2&) noexcept = default; + constexpr segment2& operator=(const segment2&) noexcept = default; + + constexpr bool is_empty() const noexcept; + + constexpr vec2 get_direction() const noexcept; + constexpr vec2 get_center() const noexcept; + + constexpr T length() const noexcept; + constexpr T length_sq() const noexcept; + + constexpr vec2 get_point(T t) const noexcept; + constexpr void translate(const vec2& offset) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + vec2 start; + vec2 end; +}; + +} diff --git a/include/VMath/segment2.inl b/include/VMath/segment2.inl new file mode 100644 index 0000000..e6bc3ef --- /dev/null +++ b/include/VMath/segment2.inl @@ -0,0 +1,77 @@ +namespace math +{ + + +template +constexpr segment2::segment2(T x1, T y1, T x2, T y2) noexcept + : start(x1, y2) + , end(x2, y2) +{ +} +template +constexpr segment2::segment2(const vec2& start, const vec2& end) noexcept + : start(start) + , end(end) +{ +} + +template +constexpr bool segment2::is_empty() const noexcept +{ + return is_equal(start, end); +} +template +constexpr vec2 segment2::get_direction() const noexcept +{ + return normalized(end - start); +} +template +constexpr vec2 segment2::get_center() const noexcept +{ + if constexpr (std::is_floating_point_v) + return (start + end) * T(0.5); + else + return (start + end) / T(2); +} + +template +constexpr T segment2::length() const noexcept +{ + return distance(end, start); +} +template +constexpr T segment2::length_sq() const noexcept +{ + return distance_sq(end, start); +} + +template +constexpr vec2 segment2::get_point(T t) const noexcept +{ + return lerp(start, end, t); +} + +template +constexpr void segment2::translate(const vec2& offset) noexcept +{ + start += offset; + end += offset; +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::segment2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.start); + tl::hash_and_combine(seed, p.end); + return seed; + } +}; + diff --git a/include/VMath/segment3.h b/include/VMath/segment3.h new file mode 100644 index 0000000..210f897 --- /dev/null +++ b/include/VMath/segment3.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +namespace math +{ + +template +struct segment3 +{ + typedef T value_t; + typedef segment3 this_t; + + constexpr segment3() noexcept = default; + constexpr segment3(const vec3& start, const vec3& end) noexcept; + + constexpr segment3(const segment3&) noexcept = default; + constexpr segment3& operator=(const segment3&) noexcept = default; + + constexpr bool is_empty() const noexcept; + + constexpr vec3 get_direction() const noexcept; + constexpr vec3 get_center() const noexcept; + + constexpr T length() const noexcept; + constexpr T length_sq() const noexcept; + + constexpr vec3 get_point(T t) const noexcept; + constexpr void translate(const vec3& offset) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + vec3 start; + vec3 end; +}; + +} diff --git a/include/VMath/segment3.inl b/include/VMath/segment3.inl new file mode 100644 index 0000000..d905776 --- /dev/null +++ b/include/VMath/segment3.inl @@ -0,0 +1,70 @@ +namespace math +{ + +template +constexpr segment3::segment3(const vec3& start, const vec3& end) noexcept + : start(start) + , end(end) +{ +} + +template +constexpr bool segment3::is_empty() const noexcept +{ + return is_equal(start, end); +} +template +constexpr vec3 segment3::get_direction() const noexcept +{ + return normalized(end - start); +} +template +constexpr vec3 segment3::get_center() const noexcept +{ + if constexpr (std::is_floating_point_v) + return (start + end) * T(0.5); + else + return (start + end) / T(2); +} + +template +constexpr T segment3::length() const noexcept +{ + return distance(end, start); +} +template +constexpr T segment3::length_sq() const noexcept +{ + return distance_sq(end, start); +} + +template +constexpr vec3 segment3::get_point(T t) const noexcept +{ + return lerp(start, end, t); +} + +template +constexpr void segment3::translate(const vec3& offset) noexcept +{ + start += offset; + end += offset; +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::segment3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.start); + tl::hash_and_combine(seed, p.end); + return seed; + } +}; + diff --git a/include/VMath/sphere.h b/include/VMath/sphere.h new file mode 100644 index 0000000..15007d3 --- /dev/null +++ b/include/VMath/sphere.h @@ -0,0 +1,64 @@ +#pragma once + +namespace math +{ + +//! Axis aligned bounding box in 3d dimensional space. +template +struct sphere +{ + typedef T value_t; + typedef sphere this_t; + + static const value_t& invalid_radius(); + +/////////////////////////////////////////////////////////////////////////////// +// constructors +/////////////////////////////////////////////////////////////////////////////// + + explicit sphere(ZUninitialized tag); + sphere(); + explicit sphere(const vec3& center, T radius = T(0)); + +/////////////////////////////////////////////////////////////////////////////// +// methods +/////////////////////////////////////////////////////////////////////////////// + + //!@return true iff the box is initialized (any test for containment will be false) + bool is_valid() const; + + //! Get the volume enclosed by the sphere in cubic units + T get_volume() const; + + //! Get the surface area of the sphere in squared units + T get_area() const; + +/////////////////////////////////////////////////////////////////////////////// +// containment +/////////////////////////////////////////////////////////////////////////////// + + ///@param p: Point to be included in the sphere (will grow accordingly). + void merge(const vec3& p); + + ///@param s: Sphere to be included in this sphere (will grow accordingly). + void merge(const sphere& b); + +/////////////////////////////////////////////////////////////////////////////// +// operators +/////////////////////////////////////////////////////////////////////////////// + + bool operator==(const sphere& other) const; + bool operator!=(const sphere& other) const; + +/////////////////////////////////////////////////////////////////////////////// +// members +/////////////////////////////////////////////////////////////////////////////// + + vec3 center; + T radius; +}; + +/////////////////////////////////////////////////////////////////////////////// + + +}//namespace math diff --git a/include/VMath/sphere.inl b/include/VMath/sphere.inl new file mode 100644 index 0000000..fa68531 --- /dev/null +++ b/include/VMath/sphere.inl @@ -0,0 +1,114 @@ +namespace math +{ + +/////////////////////////////////////////////////////////////////////////////// +// constructors +/////////////////////////////////////////////////////////////////////////////// + +template +const typename sphere::value_t& sphere::invalid_radius() +{ + static value_t value(-limits::infinity()); + return value; +} + +template +sphere::sphere(ZUninitialized tag) : center(tag) +{ +} + +template +sphere::sphere() : center(constants::zero()), radius(invalid_radius()) +{ +} + +template +sphere::sphere(const vec3& _center, T _radius) : center(_center), radius(_radius) +{ + JTL_ASSERT(radius >= T(0)); +} + +/////////////////////////////////////////////////////////////////////////////// +// methods +/////////////////////////////////////////////////////////////////////////////// + +template +void sphere::merge(const vec3& p) +{ + if (radius != invalid_radius()) + { + const vec3 pc = p - center; + const value_t pc_len2 = length_sq(pc); + if (pc_len2 > square(radius)) + {// point is outside of the sphere + const value_t pc_len = std::sqrt(pc_len2); + radius = constants::half() * (radius + pc_len); + center = center + (pc_len - radius) / pc_len * pc; + } + } + else + {// the empty sphere becomes the point + center = p; + radius = constants::zero(); + } +} + +template +void sphere::merge(const sphere& b) +{ + vec3 c1c2 = b.center - center; + value_t c1c2_len2 = length_sq(c1c2); + value_t dr = b.radius - radius; + + if (square(dr) >= c1c2_len2) + { + if (is_positive(dr)) + {// b contains this + *this = b; + } + } + else + { + value_t c1c2_len = sqrt(c1c2_len2); + if (is_positive(c1c2_len)) { + center += (c1c2_len + dr) / (constants::two() * c1c2_len) * c1c2; + } + radius = constants::half() * (c1c2_len + radius + b.radius); + } +} + +template +bool sphere::is_valid() const +{ + return !is_negative(radius); +} + +template +T sphere::get_volume() const +{ + return constants::pi() * radius*radius*radius * T(4) / T(3); +} + +template +T sphere::get_area() const +{ + return T(4) * constants::pi() * radius*radius; +} + +/////////////////////////////////////////////////////////////////////////////// +// operators +/////////////////////////////////////////////////////////////////////////////// + +template +bool sphere::operator==(const sphere& other) const +{ + return (center == other.center && radius == other.radius); +} + +template +bool sphere::operator!=(const sphere& other) const +{ + return !(*this == other); +} + +}//namespace math diff --git a/include/VMath/trans2.h b/include/VMath/trans2.h new file mode 100644 index 0000000..a639366 --- /dev/null +++ b/include/VMath/trans2.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +namespace math +{ + +template struct vec2; +template struct mat2; + +// mat3 layout trans2 layout +// 0 3 6 0 2 tx +// 1 4 7 -> 1 3 ty +// 2 5 8 +// the last row is always constant (0 0 1) for the trans + +template +struct trans2 +{ + typedef T value_t; + using this_t = trans2; + + constexpr trans2() noexcept = default; + constexpr explicit trans2(math::ZUninitialized) noexcept; + constexpr trans2(trans2 const&) noexcept = default; + + constexpr explicit trans2(vec2 const& translation) noexcept; + constexpr trans2(vec2 const& translation, T rotation) noexcept; + constexpr trans2(vec2 const& translation, T rotation, vec2 const& scale) noexcept; + constexpr trans2(vec2 const& translation, mat2 const& rotation_scale) noexcept; + constexpr trans2(vec2 const& translation, mat2 const& rotation_scale, vec2 const& scale) noexcept; + + constexpr trans2& operator=(trans2 const&) = default; + + constexpr trans2 operator*(trans2 const& other) const noexcept; + constexpr trans2& operator*=(trans2 const& other) noexcept; + + constexpr trans2& post_scale(vec2 const& scale) noexcept; + constexpr trans2& post_translate(vec2 const& trans) noexcept; + + constexpr vec2 const& get_axis_x() const noexcept; + constexpr trans2& set_axis_x(vec2 const& axis) noexcept; + + constexpr vec2 const& get_axis_y() const noexcept; + constexpr trans2& set_axis_y(vec2 const& axis) noexcept; + + template + constexpr vec2 get_scale() const noexcept; + constexpr trans2& set_scale(vec2 const& scale) noexcept; + + constexpr trans2& set_rotation_scale(T rotation) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + + vec2 translation; + mat2 rotation_scale; +}; + +} diff --git a/include/VMath/trans2.inl b/include/VMath/trans2.inl new file mode 100644 index 0000000..244fe0c --- /dev/null +++ b/include/VMath/trans2.inl @@ -0,0 +1,124 @@ +namespace math +{ + +template +constexpr trans2::trans2(math::ZUninitialized) noexcept + : rotation_scale(math::uninitialized) +{ +} + +template +constexpr trans2::trans2(vec2 const& translation) noexcept + : translation(translation) +{ +} +template +constexpr trans2::trans2(vec2 const& translation, T rotation) noexcept + : trans2(translation, mat2::from_rotation(rotation)) +{ +} +template +constexpr trans2::trans2(vec2 const& translation, T rotation, vec2 const& scale) noexcept + : trans2(translation, mat2::from_rotation(rotation), scale) +{ +} +template +constexpr trans2::trans2(vec2 const& translation, mat2 const& rotation_scale, vec2 const& scale) noexcept + : translation(translation) + , rotation_scale(rotation_scale) +{ + post_scale(scale); +} +template +constexpr trans2::trans2(vec2 const& translation, mat2 const& rotation_scale) noexcept + : translation(translation) + , rotation_scale(rotation_scale) +{ +} + +template +constexpr vec2 const& trans2::get_axis_x() const noexcept +{ + return rotation_scale.column0; +} +template +constexpr vec2 const& trans2::get_axis_y() const noexcept +{ + return rotation_scale.column1; +} +template +template +constexpr vec2 trans2::get_scale() const noexcept +{ + return vec2(get_axis_x().template length(), get_axis_y().template length()); +} +template +constexpr trans2& trans2::set_axis_x(vec2 const& axis) noexcept +{ + rotation_scale.set_axis_x(axis); + return *this; +} +template +constexpr trans2& trans2::set_axis_y(vec2 const& axis) noexcept +{ + rotation_scale.set_axis_y(axis); + return *this; +} +template +constexpr trans2& trans2::set_scale(vec2 const& s) noexcept +{ + rotation_scale.set_scale(s); + return *this; +} +template +constexpr trans2& trans2::set_rotation_scale(T rotation) noexcept +{ + rotation_scale = mat2::from_rotation(rotation); + return *this; +} +template +constexpr trans2& trans2::post_scale(const vec2& scale) noexcept +{ + rotation_scale.column0 *= scale.x; + rotation_scale.column1 *= scale.y; + return *this; +} +template +constexpr trans2& trans2::post_translate(vec2 const& trans) noexcept +{ + translation += rotate(rotation_scale, trans); + return *this; +} + +template +constexpr trans2 trans2::operator*(trans2 const& other) const noexcept +{ + trans2 ret; + multiply(ret, *this, other); + return ret; +} +template +constexpr trans2& trans2::operator*=(const trans2& other) noexcept +{ + trans2 a(*this); + multiply(*this, a, other); + return *this; +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::trans2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.rotation_scale); + tl::hash_and_combine(seed, p.translation); + return seed; + } +}; + diff --git a/include/VMath/trans3.h b/include/VMath/trans3.h new file mode 100644 index 0000000..5362086 --- /dev/null +++ b/include/VMath/trans3.h @@ -0,0 +1,67 @@ +#pragma once + +#include + +namespace math +{ + +template struct vec3; +template struct mat4; +template struct quat; + +// mat4 layout trans3 layout (4 vec3) +// 0 4 8 12 0 3 6 tx +// 1 5 9 13 -> 1 4 7 ty +// 2 6 10 14 2 5 8 tz +// 3 7 11 15 +// the last row is always constant (0 0 0 1) for the trans + +template +struct trans3 +{ + typedef T value_t; + using this_t = trans3; + + constexpr trans3() noexcept = default; + constexpr explicit trans3(math::ZUninitialized) noexcept; + constexpr trans3(trans3 const&) noexcept = default; + + constexpr explicit trans3(vec3 const& translation) noexcept; + constexpr trans3(vec3 const& translation, quat const& rotation_scale) noexcept; + constexpr trans3(vec3 const& translation, quat const& rotation_scale, vec3 const& scale) noexcept; + constexpr trans3(vec3 const& translation, mat3 const& rotation_scale) noexcept; + constexpr trans3(vec3 const& translation, mat3 const& rotation_scale, vec3 const& scale) noexcept; + + constexpr trans3& operator=(trans3 const&) noexcept = default; + + constexpr trans3 operator*(trans3 const& other) const noexcept; + constexpr trans3& operator*=(trans3 const& other) noexcept; + + constexpr trans3& post_scale(vec3 const& scale) noexcept; + constexpr trans3& post_translate(vec3 const& translation) noexcept; + + constexpr vec3 const& get_axis_x() const noexcept; + constexpr trans3& set_axis_x(vec3 const& axis) noexcept; + + constexpr vec3 const& get_axis_y() const noexcept; + constexpr trans3& set_axis_y(vec3 const& axis) noexcept; + + constexpr vec3 const& get_axis_z() const noexcept; + constexpr trans3& set_axis_z(vec3 const& axis) noexcept; + + template + constexpr vec3 get_scale() const noexcept; + constexpr trans3& set_scale(vec3 const& scale) noexcept; + + constexpr trans3& set_rotation_scale(quat const& rotation_scale) noexcept; + + constexpr auto operator<=>(const this_t&) const noexcept = default; + constexpr bool operator==(const this_t&) const noexcept = default; + + + + vec3 translation; + mat3 rotation_scale; +}; + +} diff --git a/include/VMath/trans3.inl b/include/VMath/trans3.inl new file mode 100644 index 0000000..89c8f6b --- /dev/null +++ b/include/VMath/trans3.inl @@ -0,0 +1,135 @@ +namespace math +{ + +template +constexpr trans3::trans3(math::ZUninitialized) noexcept + : rotation_scale(math::uninitialized) +{ +} +template +constexpr trans3::trans3(vec3 const& translation) noexcept + : translation(translation) +{ +} +template +constexpr trans3::trans3(vec3 const& translation, quat const& rs) noexcept + : trans3(translation, to_mat3(rs)) +{ +} +template +constexpr trans3::trans3(vec3 const& translation, quat const& rs, vec3 const& scale) noexcept + : trans3(translation, to_mat3(rs), scale) +{ +} +template +constexpr trans3::trans3(vec3 const& translation, mat3 const& rs) noexcept + : translation(translation) + , rotation_scale(rs) +{ +} +template +constexpr trans3::trans3(vec3 const& translation, mat3 const& rs, vec3 const& scale) noexcept + : translation(translation) + , rotation_scale(rs) +{ + post_scale(scale); +} + +template +constexpr vec3 const& trans3::get_axis_x() const noexcept +{ + return rotation_scale.get_axis_x(); +} +template +constexpr vec3 const& trans3::get_axis_y() const noexcept +{ + return rotation_scale.get_axis_y(); +} +template +constexpr vec3 const& trans3::get_axis_z() const noexcept +{ + return rotation_scale.get_axis_z(); +} +template +template +constexpr vec3 trans3::get_scale() const noexcept +{ + return vec3(get_axis_x().template length(), get_axis_y().template length(), get_axis_z().template length()); +} +template +constexpr trans3& trans3::set_axis_x(vec3 const& axis) noexcept +{ + rotation_scale.set_axis_x(axis); + return *this; +} +template +constexpr trans3& trans3::set_axis_y(vec3 const& axis) noexcept +{ + rotation_scale.set_axis_y(axis); + return *this; +} +template +constexpr trans3& trans3::set_axis_z(vec3 const& axis) noexcept +{ + rotation_scale.set_axis_z(axis); + return *this; +} +template +constexpr trans3& trans3::set_scale(vec3 const& s) noexcept +{ + rotation_scale.set_scale(s); + return *this; +} +template +constexpr trans3& trans3::set_rotation_scale(quat const& rs) noexcept +{ + rotation_scale = mat3_from_trans2(rs); + return *this; +} +template +constexpr trans3& trans3::post_scale(vec3 const& scale) noexcept +{ + rotation_scale.column0 *= scale.x; + rotation_scale.column1 *= scale.y; + rotation_scale.column2 *= scale.z; + return *this; +} +template +constexpr trans3& trans3::post_translate(vec3 const& translation) noexcept +{ + translation += rotate(rotation_scale, translation); + return *this; +} + +template +constexpr trans3 trans3::operator*(trans3 const& other) const noexcept +{ + trans3 ret; + multiply(ret, *this, other); + return ret; +} +template +constexpr trans3& trans3::operator*=(trans3 const& other) noexcept +{ + trans3 a(*this); + multiply(*this, a, other); + return *this; +} + +} + + +#include "tl/hash_and_combine.h" + +template +struct eastl::hash> +{ + std::size_t operator()(const math::trans3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.rotation_scale); + tl::hash_and_combine(seed, p.translation); + return seed; + } +}; + diff --git a/include/VMath/triangle3.h b/include/VMath/triangle3.h new file mode 100644 index 0000000..2591b87 --- /dev/null +++ b/include/VMath/triangle3.h @@ -0,0 +1,39 @@ +#pragma once + +#include + +namespace math +{ + +template struct vec3; + +//! 3d triangle template class +template +struct triangle3 +{ + constexpr triangle3() noexcept = default; + constexpr triangle3(const vec3& a, const vec3& b, const vec3& c) noexcept; + constexpr triangle3(triangle3 const&) noexcept = default; + + constexpr auto operator<=>(const triangle3&) const noexcept = default; + constexpr bool operator==(const triangle3&) const noexcept = default; + + constexpr triangle3& operator=(triangle3 const&) noexcept = default; + + //! Get the normal of the triangle. + /** Please note: The normal is not always normalized. */ + template + constexpr vec3 get_normal() const noexcept; + constexpr plane get_plane() const noexcept; + + template + constexpr T get_area() const noexcept; + constexpr T get_area_sq() const noexcept; + + + vec3 a; + vec3 b; + vec3 c; +}; + +} diff --git a/include/VMath/triangle3.inl b/include/VMath/triangle3.inl new file mode 100644 index 0000000..1511e6e --- /dev/null +++ b/include/VMath/triangle3.inl @@ -0,0 +1,30 @@ +namespace math +{ + +template +constexpr triangle3::triangle3(const vec3& a, const vec3& b, const vec3& c) noexcept : a(a), b(b), c(c) {} + +template +template +constexpr vec3 triangle3::get_normal() const noexcept +{ + return normalized(cross(b - a, c - a)); +} +template +constexpr plane triangle3::get_plane() const noexcept +{ + return plane(a, b, c); +} +template +template +constexpr T triangle3::get_area() const noexcept +{ + return length(cross(b - a, c - a)) * T(0.5); +} +template +constexpr T triangle3::get_area_sq() const noexcept +{ + return length_sq(cross(b - a, c - a)) * T(0.25); +} + +} \ No newline at end of file diff --git a/include/VMath/vec2.h b/include/VMath/vec2.h new file mode 100644 index 0000000..6dedfb9 --- /dev/null +++ b/include/VMath/vec2.h @@ -0,0 +1,73 @@ +#pragma once + +#include +#include + +namespace math +{ + +template +struct vec2 +{ + typedef T value_t; + static constexpr size_t element_count = 2; + + constexpr vec2() noexcept = default; + constexpr vec2(T x, T y) noexcept; + constexpr explicit vec2(T s) noexcept; + constexpr explicit vec2(std::array const& v) noexcept; + + constexpr vec2(vec2 const& v) noexcept = default; + + constexpr T& operator[](size_t i) noexcept; + constexpr T const& operator[](size_t i) const noexcept; + + constexpr auto operator<=>(const vec2&) const noexcept = default; + constexpr bool operator==(const vec2&) const noexcept = default; + + constexpr vec2 operator-() const noexcept; + + constexpr vec2 operator+(T s) const noexcept; + constexpr vec2 operator+(vec2 const& v2) const noexcept; + + constexpr vec2 operator-(T s) const noexcept; + constexpr vec2 operator-(vec2 const& v2) const noexcept; + + constexpr vec2 operator*(T s) const noexcept; + constexpr vec2 operator/(T s) const noexcept; + + constexpr vec2& operator=(vec2 const& v) noexcept = default; + + constexpr vec2& operator+=(T s) noexcept; + constexpr vec2& operator+=(vec2 const& v) noexcept; + + constexpr vec2& operator-=(T s) noexcept; + constexpr vec2& operator-=(vec2 const& v) noexcept; + + constexpr vec2& operator*=(T s) noexcept; + constexpr vec2& operator*=(vec2 const& v) noexcept; + + constexpr vec2& operator/=(T s) noexcept; + constexpr vec2& operator/=(vec2 const& v) noexcept; + + template + constexpr vec2& normalize() noexcept; + template + constexpr T length() const noexcept; + constexpr T length_sq() const noexcept; + + T x = T(0); + T y = T(0); +}; + +template constexpr vec2 operator*(T s, vec2 const&) noexcept; +template constexpr vec2 operator/(T s, vec2 const&) noexcept; + +template constexpr vec2 operator*(vec2 const& v0, vec2 const& v1) noexcept; +template constexpr vec2 operator/(vec2 const& v0, vec2 const& v2) noexcept; + +////////////////////////////////////////////////////////////////////////// + +} //math + + diff --git a/include/VMath/vec2.inl b/include/VMath/vec2.inl new file mode 100644 index 0000000..b17e050 --- /dev/null +++ b/include/VMath/vec2.inl @@ -0,0 +1,256 @@ +namespace math +{ + +template constexpr vec2::vec2(T x, T y) noexcept : x(x), y(y) {} +template constexpr vec2::vec2(T v) noexcept : x(v), y(v) {} +template constexpr vec2::vec2(std::array const& v) noexcept : x(v[0]), y(v[1]) {} + +template +constexpr T& vec2::operator[](size_t i) noexcept +{ + TL_PLAIN_ASSERT(i < sizeof(*this) / sizeof(T)); + + //if you hit this TL_PLAIN_ASSERT, your compiler introduces padding. Check for #pragma packs without pop + TL_PLAIN_ASSERT(&((T*)this)[0] == &x && &((T*)this)[1] == &y); + + return ((T*)this)[i]; +} + +template +constexpr T const& vec2::operator[](size_t i) const noexcept +{ + TL_PLAIN_ASSERT(i < sizeof(*this) / sizeof(T)); + + //if you hit this TL_PLAIN_ASSERT, your compiler introduces padding. Check for #pragma packs without pop + TL_PLAIN_ASSERT(&((T*)this)[0] == &x && &((T*)this)[1] == &y); + + return ((T*)this)[i]; +} + +template +constexpr vec2 vec2::operator-() const noexcept +{ + return vec2(-x, -y); +} + +template +constexpr vec2 vec2::operator+(T s) const noexcept +{ + return vec2(x + s, y + s); +} +template +constexpr vec2 vec2::operator+(vec2 const& v) const noexcept +{ + return vec2(x + v.x, y + v.y); +} + +template +constexpr vec2 vec2::operator-(T s) const noexcept +{ + return vec2(x - s, y - s); +} +template +constexpr vec2 vec2::operator-(vec2 const& v2) const noexcept +{ + return vec2(x - v2.x, y - v2.y); +} + +template +constexpr vec2 vec2::operator*(T s) const noexcept +{ + return vec2(x * s, y * s); +} + +template +constexpr vec2 vec2::operator/(T s) const noexcept +{ + TL_PLAIN_ASSERT(s != T(0)); + if constexpr (std::is_floating_point_v) + { + T is = T(1) / s; + return vec2(x * is, y * is); + } + else + return vec2(x / s, y / s); +} + +template +constexpr vec2& vec2::operator+=(T s) noexcept +{ + x += s; + y += s; + return *this; +} +template +constexpr vec2& vec2::operator+=(vec2 const& v) noexcept +{ + x += v.x; + y += v.y; + return *this; +} + +template +constexpr vec2& vec2::operator-=(T s) noexcept +{ + x -= s; + y -= s; + return *this; +} +template +constexpr vec2& vec2::operator-=(vec2 const& v) noexcept +{ + x -= v.x; + y -= v.y; + return *this; +} + +template +constexpr vec2& vec2::operator*=(T s) noexcept +{ + x *= s; + y *= s; + return *this; +} +template +constexpr vec2& vec2::operator*=(vec2 const& v) noexcept +{ + x *= v.x; + y *= v.y; + return *this; +} + +template +constexpr vec2& vec2::operator/=(T s) noexcept +{ + TL_PLAIN_ASSERT(!is_zero(s)); + if constexpr (std::is_floating_point_v) + { + T is = T(1) / s; + x *= is; + y *= is; + } + else + { + x /= s; + y /= s; + } + return *this; +} +template +constexpr vec2& vec2::operator/=(vec2 const& v) noexcept +{ + TL_PLAIN_ASSERT(!cwise::any(cwise::is_zero(v))); + x /= v.x; + y /= v.y; + return *this; +} + +template +constexpr vec2 operator*(T s, vec2 const& v) noexcept +{ + return v*s; +} +template +constexpr vec2 operator/(T s, vec2 const& v) noexcept +{ + return vec2(s) / v; +} + +template +constexpr vec2 operator*(vec2 const& v0, vec2 const& v1) noexcept +{ + return vec2(v0.x * v1.x, v0.y * v1.y); +} + +template +constexpr vec2 operator/(vec2 const& u, vec2 const& v) noexcept +{ + TL_PLAIN_ASSERT(!cwise::any(cwise::is_zero(v))); + return vec2(u.x / v.x, u.y / v.y); +} + +template +template +constexpr vec2& vec2::normalize() noexcept +{ + const T len_sq = x * x + y * y; + const T len_inv = inv_sqrt(len_sq); + x *= len_inv; + y *= len_inv; + return *this; +} +template +template +constexpr T vec2::length() const noexcept +{ + return sqrt(x * x + y * y); +} +template +constexpr T vec2::length_sq() const noexcept +{ + return x * x + y * y; +} + + +} //math + +#include "tl/hash_and_combine.h" + +namespace eastl +{ +template +struct hash> +{ + std::size_t operator()(const math::vec2& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.x); + tl::hash_and_combine(seed, p.y); + return seed; + } +}; + +template +class numeric_limits> +{ +public: + typedef math::vec2 value_type; + + static EA_CONSTEXPR_OR_CONST bool is_specialized = true; + static EA_CONSTEXPR_OR_CONST int digits = tl::numeric_limits::digits; + static EA_CONSTEXPR_OR_CONST int digits10 = tl::numeric_limits::digits10; + static EA_CONSTEXPR_OR_CONST int max_digits10 = tl::numeric_limits::max_digits10; + static EA_CONSTEXPR_OR_CONST bool is_signed = tl::numeric_limits::is_signed; + static EA_CONSTEXPR_OR_CONST bool is_integer = tl::numeric_limits::is_integer; + static EA_CONSTEXPR_OR_CONST bool is_exact = tl::numeric_limits::is_exact; + static EA_CONSTEXPR_OR_CONST int radix = tl::numeric_limits::radix; + static EA_CONSTEXPR_OR_CONST int min_exponent = tl::numeric_limits::min_exponent; + static EA_CONSTEXPR_OR_CONST int min_exponent10 = tl::numeric_limits::min_exponent10; + static EA_CONSTEXPR_OR_CONST int max_exponent = tl::numeric_limits::max_exponent; + static EA_CONSTEXPR_OR_CONST int max_exponent10 = tl::numeric_limits::max_exponent10; + static EA_CONSTEXPR_OR_CONST bool is_bounded = tl::numeric_limits::is_bounded; + static EA_CONSTEXPR_OR_CONST bool is_modulo = tl::numeric_limits::is_modulo; + static EA_CONSTEXPR_OR_CONST bool traps = tl::numeric_limits::traps; + static EA_CONSTEXPR_OR_CONST bool tinyness_before = tl::numeric_limits::tinyness_before; + static EA_CONSTEXPR_OR_CONST float_round_style round_style = tl::numeric_limits::round_style; + static EA_CONSTEXPR_OR_CONST bool has_infinity = tl::numeric_limits::has_infinity; + static EA_CONSTEXPR_OR_CONST bool has_quiet_NaN = tl::numeric_limits::has_quiet_NaN; + static EA_CONSTEXPR_OR_CONST bool has_signaling_NaN = tl::numeric_limits::has_signaling_NaN; + static EA_CONSTEXPR_OR_CONST float_denorm_style has_denorm = tl::numeric_limits::has_denorm; + static EA_CONSTEXPR_OR_CONST bool has_denorm_loss = tl::numeric_limits::has_denorm_loss; + static EA_CONSTEXPR_OR_CONST bool is_iec559 = tl::numeric_limits::is_iec559; + + static value_type min() noexcept { return value_type(tl::numeric_limits::min()); } + static value_type max() noexcept { return value_type(tl::numeric_limits::max()); } + static value_type lowest() noexcept { return value_type(tl::numeric_limits::lowest()); } + static value_type epsilon() noexcept { return value_type(tl::numeric_limits::epsilon()); } + static value_type round_error() noexcept { return value_type(tl::numeric_limits::round_error()); } + static value_type infinity() noexcept { return value_type(tl::numeric_limits::infinity()); } + static value_type quiet_NaN() noexcept { return value_type(tl::numeric_limits::quiet_NaN()); } + static value_type signaling_NaN() noexcept { return value_type(tl::numeric_limits::signaling_NaN()); } + static value_type denorm_min() noexcept { return value_type(tl::numeric_limits::denorm_min()); } +}; + + +} + diff --git a/include/VMath/vec3.h b/include/VMath/vec3.h new file mode 100644 index 0000000..e2d9654 --- /dev/null +++ b/include/VMath/vec3.h @@ -0,0 +1,73 @@ +#pragma once + +#include +#include + +namespace math +{ + +template +struct vec3 +{ + typedef T value_t; + static constexpr size_t element_count = 3; + + constexpr vec3() noexcept = default; + constexpr vec3(T x, T y, T z) noexcept; + constexpr explicit vec3(T s) noexcept; + constexpr explicit vec3(std::array const& v) noexcept; + constexpr vec3(vec3 const& v) noexcept = default; + + constexpr T& operator[](size_t i) noexcept; + constexpr T const& operator[](size_t i) const noexcept; + + constexpr auto operator<=>(const vec3&) const noexcept = default; + constexpr bool operator==(const vec3&) const noexcept = default; + + constexpr vec3 operator-() const noexcept; + + constexpr vec3 operator+(T s) const noexcept; + constexpr vec3 operator+(vec3 const& v) const noexcept; + + constexpr vec3 operator-(T s) const noexcept; + constexpr vec3 operator-(vec3 const& v) const noexcept; + + constexpr vec3 operator*(T s) const noexcept; + constexpr vec3 operator/(T s) const noexcept; + + constexpr vec3& operator=(vec3 const& v) noexcept = default; + + constexpr vec3& operator+=(T s) noexcept; + constexpr vec3& operator+=(vec3 const& v) noexcept; + + constexpr vec3& operator-=(T s) noexcept; + constexpr vec3& operator-=(vec3 const& v) noexcept; + + constexpr vec3& operator*=(T s) noexcept; + constexpr vec3& operator*=(vec3 const& v) noexcept; + + constexpr vec3& operator/=(T s) noexcept; + constexpr vec3& operator/=(vec3 const& v) noexcept; + + template + constexpr vec3& normalize() noexcept; + template + constexpr T length() const noexcept; + constexpr T length_sq() const noexcept; + + + T x = T(0); + T y = T(0); + T z = T(0); +}; + +template constexpr vec3 operator*(T s, vec3 const&) noexcept; +template constexpr vec3 operator*(vec3 const& v0, vec3 const& v1) noexcept; +template constexpr vec3 operator/(T s, vec3 const&) noexcept; +template constexpr vec3 operator/(vec3 const& v0, vec3 const& v1) noexcept; + +////////////////////////////////////////////////////////////////////////// + +} //math + + diff --git a/include/VMath/vec3.inl b/include/VMath/vec3.inl new file mode 100644 index 0000000..5b97650 --- /dev/null +++ b/include/VMath/vec3.inl @@ -0,0 +1,258 @@ +namespace math +{ + +template constexpr vec3::vec3(T x, T y, T z) noexcept : x(x), y(y), z(z) {} +template constexpr vec3::vec3(T v) noexcept : x(v), y(v), z(v) {} +template constexpr vec3::vec3(std::array const& v) noexcept : x(v[0]), y(v[1]), z(v[2]) {} + +template +constexpr vec3 vec3::operator-() const noexcept +{ + return vec3(-x, -y, -z); +} + +template +constexpr T& vec3::operator[](size_t i) noexcept +{ + TL_PLAIN_ASSERT(i < 3); + return ((T*)this)[i]; +} + +template +constexpr T const& vec3::operator[](size_t i) const noexcept +{ + TL_PLAIN_ASSERT(i < 3); + return ((T*)this)[i]; +} + +template +constexpr vec3 vec3::operator+(T s) const noexcept +{ + return vec3(x + s, y + s, z + s); +} +template +constexpr vec3 vec3::operator+(vec3 const& v) const noexcept +{ + return vec3(x + v.x, y + v.y, z + v.z); +} + +template +constexpr vec3 vec3::operator-(T s) const noexcept +{ + return vec3(x - s, y - s, z - s); +} +template +constexpr vec3 vec3::operator-(vec3 const& v) const noexcept +{ + return vec3(x - v.x, y - v.y, z - v.z); +} + +template +constexpr vec3 vec3::operator*(T s) const noexcept +{ + return vec3(x * s, y * s, z * s); +} + +template +constexpr vec3 vec3::operator/(T s) const noexcept +{ + TL_PLAIN_ASSERT(!is_zero(s)); + if constexpr (std::is_floating_point_v) + { + T is = T(1) / s; + return vec3(x * is, y * is, z * is); + } + else + return vec3(x / s, y / s, z / s); +} + +template +constexpr vec3& vec3::operator+=(T s) noexcept +{ + x += s; + y += s; + z += s; + return *this; +} +template +constexpr vec3& vec3::operator+=(vec3 const& v) noexcept +{ + x += v.x; + y += v.y; + z += v.z; + return *this; +} + +template +constexpr vec3& vec3::operator-=(T s) noexcept +{ + x -= s; + y -= s; + z -= s; + return *this; +} +template +constexpr vec3& vec3::operator-=(vec3 const& v) noexcept +{ + x -= v.x; + y -= v.y; + z -= v.z; + return *this; +} + +template +constexpr vec3& vec3::operator*=(T s) noexcept +{ + x *= s; + y *= s; + z *= s; + return *this; +} +template +constexpr vec3& vec3::operator*=(vec3 const& v) noexcept +{ + x *= v.x; + y *= v.y; + z *= v.z; + return *this; +} + +template +constexpr vec3& vec3::operator/=(T s) noexcept +{ + TL_PLAIN_ASSERT(s != T(0)); + if constexpr (std::is_floating_point_v) + { + T is = T(1) / s; + x *= is; + y *= is; + z *= is; + } + else + { + x /= s; + y /= s; + z /= s; + } + return *this; +} + +template +constexpr vec3& vec3::operator/=(vec3 const& v) noexcept +{ + TL_PLAIN_ASSERT(!cwise::any(cwise::is_zero(v))); + x /= v.x; + y /= v.y; + z /= v.z; + return *this; +} + +template +constexpr vec3 operator*(T s, vec3 const& v) noexcept +{ + return v*s; +} +template +constexpr vec3 operator/(T s, vec3 const& v) noexcept +{ + return vec3(s) / v; +} + +template +constexpr vec3 operator*(vec3 const& v0, vec3 const& v1) noexcept +{ + return vec3(v0.x * v1.x, v0.y * v1.y, v0.z * v1.z); +} + +template +constexpr vec3 operator/(vec3 const& u, vec3 const& v) noexcept +{ + TL_PLAIN_ASSERT(!cwise::any(cwise::is_zero(v))); + return vec3(u.x / v.x, u.y / v.y, u.z / v.z); +} + +template +template +constexpr vec3& vec3::normalize() noexcept +{ + const T len_sq = x * x + y * y + z * z; + const T len_inv = inv_sqrt(len_sq); + x *= len_inv; + y *= len_inv; + z *= len_inv; + return *this; +} +template +template +constexpr T vec3::length() const noexcept +{ + return sqrt(x * x + y * y + z * z); +} +template +constexpr T vec3::length_sq() const noexcept +{ + return x * x + y * y + z * z; +} + + +} //math + + +#include "tl/hash_and_combine.h" + +namespace eastl +{ +template +struct hash> +{ + std::size_t operator()(const math::vec3& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.x); + tl::hash_and_combine(seed, p.y); + tl::hash_and_combine(seed, p.z); + return seed; + } +}; + +template +class numeric_limits> +{ +public: + typedef math::vec3 value_type; + + static EA_CONSTEXPR_OR_CONST bool is_specialized = true; + static EA_CONSTEXPR_OR_CONST int digits = tl::numeric_limits::digits; + static EA_CONSTEXPR_OR_CONST int digits10 = tl::numeric_limits::digits10; + static EA_CONSTEXPR_OR_CONST int max_digits10 = tl::numeric_limits::max_digits10; + static EA_CONSTEXPR_OR_CONST bool is_signed = tl::numeric_limits::is_signed; + static EA_CONSTEXPR_OR_CONST bool is_integer = tl::numeric_limits::is_integer; + static EA_CONSTEXPR_OR_CONST bool is_exact = tl::numeric_limits::is_exact; + static EA_CONSTEXPR_OR_CONST int radix = tl::numeric_limits::radix; + static EA_CONSTEXPR_OR_CONST int min_exponent = tl::numeric_limits::min_exponent; + static EA_CONSTEXPR_OR_CONST int min_exponent10 = tl::numeric_limits::min_exponent10; + static EA_CONSTEXPR_OR_CONST int max_exponent = tl::numeric_limits::max_exponent; + static EA_CONSTEXPR_OR_CONST int max_exponent10 = tl::numeric_limits::max_exponent10; + static EA_CONSTEXPR_OR_CONST bool is_bounded = tl::numeric_limits::is_bounded; + static EA_CONSTEXPR_OR_CONST bool is_modulo = tl::numeric_limits::is_modulo; + static EA_CONSTEXPR_OR_CONST bool traps = tl::numeric_limits::traps; + static EA_CONSTEXPR_OR_CONST bool tinyness_before = tl::numeric_limits::tinyness_before; + static EA_CONSTEXPR_OR_CONST float_round_style round_style = tl::numeric_limits::round_style; + static EA_CONSTEXPR_OR_CONST bool has_infinity = tl::numeric_limits::has_infinity; + static EA_CONSTEXPR_OR_CONST bool has_quiet_NaN = tl::numeric_limits::has_quiet_NaN; + static EA_CONSTEXPR_OR_CONST bool has_signaling_NaN = tl::numeric_limits::has_signaling_NaN; + static EA_CONSTEXPR_OR_CONST float_denorm_style has_denorm = tl::numeric_limits::has_denorm; + static EA_CONSTEXPR_OR_CONST bool has_denorm_loss = tl::numeric_limits::has_denorm_loss; + static EA_CONSTEXPR_OR_CONST bool is_iec559 = tl::numeric_limits::is_iec559; + + static value_type min() noexcept { return value_type(tl::numeric_limits::min()); } + static value_type max() noexcept { return value_type(tl::numeric_limits::max()); } + static value_type lowest() noexcept { return value_type(tl::numeric_limits::lowest()); } + static value_type epsilon() noexcept { return value_type(tl::numeric_limits::epsilon()); } + static value_type round_error() noexcept { return value_type(tl::numeric_limits::round_error()); } + static value_type infinity() noexcept { return value_type(tl::numeric_limits::infinity()); } + static value_type quiet_NaN() noexcept { return value_type(tl::numeric_limits::quiet_NaN()); } + static value_type signaling_NaN() noexcept { return value_type(tl::numeric_limits::signaling_NaN()); } + static value_type denorm_min() noexcept { return value_type(tl::numeric_limits::denorm_min()); } +}; +} diff --git a/include/VMath/vec4.h b/include/VMath/vec4.h new file mode 100644 index 0000000..e8329a9 --- /dev/null +++ b/include/VMath/vec4.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include + +namespace math +{ + +template +struct vec4 +{ + typedef T value_t; + static constexpr size_t element_count = 4; + + constexpr vec4() noexcept = default; + constexpr vec4(T x, T y, T z, T w) noexcept; + constexpr explicit vec4(T s) noexcept; + constexpr explicit vec4(std::array const& v) noexcept; + + constexpr vec4(vec4 const& v) noexcept = default; + + constexpr T& operator[](size_t i) noexcept; + constexpr T const& operator[](size_t i) const noexcept; + + constexpr auto operator<=>(const vec4&) const noexcept = default; + constexpr bool operator==(const vec4&) const noexcept = default; + + constexpr vec4 operator-() const noexcept; + + constexpr vec4 operator+(T s) const noexcept; + constexpr vec4 operator+(vec4 const& v) const noexcept; + + constexpr vec4 operator-(T s) const noexcept; + constexpr vec4 operator-(vec4 const& v) const noexcept; + + constexpr vec4 operator*(T s) const noexcept; + constexpr vec4 operator/(T s) const noexcept; + + constexpr vec4& operator=(vec4 const& v) noexcept = default; + + constexpr vec4& operator+=(T s) noexcept; + constexpr vec4& operator+=(vec4 const& v) noexcept; + + constexpr vec4& operator-=(T s) noexcept; + constexpr vec4& operator-=(vec4 const& v) noexcept; + + constexpr vec4& operator*=(T s) noexcept; + constexpr vec4& operator*=(vec4 const& v) noexcept; + + constexpr vec4& operator/=(T s) noexcept; + constexpr vec4& operator/=(vec4 const& v) noexcept; + + template + constexpr vec4& normalize() noexcept; + template + constexpr T length() const noexcept; + constexpr T length_sq() const noexcept; + + + T x = T(0); + T y = T(0); + T z = T(0); + T w = T(0); +}; + +template constexpr math::vec4 operator*(T s, math::vec4 const&) noexcept; +template constexpr math::vec4 operator/(T s, math::vec4 const&) noexcept; +template constexpr math::vec4 operator*(math::vec4 const&, math::vec4 const&) noexcept; +template constexpr math::vec4 operator/(math::vec4 const&, math::vec4 const&) noexcept; + +////////////////////////////////////////////////////////////////////////// + +} //math + diff --git a/include/VMath/vec4.inl b/include/VMath/vec4.inl new file mode 100644 index 0000000..ef2eeaf --- /dev/null +++ b/include/VMath/vec4.inl @@ -0,0 +1,276 @@ +namespace math +{ + +template constexpr vec4::vec4(T x, T y, T z, T w) noexcept : x(x), y(y), z(z), w(w) {} +template constexpr vec4::vec4(T v) noexcept : x(v), y(v), z(v), w(v) {} +template constexpr vec4::vec4(std::array const& v) noexcept : x(v[0]), y(v[1]), z(v[2]), w(v[3]) {} + +template +constexpr vec4 vec4::operator-() const noexcept +{ + return vec4(-x, -y, -z, -w); +} + +template +constexpr T& vec4::operator[](size_t i) noexcept +{ + TL_PLAIN_ASSERT(i < sizeof(*this) / sizeof(T)); + + //if you hit this TL_PLAIN_ASSERT, your compiler introduces padding. Check for #pragma packs without pop + TL_PLAIN_ASSERT(&((T*)this)[0] == &x && &((T*)this)[1] == &y); + + return ((T*)this)[i]; +} + +template +constexpr T const& vec4::operator[](size_t i) const noexcept +{ + TL_PLAIN_ASSERT(i < sizeof(*this) / sizeof(T)); + + //if you hit this TL_PLAIN_ASSERT, your compiler introduces padding. Check for #pragma packs without pop + TL_PLAIN_ASSERT(&((T*)this)[0] == &x && &((T*)this)[1] == &y); + + return ((T*)this)[i]; +} + +template +constexpr vec4 vec4::operator+(T s) const noexcept +{ + return vec4(x + s, y + s, z + s, w + s); +} +template +constexpr vec4 vec4::operator+(vec4 const& v) const noexcept +{ + return vec4(x + v.x, y + v.y, z + v.z, w + v.w); +} + +template +constexpr vec4 vec4::operator-(T s) const noexcept +{ + return vec4(x - s, y - s, z - s, w - s); +} +template +constexpr vec4 vec4::operator-(vec4 const& v) const noexcept +{ + return vec4(x - v.x, y - v.y, z - v.z, w - v.w); +} + +template +constexpr vec4 vec4::operator*(T s) const noexcept +{ + return vec4(x * s, y * s, z * s, w * s); +} + +template +constexpr vec4 vec4::operator/(T s) const noexcept +{ + TL_PLAIN_ASSERT(!is_zero(s)); + if constexpr (std::is_floating_point_v) + { + T is = T(1) / s; + return vec4(x * is, y * is, z * is, w * is); + } + else + return vec4(x / s, y / s, z / s, w / s); +} + +template +constexpr vec4& vec4::operator+=(T s) noexcept +{ + x += s; + y += s; + z += s; + w += s; + return *this; +} +template +constexpr vec4& vec4::operator+=(vec4 const& v) noexcept +{ + x += v.x; + y += v.y; + z += v.z; + w += v.w; + return *this; +} + +template +constexpr vec4& vec4::operator-=(T s) noexcept +{ + x -= s; + y -= s; + z -= s; + w -= s; + return *this; +} +template +constexpr vec4& vec4::operator-=(vec4 const& v) noexcept +{ + x -= v.x; + y -= v.y; + z -= v.z; + w -= v.w; + return *this; +} + +template +constexpr vec4& vec4::operator*=(T s) noexcept +{ + x *= s; + y *= s; + z *= s; + w *= s; + return *this; +} +template +constexpr vec4& vec4::operator*=(vec4 const& v) noexcept +{ + x *= v.x; + y *= v.y; + z *= v.z; + w *= v.w; + return *this; +} + +template +constexpr vec4& vec4::operator/=(T s) noexcept +{ + TL_PLAIN_ASSERT(!is_zero(s)); + if constexpr (std::is_floating_point_v) + { + T is = T(1) / s; + x *= is; + y *= is; + z *= is; + w *= is; + } + else + { + x /= s; + y /= s; + z /= s; + w /= s; + } + return *this; +} + +template +constexpr vec4& vec4::operator/=(vec4 const& v) noexcept +{ + TL_PLAIN_ASSERT(!cwise::any(cwise::is_zero(v))); + x /= v.x; + y /= v.y; + z /= v.z; + w /= v.w; + return *this; +} + +template +constexpr vec4 operator*(T s, vec4 const& v) noexcept +{ + return v*s; +} +template +constexpr vec4 operator/(T s, vec4 const& v) noexcept +{ + return vec4(s) / v; +} + +template +constexpr vec4 operator*(vec4 const& v0, vec4 const& v1) noexcept +{ + return vec4(v0.x * v1.x, v0.y * v1.y, v0.z * v1.z, v0.w * v1.w); +} + +template +constexpr vec4 operator/(vec4 const& u, vec4 const& v) noexcept +{ + TL_PLAIN_ASSERT(!cwise::any(cwise::is_zero(v))); + return vec4(u.x / v.x, u.y / v.y, u.z / v.z, u.w / v.w); +} + +template +template +constexpr vec4& vec4::normalize() noexcept +{ + const T len_sq = x * x + y * y + z * z + w * w; + const T len_inv = inv_sqrt(len_sq); + x *= len_inv; + y *= len_inv; + z *= len_inv; + w *= len_inv; + return *this; +} +template +template +constexpr T vec4::length() const noexcept +{ + return sqrt(x * x + y * y + z * z + w * w); +} +template +constexpr T vec4::length_sq() const noexcept +{ + return x * x + y * y + z * z + w * w; +} + +} //math + + +#include "tl/hash_and_combine.h" + +namespace eastl +{ +template +struct hash> +{ + std::size_t operator()(const math::vec4& p) const + { + size_t seed = 0; + tl::hash_and_combine(seed, p.x); + tl::hash_and_combine(seed, p.y); + tl::hash_and_combine(seed, p.z); + tl::hash_and_combine(seed, p.w); + return seed; + } +}; + +template +class numeric_limits> +{ +public: + typedef math::vec4 value_type; + + static EA_CONSTEXPR_OR_CONST bool is_specialized = true; + static EA_CONSTEXPR_OR_CONST int digits = tl::numeric_limits::digits; + static EA_CONSTEXPR_OR_CONST int digits10 = tl::numeric_limits::digits10; + static EA_CONSTEXPR_OR_CONST int max_digits10 = tl::numeric_limits::max_digits10; + static EA_CONSTEXPR_OR_CONST bool is_signed = tl::numeric_limits::is_signed; + static EA_CONSTEXPR_OR_CONST bool is_integer = tl::numeric_limits::is_integer; + static EA_CONSTEXPR_OR_CONST bool is_exact = tl::numeric_limits::is_exact; + static EA_CONSTEXPR_OR_CONST int radix = tl::numeric_limits::radix; + static EA_CONSTEXPR_OR_CONST int min_exponent = tl::numeric_limits::min_exponent; + static EA_CONSTEXPR_OR_CONST int min_exponent10 = tl::numeric_limits::min_exponent10; + static EA_CONSTEXPR_OR_CONST int max_exponent = tl::numeric_limits::max_exponent; + static EA_CONSTEXPR_OR_CONST int max_exponent10 = tl::numeric_limits::max_exponent10; + static EA_CONSTEXPR_OR_CONST bool is_bounded = tl::numeric_limits::is_bounded; + static EA_CONSTEXPR_OR_CONST bool is_modulo = tl::numeric_limits::is_modulo; + static EA_CONSTEXPR_OR_CONST bool traps = tl::numeric_limits::traps; + static EA_CONSTEXPR_OR_CONST bool tinyness_before = tl::numeric_limits::tinyness_before; + static EA_CONSTEXPR_OR_CONST float_round_style round_style = tl::numeric_limits::round_style; + static EA_CONSTEXPR_OR_CONST bool has_infinity = tl::numeric_limits::has_infinity; + static EA_CONSTEXPR_OR_CONST bool has_quiet_NaN = tl::numeric_limits::has_quiet_NaN; + static EA_CONSTEXPR_OR_CONST bool has_signaling_NaN = tl::numeric_limits::has_signaling_NaN; + static EA_CONSTEXPR_OR_CONST float_denorm_style has_denorm = tl::numeric_limits::has_denorm; + static EA_CONSTEXPR_OR_CONST bool has_denorm_loss = tl::numeric_limits::has_denorm_loss; + static EA_CONSTEXPR_OR_CONST bool is_iec559 = tl::numeric_limits::is_iec559; + + static value_type min() noexcept { return value_type(tl::numeric_limits::min()); } + static value_type max() noexcept { return value_type(tl::numeric_limits::max()); } + static value_type lowest() noexcept { return value_type(tl::numeric_limits::lowest()); } + static value_type epsilon() noexcept { return value_type(tl::numeric_limits::epsilon()); } + static value_type round_error() noexcept { return value_type(tl::numeric_limits::round_error()); } + static value_type infinity() noexcept { return value_type(tl::numeric_limits::infinity()); } + static value_type quiet_NaN() noexcept { return value_type(tl::numeric_limits::quiet_NaN()); } + static value_type signaling_NaN() noexcept { return value_type(tl::numeric_limits::signaling_NaN()); } + static value_type denorm_min() noexcept { return value_type(tl::numeric_limits::denorm_min()); } +}; +} diff --git a/src/data.cpp b/src/data.cpp new file mode 100644 index 0000000..fce19d8 --- /dev/null +++ b/src/data.cpp @@ -0,0 +1,122 @@ +#include "VMath.h" + +namespace math +{ +namespace detail +{ + +// for (int i = 0; i < 806; i++) +// { +// //THERE are 805 elements for 0 .. pi/2 +// float a = math::clamp((float(i) / 804.f) * constantsf::pi2, 0.f, constantsf::pi2); +// float _sin = std::sin(a) + 1; +// float _cos = std::cos(a) + 1; +// +// uint32_t _cosx = *reinterpret_cast(&_cos) >> 7; +// if (_cos >= 2.f) +// { +// _cosx = 0xFFFFu; +// } +// uint32_t _sinx = *reinterpret_cast(&_sin) >> 7; +// if (_sin >= 2.f) +// { +// _sinx = 0xFFFFu; +// } +// +// if (i % 10 == 0) +// { +// printf("\n"); +// } +// +// printf("0x%04x%04x, ", _sinx & 0xFFFF, _cosx & 0xFFFF); +// } + + //the last element is to allow interpolating without checking for out of bounds + const uint32_t k_sin_cos[806] = + { + 0x0000ffff, 0x0080ffff, 0x0100ffff, 0x0180fffe, 0x0200fffe, 0x0280fffc, 0x0300fffb, 0x0380fff9, 0x0400fff8, 0x0480fff5, + 0x0500fff3, 0x0580fff0, 0x0600ffed, 0x0680ffea, 0x0700ffe7, 0x0780ffe3, 0x0800ffdf, 0x0880ffdb, 0x0900ffd7, 0x0980ffd2, + 0x0a00ffcd, 0x0a80ffc8, 0x0b00ffc3, 0x0b7fffbd, 0x0bffffb7, 0x0c7fffb1, 0x0cffffab, 0x0d7fffa4, 0x0dffff9d, 0x0e7fff96, + 0x0efeff8f, 0x0f7eff87, 0x0ffeff7f, 0x107eff77, 0x10feff6f, 0x117dff66, 0x11fdff5d, 0x127dff54, 0x12fdff4b, 0x137cff41, + 0x13fcff37, 0x147cff2d, 0x14fbff23, 0x157bff18, 0x15faff0e, 0x167aff02, 0x16f9fef7, 0x1779feeb, 0x17f8fee0, 0x1878fed3, + 0x18f7fec7, 0x1977feba, 0x19f6feae, 0x1a75fea0, 0x1af5fe93, 0x1b74fe86, 0x1bf3fe78, 0x1c73fe6a, 0x1cf2fe5b, 0x1d71fe4d, + 0x1df0fe3e, 0x1e6ffe2f, 0x1eeffe1f, 0x1f6efe10, 0x1fedfe00, 0x206cfdf0, 0x20ebfddf, 0x216afdcf, 0x21e9fdbe, 0x2267fdad, + 0x22e6fd9c, 0x2365fd8a, 0x23e4fd78, 0x2463fd66, 0x24e1fd54, 0x2560fd41, 0x25dffd2e, 0x265dfd1b, 0x26dcfd08, 0x275bfcf4, + 0x27d9fce1, 0x2857fccd, 0x28d6fcb8, 0x2954fca4, 0x29d3fc8f, 0x2a51fc7a, 0x2acffc65, 0x2b4dfc4f, 0x2bccfc39, 0x2c4afc23, + 0x2cc8fc0d, 0x2d46fbf6, 0x2dc4fbe0, 0x2e42fbc9, 0x2ec0fbb1, 0x2f3efb9a, 0x2fbbfb82, 0x3039fb6a, 0x30b7fb52, 0x3135fb39, + 0x31b2fb21, 0x3230fb08, 0x32adfaef, 0x332bfad5, 0x33a8fabb, 0x3426faa1, 0x34a3fa87, 0x3520fa6d, 0x359dfa52, 0x361bfa37, + 0x3698fa1c, 0x3715fa00, 0x3792f9e5, 0x380ff9c9, 0x388cf9ad, 0x3908f990, 0x3985f974, 0x3a02f957, 0x3a7ff93a, 0x3afbf91c, + 0x3b78f8ff, 0x3bf4f8e1, 0x3c71f8c3, 0x3cedf8a4, 0x3d6af886, 0x3de6f867, 0x3e62f848, 0x3edef828, 0x3f5af809, 0x3fd6f7e9, + 0x4052f7c9, 0x40cef7a9, 0x414af788, 0x41c6f767, 0x4241f746, 0x42bdf725, 0x4339f704, 0x43b4f6e2, 0x4430f6c0, 0x44abf69e, + 0x4526f67b, 0x45a2f659, 0x461df636, 0x4698f612, 0x4713f5ef, 0x478ef5cb, 0x4809f5a7, 0x4884f583, 0x48fef55f, 0x4979f53a, + 0x49f4f515, 0x4a6ef4f0, 0x4ae9f4cb, 0x4b63f4a5, 0x4bddf47f, 0x4c58f459, 0x4cd2f433, 0x4d4cf40d, 0x4dc6f3e6, 0x4e40f3bf, + 0x4ebaf398, 0x4f34f370, 0x4fadf348, 0x5027f320, 0x50a1f2f8, 0x511af2d0, 0x5193f2a7, 0x520df27e, 0x5286f255, 0x52fff22c, + 0x5378f202, 0x53f1f1d8, 0x546af1ae, 0x54e3f184, 0x555cf159, 0x55d4f12e, 0x564df103, 0x56c5f0d8, 0x573ef0ac, 0x57b6f081, + 0x582ef055, 0x58a7f028, 0x591feffc, 0x5997efcf, 0x5a0fefa2, 0x5a86ef75, 0x5afeef48, 0x5b76ef1a, 0x5bedeeec, 0x5c65eebe, + 0x5cdcee90, 0x5d53ee61, 0x5dcbee32, 0x5e42ee03, 0x5eb9edd4, 0x5f30eda5, 0x5fa6ed75, 0x601ded45, 0x6094ed15, 0x610aece4, + 0x6181ecb4, 0x61f7ec83, 0x626dec52, 0x62e3ec20, 0x6359ebef, 0x63cfebbd, 0x6445eb8b, 0x64bbeb59, 0x6531eb26, 0x65a6eaf4, + 0x661ceac1, 0x6691ea8d, 0x6706ea5a, 0x677cea26, 0x67f1e9f2, 0x6866e9be, 0x68dae98a, 0x694fe955, 0x69c4e921, 0x6a38e8ec, + 0x6aade8b6, 0x6b21e881, 0x6b95e84b, 0x6c0ae815, 0x6c7ee7df, 0x6cf1e7a9, 0x6d65e772, 0x6dd9e73b, 0x6e4de704, 0x6ec0e6cd, + 0x6f33e696, 0x6fa7e65e, 0x701ae626, 0x708de5ee, 0x7100e5b5, 0x7173e57d, 0x71e5e544, 0x7258e50b, 0x72cbe4d1, 0x733de498, + 0x73afe45e, 0x7421e424, 0x7493e3ea, 0x7505e3b0, 0x7577e375, 0x75e9e33a, 0x765be2ff, 0x76cce2c4, 0x773de288, 0x77afe24c, + 0x7820e210, 0x7891e1d4, 0x7902e198, 0x7972e15b, 0x79e3e11e, 0x7a54e0e1, 0x7ac4e0a4, 0x7b34e066, 0x7ba4e029, 0x7c15dfeb, + 0x7c84dfad, 0x7cf4df6e, 0x7d64df30, 0x7dd4def1, 0x7e43deb2, 0x7eb2de72, 0x7f21de33, 0x7f91ddf3, 0x8000ddb3, 0x806edd73, + 0x80dddd33, 0x814cdcf2, 0x81badcb2, 0x8228dc71, 0x8297dc2f, 0x8305dbee, 0x8373dbac, 0x83e0db6a, 0x844edb28, 0x84bcdae6, + 0x8529daa4, 0x8596da61, 0x8603da1e, 0x8670d9db, 0x86ddd997, 0x874ad954, 0x87b7d910, 0x8823d8cc, 0x8890d888, 0x88fcd844, + 0x8968d7ff, 0x89d4d7ba, 0x8a40d775, 0x8aabd730, 0x8b17d6ea, 0x8b82d6a5, 0x8beed65f, 0x8c59d619, 0x8cc4d5d2, 0x8d2fd58c, + 0x8d9ad545, 0x8e04d4fe, 0x8e6fd4b7, 0x8ed9d470, 0x8f43d428, 0x8fadd3e0, 0x9017d398, 0x9081d350, 0x90ead308, 0x9154d2bf, + 0x91bdd277, 0x9226d22e, 0x928fd1e4, 0x92f8d19b, 0x9361d151, 0x93cad108, 0x9432d0be, 0x949bd073, 0x9503d029, 0x956bcfde, + 0x95d3cf93, 0x963acf48, 0x96a2cefd, 0x9709ceb2, 0x9771ce66, 0x97d8ce1a, 0x983fcdce, 0x98a6cd82, 0x990dcd35, 0x9973cce9, + 0x99d9cc9c, 0x9a40cc4f, 0x9aa6cc02, 0x9b0ccbb4, 0x9b72cb67, 0x9bd7cb19, 0x9c3dcacb, 0x9ca2ca7c, 0x9d07ca2e, 0x9d6cc9df, + 0x9dd1c991, 0x9e36c941, 0x9e9bc8f2, 0x9effc8a3, 0x9f63c853, 0x9fc7c803, 0xa02bc7b3, 0xa08fc763, 0xa0f3c713, 0xa156c6c2, + 0xa1bac671, 0xa21dc620, 0xa280c5cf, 0xa2e3c57e, 0xa345c52c, 0xa3a8c4db, 0xa40ac489, 0xa46dc436, 0xa4cfc3e4, 0xa530c392, + 0xa592c33f, 0xa5f4c2ec, 0xa655c299, 0xa6b6c246, 0xa718c1f2, 0xa778c19e, 0xa7d9c14b, 0xa83ac0f6, 0xa89ac0a2, 0xa8fbc04e, + 0xa95bbff9, 0xa9bbbfa4, 0xaa1abf4f, 0xaa7abefa, 0xaad9bea5, 0xab39be4f, 0xab98bdfa, 0xabf7bda4, 0xac55bd4e, 0xacb4bcf7, + 0xad12bca1, 0xad71bc4a, 0xadcfbbf3, 0xae2dbb9c, 0xae8abb45, 0xaee8baee, 0xaf45ba96, 0xafa3ba3e, 0xb000b9e6, 0xb05db98e, + 0xb0b9b936, 0xb116b8de, 0xb172b885, 0xb1ceb82c, 0xb22bb7d3, 0xb286b77a, 0xb2e2b720, 0xb33eb6c7, 0xb399b66d, 0xb3f4b613, + 0xb44fb5b9, 0xb4aab55f, 0xb504b504, 0xb55fb4aa, 0xb5b9b44f, 0xb613b3f4, 0xb66db399, 0xb6c7b33e, 0xb720b2e2, 0xb77ab286, + 0xb7d3b22b, 0xb82cb1ce, 0xb885b172, 0xb8deb116, 0xb936b0b9, 0xb98eb05d, 0xb9e6b000, 0xba3eafa3, 0xba96af45, 0xbaeeaee8, + 0xbb45ae8a, 0xbb9cae2d, 0xbbf3adcf, 0xbc4aad71, 0xbca1ad12, 0xbcf7acb4, 0xbd4eac55, 0xbda4abf7, 0xbdfaab98, 0xbe4fab39, + 0xbea5aad9, 0xbefaaa7a, 0xbf4faa1a, 0xbfa4a9bb, 0xbff9a95b, 0xc04ea8fb, 0xc0a2a89a, 0xc0f7a83a, 0xc14ba7d9, 0xc19ea778, + 0xc1f2a718, 0xc246a6b6, 0xc299a655, 0xc2eca5f4, 0xc33fa592, 0xc392a530, 0xc3e4a4cf, 0xc436a46d, 0xc489a40a, 0xc4dba3a8, + 0xc52ca345, 0xc57ea2e3, 0xc5cfa280, 0xc620a21d, 0xc671a1ba, 0xc6c2a156, 0xc713a0f3, 0xc763a08f, 0xc7b3a02b, 0xc8039fc7, + 0xc8539f63, 0xc8a39eff, 0xc8f29e9b, 0xc9419e36, 0xc9919dd1, 0xc9df9d6c, 0xca2e9d07, 0xca7c9ca2, 0xcacb9c3d, 0xcb199bd7, + 0xcb679b72, 0xcbb49b0c, 0xcc029aa6, 0xcc4f9a40, 0xcc9c99d9, 0xcce99973, 0xcd35990d, 0xcd8298a6, 0xcdce983f, 0xce1a97d8, + 0xce669771, 0xceb29709, 0xcefd96a2, 0xcf48963a, 0xcf9395d3, 0xcfde956b, 0xd0299503, 0xd073949b, 0xd0be9432, 0xd10893ca, + 0xd1519361, 0xd19b92f8, 0xd1e4928f, 0xd22e9226, 0xd27791bd, 0xd2bf9154, 0xd30890ea, 0xd3509081, 0xd3989017, 0xd3e08fad, + 0xd4288f43, 0xd4708ed9, 0xd4b78e6f, 0xd4fe8e04, 0xd5458d9a, 0xd58c8d2f, 0xd5d28cc4, 0xd6198c59, 0xd65f8bee, 0xd6a58b82, + 0xd6ea8b17, 0xd7308aab, 0xd7758a40, 0xd7ba89d4, 0xd7ff8968, 0xd84488fc, 0xd8888890, 0xd8cc8823, 0xd91087b7, 0xd954874a, + 0xd99786dd, 0xd9db8670, 0xda1e8603, 0xda618596, 0xdaa48529, 0xdae684bc, 0xdb28844e, 0xdb6a83e0, 0xdbac8373, 0xdbee8305, + 0xdc2f8297, 0xdc718228, 0xdcb281ba, 0xdcf2814c, 0xdd3380dd, 0xdd73806e, 0xddb38000, 0xddf37f91, 0xde337f21, 0xde727eb2, + 0xdeb27e43, 0xdef17dd4, 0xdf307d64, 0xdf6e7cf4, 0xdfad7c84, 0xdfeb7c15, 0xe0297ba4, 0xe0667b34, 0xe0a47ac4, 0xe0e17a54, + 0xe11e79e3, 0xe15b7972, 0xe1987902, 0xe1d47891, 0xe2107820, 0xe24c77af, 0xe288773d, 0xe2c476cc, 0xe2ff765b, 0xe33a75e9, + 0xe3757577, 0xe3b07505, 0xe3ea7493, 0xe4247421, 0xe45e73af, 0xe498733d, 0xe4d172cb, 0xe50b7258, 0xe54471e5, 0xe57d7173, + 0xe5b57100, 0xe5ee708d, 0xe626701a, 0xe65e6fa7, 0xe6966f33, 0xe6cd6ec0, 0xe7046e4d, 0xe73b6dd9, 0xe7726d65, 0xe7a96cf1, + 0xe7df6c7e, 0xe8156c0a, 0xe84b6b95, 0xe8816b21, 0xe8b66aad, 0xe8ec6a38, 0xe92169c4, 0xe955694f, 0xe98a68da, 0xe9be6866, + 0xe9f267f1, 0xea26677c, 0xea5a6706, 0xea8d6691, 0xeac1661c, 0xeaf465a6, 0xeb266531, 0xeb5964bb, 0xeb8b6445, 0xebbd63cf, + 0xebef6359, 0xec2062e3, 0xec52626d, 0xec8361f7, 0xecb46181, 0xece4610a, 0xed156094, 0xed45601d, 0xed755fa6, 0xeda55f30, + 0xedd45eb9, 0xee035e42, 0xee325dcb, 0xee615d53, 0xee905cdc, 0xeebe5c65, 0xeeec5bed, 0xef1a5b76, 0xef485afe, 0xef755a86, + 0xefa25a0f, 0xefcf5997, 0xeffc591f, 0xf02858a7, 0xf055582e, 0xf08157b6, 0xf0ac573e, 0xf0d856c5, 0xf103564d, 0xf12e55d4, + 0xf159555c, 0xf18454e3, 0xf1ae546a, 0xf1d853f1, 0xf2025378, 0xf22c52ff, 0xf2555286, 0xf27e520d, 0xf2a75193, 0xf2d0511a, + 0xf2f850a1, 0xf3205027, 0xf3484fad, 0xf3704f34, 0xf3984eba, 0xf3bf4e40, 0xf3e64dc6, 0xf40d4d4c, 0xf4334cd2, 0xf4594c58, + 0xf47f4bdd, 0xf4a54b63, 0xf4cb4ae9, 0xf4f04a6e, 0xf51549f4, 0xf53a4979, 0xf55f48fe, 0xf5834884, 0xf5a74809, 0xf5cb478e, + 0xf5ef4713, 0xf6124698, 0xf636461d, 0xf65945a2, 0xf67b4526, 0xf69e44ab, 0xf6c04430, 0xf6e243b4, 0xf7044339, 0xf72542bd, + 0xf7464241, 0xf76741c6, 0xf788414a, 0xf7a940ce, 0xf7c94052, 0xf7e93fd6, 0xf8093f5a, 0xf8283ede, 0xf8483e62, 0xf8673de6, + 0xf8863d6a, 0xf8a43ced, 0xf8c33c71, 0xf8e13bf4, 0xf8ff3b78, 0xf91c3afb, 0xf93a3a7f, 0xf9573a02, 0xf9743985, 0xf9903908, + 0xf9ad388c, 0xf9c9380f, 0xf9e53792, 0xfa003715, 0xfa1c3698, 0xfa37361b, 0xfa52359d, 0xfa6d3520, 0xfa8734a3, 0xfaa13426, + 0xfabb33a8, 0xfad5332b, 0xfaef32ad, 0xfb083230, 0xfb2131b2, 0xfb393135, 0xfb5230b7, 0xfb6a3039, 0xfb822fbb, 0xfb9a2f3e, + 0xfbb12ec0, 0xfbc92e42, 0xfbe02dc4, 0xfbf62d46, 0xfc0d2cc8, 0xfc232c4a, 0xfc392bcc, 0xfc4f2b4d, 0xfc652acf, 0xfc7a2a51, + 0xfc8f29d3, 0xfca42954, 0xfcb828d6, 0xfccd2857, 0xfce127d9, 0xfcf4275b, 0xfd0826dc, 0xfd1b265d, 0xfd2e25df, 0xfd412560, + 0xfd5424e1, 0xfd662463, 0xfd7823e4, 0xfd8a2365, 0xfd9c22e6, 0xfdad2267, 0xfdbe21e9, 0xfdcf216a, 0xfddf20eb, 0xfdf0206c, + 0xfe001fed, 0xfe101f6e, 0xfe1f1eef, 0xfe2f1e6f, 0xfe3e1df0, 0xfe4d1d71, 0xfe5b1cf2, 0xfe6a1c73, 0xfe781bf3, 0xfe861b74, + 0xfe931af5, 0xfea01a75, 0xfeae19f6, 0xfeba1977, 0xfec718f7, 0xfed31878, 0xfee017f8, 0xfeeb1779, 0xfef716f9, 0xff02167a, + 0xff0e15fa, 0xff18157b, 0xff2314fb, 0xff2d147c, 0xff3713fc, 0xff41137c, 0xff4b12fd, 0xff54127d, 0xff5d11fd, 0xff66117d, + 0xff6f10fe, 0xff77107e, 0xff7f0ffe, 0xff870f7e, 0xff8f0efe, 0xff960e7f, 0xff9d0dff, 0xffa40d7f, 0xffab0cff, 0xffb10c7f, + 0xffb70bff, 0xffbd0b7f, 0xffc30b00, 0xffc80a80, 0xffcd0a00, 0xffd20980, 0xffd70900, 0xffdb0880, 0xffdf0800, 0xffe30780, + 0xffe70700, 0xffea0680, 0xffed0600, 0xfff00580, 0xfff30500, 0xfff50480, 0xfff80400, 0xfff90380, 0xfffb0300, 0xfffc0280, + 0xfffe0200, 0xfffe0180, 0xffff0100, 0xffff0080, 0xffff0000, 0xffff0000 + }; + + +} +}