diff --git a/brew/vecfit.m b/brew/vecfit.m index 87b2ab0..f4840a5 100644 --- a/brew/vecfit.m +++ b/brew/vecfit.m @@ -22,59 +22,68 @@ ## Author: Jens ## Created: 2023-06-10 -function [P1, Pa1, D] = vecfit (Pp, mask, Pt, P0, Pa0, step) +function [P1, na_1, D] = vecfit (Pp, ignore_mask, Pt, P0, na_0, step) [N_subst, N_elem] = size(Pp); - # Create default mask - if isempty(mask), - mask = ones(1,N_elem); + # Create default ignore_mask + if isempty(ignore_mask), + ignore_mask = ones(1,N_elem); end # A-Priory state with mask - P1 = P0 + sum(Pp.*Pa0.*mask); + P1 = P0 + sum(Pp.*na_0.*ignore_mask); # Calc distance to target d = (Pt-P1); D = norm(d); dn = d/D; - # Find best prototype - best_i = 0; - best_k = 10000; - best_dir = 1; + subst_list = 1:N_subst; - for i=1:N_subst, - p = Pp(i,:); - pn = p/norm(p); + while 1, + # Find best prototype + best_i = 0; + best_k = 10000; + best_dir = 1; + for i=subst_list, + p = Pp(i,:); + pn = p/norm(p); - # compare d and p - for vecdir=[1,-1], - k = norm(vecdir*pn - dn); - if k < best_k, - best_k = k; - best_i = i; - best_dir = vecdir; + # compare d and p + for vecdir=[1,-1], + k = norm(vecdir*pn - dn); + if k < best_k, + best_k = k; + best_i = i; + best_dir = vecdir; + end end end - end - # Calc next Pa - Pa1 = Pa0; + # Calc next Pa + na_1 = na_0; - # Calc P1 - P1 = P0 + sum(Pp.*Pa0); + # Calc P1 + P1 = P0 + sum(Pp.*na_0); - if best_i, - # Get value from best prototype - delta = best_dir*step; - dps = delta*Pp(best_i,:); + if best_i, + # Get value from best prototype + delta = best_dir*step; + dps = delta*Pp(best_i,:); - # Update point - P1 += dps; - - # Update Point accu - Pa1(best_i) += delta; - end + # Check for negative n + accu = na_1(best_i) + delta; + if accu < 0, + subst_list(best_i) = []; + continue; + end + # Update point + P1 += dps; + # Update Point accu + na_1(best_i) = accu; + break; + end +end endfunction