goal_object_pos.m

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

function [dist,goal_object_angle]=goal_object_pos(Object_Config)
% GOAL_OBJECT_POS	Calculate the angle from the object and the goal.

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

global Current_Goal;

diff_Config=Current_Goal-Object_Config;

dist=sqrt(diff_Config(1)^2+diff_Config(2)^2);

goal_object_angle=atan2(diff_Config(2),diff_Config(1));

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