% R_ALLOBJ	Draw object movement process for square box pushing.

%     Copyright (c) 1998-2000 Jiming Liu and Jianbing Wu

close all;
clear all;

%FLname='d:\maxplus2\rect\result\rect01';
FLname='d:\maxplus2\rect\result\rect02';

eval(['load ' FLname ';']);

BeginPoint=1;
EndPoint=152;

OHistory=ObjectHistory(BeginPoint:EndPoint,:);

  clf;
  plot(0,1,'color',[1 1 1]);
  axis([10,90,15,95]);
  hold on;
  plot(70,90, 'Marker','^');
  plot(39,26, 'Marker','d');
  plot(77,83, 'Marker','+');

SplineLength=EndPoint-BeginPoint+1;;

    X_Vector=[];
    Y_Vector=[];
    for nn=1:SplineLength
      Last_X=OHistory(nn,1);
      Last_Y=OHistory(nn,2);
      X_Vector=[X_Vector Last_X];
      Y_Vector=[Y_Vector Last_Y];
    end    
    t=1:SplineLength;
    ts=1:0.1:SplineLength;
    xs=spline(t,X_Vector,ts);
    ys=spline(t,Y_Vector,ts);
    plot(xs,ys,':');


StepLib=[1 2 5 9 10 11 12 13 14 15 16 17 20 24 29 31 34 43 54 66 76 83 ...
         85 86 87 88 89 92 95 98 101 105 112 119 126 134 144 152];
%         85 86 87 88 89 92 95 99 104 107 114 123 131 139 149 158];

[Novalue,SizeNum]=size(StepLib);
for mm=1:SizeNum

%  Step=(mm-1)*6+1;
  Step=StepLib(mm);

   [FirstArray,SecondArray,WhichPoint]=r_plotsquare(OHistory(Step,:),ObjectSize);
      line(FirstArray,SecondArray);
      line([OHistory(Step,1) WhichPoint(1)],[OHistory(Step,2) WhichPoint(2)]);

  
end;
  drawnow;
  title('Object Motion Process');
  SaveString='d:\maxplus2\rect\result\ps\r2all';
  eval(['print ' SaveString ' -dps;']);