skew_symmetric.h

Go to the documentation of this file.

#ifndef FUSED_ALGORITHMS_SKEW_SYMMETRIC_H
#define FUSED_ALGORITHMS_SKEW_SYMMETRIC_H
 
#include "config.h"
#include "fused/fused_matrix.h"
#include "fused/fused_vector.h"
#include "math/math_utils.h"             // math::sqrt, math::sin, math::cos
#include "algorithms/operations/norms.h" // norm2
 
namespace matrix_algorithms
{
 
    template <typename T>
    FusedMatrix<T, 3, 3> skew_symmetric(const FusedVector<T, 3> &v)
    {
        FusedMatrix<T, 3, 3> S(T(0));
 
        S(0, 1) = -v(2);
        S(0, 2) = v(1);
        S(1, 0) = v(2);
        S(1, 2) = -v(0);
        S(2, 0) = -v(1);
        S(2, 1) = v(0);
 
        return S;
    }
 
    template <typename T>
    FusedMatrix<T, 3, 3> rodrigues(const FusedVector<T, 3> &omega, T t = T(1))
    {
        static_assert(is_floating_point_v<T>,
                      "rodrigues requires a floating-point scalar type");
 
        FusedMatrix<T, 3, 3> I(T(0));
        I.setIdentity();
 
        // ‖ω‖
        T norm = norm2(omega);
 
        // θ = t · ‖ω‖
        T theta = t * norm;
 
        if (theta <= T(PRECISION_TOLERANCE))
        {
            // Small angle: R ≈ I + t · [ω]×
            auto S = skew_symmetric(omega);
 
            FusedMatrix<T, 3, 3> R(T(0));
            R = I;
            R(0, 1) += t * S(0, 1);
            R(0, 2) += t * S(0, 2);
            R(1, 0) += t * S(1, 0);
            R(1, 2) += t * S(1, 2);
            R(2, 0) += t * S(2, 0);
            R(2, 1) += t * S(2, 1);
 
            return R;
        }
 
        // [ω̂]× where ω̂ = ω/‖ω‖ (unit axis)
        FusedVector<T, 3> omega_hat(T(0));
        omega_hat(0) = omega(0) / norm;
        omega_hat(1) = omega(1) / norm;
        omega_hat(2) = omega(2) / norm;
 
        auto K = skew_symmetric(omega_hat);
 
        // K² = [ω̂]ײ
        auto K2 = FusedMatrix<T, 3, 3>::matmul(K, K);
 
        // R = I + sin(θ)·K + (1 − cos(θ))·K²
        T s = math::sin(theta);
        T c = math::cos(theta);
 
        FusedMatrix<T, 3, 3> R(T(0));
 
        for (my_size_t i = 0; i < 3; ++i)
        {
            for (my_size_t j = 0; j < 3; ++j)
            {
                R(i, j) = I(i, j) + s * K(i, j) + (T(1) - c) * K2(i, j);
            }
        }
 
        return R;
    }
 
} // namespace matrix_algorithms
 
#endif // FUSED_ALGORITHMS_SKEW_SYMMETRIC_H