#include "private.h"


// matrix

void heliMatrix4Identity(double *r) {
	r[ 0] = 1; r[ 1] = 0; r[ 2] = 0; r[ 3] = 0;
	r[ 4] = 0; r[ 5] = 1; r[ 6] = 0; r[ 7] = 0;
	r[ 8] = 0; r[ 9] = 0; r[10] = 1; r[11] = 0;
	r[12] = 0; r[13] = 0; r[14] = 0; r[15] = 1;
}

void heliMatrix4Translation(double *r, double x, double y, double z) {
	r[ 0] = 1; r[ 1] = 0; r[ 2] = 0; r[ 3] = 0;
	r[ 4] = 0; r[ 5] = 1; r[ 6] = 0; r[ 7] = 0;
	r[ 8] = 0; r[ 9] = 0; r[10] = 1; r[11] = 0;
	r[12] = x; r[13] = y; r[14] = z; r[15] = 1;
}

void heliMatrix4Scale(double *r, double x, double y, double z) {
	r[ 0] = x; r[ 1] = 0; r[ 2] = 0; r[ 3] = 0;
	r[ 4] = 0; r[ 5] = y; r[ 6] = 0; r[ 7] = 0;
	r[ 8] = 0; r[ 9] = 0; r[10] = z; r[11] = 0;
	r[12] = 0; r[13] = 0; r[14] = 0; r[15] = 1;
}

void heliMatrix4RotationZ(double *r, double a) {
	double s = sin(a);
	double c = cos(a);
	r[ 0] = c; r[ 1] = s; r[ 2] = 0; r[ 3] = 0;
	r[ 4] =-s; r[ 5] = c; r[ 6] = 0; r[ 7] = 0;
	r[ 8] = 0; r[ 9] = 0; r[10] = 1; r[11] = 0;
	r[12] = 0; r[13] = 0; r[14] = 0; r[15] = 1;
}

void heliMatrix4MultVec(double *r, const double *m, const double *v) {
	r[0] = v[0]*m[0] + v[1]*m[4] + v[2]*m[ 8] + v[3]*m[12];
	r[1] = v[0]*m[1] + v[1]*m[5] + v[2]*m[ 9] + v[3]*m[13];
	r[2] = v[0]*m[2] + v[1]*m[6] + v[2]*m[10] + v[3]*m[14];
	r[3] = v[0]*m[3] + v[1]*m[7] + v[2]*m[11] + v[3]*m[15];
}

void heliMatrix4Mult(double *r, const double *a, const double *b) {
	heliMatrix4MultVec(r +  0, a, b +  0);
	heliMatrix4MultVec(r +  4, a, b +  4);
	heliMatrix4MultVec(r +  8, a, b +  8);
	heliMatrix4MultVec(r + 12, a, b + 12);
}

int heliMatrix4Invert(double *r, const double *m) {
	r[ 0] = m[ 5]*(m[10]*m[15] - m[11]*m[14]) + m[ 6]*(m[11]*m[13] - m[ 9]*m[15]) + m[ 7]*(m[ 9]*m[14] - m[10]*m[13]);
	r[ 4] = m[ 4]*(m[11]*m[14] - m[10]*m[15]) + m[ 6]*(m[ 8]*m[15] - m[11]*m[12]) + m[ 7]*(m[10]*m[12] - m[ 8]*m[14]);
	r[ 8] = m[ 4]*(m[ 9]*m[15] - m[11]*m[13]) + m[ 5]*(m[11]*m[12] - m[ 8]*m[15]) + m[ 7]*(m[ 8]*m[13] - m[ 9]*m[12]);
	r[12] = m[ 4]*(m[10]*m[13] - m[ 9]*m[14]) + m[ 5]*(m[ 8]*m[14] - m[10]*m[12]) + m[ 6]*(m[ 9]*m[12] - m[ 8]*m[13]);
	
	double det = m[ 0]*r[0] + m[ 1]*r[4] + m[ 2]*r[8] + m[ 3]*r[12];
	if (fabs(det) <= HELI_PRECISION_SQR*HELI_PRECISION) {
		memset(r, 0, sizeof(*r)*16);
		return FALSE;
	}
	det = 1/det;
	r[ 0] *= det;
	r[ 4] *= det;
	r[ 8] *= det;
	r[12] *= det;
	
	r[ 1] = det*(m[ 1]*(m[11]*m[14] - m[10]*m[15]) + m[ 2]*(m[ 9]*m[15] - m[11]*m[13]) + m[ 3]*(m[10]*m[13] - m[ 9]*m[14]));
	r[ 5] = det*(m[ 0]*(m[10]*m[15] - m[11]*m[14]) + m[ 2]*(m[11]*m[12] - m[ 8]*m[15]) + m[ 3]*(m[ 8]*m[14] - m[10]*m[12]));
	r[ 9] = det*(m[ 0]*(m[11]*m[13] - m[ 9]*m[15]) + m[ 1]*(m[ 8]*m[15] - m[11]*m[12]) + m[ 3]*(m[ 9]*m[12] - m[ 8]*m[13]));
	r[13] = det*(m[ 0]*(m[ 9]*m[14] - m[10]*m[13]) + m[ 1]*(m[10]*m[12] - m[ 8]*m[14]) + m[ 2]*(m[ 8]*m[13] - m[ 9]*m[12]));
	r[ 2] = det*(m[ 1]*(m[ 6]*m[15] - m[ 7]*m[14]) + m[ 2]*(m[ 7]*m[13] - m[ 5]*m[15]) + m[ 3]*(m[ 5]*m[14] - m[ 6]*m[13]));
	r[ 6] = det*(m[ 0]*(m[ 7]*m[14] - m[ 6]*m[15]) + m[ 2]*(m[ 4]*m[15] - m[ 7]*m[12]) + m[ 3]*(m[ 6]*m[12] - m[ 4]*m[14]));
	r[10] = det*(m[ 0]*(m[ 5]*m[15] - m[ 7]*m[13]) + m[ 1]*(m[ 7]*m[12] - m[ 4]*m[15]) + m[ 3]*(m[ 4]*m[13] - m[ 5]*m[12]));
	r[14] = det*(m[ 0]*(m[ 6]*m[13] - m[ 5]*m[14]) + m[ 1]*(m[ 4]*m[14] - m[ 6]*m[12]) + m[ 2]*(m[ 5]*m[12] - m[ 4]*m[13]));
	r[ 3] = det*(m[ 1]*(m[ 7]*m[10] - m[ 6]*m[11]) + m[ 2]*(m[ 5]*m[11] - m[ 7]*m[ 9]) + m[ 3]*(m[ 6]*m[ 9] - m[ 5]*m[10]));
	r[ 7] = det*(m[ 0]*(m[ 6]*m[11] - m[ 7]*m[10]) + m[ 2]*(m[ 7]*m[ 8] - m[ 4]*m[11]) + m[ 3]*(m[ 4]*m[10] - m[ 6]*m[ 8]));
	r[11] = det*(m[ 0]*(m[ 7]*m[ 9] - m[ 5]*m[11]) + m[ 1]*(m[ 4]*m[11] - m[ 7]*m[ 8]) + m[ 3]*(m[ 5]*m[ 8] - m[ 4]*m[ 9]));
	r[15] = det*(m[ 0]*(m[ 5]*m[10] - m[ 6]*m[ 9]) + m[ 1]*(m[ 6]*m[ 8] - m[ 4]*m[10]) + m[ 2]*(m[ 4]*m[ 9] - m[ 5]*m[ 8]));
	
	return TRUE;
}