quaternion.hpp

#pragma once

#include <cmath>
#include <ostream>

const double PI = 3.141592653589793;

struct Quaternion {
    double a, b, c, d;

    Quaternion() : a(1.0), b{}, c{}, d{} {}
    Quaternion(double a, double b, double c, double d) : a(a), b(b), c(c), d(d) {}
    Quaternion(Quaternion const&) = default;
    Quaternion &operator=(Quaternion const &) = default;

    // right-handed system, top is +Y
    static Quaternion from_axis(double x, double y, double z, double twist) {
        const double sin_half = std::sin(0.5 * twist);
        return {std::cos(0.5 * twist), x * sin_half, y * sin_half, z * sin_half};
    }

    Quaternion operator+(Quaternion const &other) const {
        return {a + other.a, b + other.b, c + other.c, d + other.d};
    }

    Quaternion& operator+=(Quaternion const &other) {
        *this = *this + other;
        return *this;
    }

    Quaternion operator*(double alpha) const {
        return {alpha * a, alpha * b, alpha * c, alpha * d};
    }

    Quaternion& operator*=(double alpha) {
        *this = *this * alpha;
        return *this;
    }

    Quaternion operator*(Quaternion const &other) const {
        // (a1 + b1 i + c1 j + d1 k) * (a2 + b2 i + c2 j + d2 k)
        // = (a1 * a2 + a1 * b2 i + a1 * c2 j + a1 * d2 k)
        //   + (b1 i * a2 + b1 i * b2 i + b1 i * c2 j + b1 i * d2 k)
        //   + (c1 j * a2 + c1 j * b2 i + c1 j * c2 j + c1 j * d2 k)
        //   + (d1 k * a2 + d1 k * b2 i + d1 k * c2 j + d1 k * d2 k)
        // = (a1 a2 + a1 b2 i + a1 c2 j + a1 d2 k)
        //   + (b1 a2 i - b1 b2 + b1 c2 k - b1 d2 j)
        //   + (c1 a2 j - c1 b2 k - c1 c2 + c1 d2 i)
        //   + (d1 a2 k + d1 b2 j - d1 c2 i - d1 d2)
        // = (a1 a2 - b1 b2 - c1 c2 - d1 d2)
        //   + (a1 b2 + b1 a2 + c1 d2 - d1 c2) i
        //   + (a1 c2 - b1 d2 + c1 a2 + d1 b2) j
        //   + (a1 d2 + b1 c2 - c1 b2 + d1 a2) k
        return {
            a * other.a - b * other.b - c * other.c - d * other.d,
            a * other.b + b * other.a + c * other.d - d * other.c,
            a * other.c - b * other.d + c * other.a + d * other.b,
            a * other.d + b * other.c - c * other.b + d * other.a,
        };
    }

    Quaternion& operator*=(Quaternion const &other) {
        *this = *this * other;
        return *this;
    }

    Quaternion conj() const {
        return {a, -b, -c, -d};
    }

    double norm_squared() const {
        return a * a + b * b + c * c + d * d;
    }

    double norm() const {
        return std::sqrt(norm_squared());
    }

    Quaternion normalized() const {
        return *this * (1.0 / norm());
    }

    Quaternion inv() const {
        return conj() * (1.0 / norm_squared());
    }
};

inline std::ostream &operator<<(std::ostream &o, Quaternion const &q) {
    o << "(" << q.a << ", " << q.b << ", " << q.c << ", " << q.d << ")";
    return o;
}

inline Quaternion operator*(double alpha, Quaternion const &q) {
    return {alpha * q.a, alpha * q.b, alpha * q.c, alpha * q.d};
}