load  path.mat
% load  tpath.mat
% 轨迹处理
% 定义参考轨迹
refPos_x = path(:,1);
refPos_y = path(:,2);
refPos = [refPos_x, refPos_y];

% 计算航向角和曲率
diff_x = diff(refPos_x) ;
diff_x(end+1) = diff_x(end);
diff_y = diff(refPos_y) ;
% plot(diff_y)
diff_y(end+1) = diff_y(end);
derivative1 = gradient(refPos_y,refPos_x);              % y对于x的一阶导数
derivative2 = 4*del2(refPos_y,refPos_x);                  % y对于x的二阶导数
refHeading = atan2(diff_y , diff_x);                   % 航向角
refK = abs(derivative2) ./ (1+derivative1.^2).^(3/2);  % 计算曲率
refHeading(201) = 0;
% refHeading(360) = 90.8584;
trajectory = zeros(length(path),3);
for i = 1:length(trajectory)
    trajectory(i,:) = [refPos_x(i),refPos_y(i),refHeading(i)]; 
end
lv = 11.5;
% len = sum(sqrt(diff(trajectory(:,1)*0.3).^2 + diff(trajectory(:,2)*0.3).^2));
% ts = 0.1;
% tf = ts*200;
% tf = ts*120;
% v = len/tf;
% 
% plot(trajectory(:,1)*0.3,trajectory(:,2)*0.3);

% cnt = length(trajectory);

% tf  = len/lv;
% ts = 0.1;
% t = 0:ts:tf;
% len = length(t);
% span = floor(cnt/len);
% idx = 1:span:cnt;
% tra = trajectory(idx,:);
% plot(tra(:,1),tra(:,2),"*");
% hold on

% pos = [0.8,-0.54]+0.3;
% dt = 0.01;
% T = 0.6;
% t = 0:dt:T;
% % 轨迹点(waypoint)的位置和速度[ px py vx vy]
% trajectory = zeros(length(t),3);
% lv = 10; % m/s
% yaw = -45;
% 
% dist = zeros(length(t),1);
% 
% for i = 1:length(t)
% %     yaw = -45; % -45° 
%     yaw = 360 * i/length(t);
%     v = [sind(yaw),cosd(yaw)]*lv;
%     movement = v*dt;
%     pos = pos + movement;
%     dist(i) = norm(pos);
% %     trajectory(i,:) = [pos,movement];
%     trajectory(i,:) = [pos,yaw];
% end 
% % % 
% subplot(1,2,1)
% plot(trajectory(:,1),trajectory(:,2),"*")
% plot(rad2deg(trajectory(:,3)));
% plot(diff_y)
% % subplot(1,2,2)
% % plot(dist);
% % step = norm(trajectory(1,:)-trajectory(2,:))