//*****************************************************************************************
// Truevision - a 3d modeler for gnome and povray
//
// rotation.cc
//
// Vincent LE PRINCE <vincentleprince@users.sourceforge.net>
// Copyright (C) 2000-2001 Vincent LE PRINCE
// This file is part of the TRUEVISION Package

//   This program is free software; you can redistribute it and/or modify
//   it under the terms of the GNU General Public License as published by
//   the Free Software Foundation; either version 2 of the License, or
//   (at your option) any later version.
//
//   This program 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 General Public License for more details.
//
//   You should have received a copy of the GNU General Public License
//   along with this program; if not, write to the Free Software
//   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.  */ 
//*******************************************************************************************
#include "include/rotation.h"
#include "include/tvio.h"
#include <string.h>
#include <math.h>
#include <float.h>
// INCLURE MENTION LEGALE SGI

//**************************************************
// Constantes
//**************************************************
 /* Some <math.h> files do not define M_PI... */
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

const double RADIANS = 180.0 / M_PI;
const int RENORMCOUNT = 97;
double zero_quat[4] = { 0, 0, 0, 1.0 };
GLfloat zero_matrix[4][4] = { {1,0,0,0}, {0,1,0,0}, {0,0,1,0}, {0,0,0,1} };
double zero_euler[3] = { 0, 0, 0 };



//**************************************************
// Reset
//**************************************************
void Rotation::reset()
{
memcpy( quatern, zero_quat, sizeof(double)*4 );
memcpy( rotmatrix, zero_matrix, sizeof(GLfloat)*16 );
memcpy( euler, zero_euler, sizeof(double)*3 );
renorm_count = 0;
euler_utd = true;
}


// Normalize
void Rotation::normalize()
{
double mag = 0;
for ( register int i = 0 ; i < 4 ; i++ ) mag += quatern[i]*quatern[i];
for ( register int i = 0 ; i < 4 ; i ++ ) quatern[i] /= mag;
}


// Add quaternion
void Rotation::add_quats( double q[4] )
{
double qt[4];
qt[0] = quatern[0]*q[3] + q[0]*quatern[3] + quatern[1]*q[2] - quatern[2]*q[1];
qt[1] = quatern[1]*q[3] + q[1]*quatern[3] + quatern[2]*q[0] - quatern[0]*q[2];
qt[2] = quatern[2]*q[3] + q[2]*quatern[3] + quatern[0]*q[1] - quatern[1]*q[0];
qt[3] = quatern[3]*q[3] - ( q[0]*quatern[0] +   q[1]*quatern[1] +  q[2]*quatern[2] );
memcpy( quatern, qt, sizeof(double)*4 );

if ( ++renorm_count > RENORMCOUNT )
	{
	renorm_count = 0;
	normalize();
	}
rebuild_matrix();
}


void Rotation::set_from_euler( double a, double b, double c )
{
a /= 2.0 * RADIANS;
b /= 2.0 * RADIANS;
c /= 2.0 * RADIANS;
double cosi[3] = { cos(a), cos(b), cos(c) };
double sinu[3] = { sin(a), sin(b), sin(c) };
 
quatern[3] = cosi[0]*cosi[1]*cosi[2] + sinu[0]*sinu[1]*sinu[2];
quatern[0] = sinu[0]*cosi[1]*cosi[2] -cosi[0]*sinu[1]*sinu[2];
quatern[1] = cosi[0]*sinu[1]*cosi[2] + sinu[0]*cosi[1]*sinu[2];
quatern[2] = cosi[0]*cosi[1]*sinu[2] - sinu[0]*sinu[1]*cosi[2];
rebuild_matrix();
euler_utd = true;
}

void Rotation::get_as_euler( double *angles )
{
if ( euler_utd == false ) 
	{
    double sqw = quatern[3]*quatern[3];
    double sqx = quatern[0]*quatern[0];
    double sqy = quatern[1]*quatern[1];
    double sqz = quatern[2]*quatern[2];
    euler[2] = atan2(2.0 * (quatern[0]*quatern[1] + quatern[2]*quatern[3]), (sqx - sqy - sqz + sqw));
    euler[0] = atan2(2.0 * (quatern[1]*quatern[2] + quatern[0]*quatern[3]), (-sqx - sqy + sqz + sqw));
    euler[1] = asin(-2.0 * (quatern[0]*quatern[2] - quatern[1]*quatern[3]));	
	for ( int i = 0 ; i < 3  ; i++ )
		{
		euler[i] *= RADIANS;
		if ( euler[i] < 0.0 ) euler[i] += 360.0;
		if ( euler[i] >= 360.0 ) euler[i] -= 360.0;		
		}
	euler_utd = true;
	}	
memcpy( angles, euler, sizeof(double)*3 );
}


