DepthGuide/src/core/math_helper.hpp

#ifndef DEPTHGUIDE_MATH_HELPER_HPP
#define DEPTHGUIDE_MATH_HELPER_HPP

#include "core/utility.hpp"

#include <glm/glm.hpp>
#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/quaternion.hpp>
#include <glm/gtx/transform.hpp>

#include <Eigen/Geometry>

// r in radius
inline glm::mat4 to_transform_mat(glm::vec3 t, glm::vec3 r) {
    static constexpr auto unit_x = glm::vec3(1.0f, 0.0f, 0.0f);
    static constexpr auto unit_y = glm::vec3(0.0f, 1.0f, 0.0f);
    static constexpr auto unit_z = glm::vec3(0.0f, 0.0f, 1.0f);
    auto rot = glm::angleAxis(r.x, unit_x)
               * glm::angleAxis(r.y, unit_y)
               * glm::angleAxis(r.z, unit_z);
    auto offset = glm::translate(t);
    return offset * glm::mat4_cast(rot);
}

inline glm::mat4 to_transform_mat(glm::vec3 t, glm::mat3 r) {
    auto ret = glm::mat4(r);
    ret[3] = glm::vec4(t, 1.f);
    return ret;
}

template<typename Scalar, int Mode>
inline glm::mat4 to_mat4(const Eigen::Transform<Scalar, 3, Mode> &m) {
    auto ret = glm::mat4();
    auto &mat = m.matrix();
    for (auto j = 0; j < 4; ++j)
        for (auto i = 0; i < 4; ++i) {
            ret[j][i] = mat(i, j);
        }
    return ret;
}

template<typename Scalar, int Mode>
inline auto to_eigen_transform(const glm::mat4 &m) {
    auto ret = Eigen::Transform<Scalar, 3, Mode>();
    auto &mat = ret.matrix();
    for (auto j = 0; j < 4; ++j)
        for (auto i = 0; i < 4; ++i) {
            mat(i, j) = m[j][i];
        }
    return ret;
}

template<typename Scalar, int Mode>
inline void to_eigen_transform(const glm::mat4 &m,
                               Eigen::Transform<Scalar, 3, Mode> &ret) {
    ret = to_eigen_transform<Scalar, Mode>(m);
}

template<typename T>
concept Vec2Type = requires(T t) {
    { t.x } -> std::convertible_to<float>;
    { t.y } -> std::convertible_to<float>;
};

template<Vec2Type T>
inline auto to_vec2(const T &v) {
    return glm::vec2(v.x, v.y);
}

template<typename T>
concept Vec3Type = requires(T t) {
    { t.x } -> std::convertible_to<float>;
    { t.y } -> std::convertible_to<float>;
    { t.z } -> std::convertible_to<float>;
};

template<Vec3Type T>
inline auto to_vec3(const T &v) {
    return glm::vec3(v.x, v.y, v.z);
}

template<typename EigenType>
glm::vec3 to_vec3(const Eigen::MatrixBase<EigenType> &v) {
    static_assert(EigenType::SizeAtCompileTime == 3);
    return glm::vec3(v.x(), v.y(), v.z());
}

inline auto to_homo(const glm::vec3 &v) {
    return glm::vec4(v, 1.f);
}

inline auto to_homo(const glm::vec2 &v) {
    return glm::vec3(v, 1.f);
}

inline auto from_homo(const glm::vec4 &v) {
    return glm::vec3(v) / v.w;
}

inline auto from_homo(const glm::vec3 &v) {
    return glm::vec2(v) / v.z;
}

// transform point
inline glm::vec3 transform_p(const glm::mat4 &mat, const glm::vec3 &point) {
    return from_homo(mat * to_homo(point));
}

inline glm::vec2 transform_p(const glm::mat3 &mat, const glm::vec2 &point) {
    return from_homo(mat * to_homo(point));
}

// transform vector
inline glm::vec3 transform_v(const glm::mat4 &mat, const glm::vec3 &vec) {
    return glm::mat3(mat) * vec;
}

inline glm::vec3 to_translation(const glm::mat4 &mat) {
    return glm::column(mat, 3);
}

inline glm::mat4 transform_mix(const glm::mat4 &x, const glm::mat4 &y, float a) {
    // translation
    auto xt = to_translation(x);
    auto yt = to_translation(y);
    auto zt = glm::mix(xt, yt, a);

    // rotation
    auto xq = glm::quat_cast(glm::mat3(x));
    auto yq = glm::quat_cast(glm::mat3(y));
    auto zq = glm::slerp(xq, yq, a);

    return to_transform_mat(zt, glm::mat3_cast(zq));
}

class transform_buffer {
public:
    transform_buffer();

    ~transform_buffer();

    void add(const glm::mat4 &mat, timestamp_type ts);

    glm::mat4 query(timestamp_type ts);

    void remove_old(int max_offset);

private:
    struct impl;
    std::unique_ptr<impl> pimpl;
};

#endif //DEPTHGUIDE_MATH_HELPER_HPP