function plotPrincipalDir(U, CHfmo1,edofMat,nelx,nely)    
    [prinDir,prinStress] = stressOritation(CHfmo1, U, edofMat);       
%     prinStress = abs(prinStress);
    
    figure;
%     ll = sqrt((prinDir(:,1)).^2 + (prinDir(:,2)).^2);
%     for i = 1:nelx
%         for j = 1:nely
%             idx = (i-1)*nely+j;
% 
%             ellipse(i,j,prinDir(idx,1)/ll(idx)/2,prinDir(idx,2)/ll(idx)/2,atan(prinDir(idx,1)));
%             hold on
%         end
%     end

    ll1 = sqrt((prinDir(:,1)).^2+1);
    ll2 = sqrt((prinDir(:,2)).^2+1);

    [X,Y] = meshgrid(1:1:nelx, 1:1:nely); 
    
    X = X(:);
    Y = Y(:);
    scalar = 5;
    for i = 1:nelx*nely
        % for tan(\theta)
%         scalar1 = scalar * prinStress(i,1);
%         line([X(i),X(i)+scalar1*(1./ll1(i))], [Y(i),Y(i)+scalar1*(prinDir(i,1)./ll1(i))]);
%         hold on;        
%         line([X(i),X(i)-scalar1*(1./ll1(i))], [Y(i),Y(i)-scalar1*(prinDir(i,1)./ll1(i))]);
        
%         scalar2 = scalar * prinStress(i,2);
%         line([X(i),X(i)+scalar2*(1./ll2(i))], [Y(i),Y(i)+scalar2*(prinDir(i,2)./ll2(i))]);
%         line([X(i),X(i)-scalar2*(1./ll2(i))], [Y(i),Y(i)-scalar2*(prinDir(i,2)./ll2(i))]);

        % for eig
        scalar1 = scalar * prinStress(i,1);
        line([X(i),X(i)+scalar1*prinDir(i,1)], [Y(i),Y(i)+scalar1*prinDir(i,2)]);
        hold on
        line([X(i),X(i)-scalar1*prinDir(i,1)], [Y(i),Y(i)-scalar1*prinDir(i,2)]);
        
        scalar2 = scalar * prinStress(i,2);
        line([X(i),X(i)+scalar2*prinDir(i,3)], [Y(i),Y(i)+scalar2*prinDir(i,4)]);
        hold on
        line([X(i),X(i)-scalar2*prinDir(i,3)], [Y(i),Y(i)-scalar2*prinDir(i,4)]);
    end    
    
    set(gca,'YDir','reverse');
    axis equal; axis off; drawnow;
end

function [prinDir,prinStress] = stressOritation(Dlist, U, edofMat)
    [eleNum,~] = size(Dlist);
    
    prinDir = zeros(eleNum,4);
    prinStress = zeros(eleNum,2);
    for i = 1:eleNum
        De = reshape(Dlist(i,:),3,3);
        Se = computeS(De);
        
        edof = edofMat(i,:);
        Ue = U(edof,:);
        stress = Se * Ue(:);
        
        sx = stress(1);
        sy = stress(2);
        sxy = stress(3);%/sqrt(2);
        
%         sx = 80;
%         sy = 40;
%         sxy = 30;
        
        [x,y] = eig([sx,sxy;sxy,sy]);
        [y,idx] = sort(diag(y),'descend');
        prinDir(i,:) = reshape(x(:,idx),4,1);
        prinStress(i,:) = y;
        
        prinStress1 = (sx+sy)/2 + sqrt(((sx-sy)/2)^2 + sxy^2);
        prinStress2 = (sx+sy)/2 - sqrt(((sx-sy)/2)^2 + sxy^2);
        
        % two methods for principal direction
        % one
        s = sxy / sqrt(sxy^2 + ((sx-sy)/2)^2);
        t = (sx-sy)/2 / sqrt(sxy^2 + ((sx-sy)/2)^2);
        if s>0
            phi1 = 1/2*acos(t);
            phi2 = 1/2*acos(t)+pi/2;
        else
            phi1 = pi - 1/2*acos(t);
            phi2 = pi/2 - 1/2*acos(t);
        end
        tan1 = tan(phi1);
        tan2 = tan(phi2);          
        
        % two
%         tan1 = (prinStress1-sx)/sxy;
%         tan2 = -sxy/(prinStress1-sx);
%         
%         prinDir(i,:) = [tan1,tan2];  
%         prinStress(i,:) = [prinStress1, prinStress2];
    end    
end

function S = computeS(D)
    d11 = D(1,1);
    d12 = D(1,2);
    d13 = D(1,3);
    d22 = D(2,2);
    d23 = D(2,3);
    d33 = D(3,3);
        
    S = ...
     [- d11/2 - d13/4, - d12/2 - d13/4, d11/2 - d13/4, d13/4 - d12/2, d11/2 + d13/4, d12/2 + d13/4, d13/4 - d11/2, d12/2 - d13/4
     - d12/2 - d23/4, - d22/2 - d23/4, d12/2 - d23/4, d23/4 - d22/2, d12/2 + d23/4, d22/2 + d23/4, d23/4 - d12/2, d22/2 - d23/4
     - d13/2 - d33/4, - d23/2 - d33/4, d13/2 - d33/4, d33/4 - d23/2, d13/2 + d33/4, d23/2 + d33/4, d33/4 - d13/2, d23/2 - d33/4;];
end