//***************************************************************************************** // Truevision - a 3d modeler for gnome and povray // // rotation.cc // // Vincent LE PRINCE // 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 #include #include // INCLURE MENTION LEGALE SGI //************************************************** // Constantes //************************************************** /* Some 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; }