%  triRigidTrans - triangle tranformation as Rigid as possible, 
%                 also called "Least distortion" 
%                 the idea is to extract out the rotation component Rnf
%                 and interpolate the rotation in angle.
%
%  Usage:       [T,Tdecomp,elements,ABCforPlot] = triRigidTrans(ABC,abc,figNo,infoOn)
%
%  Arguments:   ABC     - the base   triangle
%               abc     - the target triangle
%               figNo   - figure number for plot, 0 for no plot.
%               infoOn  - 0: no info display on screen
%                         1: minimal info display on screen (2 lines)
%                         2: more info display on screen
%                         3: lots of info display on screen (debug mode)
%
%  Returns:     T       - the 3 x 3 affine transformation matrix,
%               Tdecomp - the (3 x 3) x 7 decomposition of T
%                         (:,:,1) - Rd,     first rotation by phi
%                         (:,:,2) - Dd,     scaling in x & y
%                         (:,:,3) - Rd2,    rotation -phi
%                         (:,:,4) - e1,     possible triangle flip x or y
%                         (:,:,5) - Rnf,    second rotation by theta
%                         (:,:,6) - e2,     possible triangle flip x or y
%                         (:,:,7) - transl, final translation
%                         Note: T = transl * e2*Rnf*e1 * Rd2 * Dd * Rd
%               elements- [phi,theta,xScale,yScale,xTransl,yTransl,det(e1*e2)]
%                         phi and theta are in radians,
%                         det(e1*e2)=-1 means triangle flip
%               ABCforPlot - structural array with 
%                           'ABCinter'    - long intermediate transformation,
%                           'ABCcompSRR'  - intermediate transformation (shape)
%                           'ABCcompSRRt' - intermediate transformation
%
%  Author: 
%  Tzu Yen Wong
%  wongt AT csse DOT uwa DOT edu DOT au
%  Department of Computer Science & Software Engineering
%  The University of Western Australia
%
%  Date:        April 2005
% 
%  Example:
%           ABC = [200 0 0; 150 150 200; 1 1 1];
%           abc = [500 550 600; 100 250 100; 1 1 1];
%           [T,Tdecomp,elements] = triRigidTrans(ABC,abc,1,1);

function [T,Tdecomp,elements,ABCforPlot] = triRigidTrans(ABC,abc,figNo,infoOn)

showInter       = 1;    % show long intermediate transformations
showCompInter   = 1;    % show intermediate transformations
showMore        = 1;    % show intermediate transf. stack up & left->right
step            = 7;    % number of intermediate steps 
delay           = 0.1;
eval('infoOn;','infoOn=0;')
eval('figNo;','figNo=0;')

% centroid of trangle
P = mean(ABC,2);
p = mean(abc,2);

minX = min(ABC(1,:));
maxX = max(ABC(1,:));
minY = min(ABC(2,:));
maxY = max(ABC(2,:));

T = abc/ABC;        % T : the affine transformation matrix

ABCinter   = [];
ABCcompSRR = [];
ABCcompSRRt= [];

% Decompose T into Rotation(VT), Scaling(D) and Rotation(U)
[U,D,VT] = svd(T(1:2,1:2));
V = VT';                                % VT = V', Transpose

% Rotation matrix, R has the form 
% | cos  -sin |
% | sin   cos |
% so R(1,1) and R(2,2) must be the same
if V(1,1)*V(2,2)<0                      
% the matrix doesn't agree with the form,
% due to triangle flipping, 
% fix up V here:
% T = U*D*V 
%   = U*D*(sw^-1)*sw*V
%   = U*(sw^-1)*D*sw*V
    if V(1,1)>0,
        sw = [1  0
              0 -1];
    else
        sw = [-1 0
               0 1];
    end
    if infoOn>1, 
        disp('fixed U*(sw^-1)*D*sw*V'), 
    end
    V  = sw*V;
    VT = V';
    U  = U/sw;
