attitude-utils.hpp

#ifndef SRC_COMMON_SPATIAL_ATTITUDE_UTILS_HPP_
#define SRC_COMMON_SPATIAL_ATTITUDE_UTILS_HPP_
 
#include <memory>
 
#include "common/decimal.hpp"
 
namespace found {
 
// At first, I wanted to have two separate Attitude classes, one storing Euler angles and converting
// to Quaterinon, and another storing as Quaternion and converting to Euler. But abstract classes
// make everything more annoying, because you need vectors of pointers...ugh!
 
 
struct Vec2 {
    decimal x;
    decimal y;
 
    // Magnitude
 
    decimal Magnitude() const;
 
    decimal MagnitudeSq() const;
 
    // Unit Vector
 
    Vec2 Normalize() const;
 
    // Vector Operations
 
    decimal operator*(const Vec2 &other) const;
 
    Vec2 operator*(const decimal &scalar) const;
 
    Vec2 operator+(const Vec2 &other) const;
 
    Vec2 operator-(const Vec2 &other) const;
};
 
class Mat3;  // define above so we can use in Vec3 class
 
class Vec3 {
 public:
    decimal x;
    decimal y;
    decimal z;
 
    constexpr Vec3(decimal x, decimal y, decimal z) : x(x), y(y), z(z) {}
 
    Vec3() = default;
 
    // Magnitude
 
    decimal Magnitude() const;
 
    decimal MagnitudeSq() const;
 
    // Unit Vector
 
    Vec3 Normalize() const;
 
    // Operations
 
    Vec3 operator-() const;
 
    Vec3 operator-(const Vec3 &) const;
 
    decimal operator*(const Vec3 &) const;
 
    Vec3 operator*(const decimal &) const;
 
    Vec3 operator/(const decimal &) const;
 
    Vec3 &operator+=(const Vec3 &);
 
    Vec3 operator*(const Mat3 &) const;
 
    Vec3 CrossProduct(const Vec3 &) const;
 
    Mat3 OuterProduct(const Vec3 &) const;
};
 
 
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 &);
 
decimal Angle(const Vec3 &, const Vec3 &);
 
decimal AngleUnit(const Vec3 &, const Vec3 &);
 
 
class Mat3 {
 public:
    decimal x[9];
 
    // Accessor
 
    decimal At(int i, int j) const;
 
    Vec3 Column(int) const;
 
    Vec3 Row(int) const;
 
    decimal Trace() const;
 
    decimal Det() const;
 
    // Operations
 
    Mat3 operator+(const Mat3 &) const;
 
    Mat3 operator*(const Mat3 &) const;
 
    Vec3 operator*(const Vec3 &) const;
 
    Mat3 operator*(const decimal &) const;
 
    // Transformations
 
    Mat3 Transpose() const;
 
    Mat3 Inverse() const;
};
 
extern const Mat3 kIdentityMat3;
 
 
class EulerAngles {
 public:
    constexpr EulerAngles(decimal ra = DECIMAL(0.0),
                decimal de = DECIMAL(0.0),
                decimal roll = DECIMAL(0.0))
        : ra(ra), de(de), roll(roll) {}
 
    decimal ra;
    decimal de;
    decimal roll;
};
 
 
class Quaternion {
 public:
    Quaternion() = default;
 
    Quaternion(const Vec3 &, decimal);
 
    explicit Quaternion(const Vec3 &);
 
    Quaternion(decimal real, decimal i, decimal j, decimal k)
        : real(real), i(i), j(j), k(k) {}
 
    Quaternion operator*(const Quaternion &other) const;
 
    Quaternion operator-() const;
 
    Quaternion Conjugate() const;
 
    Vec3 Vector() const;
 
    void SetVector(const Vec3 &);
 
    Vec3 Rotate(const Vec3 &) const;
 
    decimal Angle() const;
 
    void SetAngle(decimal);
 
    bool IsUnit(decimal tolerance) const;
 
    Quaternion Canonicalize() const;
 
    EulerAngles ToSpherical() const;
 
    decimal real;
    decimal i;
    decimal j;
    decimal k;
};
 
 
class Attitude {
 public:
    Attitude() = default;
 
    explicit Attitude(const Quaternion &quat) : quaternion(quat), type(QuaternionType) {}
 
    explicit Attitude(const Mat3 &matrix) : dcm(matrix), type(DCMType) {}
 
    Quaternion GetQuaternion() const;
 
    Mat3 GetDCM() const;
 
    EulerAngles ToSpherical() const;
 
    Vec3 Rotate(const Vec3 &) const;
 
 private:
    enum AttitudeType {
        NullType,
        QuaternionType,
        DCMType,
    };
 
    Quaternion quaternion;
    Mat3 dcm;
    AttitudeType type;
};
 
 
 
// Attitude Conversions
 
 
Mat3 QuaternionToDCM(const Quaternion &);
 
Quaternion DCMToQuaternion(const Mat3 &);
 
Quaternion SphericalToQuaternion(decimal ra, decimal dec, decimal roll);
 
inline Quaternion SphericalToQuaternion(EulerAngles angles)
    { return SphericalToQuaternion(angles.ra, angles.de, angles.roll); }
 
 
// Spherical-Vector Conversions
 
 
Vec3 SphericalToSpatial(decimal ra, decimal de);
 
void SpatialToSpherical(const Vec3 &, decimal &ra, decimal &de);
 
 
// Angle Conversions
 
 
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_