Univerxel/src/core/geometry/math.hpp

#pragma once

#include "glm.hpp"
#include <glm/gtx/hash.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/quaternion.hpp>

namespace glm {
    template<typename T>
    constexpr vec<3, T> inline round(const vec3& p) {
        return vec<3, T>(std::round<T>(p.x), std::round<T>(p.y), std::round<T>(p.z));
    }

    template<typename U>
    constexpr U inline pow2(U v) {
        return v * v;
    }
    template<typename U>
    constexpr U inline pow3(U v) {
        return v * v * v;
    }
    constexpr ll inline length2(const llvec3& v) {
        return pow2(v.x) + pow2(v.y) + pow2(v.z);
    }
    constexpr ll inline length2(const lvec3& v) {
        return length2((llvec3)v);
    }

    constexpr lfvec3 inline abs(const lfvec3& v) {
        return lfvec3(fabsl(v.x), fabsl(v.y), fabsl(v.z));
    }
    constexpr mat4 inline abs(const mat4& mat) {
        mat4 m(0);
        for (int i = 0; i < mat.length(); i++)
            m[i] = abs(mat[i]);
        return m;
    }

    template<typename T>
    constexpr vec<3, T> inline diff(const vec<3, T>& a, const vec<3, T>& b) {
        return glm::abs(a - b);
    }

    constexpr us inline rem(ll value, us m) {
        return value < 0 ? ((value+1) % (ll)m) + m - 1 : value % (ll)m;
    }
    constexpr l inline div(ll value, us m) {
        return value < 0 ? ((value+1) / (ll)m) - 1 : value / (ll)m;
    }
    constexpr float inline remf(lf value, us m) {
        return value < 0 ? fmodl(value+1, m) + m - 1 : fmodl(value, m);
    }
    constexpr ucvec3 inline modulo(const llvec3& value, const ucvec3& m = ucvec3(IDX_LENGTH)) {
        return ucvec3(rem(value.x, m.x), rem(value.y, m.y), rem(value.z, m.z));
    }
    constexpr vec3 inline modulo(const lfvec3& value, const usvec3& m = usvec3(IDX_LENGTH)) {
        return vec3(remf(value.x, m.x), remf(value.y, m.y), remf(value.z, m.z));
    }
    constexpr lvec3 inline divide(const llvec3 &value, const uvec3 &m) {
        return lvec3(div(value.x, m.x), div(value.y, m.y), div(value.z, m.z));
    }
    constexpr lvec3 inline divide(const llvec3 &value, const ucvec3 &m = ucvec3(IDX_LENGTH)) {
        return lvec3(div(value.x, m.x), div(value.y, m.y), div(value.z, m.z));
    }
    constexpr llvec3 inline multiply(const lvec3 &value, const ucvec3 &m = ucvec3(IDX_LENGTH)) {
        return llvec3(value) * llvec3(m);
    }
    constexpr std::pair<lvec3, ucvec3> inline split(const llvec3 &value, const ucvec3 &m = ucvec3(IDX_LENGTH)) {
        return {divide(value, m), modulo(value, m)};
    }
    /// idx order is z-y-x
    constexpr ucvec3 inline fromIdx(idx idx) {
        assert(idx < IDX_SIZE);
        return ucvec3(idx % IDX_LENGTH, (idx / IDX_LENGTH) % IDX_LENGTH, idx / IDX_LENGTH2);
    }
    template <idx S = IDX_SHIFT>
    constexpr ucvec3 inline fromIdxShift(idx idx) {
        assert(idx < IDX_SIZE);
        constexpr glm::idx MASK = (1u<<S)-1u;
        return ucvec3(idx & MASK, (idx >> S) & MASK, (idx >> (2*S)) & MASK);
    }
    constexpr usvec3 inline fromIdx(size_t idx, lvec3 size) {
        assert(size.x>=0 && size.y>=0 && size.z>=0);
        assert((lvec3::value_type)idx < size.x * size.y * size.z);
        return usvec3(idx % size.x, (idx / size.x) % size.y, idx / (size.y * size.x));
    }
    template <idx S = IDX_SHIFT>
    constexpr idx inline toIdxShift(glm::us x, glm::us y, glm::us z) {
        return (z << (2*S)) | (y << S) | x;
    }
    constexpr idx inline toIdx(glm::uc x, glm::uc y, glm::uc z) {
        return (z * IDX_LENGTH + y) * IDX_LENGTH + x;
    }
    constexpr idx inline toIdx(glm::uc x, glm::uc y, glm::uc z, glm::uc sy, glm::uc sx) {
        return z * sy * sx + y * sx + x;
    }
    constexpr idx inline toIdx(ucvec3 pos) {
        return toIdx(pos.x, pos.y, pos.z);
    }
    constexpr idx inline toIdx(ucvec3 pos, ucvec3 size) {
        return toIdx(pos.x, pos.y, pos.z, size.y, size.x);
    }
    constexpr l inline toIdx(lvec3 pos) {
        return toIdx(pos.x, pos.y, pos.z);
    }
    constexpr l inline toIdx(lvec3 pos, lvec3 size) {
        return toIdx(pos.x, pos.y, pos.z, size.y, size.x);
    }
    constexpr std::pair<lvec3, idx> inline splitIdx(const llvec3 &value, const ucvec3 &m = ucvec3(IDX_LENGTH)) {
        return {divide(value, m), toIdx(rem(value.x, m.x), rem(value.y, m.y), rem(value.z, m.z))};
    }
    template <typename T>
    constexpr T inline min_axis(const vec<3, T> &v) {
        return std::max({v.x, v.y, v.z});
    }
    template <typename T>
    constexpr T inline max_axis(const vec<3, T> &v) {
        return std::max({v.x, v.y, v.z});
    }
    template <typename T>
    ivec3 inline inormalize(const vec<3, T> &v) {
        const auto a = glm::abs(v);
        return a.x >= a.y ? (a.x >= a.z ? ivec3(glm::sign(v.x), 0, 0) : ivec3(0, 0, glm::sign(v.z))) :
                            (a.y >= a.z ? ivec3(0, glm::sign(v.y), 0) : ivec3(0, 0, glm::sign(v.z)));
    }
}