% R_VIEWOBJ	View the square box trajectory when pushing.

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

close all;
clear all;

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

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

figHistory=figure(...
            'Color',[0 0 0],...
            'Position',[100 0 600 600],...
            'Resize','on',...
            'NumberTitle','off',...
            'Name','Distribution Show');

  Point_X=0;
  Point_Y=0;
  View_Width=100;

  End_X=Point_X+View_Width;
  End_Y=Point_Y+View_Width;

BeginPoint=1;
EndPoint=158;

OHistory=ObjectHistory(BeginPoint:EndPoint,:);
GHistory=GoalHistory(BeginPoint:EndPoint,:);

clf;                     

plot(Point_X,Point_Y,'color',[1 1 1]);
axis([Point_X,End_X,Point_Y,End_Y]);
axis on;                 
hold on;                 

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,'k-');

    [FirstArray,SecondArray,WhichPoint]=r_plotsquare(OHistory(SplineLength,:),ObjectSize);
    line(FirstArray,SecondArray);
    line([OHistory(SplineLength,1) WhichPoint(1)],[OHistory(SplineLength,2) WhichPoint(2)]);
 
plot(GHistory(1,1), GHistory(1,2), 'Marker','^');
%plot(39,26, 'Marker','d');
plot(GHistory(SplineLength,1), GHistory(SplineLength,2), 'Marker','+');
title('Object-Pushing Trajectory');
drawnow;

gtext('t=158');
%gtext('t=90');
gtext('t=83');
gtext('t=1');
print d:\maxplus2\rect\result\ps\r1obj -dps;