utility.hpp

Go to the documentation of this file.

 
#ifndef FLEXIV_RDK_UTILITY_HPP_
#define FLEXIV_RDK_UTILITY_HPP_
 
#include <Eigen/Eigen>
#include <array>
 
namespace flexiv {
namespace rdk {
namespace utility {
 
inline std::array<double, 3> Quat2EulerZYX(const std::array<double, 4>& quat)
{
    // Form quaternion
    Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
 
    // The returned array is in [z,y,x] order
    auto euler_ZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
 
    // Convert to general [x,y,z] order
    return (std::array<double, 3> {euler_ZYX[2], euler_ZYX[1], euler_ZYX[0]});
}
 
inline double Rad2Deg(double rad)
{
    constexpr double kPi = 3.14159265358979323846;
    return (rad / kPi * 180.0);
}
 
template <size_t N>
inline std::array<double, N> Rad2Deg(const std::array<double, N>& rad_arr)
{
    std::array<double, N> deg_arr;
    for (size_t i = 0; i < N; i++) {
        deg_arr[i] = Rad2Deg(rad_arr[i]);
    }
    return deg_arr;
}
 
template <typename T>
inline std::string Vec2Str(const std::vector<T>& vec, size_t decimal = 3,
    bool trailing_space = true, const std::string& separator = " ")
{
    std::string padding = "";
    std::stringstream ss;
    ss.precision(decimal);
    ss << std::fixed;
 
    for (const auto& v : vec) {
        ss << padding << v;
        padding = separator;
    }
 
    if (trailing_space) {
        ss << " ";
    }
    return ss.str();
}
 
template <typename T, size_t N>
inline std::string Arr2Str(const std::array<T, N>& arr, size_t decimal = 3,
    bool trailing_space = true, const std::string& separator = " ")
{
    std::vector<T> vec(N);
    std::copy(arr.begin(), arr.end(), vec.begin());
    return Vec2Str(vec, decimal, trailing_space, separator);
}
 
inline bool ProgramArgsExistAny(int argc, char** argv, const std::vector<std::string>& ref_strings)
{
    for (int i = 0; i < argc; i++) {
        for (const auto& v : ref_strings) {
            if (v == std::string(argv[i])) {
                return true;
            }
        }
    }
    return false;
}
 
inline bool ProgramArgsExist(int argc, char** argv, const std::string& ref_strings)
{
    return ProgramArgsExistAny(argc, argv, {ref_strings});
}
 
inline std::string ParsePtStates(
    const std::vector<std::string>& pt_states, const std::string& parse_target)
{
    for (const auto& state : pt_states) {
        // Skip if empty
        if (state.empty()) {
            continue;
        }
        std::stringstream ss(state);
        std::string buffer;
        std::vector<std::string> parsed_state;
        while (ss >> buffer) {
            parsed_state.push_back(buffer);
        }
        if (parsed_state.front() == parse_target) {
            return parsed_state.back();
        }
    }
 
    return "";
}
 
} /* namespace utility */
} /* namespace rdk */
} /* namespace flexiv */
 
#endif /* FLEXIV_RDK_UTILITY_HPP_ */