view_spline.m

来自「多智能体工具包」· M 代码 · 共 116 行

% VIEW_SPLINE	Draw the splint trajetory.

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

close all;
clear all;

Age=33;    % 62, 64/95, 33, 101
Before=32; % 61, 63/25, 32, 100

%FLname='f1_2';
%FLname='f1_2f2_2f6_4';
FLname='f2_1';
%FLname='f6_2';

Max_Age=101;           
Loop_Inc=5;
Robot_Num=3;

eval(['load ' FLname ';']);    % load the DATA

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

  Point_X=0;
  Point_Y=0;
  View_Width=10;

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


clg;                     

plot(Point_X,Point_Y,'color',[1 1 1]);
%plot(2,4,'color',[1 1 1]);
axis([Point_X,End_X,Point_Y,End_Y]);
%axis([2,7,4,9]);
axis on;                 
hold on;                 

plot(New_History((Age-1)*Loop_Inc+1,4),New_History((Age-1)*Loop_Inc+1,5),...
        'LineStyle','+','Color','g',...
        'markersize',100/View_Width);

  for mm=1:(Robot_Num+1)
    LocationX=New_History((Age-1)*Loop_Inc+mm+1,4);
    LocationY=New_History((Age-1)*Loop_Inc+mm+1,5);

    if New_History((Age-1)*Loop_Inc+mm+1,1)==1 
      Line_Style='*';
      Cell_Color='r';
      Mark_Size=75/View_Width;
    elseif New_History((Age-1)*Loop_Inc+mm+1,1)==3
      Line_Style='.';
      Cell_Color='w';
      Mark_Size=200/View_Width;
    end 
    plot(LocationX,LocationY,'LineStyle',Line_Style,'Color',Cell_Color,...
         'markersize',Mark_Size);

    X_Vector=[];
    Y_Vector=[];
    for nn=Before:-1:1
      Last_X=New_History((Age-nn-1)*Loop_Inc+mm+1,4);
      Last_Y=New_History((Age-nn-1)*Loop_Inc+mm+1,5);
      X_Vector=[X_Vector Last_X];
      Y_Vector=[Y_Vector Last_Y];
    end    
    X_Vector=[X_Vector LocationX];
    Y_Vector=[Y_Vector LocationY];  
    t=1:Before+1;
    ts=1:0.1:Before+1;
    xs=spline(t,X_Vector,ts);
    ys=spline(t,Y_Vector,ts);
    if mm==1
      plot(xs,ys,'y:');
    elseif mm==2
      plot(xs,ys,'r--'); 
    elseif mm==3
      plot(xs,ys,'m-.');
    elseif mm==4
      plot(xs,ys,'c-');
    end
  end

%Age_Matr=[30 60];  % 36, 40 ,20, 30/60
%for nn=1:2
Now_Age=20;
%  Now_Age=Age_Matr(nn);
  for mm=1:Robot_Num
    LocationX=New_History((Now_Age-1)*Loop_Inc+mm+1,4);
    LocationY=New_History((Now_Age-1)*Loop_Inc+mm+1,5);

    plot(LocationX,LocationY,'LineStyle','*','Color','r',...
         'markersize',75/View_Width);
  end
  LocationX=New_History(Now_Age*Loop_Inc,4);
  LocationY=New_History(Now_Age*Loop_Inc,5);
  plot(LocationX,LocationY,'LineStyle','.','Color','w',...
         'markersize',200/View_Width);
%end
%    LocationX=New_History(1,4);
%    LocationY=New_History(1,5);
%    plot(LocationX,LocationY,'LineStyle','o','Color','g',...
%         'markersize',120/View_Width);

%title('Motion Trajectory (Zoom View)');
title('Motion Trajectory');   

drawnow;