motion_planning/RRT/indexSplanPRM.m

39 lines
1.1 KiB
Mathematica

## Copyright (C) 2023 borie
##
## This program is free software: you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation, either version 3 of the License, or
## (at your option) any later version.
##
## 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.
##
## You should have received a copy of the GNU General Public License
## along with this program. If not, see
## <https://www.gnu.org/licenses/>.
## -*- texinfo -*-
## @deftypefn {} {@var{retval} =} indexSplanPRM (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn
## Author: borie <borie@LAPTOP-D62TNEVS>
## Created: 2023-01-20
function [index2] = indexGplanPRM (S, dx_S, points)
for i=1:columns(points)
u = norm(S-points(1,i));
dx_S(i) = [u]
i++
[minval, index1] = min(dx_S)
endfor
endfunction