You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
26 lines
612 B
26 lines
612 B
function [p1,m1] = find_next_switch_pos(start_pos,X,Y,Z,switch_offset)
|
|
D = [];
|
|
for i = [1:length(X)]
|
|
dist = euklid_dist(start_pos,[X(i),Y(i),Z(i)]);
|
|
D = [D, dist];
|
|
end
|
|
D2=D;
|
|
start_idx = local_minimas(abs(D));
|
|
D(start_idx)=0;
|
|
|
|
for i = [start_idx+1:length(X)-1]
|
|
dist = euklid_dist([X(i+1),Y(i+1),Z(i+1)],[X(i),Y(i),Z(i)]);
|
|
D(i+1)= D(i) + dist;
|
|
end
|
|
|
|
for i = [start_idx-1:-1:2]
|
|
dist = euklid_dist([X(i-1),Y(i-1),Z(i-1)],[X(i),Y(i),Z(i)]);
|
|
D(i-1)= D(i) + dist;
|
|
end
|
|
|
|
#plot (D);
|
|
minimas = local_minimas(abs(D - switch_offset));
|
|
p1 = minimas(1);
|
|
m1 = minimas(2);
|
|
|
|
end
|