#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;

}