65 lines
1.4 KiB
Objective-C
65 lines
1.4 KiB
Objective-C
## Copyright (C) 2023 Jens
|
|
##
|
|
## 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} =} vecfit (@var{input1}, @var{input2})
|
|
##
|
|
## @seealso{}
|
|
## @end deftypefn
|
|
|
|
## Author: Jens <jens@orion>
|
|
## Created: 2023-06-10
|
|
|
|
function [P1, Pa1, D] = vecfit (Pp, Pt, P0, Pa0, step)
|
|
|
|
# A-Priory state
|
|
P1 = P0 + sum(Pp.*Pa0);
|
|
|
|
# Calc distance to target
|
|
d = (Pt-P1);
|
|
D = norm(d);
|
|
dn = d/D;
|
|
|
|
# Find best prototype
|
|
best_i = 0;
|
|
best_k = 10000;
|
|
|
|
for i=1:size(Pp)(1),
|
|
p = Pp(i,:);
|
|
pn = p/norm(p);
|
|
# compare d and p
|
|
k = norm(pn - dn);
|
|
if k < best_k,
|
|
best_k = k;
|
|
best_i = i;
|
|
end
|
|
end
|
|
|
|
# Calc next Pa
|
|
Pa1 = Pa0;
|
|
|
|
if best_i,
|
|
# Get value from best prototype
|
|
dps = step*Pp(best_i,:);
|
|
|
|
# Update point
|
|
P1 += dps;
|
|
|
|
# Update Point accu
|
|
Pa1(best_i) += step;
|
|
end
|
|
|
|
endfunction
|