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.
35 lines
955 B
35 lines
955 B
#
|
|
# Copyright 2022 Philipp Schönberger - mail@phschoen.de
|
|
#
|
|
# This program is distributed in the hope that it will be useful,
|
|
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
# GNU General Public License for more details.
|
|
#
|
|
# See <http://www.gnu.org/licenses/>.
|
|
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
|