takerole.m

多智能体工具包

function [TakeRole,DeltaAngle,WidthAngle]=takerole(RobotPos,RobotDir)
% TAKEROLE	Test whether the robot acts  on the Object.

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

global RegionNum DirectionNum AxisDirection ObjectPosition;

TakeRole=0;

ObjectAngle=ObjectPosition(3);
InSubAngle=ObjectAngle-AxisDirection;
while InSubAngle<0
  InSubAngle=InSubAngle+pi/2;
end;

ForceAngle=RobotDir*2*pi/DirectionNum;
RobotInSub=RobotPos*2*pi/RegionNum;

FirstAngle=InSubAngle-pi/2;
SecondAngle=InSubAngle;
while ~(RobotInSub>=FirstAngle & RobotInSub<SecondAngle)
  FirstAngle=FirstAngle+pi/2;
  SecondAngle=SecondAngle+pi/2;
end;

if RobotInSub==FirstAngle
  CompareAngle=FirstAngle+pi*3/4;
  DeltaAngle=ForceAngle-CompareAngle;

  if (cos(DeltaAngle)>0.000001 & sin(DeltaAngle)>0.000001)
    TakeRole=1;
  end;  
else
  CompareAngle=FirstAngle-pi*3/4;
  DeltaAngle=CompareAngle-ForceAngle;

  if cos(DeltaAngle)>0.000001
    TakeRole=1;
  end;
end;

InverRobotPos=RobotPos+RegionNum/2;
if InverRobotPos>=RegionNum
  InverRobotPos=InverRobotPos-RegionNum;
end;

DeltaAngle=(RobotDir/DirectionNum-InverRobotPos/RegionNum)*2*pi;
WidthAngle=RobotInSub-FirstAngle+pi/4;