// This is a rewrite version (in C) from Horde3D (utMath.h) , http://www.horde3d.org
// Math library
//
// Coordinate system is left-handed with positive y as up axis
//

#ifndef ejoy3d_math_h
#define ejoy3d_math_h

#include <math.h>
#include <float.h>
#include <stddef.h>

struct vector3
{
	float x, y, z;
};

struct vector4
{
	float x, y, z, w;
};

struct quaternion
{
	float x, y, z, w;
};

union matrix44
{
	float c[4][4];
	float x[16];
};

struct plane
{
	struct vector3 normal;
	float dist;
};

// vector

static inline float *
vector3_array(struct vector3 *v)
{
	return (float *)v;
}

static inline float *
vector4_array(struct vector4 *v)
{
	return (float *)v;
}

static inline float
vector3_dot(const struct vector3 *a, const struct vector3 *b)
{
	return a->x * b->x + a->y * b->y + a->z * b->z;
}

static inline struct vector3 *
vector3_cross(struct vector3 *v, const struct vector3 *a, const struct vector3 *b)
{
	float x = a->y * b->z - a->z * b->y;
	float y = a->z * b->x - a->x * b->z;
	float z = a->x * b->y - a->y * b->x;

	v->x = x;
	v->y = y;
	v->z = z;

	return v;
}

static inline struct vector3 *
vector3_vector(struct vector3 *v, const struct vector3 *p1, const struct vector3 *p2)
{
	v->x = p1->x - p2->x;
	v->y = p1->y - p2->y;
	v->z = p1->z - p2->z;

	return v;
}

static inline float
vector3_length(const struct vector3 *v)
{
	return sqrtf(v->x * v->x + v->y * v->y + v->z * v->z);
}

static inline struct vector3 *
vector3_normalize(struct vector3 *v)
{
	float invLen = 1.0f / vector3_length(v);
	v->x *= invLen;
	v->y *= invLen;
	v->z *= invLen;

	return v;
}

static inline struct vector3 *
vector3_to_rotation(struct vector3 *v, const struct vector3 *r)
{
	// Assumes that the unrotated view vector is (0, 0, -1)
	v->x = v->y = v->z = 0;
	if (r->y != 0)
	{
		v->x = atan2f(r->y, sqrtf(r->x * r->x + r->z * r->z));
	}
	if (r->x != 0 || r->z != 0)
	{
		v->y = atan2f(-r->x, -r->z);
	}

	return v;
}

static inline struct vector3 *
vector3_lerp(struct vector3 *v, const struct vector3 *a, const struct vector3 *b, float f)
{
	float x = a->x + (b->x - a->x) * f;
	float y = a->y + (b->y - a->y) * f;
	float z = a->z + (b->z - a->z) * f;

	v->x = x;
	v->y = y;
	v->z = z;

	return v;
}

// quaternion

static inline struct quaternion *
quaternion_mul(struct quaternion *q, const struct quaternion *a, const struct quaternion *b)
{
	float x = a->y * b->z - a->z * b->y + b->x * a->w + a->x * b->w;
	float y = a->z * b->x - a->x * b->z + b->y * a->w + a->y * b->w;
	float z = a->x * b->y - a->y * b->x + b->z * a->w + a->z * b->w;
	float w = a->w * b->w - (a->x * b->x + a->y * b->y + a->z * b->z);

	q->x = x;
	q->y = y;
	q->z = z;
	q->w = w;

	return q;
}

static inline struct quaternion *
quaternion_init(struct quaternion *q, float x, float y, float z)
{
	struct quaternion roll = {sinf(x * 0.5f), 0, 0, cosf(x * 0.5f)};
	struct quaternion pitch = {0, sinf(y * 0.5f), 0, cosf(y * 0.5f)};
	struct quaternion yaw = {0, 0, sinf(z * 0.5f), cosf(z * 0.5f)};

	// Order: y * x * z
	quaternion_mul(q, &pitch, &roll);
	quaternion_mul(q, q, &yaw);

	return q;
}

