attitude-utils.hpp

Go to the documentation of this file.

#ifndef ATTITUDE_UTILS_H
#define ATTITUDE_UTILS_H
 
#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 of this
 
    Vec2 Normalize() const;
 
    // Operations
 
    decimal operator*(const Vec2 &) const;
    Vec2 operator*(const decimal &) const;
    Vec2 operator-(const Vec2 &) const;
    Vec2 operator+(const Vec2 &) 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() {}
 
    // Magnitude
 
    decimal Magnitude() const;
    decimal MagnitudeSq() const;
 
    // Unit Vector
 
    Vec3 Normalize() const;
 
    decimal getRightAscension() const;
    decimal getDeclination() const;
 
    // Operations
 
    decimal operator*(const Vec3 &) const;
    Vec3 operator*(const decimal &) const;
    Vec3 operator*(const Mat3 &) const;
    Vec3 operator/(const decimal &) const;
    Vec3 operator-(const Vec3 &) const;
    Vec3 operator-() const;
    Vec3 &operator+=(const Vec3 &);
    Vec3 CrossProduct(const Vec3 &) const;
    Mat3 OuterProduct(const Vec3 &) const;
};
 
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;
};
 
// Identity Matrix
 
extern const Mat3 kIdentityMat3;
 
// Buffer-Vector Functions
 
int64_t SerializeLengthVec3();
void SerializeVec3(const Vec3 &, unsigned char *);
Vec3 DeserializeVec3(const unsigned char *);
 
// Distance between Vectors
 
decimal Distance(const Vec2 &, const Vec2 &);
decimal Distance(const Vec3 &, const Vec3 &);
 
class EulerAngles {
 public:
    constexpr EulerAngles(decimal ra = DECIMAL(0.0),
                decimal de = DECIMAL(0.0),
                decimal roll = DECIMAL(0.0))  // NOLINT(runtime/explicit)
        : ra(ra), de(de), roll(roll) {}
 
    decimal ra;
    decimal de;
    decimal roll;
};
 
class Quaternion {
 public:
    Quaternion() = default;
    explicit Quaternion(const Vec3 &);
    Quaternion(const Vec3 &, decimal);
 
    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);
    EulerAngles ToSpherical() const;
    bool IsUnit(decimal tolerance) const;
    Quaternion Canonicalize() const;
 
    decimal real;
    decimal i;
    decimal j;
    decimal k;
};
 
 
class Attitude {
 public:
    Attitude() = default;
    explicit Attitude(const Quaternion &);
    explicit Attitude(const Mat3 &dcm);
 
    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;
};
 
// Vector operations
Vec2 midpoint(const Vec2 &, const Vec2 &);
Vec3 midpoint(const Vec3 &, const Vec3 &);
Vec3 midpoint(const Vec3 &, const Vec3 &, const Vec3 &);
 
// DCM-Quaternion-Spherical 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 Between Vectors
 
decimal Angle(const Vec3 &, const Vec3 &);
decimal AngleUnit(const Vec3 &, const Vec3 &);
 
// 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;
}
 
decimal RadToArcSec(decimal);
decimal ArcSecToRad(decimal);
 
}  // namespace found
 
#endif