CS计算机代考程序代写 algorithm classdef OptPlanner < handle

classdef OptPlanner < handle % Reference: properties (SetAccess=public, GetAccess=public) costmap % world cost map: obstacle = Inf G % index of goal point (x, y, theta) S % index of start point (x, y, theta) quiet = false; nstep = 20; refpath newpath iterpath poly % Obtacles goal start time resolution = 1; cost_profile end methods %%%%%%%%%%%%%%%%%%%% constructor %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function obj = OptPlanner(world, goal, start,refpath) obj.goal = goal; obj.start = start; obj.costmap = world; obj.refpath = obj.resample(refpath); obj.poly = obj.map2poly(world); end %%%%%%%%%%%%%%%%%%% geometry %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% function poly = map2poly(obj,map) poly = {}; % poly{1} = [4 6; 12 6; 12 92; 4 92]; % poly{2} = [12 77; 12 69; 37 69; 37 77]; % poly{3} = [12 14; 12 21; 58 21; 58 14]; % poly{4} = [21 57; 58 57; 58 45; 21 45]; % poly{5} = [58 6; 58 92; 65 92; 78 57; 80 21; 80 14; 65 6]; tic mapsize = size(map); npoly = 0; for i=1:mapsize(2) for j=1:mapsize(1) if map(j,i) > 0
flag = 1;
for k = 1:size(poly,2)
[~,~,d] = obj.d2poly([i,j],poly{k});
if d <= 0 flag = 0; end end if flag == 0 continue end npoly = npoly + 1; poly{npoly} = [i,j]; row = j; col = i; while row < mapsize(1) && map(row+1,col) > 0
row = row + 1;
end
poly{npoly} = [poly{npoly};[col,row]];
while map(row,col+1) > 0
col = col + 1;
end
poly{npoly} = [poly{npoly};[col,row];[col,j]];

end
end
end
for i = 1:npoly
poly{i} = poly{i}*obj.resolution;
end
toc
end

function [c,ceq] = distance(obj,path,poly,margin)
c = [];
dim = 2;
nstep = length(path)/2;
npoly = size(poly,2);
nside = size(poly{1},1);
for j =1:nstep
point = path((j-1)*dim+1:j*dim);
for k =1:npoly
d = Inf;
for i=1:nside
x1 = poly{k}(i,1); y1 = poly{k}(i,2);
x2 = poly{k}(mod(i,nside)+1,1); y2 = poly{k}(mod(i,nside)+1,2);

trid = [];
trid(1) = norm([x1-x2,y1-y2]);
trid(2) = norm([x1-point(1),y1-point(2)]);
trid(3) = norm([x2-point(1),y2-point(2)]);

Lr = [y1-y2,x2-x1];
Sr = -x1*y2+x2*y1;
vd = abs(Lr(1)*point(1)+Lr(2)*point(2)-Sr)/trid(1);

if trid(2)^2>trid(1)^2+trid(3)^2
vd = trid(3);
end
if trid(3)^2>trid(1)^2+trid(2)^2
vd = trid(2);
end

if vd < d d = vd; end end if d ~= 0 area = 0; polyarea = 0; for i=1:nside area = area + obj.triArea(point',poly{k}(i,:),poly{k}(mod(i,nside)+1,:)); end for i=2:nside-1 polyarea = polyarea + obj.triArea(poly{k}(1,:),poly{k}(i,:),poly{k}(mod(i,nside)+1,:)); end if norm(polyarea-area) < 0.01 d = -d; end end c = [c;-d+margin]; end end ceq = []; end function [L,S,d] = d2poly(obj,point,poly) d = Inf; nside = size(poly,1); for i=1:nside x1 = poly(i,1); y1 = poly(i,2); x2 = poly(mod(i,nside)+1,1); y2 = poly(mod(i,nside)+1,2); trid = []; trid(1) = norm([x1-x2,y1-y2]); trid(2) = norm([x1-point(1),y1-point(2)]); trid(3) = norm([x2-point(1),y2-point(2)]); Lr = [y1-y2,x2-x1]; Sr = -x1*y2+x2*y1; vd = abs(Lr(1)*point(1)+Lr(2)*point(2)-Sr)/trid(1); if trid(2)^2>trid(1)^2+trid(3)^2
vd = trid(3);
Lr = [point(1)-x2,point(2)-y2];
Sr = [point(1)-x2,point(2)-y2]*[x2,y2]’;
end
if trid(3)^2>trid(1)^2+trid(2)^2
vd = trid(2);
Lr = [point(1)-x1,point(2)-y1];
Sr = [point(1)-x1,point(2)-y1]*[x1,y1]’;
end

if vd < d d = vd; L = Lr; S = Sr; ii = i; end end nL = norm(L); L = L./nL; S = S/nL; if L*poly(mod(ii+1,nside)+1,:)'2 && refpath(i,4) ~= 1
break;
end
pathlength = pathlength + norm(refpath(i,1:2)-refpath(i-1,1:2));
end
unitlength = pathlength / (obj.nstep-1);
fpoint = i;