c3c/lib/std/math/math_quaternion.c3

module std::math::quaternion<Real>;
import std::math::vector;
union Quaternion
{
	struct
    {
    	Real i, j, k, l;
    }
	Real[<4>] v;
}

macro Quaternion identity() => { 0, 0, 0, 1 };
macro Quaternion Quaternion.add(Quaternion a, Quaternion b) => Quaternion { .v = a.v + b.v };
macro Quaternion Quaternion.add_each(Quaternion a, Real b) => Quaternion { .v = a.v + b };
macro Quaternion Quaternion.sub(Quaternion a, Quaternion b) => Quaternion { .v = a.v - b.v };
macro Quaternion Quaternion.sub_each(Quaternion a, Real b) => Quaternion { .v = a.v - b };
macro Quaternion Quaternion.scale(Quaternion a, Real s) => Quaternion { .v = a.v * s };
macro Quaternion Quaternion.normalize(Quaternion q) => { .v = q.v.normalize() };
macro Real Quaternion.length(Quaternion q) => q.v.length();
macro Quaternion Quaternion.lerp(Quaternion q1, Quaternion q2, Real amount) => { .v = q1.v.lerp(q2.v, amount) };
fn Quaternion Quaternion.nlerp(Quaternion q1, Quaternion q2, Real amount) => { .v = q1.v.lerp(q2.v, amount).normalize() };

fn Quaternion Quaternion.invert(Quaternion q)
{
	Real length_sq = q.v.dot(q.v);
	if (length_sq <= 0) return q;
    Real inv_length = 1 / length_sq;
	return { q.v[0] * -inv_length, q.v[1] * -inv_length, q.v[2] * -inv_length, q.v[3] * inv_length };
}

fn Quaternion Quaternion.slerp(Quaternion q1, Quaternion q2, Real amount)
{
    Quaternion result = {};

	Real[<4>] q2v = q2.v;
    Real cos_half_theta = q1.v.dot(q2v);

    if (cos_half_theta < 0)
    {
        q2v = -q2v;
        cos_half_theta = -cos_half_theta;
    }

	if (cos_half_theta >= 1) return q1;

	Real[<4>] q1v = q1.v;
	if (cos_half_theta > 0.95f) return { .v = q1v.lerp(q2v, amount) };

	Real half_theta = math::cos(cos_half_theta);
	Real sin_half_theta = math::sqrt(1 - cos_half_theta * cos_half_theta);
	if (math::abs(sin_half_theta) < 0.001f)
	{
		return { .v = (q1v + q2v) * 0.5f };
	}
    Real ratio_a = math::sin((1 - amount) * half_theta) / sin_half_theta;
    Real ratio_b = math::sin(amount * half_theta) / sin_half_theta;
	return { .v = q1v * ratio_a + q2v * ratio_b };
}

fn Quaternion Quaternion.mul(Quaternion a, Quaternion b)
{
	return { a.i * b.l + a.l * b.i + a.j * b.k - a.k * b.j,
	         a.j * b.l + a.l * b.j + a.k * b.i - a.i * b.k,
	         a.k * b.l + a.l * b.k + a.i * b.j - a.j * b.i,
	         a.l * b.l - a.i * b.i - a.j * a.j - a.k * a.k };
}