359 lines
7.8 KiB
C++
359 lines
7.8 KiB
C++
#pragma once
|
|
|
|
#include <psemek/geom/detail/array.hpp>
|
|
|
|
#include <cstddef>
|
|
#include <utility>
|
|
#include <type_traits>
|
|
#include <stdexcept>
|
|
#include <cmath>
|
|
|
|
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>
|
|
vector(Args && ... args)
|
|
: coords{ static_cast<T>(std::forward<Args>(args))... }
|
|
{
|
|
static_assert(sizeof...(Args) == N);
|
|
}
|
|
|
|
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 T, std::size_t N>
|
|
bool operator == (vector<T, N> const & v1, vector<T, N> const & v2)
|
|
{
|
|
return std::equal(v1.coords, v1.coords + N, v2.coords);
|
|
}
|
|
|
|
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 std::lexicographical_compare(v1.coords, v1.coords + N, v2.coords, v2.coords + N);
|
|
}
|
|
|
|
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 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>
|
|
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, 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 = std::acos(dot(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 Stream, typename T, std::size_t N>
|
|
Stream & operator << (Stream & 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;
|
|
}
|
|
|
|
}
|