static inline struct quaternion *
quaternion_slerp(struct quaternion *q, const struct quaternion *a, const struct quaternion *b, float t)
{
	float cosTheta = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;
	if (cosTheta < 0)
	{
		cosTheta = -cosTheta;
		q->x = -b->x;
		q->y = -b->y;
		q->z = -b->z;
		q->w = -b->w;
	}
	else
	{
		*q = *b;
	}
	float scale0 = 1 - t, scale1 = t;
	if ((1 - cosTheta) > 0.001f)
	{
		// use spherical interpolation
		float theta = acosf(cosTheta);
		float sinTheta = sinf(theta);
		scale0 = sinf((1 - t) * theta) / sinTheta;
		scale1 = sinf(t * theta) / sinTheta;
	}

	q->x = a->x * scale0 + q->x * scale1;
	q->y = a->y * scale0 + q->y * scale1;
	q->z = a->z * scale0 + q->z * scale1;
	q->w = a->w * scale0 + q->w * scale1;

	return q;
}

static inline struct quaternion *
quaternion_nslerp(struct quaternion *q, const struct quaternion *a, const struct quaternion *b, float t)
{
	// Normalized linear quaternion interpolation
	// Note: NLERP is faster than SLERP and commutative but does not yield constant velocity

	float cosTheta = a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w;

	if (cosTheta < 0)
	{
		q->x = a->x + (-b->x - a->x) * t;
		q->y = a->y + (-b->y - a->y) * t;
		q->z = a->z + (-b->z - a->z) * t;
		q->w = a->w + (-b->w - a->w) * t;
	}
	else
	{
		q->x = a->x + (b->x - a->x) * t;
		q->y = a->y + (b->y - a->y) * t;
		q->z = a->z + (b->z - a->z) * t;
		q->w = a->w + (b->w - a->w) * t;
	}

	float invLen = 1.0f / sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);

	q->x *= invLen;
	q->y *= invLen;
	q->z *= invLen;
	q->w *= invLen;

	return q;
}

static inline struct quaternion *
quaternion_inverted(struct quaternion *q)
{
	float len = q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w;
	if (len > 0)
	{
		float invLen = -1.0f / len;
		q->x *= invLen;
		q->y *= invLen;
		q->z *= invLen;
		q->w *= invLen;
		q->w = -q->w;
	}
	else
	{
		q->x = q->y = q->z = q->w = 0;
	}
	return q;
}

// matrix 4*4

#define C m->c

static inline union matrix44 *
matrix44_identity(union matrix44 *m)
{
	C[0][0] = 1;
	C[1][0] = 0;
	C[2][0] = 0;
	C[3][0] = 0;
	C[0][1] = 0;
	C[1][1] = 1;
	C[2][1] = 0;
	C[3][1] = 0;
	C[0][2] = 0;
	C[1][2] = 0;
	C[2][2] = 1;
	C[3][2] = 0;
	C[0][3] = 0;
	C[1][3] = 0;
	C[2][3] = 0;
	C[3][3] = 1;

	return m;
}

static inline union matrix44 *
matrix44_from_quaternion(union matrix44 *m, const struct quaternion *q)
{
	// Calculate coefficients
	float x2 = q->x + q->x, y2 = q->y + q->y, z2 = q->z + q->z;
	float xx = q->x * x2, xy = q->x * y2, xz = q->x * z2;
	float yy = q->y * y2, yz = q->y * z2, zz = q->z * z2;
	float wx = q->w * x2, wy = q->w * y2, wz = q->w * z2;

	C[0][0] = 1 - (yy + zz);
	C[1][0] = xy + wz;
	C[2][0] = xz - wy;
	C[3][0] = 0;
	C[0][1] = xy - wz;
	C[1][1] = 1 - (xx + zz);
	C[2][1] = yz + wx;
	C[3][1] = 0;
	C[0][2] = xz + wy;
	C[1][2] = yz - wx;
	C[2][2] = 1 - (xx + yy);
	C[3][2] = 0;
	C[0][3] = 0;
	C[1][3] = 0;
	C[2][3] = 0;
	C[3][3] = 1;

	return m;
}

static inline union matrix44 *
matrix44_trans(union matrix44 *m, float x, float y, float z)
{
	matrix44_identity(m);
	C[3][0] = x;
	C[3][1] = y;
	C[3][2] = z;

	return m;
}

static inline union matrix44 *
matrix44_scale(union matrix44 *m, float x, float y, float z)
{
	matrix44_identity(m);
	C[0][0] = x;
	C[1][1] = y;
	C[2][2] = z;

	return m;
}

static inline union matrix44 *
matrix44_rot(union matrix44 *m, float x, float y, float z)
{
	// Rotation order: YXZ [* Vector]
	struct quaternion q;
	quaternion_init(&q, x, y, z);

	return matrix44_from_quaternion(m, &q);
}

