orthogonal.hpp

 
#ifndef UFO_VISION_CAMERA_ORTHOGONAL_HPP
#define UFO_VISION_CAMERA_ORTHOGONAL_HPP
 
// UFO
#include <ufo/execution/algorithm.hpp>
#include <ufo/execution/execution.hpp>
#include <ufo/geometry/ray.hpp>
#include <ufo/numeric/mat.hpp>
#include <ufo/numeric/transform.hpp>
#include <ufo/numeric/vec.hpp>
#include <ufo/vision/camera/orthogonal_intrinsics.hpp>
#include <ufo/vision/image.hpp>
 
// STL
#include <cmath>
#include <cstddef>
#include <format>
#include <ostream>
#include <utility>
 
namespace ufo
{
struct OrthogonalCamera {
    Transform3f pose;
    OrthogonalIntrinsics intrinsics;
    float near_clip;
    float far_clip;
 
    void lookAt(Vec3f center, Vec3f const& target, Vec3f const& up)
    {
        pose = Transform3f{inverse(ufo::lookAt(center, target, up))};
    }
 
    [[nodiscard]] Mat4f projection() const
    {
        float const half_w = intrinsics.width / 2.0f;
        float const half_h = intrinsics.height / 2.0f;
        Mat4f       m = orthogonal(-half_w, half_w, -half_h, half_h, near_clip, far_clip);
        m[0][0] *= intrinsics.zoom;
        m[1][1] *= intrinsics.zoom;
        return m;
    }
 
    [[nodiscard]] Mat4f view() const { return Mat4f(pose); }
 
    [[nodiscard]] Image<Ray3f> rays() const { return rays(ufo::execution::seq); }
 
    template <class ExecutionPolicy>
    [[nodiscard]] Image<Ray3f> rays(ExecutionPolicy&& policy) const
    {
        auto const   proj_inv = inverse(projection());
        auto const   view_inv = view();
        Image<Ray3f> rays(intrinsics.rows, intrinsics.cols);
 
        ufo::for_each(std::forward<ExecutionPolicy>(policy), std::size_t{}, intrinsics.rows,
                      [&rays, &proj_inv, &view_inv](std::size_t y) {
                          float const vy = ((y + 0.5f) / rays.rows()) * 2.0f - 1.0f;
                          for (std::size_t x{}; rays.cols() > x; ++x) {
                              float const vx = ((x + 0.5f) / rays.cols()) * 2.0f - 1.0f;
                              Vec3f       origin(ufo::convert<Vec3f>(view_inv * proj_inv *
                                                                     Vec4f(vx, vy, 0.0f, 1.0f)));
                              Vec3f       direction(
                          ufo::convert<Vec3f>(view_inv * Vec4f(0.0f, 0.0f, 1.0f, 0.0f)));
                              rays[y, x] = {origin, direction};
                          }
                      });
        return rays;
    }
 
    [[nodiscard]] friend bool operator==(OrthogonalCamera const&,
                                         OrthogonalCamera const&) noexcept = default;
};
 
inline std::ostream& operator<<(std::ostream& os, OrthogonalCamera const& camera)
{
    os << "Pose: " << camera.pose << '\n'
       << camera.intrinsics << "\nNear clip: " << camera.near_clip
       << ", Far clip: " << camera.far_clip << '\n';
    return os;
}
 
}  // namespace ufo
 
template <>
struct std::formatter<ufo::OrthogonalCamera> {
    constexpr auto parse(std::format_parse_context& ctx) const { return ctx.begin(); }
 
    auto format(ufo::OrthogonalCamera const& camera, std::format_context& ctx) const
    {
        return std::format_to(ctx.out(), "Pose: {}\n{}\nNear clip: {}, Far clip: {}",
                              camera.pose, camera.intrinsics, camera.near_clip,
                              camera.far_clip);
    }
};
 
#endif  // UFO_VISION_CAMERA_ORTHOGONAL_HPP