/* libsegl - Sebas Extended GL Library
 *           Collection of Opengl/3D-Math helpers
 *
 *      Copyright (c) 2008 by Sebastian Lohff, seba@seba-geek.de
 *      http://www.seba-geek.de
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 * 
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
 * Boston, MA  02110-1301, USA.
 */

#include "quaternion.h"

namespace segl {

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

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

Quaternion::Quaternion(float deg, Punkt3D p) {
	set(deg, p);
}

Quaternion::Quaternion(float _x, float _y, float _z) {
	set(_x, _y, _z);
}

void Quaternion::set(float rotx, float roty, float rotz) {
	rotx = deg2rad(rotx);
	roty = deg2rad(roty);
	rotz = deg2rad(rotz);
	
	float sinx = sin(rotx);
	float siny = sin(roty);
	float sinz = sin(rotz);
	float cosx = cos(rotx);
	float cosy = cos(roty);
	float cosz = cos(rotz);
	
	x = sinz * cosx * cosy - cosz * sinx * siny;
	y = cosz * sinx * cosy + sinz * cosx * siny;
	z = cosz * cosx * siny - sinz * sinx * cosy;
	w = cosz * cosx * cosy - sinz * sinx * siny;
	
	normalize();
}

void Quaternion::set(float deg, Punkt3D p) {
	float angle = deg2rad(0.5f*deg);
	float sinangle = sin(angle);
	
	x = p.x * sinangle;
	y = p.y * sinangle;
	z = p.z * sinangle;
	w = cos(angle);
	
	normalize();
}

void Quaternion::normalize() {
	float len = x*x+y*y+z*z+w*w;
	if(std::fabs(len-1.0f)>0.00001f) {
		len = sqrt(len);
		x /= len;
		y /= len;
		z /= len;
		w /= len;
	}
}

Quaternion Quaternion::getConjugate() {
	return Quaternion(-x, -y, -z, w);
}

// Rotationsmatrix Quaternion::getRotMat() {
// 	
// }

Punkt3D Quaternion::rotP3D(const Punkt3D &p) {
	Quaternion q(p.x, p.y, p.z, 0.0f);
	
	Quaternion erg = *this * q * this->getConjugate();
	
	return Punkt3D(erg.x, erg.y, erg.z);
}

Quaternion Quaternion::operator*(const Quaternion &q) {
	return Quaternion(w*q.x - x*q.w + y*q.z - z*q.y,
					  w*q.y - y*q.w + z*q.x - x*q.z,
					  w*q.z - z*q.w + x*q.y - y*q.x,
					  w*q.w - x*q.x - y*q.y - z*q.z);
}



// 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);
// }

} // namespace segl