% script file to generate a figure showing all the trajectories that
%   make up a single slice of the left barrier
%
%   for the collision avoidance example
%   (so swap x and y, scale by 5)
%
% not currently set up to handle bcase(1) == 2
%     (ie single trajectory case for theta near zero)
%
% Ian Mitchell, 7/17/02

slices = [ -150; ] * pi / 180;
beta = 1;
scale = [ 5, 5, 180 / pi ];
aspect = scale .* [ 1 1 2 ];
dtIntraTraj = 0.05;
dtInterTraj = 0.1;
dpsi = dtInterTraj;

%figure

for i = 1 : length(slices)

  theta = slices(i);

  %subplot(1,length(slices),i);
  hold on;

  % plot the barrier (for comparison purposes)
  [ xp xm xc sp sm ] = barrier(theta, beta);
  xp = xp(:, [ 2 1 3]) .* repmat(scale, size(xp, 1), 1);
  xm = xm(:, [ 2 1 3]) .* repmat(scale, size(xm, 1), 1);
  xc = xc(:, [ 2 1 3]) .* repmat(scale, size(xc, 1), 1);

  % complete the capture circle
  xc = [ xc; -xc(:, 1:2), xc(:,3) ];

  lhTemp = plot3(xp(:,1), xp(:,2), xp(:,3) + 360, 'm:', ...
                 xm(:,1), xm(:,2), xm(:,3) + 360, 'm:', ...
                 xc(:,1), xc(:,2), xc(:,3) + 360, 'm:');
  lh(1) = lhTemp(1);
  hold on;

  % put in an additional capture circle at theta == 0
  plot3(xc(:,1), xc(:,2), zeros(size(xc(:,3))) + 360, 'm:');


  % get the crossover stats
  [ bcase, p, pos ] = crossover(theta, beta);

  % left barrier
  sigmaE = -1;

  % the curved portion of the positive barrier
  ts = [ p.tau1m : -dtInterTraj : 0, 0 ]';
  for i = 1:length(ts)
    t1 = ts(i);
    t2 = 0.5 * (t1 - theta);
    z0 = [ 0; -beta; 0 ];
    [ z2, z1, z12, z01 ] = optTraj2(3, 1, sigmaE,sigmaE,t1,t2,z0,dtIntraTraj);
    z = [ z01; z12 ];
    z = z(:, [ 2 1 3]) .* repmat(scale, size(z, 1), 1);
    plot3(z(:,1), z(:,2), z(:,3) + 360, 'b-');
  end

  % the straight portion of the positive barrier
  psis = [ -dpsi : -dpsi : theta / 2, theta / 2 ];
  for i = 1 : length(psis)
    psi = psis(i);
    t2 = 0.5 * (- theta + 2 * psi);
    z0 = [ -beta * sin(psi); -beta * cos(psi); 2 * psi ];
    z = optTraj(1, sigmaE, z0, [ 0:dtIntraTraj:t2, t2 ]');
    z = z(:, [ 2 1 3]) .* repmat(scale, size(z, 1), 1);
    plot3(z(:,1), z(:,2), z(:,3) + 360, 'b-');
  end

end

for i = 1 : length(slices)
  %subplot(1, length(slices), i);

  xlabel('x_r');
  ylabel('y_r');
  zlabel('\psi_r');
  title([ 'Slice at \psi_r = ' num2str(slices(i) * 180 / pi + 360) '^\circ' ]);
  axis([ -6 20 -6 8 200 360 ]);
  daspect(aspect);
  grid on;
end