#include <stdio.h>
static inline union matrix44 *
matrix44_rot_axis(union matrix44 *m, const struct vector3 *axis, float angle)
{
	float t = sinf(angle * 0.5f);
	float x = axis->x * t;
	float y = axis->y * t;
	float z = axis->z * t;
	struct quaternion q = {x, y, z, cosf(angle * 0.5f)};

	return matrix44_from_quaternion(m, &q);
}

static inline union matrix44 *
matrix44_perspective(union matrix44 *m, float l, float r, float b, float t, float n, float f)
{
	matrix44_identity(m);
	float *mx = m->x;

	mx[0] = 2 * n / (r - l);
	mx[5] = 2 * n / (t - b);
	mx[8] = -(r + l) / (r - l);
	mx[9] = -(t + b) / (t - b);
	mx[10] = (f + n) / (f - n);
	mx[11] = 1;
	mx[14] = -2 * f * n / (f - n);
	mx[15] = 0;

	return m;
}

static inline union matrix44 *
matrix44_ortho(union matrix44 *m, float l, float r, float b, float t, float n, float f)
{
	matrix44_identity(m);
	float *mx = m->x;

	mx[0] = 2 / (r - l);
	mx[5] = 2 / (t - b);
	mx[10] = 2 / (f - n);
	mx[12] = -(r + l) / (r - l);
	mx[13] = -(t + b) / (t - b);
	mx[14] = -(f + n) / (f - n);

	return m;
}

static inline union matrix44 *
matrix44_fastmul43(union matrix44 *m, const union matrix44 *m1, const union matrix44 *m2)
{
	// Note: m may not be the same as m1 or m2

	const float *m1x = m1->x;
	const float *m2x = m2->x;
	float *mx = m->x;

	mx[0] = m1x[0] * m2x[0] + m1x[1] * m2x[4] + m1x[2] * m2x[8];
	mx[1] = m1x[0] * m2x[1] + m1x[1] * m2x[5] + m1x[2] * m2x[9];
	mx[2] = m1x[0] * m2x[2] + m1x[1] * m2x[6] + m1x[2] * m2x[10];
	mx[3] = 0.0f;

	mx[4] = m1x[4] * m2x[0] + m1x[5] * m2x[4] + m1x[6] * m2x[8];
	mx[5] = m1x[4] * m2x[1] + m1x[5] * m2x[5] + m1x[6] * m2x[9];
	mx[6] = m1x[4] * m2x[2] + m1x[5] * m2x[6] + m1x[6] * m2x[10];
	mx[7] = 0.0f;

	mx[8] = m1x[8] * m2x[0] + m1x[9] * m2x[4] + m1x[10] * m2x[8];
	mx[9] = m1x[8] * m2x[1] + m1x[9] * m2x[5] + m1x[10] * m2x[9];
	mx[10] = m1x[8] * m2x[2] + m1x[9] * m2x[6] + m1x[10] * m2x[10];
	mx[11] = 0.0f;

	mx[12] = m1x[12] * m2x[0] + m1x[13] * m2x[4] + m1x[14] * m2x[8] + m1x[15] * m2x[12];
	mx[13] = m1x[12] * m2x[1] + m1x[13] * m2x[5] + m1x[14] * m2x[9] + m1x[15] * m2x[13];
	mx[14] = m1x[12] * m2x[1] + m1x[13] * m2x[6] + m1x[14] * m2x[10] + m1x[15] * m2x[14];
	mx[15] = 1.0f;

	return m;
}

