/* * * * * * * * * * * * * Author's note * * * * * * * * * * * *\
*   _       _   _       _   _       _   _       _     _ _ _ _   *
*  |_|     |_| |_|     |_| |_|_   _|_| |_|     |_|  _|_|_|_|_|  *
*  |_|_ _ _|_| |_|     |_| |_|_|_|_|_| |_|     |_| |_|_ _ _     *
*  |_|_|_|_|_| |_|     |_| |_| |_| |_| |_|     |_|   |_|_|_|_   *
*  |_|     |_| |_|_ _ _|_| |_|     |_| |_|_ _ _|_|  _ _ _ _|_|  *
*  |_|     |_|   |_|_|_|   |_|     |_|   |_|_|_|   |_|_|_|_|    *
*                                                               *
*                     http://www.humus.name                     *
*                                                                *
* This file is a part of the work done by Humus. You are free to   *
* use the code in any way you like, modified, unmodified or copied   *
* into your own work. However, I expect you to respect these points:  *
*  - If you use this file and its contents unmodified, or use a major *
*    part of this file, please credit the author and leave this note. *
*  - For use in anything commercial, please request my approval.     *
*  - Share your work and ideas too as much as you can.             *
*                                                                *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

// I would like to thank Humus for giving away this code, it
// is very generous and makes my life easier.
// - Bill Whitacre

#include "vector.h"

half::half(const float x){
	union {
		float floatI;
		unsigned int i;
	};
	floatI = x;

//	unsigned int i = *((unsigned int *) &x);
	int e = ((i >> 23) & 0xFF) - 112;
	int m =  i & 0x007FFFFF;

	sh = (i >> 16) & 0x8000;
	if (e <= 0){
		// Denorm
		m = ((m | 0x00800000) >> (1 - e)) + 0x1000;
		sh |= (m >> 13);
	} else if (e == 143){
		sh |= 0x7C00;
		if (m != 0){
			// NAN
			m >>= 13;
			sh |= m | (m == 0);
		}
	} else {
		m += 0x1000;
		if (m & 0x00800000){
			// Mantissa overflow
			m = 0;
			e++;
		}
		if (e >= 31){
			// Exponent overflow
			sh |= 0x7C00;
		} else {
			sh |= (e << 10) | (m >> 13);
		}
	}
}

half::operator float () const {
	union {
		unsigned int s;
		float result;
	};

	s = (sh & 0x8000) << 16;
	unsigned int e = (sh >> 10) & 0x1F;
	unsigned int m = sh & 0x03FF;

	if (e == 0){
		// +/- 0
		if (m == 0) return result;

		// Denorm
		while ((m & 0x0400) == 0){
			m += m;
			e--;
		}
		e++;
		m &= ~0x0400;
	} else if (e == 31){
		// INF / NAN
		s |= 0x7F800000 | (m << 13);
		return result;
	}

	s |= ((e + 112) << 23) | (m << 13);

	return result;
}

/* --------------------------------------------------------------------------------- */

void Vector2::operator += (const float s){
	x += s;
	y += s;
}

void Vector2::operator += (const Vector2 &v){
	x += v.x;
	y += v.y;
}

void Vector2::operator -= (const float s){
	x -= s;
	y -= s;
}

void Vector2::operator -= (const Vector2 &v){
	x -= v.x;
	y -= v.y;
}

void Vector2::operator *= (const float s){
	x *= s;
	y *= s;
}

void Vector2::operator *= (const Vector2 &v){
	x *= v.x;
	y *= v.y;
}

void Vector2::operator /= (const float s){
	x /= s;
	y /= s;
}

void Vector2::operator /= (const Vector2 &v){
	x /= v.x;
	y /= v.y;
}

Vector2 operator + (const Vector2 &u, const Vector2 &v){
	return Vector2(u.x + v.x, u.y + v.y);
}

Vector2 operator + (const Vector2 &v, const float s){
	return Vector2(v.x + s, v.y + s);
}

Vector2 operator + (const float s, const Vector2 &v){
	return Vector2(v.x + s, v.y + s);
}

Vector2 operator - (const Vector2 &u, const Vector2 &v){
	return Vector2(u.x - v.x, u.y - v.y);
}

Vector2 operator - (const Vector2 &v, const float s){
	return Vector2(v.x - s, v.y - s);
}

Vector2 operator - (const float s, const Vector2 &v){
	return Vector2(v.x - s, v.y - s);
}

Vector2 operator - (const Vector2 &v){
	return Vector2(-v.x, -v.y);
}

Vector2 operator * (const Vector2 &u, const Vector2 &v){
	return Vector2(u.x * v.x, u.y * v.y);
}

Vector2 operator * (const float s, const Vector2 &v){
	return Vector2(v.x * s, v.y * s);
}

Vector2 operator * (const Vector2 &v, const float s){
	return Vector2(v.x * s, v.y * s);
}

Vector2 operator / (const Vector2 &u, const Vector2 &v){
	return Vector2(u.x / v.x, u.y / v.y);
}

Vector2 operator / (const Vector2 &v, const float s){
	return Vector2(v.x / s, v.y / s);
}

Vector2 operator / (const float s, const Vector2 &v){
	return Vector2(s / v.x, s / v.y);
}

bool operator == (const Vector2 &u, const Vector2 &v){
	return (u.x == v.x && u.y == v.y);
}

