calcuforce.m

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

function [Sum_Force,Force_Direction,Force_Vector]=calcuforce(...
               Robot_Config,Object_Config,Total_Cell_Num)
% CALCUFORCE	Calculate the sum force of the Object.

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

Class_Change_Dist=0.8;
Red_White_Distance=0.2; 

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

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);

Force_Vector=[-Sum_Cos -Sum_Sin];