function [PStrains, Labels_out]=PrinStrains_v2( xyz, T, SG, ps, th, varargin ) 

% PrinStrains_v2 calculates the surface principal strains from 3D strains
% tensor extrated from finite element models (FEMs) and projects them on a
% plane defined by the position and orientation of a strain gage.
%
%
% Inputs
%
%  xyz - (Np by 3 array) 3D coordinates of the SURFACE points of the
%                        model to be evaluated (Np = number of points).
%
%  T   - (Nt by 3 array) Triangles IDs that define the model
%                        (Nt = number of triangles. Can vary among
%                        models).
%
%  SG  - (4 by 3 array) 3D position of the surface points that represent
%                       the vertices of the strain gage.
%
%  ps  - (Np by 6 array) normal and shear strains entered as row vector
%                        (e.g., [xx yy zz xy yz xz])
%
%  th  - (1 by 1 array)  thickness of the section to estimate
%
%  (OPTIONAL)
%  Labels_in - (Np by Nl array) cell array with the text labels for each
%                        brick (Nl = number of label columns)
%
%
% Output
%
%  PStrains - (Nsg by 9 array) Principal strains for each point on the
%                             coordinate system defined by the position and
%                             orientation of the strain gage.
%                           The columns are:
%                             1) X component of Tension unit vector
%                             2) Y component of Tension unit vector
%                             3) Magnitude of Tension vector
%                             4) X component of Compression unit vector
%                             5) Y component of Compression unit vector
%                             6) Magnitude of Compression vector
%                          (Nsg = number of points defined by the strain
%                          gage).
%
%  Labels_out - (Nsg by Nl) cell array with the text labels for the
%                               points defined by the strain gage.
%
%
% Examples:
%
% [PStrains, Labels_out] = PrinStrains_v2 (xyz, T, SG, ps, labels) ;
% PStrains = PrinStrains_v2 (xyz, T, SG, ps, labels) ;
% PStrains = PrinStrains_v2 (xyz, T, SG, ps) ;
% PrinStrains_v2 (xyz, T, SG, ps, labels) ;
% PrinStrains_v2 (xyz, T, SG, ps) ;
% 
%
% Written by Jose Iriarte-Diaz & Callum Ross
% v. 2.0 (January 2010)

close all

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Data Checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if size( xyz, 2 )~=3
    error('Position matrix (xyz) must have 3 columns')
end

if size( T, 2 )~=3
    error('Triangulation matrix (T) must have 3 columns')
end

if size( SG, 1 )~=4
    error('Strain gage matrix (SG) must have 4 rows')
end

if size( SG, 2 )~=3
    error('Strain gage matrix (SG) must have 3 columns')
end

if size( ps, 2 )~=6
    error('Strain matrix must have 6 columns')
end

if size(xyz,1)~=size(ps,1)
    error('Position and strain matrices must have equal number of rows')
end

if isempty( varargin )
    Labels_in = [] ;
else
    if size(varargin,2)~=1
        error('Too many variables!')
    end
    Labels_in = varargin{:} ;
end
        
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot of original data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
set(figure(1),'WindowStyle','docked')
trimesh(T, xyz(:,1), xyz(:,2), xyz(:,3), 'edgecolor', [0.6 0.6 0.6])
axis equal; axis vis3d; hold on
plot3(SG(:,1), SG(:,2), SG(:,3) ,'or', 'markersize', 5, ...
    'markerfacecolor','r')
title( 'Original position and orientation' )

%center the data on origin
xyz1 = xyz - repmat( SG(1,:), size( xyz, 1 ), 1 ) ;
SG1 = SG - repmat( SG(1,:), 4, 1 ) ;

set( figure(2), 'WindowStyle', 'docked' )
trimesh( T, xyz1(:,1), xyz1(:,2), xyz1(:,3), 'edgecolor', [0.6 0.6 0.6] )
hold on; axis equal; axis vis3d
plot3( SG1(:,1), SG1(:,2), SG1(:,3), 'or', 'markersize', 5, ...
    'markerfacecolor','r')
title( 'Centered model' )

