%**************************************************************************
% Project   : Autonomous Helicopter
% Group     : 05gr835 
% Created   : 2005-04-28
% Edited    : 2005-05-25
% -------------------------------------------------------------------------
% The non-linear model contains the following files:
% 
% thrusten.m        : Main rotor thrust equations
% rigid.m           : Rigid body equations
% parameters.m      : Helicopter parameters
% mrflap.m          : Main rotor flapping equations
% force_torque.m    : Force and torque equations
% eigenaxis.m       : Eigen axis rotation for use in VR toolbox

%**************************************************************************

% eigenaxis.m       : This file contains the eigen axis rotation
%algorithm. This file is created by group 05gr830.

function y = eigenaxis(u)


if abs(u(1))< 0.0001
    u(1) = 0.0001;
end

if abs(u(2))< 0.0001
    u(2) = 0.0001;
end

if abs(u(3))< 0.0001
    u(3) = 0.0001;
end

u = [ -u(1); -u(2); u(3) ];% [Pitch, Yaw, Roll] 

C11 = cos(u(2))*cos(u(3));
C12 = cos(u(2))*sin(u(3));
C13 = -sin(u(2));
C21 = sin(u(1))*sin(u(2))*cos(u(3))-cos(u(1))*sin(u(3));
C22 = sin(u(1))*sin(u(2))*sin(u(3))+cos(u(1))*cos(u(3));
C23 = sin(u(1))*cos(u(2));
C31 = cos(u(1))*sin(u(2))*cos(u(3))+sin(u(1))*sin(u(3));
C32 = cos(u(1))*sin(u(2))*sin(u(3))-sin(u(1))*cos(u(3));
C33 = cos(u(1))*cos(u(2));
    
theta = acos(0.5*(C11+C22+C33-1));

e = [C23-C32; C31-C13; C12-C21]/(2*sin(theta));
    
y = [e; theta];