%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solution to Problem 7 of
%  Achieving a Common Viewpoint: Yaw, Pitch, and Roll
%  Dianne P. O'Leary and David A. Schug
%  Computing in Science and Engineering
%
%  In this problem, we illustrate two facts:
%
%    1. If the given points are co-planar, then
%       the transformation Q is not uniquely defined.
%
%    2. If the pitch is near vertical, the Euler
%       angles cannot be determined in a stable manner,
%       even though Q is stable.  This is called
%       "gymbal lock".
%
%  problem7.m   06/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

anglsav = [];
qerror = [];
aerror = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1.  Illustration of a degenerate case; the data points
%     lie in a plane.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Here is the given data.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

A = [1 2 3 4 5 6 7;
     1 2 3 4 5 6 7;
     0 0 0 0 0 0 0];
Qtrue = compute_Q_from_angles([pi/3,pi/6,5*pi/6]);
B = Qtrue * A

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Here we compute a solution to the problem.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  [rmsd,angl,t,Q,newB] = viewpoint(A,B,0);

  qerror = [qerror,norm(Qtrue-Q,'fro')];
  aerror = [aerror,rmsd];

disp('Illustration of degenerate case in which the')
disp('matrix Q is not uniquely defined.')
disp('    Original Q                    Computed Q')
disp([Qtrue,Q])
disp('Original angles (radians)')
disp([pi/3,pi/6,5*pi/6])
disp('Computed angles')
disp(angl')
disp('RMSD using computed angles')
disp(rmsd)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2.  Illustration of gymbal lock.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  angl_orig = [pi/4;pi/2-.001;pi/9];
  [A,B,Q_orig] = makedata(angl_orig);
  [rmsd,angl,t,Q,newB] = viewpoint(A,B,0);

  angltrue_p = [pi/4;pi/2+.001;pi/9];
  [A,B,Qtrue_p] = makedata(angltrue_p);
  [rmsd_p,angl_p,t_p,Q_p,newB_p] = viewpoint(A,B,0);
  QQ_p = compute_Q_from_angles(angl_p);

disp('Illustration of gymbal lock, in which the')
disp('matrix Q is uniquely defined but the angles are')
disp('not stable with respect to perturbations.')
disp('Case 1: theta just less than pi/2')
disp('    Original Q                    Computed Q')
disp([Q_orig,Q])
disp(sprintf('Change in Q = %e',norm(Q_orig-Q,'fro')))
disp('Original angles (radians)')
disp(angl_orig')
disp('Computed angles')
disp(angl')
disp('Case 2: theta just greater than pi/2')
disp('    Original Q                    Computed Q')
disp([Qtrue_p,Q_p])
disp(sprintf('Change in Q = %e',norm(Qtrue_p-Q_p,'fro')))
disp('Original angles (radians)')
disp(angltrue_p')
disp('Computed angles')
disp(angl_p')
disp('Note the large change in the angles')
disp('  between Case 1 and Case 2, despite the small')
disp('  change in theta and in Q.')