423 lines
9.2 KiB
C++
423 lines
9.2 KiB
C++
#pragma once
|
|
|
|
#include <psemek/geom/detail/array.hpp>
|
|
#include <psemek/util/hash.hpp>
|
|
|
|
#include <iostream>
|
|
#include <cstddef>
|
|
#include <utility>
|
|
#include <type_traits>
|
|
#include <stdexcept>
|
|
#include <cmath>
|
|
#include <compare>
|
|
|
|
namespace psemek::geom
|
|
{
|
|
|
|
template <typename T, std::size_t N>
|
|
struct vector
|
|
{
|
|
using scalar_type = T;
|
|
static constexpr std::size_t static_dimension = N;
|
|
|
|
typename detail::array<T, N>::type coords;
|
|
|
|
vector() = default;
|
|
vector(vector const &) = default;
|
|
vector(vector &) = default;
|
|
vector(vector &&) = default;
|
|
vector & operator = (vector const &) = default;
|
|
vector & operator = (vector &) = default;
|
|
vector & operator = (vector &&) = default;
|
|
|
|
template <typename ... Args, typename = std::enable_if_t<(sizeof...(Args) == N) && detail::all_convertible_to<T, Args...>::value>>
|
|
vector(Args && ... args)
|
|
: coords{ static_cast<T>(std::forward<Args>(args))... }
|
|
{}
|
|
|
|
std::size_t dimension() const
|
|
{
|
|
return N;
|
|
}
|
|
|
|
T & operator[](std::size_t i)
|
|
{
|
|
return coords[i];
|
|
}
|
|
|
|
T const & operator[](std::size_t i) const
|
|
{
|
|
return coords[i];
|
|
}
|
|
|
|
vector & operator *= (T const & s);
|
|
vector & operator /= (T const & s);
|
|
|
|
vector & operator += (vector const & v);
|
|
vector & operator -= (vector const & v);
|
|
|
|
static vector zero();
|
|
};
|
|
|
|
template <typename ... Args>
|
|
vector(Args && ...) -> vector<std::common_type_t<Args...>, sizeof...(Args)>;
|
|
|
|
template <typename T1, typename T, std::size_t N>
|
|
vector<T1, N> cast(vector<T, N> const & v)
|
|
{
|
|
vector<T1, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = static_cast<T1>(v[i]);
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
std::strong_ordering operator <=> (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
{
|
|
if (v1[i] < v2[i])
|
|
return std::strong_ordering::less;
|
|
else if (v1[i] > v2[i])
|
|
return std::strong_ordering::greater;
|
|
}
|
|
return std::strong_ordering::equal;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool operator == (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return (v1 <=> v2) == std::strong_ordering::equal;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool operator != (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return !(v1 == v2);
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool operator < (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return (v1 <=> v2) == std::strong_ordering::less;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool operator > (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return (v2 < v1);
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool operator <= (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return !(v2 < v1);
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
bool operator >= (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return !(v1 < v2);
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> operator * (vector<T, N> const & v, T const & s)
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = v[i] * s;
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> operator * (T const & s, vector<T, N> const & v)
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = s * v[i];
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> operator / (vector<T, N> const & v, T const & s)
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = v[i] / s;
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> operator - (vector<T, N> const & v)
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = -v[i];
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> operator + (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = v1[i] + v2[i];
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> operator - (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = v1[i] - v2[i];
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> & vector<T, N>::operator *= (T const & s)
|
|
{
|
|
return (*this) = (*this) * s;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> & vector<T, N>::operator /= (T const & s)
|
|
{
|
|
return (*this) = (*this) / s;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> & vector<T, N>::operator += (vector<T, N> const & v)
|
|
{
|
|
return (*this) = (*this) + v;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> & vector<T, N>::operator -= (vector<T, N> const & v)
|
|
{
|
|
return (*this) = (*this) - v;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> vector<T, N>::zero()
|
|
{
|
|
vector<T, N> r;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r[i] = T(0);
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
T dot(vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
T r{};
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
r += v1[i] * v2[i];
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
T length_sqr(vector<T, N> const & v)
|
|
{
|
|
return dot(v, v);
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
T linf_norm(vector<T, N> const & v)
|
|
{
|
|
T r = std::abs(v[0]);
|
|
for (std::size_t i = 1; i < N; ++i)
|
|
r = std::max(r, std::abs(v[i]));
|
|
return r;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
T length(vector<T, N> const & v)
|
|
{
|
|
T const m = linf_norm(v);
|
|
if (m == 0) return m;
|
|
return m * std::sqrt(length_sqr(v / m));
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> normalized(vector<T, N> const & v)
|
|
{
|
|
return v / length(v);
|
|
}
|
|
|
|
// TODO: generic implementation
|
|
template <typename T>
|
|
T det(vector<T, 2> const & v0, vector<T, 2> const & v1)
|
|
{
|
|
return v0[0] * v1[1] - v0[1] * v1[0];
|
|
}
|
|
|
|
template <typename T>
|
|
T det(vector<T, 3> const & v0, vector<T, 3> const & v1, vector<T, 3> const & v2)
|
|
{
|
|
return
|
|
+ v0[0] * v1[1] * v2[2]
|
|
- v0[0] * v1[2] * v2[1]
|
|
- v0[1] * v1[0] * v2[2]
|
|
+ v0[1] * v1[2] * v2[0]
|
|
+ v0[2] * v1[0] * v2[1]
|
|
- v0[2] * v1[1] * v2[0]
|
|
;
|
|
}
|
|
|
|
// TODO: generic implementation (shares internals with det)
|
|
template <typename T>
|
|
vector<T, 3> ort(vector<T, 3> const & v0, vector<T, 3> const & v1)
|
|
{
|
|
return vector<T, 3>{
|
|
v0[1] * v1[2] - v0[2] * v1[1],
|
|
v0[2] * v1[0] - v0[0] * v1[2],
|
|
v0[0] * v1[1] - v0[1] * v1[0],
|
|
};
|
|
}
|
|
|
|
// Any vector orthogonal to v
|
|
// N.B.: discontinuous in odd dimensions
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> ort(vector<T, N> const & v)
|
|
{
|
|
vector<T, N> result;
|
|
if constexpr ((N % 2) == 0)
|
|
{
|
|
for (std::size_t i = 0; i < N; i += 2)
|
|
{
|
|
result[i] = -v[i + 1];
|
|
result[i + 1] = v[i];
|
|
}
|
|
}
|
|
else
|
|
{
|
|
std::size_t i = 0, j = 1;
|
|
|
|
for (std::size_t k = 1; k < N; ++k)
|
|
{
|
|
if (std::abs(v[k]) > std::abs(v[i]))
|
|
{
|
|
j = i;
|
|
i = k;
|
|
}
|
|
}
|
|
|
|
result = vector<T, N>::zero();
|
|
result[i] = -v[j];
|
|
result[j] = v[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
template <typename T>
|
|
vector<T, 3> cross(vector<T, 3> const & v0, vector<T, 3> const & v1)
|
|
{
|
|
return ort(v0, v1);
|
|
}
|
|
|
|
template <typename T>
|
|
vector<T, 2> direction(T angle)
|
|
{
|
|
return {std::cos(angle), std::sin(angle)};
|
|
}
|
|
|
|
template <typename T>
|
|
T angle(vector<T, 2> const & v)
|
|
{
|
|
return std::atan2(v[1], v[0]);
|
|
}
|
|
|
|
template <typename T>
|
|
T angle(vector<T, 2> const & v0, vector<T, 2> const & v1)
|
|
{
|
|
return std::atan2(det(v0, v1), dot(v0, v1));
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
T angle(vector<T, N> const & v0, vector<T, N> const & v1)
|
|
{
|
|
return std::acos(std::min(T{1}, std::max(T{-1}, dot(v0, v1))));
|
|
}
|
|
|
|
template <typename T>
|
|
vector<T, 2> rotate(vector<T, 2> const & v, T angle)
|
|
{
|
|
auto const c = std::cos(angle);
|
|
auto const s = std::sin(angle);
|
|
return {v[0] * c - v[1] * s, v[0] * s + v[1] * c};
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> pointwise_mult(vector<T, N> const & v0, vector<T, N> const & v1)
|
|
{
|
|
vector<T, N> result;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
result[i] = v0[i] * v1[i];
|
|
return result;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> lerp(vector<T, N> const & v0, vector<T, N> const & v1, T const & t)
|
|
{
|
|
return v0 * (T(1) - t) + v1 * t;
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
vector<T, N> slerp(vector<T, N> const & v0, vector<T, N> const & v1, T const & t)
|
|
{
|
|
// threshold is chosen so that for abs(x) < threshold the second term in
|
|
// sin(x) Taylor series is less than the minimum value representable by T
|
|
static auto const threshold = std::pow(6 * std::numeric_limits<T>::min(), T{1}/T{3});
|
|
|
|
auto const omega = angle(normalized(v0), normalized(v1));
|
|
|
|
// NB: the case of omega ~ pi is ambiguous and isn't handled in any special way
|
|
|
|
if (std::abs(omega) < threshold)
|
|
{
|
|
// prevent division by zero
|
|
return normalized(lerp(v0, v1, t));
|
|
}
|
|
else
|
|
{
|
|
auto const s = std::sin(omega);
|
|
return v0 * std::sin((1 - t) * omega) / s + v1 * std::sin(t * omega) / s;
|
|
}
|
|
}
|
|
|
|
template <typename T, std::size_t N>
|
|
std::ostream & operator << (std::ostream & os, vector<T, N> const & v)
|
|
{
|
|
if constexpr (N == 0)
|
|
{
|
|
os << "()";
|
|
return os;
|
|
}
|
|
|
|
os << '(' << v[0];
|
|
for (std::size_t i = 1; i < N; ++i)
|
|
os << ", " << v[i];
|
|
os << ')';
|
|
return os;
|
|
}
|
|
|
|
}
|
|
|
|
namespace std
|
|
{
|
|
|
|
template <typename T, std::size_t N>
|
|
struct hash<::psemek::geom::vector<T, N>>
|
|
{
|
|
std::size_t operator()(::psemek::geom::vector<T, N> const & v) const noexcept
|
|
{
|
|
std::hash<T> h;
|
|
std::size_t r = 0;
|
|
for (std::size_t i = 0; i < N; ++i)
|
|
::psemek::util::hash_combine(r, h(v[i]));
|
|
return r;
|
|
}
|
|
};
|
|
|
|
}
|