static inline union matrix44 *
matrix44_mul(union matrix44 *m, const union matrix44 *m1, const union matrix44 *m2)
{
	union matrix44 mf;
	const float *m1x = m1->x;
	const float *m2x = m2->x;

	mf.x[0] = m1x[0] * m2x[0] + m1x[1] * m2x[4] + m1x[2] * m2x[8] + m1x[3] * m2x[12];
	mf.x[1] = m1x[0] * m2x[1] + m1x[1] * m2x[5] + m1x[2] * m2x[9] + m1x[3] * m2x[13];
	mf.x[2] = m1x[0] * m2x[2] + m1x[1] * m2x[6] + m1x[2] * m2x[10] + m1x[3] * m2x[14];
	mf.x[3] = m1x[0] * m2x[3] + m1x[1] * m2x[7] + m1x[2] * m2x[11] + m1x[3] * m2x[15];

	mf.x[4] = m1x[4] * m2x[0] + m1x[5] * m2x[4] + m1x[6] * m2x[8] + m1x[7] * m2x[12];
	mf.x[5] = m1x[4] * m2x[1] + m1x[5] * m2x[5] + m1x[6] * m2x[9] + m1x[7] * m2x[13];
	mf.x[6] = m1x[4] * m2x[2] + m1x[5] * m2x[6] + m1x[6] * m2x[10] + m1x[7] * m2x[14];
	mf.x[7] = m1x[4] * m2x[3] + m1x[5] * m2x[7] + m1x[6] * m2x[11] + m1x[7] * m2x[15];

	mf.x[8] = m1x[8] * m2x[0] + m1x[9] * m2x[4] + m1x[10] * m2x[8] + m1x[11] * m2x[12];
	mf.x[9] = m1x[8] * m2x[1] + m1x[9] * m2x[5] + m1x[10] * m2x[9] + m1x[11] * m2x[13];
	mf.x[10] = m1x[8] * m2x[2] + m1x[9] * m2x[6] + m1x[10] * m2x[10] + m1x[11] * m2x[14];
	mf.x[11] = m1x[8] * m2x[3] + m1x[9] * m2x[7] + m1x[10] * m2x[11] + m1x[11] * m2x[15];

	mf.x[12] = m1x[12] * m2x[0] + m1x[13] * m2x[4] + m1x[14] * m2x[8] + m1x[15] * m2x[12];
	mf.x[13] = m1x[12] * m2x[1] + m1x[13] * m2x[5] + m1x[14] * m2x[9] + m1x[15] * m2x[13];
	mf.x[14] = m1x[12] * m2x[2] + m1x[13] * m2x[6] + m1x[14] * m2x[10] + m1x[15] * m2x[14];
	mf.x[15] = m1x[12] * m2x[3] + m1x[13] * m2x[7] + m1x[14] * m2x[11] + m1x[15] * m2x[15];

	*m = mf;

	return m;
}

static inline struct vector3 *
vector3_mul(struct vector3 *v, const union matrix44 *m)
{
	float x = v->x * C[0][0] + v->y * C[0][1] + v->z * C[0][2] + C[0][3];
	float y = v->x * C[1][0] + v->y * C[1][1] + v->z * C[1][2] + C[1][3];
	float z = v->x * C[2][0] + v->y * C[2][1] + v->z * C[2][2] + C[2][3];

	v->x = x;
	v->y = y;
	v->z = z;

	return v;
}

static inline struct vector4 *
vector4_mul(struct vector4 *v, const union matrix44 *m)
{
	float x = v->x * C[0][0] + v->y * C[0][1] + v->z * C[0][2] + v->w * C[0][3];
	float y = v->x * C[1][0] + v->y * C[1][1] + v->z * C[1][2] + v->w * C[1][3];
	float z = v->x * C[2][0] + v->y * C[2][1] + v->z * C[2][2] + v->w * C[2][3];
	float w = v->x * C[3][0] + v->y * C[3][1] + v->z * C[3][2] + v->w * C[3][3];

	v->x = x;
	v->y = y;
	v->z = z;
	v->w = w;
	return v;
}

static inline struct vector3 *
vector3_mul33(struct vector3 *v, const union matrix44 *m)
{
	float x = v->x * C[0][0] + v->y * C[0][1] + v->z * C[0][2];
	float y = v->x * C[1][0] + v->y * C[1][1] + v->z * C[1][2];
	float z = v->x * C[2][0] + v->y * C[2][1] + v->z * C[2][2];

	v->x = x;
	v->y = y;
	v->z = z;

	return v;
}

static inline union matrix44 *
matrix44_transposed(union matrix44 *m)
{
	int x, y;
	for (y = 0; y < 4; ++y)
	{
		for (x = y + 1; x < 4; ++x)
		{
			float tmp = C[x][y];
			C[x][y] = C[y][x];
			C[y][x] = tmp;
		}
	}

	return m;
}