/* --------------------------------------------------------------------------------- */

void Vector3::operator += (const float s){
	x += s;
	y += s;
	z += s;
}

void Vector3::operator += (const Vector3 &v){
	x += v.x;
	y += v.y;
	z += v.z;
}

void Vector3::operator -= (const float s){
	x -= s;
	y -= s;
	z -= s;
}

void Vector3::operator -= (const Vector3 &v){
	x -= v.x;
	y -= v.y;
	z -= v.z;
}

void Vector3::operator *= (const float s){
	x *= s;
	y *= s;
	z *= s;
}

void Vector3::operator *= (const Vector3 &v){
	x *= v.x;
	y *= v.y;
	z *= v.z;
}

void Vector3::operator /= (const float s){
	x /= s;
	y /= s;
	z /= s;
}

void Vector3::operator /= (const Vector3 &v){
	x /= v.x;
	y /= v.y;
	z /= v.z;
}

Vector3 operator + (const Vector3 &u, const Vector3 &v){
	return Vector3(u.x + v.x, u.y + v.y, u.z + v.z);
}

Vector3 operator + (const Vector3 &v, const float s){
	return Vector3(v.x + s, v.y + s, v.z + s);
}

Vector3 operator + (const float s, const Vector3 &v){
	return Vector3(v.x + s, v.y + s, v.z + s);
}

Vector3 operator - (const Vector3 &u, const Vector3 &v){
	return Vector3(u.x - v.x, u.y - v.y, u.z - v.z);
}

Vector3 operator - (const Vector3 &v, const float s){
	return Vector3(v.x - s, v.y - s, v.z - s);
}

Vector3 operator - (const float s, const Vector3 &v){
	return Vector3(v.x - s, v.y - s, v.z - s);
}

Vector3 operator - (const Vector3 &v){
	return Vector3(-v.x, -v.y, -v.z);
}

Vector3 operator * (const Vector3 &u, const Vector3 &v){
	return Vector3(u.x * v.x, u.y * v.y, u.z * v.z);
}

Vector3 operator * (const float s, const Vector3 &v){
	return Vector3(v.x * s, v.y * s, v.z * s);
}

Vector3 operator * (const Vector3 &v, const float s){
	return Vector3(v.x * s, v.y * s, v.z * s);
}

Vector3 operator / (const Vector3 &u, const Vector3 &v){
	return Vector3(u.x / v.x, u.y / v.y, u.z / v.z);
}

Vector3 operator / (const Vector3 &v, const float s){
	return Vector3(v.x / s, v.y / s, v.z / s);
}

Vector3 operator / (const float s, const Vector3 &v){
	return Vector3(s / v.x, s / v.y, s / v.z);
}

bool operator == (const Vector3 &u, const Vector3 &v){
	return (u.x == v.x && u.y == v.y && u.z == v.z);
}

/* --------------------------------------------------------------------------------- */

void Vector4::operator += (const float s){
	x += s;
	y += s;
	z += s;
	w += s;
}

void Vector4::operator += (const Vector4 &v){
	x += v.x;
	y += v.y;
	z += v.z;
	w += v.w;
}

void Vector4::operator -= (const float s){
	x -= s;
	y -= s;
	z -= s;
	w -= s;
}

void Vector4::operator -= (const Vector4 &v){
	x -= v.x;
	y -= v.y;
	z -= v.z;
	w -= v.w;
}

void Vector4::operator *= (const float s){
	x *= s;
	y *= s;
	z *= s;
	w *= s;
}

void Vector4::operator *= (const Vector4 &v){
	x *= v.x;
	y *= v.y;
	z *= v.z;
	w *= v.w;
}

void Vector4::operator /= (const float s){
	x /= s;
	y /= s;
	z /= s;
	w /= s;
}

void Vector4::operator /= (const Vector4 &v){
	x /= v.x;
	y /= v.y;
	z /= v.z;
	w /= v.w;
}

Vector4 operator + (const Vector4 &u, const Vector4 &v){
	return Vector4(u.x + v.x, u.y + v.y, u.z + v.z, u.w + v.w);
}

Vector4 operator + (const Vector4 &v, const float s){
	return Vector4(v.x + s, v.y + s, v.z + s, v.w + s);
}

Vector4 operator + (const float s, const Vector4 &v){
	return Vector4(v.x + s, v.y + s, v.z + s, v.w + s);
}

Vector4 operator - (const Vector4 &u, const Vector4 &v){
	return Vector4(u.x - v.x, u.y - v.y, u.z - v.z, u.w - v.w);
}

Vector4 operator - (const Vector4 &v, const float s){
	return Vector4(v.x - s, v.y - s, v.z - s, v.w - s);
}

Vector4 operator - (const float s, const Vector4 &v){
	return Vector4(v.x - s, v.y - s, v.z - s, v.w - s);
}

Vector4 operator - (const Vector4 &v){
	return Vector4(-v.x, -v.y, -v.z, -v.w);
}

Vector4 operator * (const Vector4 &u, const Vector4 &v){
	return Vector4(u.x * v.x, u.y * v.y, u.z * v.z, u.w * v.w);
}

Vector4 operator * (const float s, const Vector4 &v){
	return Vector4(v.x * s, v.y * s, v.z * s, v.w * s);
}

