homogeneous_transform.h

Go to the documentation of this file.

#ifndef FUSED_ALGORITHMS_HOMOGENEOUS_TRANSFORM_H
#define FUSED_ALGORITHMS_HOMOGENEOUS_TRANSFORM_H
 
#include "config.h"
#include "fused/fused_matrix.h"
#include "fused/fused_vector.h"
 
namespace matrix_algorithms
{
 
    template <typename T>
    FusedMatrix<T, 4, 4> homogeneous_inverse(const FusedMatrix<T, 4, 4> &H)
    {
        static_assert(is_floating_point_v<T>,
                      "homogeneous_inverse requires a floating-point scalar type");
 
        FusedMatrix<T, 4, 4> result(T(0));
 
        // Rᵀ — transpose the 3×3 rotation block
        for (my_size_t i = 0; i < 3; ++i)
        {
            for (my_size_t j = 0; j < 3; ++j)
            {
                result(i, j) = H(j, i);
            }
        }
 
        // -Rᵀ · t
        for (my_size_t i = 0; i < 3; ++i)
        {
            // clang-format off
            result(i, 3) =  -(result(i, 0) * H(0, 3) 
                            + result(i, 1) * H(1, 3) 
                            + result(i, 2) * H(2, 3));
            // clang-format on
        }
 
        // Bottom row: [0 0 0 1]
        result(3, 3) = T(1);
 
        return result;
    }
 
} // namespace matrix_algorithms
 
#endif // FUSED_ALGORITHMS_HOMOGENEOUS_TRANSFORM_H