r_viewspli.m

多智能体工具包

% R_VIEWSPLI	View the movement trajectory of robots and square box.

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

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=84;
EndPoint=152;

ROHistory=[RobotHistory(BeginPoint:EndPoint,:) ObjectHistory(BeginPoint:EndPoint,1:2)];
OHistory=ObjectHistory(BeginPoint:EndPoint,:);
%ChHistory=ChromHistory(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;;

  for mm=1:4
    LocationX=ROHistory(SplineLength,(mm-1)*2+1);
    LocationY=ROHistory(SplineLength,mm*2);

    X_Vector=[];
    Y_Vector=[];
    for nn=1:SplineLength
      Last_X=ROHistory(nn,(mm-1)*2+1);
      Last_Y=ROHistory(nn,mm*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);
    if mm==1
      plot(xs,ys,'b:');
    elseif mm==2
      plot(xs,ys,'r--'); 
    elseif mm==3
      plot(xs,ys,'g-.');
    elseif mm==4
      plot(xs,ys,'k-');
    end

    if mm==4
      [FirstArray,SecondArray,WhichPoint]=plotsquare(OHistory(SplineLength,:),ObjectSize);
      line(FirstArray,SecondArray);
      line([OHistory(SplineLength,1) WhichPoint(1)],[OHistory(SplineLength,2) WhichPoint(2)]);
    else
      ObjectPosition=OHistory(SplineLength,1:2);
      CurrentRobot=ROHistory(SplineLength,(mm-1)*2+1:2*mm);
      [FirstArray,SecondArray,Fill1Array,Fill2Array]=plotrob(ObjectPosition,CurrentRobot,RobotSize,ObjectSize);
      line(FirstArray,SecondArray);
      fill(Fill1Array,Fill2Array,'k');
    end;

  end
plot(70,90, 'Marker','^');
plot(39,26, 'Marker','d');
plot(GHistory(SplineLength,1), GHistory(SplineLength,2), 'Marker','+');
drawnow;
title('Motion Trajectory');


gtext('t=152');
gtext('t=90');
gtext('t=84');

print d:\maxplus2\rect\result\ps\r284_152 -dps;