#include "hcoord.h"
#include <SDL/SDL_opengl.h>

HVector::HVector() {
	w = 1.0;
}

HVector::HVector(double _x, double _y, double _z, double _w) {
	x = _x;
	y = _y;
	z = _z;
	w = _w;
}

HMatrix::HMatrix() {
	m[3][0] = m[3][1] = m[3][2] = 0;
	m[3][3] = 1.0;
}

HMatrix HMatrix::invtr() {
	HMatrix res;
	
	res.m[0][0] = m[0][0];
	res.m[0][1] = m[1][0];
	res.m[0][2] = m[2][0];
	
	res.m[1][0] = m[0][1];
	res.m[1][1] = m[1][1];
	res.m[1][2] = m[2][1];
	
	res.m[2][0] = m[0][2];
	res.m[2][1] = m[1][2];
	res.m[2][2] = m[2][2];
	
	res.m[0][3] = - res.m[0][0] * m[0][3] - res.m[0][1] * m[1][3] - res.m[0][2] * m[2][3];
	res.m[1][3] = - res.m[1][0] * m[0][3] - res.m[1][1] * m[1][3] - res.m[1][2] * m[2][3];
	res.m[2][3] = - res.m[2][0] * m[0][3] - res.m[2][1] * m[1][3] - res.m[2][2] * m[2][3];
	
	return res;
}

HMatrix HMatrix::invtrs() {
	HMatrix res;
	
	double s = 1 / (m[0][0] * m[0][0] + m[0][1] * m[0][1] + m[0][2] * m[0][2]);
	
	res.m[0][0] = s * m[0][0];
	res.m[0][1] = s * m[1][0];
	res.m[0][2] = s * m[2][0];
	
	res.m[1][0] = s * m[0][1];
	res.m[1][1] = s * m[1][1];
	res.m[1][2] = s * m[2][1];
	
	res.m[2][0] = s * m[0][2];
	res.m[2][1] = s * m[1][2];
	res.m[2][2] = s * m[2][2];
	
	res.m[0][3] = - res.m[0][0] * m[0][3] - res.m[0][1] * m[1][3] - res.m[0][2] * m[2][3];
	res.m[1][3] = - res.m[1][0] * m[0][3] - res.m[1][1] * m[1][3] - res.m[1][2] * m[2][3];
	res.m[2][3] = - res.m[2][0] * m[0][3] - res.m[2][1] * m[1][3] - res.m[2][2] * m[2][3];
	
	return res;
}

HVector HMatrix::operator*(const HVector& vec) {
	HVector res;
	res.x = m[0][0] * vec.x + m[0][1] * vec.y + m[0][2] * vec.z + m[0][3];
	res.y = m[1][0] * vec.x + m[1][1] * vec.y + m[1][2] * vec.z + m[1][3];
	res.z = m[2][0] * vec.x + m[2][1] * vec.y + m[2][2] * vec.z + m[2][3];
	return res;
}

HVector HMatrix::getTranslation() {
	return HVector(m[0][3], m[1][3], m[2][3]);
}

void HMatrix::glLoad() {
	glLoadTransposeMatrixd((double*)m);
}