static inline float
matrix44_determinant(const union matrix44 *m)
{
	return C[0][3] * C[1][2] * C[2][1] * C[3][0] - C[0][2] * C[1][3] * C[2][1] * C[3][0] - C[0][3] * C[1][1] * C[2][2] * C[3][0] + C[0][1] * C[1][3] * C[2][2] * C[3][0] +
		   C[0][2] * C[1][1] * C[2][3] * C[3][0] - C[0][1] * C[1][2] * C[2][3] * C[3][0] - C[0][3] * C[1][2] * C[2][0] * C[3][1] + C[0][2] * C[1][3] * C[2][0] * C[3][1] +
		   C[0][3] * C[1][0] * C[2][2] * C[3][1] - C[0][0] * C[1][3] * C[2][2] * C[3][1] - C[0][2] * C[1][0] * C[2][3] * C[3][1] + C[0][0] * C[1][2] * C[2][3] * C[3][1] +
		   C[0][3] * C[1][1] * C[2][0] * C[3][2] - C[0][1] * C[1][3] * C[2][0] * C[3][2] - C[0][3] * C[1][0] * C[2][1] * C[3][2] + C[0][0] * C[1][3] * C[2][1] * C[3][2] +
		   C[0][1] * C[1][0] * C[2][3] * C[3][2] - C[0][0] * C[1][1] * C[2][3] * C[3][2] - C[0][2] * C[1][1] * C[2][0] * C[3][3] + C[0][1] * C[1][2] * C[2][0] * C[3][3] +
		   C[0][2] * C[1][0] * C[2][1] * C[3][3] - C[0][0] * C[1][2] * C[2][1] * C[3][3] - C[0][1] * C[1][0] * C[2][2] * C[3][3] + C[0][0] * C[1][1] * C[2][2] * C[3][3];
}

static inline union matrix44 *
matrix44_inverted(union matrix44 *dst, const union matrix44 *m)
{
	float d = matrix44_determinant(m);
	if (d == 0)
	{
		*dst = *m;
		return dst;
	}
	d = 1.0f / d;

	dst->c[0][0] = d * (C[1][2] * C[2][3] * C[3][1] - C[1][3] * C[2][2] * C[3][1] + C[1][3] * C[2][1] * C[3][2] - C[1][1] * C[2][3] * C[3][2] - C[1][2] * C[2][1] * C[3][3] + C[1][1] * C[2][2] * C[3][3]);
	dst->c[0][1] = d * (C[0][3] * C[2][2] * C[3][1] - C[0][2] * C[2][3] * C[3][1] - C[0][3] * C[2][1] * C[3][2] + C[0][1] * C[2][3] * C[3][2] + C[0][2] * C[2][1] * C[3][3] - C[0][1] * C[2][2] * C[3][3]);
	dst->c[0][2] = d * (C[0][2] * C[1][3] * C[3][1] - C[0][3] * C[1][2] * C[3][1] + C[0][3] * C[1][1] * C[3][2] - C[0][1] * C[1][3] * C[3][2] - C[0][2] * C[1][1] * C[3][3] + C[0][1] * C[1][2] * C[3][3]);
	dst->c[0][3] = d * (C[0][3] * C[1][2] * C[2][1] - C[0][2] * C[1][3] * C[2][1] - C[0][3] * C[1][1] * C[2][2] + C[0][1] * C[1][3] * C[2][2] + C[0][2] * C[1][1] * C[2][3] - C[0][1] * C[1][2] * C[2][3]);
	dst->c[1][0] = d * (C[1][3] * C[2][2] * C[3][0] - C[1][2] * C[2][3] * C[3][0] - C[1][3] * C[2][0] * C[3][2] + C[1][0] * C[2][3] * C[3][2] + C[1][2] * C[2][0] * C[3][3] - C[1][0] * C[2][2] * C[3][3]);
	dst->c[1][1] = d * (C[0][2] * C[2][3] * C[3][0] - C[0][3] * C[2][2] * C[3][0] + C[0][3] * C[2][0] * C[3][2] - C[0][0] * C[2][3] * C[3][2] - C[0][2] * C[2][0] * C[3][3] + C[0][0] * C[2][2] * C[3][3]);
	dst->c[1][2] = d * (C[0][3] * C[1][2] * C[3][0] - C[0][2] * C[1][3] * C[3][0] - C[0][3] * C[1][0] * C[3][2] + C[0][0] * C[1][3] * C[3][2] + C[0][2] * C[1][0] * C[3][3] - C[0][0] * C[1][2] * C[3][3]);
	dst->c[1][3] = d * (C[0][2] * C[1][3] * C[2][0] - C[0][3] * C[1][2] * C[2][0] + C[0][3] * C[1][0] * C[2][2] - C[0][0] * C[1][3] * C[2][2] - C[0][2] * C[1][0] * C[2][3] + C[0][0] * C[1][2] * C[2][3]);
	dst->c[2][0] = d * (C[1][1] * C[2][3] * C[3][0] - C[1][3] * C[2][1] * C[3][0] + C[1][3] * C[2][0] * C[3][1] - C[1][0] * C[2][3] * C[3][1] - C[1][1] * C[2][0] * C[3][3] + C[1][0] * C[2][1] * C[3][3]);
	dst->c[2][1] = d * (C[0][3] * C[2][1] * C[3][0] - C[0][1] * C[2][3] * C[3][0] - C[0][3] * C[2][0] * C[3][1] + C[0][0] * C[2][3] * C[3][1] + C[0][1] * C[2][0] * C[3][3] - C[0][0] * C[2][1] * C[3][3]);
	dst->c[2][2] = d * (C[0][1] * C[1][3] * C[3][0] - C[0][3] * C[1][1] * C[3][0] + C[0][3] * C[1][0] * C[3][1] - C[0][0] * C[1][3] * C[3][1] - C[0][1] * C[1][0] * C[3][3] + C[0][0] * C[1][1] * C[3][3]);
	dst->c[2][3] = d * (C[0][3] * C[1][1] * C[2][0] - C[0][1] * C[1][3] * C[2][0] - C[0][3] * C[1][0] * C[2][1] + C[0][0] * C[1][3] * C[2][1] + C[0][1] * C[1][0] * C[2][3] - C[0][0] * C[1][1] * C[2][3]);
	dst->c[3][0] = d * (C[1][2] * C[2][1] * C[3][0] - C[1][1] * C[2][2] * C[3][0] - C[1][2] * C[2][0] * C[3][1] + C[1][0] * C[2][2] * C[3][1] + C[1][1] * C[2][0] * C[3][2] - C[1][0] * C[2][1] * C[3][2]);
	dst->c[3][1] = d * (C[0][1] * C[2][2] * C[3][0] - C[0][2] * C[2][1] * C[3][0] + C[0][2] * C[2][0] * C[3][1] - C[0][0] * C[2][2] * C[3][1] - C[0][1] * C[2][0] * C[3][2] + C[0][0] * C[2][1] * C[3][2]);
	dst->c[3][2] = d * (C[0][2] * C[1][1] * C[3][0] - C[0][1] * C[1][2] * C[3][0] - C[0][2] * C[1][0] * C[3][1] + C[0][0] * C[1][2] * C[3][1] + C[0][1] * C[1][0] * C[3][2] - C[0][0] * C[1][1] * C[3][2]);
	dst->c[3][3] = d * (C[0][1] * C[1][2] * C[2][0] - C[0][2] * C[1][1] * C[2][0] + C[0][2] * C[1][0] * C[2][1] - C[0][0] * C[1][2] * C[2][1] - C[0][1] * C[1][0] * C[2][2] + C[0][0] * C[1][1] * C[2][2]);

	return dst;
}

