attitude-utils.hpp

Go to the documentation of this file.

#ifndef ATTITUDE_UTILS_H
#define ATTITUDE_UTILS_H
 
#include <memory>
#include <vector>
 
#include "serialize-helpers.hpp"
#include "decimal.hpp"
 
namespace lost {
 
// 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;
 
    decimal Magnitude() const;
    decimal MagnitudeSq() const;
 
    Vec2 Normalize() const;
 
    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;
 
    decimal Magnitude() const;
    decimal MagnitudeSq() const;
    Vec3 Normalize() const;
 
    decimal operator*(const Vec3 &) const;
    Vec3 operator*(const decimal &) const;
    Vec3 operator*(const Mat3 &) const;
    Vec3 operator-(const Vec3 &) const;
    Vec3 CrossProduct(const Vec3 &) const;
    Mat3 OuterProduct(const Vec3 &) const;
};
 
class Mat3 {
public:
    decimal x[9];
 
    decimal At(int i, int j) const;
    Mat3 operator+(const Mat3 &) const;
    Mat3 operator*(const Mat3 &) const;
    Vec3 operator*(const Vec3 &) const;
    Mat3 operator*(const decimal &) const;
    Mat3 Transpose() const;
    Vec3 Column(int) const;
    Vec3 Row(int) const;
    decimal Trace() const;
    decimal Det() const;
    Mat3 Inverse() const;
};
 
extern const Mat3 kIdentityMat3;
 
void SerializeVec3(SerializeContext *, const Vec3 &);
Vec3 DeserializeVec3(DeserializeContext *des);
 
decimal Distance(const Vec2 &, const Vec2 &);
decimal Distance(const Vec3 &, const Vec3 &);
 
class EulerAngles {
public:
    EulerAngles(decimal ra, decimal de, decimal roll)
        : 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 Conjugate() const;
    Vec3 Vector() const;
    void SetVector(const Vec3 &);
    Vec3 Rotate(const Vec3 &) const;
    decimal Angle() const;
    decimal SmallestAngle() 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 &); // NOLINT
    explicit Attitude(const Mat3 &dcm);
 
    Quaternion GetQuaternion() const;
    Mat3 GetDCM() const;
    EulerAngles ToSpherical() const;
    Vec3 Rotate(const Vec3 &) const;
    bool IsKnown() const;
 
private:
    enum class AttitudeType {
        UnknownType, 
        QuaternionType,
        DCMType,
    };
 
    AttitudeType type;
    Quaternion quaternion;
    Mat3 dcm; // direction cosine matrix
};
 
Mat3 QuaternionToDCM(const Quaternion &);
Quaternion DCMToQuaternion(const Mat3 &);
 
Quaternion SphericalToQuaternion(decimal ra, decimal dec, decimal roll);
 
Vec3 SphericalToSpatial(decimal ra, decimal de);
void SpatialToSpherical(const Vec3 &, decimal *ra, decimal *de);
decimal Angle(const Vec3 &, const Vec3 &);
decimal AngleUnit(const Vec3 &, const Vec3 &);
 
decimal RadToDeg(decimal);
decimal DegToRad(decimal);
decimal RadToArcSec(decimal);
decimal ArcSecToRad(decimal);
decimal DecimalModulo(decimal x, decimal mod);
 
// TODO: quaternion and euler angle conversion, conversion between ascension/declination to rec9tu
 
}
 
#endif