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

1238 lines
33 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 <string>
#include <algorithm>
#include <array>
#include <ostream>
#include <fmt/format.h>
#include <tuple>
#include "common/mathlib.hh"
#include "common/cmdlib.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;
inline 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 = min(N, count);
size_t i = 0;
((i++ < copy_size ? (v[i - 1] = a, true) : false), ...);
}
}
#ifdef __clang__
#pragma clang diagnostic pop
#endif
// copy from C-style array, exact lengths only
template<typename T2>
constexpr qvec(const T2 (&array)[N])
{
for (size_t i = 0; i < N; i++)
v[i] = static_cast<T>(array[i]);
}
// copy from std::array, exact lengths only
template<typename T2>
constexpr qvec(const std::array<T2, N> &array)
{
for (size_t i = 0; i < N; i++)
v[i] = static_cast<T>(array[i]);
}
/**
* Casting from another vector type of the same length
*/
template<class T2>
constexpr qvec(const qvec<T2, N> &other)
{
for (size_t i = 0; i < N; i++)
v[i] = static_cast<T>(other[i]);
}
/**
* Casting from another vector type of the same type but
* different length
*/
template<size_t N2>
constexpr qvec(const qvec<T, N2> &other)
{
constexpr size_t minSize = min(N, N2);
// truncates if `other` is longer than `this`
for (size_t i = 0; i < minSize; i++)
v[i] = other[i];
// zero-fill if `other` is smaller than `this`
if constexpr (N2 < N) {
for (size_t i = minSize; i < N; i++)
v[i] = 0;
}
}
/**
* 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);
}
template<typename F>
[[nodiscard]] inline auto operator+(const qvec<F, N> &other) const
{
qvec<decltype(T() + F()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = at(i) + other[i];
}
return v;
}
template<typename F>
[[nodiscard]] inline auto operator-(const qvec<F, N> &other) const
{
qvec<decltype(T() - F()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = at(i) - other[i];
}
return v;
}
template<typename S>
[[nodiscard]] inline auto operator*(const S &scale) const
{
qvec<decltype(T() * S()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = at(i) * scale;
}
return v;
}
template<typename F>
[[nodiscard]] inline auto operator*(const qvec<F, N> &scale) const
{
qvec<decltype(T() * F()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = at(i) * scale[i];
}
return v;
}
template<typename S>
[[nodiscard]] inline auto operator/(const S &scale) const
{
qvec<decltype(T() / S()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = at(i) / scale;
}
return v;
}
template<typename F>
[[nodiscard]] inline auto operator/(const qvec<F, N> &scale) const
{
qvec<decltype(T() / F()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = at(i) / scale[i];
}
return v;
}
[[nodiscard]] inline auto operator-() const
{
qvec<decltype(-T()), N> v;
for (size_t i = 0; i < N; i++) {
v[i] = -at(i);
}
return v;
}
template<typename F>
inline qvec operator+=(const qvec<F, N> &other)
{
return *this = *this + other;
}
template<typename F>
inline qvec operator-=(const qvec<F, N> &other)
{
return *this = *this - other;
}
template<typename S>
inline qvec operator*=(const S &scale)
{
return *this = *this * scale;
}
template<typename F>
inline qvec &operator*=(const qvec<F, N> &other)
{
return *this = *this * other;
}
template<typename S>
inline qvec operator/=(const S &scale)
{
return *this = *this / scale;
}
template<typename F>
inline 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();
}
// for Google Test
friend std::ostream &operator<<(std::ostream &os, const qvec &v)
{
return os << fmt::format("{}", v);
}
};
// Fmt support
template<class T, size_t N>
struct fmt::formatter<qvec<T, N>> : formatter<T>
{
template<typename FormatContext>
auto format(const qvec<T, N> &p, FormatContext &ctx) -> decltype(ctx.out())
{
for (size_t i = 0; i < N - 1; i++) {
formatter<T>::format(p[i], ctx);
format_to(ctx.out(), " ");
}
return formatter<T>::format(p[N - 1], ctx);
}
};
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, 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");
L result = 0;
for (size_t i = 0; i < std::size(v1); i++) {
result += v1[i] * v2[i];
}
return result;
}
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 = ::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 = ::max(c, res);
}
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] = ::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] = ::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);
}
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<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
if (0) {
auto n = qv::normalize(qv::cross(tangent, bitangent));
double d = qv::distance(n, normal);
assert(d < 0.0001);
}
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 = ::min(::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>
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]] inline T dist_above(const qvec<T, 3> &pt) const { return qv::dot(pt, normal) - dist; }
[[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
auto stream_data() { return std::tie(normal, dist); }
};
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>
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());
}
// 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;
}
};
// 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++) {
format_to(ctx.out(), "[ ");
formatter<qvec<T, NCol>>::format(p.row(i), ctx);
format_to(ctx.out(), " ]");
if (i != NRow - 1) {
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);
}; // 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 vec_t 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 GLM_FaceNormal(std::vector<qvec3f> points);
std::pair<bool, qvec4f> GLM_MakeInwardFacingEdgePlane(const qvec3f &v0, const qvec3f &v1, const qvec3f &faceNormal);
std::vector<qvec4f> GLM_MakeInwardFacingEdgePlanes(const std::vector<qvec3f> &points);
bool GLM_EdgePlanes_PointInside(const std::vector<qvec4f> &edgeplanes, const qvec3f &point);
float GLM_EdgePlanes_PointInsideDist(const std::vector<qvec4f> &edgeplanes, const qvec3f &point);
qvec4f GLM_MakePlane(const qvec3f &normal, const qvec3f &point);
float GLM_DistAbovePlane(const qvec4f &plane, const qvec3f &point);
qvec3f GLM_ProjectPointOntoPlane(const qvec4f &plane, const qvec3f &point);
qvec4f GLM_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> GLM_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> GLM_InterpolateNormal(
const std::vector<qvec3f> &points, const std::vector<face_normal_t> &normals, const qvec3f &point);
std::pair<bool, qvec3f> GLM_InterpolateNormal(
const std::vector<qvec3f> &points, const std::vector<qvec3f> &normals, const qvec3f &point);
std::vector<qvec3f> GLM_ShrinkPoly(const std::vector<qvec3f> &poly, const float amount);
/// Returns (front part, back part)
std::pair<std::vector<qvec3f>, std::vector<qvec3f>> GLM_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 GLM_PolyRandomPoint_Setup(const std::vector<qvec3f> &points);
qvec3f GLM_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
inline qvec4f bilinearWeights(const float x, const float y)
{
Q_assert(x >= 0.0f);
Q_assert(x <= 1.0f);
Q_assert(y >= 0.0f);
Q_assert(y <= 1.0f);
return qvec4f((1.0f - x) * (1.0f - y), x * (1.0f - y), (1.0f - x) * y, x * 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).
inline std::array<std::pair<qvec2i, float>, 4> bilinearWeightsAndCoords(qvec2f pos, const qvec2i &size)
{
Q_assert(pos[0] >= -0.5f && pos[0] <= (size[0] - 0.5f));
Q_assert(pos[1] >= -0.5f && pos[1] <= (size[1] - 0.5f));
// Handle extrapolation.
for (int i = 0; i < 2; i++) {
if (pos[i] < 0)
pos[i] = 0;
if (pos[i] > (size[i] - 1))
pos[i] = (size[i] - 1);
}
Q_assert(pos[0] >= 0.f && pos[0] <= (size[0] - 1));
Q_assert(pos[1] >= 0.f && pos[1] <= (size[1] - 1));
qvec2i integerPart{static_cast<int>(qv::floor(pos)[0]), static_cast<int>(qv::floor(pos)[1])};
qvec2f fractionalPart(pos - qv::floor(pos));
// ensure integerPart + (1, 1) is still in bounds
for (int i = 0; i < 2; i++) {
if (fractionalPart[i] == 0.0f && integerPart[i] > 0) {
integerPart[i] -= 1;
fractionalPart[i] = 1.0f;
}
}
Q_assert(integerPart[0] + 1 < size[0]);
Q_assert(integerPart[1] + 1 < size[1]);
Q_assert(qvec2f(integerPart) + fractionalPart == pos);
// f(0,0), f(1,0), f(0,1), f(1,1)
const qvec4f weights = bilinearWeights(fractionalPart[0], fractionalPart[1]);
std::array<std::pair<qvec2i, float>, 4> result;
for (int i = 0; i < 4; i++) {
const float weight = weights[i];
qvec2i pos(integerPart);
if ((i % 2) == 1)
pos[0] += 1;
if (i >= 2)
pos[1] += 1;
Q_assert(pos[0] >= 0);
Q_assert(pos[0] < size[0]);
Q_assert(pos[1] >= 0);
Q_assert(pos[1] < size[1]);
result[i] = std::make_pair(pos, weight);
}
return result;
}
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 vec_t &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
Reference in New Issue View Git Blame Copy Permalink