Vector4 operator * (const Vector4 &v, const float s){
	return Vector4(v.x * s, v.y * s, v.z * s, v.w * s);
}

Vector4 operator / (const Vector4 &u, const Vector4 &v){
	return Vector4(u.x / v.x, u.y / v.y, u.z / v.z, u.w / v.w);
}

Vector4 operator / (const Vector4 &v, const float s){
	return Vector4(v.x / s, v.y / s, v.z / s, v.w / s);
}

Vector4 operator / (const float s, const Vector4 &v){
	return Vector4(s / v.x, s / v.y, s / v.z, s / v.w);
}

bool operator == (const Vector4 &u, const Vector4 &v){
	return (u.x == v.x && u.y == v.y && u.z == v.z && u.w && v.w);
}

/* --------------------------------------------------------------------------------- */

float dot(const Vector2 &u, const Vector2 &v){
	return u.x * v.x + u.y * v.y;
}

float dot(const Vector3 &u, const Vector3 &v){
	return u.x * v.x + u.y * v.y + u.z * v.z;
}

float dot(const Vector4 &u, const Vector4 &v){
	return u.x * v.x + u.y * v.y + u.z * v.z + u.w * v.w;
}

float lerp(const float u, const float v, const float x){
	return u + x * (v - u);
}

Vector2 lerp(const Vector2 &u, const Vector2 &v, const float x){
	return u + x * (v - u);
}

Vector3 lerp(const Vector3 &u, const Vector3 &v, const float x){
	return u + x * (v - u);
}

Vector4 lerp(const Vector4 &u, const Vector4 &v, const float x){
	return u + x * (v - u);
}

Vector2 lerp(const Vector2 &u, const Vector2 &v, const Vector2 &x){
	return u + x * (v - u);
}

Vector3 lerp(const Vector3 &u, const Vector3 &v, const Vector3 &x){
	return u + x * (v - u);
}

Vector4 lerp(const Vector4 &u, const Vector4 &v, const Vector4 &x){
	return u + x * (v - u);
}

float cerp(const float u0, const float u1, const float u2, const float u3, float x){
	float p = (u3 - u2) - (u0 - u1);
	float q = (u0 - u1) - p;
	float r = u2 - u0;
	return x * (x * (x * p + q) + r) + u1;
}

Vector2 cerp(const Vector2 &u0, const Vector2 &u1, const Vector2 &u2, const Vector2 &u3, float x){
	Vector2 p = (u3 - u2) - (u0 - u1);
	Vector2 q = (u0 - u1) - p;
	Vector2 r = u2 - u0;
	return x * (x * (x * p + q) + r) + u1;
}

Vector3 cerp(const Vector3 &u0, const Vector3 &u1, const Vector3 &u2, const Vector3 &u3, float x){
	Vector3 p = (u3 - u2) - (u0 - u1);
	Vector3 q = (u0 - u1) - p;
	Vector3 r = u2 - u0;
	return x * (x * (x * p + q) + r) + u1;
}

Vector4 cerp(const Vector4 &u0, const Vector4 &u1, const Vector4 &u2, const Vector4 &u3, float x){
	Vector4 p = (u3 - u2) - (u0 - u1);
	Vector4 q = (u0 - u1) - p;
	Vector4 r = u2 - u0;
	return x * (x * (x * p + q) + r) + u1;
}

float sign(const float v){
	return (v > 0)? 1.0f : (v < 0)? -1.0f : 0.0f;
}

Vector2 sign(const Vector2 &v){
	return Vector2(sign(v.x), sign(v.y));
}

Vector3 sign(const Vector3 &v){
	return Vector3(sign(v.x), sign(v.y), sign(v.z));
}

Vector4 sign(const Vector4 &v){
	return Vector4(sign(v.x), sign(v.y), sign(v.z), sign(v.w));
}

float clamp(const float v, const float c0, const float c1){
	return min(max(v, c0), c1);
}

Vector2 clamp(const Vector2 &v, const float c0, const float c1){
	return Vector2(min(max(v.x, c0), c1), min(max(v.y, c0), c1));
}

Vector2 clamp(const Vector2 &v, const Vector2 &c0, const Vector2 &c1){
	return Vector2(min(max(v.x, c0.x), c1.x), min(max(v.y, c0.y), c1.y));
}

Vector3 clamp(const Vector3 &v, const float c0, const float c1){
	return Vector3(min(max(v.x, c0), c1), min(max(v.y, c0), c1), min(max(v.z, c0), c1));
}

Vector3 clamp(const Vector3 &v, const Vector3 &c0, const Vector3 &c1){
	return Vector3(min(max(v.x, c0.x), c1.x), min(max(v.y, c0.y), c1.y), min(max(v.z, c0.z), c1.z));
}

Vector4 clamp(const Vector4 &v, const float c0, const float c1){
	return Vector4(min(max(v.x, c0), c1), min(max(v.y, c0), c1), min(max(v.z, c0), c1), min(max(v.z, c0), c1));
}

Vector4 clamp(const Vector4 &v, const Vector4 &c0, const Vector4 &c1){
	return Vector4(min(max(v.x, c0.x), c1.x), min(max(v.y, c0.y), c1.y), min(max(v.z, c0.z), c1.z), min(max(v.w, c0.w), c1.w));
}

Vector2 normalize(const Vector2 &v){
	float invLen = 1.0f / sqrtf(v.x * v.x + v.y * v.y);
	return v * invLen;
}

