/*********************************
 Implementation of quaternion-based
 object rotation
 
 Functions
 clickmotion - call when holded mouse moved
 click       - call when user clicks
 motion      - call when mouse just moves
 quatinit    - flush rotation value
 rotate      - multiply GL matrix
 
 Dependency: rotate.h,
 Standard: math.h 
 Specific: glut.h
 **********************************/

#include "my_glut.h"
#include "rotate.h"
#include "math.h"
#include <vector>

struct quaternion
{
	double x,y,z,w;
};

struct vector
{
	double x,y,z;
};

// Rotation
struct quaternion q;
struct vector drag, onclick;
double mx,my;
extern int width, height;
extern int interactive;
//
std::vector<quaternion> storage;

void clickmotion(int nmx, int nmy) // Mouse
{
	struct vector n;
	double norm,length,t;
	mx = 2.*(nmx/(double)width - 0.5);
	my = 2.*(0.5 - nmy/(double)height);
	norm = mx*mx + my*my;
	if( norm > 1.0 )
	{
		length = sqrt(norm);
		drag.x = mx/length;
		drag.y = my/length;
		drag.z = 0.0;
	}
	else
	{
		drag.x = mx;
		drag.y = my;
		drag.z = sqrt(1.0-norm);
	}
	n.x = drag.y*onclick.z - drag.z*onclick.y;
	n.y = drag.z*onclick.x - drag.x*onclick.z;
	n.z = drag.x*onclick.y - drag.y*onclick.x;
	if ( n.x*n.x + n.y*n.y + n.z*n.z > 10e-7 )
	{
		t = drag.x*onclick.x + drag.y*onclick.y + drag.z*onclick.z;
		q.x = + q.x*t + q.y*n.z - q.z*n.y + q.w*n.x;
		q.y = - q.x*n.z + q.y*t + q.z*n.x + q.w*n.y;
		q.z = + q.x*n.y - q.y*n.x + q.z*t + q.w*n.z;
		q.w = - q.x*n.x - q.y*n.y - q.z*n.z + q.w*t;
		onclick.x = drag.x;
		onclick.y = drag.y;
		onclick.z = drag.z;
	}
	glutPostRedisplay();
}
void motion(int nmx, int nmy) // Mouse
{
	mx = 2.*(nmx/(double)width - 0.5);
	my = 2.*(0.5 - nmy/(double)height);
}
void click(int b, int s, int nmx, int nmy) // Mouse
{
	double norm,length;
	switch(b)
	{
		case GLUT_LEFT_BUTTON:
			if( s == GLUT_DOWN ) interactive = 1;
			else interactive = 0;
			mx = 2.*(nmx/(double)width - 0.5);
			my = 2.*(0.5 - nmy/(double)height);
			norm = mx*mx + my*my;
			if( norm > 1.0 )
			{
				length = sqrt(norm);
				drag.x = mx/length;
				drag.y = my/length;
				drag.z = 0.0;
			}
			else	
			{
				drag.x = mx;
				drag.y = my;
				drag.z = sqrt(1.0-norm);
			}
			onclick.x = drag.x;
			onclick.y = drag.y;
			onclick.z = drag.z;
			break;
	}
	glutPostRedisplay();
}


void quatinit()
{
	q.x = 0.0;
	q.y = 0.0;
	q.z = 0.0;
	q.w = 1.0;
}

void quatpush()
{
	storage.push_back(q);
}

void quatpop()
{
	if( !storage.empty() )
	{
		q = storage.back();
		storage.pop_back();
	}
}

void rotatevector(double * vec)
{
	int i;
	double rot[16];
	double temp[4] = {vec[0],vec[1],vec[2],1.0};
	double ret[4];
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	for(i=0; i < 4; i++)
	{
		ret[i]  = temp[0] * rot[i*4];
		ret[i] += temp[1] * rot[i*4+1];
		ret[i] += temp[2] * rot[i*4+2];
		ret[i] += temp[3] * rot[i*4+3];
	}
	vec[0] = ret[0]/ret[3];
	vec[1] = ret[1]/ret[3];
	vec[2] = ret[2]/ret[3];
}