static inline struct vector3 *
matrix44_gettrans(const union matrix44 *m, struct vector3 *trans)
{
	// Getting translation is trivial
	trans->x = C[3][0];
	trans->y = C[3][1];
	trans->z = C[3][2];

	return trans;
}

static inline struct vector3 *
matrix44_getscale(const union matrix44 *m, struct vector3 *scale)
{
	// Scale is length of columns
	scale->x = sqrtf(C[0][0] * C[0][0] + C[0][1] * C[0][1] + C[0][2] * C[0][2]);
	scale->y = sqrtf(C[1][0] * C[1][0] + C[1][1] * C[1][1] + C[1][2] * C[1][2]);
	scale->z = sqrtf(C[2][0] * C[2][0] + C[2][1] * C[2][1] + C[2][2] * C[2][2]);

	return scale;
}

// NOTICE: Huanzai
// this function may be need to convert from right to left handed coordinate
static inline void
matrix44_decompose(const union matrix44 *m, struct vector3 *trans, struct vector3 *rot, struct vector3 *scale)
{
	matrix44_gettrans(m, trans);
	matrix44_getscale(m, scale);

	if (scale->x == 0 || scale->y == 0 || scale->z == 0)
	{
		rot->x = 0;
		rot->y = 0;
		rot->z = 0;
		return;
	}

	// Detect negative scale with determinant and flip one arbitrary axis
	if (matrix44_determinant(m) < 0)
		scale->x = -scale->x;

	// Combined rotation matrix YXZ
	//
	// Cos[y]*Cos[z]+Sin[x]*Sin[y]*Sin[z]   Cos[z]*Sin[x]*Sin[y]-Cos[y]*Sin[z]  Cos[x]*Sin[y]
	// Cos[x]*Sin[z]                        Cos[x]*Cos[z]                       -Sin[x]
	// -Cos[z]*Sin[y]+Cos[y]*Sin[x]*Sin[z]  Cos[y]*Cos[z]*Sin[x]+Sin[y]*Sin[z]  Cos[x]*Cos[y]

	rot->x = asinf(-C[2][1] / scale->z);

	// Special case: Cos[x] == 0 (when Sin[x] is +/-1)
	float f = fabsf(C[2][1] / scale->z);

	if (f > 0.999f && f < 1.001f)
	{
		// Pin arbitrarily one of y or z to zero
		// Mathematical equivalent of gimbal lock
		rot->y = 0;

		// Now: Cos[x] = 0, Sin[x] = +/-1, Cos[y] = 1, Sin[y] = 0
		// => m[0][0] = Cos[z] and m[1][0] = Sin[z]
		rot->z = atan2f(-C[1][0] / scale->y, C[0][0] / scale->x);
	}
	else
	{
		// Standard case
		rot->y = atan2f(C[2][0] / scale->z, C[2][2] / scale->z);
		rot->z = atan2f(C[0][1] / scale->x, C[1][1] / scale->y);
	}
}

