diff --git a/Physics3D/math/linalg/vec.h b/Physics3D/math/linalg/vec.h index 34f9308c..95764e02 100644 --- a/Physics3D/math/linalg/vec.h +++ b/Physics3D/math/linalg/vec.h @@ -3,8 +3,7 @@ #include #include #include -#include -#include + namespace P3D { template struct Vector { @@ -242,36 +241,9 @@ typedef Vector Vec6l; typedef Vector Vec6i; -// template -// constexpr auto operator*(const Vector& a, const Vector& b) noexcept -> decltype(a[0] * b[0] + a[1] * b[1]) { -// decltype(a[0] * b[0] + a[1] * b[1]) result = a[0] * b[0]; -// for(size_t i = 1; i < Size; i++) { -// result += a[i] * b[i]; -// } -// return result; -// } - -namespace stdx = std::experimental; - - template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr auto operator*(const Vector& a, const Vector& b) noexcept -> decltype(a[0] * b[0]) { - stdx::fixed_size_simd result; - stdx::fixed_size_simd a_simd(&a[0], stdx::element_aligned); - stdx::fixed_size_simd b_simd(&b[0], stdx::element_aligned); - result = a_simd * b_simd; - return stdx::reduce(result); - // decltype(a[0] * b[0] + a[1] * b[1]) result = a[0] * b[0]; - // for(size_t i = 1; i < Size; i++) { - // result += a[i] * b[i]; - // } - // return result; -} -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) -constexpr auto operator*(const Vector& a, const Vector& b) noexcept -> decltype(a[0] * b[0]) { - decltype(a[0] * b[0]) result = a[0] * b[0]; +constexpr auto operator*(const Vector& a, const Vector& b) noexcept -> decltype(a[0] * b[0] + a[1] * b[1]) { + decltype(a[0] * b[0] + a[1] * b[1]) result = a[0] * b[0]; for(size_t i = 1; i < Size; i++) { result += a[i] * b[i]; } @@ -279,43 +251,16 @@ constexpr auto operator*(const Vector& a, const Vector& b) n } template -constexpr auto dot(const Vector& a, const Vector& b) noexcept -> decltype(a[0] * b[0]) { +constexpr auto dot(const Vector& a, const Vector& b) noexcept -> decltype(a[0] * b[0] + a[1] * b[1]) { return a * b; } template -constexpr auto dot(const Vector& vec) noexcept -> decltype(vec[0] * vec[0]) { +constexpr auto dot(const Vector& vec) noexcept -> decltype(vec[0] * vec[0] + vec[1] * vec[1]) { return vec * vec; } -// template -// constexpr auto operator+(const Vector& a, const Vector& b) noexcept -> Vector { -// Vector result; -// for(size_t i = 0; i < Size; i++) { -// result[i] = a[i] + b[i]; -// } -// return result; -// } - -template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr auto operator+(const Vector& a, const Vector& b) noexcept -> Vector { - stdx::fixed_size_simd result; - stdx::fixed_size_simd a_simd(&a[0], stdx::element_aligned); - stdx::fixed_size_simd b_simd(&b[0], stdx::element_aligned); - result = a_simd + b_simd; - Vector result_vec; - result.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - // Vector result; - // for(size_t i = 0; i < Size; i++) { - // result[i] = a[i] + b[i]; - // } - // return result; -} - template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr auto operator+(const Vector& a, const Vector& b) noexcept -> Vector { Vector result; for(size_t i = 0; i < Size; i++) { @@ -325,7 +270,6 @@ constexpr auto operator+(const Vector& a, const Vector& b) n } template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr auto operator-(const Vector& a, const Vector& b) noexcept -> Vector { Vector result; for(size_t i = 0; i < Size; i++) { @@ -335,25 +279,6 @@ constexpr auto operator-(const Vector& a, const Vector& b) n } template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr auto operator-(const Vector& a, const Vector& b) noexcept -> Vector { - stdx::fixed_size_simd result; - stdx::fixed_size_simd a_simd(&a[0], stdx::element_aligned); - stdx::fixed_size_simd b_simd(&b[0], stdx::element_aligned); - result = a_simd - b_simd; - Vector result_vec; - result.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - - // Vector result; - // for(size_t i = 0; i < Size; i++) { - // result[i] = a[i] - b[i]; - // } - // return result; -} - -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr auto operator*(const Vector& vec, const T2& factor) noexcept -> Vector { Vector result; for(size_t i = 0; i < Size; i++) { @@ -363,26 +288,6 @@ constexpr auto operator*(const Vector& vec, const T2& factor) noexcept } template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr auto operator*(const Vector& vec, const T2& factor) noexcept -> Vector { - // store decltype(vec[0] * factor) as a data type for use later - decltype(vec[0] * factor) type; - stdx::fixed_size_simd result; - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - stdx::fixed_size_simd factor_simd = factor; - result = vec_simd * factor_simd; - Vector result_vec; - result.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - // Vector result; - // for(size_t i = 0; i < Size; i++) { - // result[i] = vec[i] * factor; - // } - // return result; -} - -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr auto operator*(const T1& factor, const Vector& vec) noexcept -> Vector { Vector result; for(size_t i = 0; i < Size; i++) { @@ -392,26 +297,6 @@ constexpr auto operator*(const T1& factor, const Vector& vec) noexcept } template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr auto operator*(const T1& factor, const Vector& vec) noexcept -> Vector { - decltype(factor* vec[0]) type; - stdx::fixed_size_simd result; - stdx::fixed_size_simd factor_simd = factor; - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - result = factor_simd * vec_simd; - Vector result_vec; - result.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - - // Vector result; - // for(size_t i = 0; i < Size; i++) { - // result[i] = factor * vec[i]; - // } - // return result; -} - -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr auto operator/(const Vector& vec, const T2& factor) noexcept -> Vector { Vector result; for(size_t i = 0; i < Size; i++) { @@ -420,27 +305,7 @@ constexpr auto operator/(const Vector& vec, const T2& factor) noexcept return result; } -template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr auto operator/(const Vector& vec, const T2& factor) noexcept -> Vector { - decltype(vec[0] / factor) type; - stdx::fixed_size_simd result; - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - stdx::fixed_size_simd factor_simd = factor; - result = vec_simd / factor_simd; - Vector result_vec; - result.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - - // Vector result; - // for(size_t i = 0; i < Size; i++) { - // result[i] = vec[i] / factor; - // } - // return result; -} - template -requires (!(std::is_arithmetic_v)) constexpr Vector operator-(const Vector& vec) noexcept { Vector result; for(size_t i = 0; i < Size; i++) { @@ -449,24 +314,7 @@ constexpr Vector operator-(const Vector& vec) noexcept { return result; } -template -requires std::is_arithmetic_v -constexpr Vector operator-(const Vector& vec) noexcept { - stdx::fixed_size_simd result(&vec[0], stdx::element_aligned); - result = -result; - Vector result_vec; - result.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - - // Vector result; - // for(size_t i = 0; i < Size; i++) { - // result[i] = -vec[i]; - // } - // return result; -} - template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr Vector& operator+=(Vector& vec, const Vector& other) noexcept { for(size_t i = 0; i < Size; i++) { vec[i] += other[i]; @@ -475,22 +323,6 @@ constexpr Vector& operator+=(Vector& vec, const Vector -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr Vector& operator+=(Vector& vec, const Vector& other) noexcept { - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - stdx::fixed_size_simd other_simd(&other[0], stdx::element_aligned); - vec_simd += other_simd; - vec_simd.copy_to(vec.data, stdx::element_aligned); - return vec; - - // for(size_t i = 0; i < Size; i++) { - // vec[i] += other[i]; - // } - // return vec; -} - -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr Vector& operator-=(Vector& vec, const Vector& other) noexcept { for(size_t i = 0; i < Size; i++) { vec[i] -= other[i]; @@ -499,22 +331,6 @@ constexpr Vector& operator-=(Vector& vec, const Vector -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr Vector& operator-=(Vector& vec, const Vector& other) noexcept { - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - stdx::fixed_size_simd other_simd(&other[0], stdx::element_aligned); - vec_simd -= other_simd; - vec_simd.copy_to(vec.data, stdx::element_aligned); - return vec; - - // for(size_t i = 0; i < Size; i++) { - // vec[i] -= other[i]; - // } - // return vec; -} - -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr Vector& operator*=(Vector& vec, const T2& factor) noexcept { for(size_t i = 0; i < Size; i++) { vec[i] *= factor; @@ -523,22 +339,6 @@ constexpr Vector& operator*=(Vector& vec, const T2& factor) } template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr Vector& operator*=(Vector& vec, const T2& factor) noexcept { - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - stdx::fixed_size_simd factor_simd = factor; - vec_simd *= factor_simd; - vec_simd.copy_to(vec.data, stdx::element_aligned); - return vec; - - // for(size_t i = 0; i < Size; i++) { - // vec[i] *= factor; - // } - // return vec; -} - -template -requires (!(std::is_arithmetic_v && std::is_arithmetic_v)) constexpr Vector& operator/=(Vector& vec, const T2& factor) noexcept { for(size_t i = 0; i < Size; i++) { vec[i] /= factor; @@ -546,21 +346,6 @@ constexpr Vector& operator/=(Vector& vec, const T2& factor) return vec; } -template -requires std::is_arithmetic_v && std::is_arithmetic_v -constexpr Vector& operator/=(Vector& vec, const T2& factor) noexcept { - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - stdx::fixed_size_simd factor_simd = factor; - vec_simd /= factor_simd; - vec_simd.copy_to(vec.data, stdx::element_aligned); - return vec; - - // for(size_t i = 0; i < Size; i++) { - // vec[i] /= factor; - // } - // return vec; -} - template constexpr auto operator%(const Vector& first, const Vector& second) noexcept -> decltype(first[0] * second[1] - first[1] * second[0]) { return first[0] * second[1] - first[1] * second[0]; @@ -646,7 +431,12 @@ constexpr Vector withoutIndex(const Vector& vec, size_t in template constexpr T lengthSquared(const Vector& vec) noexcept { - return vec * vec; + T sum = vec[0] * vec[0]; + + for(size_t i = 1; i < Size; i++) { + sum += vec[i] * vec[i]; + } + return sum; } template @@ -696,26 +486,12 @@ constexpr Vector normalize(const Vector& vec) noexcept { return vec / length(vec); } -// template -// constexpr Vector abs(const Vector& vec) noexcept { -// Vector result; -// for(size_t i = 0; i < Size; i++) -// result[i] = std::abs(vec[i]); -// return result; -// } - template constexpr Vector abs(const Vector& vec) noexcept { - stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); - vec_simd = stdx::abs(vec_simd); - Vector result_vec; - vec_simd.copy_to(result_vec.data, stdx::element_aligned); - return result_vec; - - // Vector result; - // for(size_t i = 0; i < Size; i++) - // result[i] = std::abs(vec[i]); - // return result; + Vector result; + for(size_t i = 0; i < Size; i++) + result[i] = std::abs(vec[i]); + return result; } /** @@ -771,34 +547,15 @@ constexpr T pointToLineDistanceSquared(const Vector& line, const Vector return lengthSquared(point - project(point, line)); } - template -// requires (!(std::is_arithmetic_v)) constexpr Vector elementWiseMul(const Vector& first, const Vector& second) noexcept { Vector result; for(size_t i = 0; i < Size; i++) result[i] = first[i] * second[i]; return result; } -// template -// requires std::is_arithmetic_v -// constexpr Vector elementWiseMul(const Vector& first, const Vector& second) noexcept { -// stdx::fixed_size_simd result; -// stdx::fixed_size_simd first_simd(&first[0], stdx::element_aligned); -// stdx::fixed_size_simd second_simd(&second[0], stdx::element_aligned); -// result = first_simd * second_simd; -// Vector result_vec; -// result.copy_to(result_vec.data, stdx::element_aligned); -// return result_vec; - -// // Vector result; -// // for(size_t i = 0; i < Size; i++) -// // result[i] = first[i] * second[i]; -// // return result; -// } template -// requires (!(std::is_arithmetic_v)) constexpr Vector elementWiseSquare(const Vector& vec) noexcept { Vector result; for(size_t i = 0; i < Size; i++) @@ -806,24 +563,7 @@ constexpr Vector elementWiseSquare(const Vector& vec) noexcept return result; } -// template -// requires std::is_arithmetic_v -// constexpr Vector elementWiseSquare(const Vector& vec) noexcept { -// stdx::fixed_size_simd result; -// stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); -// result = vec_simd * vec_simd; -// Vector result_vec; -// result.copy_to(result_vec.data, stdx::element_aligned); -// return result_vec; - -// // Vector result; -// // for(size_t i = 0; i < Size; i++) -// // result[i] = vec[i] * vec[i]; -// // return result; -// } - template -// requires (!(std::is_arithmetic_v)) constexpr Vector elementWiseCube(const Vector& vec) noexcept { Vector result; for(size_t i = 0; i < Size; i++) @@ -831,22 +571,6 @@ constexpr Vector elementWiseCube(const Vector& vec) noexcept { return result; } -// template -// requires std::is_arithmetic_v -// constexpr Vector elementWiseCube(const Vector& vec) noexcept { -// stdx::fixed_size_simd result; -// stdx::fixed_size_simd vec_simd(&vec[0], stdx::element_aligned); -// result = vec_simd * vec_simd * vec_simd; -// Vector result_vec; -// result.copy_to(result_vec.data, stdx::element_aligned); -// return result_vec; - -// // Vector result; -// // for(size_t i = 0; i < Size; i++) -// // result[i] = vec[i] * vec[i] * vec[i]; -// // return result; -// } - /* computes ( a.y * b.z + a.z * b.y, a.z * b.x + a.x * b.z,