kerncalc.m

多智能体工具包

function kerncalc(ThisChrom)
% KERNCALC	Calculate some parameters for the selected chromosome.

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

global RobotNum ObjectPosition GoalPosition AxisDirection;
global RobotHistory ObjectHistory GoalHistory ChromHistory;
global RegionNum DirectionNum StepRatio EdgeLimit WorkArea;
global ObjectSize RotateRatio LastMoveDierction;

ThisCase=getreal(ThisChrom);
ChromHistory=[ChromHistory;ThisCase];

ObjectCenter=ObjectPosition(1:2);
ObjectAngle=ObjectPosition(3);
ObjectAccumuAngle=ObjectPosition(4);

InSubAngle=ObjectAngle-AxisDirection;
while InSubAngle<0
  InSubAngle=InSubAngle+pi/2;
end;

if isempty(RobotHistory)
  for loops=1:RobotNum
    RobotInSub=ThisCase(loops)*2*pi/RegionNum;

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

    WidthAngle=RobotInSub-FirstAngle+pi/4;

    InverRobotPos=ThisCase(loops)+RegionNum/2;
    if InverRobotPos>=RegionNum
      InverRobotPos=InverRobotPos-RegionNum;
    end;
    DeltaAngle=(ThisCase(loops+RobotNum)/DirectionNum-InverRobotPos/RegionNum)*2*pi;
    CurrentRadius=ObjectSize/(2*sin(WidthAngle));

    ThisDirection=RobotInSub+AxisDirection;
    RobotX=ObjectCenter(1)+abs(CurrentRadius)*cos(ThisDirection);
    RobotY=ObjectCenter(2)+abs(CurrentRadius)*sin(ThisDirection);
    RobotHistory=[RobotHistory RobotX RobotY];
  end;
end;

% Join Force & Join Torque
XForce=0;
YForce=0;
JoinTorque=0;
for loop1=1:RobotNum
  [TakeRole,DeltaAngle,WidthAngle]=takerole(ThisCase(loop1),ThisCase(loop1+RobotNum));
  if TakeRole
    XForce=XForce+cos(ThisCase(loop1+RobotNum)*2*pi/DirectionNum);
    YForce=YForce+sin(ThisCase(loop1+RobotNum)*2*pi/DirectionNum);

    [ThisTorque,TheSign]=gettorqu(DeltaAngle,WidthAngle);
    JoinTorque=JoinTorque+ThisTorque*TheSign;
  end;
end;

RotateAngle=RotateRatio*JoinTorque;

JoinForce=sqrt(XForce*XForce+YForce*YForce);
JoinForceDir=atan2(YForce,XForce);

LastMoveDierction=JoinForceDir;

Step=JoinForce*StepRatio;
MoveDirection=AxisDirection+JoinForceDir;

ObjectX=ObjectPosition(1)+Step*cos(MoveDirection);
ObjectY=ObjectPosition(2)+Step*sin(MoveDirection);
if ObjectX<EdgeLimit
  ObjectX=EdgeLimit;
elseif ObjectX>(WorkArea(1)-EdgeLimit)
  ObjectX=WorkArea(1)-EdgeLimit;
end;
if ObjectY<EdgeLimit
  ObjectY=EdgeLimit;
elseif ObjectY>(WorkArea(2)-EdgeLimit)
  ObjectY=WorkArea(2)-EdgeLimit;
end;

ObjectCenter=[ObjectX ObjectY];
ObjectAngle=ObjectAngle+RotateAngle;
ObjectAccumuAngle=ObjectAccumuAngle+RotateAngle;

if ObjectAngle<0
  ObjectAngle=ObjectAngle+2*pi;
end;
while ~(ObjectAngle>=0 & ObjectAngle<pi/2)
  ObjectAngle=ObjectAngle-pi/2;
end;
ObjectPosition=[ObjectCenter ObjectAngle ObjectAccumuAngle];
ObjectHistory=[ObjectHistory;ObjectPosition];
GoalHistory=[GoalHistory;GoalPosition];

RobotPosition=[];
InSubAngle=ObjectAngle-AxisDirection;
while InSubAngle<0
  InSubAngle=InSubAngle+pi/2;
end;
for loops=1:RobotNum
  RobotInSub=ThisCase(loops)*2*pi/RegionNum+RotateAngle;
  if RobotInSub<0
    RobotInSub=RobotInSub+2*pi;
  end;

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

  WidthAngle=RobotInSub-FirstAngle+pi/4;

  InverRobotAng=(ThisCase(loops)+RegionNum/2)*2*pi/RegionNum+RotateAngle;
  DeltaAngle=ThisCase(loops+RobotNum)*2*pi/DirectionNum-InverRobotAng;
  CurrentRadius=ObjectSize/(2*sin(WidthAngle));

  ThisDirection=RobotInSub+AxisDirection;
  RobotX=ObjectCenter(1)+abs(CurrentRadius)*cos(ThisDirection);
  RobotY=ObjectCenter(2)+abs(CurrentRadius)*sin(ThisDirection);
  RobotPosition=[RobotPosition RobotX RobotY];

end;
RobotHistory=[RobotHistory;RobotPosition];