static inline float *
matrix44_to33(const union matrix44 *m, float m33[9])
{
	m33[0] = C[0][0];
	m33[1] = C[0][1];
	m33[2] = C[0][2];
	m33[3] = C[1][0];
	m33[4] = C[1][1];
	m33[5] = C[1][2];
	m33[6] = C[2][0];
	m33[7] = C[2][1];
	m33[8] = C[2][2];

	return m33;
}

#undef C

// plane

static inline struct plane *
plane_init(struct plane *p, const struct vector3 *normal, float d)
{
	p->normal = *normal;
	// normalize
	float invLen = 1.0f / vector3_length(normal);
	p->normal.x *= invLen;
	p->normal.y *= invLen;
	p->normal.z *= invLen;
	p->dist = d * invLen;

	return p;
}

static inline struct plane *
plane_init_dot3(struct plane *p, const struct vector3 *v0, const struct vector3 *v1, const struct vector3 *v2)
{
	struct vector3 a, b;
	vector3_vector(&a, v1, v0);
	vector3_vector(&b, v2, v0);

	vector3_cross(&p->normal, &a, &b);
	vector3_normalize(&p->normal);
	p->dist = -vector3_dot(&p->normal, v0);

	return p;
}

static inline float
plane_dist(const struct plane *p, const struct vector3 *v)
{
	float d = vector3_dot(&p->normal, v);
	return d + p->dist;
}

// Intersection

static inline struct vector3 *
intersection_raytriangle(const struct vector3 *rayOrig, const struct vector3 *rayDir,
						 const struct vector3 *vert0, const struct vector3 *vert1, const struct vector3 *vert2,
						 struct vector3 *intsPoint)
{
	// Idea: Tomas Moeller and Ben Trumbore
	// in Fast, Minimum Storage Ray/Triangle Intersection

	// Find vectors for two edges sharing vert0
	struct vector3 edge1, edge2;
	vector3_vector(&edge1, vert1, vert0);
	vector3_vector(&edge2, vert2, vert0);

	// Begin calculating determinant - also used to calculate U parameter
	struct vector3 pvec;
	vector3_cross(&pvec, rayDir, &edge2);

	// If determinant is near zero, ray lies in plane of triangle
	float det = vector3_dot(&edge1, &pvec);

	// *** Culling branch ***
	/*if( det < FLT_EPSILON )
		return NULL;

	// Calculate distance from vert0 to ray origin
	struct vector3 tvec;
	vector3_vector(&tvec, rayOrig, &vert0);

	// Calculate U parameter and test bounds
	float u = vector3_dot(&tvec, &pvec);
	if (u < 0 || u > det ) 
		return NULL;

	// Prepare to test V parameter
	struct vector3 qvec;
	vector3_cross(&qvec, &tvec, &edge1);

	// Calculate V parameter and test bounds
	float v = vector3_dot(rayDir, &qvec);
	if (v < 0 || u + v > det ) 
		return NULL;

	// Calculate t, scale parameters, ray intersects triangle
	float t = vector3_dot(&edge2, &qvec ) / det;*/

