Program Listing for File math.hpp

↰ Return to documentation for file (src/navtk/navutils/math.hpp)

#pragma once

#include <navtk/factory.hpp>
#include <navtk/inspect.hpp>
#include <navtk/tensors.hpp>
#include <xtensor/containers/xadapt.hpp>

namespace navtk {
namespace navutils {
extern const double PI;
extern const double DEG2RAD;
extern const double RAD2DEG;

template <typename S = Vector, IfTensorOfDim<S, 1>* = nullptr>
Matrix3 skew(const S& angles) {
    auto x = angles(0);
    auto y = angles(1);
    auto z = angles(2);

    return {{0, -z, y}, {z, 0, -x}, {-y, x, 0}};
}

template <typename B = Matrix, IfTensorOfDim<B, 2>* = nullptr>
Tensor<3> skew(const B& angles) {
    const size_t N = angles.shape()[0];

    // allocate return tensor
    auto skews = empty(N, 3, 3);

    Scalar x, y, z;

    for (size_t i = 0; i < N; i++) {
        x = angles(i, 0);
        y = angles(i, 1);
        z = angles(i, 2);

        skews(i, 0, 0) = 0;
        skews(i, 0, 1) = -z;
        skews(i, 0, 2) = y;
        skews(i, 1, 0) = z;
        skews(i, 1, 1) = 0;
        skews(i, 1, 2) = -x;
        skews(i, 2, 0) = -y;
        skews(i, 2, 1) = x;
        skews(i, 2, 2) = 0;
    }

    return skews;
}

Matrix3 ortho_dcm(const Matrix3& dcm);

double wrap_to_pi(double orig);

double wrap_to_2_pi(double orig);

}  // namespace navutils
}  // namespace navtk