DCMUtils.hpp

Go to the documentation of this file.

// Vulcan DCM Utilities
// Direction Cosine Matrix utilities: skew, composition, small-angle
// approximation
#pragma once
 
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/math/Arithmetic.hpp>
#include <janus/math/Linalg.hpp>
#include <janus/math/Logic.hpp>
#include <janus/math/Rotations.hpp>
#include <janus/math/Trig.hpp>
 
namespace vulcan {
 
// =============================================================================
// Skew-Symmetric Matrix
// =============================================================================

template <typename Scalar> Mat3<Scalar> skew(const Vec3<Scalar> &v) {
    Mat3<Scalar> S;
    S(0, 0) = Scalar(0);
    S(0, 1) = -v(2);
    S(0, 2) = v(1);
    S(1, 0) = v(2);
    S(1, 1) = Scalar(0);
    S(1, 2) = -v(0);
    S(2, 0) = -v(1);
    S(2, 1) = v(0);
    S(2, 2) = Scalar(0);
    return S;
}

template <typename Scalar> Vec3<Scalar> unskew(const Mat3<Scalar> &S) {
    Vec3<Scalar> v;
    v(0) = S(2, 1);
    v(1) = S(0, 2);
    v(2) = S(1, 0);
    return v;
}
 
// =============================================================================
// DCM Composition
// =============================================================================

template <typename Scalar>
Mat3<Scalar> compose_dcm(const Mat3<Scalar> &R1, const Mat3<Scalar> &R2) {
    return R2 * R1;
}

template <typename Scalar>
Mat3<Scalar> relative_dcm(const Mat3<Scalar> &R_A, const Mat3<Scalar> &R_B) {
    return R_B.transpose() * R_A;
}
 
// =============================================================================
// Small-Angle Approximation
// =============================================================================

template <typename Scalar>
Mat3<Scalar> dcm_from_small_angle(const Vec3<Scalar> &theta) {
    return Mat3<Scalar>::Identity() + skew(theta);
}

template <typename Scalar>
Vec3<Scalar> small_angle_from_dcm(const Mat3<Scalar> &R) {
    Scalar half = Scalar(0.5);
    Vec3<Scalar> theta;
    theta(0) = half * (R(2, 1) - R(1, 2));
    theta(1) = half * (R(0, 2) - R(2, 0));
    theta(2) = half * (R(1, 0) - R(0, 1));
    return theta;
}
 
// =============================================================================
// DCM Validation
// =============================================================================

template <typename Derived>
auto is_valid_dcm(const Eigen::MatrixBase<Derived> &R, double tol = 1e-9) {
    return janus::is_valid_rotation_matrix(R, tol);
}
 
// =============================================================================
// DCM from Principal Axis
// =============================================================================

template <typename Scalar>
Mat3<Scalar> dcm_principal_axis(Scalar theta, int axis) {
    return janus::rotation_matrix_3d(theta, axis);
}
 
} // namespace vulcan