% C_VIEWOBJ	Draw object trajectory when pushing round box.

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

close all;
clear all;

FLname='d:\maxplus2\circ\result\circ02';

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=131;

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;;

    LocationX=OHistory(SplineLength,1);
    LocationY=OHistory(SplineLength,2);

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

    plot(LocationX,LocationY,'Marker','o','Color','k','markersize',14);

plot(GHistory(1,1), GHistory(1,2), 'Marker','^');
plot(25,57, 'Marker','d');
plot(GHistory(SplineLength,1), GHistory(SplineLength,2), 'Marker','+');
title('Object-Pushing Trajectory');
drawnow;

gtext('t=131');
gtext('t=80');
gtext('t=67');
gtext('t=1');
print d:\maxplus2\circ\result\ps\c2obj -dps;