From 24d068d9fd84b736c77bec85b962f269d93442ea Mon Sep 17 00:00:00 2001 From: korenkonder Date: Sat, 30 Dec 2023 20:56:56 +0300 Subject: [PATCH] Double stuff --- src/CRE/Glitter/glitter.hpp | 6 +- src/KKdLib/default.hpp | 2 +- src/KKdLib/interpolation.cpp | 44 +++++ src/KKdLib/interpolation.hpp | 113 +++++++++++ src/KKdLib/vec.cpp | 7 + src/KKdLib/vec.hpp | 355 +++++++++++++++++++++++++++++++++++ 6 files changed, 523 insertions(+), 4 deletions(-) diff --git a/src/CRE/Glitter/glitter.hpp b/src/CRE/Glitter/glitter.hpp index 16dda5ad..98bf5718 100644 --- a/src/CRE/Glitter/glitter.hpp +++ b/src/CRE/Glitter/glitter.hpp @@ -576,16 +576,16 @@ namespace Glitter { const T t_2 = t * t; const T t_3 = t_2 * t; return p - + (t_3 - 2.0f * t_2 + t) * (t1 * df) + + (t_3 - (T)2 * t_2 + t) * (t1 * df) + (t_3 - t_2) * (t2 * df) - + (3.0f * t_2 - 2.0f * t_3) * dv; + + ((T)3 * t_2 - (T)2 * t_3) * dv; }; template inline static T InterpolateLinear(const T p1, const T p2, const T f1, const T f2, const T f) { const T t = (f - f1) / (f2 - f1); - return (1.0f - t) * p1 + t * p2; + return ((T)1 - t) * p1 + t * p2; }; }; diff --git a/src/KKdLib/default.hpp b/src/KKdLib/default.hpp index c23e28f5..ad90eea0 100644 --- a/src/KKdLib/default.hpp +++ b/src/KKdLib/default.hpp @@ -113,7 +113,7 @@ inline double __CRTDECL actgh(double _X) { template inline T lerp_def(T x, T y, U blend) { - return ((U)1 - blend) * x + blend * y; + return ((T)1 - (T)blend) * x + blend * y; } extern void* force_malloc(size_t size); diff --git a/src/KKdLib/interpolation.cpp b/src/KKdLib/interpolation.cpp index ae4276ae..5edef84f 100644 --- a/src/KKdLib/interpolation.cpp +++ b/src/KKdLib/interpolation.cpp @@ -28,6 +28,29 @@ void interpolate_chs_reverse_value(float_t* arr, size_t length, t2 = h10.y * t1_t2.x - h10.x * t1_t2.y; } +void interpolate_chs_reverse_value(double_t* arr, size_t length, + double_t& t1, double_t& t2, size_t f1, size_t f2, size_t f) { + vec2d t = vec2d( + (double_t)(int64_t)(f - f1 + 0), + (double_t)(int64_t)(f - f1 + 1) + ) / (double_t)(int64_t)(f2 - f1); + vec2d t_2 = t * t; + vec2d t_3 = t_2 * t; + vec2d t_23 = 3.0 * t_2; + vec2d t_32 = 2.0 * t_3; + + vec2d h00 = t_32 - t_23 + 1.0; + vec2d h01 = t_23 - t_32; + vec2d h10 = t_3 - 2.0 * t_2 + t; + vec2d h11 = t_3 - t_2; + + vec2d t1_t2 = *(vec2d*)&arr[f] - h00 * arr[f1] - h01 * arr[f2]; + t1_t2 /= (t_2.x - t.x) * (t_2.y - t.y); + + t1 = -h11.y * t1_t2.x + h11.x * t1_t2.y; + t2 = h10.y * t1_t2.x - h10.x * t1_t2.y; +} + void interpolate_chs_reverse(float_t* arr, size_t length, float_t& t1, float_t& t2, size_t f1, size_t f2) { t1 = 0.0f; @@ -49,6 +72,27 @@ void interpolate_chs_reverse(float_t* arr, size_t length, t2 = (float_t)(tt2 / (double_t)(f2 - f1 - 2)); } +void interpolate_chs_reverse(double_t* arr, size_t length, + double_t& t1, double_t& t2, size_t f1, size_t f2) { + t1 = 0.0; + t2 = 0.0; + + if (f2 - f1 - 2 < 1) + return; + + double_t _t1 = 0.0; + double_t _t2 = 0.0; + double_t tt1 = 0.0; + double_t tt2 = 0.0; + for (size_t i = f1 + 1; i < f2 - 1; i++) { + interpolate_chs_reverse_value(arr, length, _t1, _t2, f1, f2, i); + tt1 += _t1; + tt2 += _t2; + } + t1 = tt1 / (double_t)(f2 - f1 - 2); + t2 = tt2 / (double_t)(f2 - f1 - 2); +} + int32_t interpolate_chs_reverse_sequence( std::vector& values_src, std::vector& values, bool fast) { size_t count = values_src.size(); diff --git a/src/KKdLib/interpolation.hpp b/src/KKdLib/interpolation.hpp index 58c53521..c18f08e6 100644 --- a/src/KKdLib/interpolation.hpp +++ b/src/KKdLib/interpolation.hpp @@ -83,6 +83,45 @@ inline std::vector interpolate_linear(float_t p1, float_t p2, size_t f1 return arr; } +inline double_t interpolate_linear_value(const double_t p1, const double_t p2, + const double_t f1, const double_t f2, const double_t f) { + if (p1 == p2) + return p1; + + double_t t = (f - f1) / (f2 - f1); + return (1.0 - t) * p1 + t * p2; +} + +inline vec2d interpolate_linear_value(const vec2d p1, const vec2d p2, + const vec2d f1, const vec2d f2, const vec2d f) { + if (p1 == p2) + return p1; + + __m128d _p1 = vec2d::load_xmm(p1); + __m128d _p2 = vec2d::load_xmm(p2); + __m128d _f1 = vec2d::load_xmm(f1); + __m128d _f2 = vec2d::load_xmm(f2); + __m128d _f = vec2d::load_xmm(f); + + const __m128d _1 = vec2d::load_xmm(1.0); + + __m128d t = _mm_div_pd(_mm_sub_pd(_f, _f1), _mm_sub_pd(_f2, _f1)); + return vec2d::store_xmm(_mm_add_pd(_mm_mul_pd(_p1, _mm_sub_pd(_1, t)), _mm_mul_pd(_p2, t))); +} + +inline std::vector interpolate_linear(double_t p1, double_t p2, size_t f1, size_t f2) { + size_t length = f2 - f1 + 1; + if (p1 == p2) + return std::vector(length, p1); + + std::vector arr(length); + double_t* a = arr.data(); + for (size_t i = 0, j = length; j; i++, j--, a++) + *a = interpolate_linear_value(p1, p2, + (double_t)f1, (double_t)f2, (double_t)(f1 + i)); + return arr; +} + inline float_t interpolate_chs_value(const float_t p1, const float_t p2, const float_t t1, const float_t t2, const float_t f1, const float_t f2, const float_t f) { if (p1 == p2 && fabsf(t1) == 0.0f && fabsf(t2) == 0.0f) @@ -225,9 +264,83 @@ inline std::vector interpolate_chs(const float_t p1, const float_t p2, return arr; } +inline double_t interpolate_chs_value(const double_t p1, const double_t p2, + const double_t t1, const double_t t2, const double_t f1, const double_t f2, const double_t f) { + if (p1 == p2 && fabs(t1) == 0.0 && fabs(t2) == 0.0) + return p1; + + double_t df = f2 - f1; + double_t t = (f - f1) / df; + double_t t_2 = t * t; + double_t t_3 = t_2 * t; + double_t t_23 = 3.0f * t_2; + double_t t_32 = 2.0f * t_3; + + double_t h00 = t_32 - t_23 + 1.0f; + double_t h01 = t_23 - t_32; + double_t h10 = t_3 - 2.0f * t_2 + t; + double_t h11 = t_3 - t_2; + + return h00 * p1 + h01 * p2 + h10 * (t1 * df) + h11 * (t2 * df); +} + +inline vec2d interpolate_chs_value(const vec2d p1, const vec2d p2, + const vec2d t1, const vec2d t2, const vec2d f1, const vec2d f2, const vec2d f) { + if (p1 == p2 && vec2d::abs(t1) == 0.0 && vec2d::abs(t2) == 0.0) + return p1; + + __m128d _p1 = vec2d::load_xmm(p1); + __m128d _p2 = vec2d::load_xmm(p2); + __m128d _t1 = vec2d::load_xmm(t1); + __m128d _t2 = vec2d::load_xmm(t2); + __m128d _f1 = vec2d::load_xmm(f1); + __m128d _f2 = vec2d::load_xmm(f2); + __m128d _f = vec2d::load_xmm(f); + + const __m128d _1 = vec2d::load_xmm(1.0); + const __m128d _2 = vec2d::load_xmm(2.0); + const __m128d _3 = vec2d::load_xmm(3.0); + + __m128d df = _mm_sub_pd(_f2, _f1); + __m128d t = _mm_div_pd(_mm_sub_pd(_f, _f1), df); + __m128d t_2 = _mm_mul_pd(t, t); + __m128d t_3 = _mm_mul_pd(t_2, t); + __m128d t_23 = _mm_mul_pd(_3, t_2); + __m128d t_32 = _mm_mul_pd(_2, t_3); + + __m128d h00 = _mm_add_pd(_mm_sub_pd(t_32, t_23), _1); + __m128d h01 = _mm_sub_pd(t_23, t_32); + __m128d h10 = _mm_add_pd(_mm_sub_pd(t_3, _mm_mul_pd(_2, t_2)), t); + __m128d h11 = _mm_sub_pd(t_3, t_2); + + _p1 = _mm_mul_pd(h00, _p1); + _p2 = _mm_mul_pd(h01, _p2); + _t1 = _mm_mul_pd(h10, _mm_mul_pd(_t1, df)); + _t2 = _mm_mul_pd(h11, _mm_mul_pd(_t2, df)); + return vec2d::store_xmm(_mm_add_pd(_mm_add_pd(_p1, _p2), _mm_add_pd(_t1, _t2))); +} + +inline std::vector interpolate_chs(const double_t p1, const double_t p2, + const double_t t1, const double_t t2, const size_t f1, const size_t f2) { + size_t length = f2 - f1 + 1; + if (p1 == p2 && fabs(t1) == 0.0 && fabs(t2) == 0.0) + return std::vector(length, p1); + + std::vector arr(length); + double_t* a = arr.data(); + for (size_t i = 0, j = length; j; i++, j--, a++) + *a = interpolate_chs_value(p1, p2, t1, t2, + (double_t)f1, (double_t)f2, (double_t)(f1 + i)); + return arr; +} + extern void interpolate_chs_reverse_value(float_t* arr, size_t length, float_t& t1, float_t& t2, size_t f1, size_t f2, size_t f); +extern void interpolate_chs_reverse_value(double_t* arr, size_t length, + double_t& t1, double_t& t2, size_t f1, size_t f2, size_t f); extern void interpolate_chs_reverse(float_t* arr, size_t length, float_t& t1, float_t& t2, size_t f1, size_t f2); +extern void interpolate_chs_reverse(double_t* arr, size_t length, + double_t& t1, double_t& t2, size_t f1, size_t f2); extern int32_t interpolate_chs_reverse_sequence( std::vector& values_src, std::vector& values, bool fast = false); diff --git a/src/KKdLib/vec.cpp b/src/KKdLib/vec.cpp index f499c766..375c9588 100644 --- a/src/KKdLib/vec.cpp +++ b/src/KKdLib/vec.cpp @@ -9,6 +9,8 @@ const __m128 vec2_neg = { -0.0f, -0.0f, 0.0f, 0.0f }; const __m128 vec3_neg = { -0.0f, -0.0f, -0.0f, 0.0f }; const __m128 vec4_neg = { -0.0f, -0.0f, -0.0f, -0.0f }; +extern const __m128d vec2d_neg = { -0.0, -0.0f }; + const __m128i vec2i_abs = { (char)0xFF, (char)0xFF, (char)0xFF, (char)0x7F, (char)0xFF, (char)0xFF, (char)0xFF, (char)0x7F, @@ -29,3 +31,8 @@ const __m128i vec4i_abs = { (char)0xFF, (char)0xFF, (char)0xFF, (char)0x7F, (char)0xFF, (char)0xFF, (char)0xFF, (char)0x7F, }; + +extern const __m128i vec2i64_abs = { + (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0x7F, + (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0xFF, (char)0x7F, +}; diff --git a/src/KKdLib/vec.hpp b/src/KKdLib/vec.hpp index 3d68ce4a..a92b3e7f 100644 --- a/src/KKdLib/vec.hpp +++ b/src/KKdLib/vec.hpp @@ -457,14 +457,54 @@ struct vec4i { static vec4i clamp(const vec4i& left, const int32_t min, const int32_t max); }; +struct vec2d { + double_t x; + double_t y; + + vec2d(); + vec2d(double_t value); + vec2d(double_t x, double_t y); + + static __m128d load_xmm(const double_t data); + static __m128d load_xmm(const vec2d& data); + static __m128d load_xmm(const vec2d&& data); + static vec2d store_xmm(const __m128d& data); + static vec2d store_xmm(const __m128d&& data); + + static double_t angle(const vec2d& left, const vec2d& right); + static double_t dot(const vec2d& left, const vec2d& right); + static double_t length(const vec2d& left); + static double_t length_squared(const vec2d& left); + static double_t distance(const vec2d& left, const vec2d& right); + static double_t distance_squared(const vec2d& left, const vec2d& right); + static vec2d abs(const vec2d& left); + static vec2d lerp(const vec2d& left, const vec2d& right, const vec2d& blend); + static vec2d lerp(const vec2d& left, const vec2d& right, const double_t blend); + static vec2d normalize(const vec2d& left); + static vec2d normalize_rcp(const vec2d& left); + static vec2d rcp(const vec2d& left); + static vec2d min(const vec2d& left, const vec2d& right); + static vec2d max(const vec2d& left, const vec2d& right); + static vec2d clamp(const vec2d& left, const vec2d& min, const vec2d& max); + static vec2d clamp(const vec2d& left, const double_t min, const double_t max); + static vec2d mult_min_max(const vec2d& left, const vec2d& min, const vec2d& max); + static vec2d mult_min_max(const vec2d& left, const double_t min, const double_t max); + static vec2d div_min_max(const vec2d& left, const vec2d& min, const vec2d& max); + static vec2d div_min_max(const vec2d& left, const double_t min, const double_t max); +}; + extern const __m128 vec2_neg; extern const __m128 vec3_neg; extern const __m128 vec4_neg; +extern const __m128d vec2d_neg; + extern const __m128i vec2i_abs; extern const __m128i vec3i_abs; extern const __m128i vec4i_abs; +extern const __m128i vec2i64_abs; + inline vec2::vec2() : x(), y() { } @@ -1686,6 +1726,321 @@ inline vec4i vec4i::clamp(const vec4i& left, const int32_t min, const int32_t ma vec4i::load_xmm(min)), vec4i::load_xmm(max))); } +inline vec2d::vec2d() : x(), y() { + +} + +inline vec2d::vec2d(double_t value) : x(value), y(value) { + +} + +inline vec2d::vec2d(double_t x, double_t y) : x(x), y(y) { + +} + +inline __m128d vec2d::load_xmm(const double_t data) { + __m128d _data = _mm_set_sd(data); + return _mm_shuffle_pd(_data, _data, 0); +} + +inline __m128d vec2d::load_xmm(const vec2d& data) { + return _mm_loadu_pd((const double_t*) & data); +} + +inline __m128d vec2d::load_xmm(const vec2d&& data) { + return _mm_loadu_pd((const double_t*) & data); +} + +inline vec2d vec2d::store_xmm(const __m128d& data) { + vec2d _data; + _mm_storeu_pd((double_t*) & _data, data); + return _data; +} + +inline vec2d vec2d::store_xmm(const __m128d&& data) { + vec2d _data; + _mm_storeu_pd((double_t*) & _data, data); + return _data; +} + +inline vec2d operator +(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_add_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator +(const vec2d& left, const double_t right) { + return vec2d::store_xmm(_mm_add_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator +(const double_t left, const vec2d& right) { + return vec2d::store_xmm(_mm_add_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator +=(vec2d& left, const vec2d& right) { + left = vec2d::store_xmm(_mm_add_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator +=(vec2d& left, const double_t right) { + left = vec2d::store_xmm(_mm_add_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator -(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator -(const vec2d& left, const double_t right) { + return vec2d::store_xmm(_mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator -(const double_t left, const vec2d& right) { + return vec2d::store_xmm(_mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator -=(vec2d& left, const vec2d& right) { + left = vec2d::store_xmm(_mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator -=(vec2d& left, const double_t right) { + left = vec2d::store_xmm(_mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator *(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_mul_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator *(const vec2d& left, const double_t right) { + return vec2d::store_xmm(_mm_mul_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator *(const double_t left, const vec2d& right) { + return vec2d::store_xmm(_mm_mul_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator *=(vec2d& left, const vec2d& right) { + left = vec2d::store_xmm(_mm_mul_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator *=(vec2d& left, const double_t right) { + left = vec2d::store_xmm(_mm_mul_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator /(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_div_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator /(const vec2d& left, const double_t right) { + return vec2d::store_xmm(_mm_div_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator /(const double_t left, const vec2d& right) { + return vec2d::store_xmm(_mm_div_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator /=(vec2d& left, const vec2d& right) { + left = vec2d::store_xmm(_mm_div_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator /=(vec2d& left, const double_t right) { + left = vec2d::store_xmm(_mm_div_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator &(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_and_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator &(const vec2d& left, const double_t right) { + return vec2d::store_xmm(_mm_and_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator &(const double_t left, const vec2d& right) { + return vec2d::store_xmm(_mm_and_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator &=(vec2d& left, const vec2d& right) { + left = vec2d::store_xmm(_mm_and_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator &=(vec2d& left, const double_t right) { + left = vec2d::store_xmm(_mm_and_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator ^(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_xor_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator ^(const vec2d& left, const double_t right) { + return vec2d::store_xmm(_mm_xor_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator ^(const double_t left, const vec2d& right) { + return vec2d::store_xmm(_mm_xor_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator ^=(vec2d& left, const vec2d& right) { + left = vec2d::store_xmm(_mm_xor_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline void operator ^=(vec2d& left, const double_t right) { + left = vec2d::store_xmm(_mm_xor_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d operator -(const vec2d& left) { + return vec2d::store_xmm(_mm_xor_pd(vec2d::load_xmm(left), vec2d_neg)); +} + +inline bool operator ==(const vec2d& left, const vec2d& right) { + return !memcmp(&left, &right, sizeof(vec2d)); +} + +inline bool operator !=(const vec2d& left, const vec2d& right) { + return !!memcmp(&left, &right, sizeof(vec2d)); +} + +inline double_t vec2d::angle(const vec2d& left, const vec2d& right) { + return acos(vec2d::dot(left, right) / (vec2d::length(left) * vec2d::length(right))); +} + +inline double_t vec2d::dot(const vec2d& left, const vec2d& right) { + __m128d zt; + zt = _mm_mul_pd(vec2d::load_xmm(left), vec2d::load_xmm(right)); + return _mm_cvtsd_f64(_mm_hadd_pd(zt, zt)); +} + +inline double_t vec2d::length(const vec2d& left) { + __m128d xt; + __m128d zt; + xt = vec2d::load_xmm(left); + zt = _mm_mul_pd(xt, xt); + return _mm_cvtsd_f64(_mm_sqrt_sd(_mm_hadd_pd(zt, zt), zt)); +} + +inline double_t vec2d::length_squared(const vec2d& left) { + __m128d xt; + __m128d zt; + xt = vec2d::load_xmm(left); + zt = _mm_mul_pd(xt, xt); + return _mm_cvtsd_f64(_mm_hadd_pd(zt, zt)); +} + +inline double_t vec2d::distance(const vec2d& left, const vec2d& right) { + __m128d zt; + zt = _mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right)); + zt = _mm_mul_pd(zt, zt); + return _mm_cvtsd_f64(_mm_sqrt_sd(_mm_hadd_pd(zt, zt), zt)); +} + +inline double_t vec2d::distance_squared(const vec2d& left, const vec2d& right) { + __m128d zt; + zt = _mm_sub_pd(vec2d::load_xmm(left), vec2d::load_xmm(right)); + zt = _mm_mul_pd(zt, zt); + return _mm_cvtsd_f64(_mm_hadd_pd(zt, zt)); +} + +inline vec2d vec2d::abs(const vec2d& left) { + return vec2d::store_xmm(_mm_castsi128_pd(_mm_and_si128(_mm_castpd_si128(vec2d::load_xmm(left)), vec2i64_abs))); +} + +inline vec2d vec2d::lerp(const vec2d& left, const vec2d& right, const vec2d& blend) { + __m128d b1; + __m128d b2; + b1 = vec2d::load_xmm(blend); + b2 = _mm_sub_pd(vec2d::load_xmm(1.0), b1); + return vec2d::store_xmm(_mm_add_pd(_mm_mul_pd(vec2d::load_xmm(left), b2), + _mm_mul_pd(vec2d::load_xmm(right), b1))); +} + +inline vec2d vec2d::lerp(const vec2d& left, const vec2d& right, const double_t blend) { + __m128d b1; + __m128d b2; + b1 = vec2d::load_xmm(blend); + b2 = _mm_sub_pd(vec2d::load_xmm(1.0), b1); + return vec2d::store_xmm(_mm_add_pd(_mm_mul_pd(vec2d::load_xmm(left), b2), + _mm_mul_pd(vec2d::load_xmm(right), b1))); +} + +inline vec2d vec2d::normalize(const vec2d& left) { + __m128d xt; + __m128d zt; + xt = vec2d::load_xmm(left); + zt = _mm_mul_pd(xt, xt); + zt = _mm_sqrt_sd(_mm_hadd_pd(zt, zt), zt); + if (_mm_cvtsd_f64(zt) != 0.0f) + return vec2d::store_xmm(_mm_div_pd(xt, _mm_shuffle_pd(zt, zt, 0))); + return vec2d::store_xmm(xt); +} + +inline vec2d vec2d::normalize_rcp(const vec2d& left) { + __m128d xt; + __m128d zt; + xt = vec2d::load_xmm(left); + zt = _mm_mul_pd(xt, xt); + zt = _mm_sqrt_sd(_mm_hadd_pd(zt, zt), zt); + if (_mm_cvtsd_f64(zt) != 0.0f) + zt = _mm_div_sd(_mm_set_sd(1.0), zt); + return vec2d::store_xmm(_mm_mul_pd(xt, _mm_shuffle_pd(zt, zt, 0))); +} + +inline vec2d vec2d::rcp(const vec2d& left) { + return vec2d::store_xmm(_mm_div_pd(vec2d::load_xmm(1.0), vec2d::load_xmm(left))); +} + +inline vec2d vec2d::min(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_min_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d vec2d::max(const vec2d& left, const vec2d& right) { + return vec2d::store_xmm(_mm_max_pd(vec2d::load_xmm(left), vec2d::load_xmm(right))); +} + +inline vec2d vec2d::clamp(const vec2d& left, const vec2d& min, const vec2d& max) { + return vec2d::store_xmm(_mm_min_pd(_mm_max_pd(vec2d::load_xmm(left), + vec2d::load_xmm(min)), vec2d::load_xmm(max))); +} + +inline vec2d vec2d::clamp(const vec2d& left, const double_t min, const double_t max) { + return vec2d::store_xmm(_mm_min_pd(_mm_max_pd(vec2d::load_xmm(left), + vec2d::load_xmm(min)), vec2d::load_xmm(max))); +} + +inline vec2d vec2d::mult_min_max(const vec2d& left, const vec2d& min, const vec2d& max) { + __m128d xt; + __m128d yt; + __m128d zt; + xt = vec2d::load_xmm(left); + yt = _mm_and_pd(_mm_cmplt_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(-min)); + zt = _mm_and_pd(_mm_cmpge_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(max)); + return vec2d::store_xmm(_mm_mul_pd(xt, _mm_or_pd(yt, zt))); +} + +inline vec2d vec2d::mult_min_max(const vec2d& left, const double_t min, const double_t max) { + __m128d xt; + __m128d yt; + __m128d zt; + xt = vec2d::load_xmm(left); + yt = _mm_and_pd(_mm_cmplt_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(-min)); + zt = _mm_and_pd(_mm_cmpge_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(max)); + return vec2d::store_xmm(_mm_mul_pd(xt, _mm_or_pd(yt, zt))); +} + +inline vec2d vec2d::div_min_max(const vec2d& left, const vec2d& min, const vec2d& max) { + __m128d xt; + __m128d yt; + __m128d zt; + xt = vec2d::load_xmm(left); + yt = _mm_and_pd(_mm_cmplt_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(-min)); + zt = _mm_and_pd(_mm_cmpge_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(max)); + return vec2d::store_xmm(_mm_div_pd(xt, _mm_or_pd(yt, zt))); +} + +inline vec2d vec2d::div_min_max(const vec2d& left, const double_t min, const double_t max) { + __m128d xt; + __m128d yt; + __m128d zt; + xt = vec2d::load_xmm(left); + yt = _mm_and_pd(_mm_cmplt_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(-min)); + zt = _mm_and_pd(_mm_cmpge_pd(xt, vec2d::load_xmm(0.0)), vec2d::load_xmm(max)); + return vec2d::store_xmm(_mm_div_pd(xt, _mm_or_pd(yt, zt))); +} + inline void vec2i8_to_vec2(const vec2i8& src, vec2& dst) { dst.x = (float_t)src.x; dst.y = (float_t)src.y;