move_object.m

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

function [Delta_Move,New_Object_Config]=move_object(Robot_Config)
% MOVE_OBJECT	Move the Object according to a certain environment.

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

global Step_Ratio Object_Distrib Last_MoveDirection;

Object_Config=get_object_config(3,1);

Delat_X=0;
Delat_Y=0;

  [Sum_Force,Force_Direction,Force_Vector]=force_calcu(Robot_Config);

  Last_MoveDirection=Force_Direction;  

  if Sum_Force               
    Step=Sum_Force*Step_Ratio;
    Delat_X=Step*cos(Force_Direction);
    Delat_Y=Step*sin(Force_Direction);                        

    New_Object_Config(1)=Object_Config(1)+Delat_X;
    New_Object_Config(2)=Object_Config(2)+Delat_Y;

    if New_Object_Config(1)>Object_Distrib(2)
      New_Object_Config(1)=Object_Distrib(2); 
      Delat_X=New_Object_Config(1)-Object_Config(1);
    elseif New_Object_Config(1)<Object_Distrib(1)
      New_Object_Config(1)=Object_Distrib(1);
      Delat_X=New_Object_Config(1)-Object_Config(1);
    end
    if New_Object_Config(2)>Object_Distrib(2)
      New_Object_Config(2)=Object_Distrib(2);
      Delat_Y=New_Object_Config(2)-Object_Config(2);
    elseif New_Object_Config(2)<Object_Distrib(1)
      New_Object_Config(2)=Object_Distrib(1);
      Delat_Y=New_Object_Config(2)-Object_Config(2);
    end
  else
    New_Object_Config=Object_Config;
  end

Delta_Move=[Delat_X Delat_Y];