attitude-utils.hpp

#ifndef SRC_COMMON_SPATIAL_ATTITUDE_UTILS_HPP_
#define SRC_COMMON_SPATIAL_ATTITUDE_UTILS_HPP_
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include <memory>
 
#include "common/decimal.hpp"
 
namespace found {
 
 
using Vec2 = Eigen::Matrix<decimal, 2, 1>;
 
using Vec3 = Eigen::Matrix<decimal, 3, 1>;
 
 
Vec2 Midpoint(const Vec2 &, const Vec2 &);
 
Vec3 Midpoint(const Vec3 &, const Vec3 &);
 
Vec3 Midpoint(const Vec3 &, const Vec3 &, const Vec3 &);
 
decimal Distance(const Vec2 &, const Vec2 &);
 
decimal Distance(const Vec3 &, const Vec3 &);
 
 
typedef Eigen::Matrix<decimal, 2, 2> Mat2;
 
typedef Eigen::Matrix<decimal, 3, 3> Mat3;
 
typedef Vec3 EulerAngles;
 
 
typedef Eigen::Quaternion<decimal> Quaternion;
 
typedef Eigen::AngleAxis<decimal> AngleAxis;
 
 
 
 
Quaternion SphericalToQuaternion(decimal ra, decimal dec, decimal roll);
 
inline Quaternion SphericalToQuaternion(EulerAngles angles)
    { return SphericalToQuaternion(angles.x(), angles.y(), angles.z()); }
 
 
 
 
constexpr decimal RadToDeg(decimal rad) {
    return rad*DECIMAL(180.0)/DECIMAL_M_PI;
}
 
constexpr decimal DegToRad(decimal deg) {
    return deg/DECIMAL(180.0)*DECIMAL_M_PI;
}
 
constexpr decimal RadToArcSec(decimal rad) {
    return RadToDeg(rad) * DECIMAL(3600.0);
}
 
constexpr decimal ArcSecToRad(decimal arcSec) {
    return DegToRad(arcSec / DECIMAL(3600.0));
}
 
}  // namespace found
 
#endif  // SRC_COMMON_SPATIAL_ATTITUDE_UTILS_HPP_