Vector3 normalize(const Vector3 &v){
	float invLen = 1.0f / sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
	return v * invLen;
}

Vector4 normalize(const Vector4 &v){
	float invLen = 1.0f / sqrtf(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
	return v * invLen;
}

Vector2 fastNormalize(const Vector2 &v){
	float invLen = rsqrtf(v.x * v.x + v.y * v.y);
	return v * invLen;
}

Vector3 fastNormalize(const Vector3 &v){
	float invLen = rsqrtf(v.x * v.x + v.y * v.y + v.z * v.z);
	return v * invLen;
}

Vector4 fastNormalize(const Vector4 &v){
	float invLen = rsqrtf(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
	return v * invLen;
}

float length(const Vector2 &v){
	return sqrtf(v.x * v.x + v.y * v.y);
}

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

float length(const Vector4 &v){
	return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z + v.w * v.w);
}

Vector3 reflect(const Vector3 &v, const Vector3 &normal){
	float n = dot(v, normal);
	return v - 2 * n * normal;
}

float distance(const Vector2 &u, const Vector2 &v){
    Vector2 d = u - v;
	return dot(d, d);
}

float distance(const Vector3 &u, const Vector3 &v){
    Vector3 d = u - v;
	return dot(d, d);
}

float distance(const Vector4 &u, const Vector4 &v){
    Vector4 d = u - v;
	return dot(d, d);
}

float planeDistance(const Vector3 &normal, const float offset, const Vector3 &point){
    return point.x * normal.x + point.y * normal.y + point.z * normal.z + offset;
}

float planeDistance(const Vector4 &plane, const Vector3 &point){
    return point.x * plane.x + point.y * plane.y + point.z * plane.z + plane.w;
}

float sCurve(const float t){
	return t * t * (3 - 2 * t);
}

Vector3 cross(const Vector3 &u, const Vector3 &v){
	return Vector3(u.y * v.z - v.y * u.z, u.z * v.x - u.x * v.z, u.x * v.y - u.y * v.x);
}

float lineProjection(const Vector3 &line0, const Vector3 &line1, const Vector3 &point){
	Vector3 v = line1 - line0;
	return dot(v, point - line0) / dot(v, v);
}

unsigned int toRGBA(const Vector4 &u){
	return (int(u.x * 255) | (int(u.y * 255) << 8) | (int(u.z * 255) << 16) | (int(u.w * 255) << 24));
}

unsigned int toBGRA(const Vector4 &u){
	return (int(u.z * 255) | (int(u.y * 255) << 8) | (int(u.x * 255) << 16) | (int(u.w * 255) << 24));
}

Vector3 rgbeToRGB(unsigned char *rgbe){
	if (rgbe[3]){
		return Vector3(rgbe[0], rgbe[1], rgbe[2]) * ldexpf(1.0f, rgbe[3] - (int) (128 + 8));
	} else return Vector3(0, 0, 0);
}

unsigned int rgbToRGBE8(const Vector3 &rgb){
	float v = max(rgb.x, rgb.y);
	v = max(v, rgb.z);

	if (v < 1e-32f){
		return 0;
	} else {
		int ex;
		float m = frexpf(v, &ex) * 256.0f / v;

		unsigned int r = (unsigned int) (m * rgb.x);
		unsigned int g = (unsigned int) (m * rgb.y);
		unsigned int b = (unsigned int) (m * rgb.z);
		unsigned int e = (unsigned int) (ex + 128);

		return r | (g << 8) | (b << 16) | (e << 24);
	}
}

unsigned int rgbToRGB9E5(const Vector3 &rgb){
	float v = max(rgb.x, rgb.y);
	v = max(v, rgb.z);

	if (v < 1.52587890625e-5f){
		return 0;
	} else if (v < 65536){
		int ex;
		float m = frexpf(v, &ex) * 512.0f / v;

		unsigned int r = (unsigned int) (m * rgb.x);
		unsigned int g = (unsigned int) (m * rgb.y);
		unsigned int b = (unsigned int) (m * rgb.z);
		unsigned int e = (unsigned int) (ex + 15);

		return r | (g << 9) | (b << 18) | (e << 27);
	} else {
		unsigned int r = (rgb.x < 65536)? (unsigned int) (rgb.x * (1.0f / 128.0f)) : 0x1FF;
		unsigned int g = (rgb.y < 65536)? (unsigned int) (rgb.y * (1.0f / 128.0f)) : 0x1FF;
		unsigned int b = (rgb.z < 65536)? (unsigned int) (rgb.z * (1.0f / 128.0f)) : 0x1FF;
		unsigned int e = 31;

		return r | (g << 9) | (b << 18) | (e << 27);
	}
}

/* --------------------------------------------------------------------------------- */

Matrix2x2 operator + (const Matrix2x2 &m, const Matrix2x2 &n){
	return Matrix2x2(m.rows[0] + n.rows[0], m.rows[1] + n.rows[1]);
}

Matrix2x2 operator - (const Matrix2x2 &m, const Matrix2x2 &n){
	return Matrix2x2(m.rows[0] - n.rows[0], m.rows[1] - n.rows[1]);
}

Matrix2x2 operator - (const Matrix2x2 &m){
	return Matrix2x2(-m.rows[0], -m.rows[1]);
}

#define rcDot2(r, c) (m.rows[r].x * n.rows[0][c] + m.rows[r].y * n.rows[1][c])

Matrix2x2 operator * (const Matrix2x2 &m, const Matrix2x2 &n){
	return Matrix2x2(rcDot2(0, 0), rcDot2(0, 1), rcDot2(1, 0), rcDot2(1, 1));
}

Vector2 operator * (const Matrix2x2 &m, const Vector2 &v){
	return Vector2(dot(m.rows[0], v), dot(m.rows[1], v));
}

Matrix2x2 operator * (const Matrix2x2 &m, const float x){
	return Matrix2x2(m.rows[0] * x, m.rows[1] * x);
}

Matrix2x2 transpose(const Matrix2x2 &m){
	return Matrix2x2(
		m.rows[0].x, m.rows[1].x,
		m.rows[0].y, m.rows[1].y);
}

float det(const Matrix2x2 &m){
	return (m.rows[0].x * m.rows[1].y - m.rows[0].y * m.rows[1].x);
}

Matrix2x2 operator ! (const Matrix2x2 &m){
	float invDet = 1.0f / det(m);

	return Matrix2x2(
		 m.rows[1].y, -m.rows[0].y,
		-m.rows[1].x,  m.rows[0].x) * invDet;
}

/* --------------------------------------------------------------------------------- */

Matrix3x3 operator + (const Matrix3x3 &m, const Matrix3x3 &n){
	return Matrix3x3(m.rows[0] + n.rows[0], m.rows[1] + n.rows[1], m.rows[2] + n.rows[2]);
}

Matrix3x3 operator - (const Matrix3x3 &m, const Matrix3x3 &n){
	return Matrix3x3(m.rows[0] - n.rows[0], m.rows[1] - n.rows[1], m.rows[2] - n.rows[2]);
}

Matrix3x3 operator - (const Matrix3x3 &m){
	return Matrix3x3(-m.rows[0], -m.rows[1], -m.rows[2]);
}

#define rcDot3(r, c) (m.rows[r].x * n.rows[0][c] + m.rows[r].y * n.rows[1][c] + m.rows[r].z * n.rows[2][c])

Matrix3x3 operator * (const Matrix3x3 &m, const Matrix3x3 &n){
	return Matrix3x3(
		rcDot3(0, 0), rcDot3(0, 1), rcDot3(0, 2),
		rcDot3(1, 0), rcDot3(1, 1), rcDot3(1, 2),
		rcDot3(2, 0), rcDot3(2, 1), rcDot3(2, 2));
}

Vector3 operator * (const Matrix3x3 &m, const Vector3 &v){
	return Vector3(dot(m.rows[0], v), dot(m.rows[1], v), dot(m.rows[2], v));
}

Matrix3x3 operator * (const Matrix3x3 &m, const float x){
	return Matrix3x3(m.rows[0] * x, m.rows[1] * x, m.rows[2] * x);
}

Matrix3x3 transpose(const Matrix3x3 &m){
	return Matrix3x3(
		m.rows[0].x, m.rows[1].x, m.rows[2].x,
		m.rows[0].y, m.rows[1].y, m.rows[2].y,
		m.rows[0].z, m.rows[1].z, m.rows[2].z);
}

float det(const Matrix3x3 &m){
	return 
		m.rows[0].x * (m.rows[1].y * m.rows[2].z - m.rows[2].y * m.rows[1].z) -
		m.rows[0].y * (m.rows[1].x * m.rows[2].z - m.rows[1].z * m.rows[2].x) +
		m.rows[0].z * (m.rows[1].x * m.rows[2].y - m.rows[1].y * m.rows[2].x);
}

Matrix3x3 operator ! (const Matrix3x3 &m){
	float invDet = 1.0f / det(m);

	return Matrix3x3(
		m.rows[1].y * m.rows[2].z - m.rows[1].z * m.rows[2].y, m.rows[2].y * m.rows[0].z - m.rows[0].y * m.rows[2].z, m.rows[0].y * m.rows[1].z - m.rows[1].y * m.rows[0].z,
		m.rows[1].z * m.rows[2].x - m.rows[1].x * m.rows[2].z, m.rows[0].x * m.rows[2].z - m.rows[2].x * m.rows[0].z, m.rows[1].x * m.rows[0].z - m.rows[0].x * m.rows[1].z,
		m.rows[1].x * m.rows[2].y - m.rows[2].x * m.rows[1].y, m.rows[2].x * m.rows[0].y - m.rows[0].x * m.rows[2].y, m.rows[0].x * m.rows[1].y - m.rows[0].y * m.rows[1].x) * invDet;
}

/* --------------------------------------------------------------------------------- */

void Matrix4x4::translate(const Vector3 &v){
	rows[0].w += dot(rows[0].xyz(), v);
	rows[1].w += dot(rows[1].xyz(), v);
	rows[2].w += dot(rows[2].xyz(), v);
	rows[3].w += dot(rows[3].xyz(), v);
}

Matrix4x4 operator + (const Matrix4x4 &m, const Matrix4x4 &n){
	return Matrix4x4(m.rows[0] + n.rows[0], m.rows[1] + n.rows[1], m.rows[2] + n.rows[2], m.rows[3] + n.rows[3]);
}

Matrix4x4 operator - (const Matrix4x4 &m, const Matrix4x4 &n){
	return Matrix4x4(m.rows[0] - n.rows[0], m.rows[1] - n.rows[1], m.rows[2] - n.rows[2], m.rows[3] - n.rows[3]);
}

Matrix4x4 operator - (const Matrix4x4 &m){
	return Matrix4x4(-m.rows[0], -m.rows[1], -m.rows[2], -m.rows[3]);
}

#define rcDot4(r, c) (m.rows[r].x * n.rows[0][c] + m.rows[r].y * n.rows[1][c] + m.rows[r].z * n.rows[2][c] + m.rows[r].w * n.rows[3][c])

Matrix4x4 operator * (const Matrix4x4 &m, const Matrix4x4 &n){
	return Matrix4x4(
		rcDot4(0, 0), rcDot4(0, 1), rcDot4(0, 2), rcDot4(0, 3),
		rcDot4(1, 0), rcDot4(1, 1), rcDot4(1, 2), rcDot4(1, 3),
		rcDot4(2, 0), rcDot4(2, 1), rcDot4(2, 2), rcDot4(2, 3),
		rcDot4(3, 0), rcDot4(3, 1), rcDot4(3, 2), rcDot4(3, 3));
}

Vector4 operator * (const Matrix4x4 &m, const Vector4 &v){
	return Vector4(dot(m.rows[0], v), dot(m.rows[1], v), dot(m.rows[2], v), dot(m.rows[3], v));
}

Matrix4x4 operator * (const Matrix4x4 &m, const float x){
	return Matrix4x4(m.rows[0] * x, m.rows[1] * x, m.rows[2] * x, m.rows[3] * x);
}

Matrix4x4 transpose(const Matrix4x4 &m){
	return Matrix4x4(
		m.rows[0].x, m.rows[1].x, m.rows[2].x, m.rows[3].x,
		m.rows[0].y, m.rows[1].y, m.rows[2].y, m.rows[3].y,
		m.rows[0].z, m.rows[1].z, m.rows[2].z, m.rows[3].z,
		m.rows[0].w, m.rows[1].w, m.rows[2].w, m.rows[3].w);
}

Matrix4x4 operator ! (const Matrix4x4 &m){
	Matrix4x4 mat;

	float p00 = m.rows[2][2] * m.rows[3][3];
	float p01 = m.rows[3][2] * m.rows[2][3];
	float p02 = m.rows[1][2] * m.rows[3][3];
	float p03 = m.rows[3][2] * m.rows[1][3];
	float p04 = m.rows[1][2] * m.rows[2][3];
	float p05 = m.rows[2][2] * m.rows[1][3];
	float p06 = m.rows[0][2] * m.rows[3][3];
	float p07 = m.rows[3][2] * m.rows[0][3];
	float p08 = m.rows[0][2] * m.rows[2][3];
	float p09 = m.rows[2][2] * m.rows[0][3];
	float p10 = m.rows[0][2] * m.rows[1][3];
	float p11 = m.rows[1][2] * m.rows[0][3];

	mat.rows[0][0] = (p00 * m.rows[1][1] + p03 * m.rows[2][1] + p04 * m.rows[3][1]) - (p01 * m.rows[1][1] + p02 * m.rows[2][1] + p05 * m.rows[3][1]);
	mat.rows[0][1] = (p01 * m.rows[0][1] + p06 * m.rows[2][1] + p09 * m.rows[3][1]) - (p00 * m.rows[0][1] + p07 * m.rows[2][1] + p08 * m.rows[3][1]);
	mat.rows[0][2] = (p02 * m.rows[0][1] + p07 * m.rows[1][1] + p10 * m.rows[3][1]) - (p03 * m.rows[0][1] + p06 * m.rows[1][1] + p11 * m.rows[3][1]);
	mat.rows[0][3] = (p05 * m.rows[0][1] + p08 * m.rows[1][1] + p11 * m.rows[2][1]) - (p04 * m.rows[0][1] + p09 * m.rows[1][1] + p10 * m.rows[2][1]);
	mat.rows[1][0] = (p01 * m.rows[1][0] + p02 * m.rows[2][0] + p05 * m.rows[3][0]) - (p00 * m.rows[1][0] + p03 * m.rows[2][0] + p04 * m.rows[3][0]);
	mat.rows[1][1] = (p00 * m.rows[0][0] + p07 * m.rows[2][0] + p08 * m.rows[3][0]) - (p01 * m.rows[0][0] + p06 * m.rows[2][0] + p09 * m.rows[3][0]);
	mat.rows[1][2] = (p03 * m.rows[0][0] + p06 * m.rows[1][0] + p11 * m.rows[3][0]) - (p02 * m.rows[0][0] + p07 * m.rows[1][0] + p10 * m.rows[3][0]);
	mat.rows[1][3] = (p04 * m.rows[0][0] + p09 * m.rows[1][0] + p10 * m.rows[2][0]) - (p05 * m.rows[0][0] + p08 * m.rows[1][0] + p11 * m.rows[2][0]);

	float q00 = m.rows[2][0] * m.rows[3][1];
	float q01 = m.rows[3][0] * m.rows[2][1];
	float q02 = m.rows[1][0] * m.rows[3][1];
	float q03 = m.rows[3][0] * m.rows[1][1];
	float q04 = m.rows[1][0] * m.rows[2][1];
	float q05 = m.rows[2][0] * m.rows[1][1];
	float q06 = m.rows[0][0] * m.rows[3][1];
	float q07 = m.rows[3][0] * m.rows[0][1];
	float q08 = m.rows[0][0] * m.rows[2][1];
	float q09 = m.rows[2][0] * m.rows[0][1];
	float q10 = m.rows[0][0] * m.rows[1][1];
	float q11 = m.rows[1][0] * m.rows[0][1];

	mat.rows[2][0] = (q00 * m.rows[1][3] + q03 * m.rows[2][3] + q04 * m.rows[3][3]) - (q01 * m.rows[1][3] + q02 * m.rows[2][3] + q05 * m.rows[3][3]);
	mat.rows[2][1] = (q01 * m.rows[0][3] + q06 * m.rows[2][3] + q09 * m.rows[3][3]) - (q00 * m.rows[0][3] + q07 * m.rows[2][3] + q08 * m.rows[3][3]);
	mat.rows[2][2] = (q02 * m.rows[0][3] + q07 * m.rows[1][3] + q10 * m.rows[3][3]) - (q03 * m.rows[0][3] + q06 * m.rows[1][3] + q11 * m.rows[3][3]);
	mat.rows[2][3] = (q05 * m.rows[0][3] + q08 * m.rows[1][3] + q11 * m.rows[2][3]) - (q04 * m.rows[0][3] + q09 * m.rows[1][3] + q10 * m.rows[2][3]);
	mat.rows[3][0] = (q02 * m.rows[2][2] + q05 * m.rows[3][2] + q01 * m.rows[1][2]) - (q04 * m.rows[3][2] + q00 * m.rows[1][2] + q03 * m.rows[2][2]);
	mat.rows[3][1] = (q08 * m.rows[3][2] + q00 * m.rows[0][2] + q07 * m.rows[2][2]) - (q06 * m.rows[2][2] + q09 * m.rows[3][2] + q01 * m.rows[0][2]);
	mat.rows[3][2] = (q06 * m.rows[1][2] + q11 * m.rows[3][2] + q03 * m.rows[0][2]) - (q10 * m.rows[3][2] + q02 * m.rows[0][2] + q07 * m.rows[1][2]);
	mat.rows[3][3] = (q10 * m.rows[2][2] + q04 * m.rows[0][2] + q09 * m.rows[1][2]) - (q08 * m.rows[1][2] + q11 * m.rows[2][2] + q05 * m.rows[0][2]);

	return mat * (1.0f / (m.rows[0][0] * mat.rows[0][0] + m.rows[1][0] * mat.rows[0][1] + m.rows[2][0] * mat.rows[0][2] + m.rows[3][0] * mat.rows[0][3]));
}


/* --------------------------------------------------------------------------------- */

Matrix2x2 rotate(const float angle){
	float cosA = cosf(angle), sinA = sinf(angle);

	return Matrix2x2(cosA, -sinA, sinA, cosA);
}

Matrix4x4 rotateX(const float angle){
	float cosA = cosf(angle), sinA = sinf(angle);
	
	return Matrix4x4(
		1, 0,     0,    0,
		0, cosA, -sinA, 0,
		0, sinA,  cosA, 0,
		0, 0,     0,    1);
}

Matrix4x4 rotateY(const float angle){
	float cosA = cosf(angle), sinA = sinf(angle);

	return Matrix4x4(
		cosA, 0, -sinA, 0,
		0,    1,  0,    0,
		sinA, 0,  cosA, 0,
		0,    0,  0,    1);
}

Matrix4x4 rotateZ(const float angle){
	float cosA = cosf(angle), sinA = sinf(angle);

	return Matrix4x4(
		cosA, -sinA, 0, 0,
		sinA,  cosA, 0, 0,
		0,     0,    1, 0,
		0,     0,    0, 1);
}

Matrix4x4 rotateXY(const float angleX, const float angleY){
	float cosX = cosf(angleX), sinX = sinf(angleX), 
		  cosY = cosf(angleY), sinY = sinf(angleY);

	return Matrix4x4(
		 cosY,        0,    -sinY,        0,
		-sinX * sinY, cosX, -sinX * cosY, 0,
		 cosX * sinY, sinX,  cosX * cosY, 0,
		 0,           0,     0,           1);
}

Matrix4x4 rotateYX(const float angleX, const float angleY){
	float cosX = cosf(angleX), sinX = sinf(angleX), 
		  cosY = cosf(angleY), sinY = sinf(angleY);

	return Matrix4x4(
		cosY, -sinX * sinY, -cosX * sinY, 0,
		0,     cosX,        -sinX,        0,
		sinY,  sinX * cosY,  cosX * cosY, 0,
		0,     0,            0,           1);
}

Matrix4x4 rotateZXY(const float angleX, const float angleY, const float angleZ){
	float cosX = cosf(angleX), sinX = sinf(angleX), 
		  cosY = cosf(angleY), sinY = sinf(angleY),
		  cosZ = cosf(angleZ), sinZ = sinf(angleZ);

	return Matrix4x4(
		cosY * cosZ + sinX * sinY * sinZ,   -cosX * sinZ,    sinX * cosY * sinZ - sinY * cosZ,  0,
		cosY * sinZ - sinX * sinY * cosZ,    cosX * cosZ,   -sinY * sinZ - sinX * cosY * cosZ,  0,
		cosX * sinY,                         sinX,           cosX * cosY,                       0,
		0,                                   0,              0,                                 1);
}

Matrix4x4 translate(const Vector3 &v){
	return Matrix4x4(1,0,0,v.x, 0,1,0,v.y, 0,0,1,v.z, 0,0,0,1);
}

Matrix4x4 translate(const float x, const float y, const float z){
	return Matrix4x4(1,0,0,x, 0,1,0,y, 0,0,1,z, 0,0,0,1);
}

Matrix4x4 scale(const float x, const float y, const float z){
	return Matrix4x4(x,0,0,0, 0,y,0,0, 0,0,z,0, 0,0,0,1);
}

Matrix4x4 perspectiveMatrix(const float fov, const float zNear, const float zFar){
	float s = cosf(0.5f * fov) / sinf(0.5f * fov);

	return Matrix4x4(
		s, 0, 0, 0,
		0, s, 0, 0,
		0, 0, (zFar + zNear) / (zFar - zNear), -(2 * zFar * zNear) / (zFar - zNear),
		0, 0, 1, 0);
}

Matrix4x4 perspectiveMatrixX(const float fov, const int width, const int height, const float zNear, const float zFar){
	float w = cosf(0.5f * fov) / sinf(0.5f * fov);
	float h = (w * width) / height;

	return Matrix4x4(
		w, 0, 0, 0,
		0, h, 0, 0,
		0, 0, (zFar + zNear) / (zFar - zNear), -(2 * zFar * zNear) / (zFar - zNear),
		0, 0, 1, 0);
}

Matrix4x4 perspectiveMatrixY(const float fov, const int width, const int height, const float zNear, const float zFar){
	float h = cosf(0.5f * fov) / sinf(0.5f * fov);
	float w = (h * height) / width;

	return Matrix4x4(
		w, 0, 0, 0,
		0, h, 0, 0,
		0, 0, (zFar + zNear) / (zFar - zNear), -(2 * zFar * zNear) / (zFar - zNear),
		0, 0, 1, 0);
}

Matrix4x4 orthoMatrixX(const float left, const float right, const float top, const float bottom, const float zNear, const float zFar){
	float rl = right - left;
	float tb = top - bottom;
	float fn = zFar - zNear;

	return Matrix4x4(
		2.0f / rl, 0,         0,         -(right + left) / rl,
		0,         2.0f / tb, 0,         -(top + bottom) / tb,
		0,         0,        -2.0f / fn, -(zFar + zNear) / fn,
		0,         0,         0,         1);
}

Matrix4x4 toD3DProjection(const Matrix4x4 &m){
	Matrix4x4 mat;

	mat.rows[0] = m.rows[0];
	mat.rows[1] = m.rows[1];
	mat.rows[2] = 0.5f * (m.rows[2] + m.rows[3]);
	mat.rows[3] = m.rows[3];

	return mat;
}

Matrix4x4 toGLProjection(const Matrix4x4 &m){
	Matrix4x4 mat;

	mat.rows[0] = m.rows[0];
	mat.rows[1] = m.rows[1];
	mat.rows[2] = m.rows[2] * 2.0 - m.rows[3];
	mat.rows[3] = m.rows[3];

	return mat;
}

Matrix4x4 cubeViewMatrix(const unsigned int side){
	switch(side){
	case POSITIVE_X:
		return Matrix4x4(
			0, 0, -1, 0,
			0, 1,  0, 0,
			1, 0,  0, 0,
			0, 0,  0, 1);
	case NEGATIVE_X:
		return Matrix4x4(
			 0, 0, 1, 0,
			 0, 1, 0, 0,
			-1, 0, 0, 0,
			 0, 0, 0, 1);
	case POSITIVE_Y:
		return Matrix4x4(
			1, 0,  0, 0,
			0, 0, -1, 0,
			0, 1,  0, 0,
			0, 0,  0, 1);
	case NEGATIVE_Y:
		return Matrix4x4(
			1,  0, 0, 0,
			0,  0, 1, 0,
			0, -1, 0, 0,
			0,  0, 0, 1);
	case POSITIVE_Z:
		return Matrix4x4(
			1, 0, 0, 0,
			0, 1, 0, 0,
			0, 0, 1, 0,
			0, 0, 0, 1);
	//case NEGATIVE_Z:
	default:
		return Matrix4x4(
			-1, 0,  0, 0,
			 0, 1,  0, 0,
			 0, 0, -1, 0,
			 0, 0,  0, 1);
	}
}

Matrix4x4 cubeProjectionMatrixGL(const float zNear, const float zFar){
	return Matrix4x4(
		1,  0, 0, 0,
		0, -1, 0, 0,
		0,  0, (zFar + zNear) / (zFar - zNear), -(2 * zFar * zNear) / (zFar - zNear),
		0,  0, 1, 0);
}

Matrix4x4 cubeProjectionMatrixD3D(const float zNear, const float zFar){
	return Matrix4x4(
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, zFar / (zFar - zNear), (zFar * zNear) / (zNear - zFar),
		0, 0, 1, 0);
}

Matrix2x2 identity2(){
	return Matrix2x2(1,0, 0,1);
}

Matrix3x3 identity3(){
	return Matrix3x3(1,0,0, 0,1,0, 0,0,1);
}

Matrix4x4 identity4(){
	return Matrix4x4(1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
}