function D = fcn_DH(node,Map_Obs,PosMap,Obs_mat)
n = length(node);
D = coder.nullcopy(zeros(n,n));
len=(1:n)';
pos_mat=PosMap(node(len),:);
coder.gpu.kernel
for i=1:n-1
for j=i+1:n
dist=sqrt((pos_mat(i,1)-pos_mat(j,1))^2+(pos_mat(i,2)-pos_mat(j,2))^2+(pos_mat(i,3)-pos_mat(j,3))^2);
if dist<=25
B = collisionCheck_SP4(PosMap(node(i),:), PosMap(node(j),:), Map_Obs, Obs_mat);
D(i,j)=dist*B;
end
end
end
D=D+D';
function B = collisionCheck_SP4(p1, p2,Map_Obs,Obs_mat)
P1=[round(p1(1)/0.3+1) round(p1(2)/0.3+1) round(p1(3)/0.3+1)];
P2=[round(p2(1)/0.3+1) round(p2(2)/0.3+1) round(p2(3)/0.3+1)];
chk_p=abs(P1-P2);
if (chk_p(1)>=chk_p(2)) && (chk_p(1)>=chk_p(3))
if P1(1)<P2(1)
X=[P1(1):P2(1)]';
else
X=[P2(1):P1(1)]';
end
Y=round(X.*(P2(2)-P1(2))/(P2(1)-P1(1))-(P2(2)-P1(2))/(P2(1)-P1(1))*P1(1)+P1(2));
Z=round(X.*(P2(3)-P1(3))/(P2(1)-P1(1))-(P2(3)-P1(3))/(P2(1)-P1(1))*P1(1)+P1(3));
elseif (chk_p(2)>chk_p(1)) && (chk_p(2)>=chk_p(3))
if P1(2)<P2(2)
Y=[P1(2):P2(2)]';
else
Y=[P2(2):P1(2)]';
end
X=round(Y.*(P2(1)-P1(1))/(P2(2)-P1(2))-(P2(1)-P1(1))/(P2(2)-P1(2))*P1(2)+P1(1));
Z=round(Y.*(P2(3)-P1(3))/(P2(2)-P1(2))-(P2(3)-P1(3))/(P2(2)-P1(2))*P1(2)+P1(3));
else
if P1(3)<P2(3)
Z=[P1(3):P2(3)]';
else
Z=[P2(3):P1(3)]';
end
X=round(Z.*(P2(1)-P1(1))/(P2(3)-P1(3))-(P2(1)-P1(1))/(P2(3)-P1(3))*P1(3)+P1(1));
Y=round(Z.*(P2(2)-P1(2))/(P2(3)-P1(3))-(P2(2)-P1(2))/(P2(3)-P1(3))*P1(3)+P1(2));
end
chk_obs=Map_Obs(Idxpt);
chk_col=sum(chk_obs,2);
if chk_col==size(chk_obs,1)
B=1;
else
B=0;
end
