%Fourier fit of motion

clear all
figure(1)
clf
set(gcf,'position',[50,600,100,75])
aa = 6;
axis([-aa aa -aa aa])
axis equal
set(gcf,'doublebuffer', 'on')

figure(2)
clf
set(gcf,'position',[50,370,100,75])
set(gcf,'doublebuffer', 'on')

figure(3)
clf
set(gcf,'position',[50,50,200,150])
axis([0 100 0 57])
set(gcf,'doublebuffer', 'on')

%number of Fourier components to use
%vary this to make a better fit
N = 2;

%time
times=0:.03:1;

for time=times
    
    %make an animated rectangle
    %note that you need my graphics library to run this. See:
    % http://www.nbb.cornell.edu/neurobio/land/PROJECTS/Hierarchy/
    figure(1)
    cla;axis([-aa aa -aa aa]);axis off
    r1 = scale(UnitSquare,2,.5,1);
    r1.facecolor='k';
    r2 = translate(rotateZ(translate(r1,2,0,0),80*sin(2*pi*time)),1,0,0);
    r2.facecolor='k';
    worm = combine(r1,r2);
    renderpatch(worm);
    %and then get the resulting image
    im = getframe(gcf);
    im = im.cdata;
    
    %find the edge pixels in the image
    fconv = ones(3,3);
    ed = filter2(fconv,im2bw(im,0.5),'valid');
    ed = (ed>=3) & (ed<=5);
    figure(2)
    imshow(ed)
    
    %order the edge pixels
    %find them in the array
    [xpos,ypos] = find(ed==1);
    npts = length(xpos);
    %keep track of visit order
    ordering=[1:npts] * 0;
    visited=[1:npts] * 0;
    %pick the first point on the edge and calculate the normal
    %move the next pixel which is: 
    %--on the edge
    %--closeest
    %now just move to the nearest pixel which has not been visited
    nextindex=1; ordering(1)=1; visited(1)=1;
    count = 1;
    while (count<npts)
        currentindex=nextindex;
        currentmin=inf;
        for i=1:npts
            d = (xpos(currentindex)-xpos(i)).^2 + (ypos(currentindex)-ypos(i)).^2 ;
            if (d<currentmin) & (visited(i)==0)
                currentmin = d;
                nextindex = i;
            end
        end
        %at this point, we have the index to the nearest point
        visited(nextindex)=1;
        count = count + 1;
        ordering(count)=nextindex;
    end
     
    %define a piece-wise linear, closed curve
    clear p
    p(:,1) = ypos(ordering);
    p(:,2) = xpos(ordering);
    K = length(p);
    %close the loop
    p(K+1,:) = p(1,:);
    
    %get fourier transform of the closed curve
    %Ghosh and Jain, IEEE conputer graphics and appl, 1994
    dx = diff(p(:,1));
    dy = diff(p(:,2));
    dL = sqrt( dx.^2 + dy.^2 );
    L = sum(dL);
    Lcum = cumsum(dL);
    
    %get a0, the x-offset
    temp = p(1,1)*Lcum(1)+ dx(1)*Lcum(1)^2/dL(1)*0.5; %for i=1
    for i=2:K  
        temp = temp + ...
            (p(i,1)-dx(i)*Lcum(i-1)/dL(i))*(Lcum(i)-Lcum(i-1)) + ...
            0.5*dx(i)/dL(i)*(Lcum(i)^2 - Lcum(i-1)^2);
    end
    a0=temp/L;
    
    %get c0, the y-offset
    temp = p(1,2)*Lcum(1)+ dy(1)*Lcum(1)^2/dL(1)*0.5; %for i=1
    for i=2:K  
        temp = temp + ...
            (p(i,2)-dy(i)*Lcum(i-1)/dL(i))*(Lcum(i)-Lcum(i-1)) + ...
            0.5*dy(i)/dL(i)*(Lcum(i)^2 - Lcum(i-1)^2);
    end
    c0=temp/L;
    
    for n=1:N
        tempa = dx(1)/dL(1)*(cos(2*pi*n*Lcum(1)/L)-1);
        tempb = dx(1)/dL(1)*sin(2*pi*n*Lcum(1)/L);
        tempc = dy(1)/dL(1)*(cos(2*pi*n*Lcum(1)/L)-1);
        tempd = dy(1)/dL(1)*sin(2*pi*n*Lcum(1)/L);
        for i=2:K
            tempa = tempa + ...
                dx(i)/dL(i)*(cos(2*pi*n*Lcum(i)/L)-cos(2*pi*n*Lcum(i-1)/L));
            tempb = tempb + ...
                dx(i)/dL(i)*(sin(2*pi*n*Lcum(i)/L)-sin(2*pi*n*Lcum(i-1)/L)); 
            tempc = tempc + ...
                dy(i)/dL(i)*(cos(2*pi*n*Lcum(i)/L)-cos(2*pi*n*Lcum(i-1)/L));
            tempd = tempd + ...
                dy(i)/dL(i)*(sin(2*pi*n*Lcum(i)/L)-sin(2*pi*n*Lcum(i-1)/L));  
        end
        a(n) = tempa * L /(2*pi^2*n^2);
        b(n) = tempb * L /(2*pi^2*n^2);
        c(n) = tempc * L /(2*pi^2*n^2);
        d(n) = tempd * L /(2*pi^2*n^2);
    end
    
    %draw the reconstruction
    figure(3)
    cla
    tt=0:.05:1;
    for t=tt
        x = a0 + sum(a.*cos(2*pi*(1:N)*t) + b.*sin(2*pi*(1:N)*t));
        y = c0 + sum(c.*cos(2*pi*(1:N)*t) + d.*sin(2*pi*(1:N)*t));
        line(x,57-y,'markersize',5,'marker','x')
    end
    %57-y is needed because of inversion in image axis
    line(a0,57-c0,'markersize',5,'marker','x','markeredgecolor','red')
    line(sum(ypos)/K, 57-sum(xpos)/K,...
        'markersize',5,'marker','x','markeredgecolor','k')
    drawnow
    
end