% creation of the new coordinate system
p1 = SG1(2,:); p2 = SG1(3,:);
Ax1 = p1 ./ vecmag(p1) ;
ax2 = p2 ./ vecmag(p2) ;
Ax3 = cross( Ax1, ax2 ) ;
Ax3 = Ax3 ./ vecmag(Ax3) ;
Ax2 = cross( Ax3, Ax1 ) ;
Ax2 = Ax2 ./ vecmag(Ax2) ;
Ax = [ Ax1 ; Ax2 ; Ax3 ] ;

% check the axis position and orientation
xLim = diff(get(gca,'xlim')) ;
yLim = diff(get(gca,'ylim')) ;
zLim = diff(get(gca,'zlim')) ;
diagDist = norm([xLim yLim zLim]) ;
scale = diagDist/10 ;
arrow3( zeros(3,3), Ax*scale, 'o' )

% rotation of data
xyz2 = ( Ax * xyz1' )' ;
SG2  = ( Ax * SG1'  )' ;
Ax2  = ( Ax * Ax'   )' ;

set( figure(3), 'WindowStyle', 'docked')
trimesh( T, xyz2(:,1), xyz2(:,2), xyz2(:,3), 'edgecolor', [0.6 0.6 0.6] ,...
    'facecolor',[1 1 1],'facealpha',0.4) ;
hold on; axis equal
plot3( SG2(:,1), SG2(:,2), SG2(:,3), 'or' ,'markersize', 5, ...
    'markerfacecolor','r')
arrow3( zeros(3,3), Ax2*scale, 'o')
title( 'Rotated model' )


% determination of the points inside of the polygon that define the
% vertices of the strain gage
IN = inhull( xyz2(:,1:2), SG2(:,1:2) ) ;

% find plane of the strain gage only
xyz3 = xyz2( IN,: ) ;
ps_in = ps(IN,:) ;
xyz4   = xyz3(   (xyz3(:,3)>-th) & (xyz3(:,3)<th), : ) ;
ps_in = ps_in( (xyz3(:,3)>-th) & (xyz3(:,3)<th), : ) ;
nSelected = size(xyz4,1) ;

if isempty(Labels_in)
    Labels = [] ;
else
    Labels = Labels_in( IN,: ) ;
    Labels = Labels( (xyz3(:,3)>-th) & (xyz3(:,3)<th), : ) ;
end

set( figure(4), 'WindowStyle', 'docked')
trimesh( T, xyz2(:,1), xyz2(:,2), xyz2(:,3), 'edgecolor', [0.6 0.6 0.6] ) ;
hold on; axis equal; axis vis3d

plot3( SG2(:,1), SG2(:,2), SG2(:,3), 'or', 'markersize', 5, ...
    'markerfacecolor','r')
plot3( xyz4(:,1), xyz4(:,2), xyz4(:,3), '.g' );
title( {'Check that the selected points (green dots) are' ...
    ['only inside the red circles... (' num2str(nSelected) ' bricks)']} );

% fit plane to the cloud of points. x0 is the position of the centroid of
% the plane and Axes(:,3) is the unit vector normal to the plane. lsplane.m
% file needed!
[x0, Axes] = lsplane(xyz4) ;

% check that the vertical axis goes up
if sign(Axes(3,3))~=sign(Ax2(3,3))
    Axes = -Axes ;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate main axis direction of strain gage in the new plane
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sgPts = projPointPlane ( SG2(1:2,:), x0', Axes(:,3)' ) ;

sgAx1 = sgPts(2,:) - sgPts(1,:) ;
sgAx1 = sgAx1 ./ norm(sgAx1) ;

% check that the axis of the plane has the same direction as the model
if sign(sgAx1(1))~=sign(Ax2(1,1))
    sgAx1 = -sgAx1 ;
end

sgAx2 = cross( Axes(:,3)', sgAx1 ) ;

sgAxModel = [ sgAx1 ; sgAx2 ; Axes(:,3)' ];

AxesR = sgAxModel * Ax ;
% AxesR is the rotation matrix from coordinate system of the centered model
% to coordinate system of the strain gage

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Determination of the principal strains
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nPoints = size(ps_in,1) ;
TensionDir = nan( nPoints, 3 ) ;
TensionMag = nan( nPoints, 1 ) ;

CompressionDir = nan( nPoints, 3 ) ;
CompressionMag = nan( nPoints, 1 ) ;

for i = 1:nPoints
    [PrincStrainMag, PrincStrainDir] = PrincStrainCalculator( ps_in(i,:), ...
        AxesR ) ;

    TensionDir(i,:)     = PrincStrainDir(1,:) ;
    TensionMag(i,1)     = PrincStrainMag(1) ;
    
    CompressionDir(i,:) = PrincStrainDir(3,:) ;
    CompressionMag(i,1) = PrincStrainMag(3) ;
end


% check quadrant for the directions of the principal strains
isNotNaN = ~isnan( TensionDir(:,1) ) ;
% for compression
[v,v,v] = svd( CompressionDir(isNotNaN,1:2) ) ;
if abs(v(1))>abs(v(3))
    isInverted = CompressionDir(:,1)<0 ;
    CompressionDir( isInverted,: ) = -CompressionDir( isInverted,: ) ;
else
    isInverted = CompressionDir(:,2)<0 ;
    CompressionDir( isInverted,: ) = -CompressionDir( isInverted,: ) ;
end

% for tension
[v,v,v] = svd( TensionDir(isNotNaN,1:2) ) ;
if abs(v(1))>abs(v(2))
    isInverted = TensionDir(:,1)<0 ;
    TensionDir( isInverted,: ) = -TensionDir( isInverted,: ) ;
else
    isInverted = TensionDir(:,2)<0 ;
    TensionDir( isInverted,: ) = -TensionDir( isInverted,: ) ;
end
% end

% Tension and Compression vectors
TensionVector = TensionDir.*repmat(TensionMag, 1, 3) ;
CompressionVector =  CompressionDir.*repmat(CompressionMag, 1, 3) ;

% mean principal strains magnitudes
TensionMeanMag     = nanmean( TensionMag ) ;
CompressionMeanMag = nanmean( CompressionMag ) ;

% mean principal strains vectors
TensionMeanVector     = nanmean( TensionDir, 1 ) .* TensionMeanMag ;
CompressionMeanVector = nanmean( CompressionDir, 1 ) .* CompressionMeanMag ;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% plot principal strains on the strain gage coordinate system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% data rotation
xyz5   = ( AxesR * xyz1' )' ;
xyz6   = ( sgAxModel * xyz4' )' ;
SGplot = ( AxesR * SG1' )' ;

set( figure(5), 'WindowStyle', 'docked')

% plot model
h1 = trisurf( T, xyz5(:,1), xyz5(:,2), xyz5(:,3),...
    'edgecolor', [0.8 0.8 0.8], 'facecolor',[1 1 1]) ;

axis equal; axis vis3d; hold on

plot3(SGplot(:,1),SGplot(:,2),SGplot(:,3),...
    'ok','markerfacecolor','k') ;

% plot of principal strains
xLim = diff(get(gca,'xlim')) ;
yLim = diff(get(gca,'ylim')) ;
Lim = min([xLim yLim]) ;

maxVector = max( max( abs( [TensionMag CompressionMag] ) ) ) ;
scale = Lim / (3 * maxVector) ;

z0 = max(xyz5(:,3)) ;

Tplot = TensionVector     .* scale ;
Cplot = CompressionVector .* scale ;

quiver3(ones(nPoints,1).*x0(1),ones(nPoints,1).*x0(2),ones(nPoints,1).*z0,...
    Tplot(:,1),Tplot(:,2),Tplot(:,3),...
    'autoscale','off','color','r') ;
quiver3(ones(nPoints,1).*x0(1),ones(nPoints,1).*x0(2),ones(nPoints,1).*z0,...
    -Tplot(:,1),-Tplot(:,2),-Tplot(:,3),...
    'autoscale','off','color','r') ;
quiver3(x0(1)-Cplot(:,1),x0(2)-Cplot(:,2),z0-Cplot(:,3),...
    Cplot(:,1),Cplot(:,2),Cplot(:,3),...
    'autoscale','off','color','b') ;
quiver3(x0(1)+Cplot(:,1),x0(2)+Cplot(:,2),z0+Cplot(:,3),...
    -Cplot(:,1),-Cplot(:,2),-Cplot(:,3),...
    'autoscale','off','color','b') ;


title( { sprintf( 'Mean Tension: %2.5e \\mu\\epsilon', TensionMeanMag ) ; ...
    sprintf( 'Mean Compression: %2.5e \\mu\\epsilon', CompressionMeanMag ) } ) ;
view(0,90)
cameratoolbar


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Data export
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
PStrains = [TensionDir(:,1:2) TensionMag CompressionDir(:,1:2) CompressionMag] ;
Labels_out = Labels ;
doSave = questdlg( 'Do you want to save the results as a text file?',...
    '', 'Yes', 'No' ,'No' ) ;

switch doSave
    case 'Yes'
        [File, Path] = uiputfile( '*.txt', 'Save as') ;

        nLabelsCol = size( Labels, 2 ) ;
        strformat1 = [repmat(' ,',1,nLabelsCol) 'TensionDir_X,TensionDir_Y,TensionMag,CompressionDir_X,CompressionDir_Y,CompressionMag\n'];
        strformat2 = [repmat('%s,',1,nLabelsCol) repmat('%6.8f,',1,5) '%6.8f\n'] ;
        if ~isempty( Labels )
            if ~iscellstr( Labels ) ;
                if ~iscell( Labels )
                    if ischar( Labels )
                        Labels = cellstr(Labels) ;
                    else
                        Labels = cellstr( num2str(Labels) ) ;
                    end
                else
                    Labels = cellfun( @numstr, Labels, 'UniformOutput', 0 ) ;
                end
            end
            exportMatrix = [Labels num2cell(PStrains)]' ;
        else
            exportMatrix = num2cell(PStrains)' ;
        end

        
        fid = fopen( [Path File], 'w' ) ;
        fprintf( fid, strformat1 ) ;
        fprintf( fid, strformat2, exportMatrix{:} ) ;
        fclose(fid);
end

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% end of function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% beginning of subfunctions       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function projPts = projPointPlane(Points, pointInPlane, normalPlane)

% projPointPlane returns the projection of points on a plane
%
%   PT2 = PROJECTEDPOINT(Points, pointInPlane, normalPlane).
%   Compute the (orthogonal) projection of 'Points' onto the line
%   'normalPlane'.
%
%   'Points' is a [N*3] array, 'pointInPlane' is a [N*3] array, and
%   'normalPlane' is a [N*9] array
%   Result 'projPoints' is a [N*3] array, containing coordinates of
%   orthogonal projections of 'Points' onto planes 'normalPlane'.
%
% Modified from routines of the GEOM3D library (Author: David Legland)
% http://www.mathworks.com/matlabcentral/fileexchange/24484

nPoints = size(Points,1) ;

Line = [Points repmat(normalPlane, nPoints, 1)];
normalPlane = repmat( normalPlane, nPoints, 1 ) ;

% test if line and plane are parallel
if abs(dot(normalPlane, Line(:,4:6), 2))<1e-14
    projPts = [NaN NaN NaN];
    return;
end

% difference between origins of plane and line
dp = repmat( pointInPlane, nPoints, 1 ) - Line(:,1:3);

% relative position of intersection on line
t = dot( normalPlane, dp, 2 ) / dot( normalPlane, Line(:,4:6), 2 );

% compute coord of intersection point
projPts = Line(:,1:3) + t*Line(:,4:6);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [magnitude]=vecmag(x)

% function to calculate the magnitude of a vector x
% x can be either a [N*2] or a [N*3] array of row vectors

if size(x,2)==3
    magnitude = sqrt( x(:,1).^2 + x(:,2).^2 + x(:,3).^2 ) ;
elseif size(x,2)==2
    magnitude = sqrt( x(:,1).^2 + x(:,2).^2 ) ;
else
    disp('error. Wrong number of columns!')
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x0, Ax, d, normd] = lsplane(X)

% ---------------------------------------------------------------------
% LSPLANE.M   Least-squares plane (orthogonal distance
%             regression).
%
% Version 1.0
% Obtained from http://www.eurometros.org/

% Last amended   I M Smith 27 May 2002.
% Created        I M Smith 08 Mar 2002
% ---------------------------------------------------------------------
% Input
% X        Array [x y z] where x = vector of x-coordinates,
%          y = vector of y-coordinates and z = vector of
%          z-coordinates.
%          Dimension: m x 3.
%
% Output
% x0       Centroid of the data = point on the best-fit plane.
%          Dimension: 3 x 1.
%
% Ax       Direction cosines of the new coordinate system
%          Ax = [x_axis y_axis normal_axis]
%          Dimension: 3 x 3.
%
% <Optional...
% d        Residuals.
%          Dimension: m x 1.
%
% normd    Norm of residual errors.
%          Dimension: 1 x 1.
% ...>
%
% [x0, a <, d, normd >] = lsplane(X)
% ---------------------------------------------------------------------

% check number of data points
m = size(X, 1);
if m < 3
    error('At least 3 data points required: ' )
end
%
% calculate centroid
x0 = mean(X)';
%
% form matrix A of centered points
A = [(X(:, 1) - x0(1)) (X(:, 2) - x0(2)) (X(:, 3) - x0(3))];
%
% calculate the SVD of A
[U, S, V] = svd(A, 0);
%
% find the smallest singular value in S and extract from V the
% corresponding right singular vector
[s, i] = sort(diag(S),'descend');
ax1 = V(:,i(1)) ;
ax2 = V(:,i(2)) ;
ax3 = V(:,i(3)) ;
Ax = [ax1 ax2 ax3] ;
%
% calculate residual distances, if required
if nargout > 2
    d = U(:, i)*s;
    normd = norm(d);
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [Magnitude,Direction] = PrincStrainCalculator (S,Rm)

% This functions employ a rotation matrix (Rm) to
% (i)   create a strain tensor (e) from vectors entered as XX,YY,ZZ,XY,YZ,XZ
% (ii)  perform a transformation of the strains from xyz to x'y'z' coord
%       system to create a tensor called (eprime)
% (iii) calculate principal strains (D) and (V) from (eprime)
%
% Arguments (INPUT)
%  S  -  1 by 6 raw vector with normal and shear strains entered as
%        [XX, YY, ZZ, XY, YZ, XZ]
%  Rm -  3 by 3 array with the rotation matrix (i.e., direction cosines
%        matrix)
%
% Arguments (OUTPUT)
%  Magnitude       - 3 by 1 array of the magnitude of the Principal
%                    strains (PS), which corresponds to the eigenvalues of
%                    the rotated strain tensor
%  Direction       - 3 by 3 array of the direction of the principal
%                    strains [e.g., direction of 1st PS = Direction(:,1) ]
%  RotStrainTensor - 3 by 3 array with the resultant strain tensor after
%                    rotation
%
% Note that this uses the strain transformation method of Ameen: i.e.,
% the shear strains are divided by 2.
% Written by Callum F. Ross.


e = [ S(1)       S(4)/2    S(6)/2 ; ...
      S(4)/2     S(2)      S(5)/2 ; ...
      S(6)/2     S(5)/2    S(3) ] ;

% transforms strain tensor to new coordinate system
% using the direction cosine matrix Rm
e1 = Rm * e * Rm';

eprime = zeros(3,3) ;
eprime(1:2,1:2) = e1(1:2,1:2) ;

% calculates eigenvalues and eigenvectors (principal strains) in new
% coordinate system
[V,D] = eig(eprime) ;

if sum( sign( diag(D) ) )~=0
    Magnitude = nan(3,1) ;
    Direction = nan(3,3) ;
else
    [Magnitude, ind] = sort( diag(D), 'descend' ) ;
    Direction = V(:, ind) ;
    isRightHanded = dot(Direction(:,3),...
        cross(Direction(:,1),Direction(:,2))) ;
    if round(isRightHanded)==-1
        Direction(:,3) = -Direction(:,3) ;
    end
    Direction = Direction' ;
end

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%