% 本文参照文献:Flocking for Multi-Agent Dynamic Systems:Algorithms and Theory
clear;
close all;
clc;
%% Parameters 初始化参数
num_agents = 100;
t_gap=1; % 迭代间隔
queue_gap=15; % 队形间隔
queue_vy=12;
queue_vx=13;
queue_r=40;
r_c=20; % 交互范围(半径)
k=1.2; % 晶格的ratio
d=r_c/k; % 晶格的scale(表示两两智能体之间的距离(论文中公式5))
v_0=2; % 初始速度
v_limit=0; % 最大速度
efs = 1; % sigma-norms parameter
h=0.4; % 设置bump function的分割点(公式10)
d_o = r_c;
r_c_sigma = sigma_norm(r_c,efs); % r_c的σ范数
d_sigma = sigma_norm(d,efs); % d的σ范数
map_width = 400; % width of a squre map
map_res = 0.5; % width of a grid to play obstacles pixel
c1=0.2;c2=0.5;c3=0.2;c4=0.1;c5=10;c6=0.01;
x = zeros(num_agents,2); % current position
x_1 = zeros(num_agents,2); % previous position
v_1 = zeros(num_agents,2); % previous velocity
x_r0= zeros(num_agents,2); % x_r0:用来存储指定的队形信息
v_r_1st_point=300;
path_num = zeros(1000,2,num_agents);
v_r=[1,0];