simian/include/ImViewGuizmoRaymath.h

#pragma once

#include "raylib.h"
#include "raymath.h"
#include "imgui.h"

#define IMVIEWGUIZMO_VEC3 Vector3
#define IMVIEWGUIZMO_VEC4 Vector4
#define IMVIEWGUIZMO_QUAT Quaternion
#define IMVIEWGUIZMO_MAT4 Matrix

namespace ImViewGuizmo {
namespace GizmoMath {
    inline float get_vec_comp(const Vector3& v, int index) {
        switch (index) {
            case 0: return v.x;
            case 1: return v.y;
            default: return v.z;
        }
    }

    inline float get_vec_comp(const Vector4& v, int index) {
        switch (index) {
            case 0: return v.x;
            case 1: return v.y;
            case 2: return v.z;
            default: return v.w;
        }
    }

    inline Vector3 make_vec3(float x, float y, float z) { return {x, y, z}; }
    inline Vector4 make_vec4(float x, float y, float z, float w) { return {x, y, z, w}; }

    inline Quaternion angleAxis(float angle, const Vector3& axis) {
        return QuaternionFromAxisAngle(axis, angle);
    }

    inline Quaternion quatLookAt(const Vector3& dir, const Vector3& up) {
        Vector3 forward = Vector3Normalize(dir);
        Vector3 right = Vector3Normalize(Vector3CrossProduct(up, forward));
        Vector3 correctedUp = Vector3CrossProduct(forward, right);

        Matrix mat = MatrixIdentity();
        mat.m0 = right.x;
        mat.m1 = right.y;
        mat.m2 = right.z;
        mat.m4 = correctedUp.x;
        mat.m5 = correctedUp.y;
        mat.m6 = correctedUp.z;
        mat.m8 = forward.x;
        mat.m9 = forward.y;
        mat.m10 = forward.z;
        return QuaternionFromMatrix(mat);
    }

    inline Matrix mat4_identity() { return MatrixIdentity(); }

    inline Vector3 cross(const Vector3& a, const Vector3& b) {
        return Vector3CrossProduct(a, b);
    }

    inline float dot(const Vector3& a, const Vector3& b) {
        return Vector3DotProduct(a, b);
    }

    inline float dot(const Quaternion& a, const Quaternion& b) {
        return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
    }

    inline float length(const Vector3& v) {
        return Vector3Length(v);
    }

    inline float length2(const Vector3& v) {
        return v.x * v.x + v.y * v.y + v.z * v.z;
    }

    inline Vector3 normalize(const Vector3& v) {
        return Vector3Normalize(v);
    }

    inline Vector3 mix(const Vector3& a, const Vector3& b, float t) {
        return Vector3Lerp(a, b, t);
    }

    inline Vector3 add_vv(const Vector3& a, const Vector3& b) {
        return Vector3Add(a, b);
    }

    inline Vector3 subtract_vv(const Vector3& a, const Vector3& b) {
        return Vector3Subtract(a, b);
    }

    inline Vector3 multiply_vf(const Vector3& v, float f) {
        return Vector3Scale(v, f);
    }

    inline Matrix mat4_cast(const Quaternion& q) {
        return QuaternionToMatrix(q);
    }

    inline Matrix transpose(const Matrix& m) {
        return MatrixTranspose(m);
    }

    inline Vector3 get_matrix_col(const Matrix& m, int col) {
        switch (col) {
            case 0: return {m.m0, m.m1, m.m2};
            case 1: return {m.m4, m.m5, m.m6};
            case 2: return {m.m8, m.m9, m.m10};
            default: return {m.m12, m.m13, m.m14};
        }
    }

    inline void set_matrix_col(Matrix& m, int col, const Vector4& v) {
        switch (col) {
            case 0:
                m.m0 = v.x; m.m1 = v.y; m.m2 = v.z; m.m3 = v.w;
                break;
            case 1:
                m.m4 = v.x; m.m5 = v.y; m.m6 = v.z; m.m7 = v.w;
                break;
            case 2:
                m.m8 = v.x; m.m9 = v.y; m.m10 = v.z; m.m11 = v.w;
                break;
            default:
                m.m12 = v.x; m.m13 = v.y; m.m14 = v.z; m.m15 = v.w;
                break;
        }
    }

    inline Quaternion multiply_qq(const Quaternion& a, const Quaternion& b) {
        return QuaternionMultiply(a, b);
    }

    inline Vector3 multiply_qv(const Quaternion& q, const Vector3& v) {
        return Vector3RotateByQuaternion(v, q);
    }

    inline Matrix multiply_mm(const Matrix& a, const Matrix& b) {
        return MatrixMultiply(a, b);
    }

    inline Vector4 multiply_mv4(const Matrix& m, const Vector4& v) {
        Vector4 result;
        result.x = m.m0 * v.x + m.m4 * v.y + m.m8 * v.z + m.m12 * v.w;
        result.y = m.m1 * v.x + m.m5 * v.y + m.m9 * v.z + m.m13 * v.w;
        result.z = m.m2 * v.x + m.m6 * v.y + m.m10 * v.z + m.m14 * v.w;
        result.w = m.m3 * v.x + m.m7 * v.y + m.m11 * v.z + m.m15 * v.w;
        return result;
    }
}
} // namespace ImViewGuizmo

#include "ImViewGuizmo.h"