%--------------------------
 % @Author: Jingqiao Hu
 % @Date: 2022-01-29 17:12:57
 % @LastEditTime: 2022-01-29 19:32:45

 % PARAs:
 % nodes: query_points to generate distance field, [n,3]
 % sp, ep: start and end points of mmc,            [1,3]
 % t is the width
%--------------------------
function phi = signed_distance(nodes, sp, ep, t)

    rim   = calculatevector(sp, ep, nodes);
    distm = dist2linevectorization(sp, ep, nodes);

    qm_sp = nodes - sp(:)';
    qm_ep = nodes - ep(:)';
    sp_norm = vecnorm(qm_sp,2,2);
    ep_norm = vecnorm(qm_ep,2,2);

    phi = distm - t;
    
    Index = (rim<=0);
    phi(Index) = sp_norm(Index) - t;
    
    Index = (rim>=1);
    phi(Index) = ep_norm(Index) - t;

    phi = -phi;
    
%     % test    
%     scatter3(nodes(phi < 0, 1), nodes(phi < 0, 2), nodes(phi < 0, 3), 'k'); hold on;
%     scatter3(nodes(phi > 0, 1), nodes(phi > 0, 2), nodes(phi > 0, 3), 'filled', 'r'); hold on;
% 
%     x = [sp(1),ep(1)];
%     y = [sp(2),ep(2)];
%     z = [sp(3),ep(3)];
%     plot3(x',y',z','LineWidth',2, 'Color','b'); hold on;  
end

function r = calculatevector(a,b,pm)
    %   Input: a, start point of the line segment, 1x3 array
    %   Input: b, end point of the line segment, 1x3 array
    %   Input: pm, query point matrix, nx3
    %   Output: r, distance matrix, nx1

    ap = pm - a(:)'; % n,3
    ab = b-a; % 1,3

    tempdot = sum(ap .* ab(:)', 2); % n,3

    r = tempdot ./ (norm(ab,2)^2);
end       
    
function [dm] = dist2linevectorization(v1, v2, pm)
    %   Input: v1, start point of the line segment, 1x3 array
    %   Input: v2, end point of the line segment, 1x3 array
    %   pm: query matrix, n x3 array
    
    m = -pm + v1(:)'; % n,3
        
    n  = v1 - v2;
    nn = repmat(n(:)', size(pm,1), 1);

    tempcross = cross(nn', m'); % 3,n

    dm = vecnorm(tempcross', 2, 2) ./ norm(n,2);
end