	// *** Non-culling branch ***
	if (det > -FLT_EPSILON && det < FLT_EPSILON)
		return 0;
	float inv_det = 1.0f / det;

	// Calculate distance from vert0 to ray origin
	struct vector3 tvec;
	vector3_vector(&tvec, rayOrig, vert0);

	// Calculate U parameter and test bounds
	float u = vector3_dot(&tvec, &pvec) * inv_det;
	if (u < 0.0f || u > 1.0f)
		return 0;

	// Prepare to test V parameter
	struct vector3 qvec;
	vector3_cross(&qvec, &tvec, &edge1);

	// Calculate V parameter and test bounds
	float v = vector3_dot(rayDir, &qvec) * inv_det;
	if (v < 0.0f || u + v > 1.0f)
		return 0;

	// Calculate t, ray intersects triangle
	float t = vector3_dot(&edge2, &qvec) * inv_det;

	// Calculate intersection point and test ray length and direction
	intsPoint->x = rayOrig->x + rayDir->x * t;
	intsPoint->y = rayOrig->y + rayDir->y * t;
	intsPoint->z = rayOrig->z + rayDir->z * t;

	struct vector3 vec;
	vector3_vector(&vec, intsPoint, rayOrig);
	if (vector3_dot(&vec, rayDir) < 0 || vector3_length(&vec) > vector3_length(rayDir))
		return NULL;

	return intsPoint;
}

static inline float
minf(float a, float b)
{
	return a < b ? a : b;
}

static inline float
maxf(float a, float b)
{
	return a > b ? a : b;
}

static inline int
intersection_rayAABB(const struct vector3 *rayOrig, const struct vector3 *rayDir,
					 const struct vector3 *mins, const struct vector3 *maxs)
{
	// SLAB based optimized ray/AABB intersection routine
	// Idea taken from http://ompf.org/ray/

	float l1 = (mins->x - rayOrig->x) / rayDir->x;
	float l2 = (maxs->x - rayOrig->x) / rayDir->x;
	float lmin = minf(l1, l2);
	float lmax = maxf(l1, l2);

	l1 = (mins->y - rayOrig->y) / rayDir->y;
	l2 = (maxs->y - rayOrig->y) / rayDir->y;
	lmin = maxf(minf(l1, l2), lmin);
	lmax = minf(maxf(l1, l2), lmax);

	l1 = (mins->z - rayOrig->z) / rayDir->z;
	l2 = (maxs->z - rayOrig->z) / rayDir->z;
	lmin = maxf(minf(l1, l2), lmin);
	lmax = minf(maxf(l1, l2), lmax);

	if ((lmax >= 0.0f) & (lmax >= lmin))
	{
		// Consider length
		const struct vector3 rayDest = {rayOrig->x + rayDir->x, rayOrig->y + rayDir->y, rayOrig->z + rayDir->z};
		const struct vector3 rayMins = {minf(rayDest.x, rayOrig->x), minf(rayDest.y, rayOrig->y), minf(rayDest.z, rayOrig->z)};
		const struct vector3 rayMaxs = {maxf(rayDest.x, rayOrig->x), maxf(rayDest.y, rayOrig->y), maxf(rayDest.z, rayOrig->z)};
		return (rayMins.x < maxs->x) && (rayMaxs.x > mins->x) &&
			   (rayMins.y < maxs->y) && (rayMaxs.y > mins->y) &&
			   (rayMins.z < maxs->z) && (rayMaxs.z > mins->z);
	}
	else
	{
		return 0;
	}
}

static inline float
vector3_distAABB(const struct vector3 *pos, const struct vector3 *mins, const struct vector3 *maxs)
{
	struct vector3 center;
	struct vector3 extent;
	center.x = (mins->x + maxs->x) * 0.5f;
	center.y = (mins->y + maxs->y) * 0.5f;
	center.z = (mins->z + maxs->z) * 0.5f;

	extent.x = (maxs->x - mins->x) * 0.5f;
	extent.y = (maxs->y - mins->y) * 0.5f;
	extent.z = (maxs->z - mins->z) * 0.5f;

	struct vector3 nearestVec;
	nearestVec.x = maxf(0, fabsf(pos->x - center.x) - extent.x);
	nearestVec.y = maxf(0, fabsf(pos->y - center.y) - extent.y);
	nearestVec.z = maxf(0, fabsf(pos->z - center.z) - extent.z);

	return vector3_length(&nearestVec);
}

#endif