force_calcu.m

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

function [Sum_Force,Force_Direction,Force_Vector]=force_calcu(Robot_Config)
% FORCE_CALCU	Calculate the sum force of the Object.

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

global Class_Change_Dist Red_White_Distance Total_Cell_Num;

Sum_Cos=0;
Sum_Sin=0;
Sum_Force=0;

Object_Config=get_object_config(3,1);

for mm=1:Total_Cell_Num
  RO_Diff=Robot_Config(mm,:)-Object_Config;
  This_Distance=sqrt(RO_Diff(1)^2+RO_Diff(2)^2);
  if This_Distance<=Class_Change_Dist
    Force_Mod=(Class_Change_Dist-This_Distance)^2/(Class_Change_Dist-...
             Red_White_Distance)^2;
    Sum_Cos=Sum_Cos+Force_Mod*RO_Diff(1)/This_Distance;
    Sum_Sin=Sum_Sin+Force_Mod*RO_Diff(2)/This_Distance;
  end
end

Sum_Force=sqrt(Sum_Cos^2+Sum_Sin^2);

Force_Direction=atan2(-Sum_Sin,-Sum_Cos);

if Force_Direction<0
  Force_Direction=2*pi+Force_Direction;
end

Force_Vector=[-Sum_Cos -Sum_Sin];