% Procrustes transformation for two triangles.
% This is an initial test before more complicated
% biological data
%
% Samuli Siltanen Nov 2015

% Parameters for controlling the plots
lwidth = .5;
msize  = 20;
fsize  = 16;

%% Define two triangles
t1 = [...
    [1 1];...
    [4 5];...
    [1 6]];

t2 = [...
    [4 2];...
    [-1 3];...
    [-3 2]];

% Plot the original triangles
figure(1)
clf
plot([t1(:,1);t1(1,1)],[t1(:,2);t1(1,2)],'k','linewidth',lwidth)
hold on
plot(t1(:,1),t1(:,2),'r.','markersize',msize)
plot([t2(:,1);t2(1,1)],[t2(:,2);t2(1,2)],'k','linewidth',lwidth)
plot(t2(:,1),t2(:,2),'b.','markersize',msize)
DT = .2;
for iii = 1:size(t1,1)
    tmp = text(t1(iii,1)+DT,t1(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[1 0 0])
    tmp = text(t2(iii,1)+DT,t2(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[0 0 1])
end
axis equal
MINx = min(min(t1(:,1)),min(t2(:,1)));
MINy = min(min(t1(:,2)),min(t2(:,2)));
MAXx = max(max(t1(:,1)),max(t2(:,1)));
MAXy = max(max(t1(:,2)),max(t2(:,2)));
DM  = 0.1*max([abs(MAXx),abs(MINx),abs(MAXy),abs(MINy)]);
axis([MINx-DM MAXx+DM MINy-DM MAXy+DM])

% Write image to file
print -depsc tri_orig.eps

%% Procrustes step #1: translation.
% We move the two triangles so that their center is
% at the origin.

% Compute the centers of the two triangles
center1 = mean(t1);
center2 = mean(t2);

% Subtract the centers from the two triangles
t1c     = t1-repmat(center1,3,1);
t2c     = t2-repmat(center2,3,1);

% Plot the translated triangles and the origin
figure(2)
clf
plot([t1c(:,1);t1c(1,1)],[t1c(:,2);t1c(1,2)],'k','linewidth',lwidth)
hold on
plot(t1c(:,1),t1c(:,2),'r.','markersize',msize)
plot([t2c(:,1);t2c(1,1)],[t2c(:,2);t2c(1,2)],'k','linewidth',lwidth)
plot(t2c(:,1),t2c(:,2),'b.','markersize',msize)
DT = .2;
for iii = 1:size(t1,1)
    tmp = text(t1c(iii,1)+DT,t1c(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[1 0 0])
    tmp = text(t2c(iii,1)+DT,t2c(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[0 0 1])
end
plot(0,0,'kp','markersize',msize)
axis equal
MINx = min(min(t1c(:,1)),min(t2c(:,1)));
MINy = min(min(t1c(:,2)),min(t2c(:,2)));
MAXx = max(max(t1c(:,1)),max(t2c(:,1)));
MAXy = max(max(t1c(:,2)),max(t2c(:,2)));
DM  = 0.1*max([abs(MAXx),abs(MINx),abs(MAXy),abs(MINy)]);
axis([MINx-DM MAXx+DM MINy-DM MAXy+DM])

% Write image to file
print -depsc tri_center.eps

%% Procrustes step #2: scaling.
% We multiply the two triangles with scalars
% so that they have unit size

% Compute the scaling coefficients for the two triangles
scale1 = 1/norm(t1c(:));
scale2 = 1/norm(t2c(:));

% Subtract the centers from the two triangles
t1cs = scale1*t1c;
t2cs = scale2*t2c;

% Plot the translated triangles and the origin
figure(3)
clf
plot([t1cs(:,1);t1cs(1,1)],[t1cs(:,2);t1cs(1,2)],'k','linewidth',lwidth)
hold on
plot(t1cs(:,1),t1cs(:,2),'r.','markersize',msize)
plot([t2cs(:,1);t2cs(1,1)],[t2cs(:,2);t2cs(1,2)],'k','linewidth',lwidth)
plot(t2cs(:,1),t2cs(:,2),'b.','markersize',msize)
DT = .02;
for iii = 1:size(t1,1)
    tmp = text(t1cs(iii,1)+DT,t1cs(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[1 0 0])
    tmp = text(t2cs(iii,1)+DT,t2cs(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[0 0 1])
end
plot(0,0,'kp','markersize',msize)
axis equal
MINx = min(min(t1cs(:,1)),min(t2cs(:,1)));
MINy = min(min(t1cs(:,2)),min(t2cs(:,2)));
MAXx = max(max(t1cs(:,1)),max(t2cs(:,1)));
MAXy = max(max(t1cs(:,2)),max(t2cs(:,2)));
DM  = 0.1*max([abs(MAXx),abs(MINx),abs(MAXy),abs(MINy)]);
axis([MINx-DM MAXx+DM MINy-DM MAXy+DM])

% Write image to file
print -depsc tri_center_scaled.eps

%% Procrustes step #3: orientation.
% We rotate triangle two and find out which angle gives
% the best fit. This is a "brute-force" approach.

% Angles of rotation
Nfii   = 128;
fiivec = [0:(Nfii-1)]*2*pi/Nfii;

% Initialize fit vector
fitvec = zeros(size(fiivec));

% Loop over all angles
for iii = 1:Nfii
    fii = fiivec(iii);
    
    % Rotate triangle two
    column1 = [cos(fii)*t2cs(:,1)-sin(fii)*t2cs(:,2)];
    column2 = [sin(fii)*t2cs(:,1)+cos(fii)*t2cs(:,2)];
    t2csr   = [column1,column2];
    
    % Measure how well do the triangles fit together
    tmp = t1cs-t2csr;
    fitvec(iii) = norm(tmp(:));
end

% Pick out the optimal angle
fii = fiivec(fitvec==min(fitvec));

% Rotate triangle two to optimal angle
column1 = [cos(fii)*t2cs(:,1)-sin(fii)*t2cs(:,2)];
column2 = [sin(fii)*t2cs(:,1)+cos(fii)*t2cs(:,2)];
t2csr   = [column1,column2];

% Measure how well do the triangles fit together
tmp = t1cs-t2csr;
fit = norm(tmp(:));

% Show the fit as function of rotation angle
figure(4)
clf
plot(fiivec,fitvec)


figure(5)
clf
plot([t1cs(:,1);t1cs(1,1)],[t1cs(:,2);t1cs(1,2)],'k','linewidth',lwidth)
hold on
plot(t1cs(:,1),t1cs(:,2),'r.','markersize',msize)
plot([t2csr(:,1);t2csr(1,1)],[t2csr(:,2);t2csr(1,2)],'k','linewidth',lwidth)
plot(t2csr(:,1),t2csr(:,2),'b.','markersize',msize)
DT = .02;
for iii = 1:size(t1,1)
    tmp = text(t1cs(iii,1)+DT,t1cs(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[1 0 0])
    tmp = text(t2csr(iii,1)+DT,t2csr(iii,2)+DT,num2str(iii),'fontsize',fsize);
    set(tmp,'color',[0 0 1])
end
plot(0,0,'kp','markersize',msize)
title(['Goodness of fit: ',num2str(fit,'%2.2f')],'fontsize',fsize)
axis equal
MINx = min(min(t1cs(:,1)),min(t2csr(:,1)));
MINy = min(min(t1cs(:,2)),min(t2csr(:,2)));
MAXx = max(max(t1cs(:,1)),max(t2csr(:,1)));
MAXy = max(max(t1cs(:,2)),max(t2csr(:,2)));
DM  = 0.1*max([abs(MAXx),abs(MINx),abs(MAXy),abs(MINy)]);
axis([MINx-DM MAXx+DM MINy-DM MAXy+DM])

% Write image to file
print -depsc tri_optimal.eps