elseif V(1,1)<0,
    if infoOn>1, 
        disp('fixed (-U)*D*(-V)')
    end
    V  = -V;
    VT = V';
    U  = -U;
end

% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE
	if infoOn>1,
		disp('U_D_VT = '),disp([U,D,VT])
	end
	T_UDV_compare = [T(1:2,1:2) [0;0] U*D*V] ;
    if sum(sum(abs(T(1:2,1:2) - U*D*V)))>0.001 | infoOn>2,
        fprintf('\nsum(sum(abs(T(1:2,1:2) - U*D*V))) = %.5f',...
            sum(sum(abs(T(1:2,1:2) - U*D*V))))
        T_UDV_compare
    end
% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE

% Decomposition of a 3x3 affine transformation
% T = transl * R * Rd2 * Dd * Rd
% Rotate phi, scale in xy, rotate phi back, rotate theta, translate.
Rd      = [ [ V      [0;0]           ] ; 0 0 1];
Dd      = [ [ D      [0;0]           ] ; 0 0 1];
Rd2     = [ [ V'     [0;0]           ] ; 0 0 1];
R       = [ [ U*V    [0;0]           ] ; 0 0 1];
transl  = [ [ eye(2) [T(1,3);T(2,3)] ] ; 0 0 1];

% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE
if sum(sum(abs(T - transl*R*Rd2*Dd*Rd)))>0.001 | infoOn>2,
    T_translRRd2DdRd_compare = [ T [0;0;0]  transl*R*Rd2*Dd*Rd ]
end
% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE

e = diag([det(R) 1 1]); % flip 
epsilon = det(R);
epsilonD = det(Rd);
if infoOn>1,
    fprintf('%s%.0f\n','epsilon = det(R) = ',epsilon)
end
if epsilonD<0 | infoOn>2,
    fprintf('%s%.0f\n','epsilonD = det(Rd) = ',epsilonD)
    Rd
end
    

%   | -a  b  | = | -1  0  |  |  a  -b  |
%   |  b  a  |   |  0  1  |  |  b   a  |
% 
%   |  a  b  | = |  a  -b  |  |  1  0  | 
%   |  b -a  |   |  b   a  |  |  0 -1  |

%   Tnf = Transformation without triangle flipping
    Tnf = T;        

if sign(epsilon) == -1,         % triangle flipping
%   Rnf: R no flip, as in standard form above
%   R = e2*Rnf*e1
    Rnf = abs(R); Rnf(1,2) = -Rnf(1,2);
    
    if sign(R(1,1))<0 & sign(R(1,2))>0 & sign(R(2,1))>0,
        if infoOn>1, if infoOn>2, R, end, disp('R: negative TL'); end
        flip1 = '';
        flip2 = 'FLIP x, ';
        e2 = [-1 0 0 
               0 1 0
               0 0 1];
        e1 = eye(3);
        Tnf(1,3)=-T(1,3);
    elseif sign(R(1,1))>0 & sign(R(2,1))<0 & sign(R(1,2))<0,
        if infoOn>1, if infoOn>2, R, end, disp('R: positive TL'); end
        flip1 = '';
        flip2 = 'FLIP y, ';
        e2 = [ 1  0  0 
               0 -1  0
               0  0  1];
        e1 = eye(3);
        Tnf(2,3)=-T(2,3);
    elseif sign(R(2,2))<0 & sign(R(2,1))>0 & sign(R(1,2))>0,
        if infoOn>1, if infoOn>2, R, end, disp('R: negative BR'); end
        flip1 = 'FLIP y, ';
        flip2 = '';
        e2 = eye(3);
        e1 = [ 1  0  0 
               0 -1  0
               0  0  1];
        Tnf(2,3)=-T(2,3);
    elseif sign(R(2,2))>0 & sign(R(2,1))<0 & sign(R(1,2))<0,
        if infoOn>1, if infoOn>2, R, end, disp('R: positive BR'); end
        flip1 = 'FLIP x, ';
        flip2 = '';
        e2 = eye(3);
        e1 = [-1  0  0 
               0  1  0
               0  0  1];
        Tnf(1,3)=-T(1,3);
    else
        warning('sth wrong?')
    end
else                            % no triangle flipping
    Rnf = R;
    flip1 ='';
    flip2 ='';
    e2 = eye(3);
    e1 = eye(3);
end

% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE
beSmall = sum(sum(abs(R - e2*Rnf*e1)));
if beSmall>0.001 | infoOn>2,
    fprintf('\n%s%.5f\n','sum(sum(abs(R - e2*Rnf*e1))) = ',beSmall)
    R
    e2_Rnf_e1 = e2*Rnf*e1
end
% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE

ABCsquare = [minX,minX,maxX,maxX,minX
             minY,maxY,maxY,minY,minY
             1    1    1    1    1   ];

ABCR     = Rd            * [ABC ABC(:,1)];
ABCRD    = Dd            * ABCR;
ABCRDR   = Rd2           * ABCRD;
ABCRDRR  = e2 * Rnf * e1 * ABCRDR;
ABCRDRRt = transl        * ABCRDRR;

ABCRsq     = Rd     * ABCsquare;
ABCRDsq    = Dd     * ABCRsq;
ABCRDRsq   = Rd2    * ABCRDsq;
ABCRDRRsq  = R      * ABCRDRsq;
ABCRDRRtsq = transl * ABCRDRRsq;
    
% Calculating the individual components of Transformation
	theta   = atan2((Rnf(2,1)),(Rnf(1,1)));
	phi     = atan2((Rd(2,1)),(Rd(1,1)));
	
	xScale  = Dd(1,1);
	yScale  = Dd(2,2);
	xTransl = transl(1,3);
	yTransl = transl(2,3);

% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE
% attempt to make intermediate tranform by decomposing
	Ddi = Dd;
	transli = [[eye(2) [xTransl;yTransl]];0 0 1];
	
	Rdi = [[[cos(phi) -sin(phi);sin(phi) cos(phi)] [0;0]];0 0 1]*diag([epsilonD,1,1]);
	Rd2i = [[[cos(phi) -sin(phi);sin(phi) cos(phi)]' [0;0]];0 0 1]*diag([epsilonD,1,1]);
	Ri = e2*[[[cos(theta) -sin(theta) ;sin(theta) cos(theta)] [0;0]];0 0 1]*e1;
	
    if sum(sum(abs(Rdi-Rd) + abs(Ddi-Dd) + abs(Ri-R)))>0.01 | infoOn>2,
        Rdi_Rd   = [Rdi [0;0;0] Rd] 
        Ddi_Dd   = [Ddi [0;0;0] Dd]
        Rd2i_Rd2 = [Rd2i [0;0;0] Rd2]
        Ri_R     = [Ri [0;0;0] R]
        transli_transl = [transli [0;0;0] transl]
    end
    clear Rdi Ddi Rd2i Ri transli
% EEEEEEEEEEEEEEEEEEEEEEEEEE Error checking EEEEEEEEEEEEEEEEEEEEEEEEEE

% OOOOOOOOOOOOOOOOO Output OOOOOOOOOOOOOOOOO
Tdecomp = zeros(3,3,7);
Tdecomp(:,:,1) = Rd;
Tdecomp(:,:,2) = Dd;
Tdecomp(:,:,3) = Rd2;
Tdecomp(:,:,4) = e1;
Tdecomp(:,:,5) = Rnf;
Tdecomp(:,:,6) = e2;
Tdecomp(:,:,7) = transl;

elements = [phi,theta,xScale,yScale,xTransl,yTransl,det(e1*e2)];
% OOOOOOOOOOOOOOOOO Output OOOOOOOOOOOOOOOOO

% DDDDDDDDDDDD Composited Intermediate Transformation DDDDDDDDDDDDDDDDDDDD
ABC_i  = [ABC ABC(:,1)];
sizeTri = size(ABC_i);

if showCompInter,
    if det(e1)<0,   
        % triangle flipped & rotation, negate rotation angle 
        thetaNF = -theta;       % theta No Flip
    else
        thetaNF = theta;
    end
        
    ABCcompS    = zeros([sizeTri step]);
    ABCcompSRR  = zeros([sizeTri step]);
    ABCcompSRRt = zeros([sizeTri step]);

    for iii=1:step,
        frac    = iii/step;
        theta_i = frac * thetaNF;
        R_i     = [ cos(theta_i)  -sin(theta_i)  0
                    sin(theta_i)   cos(theta_i)  0
                          0              0       1  ];
%       Ddi      = eye(3) + frac * diag([(xScale-1) (yScale-1) 0]);
%                = (1-frac) * eye(3) + frac * diag([xScale yScale 0]);
        Ddi      = (1-frac) * eye(3) + frac * Dd;
        transl_i = [[eye(2) [frac*Tnf(1,3);frac*Tnf(2,3)]];0 0 1];
        
        ABCRD_i = Rd2 * Ddi * Rd * ABC_i;       % intermediate scaled
        ABCRDRR_i = R_i * ABCRD_i;              % intermediate scaled Rotated
        ABCRDRRt_i = transl_i * ABCRDRR_i;      
        % intermediate translation doesn't work well
        
        ABCcompSRR(:,:,iii) = ABCRDRR_i;
        ABCcompSRRt(:,:,iii) = ABCRDRRt_i;
        
        P_i = frac*p + (1-frac)*P;
        ABCcompSRRt(:,:,iii) = shiftTri(ABCRDRR_i,P_i);
    end
    if det(e2)<0,
        if infoOn>1, disp('e2 = '),disp(e2), end
        ABCRDRR_i = e2 * ABCRDRR_i;
        ABCcompSRR(:,:,step+1) = ABCRDRR_i;
        ABCcompSRRt(:,:,step+1) = shiftTri(ABCRDRR_i,P_i);
    elseif det(e1)<0,
        if infoOn>1, disp('e1 = '),disp(e1), end
        ABCRDRR_i = e1 * ABCRDRR_i;
        ABCcompSRR(:,:,step+1) = ABCRDRR_i;
        ABCcompSRRt(:,:,step+1) = shiftTri(ABCRDRR_i,P_i);
    end   
end
% DDDDDDDDDDDD Composited Intermediate Transformation DDDDDDDDDDDDDDDDDDDD

        
    
% DDDDDDDDDDDDDDDDDDD Intermediate Transformation DDDDDDDDDDDDDDDDDDD
%     The transformation is applied one step at a time.
if showInter ==1 
    % RRRRRRRRRR Setup First Rotation RRRRRRRRRR
    stepRd  = max(round(phi/(pi/6)),3);
    phifrac = phi/stepRd;
	Rd_j    = [ cos(phifrac)  -sin(phifrac)  0
                sin(phifrac)   cos(phifrac)  0
                      0             0         1  ];
	% DDDDDDDDDD Setup Scaling DDDDDDDDDD
    DdxInc  = (xScale-1)/step;
	Ddjx    = diag([DdxInc 0 0]);
    DdyInc  = (yScale-1)/step;
	Ddjy    = diag([0 DdyInc 0]);
	% RRRRRRRRRR Setup Undo First Rotation RRRRRRRRRR
	Rd2_j   = Rd_j';
    % RRRRRRRRRR Setup Second Rotation RRRRRRRRRR
    stepR   = max(round(theta/(pi/6)),3);
    thetafrac = theta/stepR;
	R_j     = [ cos(thetafrac)  -sin(thetafrac)  0
                sin(thetafrac)   cos(thetafrac)  0
                       0               0         1  ];
	% ttttttttttt Setup Final Translation ttttttttttt
    transInc = [xTransl/(step+1);yTransl/(step+1)];
    transl_j = [[eye(2) transInc];0 0 1];

    NumTotalStep = stepRd + step + stepRd + stepR + step+1;
    if det(e2)<0,
        NumTotalStep = NumTotalStep+1;
    end
    if det(e1)<0,
        NumTotalStep = NumTotalStep+1;
    end
    
    % Matrix to hold all triangle in transition
    ABCinter = zeros([sizeTri NumTotalStep]); 

% RRRRRRRRRR First Rotation RRRRRRRRRR
    ABC_j = [ABC ABC(:,1)];
    ABCR_j = ABC_j;
    for jjj=1:stepRd,
        ABCR_j = Rd_j * ABCR_j;
        ABCinter(:,:,jjj) = ABCR_j;
% 		plot(ABCinter(1,:,jjj)',ABCinter(2,:,jjj)','oy-')
    end
    stepOver = stepRd;
    
% DDDDDDDDDD Scaling DDDDDDDDDD
    ABCRD_j = ABCR_j;
    for jjj=1:step,
        ABCRD_j = ABCRD_j + Ddjx * ABCR_j;
        ABCinter(:,:,jjj+stepOver) = ABCRD_j;
% 		plot(ABCinter(1,:,jjj+stepOver)',ABCinter(2,:,jjj+stepOver)','xy-')
    end
    stepOver = stepOver + step;
    
    for jjj=1:step,
        ABCRD_j = ABCRD_j + Ddjy * ABCR_j;
        ABCinter(:,:,jjj+stepOver) = ABCRD_j;
% 		plot(ABCinter(1,:,jjj+stepOver)',ABCinter(2,:,jjj+stepOver)','xy-')
    end
    stepOver = stepOver + step;

% RRRRRRRRRR Undo First Rotation RRRRRRRRRR
	ABCRDR_j = ABCRD_j;
    for jjj=1:stepRd,
        ABCRDR_j = Rd2_j * ABCRDR_j;
        ABCinter(:,:,jjj+stepOver) = ABCRDR_j;
% 		plot(ABCinter(1,:,jjj+stepOver)',ABCinter(2,:,jjj+stepOver)','+y-')
    end
    stepOver = stepOver + stepRd;

% FFFFFFFFFF Possible Triangle Flip FFFFFFFFFF
    if det(e1)<0,
        ABCRDR_j = e1 * ABCRDR_j;
        ABCinter(:,:,stepOver+1) = ABCRDR_j;
% 		plot(ABCinter(1,:,stepOver+1)',ABCinter(2,:,stepOver+1)','ok-')
		stepOver = stepOver + 1;
    end   
    
% RRRRRRRRRR Second Rotation RRRRRRRRRR
    ABCRDRR_j = ABCRDR_j;
    for jjj=1:stepR,
        ABCRDRR_j = R_j * ABCRDRR_j;
        ABCinter(:,:,jjj+stepOver) = ABCRDRR_j;
% 		plot(ABCinter(1,:,jjj+stepOver)',ABCinter(2,:,jjj+stepOver)','*y-')
    end
    stepOver = stepOver + stepR;
    
% FFFFFFFFFF Possible Triangle Flip FFFFFFFFFF
	if det(e2)<0,
		ABCRDRR_j = e2 * ABCRDRR_j;
        ABCinter(:,:,stepOver+1) = ABCRDRR_j;
% 		plot(ABCinter(1,:,stepOver+1)',ABCinter(2,:,stepOver+1)','ok-')
    stepOver = stepOver + 1;
	end   

% ttttttttttt Final Translation ttttttttttt
    ABCRDRRt_j = ABCRDRR_j;
    for jjj=1:step+1,
		ABCRDRRt_j = transl_j * ABCRDRRt_j;
        ABCinter(:,:,jjj+stepOver) = ABCRDRRt_j;
% 		plot(ABCinter(1,:,jjj+stepOver)',ABCinter(2,:,jjj+stepOver)','vy-')
    end

% EEEEEEEEEE Error checking EEEEEEEEEE
if sum(sum(abs(ABCRDRRt-ABCRDRRt_j)))>0.001 | infoOn>2,
    fprintf('%s%.5f\n',...
        'sum(sum(abs(ABCRDRRt-ABCRDRRt_j))) = ',...
         sum(sum(abs(ABCRDRRt-ABCRDRRt_j))) )
     disp([ABCRDRRt [0;0;0] ABCRDRRt_j])
end
% EEEEEEEEEE Error checking EEEEEEEEEE

% DDDDDDDDDDDDDDDDDDD Intermediate Transformation DDDDDDDDDDDDDDDDDDD
end

% OOOOOOOOOOOOOOOOO Output OOOOOOOOOOOOOOOOO
ABCforPlot = struct(...
    'ABCinter',ABCinter,'ABCcompSRR',ABCcompSRR,'ABCcompSRRt',ABCcompSRRt);
% OOOOOOOOOOOOOOOOO Output OOOOOOOOOOOOOOOOO

% DDDDDDDDDDDDDDDDDDDDDDDDDDDD Display on screen DDDDDDDDDDDDDDDDDDDDDDDDDDDD
if infoOn>0,
	if phi >0,        
        dispRot1 = sprintf('Rotate %6.1f deg ccw',phi*180/pi);
        dispRot2 = sprintf('Rotate %6.1f deg cw ',phi*180/pi);
	else        
        dispRot1 = sprintf('Rotate %6.1f deg cw ',-phi*180/pi);
        dispRot2 = sprintf('Rotate %6.1f deg ccw',-phi*180/pi);
	end
	dispRot3 = sprintf('Rotate another %6.1f degree cw', -theta*180/pi);
	
	disp(sprintf('%s, Scale %6.2f in x, Scale %6.2f in y, %s back,',...
        dispRot1,xScale,yScale,dispRot2))
	disp(sprintf('%s%s, %stranslate %6.1f in x, translate %6.1f in y.',...
        flip1,dispRot3,flip2,xTransl,yTransl))
end
if infoOn>1, hr,br, end
% DDDDDDDDDDDDDDDDDDDDDDDDDDDD Display on screen DDDDDDDDDDDDDDDDDDDDDDDDDDDD

% DDDDDDDDDDDDDDDDDDD Figure plot DDDDDDDDDDDDDDDDDDD
if figNo,
    if showInter ==1 | showCompInter ==1,
	figure(figNo),clf,hold on
	set(figNo,'name','Triangle Rigid Transformation')

		arrowsimple([ABC(1,:);abc(1,:)],[ABC(2,:);abc(2,:)],'b--')
	
		patch(ABC(1,:)'     ,ABC(2,:)'     ,'y','FaceAlpha',0.9,'EdgeColor','k','LineWidth',2)
		patch(ABCRDRRt(1,:)',ABCRDRRt(2,:)','y','FaceAlpha',0.9,'EdgeColor','k','LineWidth',2)
	
        plot(ABCR(1,:)'    ,ABCR(2,:)'    ,'or-')
		plot(ABCRD(1,:)'   ,ABCRD(2,:)'   ,'xr-')
		plot(ABCRDR(1,:)'  ,ABCRDR(2,:)'  ,'+r-')
		plot(ABCRDRR(1,:)' ,ABCRDRR(2,:)' ,'*r-')
		plot(ABCRDRRt(1,:)',ABCRDRRt(2,:)','r:')
	
		plot(ABCsquare(1,:)' ,ABCsquare(2,:)' ,'k')
		plot(ABCRsq(1,:)'    ,ABCRsq(2,:)'    ,'or-.')
		plot(ABCRDsq(1,:)'   ,ABCRDsq(2,:)'   ,'xr-.')
		plot(ABCRDRsq(1,:)'  ,ABCRDRsq(2,:)'  ,'+r-.')
		plot(ABCRDRRsq(1,:)' ,ABCRDRRsq(2,:)' ,'*r-.')
		plot(ABCRDRRtsq(1,:)',ABCRDRRtsq(2,:)','k')
     
		axis equal
		a= axis;
		plot([0 a(1);0 a(2)],[a(3) 0;a(4) 0],'b:')
    else
        figNo = figNo-1;
    end
    
	if showInter ==1,
        pause
		for jjj = 1:size(ABCinter,3),
            patch(ABCinter(1,:,jjj)',ABCinter(2,:,jjj)','y',...
                'FaceAlpha',0.4,'EdgeColor','r')
            drawnow, pause(delay)
		end    
	end
	if showCompInter==1,
		pause
		for iii = 1:size(ABCcompSRR,3),
            patch(ABCcompSRR(1,:,iii)',ABCcompSRR(2,:,iii)','b',...
                'FaceAlpha',0.2,'EdgeColor','k')
            drawnow, pause(delay)
		end
		for iii = 1:size(ABCcompSRRt,3),
            patch(ABCcompSRRt(1,:,iii)',ABCcompSRRt(2,:,iii)','r',...
                'FaceAlpha',0.2,'EdgeColor','k')
            drawnow, pause(delay)
		end    
	end
	
% TTTTTTTTTTTTTTTTTTT Triangle transformation line up TTTTTTTTTTTTTTTTTTT 
if showMore,
    figure(figNo+1), clf
	set(figNo+1,'name','Triangle Rigid Transformation Lined up')
	axis equal,axis off
	ABCo = shiftTri(ABC,[0;0;1]);
	abco = shiftTri(abc,[0;0;1]);
    
	arrowsimple([ABCo(1,:);abco(1,:)],[ABCo(2,:);abco(2,:)],'b--')
	patch(ABCo(1,:)',ABCo(2,:)','b','FaceAlpha',0.5,'EdgeColor','k','LineWidth',2)
	patch(abco(1,:)',abco(2,:)','b','FaceAlpha',0.05,'EdgeColor','k','LineWidth',2)
	for iii = 1:size(ABCcompSRR,3),
		ABCcompSRRo(:,:,iii) = shiftTri(ABCcompSRR(:,:,iii),[0;0;1]);
        patch(ABCcompSRRo(1,:,iii)',ABCcompSRRo(2,:,iii)','b',...
            'FaceAlpha',0.5,'EdgeColor','k')
        drawnow, %pause(delay)
	end
	plot([ABCo(1,1); permute(ABCcompSRRo(1,1,:),[3,2,1])],...
         [ABCo(2,1); permute(ABCcompSRRo(2,1,:),[3,2,1])],'b--')
	plot([ABCo(1,2); permute(ABCcompSRRo(1,2,:),[3,2,1])],...
         [ABCo(2,2); permute(ABCcompSRRo(2,2,:),[3,2,1])],'b--')
	plot([ABCo(1,3); permute(ABCcompSRRo(1,3,:),[3,2,1])],...
         [ABCo(2,3); permute(ABCcompSRRo(2,3,:),[3,2,1])],'b--')
    
    axisfig = axis; pause
	set(figNo+1,'DoubleBuffer','On')
	for iii = 1:size(ABCcompSRR,3),
        clf,
		patch(ABCo(1,:)',ABCo(2,:)','b','FaceAlpha',0.5,'EdgeColor','k','LineWidth',2)
		patch(abco(1,:)',abco(2,:)','b','FaceAlpha',0.05,'EdgeColor','k','LineWidth',2)
        axis(axisfig), axis off
		ABCcompSRRo(:,:,iii) = shiftTri(ABCcompSRR(:,:,iii),[0;0;1]);
        for iiii = 1:iii-1,
            patch(ABCcompSRRo(1,:,iiii)',ABCcompSRRo(2,:,iiii)','b',...
                'FaceAlpha',0.8/step,'EdgeColor','None')
        end
        patch(ABCcompSRRo(1,:,iii)',ABCcompSRRo(2,:,iii)','b',...
            'FaceAlpha',0.5,'EdgeColor','k')
        drawnow, pause(delay)
	end
    pause
    
	set(figNo+1,'DoubleBuffer','Off')
	arrowsimple([ABCo(1,:);abco(1,:)],[ABCo(2,:);abco(2,:)],'b--')
	patch(ABCo(1,:)',ABCo(2,:)','b','FaceAlpha',0.5,'EdgeColor','k','LineWidth',2)
	patch(abco(1,:)',abco(2,:)','b','FaceAlpha',0.05,'EdgeColor','k','LineWidth',2)
	for iii = 1:size(ABCcompSRR,3),
		ABCcompSRRo(:,:,iii) = shiftTri(ABCcompSRR(:,:,iii),[0;0;1]);
        patch(ABCcompSRRo(1,:,iii)',ABCcompSRRo(2,:,iii)','b',...
            'FaceAlpha',0.5,'EdgeColor','k')
	end
	plot([ABCo(1,1); permute(ABCcompSRRo(1,1,:),[3,2,1])],...
         [ABCo(2,1); permute(ABCcompSRRo(2,1,:),[3,2,1])],'b--')
	plot([ABCo(1,2); permute(ABCcompSRRo(1,2,:),[3,2,1])],...
         [ABCo(2,2); permute(ABCcompSRRo(2,2,:),[3,2,1])],'b--')
	plot([ABCo(1,3); permute(ABCcompSRRo(1,3,:),[3,2,1])],...
         [ABCo(2,3); permute(ABCcompSRRo(2,3,:),[3,2,1])],'b--')
    drawnow
end
% TTTTTTTTTTTTTTTTTTT Triangle transformation line up TTTTTTTTTTTTTTTTTTT 
	
% TTTTTTTTTTTTTTTTTTT Triangle transformation left to right TTTTTTTTTTTTTTTTTTT
if showMore,
    figure(figNo+2), clf
	set(figNo+2,'name','Triangle Rigid Transformation Left to Right')
	axis equal,axis off
	ccc=size(ABCcompSRR,3);
    if det(e2*e1)<1,
        ccc = ccc-1;
    end
    lastposX = ccc*max(max(ABC(1,:))-min(ABC(1,:)),max(abc(1,:))-min(abc(1,:)));
	ABCo = shiftTri(ABC,[0;0;1]);
	abco = shiftTri(abc,[lastposX;0;1]);
	arrowsimple([ABCo(1,:);abco(1,:)],[ABCo(2,:);abco(2,:)],'b--')
	patch(ABCo(1,:)',ABCo(2,:)','b','FaceAlpha',0.5,'EdgeColor','k','LineWidth',2)
	patch(abco(1,:)',abco(2,:)','b','FaceAlpha',0.05,'EdgeColor','k','LineWidth',2)
	for iii = 1:ccc,
        fracc = iii/ccc;
        P_ii = fracc*[lastposX;0;1]+(1-fracc)*[0;0;1];
		ABCcompSRRo = shiftTri(ABCcompSRR(:,:,iii),P_ii);
        patch(ABCcompSRRo(1,:)',ABCcompSRRo(2,:)','b',...
            'FaceAlpha',0.5,'EdgeColor','k')
        drawnow, pause(delay)
	end
    if det(e2*e1)<1,
		ABCcompSRRo = shiftTri(ABCcompSRR(:,:,iii+1),P_ii);
        patch(ABCcompSRRo(1,:)',ABCcompSRRo(2,:)','b',...
            'FaceAlpha',0.5,'EdgeColor','k')
        drawnow
    end
end
% TTTTTTTTTTTTTTTTTTT Triangle transformation left to right TTTTTTTTTTTTTTTTTTT

end
% DDDDDDDDDDDDDDDDDDD Figure plot DDDDDDDDDDDDDDDDDDD

% subfunction to shift triangle to new location
function triOut = shiftTri(triIn,P)
	P_x = mean(triIn(:,1:3),2);
    transl = [[eye(2) [P(1:2)-P_x(1:2)]];0 0 1];
    triOut = transl * triIn;