void reverse_rotatevector(double * vec)
{
	int i;
	double rot[16];
	double temp[4] = {vec[0],vec[1],vec[2],1.0};
	double ret[4];
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	for(i=0; i < 4; i++)
	{
		ret[i]  = temp[0] * rot[i*4];
		ret[i] += temp[1] * rot[i*4+1];
		ret[i] += temp[2] * rot[i*4+2];
		ret[i] += temp[3] * rot[i*4+3];
	}
	vec[0] = ret[0]/ret[3];
	vec[1] = ret[1]/ret[3];
	vec[2] = ret[2]/ret[3];
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;
}

void revrotatevector(double * vec)
{
	int i;
	double rot[16];
	double temp[4] = {vec[0],vec[1],vec[2],1.0};
	double ret[4];
	q.w = -q.w;
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	for(i=0; i < 4; i++)
	{
		ret[i]  = temp[0] * rot[i*4];
		ret[i] += temp[1] * rot[i*4+1];
		ret[i] += temp[2] * rot[i*4+2];
		ret[i] += temp[3] * rot[i*4+3];
	}
	vec[0] = ret[0]/ret[3];
	vec[1] = ret[1]/ret[3];
	vec[2] = ret[2]/ret[3];
	q.w = -q.w;
}


void rotatevector_from_stack(double * vec)
{
	int i;
	struct quaternion q = storage.back();
	double rot[16];
	double temp[4] = {vec[0],vec[1],vec[2],1.0};
	double ret[4];
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	for(i=0; i < 4; i++)
	{
		ret[i]  = temp[0] * rot[i*4];
		ret[i] += temp[1] * rot[i*4+1];
		ret[i] += temp[2] * rot[i*4+2];
		ret[i] += temp[3] * rot[i*4+3];
	}
	vec[0] = ret[0]/ret[3];
	vec[1] = ret[1]/ret[3];
	vec[2] = ret[2]/ret[3];
}

void quatget(double *vec)
{
	vec[0] = q.x / q.w;
	vec[1] = q.y / q.w;
	vec[2] = q.z / q.w;
}

void reverse_rotatevector_from_stack(double * vec)
{
	int i;
	struct quaternion q = storage.back();
	q.x = -q.x;
	q.y = -q.y;
	q.z = -q.z;
	double rot[16];
	double temp[4] = {vec[0],vec[1],vec[2],1.0};
	double ret[4];
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	for(i=0; i < 4; i++)
	{
		ret[i]  = temp[0] * rot[i*4];
		ret[i] += temp[1] * rot[i*4+1];
		ret[i] += temp[2] * rot[i*4+2];
		ret[i] += temp[3] * rot[i*4+3];
	}
	vec[0] = ret[0]/ret[3];
	vec[1] = ret[1]/ret[3];
	vec[2] = ret[2]/ret[3];
}

void rotate()
{
	double rot[16];
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	glMultMatrixd(rot);
}


void rotate_from_stack()
{
	double rot[16];
	struct quaternion q = storage.back();
	rot[ 0] = (q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
	rot[ 1] = 2.*(q.x*q.y - q.w*q.z);
	rot[ 2] = 2.*(q.x*q.z + q.w*q.y);
	rot[ 3] = 0.0;
	rot[ 4] = 2.*(q.x*q.y + q.w*q.z);
	rot[ 5] = (q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z);
	rot[ 6] = 2.*(q.y*q.z - q.w*q.x);
	rot[ 7] = 0.0;
	rot[ 8] = 2.*(q.x*q.z - q.w*q.y);
	rot[ 9] = 2.*(q.y*q.z + q.w*q.x);
	rot[10] = (q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
	rot[11] = 0.0;
	rot[12] = 0.0;
	rot[13] = 0.0;
	rot[14] = 0.0;
	rot[15] = (q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
	glMultMatrixd(rot);
}