%Script prova metodo Flowmesher per generazione pointcloud iniziale
%Scritto da Pavan Andrea - 20/11/2021
clear;
clc;

%% dati input
[xa] = naca4_generator('4412',100);     %punti profilo
Np = 1500;      %numero punti
d = 3;     %diametro contorno esterno
Nbf = 30;      %numero punti contorno esterno


%% pointcloud
xbf(:,1) = 0.5*d*cos(linspace(0,2*pi,Nbf));     %punti contorno esterno
xbf(:,2) = 0.5*d*sin(linspace(0,2*pi,Nbf));

%flowmesher (basato su https://arxiv.org/abs/2103.05640v1)
P = zeros(Np,2);        %punti
P(1:Nbf-1,:) = xbf(1:Nbf-1,:);
P(Nbf:Nbf+length(xa)-1,:) = xa;
NS = 10;        %numero sorgenti
S(:,1) = 0.5*(d/2)*cos(linspace(0,2*pi,NS+1));        %sorgenti punti
S(:,2) = 0.5*(d/2)*sin(linspace(0,2*pi,NS+1));
S(end,:) = [];

%generazione punti e propagazione
for i=Nbf+length(xa):Np
    P(i,:) = S(1+mod(i,NS),:);
end
m = 1;      %massa virtuale particella
dt = 0.1;       %passo temporale
kv = 0.1;       %costante dissipazione
ks = 0.5;       %costante elastica
alpha = 1/6;        %costante kernel function
h = 0.1;        %target mesh size
W = @(q) alpha*((q<1).*((2-q).^3-4.*(1-q).^3) + (q<2).*(q>=1).*(2-q).^3 + (q>=2)*0);        %kernel function
di = zeros(Np,1);       %distanza punti i-j
x = P;      %vettore posizione punti
v = 2*d*(rand(Np,2)-0.5);        %vettore velocità punti
v(1:Nbf+length(xa)-1,:) = 0;
while mean(abs(v))>1e-2
    %aggiornamento posizione particelle
    for i=Nbf+length(xa):Np
        di = sqrt((x(i,1)-x(:,1)).^2+(x(i,2)-x(:,2)).^2);
        di(di<eps) = 1;
        v(i,1) = (1-kv)*v(i,1) + (dt*ks/m)*dot(W(di./h),(x(i,1)-x(:,1))./di);
        v(i,2) = (1-kv)*v(i,2) + (dt*ks/m)*dot(W(di./h),(x(i,2)-x(:,2))./di);
    end
    x = x + dt*v;

    %particelle fuori dal dominio
    for i=Nbf+length(xa):Np
        if norm(x(i,:),2) > d/2
            x(i,:) = x(i,:)*(0.5*d/norm(x(i,:),2))^2;
            npi = x(i,:)/norm(x(i,:),2);
            v(i,:) = v(i,:) - 2*dot(v(i,:),npi)*npi;
        end

        P2 =  InterX(xa',[x(i,1) x(i,1); x(i,2) -1]);
        if rem(length(P2(1,:)), 2) == 1
            x(i,2) = x(i,2)-2*P2(2,end);
            v(i,2) = -v(i,2);
        end
    end

    %grafico pointcloud
    figure(1);
    plot(x(:,1),x(:,2),'k.');
    hold on;
    plot(xa(:,1),xa(:,2),'b-');
    plot(xbf(:,1),xbf(:,2),'b-');
    title('Grafico pointcloud');
    xlabel('x');
    ylabel('y');
    axis square;
    axis equal;
    hold off;
end



%---------------------
function P = InterX(L1,varargin)
%INTERX Intersection of curves
%   P = INTERX(L1,L2) returns the intersection points of two curves L1 
%   and L2. The curves L1,L2 can be either closed or open and are described
%   by two-row-matrices, where each row contains its x- and y- coordinates.
%   The intersection of groups of curves (e.g. contour lines, multiply 
%   connected regions etc) can also be computed by separating them with a
%   column of NaNs as for example
%
%         L  = [x11 x12 x13 ... NaN x21 x22 x23 ...;
%               y11 y12 y13 ... NaN y21 y22 y23 ...]
%
%   P has the same structure as L1 and L2, and its rows correspond to the
%   x- and y- coordinates of the intersection points of L1 and L2. If no
%   intersections are found, the returned P is empty.
%
%   P = INTERX(L1) returns the self-intersection points of L1. To keep
%   the code simple, the points at which the curve is tangent to itself are
%   not included. P = INTERX(L1,L1) returns all the points of the curve 
%   together with any self-intersection points.
%   
%   Example:
%       t = linspace(0,2*pi);
%       r1 = sin(4*t)+2;  x1 = r1.*cos(t); y1 = r1.*sin(t);
%       r2 = sin(8*t)+2;  x2 = r2.*cos(t); y2 = r2.*sin(t);
%       P = InterX([x1;y1],[x2;y2]);
%       plot(x1,y1,x2,y2,P(1,:),P(2,:),'ro')
%   Author : NS
%   Version: 3.0, 21 Sept. 2010
%   Two words about the algorithm: Most of the code is self-explanatory.
%   The only trick lies in the calculation of C1 and C2. To be brief, this
%   is essentially the two-dimensional analog of the condition that needs
%   to be satisfied by a function F(x) that has a zero in the interval
%   [a,b], namely
%           F(a)*F(b) <= 0
%   C1 and C2 exactly do this for each segment of curves 1 and 2
%   respectively. If this condition is satisfied simultaneously for two
%   segments then we know that they will cross at some point. 
%   Each factor of the 'C' arrays is essentially a matrix containing 
%   the numerators of the signed distances between points of one curve
%   and line segments of the other.
    %...Argument checks and assignment of L2
    error(nargchk(1,2,nargin));
    if nargin == 1,
        L2 = L1;    hF = @lt;   %...Avoid the inclusion of common points
    else
        L2 = varargin{1}; hF = @le;
    end
       
    %...Preliminary stuff
    x1  = L1(1,:)';  x2 = L2(1,:);
    y1  = L1(2,:)';  y2 = L2(2,:);
    dx1 = diff(x1); dy1 = diff(y1);
    dx2 = diff(x2); dy2 = diff(y2);
    
    %...Determine 'signed distances'   
    S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1);
    S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1);
    
    C1 = feval(hF,D(bsxfun(@times,dx1,y2)-bsxfun(@times,dy1,x2),S1),0);
    C2 = feval(hF,D((bsxfun(@times,y1,dx2)-bsxfun(@times,x1,dy2))',S2'),0)';
    %...Obtain the segments where an intersection is expected
    [i,j] = find(C1 & C2); 
    if isempty(i),P = zeros(2,0);return; end;
    
    %...Transpose and prepare for output
    i=i'; dx2=dx2'; dy2=dy2'; S2 = S2';
    L = dy2(j).*dx1(i) - dy1(i).*dx2(j);
    i = i(L~=0); j=j(L~=0); L=L(L~=0);  %...Avoid divisions by 0
    
    %...Solve system of eqs to get the common points
    P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ...
                dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')';
              
    function u = D(x,y)
        u = bsxfun(@minus,x(:,1:end-1),y).*bsxfun(@minus,x(:,2:end),y);
    end
end
