straightline.m

来自「粒子群(PSO)路径规划。这个是一个局部路径规划」· M 代码 · 共 12 行

%----求直线方程，知道两点得出直线的k和b值
function theta = straightLine(x1,y1,x2,y2)

a=x2-x1+j*(y2-y1);
angleA=angle(a);
if angleA >=0
    theta = angleA;
else
    theta = 2*pi+angleA;
end