// Rebuild matrix
void Rotation::rebuild_matrix()
{
rotmatrix[0][0] = (GLfloat)(1.0 - 2.0 * ( quatern[1]*quatern[1] + quatern[2]*quatern[2] ));
rotmatrix[0][1] = (GLfloat)(2.0 * ( quatern[0]*quatern[1] - quatern[2]*quatern[3] ));
rotmatrix[0][2] = (GLfloat)(2.0 * ( quatern[2]*quatern[0] + quatern[1]*quatern[3] ));

rotmatrix[1][0] = (GLfloat)(2.0 * ( quatern[0]*quatern[1] + quatern[2]*quatern[3] ));
rotmatrix[1][1] = (GLfloat)(1.0 - 2.0 * ( quatern[2]*quatern[2] + quatern[0]*quatern[0] ));
rotmatrix[1][2] = (GLfloat)(2.0 * ( quatern[1]*quatern[2] - quatern[0]*quatern[3] ));

rotmatrix[2][0] = (GLfloat)(2.0 * ( quatern[2]*quatern[0] - quatern[1]*quatern[3] ));
rotmatrix[2][1] = (GLfloat)(2.0 * ( quatern[1]*quatern[2] + quatern[0]*quatern[3] ));
rotmatrix[2][2] =(GLfloat)( 1.0 - 2.0 * ( quatern[1]*quatern[1] + quatern[0]*quatern[0] ));

euler_utd = false;
}



// Axis to quat
void Rotation::axis_to_quat( double a[3], double phi, double *q )
{
/*double len = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
phi /= 2.0;
for ( register int i = 0 ; i < 3 ; i++ ) 	q[i] = (a[i]/len) * sin(phi);
q[3] = cos(phi);*/

double sphi = sin( phi/2 );
double len = sphi / sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
for ( register int i = 0 ; i < 3 ; i++ ) q[i] = a[i] * len;
q[3] = cos( phi/2 );
}



// Projection on the trackball sphere
static double tb_project_to_sphere( double r, double x, double y )
{
double z;
double d = sqrt( x*x + y*y );
if ( d < r * 0.70710678118654752440 ) z = sqrt( r*r - d*d );
else
	{
	double t = r / 1.41421356237309504880;
	z = t*t / d; 
	}
return z;
}


// Trackball
void Rotation::trackball( double p1x, double p1y, double p2x, double p2y )
{
if ( p1x == p2x && p1y == p2y ) 	return;
//cout <<	"\n tkb < " <<  p1x << ", " <<  p1y << ", " << p2x << ", " <<  p2y << ">" ; cout.flush();

double p1[3] = { p1x, p1y, tb_project_to_sphere( trackball_size, p1x, p1y ) };
double p2[3] = { p2x, p2y, tb_project_to_sphere( trackball_size, p2x, p2y ) };
double axis[3] = { p2[1]*p1[2] - p2[2]*p1[1], p2[2]*p1[0] - p2[0]*p1[2], p2[0]*p1[1] - p2[1]*p1[0] };

double t = 0.0;
double temp;
for ( register int i = 0 ; i < 3 ; i++ )
	{
	temp = p1[i] - p2[i];
	t += temp*temp; 
	}
t = sqrt( t ) / 2.0 / trackball_size;

if ( t > 1.0 ) t = 1.0;
if ( t < -1.0 ) t = -1.0;
double phi = 2.0 * asin(t);

double q[4];
axis_to_quat( axis, phi, q );
add_quats( q );
}


void Rotation::add( double x, double y, double z, double angle )
{
double quat[4];
double axis[3] = { x, y, z };
axis_to_quat( axis, angle, quat );
add_quats( quat );
}

void Rotation::add( double *axis, double angle )
{
double quat[4];
axis_to_quat( axis, angle, quat );
add_quats( quat );
}


void Rotation::rotate_z( double *v )
{
v[0] = (double)rotmatrix[0][2];
v[1] = (double)rotmatrix[1][2];
v[2] = (double)rotmatrix[2][2];
}

void Rotation::rotate_vector( double *v, double x, double y, double z )
{
for ( register int i = 0 ; i < 3 ; i ++ )
	v[i] = (double)rotmatrix[i][0]*x + (double)rotmatrix[i][1]*y +(double)rotmatrix[i][2]*z; 
}

void Rotation::rotate_vector( float *v, double x, double y, double z )
{
for ( register int i = 0 ; i < 3 ; i ++ )
	v[i] = (float)(rotmatrix[i][0]*x + rotmatrix[i][1]*y + rotmatrix[i][2]*z); 
}

void Rotation::rotate_vector_inverse( float *v, double x, double y, double z )
{
for ( register int i = 0 ; i < 3 ; i ++ )
	v[i] = (float)(rotmatrix[0][i]*x + rotmatrix[1][i]*y + rotmatrix[2][i]*z); 
}

void Rotation::save( ofstream & file )
{
file << "{ ";
for ( int i = 0 ; i < 4 ; i ++ )
	file << 'Q' << i << '=' << quatern[i] << ' ';
file << "}";
}


void Rotation::load( ifstream & file )
{
char * val = NULL;
do
	{
	val = tvio_get_next_val( file );
	if ( val == NULL ) break;

	if ( ! strcmp( val, "Q0" ) ) { quatern[0] = tvio_get_value_as_float( file ); continue; }
	if ( ! strcmp( val, "Q1" ) ) { quatern[1] = tvio_get_value_as_float( file ); continue; }
	if ( ! strcmp( val, "Q2" ) ) { quatern[2] = tvio_get_value_as_float( file ); continue; }
	if ( ! strcmp( val, "Q3" ) ) { quatern[3] = tvio_get_value_as_float( file ); continue; }	
	} while ( val != NULL );
rebuild_matrix();	
return;
}
