straightline.m

用深度优先算法做的一个路径规划。可以从开始点一直移动到目标点

%----求直线方程，知道两点得出直线的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