Quat.hh

#pragma once
 
#include "egg/math/Math.hh"
#include "egg/math/Vector.hh"
 
namespace EGG {
 
struct Quatf {
#ifdef BUILD_DEBUG
    constexpr Quatf() : w(std::numeric_limits<f32>::signaling_NaN()) {}
#else
    constexpr Quatf() = default;
#endif
    constexpr Quatf(const Quatf &q) = default;
    constexpr Quatf(f32 w_, const Vector3f &v_) : v(v_), w(w_) {}
    constexpr Quatf(f32 w_, f32 x_, f32 y_, f32 z_) : v(x_, y_, z_), w(w_) {}
    ~Quatf() = default;
 
    Quatf &operator=(const Quatf &q) {
        w = q.w;
        v = q.v;
 
        return *this;
    }
 
    [[nodiscard]] Quatf operator+(const Quatf &rhs) const {
        return Quatf(w + rhs.w, v + rhs.v);
    }
 
    Quatf &operator+=(const Quatf &rhs) {
        return *this = *this + rhs;
    }
 
    [[nodiscard]] Quatf operator*(const Vector3f &vec) const {
        Vector3f cross = v.cross(vec);
        Vector3f scale = vec * w;
        return Quatf(-v.dot(vec), cross + scale);
    }
 
    [[nodiscard]] Quatf operator*(f32 scalar) const {
        return Quatf(w * scalar, v * scalar);
    }
 
    Quatf &operator*=(f32 scalar) {
        return *this = *this * scalar;
    }
 
    [[nodiscard]] Quatf operator*(const Quatf &rhs) const {
        f32 _w = w * rhs.w - v.x * rhs.v.x - v.y * rhs.v.y - v.z * rhs.v.z;
        f32 _x = v.y * rhs.v.z + (v.x * rhs.w + w * rhs.v.x) - v.z * rhs.v.y;
        f32 _y = v.z * rhs.v.x + (v.y * rhs.w + w * rhs.v.y) - v.x * rhs.v.z;
        f32 _z = v.x * rhs.v.y + (v.z * rhs.w + w * rhs.v.z) - v.y * rhs.v.x;
 
        return Quatf(_w, _x, _y, _z);
    }
 
    Quatf &operator*=(const Quatf &q) {
        return *this = *this * q;
    }
 
    bool operator==(const Quatf &rhs) const {
        return w == rhs.w && v == rhs.v;
    }
 
    bool operator!=(const Quatf &rhs) const {
        return !(*this == rhs);
    }
 
    explicit operator std::string() const {
        return std::format("[0x{:08X}, 0x{:08X}, 0x{:08X}, 0x{:08X}] | [{}, {}, {}, {}]", f2u(v.x),
                f2u(v.y), f2u(v.z), f2u(w), v.x, v.y, v.z, w);
    }
 
    void setRPY(const Vector3f &rpy);
    void setRPY(f32 r, f32 p, f32 y);
    void normalise();
    void makeVectorRotation(const Vector3f &from, const Vector3f &to);
 
    Quatf conjugate() const {
        return Quatf(w, -v);
    }
 
    Vector3f rotateVector(const Vector3f &vec) const;
    Vector3f rotateVectorInv(const Vector3f &vec) const;
    Quatf slerpTo(const Quatf &q2, f32 t) const;
 
    f32 squaredNorm() const {
        return w * w + v.squaredLength();
    }
 
    f32 norm() const {
        return Mathf::sqrt(squaredNorm());
    }
 
    f32 dot(const Quatf &q) const {
        return w * q.w + v.dot(q.v);
    }
 
    void setAxisRotation(f32 angle, const Vector3f &axis);
    Quatf multSwap(const Vector3f &v) const;
    Quatf multSwap(const Quatf &q) const;
 
    void read(Stream &stream);
 
    static Quatf FromRPY(const EGG::Vector3f &rpy);
    static Quatf FromRPY(f32 r, f32 p, f32 y);
 
    Vector3f v;
    f32 w;
 
    static const Quatf ident;
};
 
inline constexpr Quatf Quatf::ident = Quatf(1.0f, Vector3f::zero);
 
} // namespace EGG