zhh/render
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
73d182135a32810094bc11a1536bc217627b1585
render/core/include/math.hpp
T
zhh 348d9275f6 Init commit
2026-08-17 06:31:43 +09:00

1748 lines
70 KiB
C++
Raw Blame History

#ifndef RENDER_MATH_HPP
#define RENDER_MATH_HPP
// Header-only C++11 rendering math utilities.
// Convention:
// - Row-major storage.
// - Row vectors: transformed = vector * matrix.
// - Col vectors: transformed = matrix * vector.
// - Composition_RowVec order: world * view * projection.
// - Composition_ColVec order: projection * view * world.
// - Translation in Matrix_ColVec is stored in the last column.
// - Translation in Matrix_RowVec is stored in the last row.
// - Direct3D NDC depth range is [0, 1].
// - Functions ending in _lh use a left-handed coordinate system.
//
// SIMD:
// - SSE2 is enabled automatically on x86/x64.
// - Define RENDER_MATH_USE_SIMD to 0 before including this file to disable it.
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <limits>
#include <type_traits>
#ifndef RENDER_MATH_USE_SIMD
# if defined(_M_X64) || defined(__x86_64__) || defined(_M_IX86) || defined(__i386__)
# define RENDER_MATH_USE_SIMD 1
# else
# define RENDER_MATH_USE_SIMD 0
# endif
#endif
#if RENDER_MATH_USE_SIMD
# include <xmmintrin.h>
# include <emmintrin.h>
#endif
#if defined(_MSC_VER)
# define RENDER_MATH_FORCE_INLINE __forceinline
#else
# define RENDER_MATH_FORCE_INLINE inline __attribute__((always_inline))
#endif
namespace math {
static constexpr float pi = 3.14159265358979323846f;
static constexpr float two_pi = 6.28318530717958647692f;
static constexpr float half_pi = 1.57079632679489661923f;
static constexpr float epsilon = 1.0e-6f;
static constexpr bool simd_enabled = (RENDER_MATH_USE_SIMD != 0);
// -----------------------------------------------------------------------------
// Scalar helpers
// -----------------------------------------------------------------------------
template<typename T>
RENDER_MATH_FORCE_INLINE constexpr T minimum(T a, T b) noexcept {
return (a < b) ? a : b;
}
template<typename T>
RENDER_MATH_FORCE_INLINE constexpr T maximum(T a, T b) noexcept {
return (a > b) ? a : b;
}
template<typename T>
RENDER_MATH_FORCE_INLINE constexpr T clamp(T value, T low, T high) noexcept {
return (value < low) ? low : ((value > high) ? high : value);
}
RENDER_MATH_FORCE_INLINE constexpr float radians(float degrees_value) noexcept {
return degrees_value * (pi / 180.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float degrees(float radians_value) noexcept {
return radians_value * (180.0f / pi);
}
RENDER_MATH_FORCE_INLINE constexpr float saturate(float value) noexcept {
return clamp(value, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float lerp(float a, float b, float t) noexcept {
return a + (b - a) * t;
}
RENDER_MATH_FORCE_INLINE float inverse_lerp(float a, float b, float value) noexcept {
const float d = b - a;
return (std::fabs(d) > epsilon) ? ((value - a) / d) : 0.0f;
}
RENDER_MATH_FORCE_INLINE float smoothstep(float edge0, float edge1, float value) noexcept {
const float t = saturate(inverse_lerp(edge0, edge1, value));
return t * t * (3.0f - 2.0f * t);
}
RENDER_MATH_FORCE_INLINE bool nearly_equal(
float a,
float b,
float absolute_epsilon = 1.0e-6f,
float relative_epsilon = 1.0e-5f) noexcept {
const float difference = std::fabs(a - b);
if (difference <= absolute_epsilon) {
return true;
}
const float largest = maximum(std::fabs(a), std::fabs(b));
return difference <= largest * relative_epsilon;
}
RENDER_MATH_FORCE_INLINE float wrap_angle(float angle) noexcept {
angle = std::fmod(angle + pi, two_pi);
if (angle < 0.0f) {
angle += two_pi;
}
return angle - pi;
}
// -----------------------------------------------------------------------------
// Integer vectors
// -----------------------------------------------------------------------------
struct int2 {
int x;
int y;
constexpr int2() noexcept : x(0), y(0) {}
constexpr explicit int2(int value) noexcept : x(value), y(value) {}
constexpr int2(int x_value, int y_value) noexcept : x(x_value), y(y_value) {}
RENDER_MATH_FORCE_INLINE int &operator[](std::size_t index) noexcept {
return (index == 0) ? x : y;
}
RENDER_MATH_FORCE_INLINE const int &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : y;
}
};
struct int3 {
int x;
int y;
int z;
constexpr int3() noexcept : x(0), y(0), z(0) {}
constexpr explicit int3(int value) noexcept : x(value), y(value), z(value) {}
constexpr int3(int x_value, int y_value, int z_value) noexcept
: x(x_value), y(y_value), z(z_value) {}
RENDER_MATH_FORCE_INLINE int &operator[](std::size_t index) noexcept {
return (index == 0) ? x : ((index == 1) ? y : z);
}
RENDER_MATH_FORCE_INLINE const int &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : ((index == 1) ? y : z);
}
};
struct int4 {
int x;
int y;
int z;
int w;
constexpr int4() noexcept : x(0), y(0), z(0), w(0) {}
constexpr explicit int4(int value) noexcept : x(value), y(value), z(value), w(value) {}
constexpr int4(int x_value, int y_value, int z_value, int w_value) noexcept
: x(x_value), y(y_value), z(z_value), w(w_value) {}
RENDER_MATH_FORCE_INLINE int &operator[](std::size_t index) noexcept {
return (index == 0) ? x : ((index == 1) ? y : ((index == 2) ? z : w));
}
RENDER_MATH_FORCE_INLINE const int &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : ((index == 1) ? y : ((index == 2) ? z : w));
}
};
RENDER_MATH_FORCE_INLINE constexpr int2 operator+(int2 a, int2 b) noexcept { return int2(a.x + b.x, a.y + b.y); }
RENDER_MATH_FORCE_INLINE constexpr int2 operator-(int2 a, int2 b) noexcept { return int2(a.x - b.x, a.y - b.y); }
RENDER_MATH_FORCE_INLINE constexpr int2 operator-(int2 v) noexcept { return int2(-v.x, -v.y); }
RENDER_MATH_FORCE_INLINE constexpr int2 operator*(int2 a, int2 b) noexcept { return int2(a.x * b.x, a.y * b.y); }
RENDER_MATH_FORCE_INLINE constexpr int2 operator/(int2 a, int2 b) noexcept { return int2(a.x / b.x, a.y / b.y); }
RENDER_MATH_FORCE_INLINE constexpr int2 operator*(int2 v, int scalar) noexcept { return int2(v.x * scalar, v.y * scalar); }
RENDER_MATH_FORCE_INLINE constexpr int2 operator*(int scalar, int2 v) noexcept { return v * scalar; }
RENDER_MATH_FORCE_INLINE constexpr int2 operator/(int2 v, int scalar) noexcept { return int2(v.x / scalar, v.y / scalar); }
RENDER_MATH_FORCE_INLINE constexpr bool operator==(int2 a, int2 b) noexcept { return a.x == b.x && a.y == b.y; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(int2 a, int2 b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator+(int3 a, int3 b) noexcept { return int3(a.x + b.x, a.y + b.y, a.z + b.z); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator-(int3 a, int3 b) noexcept { return int3(a.x - b.x, a.y - b.y, a.z - b.z); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator-(int3 v) noexcept { return int3(-v.x, -v.y, -v.z); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator*(int3 a, int3 b) noexcept { return int3(a.x * b.x, a.y * b.y, a.z * b.z); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator/(int3 a, int3 b) noexcept { return int3(a.x / b.x, a.y / b.y, a.z / b.z); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator*(int3 v, int scalar) noexcept { return int3(v.x * scalar, v.y * scalar, v.z * scalar); }
RENDER_MATH_FORCE_INLINE constexpr int3 operator*(int scalar, int3 v) noexcept { return v * scalar; }
RENDER_MATH_FORCE_INLINE constexpr int3 operator/(int3 v, int scalar) noexcept { return int3(v.x / scalar, v.y / scalar, v.z / scalar); }
RENDER_MATH_FORCE_INLINE constexpr bool operator==(int3 a, int3 b) noexcept { return a.x == b.x && a.y == b.y && a.z == b.z; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(int3 a, int3 b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator+(int4 a, int4 b) noexcept { return int4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator-(int4 a, int4 b) noexcept { return int4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator-(int4 v) noexcept { return int4(-v.x, -v.y, -v.z, -v.w); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator*(int4 a, int4 b) noexcept { return int4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator/(int4 a, int4 b) noexcept { return int4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator*(int4 v, int scalar) noexcept { return int4(v.x * scalar, v.y * scalar, v.z * scalar, v.w * scalar); }
RENDER_MATH_FORCE_INLINE constexpr int4 operator*(int scalar, int4 v) noexcept { return v * scalar; }
RENDER_MATH_FORCE_INLINE constexpr int4 operator/(int4 v, int scalar) noexcept { return int4(v.x / scalar, v.y / scalar, v.z / scalar, v.w / scalar); }
RENDER_MATH_FORCE_INLINE constexpr bool operator==(int4 a, int4 b) noexcept { return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(int4 a, int4 b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE constexpr std::int64_t dot(int2 a, int2 b) noexcept {
return static_cast<std::int64_t>(a.x) * b.x + static_cast<std::int64_t>(a.y) * b.y;
}
RENDER_MATH_FORCE_INLINE constexpr std::int64_t dot(int3 a, int3 b) noexcept {
return static_cast<std::int64_t>(a.x) * b.x + static_cast<std::int64_t>(a.y) * b.y + static_cast<std::int64_t>(a.z) * b.z;
}
RENDER_MATH_FORCE_INLINE constexpr std::int64_t dot(int4 a, int4 b) noexcept {
return static_cast<std::int64_t>(a.x) * b.x + static_cast<std::int64_t>(a.y) * b.y +
static_cast<std::int64_t>(a.z) * b.z + static_cast<std::int64_t>(a.w) * b.w;
}
RENDER_MATH_FORCE_INLINE constexpr int2 component_min(int2 a, int2 b) noexcept { return int2(minimum(a.x, b.x), minimum(a.y, b.y)); }
RENDER_MATH_FORCE_INLINE constexpr int2 component_max(int2 a, int2 b) noexcept { return int2(maximum(a.x, b.x), maximum(a.y, b.y)); }
RENDER_MATH_FORCE_INLINE constexpr int3 component_min(int3 a, int3 b) noexcept { return int3(minimum(a.x, b.x), minimum(a.y, b.y), minimum(a.z, b.z)); }
RENDER_MATH_FORCE_INLINE constexpr int3 component_max(int3 a, int3 b) noexcept { return int3(maximum(a.x, b.x), maximum(a.y, b.y), maximum(a.z, b.z)); }
RENDER_MATH_FORCE_INLINE constexpr int4 component_min(int4 a, int4 b) noexcept { return int4(minimum(a.x, b.x), minimum(a.y, b.y), minimum(a.z, b.z), minimum(a.w, b.w)); }
RENDER_MATH_FORCE_INLINE constexpr int4 component_max(int4 a, int4 b) noexcept { return int4(maximum(a.x, b.x), maximum(a.y, b.y), maximum(a.z, b.z), maximum(a.w, b.w)); }
RENDER_MATH_FORCE_INLINE constexpr int2 clamp(int2 value, int2 low, int2 high) noexcept {
return component_min(component_max(value, low), high);
}
RENDER_MATH_FORCE_INLINE constexpr int3 clamp(int3 value, int3 low, int3 high) noexcept {
return component_min(component_max(value, low), high);
}
RENDER_MATH_FORCE_INLINE constexpr int4 clamp(int4 value, int4 low, int4 high) noexcept {
return component_min(component_max(value, low), high);
}
// -----------------------------------------------------------------------------
// Floating-point vectors
// -----------------------------------------------------------------------------
struct float2 {
float x;
float y;
constexpr float2() noexcept : x(0.0f), y(0.0f) {}
constexpr explicit float2(float value) noexcept : x(value), y(value) {}
constexpr float2(float x_value, float y_value) noexcept : x(x_value), y(y_value) {}
RENDER_MATH_FORCE_INLINE float &operator[](std::size_t index) noexcept {
return (index == 0) ? x : y;
}
RENDER_MATH_FORCE_INLINE const float &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : y;
}
};
struct float3 {
float x;
float y;
float z;
constexpr float3() noexcept : x(0.0f), y(0.0f), z(0.0f) {}
constexpr explicit float3(float value) noexcept : x(value), y(value), z(value) {}
constexpr float3(float x_value, float y_value, float z_value) noexcept
: x(x_value), y(y_value), z(z_value) {}
constexpr float3(float2 xy, float z_value) noexcept : x(xy.x), y(xy.y), z(z_value) {}
RENDER_MATH_FORCE_INLINE float &operator[](std::size_t index) noexcept {
return (index == 0) ? x : ((index == 1) ? y : z);
}
RENDER_MATH_FORCE_INLINE const float &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : ((index == 1) ? y : z);
}
};
struct alignas(16) float4 {
float x;
float y;
float z;
float w;
constexpr float4() noexcept : x(0.0f), y(0.0f), z(0.0f), w(0.0f) {}
constexpr explicit float4(float value) noexcept : x(value), y(value), z(value), w(value) {}
constexpr float4(float x_value, float y_value, float z_value, float w_value) noexcept
: x(x_value), y(y_value), z(z_value), w(w_value) {}
constexpr float4(float3 xyz, float w_value) noexcept : x(xyz.x), y(xyz.y), z(xyz.z), w(w_value) {}
constexpr float4(float2 xy, float z_value, float w_value) noexcept : x(xy.x), y(xy.y), z(z_value), w(w_value) {}
RENDER_MATH_FORCE_INLINE float &operator[](std::size_t index) noexcept {
return (index == 0) ? x : ((index == 1) ? y : ((index == 2) ? z : w));
}
RENDER_MATH_FORCE_INLINE const float &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : ((index == 1) ? y : ((index == 2) ? z : w));
}
};
namespace detail {
#if RENDER_MATH_USE_SIMD
RENDER_MATH_FORCE_INLINE __m128 load(float4 value) noexcept {
return _mm_loadu_ps(&value.x);
}
RENDER_MATH_FORCE_INLINE float4 store_float4(__m128 value) noexcept {
float4 result;
_mm_storeu_ps(&result.x, value);
return result;
}
RENDER_MATH_FORCE_INLINE float lane(__m128 value, int index) noexcept {
switch (index) {
case 0: return _mm_cvtss_f32(value);
case 1: return _mm_cvtss_f32(_mm_shuffle_ps(value, value, _MM_SHUFFLE(1, 1, 1, 1)));
case 2: return _mm_cvtss_f32(_mm_shuffle_ps(value, value, _MM_SHUFFLE(2, 2, 2, 2)));
default: return _mm_cvtss_f32(_mm_shuffle_ps(value, value, _MM_SHUFFLE(3, 3, 3, 3)));
}
}
#endif
} // namespace detail
RENDER_MATH_FORCE_INLINE constexpr float2 operator+(float2 a, float2 b) noexcept { return float2(a.x + b.x, a.y + b.y); }
RENDER_MATH_FORCE_INLINE constexpr float2 operator-(float2 a, float2 b) noexcept { return float2(a.x - b.x, a.y - b.y); }
RENDER_MATH_FORCE_INLINE constexpr float2 operator-(float2 v) noexcept { return float2(-v.x, -v.y); }
RENDER_MATH_FORCE_INLINE constexpr float2 operator*(float2 a, float2 b) noexcept { return float2(a.x * b.x, a.y * b.y); }
RENDER_MATH_FORCE_INLINE constexpr float2 operator/(float2 a, float2 b) noexcept { return float2(a.x / b.x, a.y / b.y); }
RENDER_MATH_FORCE_INLINE constexpr float2 operator*(float2 v, float scalar) noexcept { return float2(v.x * scalar, v.y * scalar); }
RENDER_MATH_FORCE_INLINE constexpr float2 operator*(float scalar, float2 v) noexcept { return v * scalar; }
RENDER_MATH_FORCE_INLINE constexpr float2 operator/(float2 v, float scalar) noexcept { return float2(v.x / scalar, v.y / scalar); }
RENDER_MATH_FORCE_INLINE constexpr bool operator==(float2 a, float2 b) noexcept { return a.x == b.x && a.y == b.y; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(float2 a, float2 b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator+(float3 a, float3 b) noexcept { return float3(a.x + b.x, a.y + b.y, a.z + b.z); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator-(float3 a, float3 b) noexcept { return float3(a.x - b.x, a.y - b.y, a.z - b.z); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator-(float3 v) noexcept { return float3(-v.x, -v.y, -v.z); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator*(float3 a, float3 b) noexcept { return float3(a.x * b.x, a.y * b.y, a.z * b.z); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator/(float3 a, float3 b) noexcept { return float3(a.x / b.x, a.y / b.y, a.z / b.z); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator*(float3 v, float scalar) noexcept { return float3(v.x * scalar, v.y * scalar, v.z * scalar); }
RENDER_MATH_FORCE_INLINE constexpr float3 operator*(float scalar, float3 v) noexcept { return v * scalar; }
RENDER_MATH_FORCE_INLINE constexpr float3 operator/(float3 v, float scalar) noexcept { return float3(v.x / scalar, v.y / scalar, v.z / scalar); }
RENDER_MATH_FORCE_INLINE constexpr bool operator==(float3 a, float3 b) noexcept { return a.x == b.x && a.y == b.y && a.z == b.z; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(float3 a, float3 b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE float4 operator+(float4 a, float4 b) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_add_ps(detail::load(a), detail::load(b)));
#else
return float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator-(float4 a, float4 b) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_sub_ps(detail::load(a), detail::load(b)));
#else
return float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator-(float4 v) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_sub_ps(_mm_setzero_ps(), detail::load(v)));
#else
return float4(-v.x, -v.y, -v.z, -v.w);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator*(float4 a, float4 b) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_mul_ps(detail::load(a), detail::load(b)));
#else
return float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator/(float4 a, float4 b) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_div_ps(detail::load(a), detail::load(b)));
#else
return float4(a.x / b.x, a.y / b.y, a.z / b.z, a.w / b.w);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator*(float4 v, float scalar) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_mul_ps(detail::load(v), _mm_set1_ps(scalar)));
#else
return float4(v.x * scalar, v.y * scalar, v.z * scalar, v.w * scalar);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator*(float scalar, float4 v) noexcept { return v * scalar; }
RENDER_MATH_FORCE_INLINE float4 operator/(float4 v, float scalar) noexcept {
#if RENDER_MATH_USE_SIMD
return detail::store_float4(_mm_div_ps(detail::load(v), _mm_set1_ps(scalar)));
#else
return float4(v.x / scalar, v.y / scalar, v.z / scalar, v.w / scalar);
#endif
}
RENDER_MATH_FORCE_INLINE constexpr bool operator==(float4 a, float4 b) noexcept { return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(float4 a, float4 b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE float2 &operator+=(float2 &a, float2 b) noexcept {
a = a + b;
return a;
}
RENDER_MATH_FORCE_INLINE float2 &operator-=(float2 &a, float2 b) noexcept {
a = a - b;
return a;
}
RENDER_MATH_FORCE_INLINE float2 &operator*=(float2 &a, float scalar) noexcept {
a = a * scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float2 &operator/=(float2 &a, float scalar) noexcept {
a = a / scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float3 &operator+=(float3 &a, float3 b) noexcept {
a = a + b;
return a;
}
RENDER_MATH_FORCE_INLINE float3 &operator-=(float3 &a, float3 b) noexcept {
a = a - b;
return a;
}
RENDER_MATH_FORCE_INLINE float3 &operator*=(float3 &a, float scalar) noexcept {
a = a * scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float3 &operator/=(float3 &a, float scalar) noexcept {
a = a / scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float4 &operator+=(float4 &a, float4 b) noexcept {
a = a + b;
return a;
}
RENDER_MATH_FORCE_INLINE float4 &operator-=(float4 &a, float4 b) noexcept {
a = a - b;
return a;
}
RENDER_MATH_FORCE_INLINE float4 &operator*=(float4 &a, float scalar) noexcept {
a = a * scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float4 &operator/=(float4 &a, float scalar) noexcept {
a = a / scalar;
return a;
}
RENDER_MATH_FORCE_INLINE constexpr float dot(float2 a, float2 b) noexcept { return a.x * b.x + a.y * b.y; }
RENDER_MATH_FORCE_INLINE constexpr float dot(float3 a, float3 b) noexcept { return a.x * b.x + a.y * b.y + a.z * b.z; }
RENDER_MATH_FORCE_INLINE constexpr float dot(float4 a, float4 b) noexcept { return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; }
RENDER_MATH_FORCE_INLINE constexpr float cross(float2 a, float2 b) noexcept { return a.x * b.y - a.y * b.x; }
RENDER_MATH_FORCE_INLINE constexpr float3 cross(float3 a, float3 b) noexcept {
return float3(a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x);
}
RENDER_MATH_FORCE_INLINE constexpr float length_squared(float2 v) noexcept { return dot(v, v); }
RENDER_MATH_FORCE_INLINE constexpr float length_squared(float3 v) noexcept { return dot(v, v); }
RENDER_MATH_FORCE_INLINE constexpr float length_squared(float4 v) noexcept { return dot(v, v); }
RENDER_MATH_FORCE_INLINE float length(float2 v) noexcept { return std::sqrt(length_squared(v)); }
RENDER_MATH_FORCE_INLINE float length(float3 v) noexcept { return std::sqrt(length_squared(v)); }
RENDER_MATH_FORCE_INLINE float length(float4 v) noexcept { return std::sqrt(length_squared(v)); }
RENDER_MATH_FORCE_INLINE float2 normalize(float2 v, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(v);
return (length_sq > tolerance * tolerance) ? (v / std::sqrt(length_sq)) : float2(0.0f);
}
RENDER_MATH_FORCE_INLINE float3 normalize(float3 v, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(v);
return (length_sq > tolerance * tolerance) ? (v / std::sqrt(length_sq)) : float3(0.0f);
}
RENDER_MATH_FORCE_INLINE float4 normalize(float4 v, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(v);
return (length_sq > tolerance * tolerance) ? (v / std::sqrt(length_sq)) : float4(0.0f);
}
RENDER_MATH_FORCE_INLINE float2 normalize_or(float2 v, float2 fallback, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(v);
return (length_sq > tolerance * tolerance) ? (v / std::sqrt(length_sq)) : fallback;
}
RENDER_MATH_FORCE_INLINE float3 normalize_or(float3 v, float3 fallback, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(v);
return (length_sq > tolerance * tolerance) ? (v / std::sqrt(length_sq)) : fallback;
}
RENDER_MATH_FORCE_INLINE float4 normalize_or(float4 v, float4 fallback, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(v);
return (length_sq > tolerance * tolerance) ? (v / std::sqrt(length_sq)) : fallback;
}
RENDER_MATH_FORCE_INLINE float distance(float2 a, float2 b) noexcept { return length(a - b); }
RENDER_MATH_FORCE_INLINE float distance(float3 a, float3 b) noexcept { return length(a - b); }
RENDER_MATH_FORCE_INLINE float distance(float4 a, float4 b) noexcept { return length(a - b); }
RENDER_MATH_FORCE_INLINE constexpr float2 component_min(float2 a, float2 b) noexcept { return float2(minimum(a.x, b.x), minimum(a.y, b.y)); }
RENDER_MATH_FORCE_INLINE constexpr float2 component_max(float2 a, float2 b) noexcept { return float2(maximum(a.x, b.x), maximum(a.y, b.y)); }
RENDER_MATH_FORCE_INLINE constexpr float3 component_min(float3 a, float3 b) noexcept { return float3(minimum(a.x, b.x), minimum(a.y, b.y), minimum(a.z, b.z)); }
RENDER_MATH_FORCE_INLINE constexpr float3 component_max(float3 a, float3 b) noexcept { return float3(maximum(a.x, b.x), maximum(a.y, b.y), maximum(a.z, b.z)); }
RENDER_MATH_FORCE_INLINE constexpr float4 component_min(float4 a, float4 b) noexcept { return float4(minimum(a.x, b.x), minimum(a.y, b.y), minimum(a.z, b.z), minimum(a.w, b.w)); }
RENDER_MATH_FORCE_INLINE constexpr float4 component_max(float4 a, float4 b) noexcept { return float4(maximum(a.x, b.x), maximum(a.y, b.y), maximum(a.z, b.z), maximum(a.w, b.w)); }
RENDER_MATH_FORCE_INLINE constexpr float2 clamp(float2 value, float2 low, float2 high) noexcept { return component_min(component_max(value, low), high); }
RENDER_MATH_FORCE_INLINE constexpr float3 clamp(float3 value, float3 low, float3 high) noexcept { return component_min(component_max(value, low), high); }
RENDER_MATH_FORCE_INLINE constexpr float4 clamp(float4 value, float4 low, float4 high) noexcept { return component_min(component_max(value, low), high); }
RENDER_MATH_FORCE_INLINE constexpr float2 saturate(float2 value) noexcept { return clamp(value, float2(0.0f), float2(1.0f)); }
RENDER_MATH_FORCE_INLINE constexpr float3 saturate(float3 value) noexcept { return clamp(value, float3(0.0f), float3(1.0f)); }
RENDER_MATH_FORCE_INLINE constexpr float4 saturate(float4 value) noexcept { return clamp(value, float4(0.0f), float4(1.0f)); }
RENDER_MATH_FORCE_INLINE constexpr float2 lerp(float2 a, float2 b, float t) noexcept { return a + (b - a) * t; }
RENDER_MATH_FORCE_INLINE constexpr float3 lerp(float3 a, float3 b, float t) noexcept { return a + (b - a) * t; }
RENDER_MATH_FORCE_INLINE float4 lerp(float4 a, float4 b, float t) noexcept { return a + (b - a) * t; }
RENDER_MATH_FORCE_INLINE constexpr float2 perpendicular(float2 v) noexcept { return float2(-v.y, v.x); }
RENDER_MATH_FORCE_INLINE constexpr float3 reflect(float3 incident, float3 normal) noexcept {
return incident - normal * (2.0f * dot(incident, normal));
}
RENDER_MATH_FORCE_INLINE float3 refract(float3 incident, float3 normal, float eta) noexcept {
const float d = dot(normal, incident);
const float k = 1.0f - eta * eta * (1.0f - d * d);
return (k < 0.0f) ? float3(0.0f) : (incident * eta - normal * (eta * d + std::sqrt(k)));
}
RENDER_MATH_FORCE_INLINE bool nearly_equal(float2 a, float2 b, float abs_eps = 1.0e-6f, float rel_eps = 1.0e-5f) noexcept {
return nearly_equal(a.x, b.x, abs_eps, rel_eps) && nearly_equal(a.y, b.y, abs_eps, rel_eps);
}
RENDER_MATH_FORCE_INLINE bool nearly_equal(float3 a, float3 b, float abs_eps = 1.0e-6f, float rel_eps = 1.0e-5f) noexcept {
return nearly_equal(a.x, b.x, abs_eps, rel_eps) && nearly_equal(a.y, b.y, abs_eps, rel_eps) && nearly_equal(a.z, b.z, abs_eps, rel_eps);
}
RENDER_MATH_FORCE_INLINE bool nearly_equal(float4 a, float4 b, float abs_eps = 1.0e-6f, float rel_eps = 1.0e-5f) noexcept {
return nearly_equal(a.x, b.x, abs_eps, rel_eps) && nearly_equal(a.y, b.y, abs_eps, rel_eps) &&
nearly_equal(a.z, b.z, abs_eps, rel_eps) && nearly_equal(a.w, b.w, abs_eps, rel_eps);
}
RENDER_MATH_FORCE_INLINE constexpr float2 to_float2(int2 value) noexcept { return float2(static_cast<float>(value.x), static_cast<float>(value.y)); }
RENDER_MATH_FORCE_INLINE constexpr float3 to_float3(int3 value) noexcept { return float3(static_cast<float>(value.x), static_cast<float>(value.y), static_cast<float>(value.z)); }
RENDER_MATH_FORCE_INLINE constexpr float4 to_float4(int4 value) noexcept { return float4(static_cast<float>(value.x), static_cast<float>(value.y), static_cast<float>(value.z), static_cast<float>(value.w)); }
RENDER_MATH_FORCE_INLINE int2 to_int2_trunc(float2 value) noexcept { return int2(static_cast<int>(value.x), static_cast<int>(value.y)); }
RENDER_MATH_FORCE_INLINE int3 to_int3_trunc(float3 value) noexcept { return int3(static_cast<int>(value.x), static_cast<int>(value.y), static_cast<int>(value.z)); }
RENDER_MATH_FORCE_INLINE int4 to_int4_trunc(float4 value) noexcept { return int4(static_cast<int>(value.x), static_cast<int>(value.y), static_cast<int>(value.z), static_cast<int>(value.w)); }
RENDER_MATH_FORCE_INLINE int2 to_int2_round(float2 value) noexcept { return int2(static_cast<int>(std::lround(value.x)), static_cast<int>(std::lround(value.y))); }
RENDER_MATH_FORCE_INLINE int3 to_int3_round(float3 value) noexcept { return int3(static_cast<int>(std::lround(value.x)), static_cast<int>(std::lround(value.y)), static_cast<int>(std::lround(value.z))); }
RENDER_MATH_FORCE_INLINE int4 to_int4_round(float4 value) noexcept { return int4(static_cast<int>(std::lround(value.x)), static_cast<int>(std::lround(value.y)), static_cast<int>(std::lround(value.z)), static_cast<int>(std::lround(value.w))); }
// -----------------------------------------------------------------------------
// Quaternion
// -----------------------------------------------------------------------------
struct alignas(16) quat {
float x;
float y;
float z;
float w;
constexpr quat() noexcept : x(0.0f), y(0.0f), z(0.0f), w(1.0f) {}
constexpr quat(float x_value, float y_value, float z_value, float w_value) noexcept
: x(x_value), y(y_value), z(z_value), w(w_value) {}
constexpr quat(float3 vector_part, float scalar_part) noexcept
: x(vector_part.x), y(vector_part.y), z(vector_part.z), w(scalar_part) {}
RENDER_MATH_FORCE_INLINE float &operator[](std::size_t index) noexcept {
return (index == 0) ? x : ((index == 1) ? y : ((index == 2) ? z : w));
}
RENDER_MATH_FORCE_INLINE const float &operator[](std::size_t index) const noexcept {
return (index == 0) ? x : ((index == 1) ? y : ((index == 2) ? z : w));
}
};
RENDER_MATH_FORCE_INLINE constexpr quat quat_identity() noexcept { return quat(0.0f, 0.0f, 0.0f, 1.0f); }
RENDER_MATH_FORCE_INLINE constexpr quat operator+(quat a, quat b) noexcept { return quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); }
RENDER_MATH_FORCE_INLINE constexpr quat operator-(quat a, quat b) noexcept { return quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); }
RENDER_MATH_FORCE_INLINE constexpr quat operator-(quat q) noexcept { return quat(-q.x, -q.y, -q.z, -q.w); }
RENDER_MATH_FORCE_INLINE constexpr quat operator*(quat q, float scalar) noexcept { return quat(q.x * scalar, q.y * scalar, q.z * scalar, q.w * scalar); }
RENDER_MATH_FORCE_INLINE constexpr quat operator*(float scalar, quat q) noexcept { return q * scalar; }
RENDER_MATH_FORCE_INLINE constexpr quat operator/(quat q, float scalar) noexcept { return quat(q.x / scalar, q.y / scalar, q.z / scalar, q.w / scalar); }
RENDER_MATH_FORCE_INLINE constexpr bool operator==(quat a, quat b) noexcept { return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w; }
RENDER_MATH_FORCE_INLINE constexpr bool operator!=(quat a, quat b) noexcept { return !(a == b); }
RENDER_MATH_FORCE_INLINE constexpr float dot(quat a, quat b) noexcept { return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w; }
RENDER_MATH_FORCE_INLINE constexpr float length_squared(quat q) noexcept { return dot(q, q); }
RENDER_MATH_FORCE_INLINE float length(quat q) noexcept { return std::sqrt(length_squared(q)); }
RENDER_MATH_FORCE_INLINE quat normalize(quat q, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(q);
return (length_sq > tolerance * tolerance) ? (q / std::sqrt(length_sq)) : quat_identity();
}
// 返回共轭四元数
RENDER_MATH_FORCE_INLINE constexpr quat conjugate(quat q) noexcept {
return quat(-q.x, -q.y, -q.z, q.w);
}
RENDER_MATH_FORCE_INLINE quat inverse(quat q, float tolerance = epsilon) noexcept {
const float length_sq = length_squared(q);
return (length_sq > tolerance * tolerance) ? (conjugate(q) / length_sq) : quat_identity();
}
// Algebraic Hamilton product: a * b.
// 两个四元数的代数汉密尔顿积
RENDER_MATH_FORCE_INLINE constexpr quat hamilton_product(quat a, quat b) noexcept {
return quat(
a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,
a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,
a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,
a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);
}
// Row-vector composition: apply first, then second.
// 行向量组合,second在先,first在后乘
RENDER_MATH_FORCE_INLINE constexpr quat concatenate(quat first, quat second) noexcept {
return hamilton_product(second, first);
}
RENDER_MATH_FORCE_INLINE quat quaternion_axis_angle(float3 axis, float angle) noexcept {
const float3 n = normalize(axis);
if (length_squared(n) <= epsilon * epsilon) {
return quat_identity();
}
const float half = angle * 0.5f;
const float s = std::sin(half);
return quat(n.x * s, n.y * s, n.z * s, std::cos(half));
}
RENDER_MATH_FORCE_INLINE float3 rotate(float3 vector, quat rotation) noexcept {
const quat q = normalize(rotation);
const float3 qv(q.x, q.y, q.z);
const float3 t = 2.0f * cross(qv, vector);
return vector + q.w * t + cross(qv, t);
}
RENDER_MATH_FORCE_INLINE quat nlerp(quat a, quat b, float t) noexcept {
if (dot(a, b) < 0.0f) {
b = -b;
}
return normalize(a + (b - a) * t);
}
RENDER_MATH_FORCE_INLINE quat slerp(quat a, quat b, float t) noexcept {
a = normalize(a);
b = normalize(b);
float cosine = dot(a, b);
if (cosine < 0.0f) {
b = -b;
cosine = -cosine;
}
cosine = clamp(cosine, -1.0f, 1.0f);
if (cosine > 0.9995f) {
return nlerp(a, b, t);
}
const float angle = std::acos(cosine);
const float sine = std::sin(angle);
if (std::fabs(sine) <= epsilon) {
return a;
}
const float weight_a = std::sin((1.0f - t) * angle) / sine;
const float weight_b = std::sin(t * angle) / sine;
return normalize(a * weight_a + b * weight_b);
}
// -----------------------------------------------------------------------------
// Matrix: row-major storage, row-vector convention
// -----------------------------------------------------------------------------
struct alignas(16) float4x4 {
float m[4][4];
float4x4() noexcept
: m{{0.0f, 0.0f, 0.0f, 0.0f},
{0.0f, 0.0f, 0.0f, 0.0f},
{0.0f, 0.0f, 0.0f, 0.0f},
{0.0f, 0.0f, 0.0f, 0.0f}} {}
constexpr float4x4(
float m00, float m01, float m02, float m03,
float m10, float m11, float m12, float m13,
float m20, float m21, float m22, float m23,
float m30, float m31, float m32, float m33) noexcept
: m{{m00, m01, m02, m03},
{m10, m11, m12, m13},
{m20, m21, m22, m23},
{m30, m31, m32, m33}} {}
constexpr float4x4(float4 row0, float4 row1, float4 row2, float4 row3) noexcept
: m{{row0.x, row0.y, row0.z, row0.w},
{row1.x, row1.y, row1.z, row1.w},
{row2.x, row2.y, row2.z, row2.w},
{row3.x, row3.y, row3.z, row3.w}} {}
RENDER_MATH_FORCE_INLINE float *operator[](std::size_t row_index) noexcept { return m[row_index]; }
RENDER_MATH_FORCE_INLINE const float *operator[](std::size_t row_index) const noexcept { return m[row_index]; }
RENDER_MATH_FORCE_INLINE float &operator()(std::size_t row_index, std::size_t column_index) noexcept { return m[row_index][column_index]; }
RENDER_MATH_FORCE_INLINE const float &operator()(std::size_t row_index, std::size_t column_index) const noexcept { return m[row_index][column_index]; }
RENDER_MATH_FORCE_INLINE float *data() noexcept { return &m[0][0]; }
[[nodiscard]]RENDER_MATH_FORCE_INLINE const float *data() const noexcept { return &m[0][0]; }
};
RENDER_MATH_FORCE_INLINE constexpr float4x4 zero_matrix() noexcept {
return float4x4(
0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f,
0.0f, 0.0f, 0.0f, 0.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 identity() noexcept {
return float4x4(
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float4 row(const float4x4 &matrix, std::size_t index) noexcept {
return float4(matrix.m[index][0], matrix.m[index][1], matrix.m[index][2], matrix.m[index][3]);
}
RENDER_MATH_FORCE_INLINE constexpr float4 column(const float4x4 &matrix, std::size_t index) noexcept {
return float4(matrix.m[0][index], matrix.m[1][index], matrix.m[2][index], matrix.m[3][index]);
}
RENDER_MATH_FORCE_INLINE void set_row(float4x4 &matrix, std::size_t index, float4 value) noexcept {
matrix.m[index][0] = value.x;
matrix.m[index][1] = value.y;
matrix.m[index][2] = value.z;
matrix.m[index][3] = value.w;
}
RENDER_MATH_FORCE_INLINE void set_column(float4x4 &matrix, std::size_t index, float4 value) noexcept {
matrix.m[0][index] = value.x;
matrix.m[1][index] = value.y;
matrix.m[2][index] = value.z;
matrix.m[3][index] = value.w;
}
RENDER_MATH_FORCE_INLINE float4x4 operator+(const float4x4 &a, const float4x4 &b) noexcept {
float4x4 result;
#if RENDER_MATH_USE_SIMD
for (int i = 0; i < 4; ++i) {
_mm_storeu_ps(result.m[i], _mm_add_ps(_mm_loadu_ps(a.m[i]), _mm_loadu_ps(b.m[i])));
}
#else
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
result.m[r][c] = a.m[r][c] + b.m[r][c];
}
}
#endif
return result;
}
RENDER_MATH_FORCE_INLINE float4x4 operator-(const float4x4 &a, const float4x4 &b) noexcept {
float4x4 result;
#if RENDER_MATH_USE_SIMD
for (int i = 0; i < 4; ++i) {
_mm_storeu_ps(result.m[i], _mm_sub_ps(_mm_loadu_ps(a.m[i]), _mm_loadu_ps(b.m[i])));
}
#else
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
result.m[r][c] = a.m[r][c] - b.m[r][c];
}
}
#endif
return result;
}
RENDER_MATH_FORCE_INLINE float4x4 operator*(const float4x4 &matrix, float scalar) noexcept {
float4x4 result;
#if RENDER_MATH_USE_SIMD
const __m128 s = _mm_set1_ps(scalar);
for (int i = 0; i < 4; ++i) {
_mm_storeu_ps(result.m[i], _mm_mul_ps(_mm_loadu_ps(matrix.m[i]), s));
}
#else
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
result.m[r][c] = matrix.m[r][c] * scalar;
}
}
#endif
return result;
}
RENDER_MATH_FORCE_INLINE float4x4 operator*(float scalar, const float4x4 &matrix) noexcept { return matrix * scalar; }
RENDER_MATH_FORCE_INLINE float4x4 operator/(const float4x4 &matrix, float scalar) noexcept {
return matrix * (1.0f / scalar);
}
RENDER_MATH_FORCE_INLINE float4x4 multiply(const float4x4 &a, const float4x4 &b) noexcept {
float4x4 result;
#if RENDER_MATH_USE_SIMD
const __m128 b0 = _mm_loadu_ps(b.m[0]);
const __m128 b1 = _mm_loadu_ps(b.m[1]);
const __m128 b2 = _mm_loadu_ps(b.m[2]);
const __m128 b3 = _mm_loadu_ps(b.m[3]);
for (int i = 0; i < 4; ++i) {
const __m128 ar = _mm_loadu_ps(a.m[i]);
const __m128 xxxx = _mm_shuffle_ps(ar, ar, _MM_SHUFFLE(0, 0, 0, 0));
const __m128 yyyy = _mm_shuffle_ps(ar, ar, _MM_SHUFFLE(1, 1, 1, 1));
const __m128 zzzz = _mm_shuffle_ps(ar, ar, _MM_SHUFFLE(2, 2, 2, 2));
const __m128 wwww = _mm_shuffle_ps(ar, ar, _MM_SHUFFLE(3, 3, 3, 3));
const __m128 r01 = _mm_add_ps(_mm_mul_ps(xxxx, b0), _mm_mul_ps(yyyy, b1));
const __m128 r23 = _mm_add_ps(_mm_mul_ps(zzzz, b2), _mm_mul_ps(wwww, b3));
_mm_storeu_ps(result.m[i], _mm_add_ps(r01, r23));
}
#else
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
result.m[r][c] =
a.m[r][0] * b.m[0][c] +
a.m[r][1] * b.m[1][c] +
a.m[r][2] * b.m[2][c] +
a.m[r][3] * b.m[3][c];
}
}
#endif
return result;
}
RENDER_MATH_FORCE_INLINE float4x4 operator*(const float4x4 &a, const float4x4 &b) noexcept {
return multiply(a, b);
}
RENDER_MATH_FORCE_INLINE float4x4 &operator+=(float4x4 &a, const float4x4 &b) noexcept {
a = a + b;
return a;
}
RENDER_MATH_FORCE_INLINE float4x4 &operator-=(float4x4 &a, const float4x4 &b) noexcept {
a = a - b;
return a;
}
RENDER_MATH_FORCE_INLINE float4x4 &operator*=(float4x4 &a, const float4x4 &b) noexcept {
a = a * b;
return a;
}
RENDER_MATH_FORCE_INLINE float4x4 &operator*=(float4x4 &a, float scalar) noexcept {
a = a * scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float4x4 &operator/=(float4x4 &a, float scalar) noexcept {
a = a / scalar;
return a;
}
RENDER_MATH_FORCE_INLINE float4x4 transpose(const float4x4 &matrix) noexcept {
float4x4 result;
#if RENDER_MATH_USE_SIMD
__m128 r0 = _mm_loadu_ps(matrix.m[0]);
__m128 r1 = _mm_loadu_ps(matrix.m[1]);
__m128 r2 = _mm_loadu_ps(matrix.m[2]);
__m128 r3 = _mm_loadu_ps(matrix.m[3]);
_MM_TRANSPOSE4_PS(r0, r1, r2, r3);
_mm_storeu_ps(result.m[0], r0);
_mm_storeu_ps(result.m[1], r1);
_mm_storeu_ps(result.m[2], r2);
_mm_storeu_ps(result.m[3], r3);
#else
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
result.m[r][c] = matrix.m[c][r];
}
}
#endif
return result;
}
RENDER_MATH_FORCE_INLINE bool nearly_equal(
const float4x4 &a,
const float4x4 &b,
float abs_eps = 1.0e-5f,
float rel_eps = 1.0e-5f) noexcept {
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
if (!nearly_equal(a.m[r][c], b.m[r][c], abs_eps, rel_eps)) {
return false;
}
}
}
return true;
}
RENDER_MATH_FORCE_INLINE float determinant(const float4x4 &matrix) noexcept {
double a[4][4];
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
a[r][c] = static_cast<double>(matrix.m[r][c]);
}
}
double det = 1.0;
int sign = 1;
for (int col = 0; col < 4; ++col) {
int pivot_row = col;
double pivot_abs = std::fabs(a[col][col]);
for (int row_index = col + 1; row_index < 4; ++row_index) {
const double candidate = std::fabs(a[row_index][col]);
if (candidate > pivot_abs) {
pivot_abs = candidate;
pivot_row = row_index;
}
}
if (pivot_abs == 0.0) {
return 0.0f;
}
if (pivot_row != col) {
for (int c = 0; c < 4; ++c) {
const double temp = a[col][c];
a[col][c] = a[pivot_row][c];
a[pivot_row][c] = temp;
}
sign = -sign;
}
const double pivot = a[col][col];
det *= pivot;
for (int row_index = col + 1; row_index < 4; ++row_index) {
const double factor = a[row_index][col] / pivot;
for (int c = col + 1; c < 4; ++c) {
a[row_index][c] -= factor * a[col][c];
}
}
}
return static_cast<float>(det * static_cast<double>(sign));
}
RENDER_MATH_FORCE_INLINE bool try_inverse(
const float4x4 &matrix,
float4x4 &result,
float *determinant_out = nullptr,
float relative_tolerance = 1.0e-8f) noexcept {
float maximum_element = 0.0f;
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
maximum_element = maximum(maximum_element, std::fabs(matrix.m[r][c]));
}
}
if (maximum_element == 0.0f) {
result = zero_matrix();
if (determinant_out) {
*determinant_out = 0.0f;
}
return false;
}
const float pivot_tolerance = maximum_element * relative_tolerance;
#if RENDER_MATH_USE_SIMD
__m128 left[4] = {
_mm_loadu_ps(matrix.m[0]),
_mm_loadu_ps(matrix.m[1]),
_mm_loadu_ps(matrix.m[2]),
_mm_loadu_ps(matrix.m[3])
};
__m128 right[4] = {
_mm_setr_ps(1.0f, 0.0f, 0.0f, 0.0f),
_mm_setr_ps(0.0f, 1.0f, 0.0f, 0.0f),
_mm_setr_ps(0.0f, 0.0f, 1.0f, 0.0f),
_mm_setr_ps(0.0f, 0.0f, 0.0f, 1.0f)
};
float det = 1.0f;
int sign = 1;
for (int col = 0; col < 4; ++col) {
int pivot_row = col;
float pivot_abs = std::fabs(detail::lane(left[col], col));
for (int row_index = col + 1; row_index < 4; ++row_index) {
const float candidate = std::fabs(detail::lane(left[row_index], col));
if (candidate > pivot_abs) {
pivot_abs = candidate;
pivot_row = row_index;
}
}
if (pivot_abs <= pivot_tolerance) {
result = zero_matrix();
if (determinant_out) {
*determinant_out = 0.0f;
}
return false;
}
if (pivot_row != col) {
const __m128 left_temp = left[col];
const __m128 right_temp = right[col];
left[col] = left[pivot_row];
right[col] = right[pivot_row];
left[pivot_row] = left_temp;
right[pivot_row] = right_temp;
sign = -sign;
}
const float pivot = detail::lane(left[col], col);
det *= pivot;
const __m128 inverse_pivot = _mm_set1_ps(1.0f / pivot);
left[col] = _mm_mul_ps(left[col], inverse_pivot);
right[col] = _mm_mul_ps(right[col], inverse_pivot);
for (int row_index = 0; row_index < 4; ++row_index) {
if (row_index == col) {
continue;
}
const float factor_value = detail::lane(left[row_index], col);
const __m128 factor = _mm_set1_ps(factor_value);
left[row_index] = _mm_sub_ps(left[row_index], _mm_mul_ps(factor, left[col]));
right[row_index] = _mm_sub_ps(right[row_index], _mm_mul_ps(factor, right[col]));
}
}
for (int r = 0; r < 4; ++r) {
_mm_storeu_ps(result.m[r], right[r]);
}
if (determinant_out) {
*determinant_out = det * static_cast<float>(sign);
}
return true;
#else
float augmented[4][8];
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
augmented[r][c] = matrix.m[r][c];
augmented[r][c + 4] = (r == c) ? 1.0f : 0.0f;
}
}
float det = 1.0f;
int sign = 1;
for (int col = 0; col < 4; ++col) {
int pivot_row = col;
float pivot_abs = std::fabs(augmented[col][col]);
for (int row_index = col + 1; row_index < 4; ++row_index) {
const float candidate = std::fabs(augmented[row_index][col]);
if (candidate > pivot_abs) {
pivot_abs = candidate;
pivot_row = row_index;
}
}
if (pivot_abs <= pivot_tolerance) {
result = zero_matrix();
if (determinant_out) {
*determinant_out = 0.0f;
}
return false;
}
if (pivot_row != col) {
for (int c = 0; c < 8; ++c) {
const float temp = augmented[col][c];
augmented[col][c] = augmented[pivot_row][c];
augmented[pivot_row][c] = temp;
}
sign = -sign;
}
const float pivot = augmented[col][col];
det *= pivot;
const float inv_pivot = 1.0f / pivot;
for (int c = 0; c < 8; ++c) {
augmented[col][c] *= inv_pivot;
}
for (int row_index = 0; row_index < 4; ++row_index) {
if (row_index == col) {
continue;
}
const float factor = augmented[row_index][col];
for (int c = 0; c < 8; ++c) {
augmented[row_index][c] -= factor * augmented[col][c];
}
}
}
for (int r = 0; r < 4; ++r) {
for (int c = 0; c < 4; ++c) {
result.m[r][c] = augmented[r][c + 4];
}
}
if (determinant_out) {
*determinant_out = det * static_cast<float>(sign);
}
return true;
#endif
}
RENDER_MATH_FORCE_INLINE float4x4 inverse(
const float4x4 &matrix,
bool *success = nullptr,
float *determinant_out = nullptr,
float relative_tolerance = 1.0e-8f) noexcept {
float4x4 result;
const bool ok = try_inverse(matrix, result, determinant_out, relative_tolerance);
if (success) {
*success = ok;
}
return result;
}
RENDER_MATH_FORCE_INLINE float4 transform(float4 vector, const float4x4 &matrix) noexcept {
#if RENDER_MATH_USE_SIMD
const __m128 r0 = _mm_loadu_ps(matrix.m[0]);
const __m128 r1 = _mm_loadu_ps(matrix.m[1]);
const __m128 r2 = _mm_loadu_ps(matrix.m[2]);
const __m128 r3 = _mm_loadu_ps(matrix.m[3]);
const __m128 x = _mm_mul_ps(_mm_set1_ps(vector.x), r0);
const __m128 y = _mm_mul_ps(_mm_set1_ps(vector.y), r1);
const __m128 z = _mm_mul_ps(_mm_set1_ps(vector.z), r2);
const __m128 w = _mm_mul_ps(_mm_set1_ps(vector.w), r3);
return detail::store_float4(_mm_add_ps(_mm_add_ps(x, y), _mm_add_ps(z, w)));
#else
return float4(
vector.x * matrix.m[0][0] + vector.y * matrix.m[1][0] + vector.z * matrix.m[2][0] + vector.w * matrix.m[3][0],
vector.x * matrix.m[0][1] + vector.y * matrix.m[1][1] + vector.z * matrix.m[2][1] + vector.w * matrix.m[3][1],
vector.x * matrix.m[0][2] + vector.y * matrix.m[1][2] + vector.z * matrix.m[2][2] + vector.w * matrix.m[3][2],
vector.x * matrix.m[0][3] + vector.y * matrix.m[1][3] + vector.z * matrix.m[2][3] + vector.w * matrix.m[3][3]);
#endif
}
RENDER_MATH_FORCE_INLINE float4 operator*(float4 vector, const float4x4 &matrix) noexcept {
return transform(vector, matrix);
}
RENDER_MATH_FORCE_INLINE float3 transform_point(float3 point, const float4x4 &matrix) noexcept {
const float4 result = transform(float4(point, 1.0f), matrix);
return float3(result.x, result.y, result.z);
}
RENDER_MATH_FORCE_INLINE float3 transform_vector(float3 vector, const float4x4 &matrix) noexcept {
const float4 result = transform(float4(vector, 0.0f), matrix);
return float3(result.x, result.y, result.z);
}
RENDER_MATH_FORCE_INLINE bool transform_coord(
float3 point,
const float4x4 &matrix,
float3 &result,
float w_tolerance = 1.0e-8f) noexcept {
const float4 homogeneous = transform(float4(point, 1.0f), matrix);
if (std::fabs(homogeneous.w) <= w_tolerance) {
result = float3(0.0f);
return false;
}
const float inverse_w = 1.0f / homogeneous.w;
result = float3(homogeneous.x * inverse_w, homogeneous.y * inverse_w, homogeneous.z * inverse_w);
return true;
}
RENDER_MATH_FORCE_INLINE float3 transform_coord(float3 point, const float4x4 &matrix) noexcept {
float3 result;
transform_coord(point, matrix, result);
return result;
}
// -----------------------------------------------------------------------------
// Matrix construction and conversion
// -----------------------------------------------------------------------------
RENDER_MATH_FORCE_INLINE constexpr float4x4 translation_ColVec(float x, float y, float z) noexcept {
return float4x4(
1.0f, 0.0f, 0.0f, x,
0.0f, 1.0f, 0.0f, y,
0.0f, 0.0f, 1.0f, z,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 translation_RowVec(float x, float y, float z) noexcept {
return float4x4(
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
x, y, z, 1.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 translation_ColVec(float3 value) noexcept {
return translation_ColVec(value.x, value.y, value.z);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 translation_RowVec(float3 value) noexcept {
return translation_RowVec(value.x, value.y, value.z);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 scaling(float x, float y, float z) noexcept {
return float4x4(
x, 0.0f, 0.0f, 0.0f,
0.0f, y, 0.0f, 0.0f,
0.0f, 0.0f, z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 scaling(float3 value) noexcept {
return scaling(value.x, value.y, value.z);
}
RENDER_MATH_FORCE_INLINE constexpr float4x4 scaling(float uniform_scale) noexcept {
return scaling(uniform_scale, uniform_scale, uniform_scale);
}
RENDER_MATH_FORCE_INLINE float4x4 rotation_x(float angle) noexcept {
const float c = std::cos(angle);
const float s = std::sin(angle);
return float4x4(
1.0f, 0.0f, 0.0f, 0.0f,
0.0f, c, s, 0.0f,
0.0f, -s, c, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 rotation_y(float angle) noexcept {
const float c = std::cos(angle);
const float s = std::sin(angle);
return float4x4(
c, 0.0f, -s, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
s, 0.0f, c, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 rotation_z(float angle) noexcept {
const float c = std::cos(angle);
const float s = std::sin(angle);
return float4x4(
c, s, 0.0f, 0.0f,
-s, c, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 rotation_quaternion(quat rotation) noexcept {
const quat q = normalize(rotation);
const float xx = q.x * q.x;
const float yy = q.y * q.y;
const float zz = q.z * q.z;
const float xy = q.x * q.y;
const float xz = q.x * q.z;
const float yz = q.y * q.z;
const float wx = q.w * q.x;
const float wy = q.w * q.y;
const float wz = q.w * q.z;
return float4x4(
1.0f - 2.0f * (yy + zz), 2.0f * (xy + wz), 2.0f * (xz - wy), 0.0f,
2.0f * (xy - wz), 1.0f - 2.0f * (xx + zz), 2.0f * (yz + wx), 0.0f,
2.0f * (xz + wy), 2.0f * (yz - wx), 1.0f - 2.0f * (xx + yy), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 rotation_axis(float3 axis, float angle) noexcept {
return rotation_quaternion(quaternion_axis_angle(axis, angle));
}
// Apply roll (Z), then pitch (X), then yaw (Y), matching row-vector composition.
RENDER_MATH_FORCE_INLINE float4x4 rotation_yaw_pitch_roll(float yaw, float pitch, float roll) noexcept {
return rotation_z(roll) * rotation_x(pitch) * rotation_y(yaw);
}
RENDER_MATH_FORCE_INLINE quat quaternion_rotation_matrix(const float4x4 &matrix) noexcept {
// Convert the transposed 3x3 to the common column-vector formula.
const float r00 = matrix.m[0][0];
const float r01 = matrix.m[1][0];
const float r02 = matrix.m[2][0];
const float r10 = matrix.m[0][1];
const float r11 = matrix.m[1][1];
const float r12 = matrix.m[2][1];
const float r20 = matrix.m[0][2];
const float r21 = matrix.m[1][2];
const float r22 = matrix.m[2][2];
quat result;
const float trace = r00 + r11 + r22;
if (trace > 0.0f) {
const float s = std::sqrt(trace + 1.0f) * 2.0f;
result.w = 0.25f * s;
result.x = (r21 - r12) / s;
result.y = (r02 - r20) / s;
result.z = (r10 - r01) / s;
} else if (r00 > r11 && r00 > r22) {
const float s = std::sqrt(1.0f + r00 - r11 - r22) * 2.0f;
result.w = (r21 - r12) / s;
result.x = 0.25f * s;
result.y = (r01 + r10) / s;
result.z = (r02 + r20) / s;
} else if (r11 > r22) {
const float s = std::sqrt(1.0f + r11 - r00 - r22) * 2.0f;
result.w = (r02 - r20) / s;
result.x = (r01 + r10) / s;
result.y = 0.25f * s;
result.z = (r12 + r21) / s;
} else {
const float s = std::sqrt(1.0f + r22 - r00 - r11) * 2.0f;
result.w = (r10 - r01) / s;
result.x = (r02 + r20) / s;
result.y = (r12 + r21) / s;
result.z = 0.25f * s;
}
return normalize(result);
}
RENDER_MATH_FORCE_INLINE quat quaternion_yaw_pitch_roll(float yaw, float pitch, float roll) noexcept {
return quaternion_rotation_matrix(rotation_yaw_pitch_roll(yaw, pitch, roll));
}
RENDER_MATH_FORCE_INLINE float4x4 compose_ColVec(float3 position, quat rotation, float3 scale) noexcept {
return translation_ColVec(position) * rotation_quaternion(rotation) * scaling(scale);
}
RENDER_MATH_FORCE_INLINE float4x4 compose_RowVec(float3 position, quat rotation, float3 scale) noexcept {
return scaling(scale) * rotation_quaternion(rotation) * translation_RowVec(position);
}
RENDER_MATH_FORCE_INLINE bool decompose_RowVec(
const float4x4 &matrix,
float3 &scale_out,
quat &rotation_out,
float3 &translation_out,
float tolerance = 1.0e-6f) noexcept {
if (std::fabs(matrix.m[0][3]) > tolerance ||
std::fabs(matrix.m[1][3]) > tolerance ||
std::fabs(matrix.m[2][3]) > tolerance ||
std::fabs(matrix.m[3][3] - 1.0f) > tolerance) {
return false;
}
translation_out = float3(matrix.m[3][0], matrix.m[3][1], matrix.m[3][2]);
float3 basis0(matrix.m[0][0], matrix.m[0][1], matrix.m[0][2]);
float3 basis1(matrix.m[1][0], matrix.m[1][1], matrix.m[1][2]);
float3 basis2(matrix.m[2][0], matrix.m[2][1], matrix.m[2][2]);
float sx = length(basis0);
float sy = length(basis1);
float sz = length(basis2);
if (sx <= tolerance || sy <= tolerance || sz <= tolerance) {
return false;
}
basis0 /= sx;
basis1 /= sy;
basis2 /= sz;
const float orthogonality_tolerance = maximum(1.0e-5f, tolerance * 10.0f);
if (std::fabs(dot(basis0, basis1)) > orthogonality_tolerance ||
std::fabs(dot(basis0, basis2)) > orthogonality_tolerance ||
std::fabs(dot(basis1, basis2)) > orthogonality_tolerance) {
return false; // Shear cannot be represented by scale/rotation/translation.
}
if (dot(basis0, cross(basis1, basis2)) < 0.0f) {
sx = -sx;
basis0 = -basis0;
}
const float4x4 rotation_matrix(
basis0.x, basis0.y, basis0.z, 0.0f,
basis1.x, basis1.y, basis1.z, 0.0f,
basis2.x, basis2.y, basis2.z, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f);
scale_out = float3(sx, sy, sz);
rotation_out = quaternion_rotation_matrix(rotation_matrix);
return true;
}
RENDER_MATH_FORCE_INLINE bool make_normal_matrix(
const float4x4 &world_matrix,
float4x4 &result,
float relative_tolerance = 1.0e-8f) noexcept {
float4x4 inverse_world;
if (!try_inverse(world_matrix, inverse_world, nullptr, relative_tolerance)) {
result = zero_matrix();
return false;
}
result = transpose(inverse_world);
result.m[0][3] = 0.0f;
result.m[1][3] = 0.0f;
result.m[2][3] = 0.0f;
result.m[3][0] = 0.0f;
result.m[3][1] = 0.0f;
result.m[3][2] = 0.0f;
result.m[3][3] = 1.0f;
return true;
}
// -----------------------------------------------------------------------------
// Camera and Direct3D projection matrices
// -----------------------------------------------------------------------------
RENDER_MATH_FORCE_INLINE float4x4 look_to_lh(float3 eye, float3 direction, float3 up) noexcept {
const float3 z_axis = normalize(direction);
const float3 x_axis = normalize(cross(up, z_axis));
const float3 y_axis = cross(z_axis, x_axis);
return float4x4(
x_axis.x, y_axis.x, z_axis.x, 0.0f,
x_axis.y, y_axis.y, z_axis.y, 0.0f,
x_axis.z, y_axis.z, z_axis.z, 0.0f,
-dot(x_axis, eye), -dot(y_axis, eye), -dot(z_axis, eye), 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 look_at_lh(float3 eye, float3 target, float3 up) noexcept {
return look_to_lh(eye, target - eye, up);
}
RENDER_MATH_FORCE_INLINE float4x4 look_to_rh(float3 eye, float3 direction, float3 up) noexcept {
return look_to_lh(eye, -direction, up);
}
RENDER_MATH_FORCE_INLINE float4x4 look_at_rh(float3 eye, float3 target, float3 up) noexcept {
return look_to_rh(eye, target - eye, up);
}
RENDER_MATH_FORCE_INLINE float4x4 perspective_fov_lh(
float vertical_fov,
float aspect_ratio,
float near_plane,
float far_plane) noexcept {
if (!(vertical_fov > 0.0f && vertical_fov < pi) || aspect_ratio <= 0.0f ||
near_plane <= 0.0f || far_plane <= near_plane) {
return zero_matrix();
}
const float y_scale = 1.0f / std::tan(vertical_fov * 0.5f);
const float x_scale = y_scale / aspect_ratio;
const float z_scale = far_plane / (far_plane - near_plane);
return float4x4(
x_scale, 0.0f, 0.0f, 0.0f,
0.0f, y_scale, 0.0f, 0.0f,
0.0f, 0.0f, z_scale, 1.0f,
0.0f, 0.0f, -near_plane * z_scale, 0.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 perspective_fov_rh(
float vertical_fov,
float aspect_ratio,
float near_plane,
float far_plane) noexcept {
if (!(vertical_fov > 0.0f && vertical_fov < pi) || aspect_ratio <= 0.0f ||
near_plane <= 0.0f || far_plane <= near_plane) {
return zero_matrix();
}
const float y_scale = 1.0f / std::tan(vertical_fov * 0.5f);
const float x_scale = y_scale / aspect_ratio;
const float z_scale = far_plane / (near_plane - far_plane);
return float4x4(
x_scale, 0.0f, 0.0f, 0.0f,
0.0f, y_scale, 0.0f, 0.0f,
0.0f, 0.0f, z_scale, -1.0f,
0.0f, 0.0f, near_plane * z_scale, 0.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 orthographic_lh(
float width,
float height,
float near_plane,
float far_plane) noexcept {
if (std::fabs(width) <= epsilon || std::fabs(height) <= epsilon || far_plane <= near_plane) {
return zero_matrix();
}
const float inverse_depth = 1.0f / (far_plane - near_plane);
return float4x4(
2.0f / width, 0.0f, 0.0f, 0.0f,
0.0f, 2.0f / height, 0.0f, 0.0f,
0.0f, 0.0f, inverse_depth, 0.0f,
0.0f, 0.0f, -near_plane * inverse_depth, 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 orthographic_rh(
float width,
float height,
float near_plane,
float far_plane) noexcept {
if (std::fabs(width) <= epsilon || std::fabs(height) <= epsilon || far_plane <= near_plane) {
return zero_matrix();
}
const float inverse_depth = 1.0f / (near_plane - far_plane);
return float4x4(
2.0f / width, 0.0f, 0.0f, 0.0f,
0.0f, 2.0f / height, 0.0f, 0.0f,
0.0f, 0.0f, inverse_depth, 0.0f,
0.0f, 0.0f, near_plane * inverse_depth, 1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 orthographic_off_center_lh(
float left,
float right,
float bottom,
float top,
float near_plane,
float far_plane) noexcept {
const float width = right - left;
const float height = top - bottom;
const float depth = far_plane - near_plane;
if (std::fabs(width) <= epsilon || std::fabs(height) <= epsilon || depth <= 0.0f) {
return zero_matrix();
}
return float4x4(
2.0f / width, 0.0f, 0.0f, 0.0f,
0.0f, 2.0f / height, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f / depth, 0.0f,
-(left + right) / width,
-(top + bottom) / height,
-near_plane / depth,
1.0f);
}
RENDER_MATH_FORCE_INLINE float4x4 orthographic_off_center_rh(
float left,
float right,
float bottom,
float top,
float near_plane,
float far_plane) noexcept {
const float width = right - left;
const float height = top - bottom;
const float depth = near_plane - far_plane;
if (std::fabs(width) <= epsilon || std::fabs(height) <= epsilon || far_plane <= near_plane) {
return zero_matrix();
}
return float4x4(
2.0f / width, 0.0f, 0.0f, 0.0f,
0.0f, 2.0f / height, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f / depth, 0.0f,
-(left + right) / width,
-(top + bottom) / height,
near_plane / depth,
1.0f);
}
// -----------------------------------------------------------------------------
// Viewport projection/unprojection
// -----------------------------------------------------------------------------
struct viewport {
float x;
float y;
float width;
float height;
float min_depth;
float max_depth;
constexpr viewport() noexcept
: x(0.0f), y(0.0f), width(0.0f), height(0.0f), min_depth(0.0f), max_depth(1.0f) {}
constexpr viewport(float x_value, float y_value, float width_value, float height_value,
float min_depth_value = 0.0f, float max_depth_value = 1.0f) noexcept
: x(x_value), y(y_value), width(width_value), height(height_value),
min_depth(min_depth_value), max_depth(max_depth_value) {}
};
RENDER_MATH_FORCE_INLINE bool project(
float3 point,
const viewport &vp,
const float4x4 &world_view_projection,
float3 &result,
float w_tolerance = 1.0e-8f) noexcept {
if (std::fabs(vp.width) <= epsilon || std::fabs(vp.height) <= epsilon ||
std::fabs(vp.max_depth - vp.min_depth) <= epsilon) {
result = float3(0.0f);
return false;
}
const float4 clip = transform(float4(point, 1.0f), world_view_projection);
if (std::fabs(clip.w) <= w_tolerance) {
result = float3(0.0f);
return false;
}
const float inverse_w = 1.0f / clip.w;
const float ndc_x = clip.x * inverse_w;
const float ndc_y = clip.y * inverse_w;
const float ndc_z = clip.z * inverse_w;
result.x = vp.x + (ndc_x + 1.0f) * 0.5f * vp.width;
result.y = vp.y + (1.0f - ndc_y) * 0.5f * vp.height;
result.z = vp.min_depth + ndc_z * (vp.max_depth - vp.min_depth);
return true;
}
RENDER_MATH_FORCE_INLINE bool project(
float3 point,
const viewport &vp,
const float4x4 &world,
const float4x4 &view,
const float4x4 &projection,
float3 &result,
float w_tolerance = 1.0e-8f) noexcept {
return project(point, vp, world * view * projection, result, w_tolerance);
}
RENDER_MATH_FORCE_INLINE bool unproject(
float3 screen_point,
const viewport &vp,
const float4x4 &world_view_projection,
float3 &result,
float relative_tolerance = 1.0e-8f) noexcept {
if (std::fabs(vp.width) <= epsilon || std::fabs(vp.height) <= epsilon ||
std::fabs(vp.max_depth - vp.min_depth) <= epsilon) {
result = float3(0.0f);
return false;
}
float4x4 inverse_wvp;
if (!try_inverse(world_view_projection, inverse_wvp, nullptr, relative_tolerance)) {
result = float3(0.0f);
return false;
}
const float ndc_x = ((screen_point.x - vp.x) / vp.width) * 2.0f - 1.0f;
const float ndc_y = 1.0f - ((screen_point.y - vp.y) / vp.height) * 2.0f;
const float ndc_z = (screen_point.z - vp.min_depth) / (vp.max_depth - vp.min_depth);
const float4 world_h = transform(float4(ndc_x, ndc_y, ndc_z, 1.0f), inverse_wvp);
if (std::fabs(world_h.w) <= epsilon) {
result = float3(0.0f);
return false;
}
const float inverse_w = 1.0f / world_h.w;
result = float3(world_h.x * inverse_w, world_h.y * inverse_w, world_h.z * inverse_w);
return true;
}
RENDER_MATH_FORCE_INLINE bool unproject(
float3 screen_point,
const viewport &vp,
const float4x4 &world,
const float4x4 &view,
const float4x4 &projection,
float3 &result,
float relative_tolerance = 1.0e-8f) noexcept {
return unproject(screen_point, vp, world * view * projection, result, relative_tolerance);
}
// -----------------------------------------------------------------------------
// Rendering conversions: UNORM8 and IEEE-754 binary16
// -----------------------------------------------------------------------------
namespace detail {
RENDER_MATH_FORCE_INLINE std::uint32_t unorm8(float value) noexcept {
if (!(value > 0.0f)) {
return 0u;
}
if (value >= 1.0f) {
return 255u;
}
return static_cast<std::uint32_t>(value * 255.0f + 0.5f);
}
} // namespace detail
// Integer layout: 0xAABBGGRR. In little-endian memory this is R, G, B, A.
RENDER_MATH_FORCE_INLINE std::uint32_t pack_rgba8_unorm(float4 color) noexcept {
const std::uint32_t r = detail::unorm8(color.x);
const std::uint32_t g = detail::unorm8(color.y);
const std::uint32_t b = detail::unorm8(color.z);
const std::uint32_t a = detail::unorm8(color.w);
return r | (g << 8u) | (b << 16u) | (a << 24u);
}
RENDER_MATH_FORCE_INLINE constexpr float4 unpack_rgba8_unorm(std::uint32_t packed) noexcept {
return float4(
static_cast<float>(packed & 0xffu) / 255.0f,
static_cast<float>((packed >> 8u) & 0xffu) / 255.0f,
static_cast<float>((packed >> 16u) & 0xffu) / 255.0f,
static_cast<float>((packed >> 24u) & 0xffu) / 255.0f);
}
RENDER_MATH_FORCE_INLINE std::uint16_t float_to_half(float value) noexcept {
std::uint32_t bits = 0u;
std::memcpy(&bits, &value, sizeof(bits));
const std::uint32_t sign = (bits >> 16u) & 0x8000u;
const std::uint32_t exponent = (bits >> 23u) & 0xffu;
std::uint32_t mantissa = bits & 0x7fffffu;
if (exponent == 0xffu) {
if (mantissa == 0u) {
return static_cast<std::uint16_t>(sign | 0x7c00u);
}
const std::uint32_t payload = maximum(1u, mantissa >> 13u);
return static_cast<std::uint16_t>(sign | 0x7c00u | payload | 0x0200u);
}
const int half_exponent = static_cast<int>(exponent) - 127 + 15;
if (half_exponent >= 31) {
return static_cast<std::uint16_t>(sign | 0x7c00u);
}
if (half_exponent <= 0) {
if (half_exponent < -10) {
return static_cast<std::uint16_t>(sign);
}
mantissa |= 0x800000u;
const int shift = 14 - half_exponent;
std::uint32_t half_mantissa = mantissa >> shift;
const std::uint32_t remainder_mask = (1u << shift) - 1u;
const std::uint32_t remainder = mantissa & remainder_mask;
const std::uint32_t halfway = 1u << (shift - 1);
if (remainder > halfway || (remainder == halfway && (half_mantissa & 1u) != 0u)) {
++half_mantissa;
}
return static_cast<std::uint16_t>(sign | half_mantissa);
}
std::uint32_t result = sign | (static_cast<std::uint32_t>(half_exponent) << 10u) | (mantissa >> 13u);
const std::uint32_t remainder = mantissa & 0x1fffu;
if (remainder > 0x1000u || (remainder == 0x1000u && (result & 1u) != 0u)) {
++result;
}
return static_cast<std::uint16_t>(result);
}
RENDER_MATH_FORCE_INLINE float half_to_float(std::uint16_t value) noexcept {
const std::uint32_t sign = (static_cast<std::uint32_t>(value & 0x8000u)) << 16u;
const std::uint32_t exponent = (value >> 10u) & 0x1fu;
std::uint32_t mantissa = value & 0x03ffu;
std::uint32_t bits = 0u;
if (exponent == 0u) {
if (mantissa == 0u) {
bits = sign;
} else {
int e = -14;
while ((mantissa & 0x0400u) == 0u) {
mantissa <<= 1u;
--e;
}
mantissa &= 0x03ffu;
const std::uint32_t exponent32 = static_cast<std::uint32_t>(e + 127);
bits = sign | (exponent32 << 23u) | (mantissa << 13u);
}
} else if (exponent == 0x1fu) {
bits = sign | 0x7f800000u | (mantissa << 13u);
if (mantissa != 0u) {
bits |= 0x00400000u;
}
} else {
const std::uint32_t exponent32 = exponent + static_cast<std::uint32_t>(127 - 15);
bits = sign | (exponent32 << 23u) | (mantissa << 13u);
}
float result = 0.0f;
std::memcpy(&result, &bits, sizeof(result));
return result;
}
RENDER_MATH_FORCE_INLINE std::uint32_t pack_half2(float2 value) noexcept {
return static_cast<std::uint32_t>(float_to_half(value.x)) |
(static_cast<std::uint32_t>(float_to_half(value.y)) << 16u);
}
RENDER_MATH_FORCE_INLINE float2 unpack_half2(std::uint32_t packed) noexcept {
return float2(
half_to_float(static_cast<std::uint16_t>(packed & 0xffffu)),
half_to_float(static_cast<std::uint16_t>(packed >> 16u)));
}
static_assert(sizeof(float2) == 8, "float2 must contain two floats");
static_assert(sizeof(float3) == 12, "float3 must contain three tightly packed floats");
static_assert(sizeof(float4) == 16, "float4 must contain four floats");
static_assert(sizeof(quat) == 16, "quat must contain four floats");
static_assert(sizeof(float4x4) == 64, "float4x4 must contain sixteen floats");
static_assert(alignof(float4) == 16, "float4 must be 16-byte aligned");
static_assert(alignof(quat) == 16, "quat must be 16-byte aligned");
static_assert(alignof(float4x4) == 16, "float4x4 must be 16-byte aligned");
static_assert(std::is_standard_layout<float4x4>::value, "float4x4 must be standard-layout");
} // namespace math
#undef RENDER_MATH_FORCE_INLINE
#endif //RENDER_MATH_HPP
Reference in New Issue View Git Blame Copy Permalink