#include "quaternion.h"

#include "SDL_opengl.h"
#include <cmath>

Quaternion::Quaternion() {
	x=0;
	y=0;
	z=0;
	w=1;
}

Quaternion::~Quaternion() {
}

void Quaternion::from_axisangle(C3f axis,float angle) {
    float ang=(angle/180.0f)*M_PI;
    float r=sinf(ang/2.0f);
		
    w=cosf(ang/2.0f);
    x=axis.x * r;
    y=axis.y * r;
    z=axis.z * r;
}

void Quaternion::to_matrix(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;  
    matrix[13] = 0;  
    matrix[14] = 0;  
    matrix[15] = 1.0f;
}

Quaternion Quaternion::operator *(Quaternion q) {
    Quaternion r;
    r.w = w*q.w - x*q.x - y*q.y - z*q.z;
    r.x = w*q.x + x*q.w + y*q.z - z*q.y;
    r.y = w*q.y + y*q.w + z*q.x - x*q.z;
    r.z = w*q.z + z*q.w + x*q.y - y*q.x;
    return r;
}
