1 Star 1 Fork 1

A.Star/pathPlanning4m

加入 Gitee
与超过 1200万 开发者一起发现、参与优秀开源项目,私有仓库也完全免费 :)
免费加入
该仓库未声明开源许可证文件(LICENSE),使用请关注具体项目描述及其代码上游依赖。
克隆/下载
adjust.m 818 Bytes
一键复制 编辑 原始数据 按行查看 历史
unknown 提交于 2017-05-30 14:37 . 新建避障路径规划算法代码
function newPoint = adjust(p,poly)
if poly(1,:) ~= poly(end,:)
poly = [poly;poly(1,:)];
end
polyn = size(poly,1);
coefficient=[];
for j=2:polyn
coefficient=[coefficient;getABC(poly(j-1,:),poly(j,:))];
d(j-1)=abs((coefficient(j-1,1)*p(1)+coefficient(j-1,2)*p(2)+coefficient(j-1,3))/sqrt(...
coefficient(j-1,1)*coefficient(j-1,1)+coefficient(j-1,2)*coefficient(j-1,2)));
end
[~,index]=min(d);
lineone=coefficient(index,:);
if lineone(1)==0
newPoint=[p(1),-lineone(3)/lineone(2)];
elseif lineone(2)==0
newPoint=[-lineone(3)/lineone(1),p(2)];
else
linetwo=[lineone(2)/lineone(1),-1,p(2)-lineone(2)*p(1)/lineone(1)];
[x,y]=jiaodian(lineone,linetwo);
newPoint=[x,y];
end
end
马建仓 AI 助手
尝试更多
代码解读
代码找茬
代码优化
Matlab
1
https://gitee.com/a_star/pathPlanning4m.git
git@gitee.com:a_star/pathPlanning4m.git
a_star
pathPlanning4m
pathPlanning4m
master

搜索帮助