#include <math.h>
#include <neofx/math.h>
#include <neofx/types.h>

float VectorDot(VECTOR v1, VECTOR v2) {
	return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

VECTOR VectorCross(VECTOR v1, VECTOR v2) {
	VECTOR vec;
	
	vec.x = v1.y * v2.z - v1.z * v2.y;
	vec.y = v1.z * v2.x - v1.x * v2.z;
	vec.z = v1.x * v2.y - v1.y * v2.x;
	
	return vec;
}

VECTOR VectorSub(VECTOR v1, VECTOR v2) {
	VECTOR vec;
	
	vec.x = v1.x - v2.x;
	vec.y = v1.y - v2.y;
	vec.z = v1.z - v2.z;
	
	return vec;
}

VECTOR VectorAdd(VECTOR v1, VECTOR v2) {
	VECTOR vec;
	
	vec.x = v1.x + v2.x;
	vec.y = v1.y + v2.y;
	vec.z = v1.z + v2.z;
	
	return vec;
}

VECTOR VectorMul(VECTOR v, float f) {
	v.x *= f;
	v.y *= f;
	v.z *= f;
	
	return v;
}

VECTOR VectorNormalize(VECTOR v) {
	float f = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
	
	v.x /= f;
	v.y /= f;
	v.z /= f;
	
	return v;
}

VECTOR VectorNeg(VECTOR v) {
	v.x = -v.x;
	v.y = -v.y;
	v.z = -v.z;
	
	return v;
}

float VectorLength(VECTOR v) {
	return sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
}

float VectorLengthSq(VECTOR v) {
	return (v.x*v.x + v.y*v.y + v.z*v.z);
}

int VectorEqual(VECTOR v1, VECTOR v2) {
	if(v1.x == v2.x && v1.y == v2.y && v1.z == v2.z) return 1;
	return 0;
}

MATRIX MatrixIdentity() {
	MATRIX m = {
		1.0, 0.0, 0.0, 0.0,
		0.0, 1.0, 0.0, 0.0,
		0.0, 0.0, 1.0, 0.0,
		0.0, 0.0, 0.0, 1.0
	};
	return m;
}

MATRIX MatrixMul(MATRIX m1, MATRIX m2) {
	MATRIX m = {
		0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0
	};
	short i, j, k;
	
	for(i = 0; i < 4; i++)
		for(j = 0; j < 4; j++)
			for(k = 0; k < 4; k++)
				m.m[i][j] += m1.m[i][k] * m2.m[k][j];
	
	return m;
}

MATRIX VectorMatrix(VERTEX p1, VECTOR v1, VERTEX p2, VECTOR v2) {
	MATRIX translate;
	MATRIX rotate;
	VECTOR axis;
	float s, c;
	
	axis = VectorCross(v1, v2);
	s = VectorLength(axis);
	c = VectorDot(v1, v2);
	if(s) axis = VectorMul(axis, 1/s);
	
	rotate.m[0][0] = (axis.x * axis.x) * (1.0 - c) + c;
	rotate.m[0][1] = (axis.x * axis.y) * (1.0 - c) - (axis.z * s);
	rotate.m[0][2] = (axis.x * axis.z) * (1.0 - c) + (axis.y * s);
	rotate.m[0][3] = 0.0;
	
	rotate.m[1][0] = (axis.y * axis.x) * (1.0 - c) + (axis.z * s);
	rotate.m[1][1] = (axis.y * axis.y) * (1.0 - c) + c;
	rotate.m[1][2] = (axis.y * axis.z) * (1.0 - c) - (axis.x * s);
	rotate.m[1][3] = 0.0;
	
	rotate.m[2][0] = (axis.z * axis.x) * (1.0 - c) - (axis.y * s);
	rotate.m[2][1] = (axis.z * axis.y) * (1.0 - c) + (axis.x * s);
	rotate.m[2][2] = (axis.z * axis.z) * (1.0 - c) + c;
	rotate.m[2][3] = 0.0;
	
	rotate.m[3][0] = 0.0;
	rotate.m[3][1] = 0.0;
	rotate.m[3][2] = 0.0;
	rotate.m[3][3] = 1.0;
	
	translate = MatrixIdentity();
	
	translate.m[3][0] = p1.x;
	translate.m[3][1] = p1.y;
	translate.m[3][2] = p1.z;
	
	rotate = MatrixMul(rotate, translate);
	
	translate.m[3][0] = -p2.x;
	translate.m[3][1] = -p2.y;
	translate.m[3][2] = -p2.z;
	
	return MatrixMul(translate, rotate);
}

VECTOR VectorMatrixMul(VECTOR v, MATRIX m) {
	VECTOR r;
	float w;
	
	r.x = v.x*m.m[0][0] + v.y*m.m[1][0] + v.z*m.m[2][0] + m.m[3][0];
	r.y = v.x*m.m[0][1] + v.y*m.m[1][1] + v.z*m.m[2][1] + m.m[3][1];
	r.z = v.x*m.m[0][2] + v.y*m.m[1][2] + v.z*m.m[2][2] + m.m[3][2];
	w = v.x*m.m[0][3] + v.y*m.m[1][3] + v.z*m.m[2][3] + m.m[3][3];
	
	r.x /= w;
	r.y /= w;
	r.z /= w;
	
	return r;
}