#include "quaternion.h"

Quaternion::Quaternion() {
	w = 1.0f;
	x = y = z = 0.0f;
}

void Quaternion::createFromRotation(float deg, float _x, float _y, float _z) {
	float tmpres = sin(deg2rad(deg) / 2.0f);
	
	w = cos(deg2rad(deg) / 2.0f);
	x = _x * tmpres;
	y = _y * tmpres;
	z = _z * tmpres;
}

Quaternion Quaternion::operator*(const Quaternion &q) {
	Quaternion ret;

	ret.w = w*q.w - x*q.x - y*q.y - z*q.z;
	ret.x = w*q.x + x*q.w + y*q.z - z*q.y;
	ret.y = w*q.y + y*q.w + z*q.x - x*q.z;
	ret.z = w*q.z + z*q.w + x*q.y - y*q.x;

	return ret;
}

void Quaternion::createGlMatrix(GLfloat *matrix) {
	if(!matrix)
		return;

	matrix[0]  = 1.0f - 2.0f * ( y*y + z*z);
	matrix[1]  = 2.0f * ( x*y + z*w);
	matrix[2]  = 2.0f * ( x*z - y*w);
	matrix[3]  = 0.0f;

	matrix[4]  = 2.0f * ( x*y - z*w);
	matrix[5]  = 1.0f - 2.0f * ( x*x + z*z);
	matrix[6]  = 2.0f * ( z*y + x*w);
	matrix[7]  = 0.0f;

	matrix[8]  = 2.0f * ( x*z + y*w);
	matrix[9]  = 2.0f * ( y*z - x*w);
	matrix[10] = 1.0f - 2.0f * ( x*x + y*y);
	matrix[11] = 0.0f;

	matrix[12] = 0.0f;
	matrix[13] = 0.0f;
	matrix[14] = 0.0f;
	matrix[15] = 1.0f;
	
}

Punkt3D Quaternion::getDirectionVector() {
	GLfloat matrix[16];
	createGlMatrix(matrix);
	
	return getDirectionVector(matrix);
}

Punkt3D Quaternion::getDirectionVector(GLfloat *matrix) {
	if(!matrix)
		return Punkt3D();
	Punkt3D tmp;
	
	tmp.x = matrix[8];
	tmp.y = matrix[9];
	tmp.z = matrix[10];
	
	return tmp;
}

void glMultMatrixf(Quaternion q) {
	GLfloat matrix[16];
	q.createGlMatrix(matrix);
	glMultMatrixf(matrix);
}