%-----------------------------------------------------------------------
% Computes the non-singular displacement field produced by
% the dislocation structure.
% This function only works for triangular loops. 
% (Based on Fivel and Depres, Phil. Mag., 2014).
%
% DDD-XRD
% nbertin@stanford.edu
%-----------------------------------------------------------------------

function u = displacement_structure(rn,links,NU,a,r)

if size(rn,1) > 3
    error('displacement_structure only valid for triangular loops');
end
u = zeros(size(r,1),3);

% Loop center
rc = mean(rn);
% Loop plane normal
n = cross(rn(2,1:3)-rn(1,1:3),rn(3,1:3)-rn(1,1:3));
n = n/norm(n);

for j=1:size(r,1)
    for i=1:size(links,1)
        n1 = links(i,1);
        n2 = links(i,2);
        r1 = rn(n1,1:3);
        r2 = rn(n2,1:3);
        b = links(i,3:5);
        
        % Check that each segment is comprised in the plane
        if dot(r2-r1,n) > 1e-5
            error('loop is not planar');
        end
        
        % Compute displacement
        u(j,:) = squeeze(u(j,:)) + triangular_displacement_ns(r1,r2,rc,b,n,NU,a,r(j,1:3));
    end
end

end

function u = triangular_displacement_ns(A,B,C,b,n,NU,a,P)

Tab = (B-A)/norm(B-A);
Tbc = (C-B)/norm(C-B);
Tca = (A-C)/norm(A-C);

% n0 is the normal counter clockwise
n0 = cross(Tab,-Tca);
n0 = n0/norm(n0);

if abs(abs(dot(n0,n))-1) > 1e-5
    error('triangular loop does not lie in its plane');
end

Pa = A-P;
Pb = B-P;
Pc = C-P;

Ra = norm(Pa);
Rb = norm(Pb);
Rc = norm(Pc);

La = Pa/Ra;
Lb = Pb/Rb;
Lc = Pc/Rc;

a0 = acos(dot(Lb,Lc));
b0 = acos(dot(La,Lc));
c0 = acos(dot(La,Lb));
s0 = 0.5*(a0+b0+c0);

tans = tan(s0/2);
tana = tan((s0-a0)/2);
tanb = tan((s0-b0)/2);
tanc = tan((s0-c0)/2);
tanp = tans*tana*tanb*tanc;
tanp = max(tanp,0);
if abs(dot(La,n0)) < 1e-10
    sgn = 0;
else
    sgn = sign(dot(La,n0));
end
Omega = -sgn*(4*atan(sqrt(tanp)));

% Solid angle
u = -Omega/4/pi*b;

% Line integrals
u = u + displacement_seg_ns(A,B,b,NU,a,P);
u = u + displacement_seg_ns(B,C,b,NU,a,P);
u = u + displacement_seg_ns(C,A,b,NU,a,P);

end

function u = displacement_seg_ns(x1,x2,b,NU,a,P)

a2 = a^2;
t = x2-x1;
t = t/norm(t);
x0 = x1 + dot(P-x1,t)*t;
d = P-x0;
d2 = dot(d,d);
s1 = dot(x1-x0,t);
s2 = dot(x2-x0,t);

Ra1 = sqrt(d2+s1^2+a2);
Ra2 = sqrt(d2+s2^2+a2);

J01 = log(Ra2+s2)-log(Ra1+s1);
J03 = s2/(d2+a2)/Ra2-s1/(d2+a2)/Ra1;
J13 = -1/Ra2+1/Ra1;
J23 = J01-s2/Ra2+s1/Ra1;

A = [t(2)*b(3)-t(3)*b(2), ...
     -t(1)*b(3)+t(3)*b(1), ...
     t(1)*b(2)-t(2)*b(1)];
A = (a2*J03+2*J01)*A;

delta = eye(3);
C = zeros(3);
for i=1:3
    for j=1:3
        C(i,j) = delta(i,j)*J01-d(i)*d(j)*J03+ ...
                 (d(i)*t(j)+t(i)*d(j))*J13-t(i)*t(j)*J23;
    end
end
B = zeros(1,3);
for i=1:3
    B(i) = t(1)*C(2,i)*b(3)+t(2)*C(3,i)*b(1)+t(3)*C(1,i)*b(2) ...
          -t(3)*C(2,i)*b(1)-t(1)*C(3,i)*b(2)-t(2)*C(1,i)*b(3);
end

u = 1/8/pi*(A+B/(1-NU));

end
