olegpz
/
ericw-tools
mirror of https://github.com/ericwa/ericw-tools.git
0
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ericw-tools/include/common/qvec.hh

1233 lines
35 KiB
C++
Raw Blame History

/* Copyright (C) 2017 Eric Wasylishen
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
See file, 'COPYING', for details.
*/
#pragma once
#include <initializer_list>
#include <cassert>
#include <cmath>
#include <cstdint>
#include <string>
#include <algorithm>
#include <array>
#include <ostream>
#include <fmt/core.h>
#include <tuple>
#include <functional>
#include "common/mathlib.hh"
template<class T, size_t N>
class qvec
{
protected:
std::array<T, N> v;
template<class T2, size_t N2>
friend class qvec;
public:
using value_type = T;
constexpr qvec() = default;
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wunused-value"
#endif
template<typename... Args,
typename = std::enable_if_t<sizeof...(Args) && std::is_convertible_v<std::common_type_t<Args...>, T>>>
constexpr qvec(Args... a)
: v({})
{
constexpr size_t count = sizeof...(Args);
// special case for single argument
if constexpr (count == 1) {
for (auto &e : v)
((e = a, true) || ...);
}
// multiple arguments; copy up to min(N, `count`),
// leave `count` -> N as zeroes
else {
constexpr size_t copy_size = std::min(N, count);
size_t i = 0;
((i++ < copy_size ? (v[i - 1] = a, true) : false), ...);
}
}
#ifdef __clang__
#pragma clang diagnostic pop
#endif
private:
template<typename FT, std::size_t... pack>
constexpr void copy_impl(const FT &from, std::index_sequence<pack...> packed)
{
((v[pack] = from[pack]), ...);
}
public:
// copy from C-style array, exact lengths only
template<typename T2>
constexpr qvec(const T2 (&array)[N])
{
copy_impl(array, std::make_index_sequence<N>());
}
// copy from std::array, exact lengths only
template<typename T2>
constexpr qvec(const std::array<T2, N> &array)
{
copy_impl(array, std::make_index_sequence<N>());
}
/**
* Casting from another vector type of the same length
*/
template<class T2>
constexpr qvec(const qvec<T2, N> &other)
{
copy_impl(other, std::make_index_sequence<N>());
}
private:
template<typename FT, std::size_t... pack>
constexpr void copy_trunc_impl(const FT &from, std::index_sequence<pack...> packed)
{
((
(pack < N) ?
(v[pack] = ((pack < from.size() ? (from[pack]) : 0)))
:
(false)
), ...);
}
public:
/**
* Casting from another vector type of the same type but
* different length
*/
template<size_t N2>
constexpr qvec(const qvec<T, N2> &other)
{
// truncates if `other` is longer than `this`
// zero-fill if `other` is smaller than `this`
copy_trunc_impl(other, std::make_index_sequence<std::max(N, N2)>());
}
/**
* Extending a vector
*/
constexpr qvec(const qvec<T, N - 1> &other, T value)
{
std::copy(other.begin(), other.end(), v.begin());
v[N - 1] = value;
}
[[nodiscard]] constexpr size_t size() const { return N; }
// Sort support
[[nodiscard]] constexpr bool operator<(const qvec &other) const { return v < other.v; }
[[nodiscard]] constexpr bool operator<=(const qvec &other) const { return v <= other.v; }
[[nodiscard]] constexpr bool operator>(const qvec &other) const { return v > other.v; }
[[nodiscard]] constexpr bool operator>=(const qvec &other) const { return v >= other.v; }
[[nodiscard]] constexpr bool operator==(const qvec &other) const { return v == other.v; }
[[nodiscard]] constexpr bool operator!=(const qvec &other) const { return v != other.v; }
[[nodiscard]] constexpr const T &at(const size_t idx) const
{
assert(idx >= 0 && idx < N);
return v[idx];
}
[[nodiscard]] constexpr T &at(const size_t idx)
{
assert(idx >= 0 && idx < N);
return v[idx];
}
[[nodiscard]] constexpr const T &operator[](const size_t idx) const { return at(idx); }
[[nodiscard]] constexpr T &operator[](const size_t idx) { return at(idx); }
private:
// OUT = op THIS[N]
template<class UnaryOperation, std::size_t... pack>
constexpr auto utransform_impl(UnaryOperation func, std::index_sequence<pack...>) const
{
using R = decltype(-T());
return qvec<R, N> { func(at(pack))... };
}
// OUT = THIS[N] op IN
template<class BinaryOperation, typename InputType, std::size_t... pack>
constexpr auto transform_impl(BinaryOperation func, const InputType &b, std::index_sequence<pack...>) const
{
using R = decltype(func(T(), InputType()));
return qvec<R, N> { func(at(pack), b)... };
}
// OUT = THIS[N] op IN[N]
template<class BinaryOperation, typename InputType, std::size_t... pack>
constexpr auto transformv_impl(BinaryOperation func, const InputType &b, std::index_sequence<pack...>) const
{
using R = decltype(func(T(), InputType()[0]));
return qvec<R, N> { func(at(pack), b[pack])... };
}
public:
template<class UnaryOperation>
constexpr auto utransform(UnaryOperation func) const
{
return utransform_impl(func, std::make_index_sequence<N>());
}
template<class BinaryOperation, typename InputType>
constexpr auto transform(BinaryOperation func, const InputType &b) const
{
return transform_impl(func, b, std::make_index_sequence<N>());
}
template<class BinaryOperation, typename InputType>
constexpr auto transformv(BinaryOperation func, const InputType &b) const
{
return transformv_impl(func, b, std::make_index_sequence<N>());
}
template<typename F>
[[nodiscard]] constexpr inline auto operator+(const qvec<F, N> &other) const
{
return transformv(std::plus(), other);
}
template<typename F>
[[nodiscard]] constexpr inline auto operator-(const qvec<F, N> &other) const
{
return transformv(std::minus(), other);
}
template<typename S>
[[nodiscard]] constexpr inline auto operator*(const S &scale) const
{
return transform(std::multiplies(), scale);
}
template<typename F>
[[nodiscard]] constexpr inline auto operator*(const qvec<F, N> &scale) const
{
return transformv(std::multiplies(), scale);
}
template<typename S>
[[nodiscard]] constexpr inline auto operator/(const S &scale) const
{
return transform(std::divides(), scale);
}
template<typename F>
[[nodiscard]] constexpr inline auto operator/(const qvec<F, N> &scale) const
{
return transformv(std::divides(), scale);
}
[[nodiscard]] constexpr inline auto operator-() const
{
return utransform(std::negate());
}
template<typename F>
constexpr qvec operator+=(const qvec<F, N> &other)
{
return *this = *this + other;
}
template<typename F>
constexpr qvec operator-=(const qvec<F, N> &other)
{
return *this = *this - other;
}
template<typename S>
constexpr qvec operator*=(const S &scale)
{
return *this = *this * scale;
}
template<typename F>
constexpr qvec &operator*=(const qvec<F, N> &other)
{
return *this = *this * other;
}
template<typename S>
constexpr qvec operator/=(const S &scale)
{
return *this = *this / scale;
}
template<typename F>
constexpr qvec operator/=(const qvec<F, N> &other)
{
return *this = *this * other;
}
[[nodiscard]] constexpr qvec<T, 3> xyz() const
{
static_assert(N >= 3);
return qvec<T, 3>(*this);
}
// stream support
auto stream_data() { return std::tie(v); }
// iterator support
constexpr auto begin() { return v.begin(); }
constexpr auto end() { return v.end(); }
constexpr auto begin() const { return v.begin(); }
constexpr auto end() const { return v.end(); }
constexpr auto cbegin() const { return v.cbegin(); }
constexpr auto cend() const { return v.cend(); }
};
// Fmt support
template<class T, size_t N>
struct fmt::formatter<qvec<T, N>>
{
constexpr auto parse(format_parse_context &ctx) -> decltype(ctx.begin()) { return ctx.end(); }
template<typename FormatContext>
auto format(const qvec<T, N> &p, FormatContext &ctx) -> decltype(ctx.out())
{
for (size_t i = 0; i < N - 1; i++) {
fmt::format_to(ctx.out(), "{}", p[i]);
fmt::format_to(ctx.out(), " ");
}
return fmt::format_to(ctx.out(), "{}", p[N - 1]);
}
};
namespace qv
{
template<class T, class T2>
[[nodiscard]] constexpr auto cross(const qvec<T, 3> &v1, const qvec<T2, 3> &v2)
{
return qvec<decltype(T() * T2() - T() * T2()), 3>{
v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2], v1[0] * v2[1] - v1[1] * v2[0]};
}
template<typename T, typename U, std::size_t... pack>
constexpr auto dot_impl(const T &v1, const U &v2, std::index_sequence<pack...> packed)
{
return ((v1[pack] * v2[pack]) + ...);
}
template<typename T, typename U, typename L = std::common_type_t<typename T::value_type, typename U::value_type>>
[[nodiscard]] constexpr L dot(const T &v1, const U &v2)
{
static_assert(std::size(T()) == std::size(U()), "Can't dot() with two differently-sized vectors");
constexpr size_t N = std::size(T());
return dot_impl(v1, v2, std::make_index_sequence<N>());
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> floor(const qvec<T, N> &v1)
{
qvec<T, N> res;
for (size_t i = 0; i < N; i++) {
res[i] = std::floor(v1[i]);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> ceil(const qvec<T, N> &v1)
{
qvec<T, N> res;
for (size_t i = 0; i < N; i++) {
res[i] = std::ceil(v1[i]);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> pow(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
qvec<T, N> res;
for (size_t i = 0; i < N; i++) {
res[i] = std::pow(v1[i], v2[i]);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] constexpr T min(const qvec<T, N> &v)
{
T res = std::numeric_limits<T>::largest();
for (auto &c : v) {
res = std::min(c, res);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] constexpr T max(const qvec<T, N> &v)
{
T res = std::numeric_limits<T>::lowest();
for (auto &c : v) {
res = std::max(c, res);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> abs(const qvec<T, N> &v)
{
qvec<T, N> res;
for (size_t i = 0; i < N; i++) {
res[i] = std::abs(v[i]);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> min(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
qvec<T, N> res;
for (size_t i = 0; i < N; i++) {
res[i] = std::min(v1[i], v2[i]);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> max(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
qvec<T, N> res;
for (size_t i = 0; i < N; i++) {
res[i] = std::max(v1[i], v2[i]);
}
return res;
}
template<size_t N, class T>
[[nodiscard]] constexpr T length2(const qvec<T, N> &v1)
{
return qv::dot(v1, v1);
}
template<size_t N, class T>
[[nodiscard]] inline T length(const qvec<T, N> &v1)
{
return std::sqrt(length2(v1));
}
template<size_t N, class T, class T2>
[[nodiscard]] inline T distance(const qvec<T, N> &v1, const qvec<T2, N> &v2)
{
return length(v2 - v1);
}
template<size_t N, class T>
[[nodiscard]] constexpr T distance2(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
return length2(v2 - v1);
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> normalize(const qvec<T, N> &v1, T &len)
{
len = length(v1);
return len ? (v1 / len) : v1;
}
template<size_t N, class T>
[[nodiscard]] inline qvec<T, N> normalize(const qvec<T, N> &v1)
{
T len = length(v1);
return len ? (v1 / len) : v1;
}
template<size_t N, class T>
inline T normalizeInPlace(qvec<T, N> &v1)
{
T len = length(v1);
if (len) {
v1 /= len;
}
return len;
}
template<typename T>
[[nodiscard]] inline std::string to_string(const qvec<T, 3> &v1)
{
return fmt::format("{}", v1);
}
// explicit specialization, for reducing compile times
template<>
[[nodiscard]] std::string to_string(const qvec<double, 3> &v1);
// explicit specialization, for reducing compile times
template<>
[[nodiscard]] std::string to_string(const qvec<int, 3> &v1);
template<typename T>
[[nodiscard]] inline bool epsilonEqual(const T &v1, const T &v2, T epsilon)
{
return fabs(v1 - v2) <= epsilon;
}
template<size_t N, class T>
[[nodiscard]] inline bool epsilonEqual(const qvec<T, N> &v1, const qvec<T, N> &v2, T epsilon)
{
for (size_t i = 0; i < N; i++) {
if (!epsilonEqual(v1[i], v2[i], epsilon))
return false;
}
return true;
}
template<size_t N, class T>
[[nodiscard]] inline bool epsilonEmpty(const qvec<T, N> &v1, T epsilon)
{
return epsilonEqual({}, v1, epsilon);
}
template<typename T>
[[nodiscard]] inline bool gate(const T &v1, T epsilon)
{
return v1 <= epsilon;
}
template<size_t N, class T>
[[nodiscard]] inline bool gate(const qvec<T, N> &v, T epsilon)
{
for (size_t i = 0; i < N; i++) {
if (!gate(v[i], epsilon)) {
return false;
}
}
return true;
}
template<size_t N, class T>
[[nodiscard]] constexpr bool equalExact(const qvec<T, N> &v1, const qvec<T, N> &v2)
{
return v1 == v2;
}
template<size_t N, class T>
[[nodiscard]] constexpr bool emptyExact(const qvec<T, N> &v1)
{
return equalExact({}, v1);
}
template<size_t N, class T>
[[nodiscard]] inline size_t indexOfLargestMagnitudeComponent(const qvec<T, N> &v)
{
size_t largestIndex = 0;
T largestMag = 0;
for (size_t i = 0; i < N; ++i) {
const T currentMag = std::fabs(v[i]);
if (currentMag > largestMag) {
largestMag = currentMag;
largestIndex = i;
}
}
return largestIndex;
}
template<typename T>
std::tuple<qvec<T, 3>, qvec<T, 3>> MakeTangentAndBitangentUnnormalized(const qvec<T, 3> &normal)
{
// 0, 1, or 2
const int axis = qv::indexOfLargestMagnitudeComponent(normal);
const int otherAxisA = (axis + 1) % 3;
const int otherAxisB = (axis + 2) % 3;
// setup two other vectors that are perpendicular to each other
qvec<T, 3> otherVecA{};
otherVecA[otherAxisA] = 1.0;
qvec<T, 3> otherVecB{};
otherVecB[otherAxisB] = 1.0;
auto tangent = qv::cross(normal, otherVecA);
auto bitangent = qv::cross(normal, otherVecB);
// We want `test` to point in the same direction as normal.
// Swap the tangent bitangent if we got the direction wrong.
qvec<T, 3> test = qv::cross(tangent, bitangent);
if (qv::dot(test, normal) < 0) {
std::swap(tangent, bitangent);
}
// debug test
#ifdef PARANOID
if (0) {
auto n = qv::normalize(qv::cross(tangent, bitangent));
double d = qv::distance(n, normal);
assert(d < 0.0001);
}
#endif
return {tangent, bitangent};
}
template<size_t N, typename T>
[[nodiscard]] inline T TriangleArea(const qvec<T, N> &v0, const qvec<T, N> &v1, const qvec<T, N> &v2)
{
return static_cast<T>(0.5) * qv::length(qv::cross(v2 - v0, v1 - v0));
}
template<typename Iter, typename T = typename std::iterator_traits<Iter>::value_type>
[[nodiscard]] inline T PolyCentroid(Iter begin, Iter end)
{
using value_type = typename T::value_type;
size_t num_points = end - begin;
if (!num_points)
return qvec<value_type, 3>(std::numeric_limits<value_type>::quiet_NaN());
else if (num_points == 1)
return *begin;
else if (num_points == 2)
return avg(*begin, *(begin + 1));
T poly_centroid{};
value_type poly_area = 0;
const T &v0 = *begin;
for (auto it = begin + 2; it != end; ++it) {
const T &v1 = *(it - 1);
const T &v2 = *it;
const value_type triarea = TriangleArea(v0, v1, v2);
const T tricentroid = avg(v0, v1, v2);
poly_area += triarea;
poly_centroid = poly_centroid + (tricentroid * triarea);
}
poly_centroid /= poly_area;
return poly_centroid;
}
template<typename Iter, typename T = typename std::iterator_traits<Iter>::value_type,
typename F = typename T::value_type>
[[nodiscard]] inline F PolyArea(Iter begin, Iter end)
{
if ((end - begin) < 3)
return static_cast<F>(0);
float poly_area = 0;
const T &v0 = *begin;
for (auto it = begin + 2; it != end; ++it) {
const T &v1 = *(it - 1);
const T &v2 = *it;
poly_area += TriangleArea(v0, v1, v2);
}
return poly_area;
}
template<typename T>
[[nodiscard]] inline qvec<T, 3> Barycentric_FromPoint(
const qvec<T, 3> &p, const qvec<T, 3> &t0, const qvec<T, 3> &t1, const qvec<T, 3> &t2)
{
const auto v0 = t1 - t0;
const auto v1 = t2 - t0;
const auto v2 = p - t0;
T d00 = qv::dot(v0, v0);
T d01 = qv::dot(v0, v1);
T d11 = qv::dot(v1, v1);
T d20 = qv::dot(v2, v0);
T d21 = qv::dot(v2, v1);
T invDenom = (d00 * d11 - d01 * d01);
invDenom = 1.0 / invDenom;
qvec<T, 3> res;
res[1] = (d11 * d20 - d01 * d21) * invDenom;
res[2] = (d00 * d21 - d01 * d20) * invDenom;
res[0] = 1.0 - res[1] - res[2];
return res;
}
// from global illumination total compendium p. 12
template<typename T>
[[nodiscard]] inline qvec<T, 3> Barycentric_Random(T r1, T r2)
{
qvec<T, 3> res;
res[0] = 1.0 - sqrt(r1);
res[1] = r2 * sqrt(r1);
res[2] = 1.0 - res[0] - res[1];
return res;
}
/// Evaluates the given barycentric coord for the given triangle
template<typename T>
[[nodiscard]] constexpr qvec<T, 3> Barycentric_ToPoint(
const qvec<T, 3> &bary, const qvec<T, 3> &t0, const qvec<T, 3> &t1, const qvec<T, 3> &t2)
{
return (t0 * bary[0]) + (t1 * bary[1]) + (t2 * bary[2]);
}
// Snap vector to nearest axial component
template<typename T>
[[nodiscard]] inline qvec<T, 3> Snap(qvec<T, 3> normal, const T &epsilon = NORMAL_EPSILON)
{
for (auto &v : normal) {
if (fabs(v - 1) < epsilon) {
normal = {};
v = 1;
break;
}
if (fabs(v - -1) < epsilon) {
normal = {};
v = -1;
break;
}
}
return normal;
}
template<typename T>
inline qvec<T, 3> mangle_from_vec(const qvec<T, 3> &v)
{
static constexpr qvec<T, 3> up(0, 0, 1);
static constexpr qvec<T, 3> east(1, 0, 0);
static constexpr qvec<T, 3> north(0, 1, 0);
// get rotation about Z axis
T x = qv::dot(east, v);
T y = qv::dot(north, v);
T theta = atan2(y, x);
// get angle away from Z axis
T cosangleFromUp = qv::dot(up, v);
cosangleFromUp = std::min(std::max(static_cast<T>(-1.0), cosangleFromUp), static_cast<T>(1.0));
T radiansFromUp = acosf(cosangleFromUp);
return qvec<T, 3>{theta, -(radiansFromUp - Q_PI / 2.0), 0} * static_cast<T>(180.0 / Q_PI);
}
/* detect colors with components in 0-1 and scale them to 0-255 */
template<typename T>
constexpr qvec<T, 3> normalize_color_format(const qvec<T, 3> &color)
{
if (color[0] >= 0 && color[0] <= 1 && color[1] >= 0 && color[1] <= 1 && color[2] >= 0 && color[2] <= 1) {
return color * 255;
}
return color;
}
}; // namespace qv
using qvec2f = qvec<float, 2>;
using qvec3f = qvec<float, 3>;
using qvec4f = qvec<float, 4>;
using qvec2d = qvec<double, 2>;
using qvec3d = qvec<double, 3>;
using qvec4d = qvec<double, 4>;
using qvec2i = qvec<int32_t, 2>;
using qvec3i = qvec<int32_t, 3>;
using qvec3s = qvec<int16_t, 3>;
using qvec3b = qvec<uint8_t, 3>;
using qvec4b = qvec<uint8_t, 4>;
template<class T>
class qplane3
{
public:
qvec<T, 3> normal;
T dist;
constexpr qplane3() = default;
constexpr qplane3(const qvec<T, 3> &normal, const T &dist)
: normal(normal),
dist(dist)
{
}
// convert from plane of a different type
template<typename T2>
explicit constexpr qplane3(const qplane3<T2> &plane)
: qplane3(plane.normal, static_cast<T2>(plane.dist))
{
}
private:
auto as_tuple() const { return std::tie(normal, dist); }
public:
// Sort support
[[nodiscard]] constexpr bool operator<(const qplane3 &other) const { return as_tuple() < other.as_tuple(); }
[[nodiscard]] constexpr bool operator<=(const qplane3 &other) const { return as_tuple() <= other.as_tuple(); }
[[nodiscard]] constexpr bool operator>(const qplane3 &other) const { return as_tuple() > other.as_tuple(); }
[[nodiscard]] constexpr bool operator>=(const qplane3 &other) const { return as_tuple() >= other.as_tuple(); }
[[nodiscard]] constexpr bool operator==(const qplane3 &other) const { return as_tuple() == other.as_tuple(); }
[[nodiscard]] constexpr bool operator!=(const qplane3 &other) const { return as_tuple() != other.as_tuple(); }
[[nodiscard]] constexpr const qvec<T, 4> vec4() const { return qvec<T, 4>(normal[0], normal[1], normal[2], dist); }
[[nodiscard]] constexpr qplane3 operator-() const { return {-normal, -dist}; }
// generic case
template<typename F>
[[nodiscard]] inline F distance_to(const qvec<F, 3> &point) const
{
return qv::dot(point, normal) - dist;
}
// stream support
void stream_write(std::ostream &s) const { s <= std::tie(normal, dist); }
void stream_read(std::istream &s) { s >= std::tie(normal, dist); }
};
// Fmt support
template<class T>
struct fmt::formatter<qplane3<T>> : formatter<qvec<T, 3>>
{
template<typename FormatContext>
auto format(const qplane3<T> &p, FormatContext &ctx) -> decltype(ctx.out())
{
fmt::format_to(ctx.out(), "{{normal: ");
fmt::formatter<qvec<T, 3>>::format(p.normal, ctx);
fmt::format_to(ctx.out(), ", dist: {}}}", p.dist);
return ctx.out();
}
};
using qplane3f = qplane3<float>;
using qplane3d = qplane3<double>;
namespace qv
{
template<typename T>
[[nodiscard]] bool epsilonEqual(
const qplane3<T> &p1, const qplane3<T> &p2, T normalEpsilon = NORMAL_EPSILON, T distEpsilon = DIST_EPSILON)
{
return epsilonEqual(p1.normal, p2.normal, normalEpsilon) && epsilonEqual(p1.dist, p2.dist, distEpsilon);
}
} // namespace qv
/**
* Row x Col matrix of T.
*/
template<class T, size_t NRow, size_t NCol>
class qmat
{
public:
/**
* Column-major order. [ (row0,col0), (row1,col0), .. ]
*/
std::array<T, NRow * NCol> m_values;
public:
/**
* Identity matrix if square, otherwise fill with 0
*/
constexpr qmat()
: m_values({})
{
if constexpr (NRow == NCol) {
// identity matrix
for (size_t i = 0; i < NCol; i++) {
this->at(i, i) = 1;
}
}
}
/**
* Fill with a value
*/
inline qmat(const T &val) { m_values.fill(val); }
// copy constructor
constexpr qmat(const qmat &other)
: m_values(other.m_values)
{
}
/**
* Casting from another matrix type of the same size
*/
template<class T2>
explicit constexpr qmat(const qmat<T2, NRow, NCol> &other)
{
for (size_t i = 0; i < NRow * NCol; i++)
this->m_values[i] = static_cast<T>(other.m_values[i]);
}
// initializer list, column-major order
constexpr qmat(std::initializer_list<T> list)
{
assert(list.size() == NRow * NCol);
std::copy(list.begin(), list.end(), m_values.begin());
}
// static factory, row-major order
static qmat row_major(std::initializer_list<T> list)
{
assert(list.size() == NRow * NCol);
qmat result;
for (size_t i = 0; i < NRow; i++) { // for each row
for (size_t j = 0; j < NCol; j++) { // for each col
result.at(i, j) = *(list.begin() + (i * NCol + j));
}
}
return result;
}
// Sort support
[[nodiscard]] constexpr bool operator<(const qmat &other) const { return m_values < other.m_values; }
[[nodiscard]] constexpr bool operator<=(const qmat &other) const { return m_values <= other.m_values; }
[[nodiscard]] constexpr bool operator>(const qmat &other) const { return m_values > other.m_values; }
[[nodiscard]] constexpr bool operator>=(const qmat &other) const { return m_values >= other.m_values; }
[[nodiscard]] constexpr bool operator==(const qmat &other) const { return m_values == other.m_values; }
[[nodiscard]] constexpr bool operator!=(const qmat &other) const { return m_values != other.m_values; }
// access to elements
[[nodiscard]] constexpr T &at(size_t row, size_t col)
{
assert(row >= 0 && row < NRow);
assert(col >= 0 && col < NCol);
return m_values[col * NRow + row];
}
[[nodiscard]] constexpr T at(size_t row, size_t col) const
{
assert(row >= 0 && row < NRow);
assert(col >= 0 && col < NCol);
return m_values[col * NRow + row];
}
// access row
[[nodiscard]] inline qvec<T, NCol> row(size_t row) const
{
assert(row >= 0 && row < NRow);
qvec<T, NCol> v;
for (size_t i = 0; i < NCol; i++) {
v[i] = at(row, i);
}
return v;
}
constexpr void set_row(size_t row, const qvec<T, NCol> &values)
{
for (size_t i = 0; i < NCol; i++) {
at(row, i) = values[i];
}
}
[[nodiscard]] constexpr const qvec<T, NRow> &col(size_t col) const
{
assert(col >= 0 && col < NCol);
return reinterpret_cast<const qvec<T, NRow> &>(m_values[col * NRow]);
}
inline void set_col(size_t col, const qvec<T, NRow> &values)
{
reinterpret_cast<qvec<T, NRow> &>(m_values[col * NRow]) = values;
}
// multiplication by a vector
[[nodiscard]] constexpr qvec<T, NRow> operator*(const qvec<T, NCol> &vec) const
{
qvec<T, NRow> res{};
for (size_t i = 0; i < NRow; i++) { // for each row
for (size_t j = 0; j < NCol; j++) { // for each col
res[i] += this->at(i, j) * vec[j];
}
}
return res;
}
// multiplication by a matrix
template<size_t PCol>
[[nodiscard]] constexpr qmat<T, NRow, PCol> operator*(const qmat<T, NCol, PCol> &other) const
{
qmat<T, NRow, PCol> res;
for (size_t i = 0; i < NRow; i++) {
for (size_t j = 0; j < PCol; j++) {
T val = 0;
for (size_t k = 0; k < NCol; k++) {
val += this->at(i, k) * other.at(k, j);
}
res.at(i, j) = val;
}
}
return res;
}
// multiplication by a scalar
[[nodiscard]] constexpr qmat operator*(const T &scalar) const
{
qmat res(*this);
for (size_t i = 0; i < NRow * NCol; i++) {
res.m_values[i] *= scalar;
}
return res;
}
[[nodiscard]] constexpr qmat<T, NCol, NRow> transpose() const
{
qmat<T, NCol, NRow> res;
for (size_t i = 0; i < NRow; i++) {
for (size_t j = 0; j < NCol; j++) {
res.at(j, i) = at(i, j);
}
}
return res;
}
};
// Fmt support
template<class T, size_t NRow, size_t NCol>
struct fmt::formatter<qmat<T, NRow, NCol>> : formatter<qvec<T, NCol>>
{
template<typename FormatContext>
auto format(const qmat<T, NRow, NCol> &p, FormatContext &ctx) -> decltype(ctx.out())
{
for (size_t i = 0; i < NRow; i++) {
fmt::format_to(ctx.out(), "[ ");
fmt::formatter<qvec<T, NCol>>::format(p.row(i), ctx);
fmt::format_to(ctx.out(), " ]");
if (i != NRow - 1) {
fmt::format_to(ctx.out(), "\n");
}
}
return ctx.out();
}
};
using qmat2x2f = qmat<float, 2, 2>;
using qmat3x3f = qmat<float, 3, 3>;
using qmat4x4f = qmat<float, 4, 4>;
using qmat2x2d = qmat<double, 2, 2>;
using qmat3x3d = qmat<double, 3, 3>;
using qmat4x4d = qmat<double, 4, 4>;
namespace qv
{
/**
* These return a matrix filled with NaN if there is no inverse.
*/
[[nodiscard]] qmat4x4f inverse(const qmat4x4f &input);
[[nodiscard]] qmat4x4d inverse(const qmat4x4d &input);
[[nodiscard]] qmat2x2f inverse(const qmat2x2f &input);
[[nodiscard]] qmat3x3f inverse(const qmat3x3f &input);
}; // namespace qv
// returns the normalized direction from `start` to `stop` in the `dir` param
// returns the distance from `start` to `stop`
template<typename Tstart, typename Tstop, typename Tdir>
inline double GetDir(const Tstart &start, const Tstop &stop, Tdir &dir)
{
return qv::normalizeInPlace(dir = (stop - start));
}
// Stores a normal, tangent and bitangent
struct face_normal_t
{
qvec3f normal, tangent, bitangent;
};
qmat3x3d RotateAboutX(double radians);
qmat3x3d RotateAboutY(double radians);
qmat3x3d RotateAboutZ(double radians);
qmat3x3f RotateFromUpToSurfaceNormal(const qvec3f &surfaceNormal);
// Returns (0 0 0) if we couldn't determine the normal
qvec3f FaceNormal(std::vector<qvec3f> points);
std::pair<bool, qvec4f> MakeInwardFacingEdgePlane(const qvec3f &v0, const qvec3f &v1, const qvec3f &faceNormal);
std::vector<qvec4f> MakeInwardFacingEdgePlanes(const std::vector<qvec3f> &points);
bool EdgePlanes_PointInside(const std::vector<qvec4f> &edgeplanes, const qvec3f &point);
float EdgePlanes_PointInsideDist(const std::vector<qvec4f> &edgeplanes, const qvec3f &point);
qvec4f MakePlane(const qvec3f &normal, const qvec3f &point);
float DistAbovePlane(const qvec4f &plane, const qvec3f &point);
qvec3f ProjectPointOntoPlane(const qvec4f &plane, const qvec3f &point);
qvec4f PolyPlane(const std::vector<qvec3f> &points);
/// Returns the index of the polygon edge, and the closest point on that edge, to the given point
std::pair<int, qvec3f> ClosestPointOnPolyBoundary(const std::vector<qvec3f> &poly, const qvec3f &point);
/// Returns `true` and the interpolated normal if `point` is in the polygon, otherwise returns false.
std::pair<bool, qvec3f> InterpolateNormal(
const std::vector<qvec3f> &points, const std::vector<face_normal_t> &normals, const qvec3f &point);
std::pair<bool, qvec3f> InterpolateNormal(
const std::vector<qvec3f> &points, const std::vector<qvec3f> &normals, const qvec3f &point);
std::vector<qvec3f> ShrinkPoly(const std::vector<qvec3f> &poly, const float amount);
/// Returns (front part, back part)
std::pair<std::vector<qvec3f>, std::vector<qvec3f>> ClipPoly(const std::vector<qvec3f> &poly, const qvec4f &plane);
class poly_random_point_state_t
{
public:
std::vector<qvec3f> points;
std::vector<float> triareas;
std::vector<float> triareas_cdf;
};
poly_random_point_state_t PolyRandomPoint_Setup(const std::vector<qvec3f> &points);
qvec3f PolyRandomPoint(const poly_random_point_state_t &state, float r1, float r2, float r3);
/// projects p onto the vw line.
/// returns 0 for p==v, 1 for p==w
float FractionOfLine(const qvec3f &v, const qvec3f &w, const qvec3f &p);
/**
* Distance from `p` to the line v<->w (extending infinitely in either direction)
*/
float DistToLine(const qvec3f &v, const qvec3f &w, const qvec3f &p);
qvec3f ClosestPointOnLine(const qvec3f &v, const qvec3f &w, const qvec3f &p);
/**
* Distance from `p` to the line segment v<->w.
* i.e., 0 if `p` is between v and w.
*/
float DistToLineSegment(const qvec3f &v, const qvec3f &w, const qvec3f &p);
qvec3f ClosestPointOnLineSegment(const qvec3f &v, const qvec3f &w, const qvec3f &p);
float SignedDegreesBetweenUnitVectors(const qvec3f &start, const qvec3f &end, const qvec3f &normal);
enum class concavity_t
{
Coplanar,
Concave,
Convex
};
concavity_t FacePairConcavity(
const qvec3f &face1Center, const qvec3f &face1Normal, const qvec3f &face2Center, const qvec3f &face2Normal);
// Returns weights for f(0,0), f(1,0), f(0,1), f(1,1)
// from: https://en.wikipedia.org/wiki/Bilinear_interpolation#Unit_Square
qvec4f bilinearWeights(const float x, const float y);
// This uses a coordinate system where the pixel centers are on integer coords.
// e.g. the corners of a 3x3 pixel bitmap are at (-0.5, -0.5) and (2.5, 2.5).
std::array<std::pair<qvec2i, float>, 4> bilinearWeightsAndCoords(qvec2f pos, const qvec2i &size);
template<typename V>
V bilinearInterpolate(const V &f00, const V &f10, const V &f01, const V &f11, const float x, const float y)
{
qvec4f weights = bilinearWeights(x, y);
const V fxy = f00 * weights[0] + f10 * weights[1] + f01 * weights[2] + f11 * weights[3];
return fxy;
}
template<typename V>
std::vector<V> PointsAlongLine(const V &start, const V &end, const float step)
{
const V linesegment = end - start;
const float len = qv::length(linesegment);
if (len == 0)
return {};
std::vector<V> result;
const int stepCount = static_cast<int>(len / step);
result.reserve(stepCount + 1);
const V dir = linesegment / len;
for (int i = 0; i <= stepCount; i++) {
result.push_back(start + (dir * (step * i)));
}
return result;
}
bool LinesOverlap(const qvec3f &p0, const qvec3f &p1, const qvec3f &q0, const qvec3f &q1,
const double &on_epsilon = DEFAULT_ON_EPSILON);
template<typename T>
struct twosided
{
T front, back;
// 0 is front, 1 is back
constexpr T &operator[](const int32_t &i)
{
switch (i) {
case 0: return front;
case 1: return back;
}
throw std::exception();
}
// 0 is front, 1 is back
constexpr const T &operator[](const int32_t &i) const
{
switch (i) {
case 0: return front;
case 1: return back;
}
throw std::exception();
}
// iterator support
T *begin() { return &front; }
T *end() { return (&back) + 1; }
const T *begin() const { return &front; }
const T *end() const { return (&back) + 1; }
// swap the front and back values
constexpr void swap() { std::swap(front, back); }
};
namespace qv
{
template<typename T>
inline qvec<T, 3> vec_from_mangle(const qvec<T, 3> &m)
{
const qvec<T, 3> mRadians = m * static_cast<T>(Q_PI / 180.0);
const qmat3x3d rotations = RotateAboutZ(mRadians[0]) * RotateAboutY(-mRadians[1]);
return {rotations * qvec3d(1, 0, 0)};
}
} // namespace qv
// for Catch2
template<typename T>
std::ostream &operator<<(std::ostream &os, const qvec<T, 3> &v)
{
return os << qv::to_string(v);
}
Reference in New Issue View Git Blame Copy Permalink