% Hull Reconstruction Motion Tracking (HRMT)
% Written by Leif Ristroph and Gordon Berman
% Started November 2006, core HRMT code completed January 2008

% This program extracts centroid and orientation information for the body
% and wings of a flying insect. The code takes in .bmp image files
% corresponding to three orthogonal cameras that capture the silhouette of
% the insect. The image files, as well as background image files, must be
% put together in a single file folder. The program then asks the user to
% enter this folder name. The program then runs HRMT on the images, and
% outputs three figures: 1) a figure with the original images along with
% the aligned images that with the original bounding boxes used in image
% registration; 2) the final aligned shadows produced by image processing;
% 3) a 3D hull reconstruction of the insect along with the vectors
% representing the body vectors A and L and the wing vectors S and C. (See
% text for the definition of these vectors.) In additon, these vectors
% originate from the centroid of each the body, right wing, and left wing.
% Finally, all 18 coordinates are contained in the vector variable
% "stroke": body x, body y, body z, psi, beta, rho, right x, right y, right
% z, right phi, right theta, right eta, left x, left y, left z, left phi,
% left theta, left eta. (See text for definitions of these coordinates.)

% **************************************************
% Get user inputs and set up related variables
% **************************************************

numCameras = 3;
cameras(1:3) = true; % indicates which cameras you have: XY, XZ, YZ
%base_file = input('Base file name: ', 's');
base_file = pwd;
if length(find(base_file=='/')) > 0
    fileNameXY = [base_file '/xy'];
    fileNameXZ = [base_file '/xz'];
    fileNameYZ = [base_file '/yz'];
else
    fileNameXY = [base_file '\xy'];
    fileNameXZ = [base_file '\xz'];
    fileNameYZ = [base_file '\yz'];
end
    
% Get the backgorund image and image to be analyzed
backgroundNumber = 1;
frameNumber = 2;

% **************************************************
% Prepare files to be read in
% **************************************************

% read in the background image files
backgroundXY = imread([fileNameXY int2str(backgroundNumber) '.bmp']);
backgroundXZ = imread([fileNameXZ int2str(backgroundNumber) '.bmp']);
backgroundYZ = imread([fileNameYZ int2str(backgroundNumber) '.bmp']);

% through out color information and make a background cell
backgroundXY = backgroundXY(:,:,1);
backgroundXZ = backgroundXZ(:,:,1);
backgroundYZ = backgroundYZ(:,:,1);

background{1} = backgroundXY;
background{2} = backgroundXZ;
background{3} = backgroundYZ;

% **************************************************
% This loop reads in the image files and thresholds
% **************************************************

% Make frame file name strings for all cameras
    
frameNameXY = [fileNameXY int2str(frameNumber) '.bmp'];
frameNameXZ = [fileNameXZ int2str(frameNumber) '.bmp'];
frameNameYZ = [fileNameYZ int2str(frameNumber) '.bmp'];
    
frameName{1} = frameNameXY;
frameName{2} = frameNameXZ;
frameName{3} = frameNameYZ;    
    
% These raw images have the origin at the upper-left corner, as is the
% MATLAB convention. Throw out the redundancy in the image file associated
% with color values.

% This loop thresholds the images and removes background noise to
% isolate the fly shadow
noiseTolerence = 12;
minBodyArea = 1500;
for i = 1:3
    if cameras(i) == true
        % image processing: a nice little algorithm for isolating the
        % fly even in the presence of junk in the background! ("masking" technique)
        rawColorImage = imread(frameName{i});
       	numPixels = size(rawColorImage,1);
       	rawImage = rawColorImage(:,:,1); % get rid of color redundancy        
       	PixelCheck = imsubtract(background{i}, rawImage); % high intensity where the fly is
       	PixelCheck = imsubtract(PixelCheck, noiseTolerence); % gets rid of noise by uniform subtraction
       	FlyPixel = immultiply(PixelCheck, 256); % essentially makes fly white, background black
       	BlackFlyPixel = imcomplement(FlyPixel); % makes fly black on white background
       	flyOnlyRaw{i} = imadd(rawImage,BlackFlyPixel); % uses above image as a "mask" to apply to the rawImage
       	WhiteFlyPixel = imcomplement(flyOnlyRaw{i}); % inverts this to get light gray fly on black background
       	WhiteFlyBW{i} = im2bw(immultiply(WhiteFlyPixel,256),0.5); % Thresholds to get a pure white fly
    end % end if loop that checks whether cameras exist
end % end for loop over cameras

% corrects y coordinate in first camera to match with third camera
% flips both z coordinates to have z be increasing upward
WhiteFlyBW{1} = flipud(WhiteFlyBW{1});
WhiteFlyBW{2} = flipud(WhiteFlyBW{2});
WhiteFlyBW{3} = flipud(WhiteFlyBW{3});

% **************************************************
% Aligns the cameras views by shifting images
% **************************************************

for cam =1:3
    [labelFly, numObject] = bwlabel(WhiteFlyBW{cam},4);
    Blobs = regionprops(labelFly, 'basic');       
    maxBlobArea = max([Blobs.Area]);
    FlyShadow = find([Blobs.Area]== maxBlobArea); % find fly shadow as biggest thing around
    col(cam) = Blobs(FlyShadow).BoundingBox(1); % find its bounding box
    row(cam) = Blobs(FlyShadow).BoundingBox(2); 
    dCol(cam) = Blobs(FlyShadow).BoundingBox(3); 
    dRow(cam) = Blobs(FlyShadow).BoundingBox(4);
end

% find appropriate shifts and scales in each coordinate that makes images
% overlap
shiftX = col(1) - col(2);
scaleX = dCol(1)./dCol(2);
shiftY = row(1) - col(3);
shiftZ = row(2) - row(3);
scaleY = dRow(1)./dRow(3);
scaleZ = dCol(2)./dCol(3);

% Shift the images according to the alignment offsets found from the
% samples

temp = zeros(numPixels,numPixels);
dims = [numPixels numPixels];

for cam =1:3
    pic = WhiteFlyBW{cam};
    if cam == 1
        temp = pic;
    end
    if cam == 2
        offset = [0 shiftX]; % [dY dX]
        offset = mod(-offset,dims(1:2));
        temp(:,:) = [pic(offset(1)+1:dims(1), offset(2)+1:dims(2)), ...
                     pic(offset(1)+1:dims(1), 1:offset(2));  ...
                     pic(1:offset(1), offset(2)+1:dims(2)), ...
                     pic(1:offset(1), 1:offset(2)) ];
    end
    if cam == 3
        offset = [shiftZ shiftY];
        offset = mod(-offset,dims(1:2));
        temp(:,:) = [pic(offset(1)+1:dims(1), offset(2)+1:dims(2)),  ...
                     pic(offset(1)+1:dims(1), 1:offset(2)); ...
                     pic(1:offset(1), offset(2)+1:dims(2)), ...
                     pic(1:offset(1), 1:offset(2)) ];
    end
    WhiteFlyBW{cam} = temp;
end

% forms black flies to be used in hull construction
for cam = 1:3
    temp = zeros(numPixels,numPixels);
    [labelFly, numObject] = bwlabel(WhiteFlyBW{cam},4);
    Blobs = regionprops(labelFly, 'Area', 'PixelList');       
    maxBlobArea = max([Blobs.Area]);
    FlyShadow = find([Blobs.Area]== maxBlobArea); % find fly shadow as biggest thing around
    pixels = Blobs(FlyShadow).PixelList;
    for dum = 1:size(pixels,1)
        temp(pixels(dum,2),pixels(dum,1)) = WhiteFlyBW{cam}(pixels(dum,2),pixels(dum,1));
    end
    CleanWhiteFly{cam} = temp;
    CleanBlackFly{cam} = imcomplement(CleanWhiteFly{cam});
end

% plot original images along with thresholded images overlayed with red bounding box
for i=1:3
    rawColorImg = imread(frameName{i});
    rawImg{i} = rawColorImg(:,:,1);
end
figure(1), hold on 
subplot(2,3,1), imshow(rawImg{1}), axis on
subplot(2,3,2), imshow(rawImg{2}), axis on
subplot(2,3,3), imshow(rawImg{3}), axis on
subplot(2,3,4), imshow(WhiteFlyBW{1}), axis on, hold on
line([col(1),col(1)+dCol(1)],[row(1),row(1)],'Color','r');
line([col(1),col(1)+dCol(1)],[row(1)+dRow(1),row(1)+dRow(1)],'Color','r');
line([col(1),col(1)],[row(1),row(1)+dRow(1)],'Color','r');
line([col(1)+dCol(1),col(1)+dCol(1)],[row(1),row(1)+dRow(1)],'Color','r');
subplot(2,3,5), imshow(WhiteFlyBW{2}), axis on, hold on
line([col(2),col(2)+dCol(2)],[row(2),row(2)],'Color','r');
line([col(2),col(2)+dCol(2)],[row(2)+dRow(2),row(2)+dRow(2)],'Color','r');
line([col(2),col(2)],[row(2),row(2)+dRow(2)],'Color','r');
line([col(2)+dCol(2),col(2)+dCol(2)],[row(2),row(2)+dRow(2)],'Color','r');
subplot(2,3,6), imshow(WhiteFlyBW{3}), axis on, hold on
line([col(3),col(3)+dCol(3)],[row(3),row(3)],'Color','r');
line([col(3),col(3)+dCol(3)],[row(3)+dRow(3),row(3)+dRow(3)],'Color','r');
line([col(3),col(3)],[row(3),row(3)+dRow(3)],'Color','r');
line([col(3)+dCol(3),col(3)+dCol(3)],[row(3),row(3)+dRow(3)],'Color','r');

% plot final aligned and clean images
figure(2), subplot(1,3,1), imshow(CleanBlackFly{1}), axis on
figure(2), subplot(1,3,2), imshow(CleanBlackFly{2}), axis on
figure(2), subplot(1,3,3), imshow(CleanBlackFly{3}), axis on

% **************************************************
% Find the visual hull of the insect and partition it up
% **************************************************

    % Get the visual hull construction parameters to be used
    voxelSize = 2;
    Q = 2;
    Qepsilon = 2;

    if mod(log(voxelSize)/log(2),1) > .0001
        voxelSize = 2^floor(log(voxelSize)/log(2)); 
    end 
    numVoxels = numPixels/voxelSize;

    totalimage{1} = CleanBlackFly{1};
    totalimage{2} = CleanBlackFly{2};
    totalimage{3} = CleanBlackFly{3};

    % finding search space for reconstruction
    for cam =1:3
        [labelFly, numObject] = bwlabel(WhiteFlyBW{cam},4);
        Blobs = regionprops(labelFly, 'Basic');       
        maxBlobArea = max([Blobs.Area]);
        FlyShadow = find([Blobs.Area]== maxBlobArea); % find fly shadow as biggest thing around
        col(cam) = Blobs(FlyShadow).BoundingBox(1); % find its bounding box
        row(cam) = Blobs(FlyShadow).BoundingBox(2); 
        dCol(cam) = Blobs(FlyShadow).BoundingBox(3); 
        dRow(cam) = Blobs(FlyShadow).BoundingBox(4);
    end
    % bounds from XY view
    xminXY = col(1);
    yminXY = row(1);
    xmaxXY = col(1)+dCol(1);
    ymaxXY = row(1)+dRow(1);
    % bounds from XZ view
    xminXZ = col(2);
    zminXZ = row(2);
    xmaxXZ = col(2)+dCol(2);
    zmaxXZ = row(2)+dRow(2);
    % bounds from YZ view
    yminYZ = col(3);
    zminYZ = row(3);
    ymaxYZ = col(3)+dCol(3);
    zmaxYZ = row(3)+dRow(3);

    xmin = min([xminXY,xminXZ]); xmax = max([xmaxXY,xmaxXZ]);
    ymin = min([yminXY,yminYZ]); ymax = max([ymaxXY,ymaxYZ]);
    zmin = min([zminXZ,zminYZ]); zmax = max([zmaxXZ,zmaxYZ]);
    imin = floor(xmin / voxelSize);
    jmin = floor(ymin / voxelSize);
    kmin = floor(zmin / voxelSize);
    imax = ceil(xmax / voxelSize);  
    jmax = ceil(ymax / voxelSize);
    kmax = ceil(zmax / voxelSize);
    
    % implementation of hull reconstruction code
    BodyVoxels = [];
    for i=imin:imax
        for j=jmin:jmax
            for k=kmin:kmax

                testCount = [0 0 0];
                for l=1:Q
            
                    randX = (i-1)*voxelSize + floor(rand*voxelSize+1);
                    randY = (j-1)*voxelSize + floor(rand*voxelSize+1);  
                    randZ = (k-1)*voxelSize + floor(rand*voxelSize+1);
            
                    if totalimage{1}(randY,randX) == 0
                        testCount(1) = testCount(1) + 1;
                    end
                    if totalimage{2}(randZ,randX) == 0
                        testCount(2) = testCount(2) + 1;
                    end
                    if totalimage{3}(randZ,randY) == 0
                        testCount(3) = testCount(3) + 1;
                    end
                end
        
                if min(testCount) >= Qepsilon
                    BodyVoxels(size(BodyVoxels,1)+1,:) = [i,j,k];
                end
                
            end
        end
    end

    % Clustering: separates the entire body and wings blob into 4 sections 
    k = 4;
    replicates = 4;
    maxiter = 100;
    total = [];
    total = BodyVoxels;
    
    [Y,C] = kmeans(total,k,'maxiter',maxiter,'replicates',replicates); 
    centers(1:k,1:3) = C;
    groupings(1:size(total,1)) = Y; % this is the labels for all the voxels
    voxels(1:size(total,1),1:3) = total; % this is the actual voxels as points (x,y,z)
    lengths = size(total,1); % this is the number of points

% clear unnecessary variables
clear CleanBlackFly CleanWhiteFly WhiteFlyBW flyOnlyRaw
pack

% initialize the 12 coordinate variables (all in lab frame)
xs = []; % body center of mass
ys = [];
zs = [];
psis = []; % body Euler angles
betas = [];
rhos = [];
x1s = []; % wing 1 center of mass
y1s = [];
z1s = [];
phi1s =[]; % right wing Euler angles
theta1s = [];
eta1s = [];
x2s = []; % wing 2 center of mass
y2s = [];
z2s = [];
phi2s =[]; % left wing Euler angles
theta2s = [];
eta2s = [];

% **************************************************
% Coordinate extraction: dissects the fly and finds the relevant coordinates
% **************************************************
   
    % **************************************************
    % dissect the whole fly into body and wings
    % **************************************************
    
    % peel off the zeros on the end of the groupings list
    Y =[];
    for j=1:lengths 
        Y(j) = groupings(j); 
    end
    
    % count up how big each cluster is
    counts = [0,0,0,0];
    for j=1:lengths
        counts(Y(j)) = counts(Y(j)) + 1;
    end
    r = sort(counts); % ascending order
    a = find(counts==r(4)); % the two body clusters are the big ones
    if length(a) > 1
        b = a(2);
        a = a(1); 
    else
        b = find(counts==r(3));
    end
    c = find(counts==r(2));
    if length(c) > 1
        d = c(2);
        c = c(1);
    else
        d = find(counts==r(1));
    end
    
    % combine the two body clusters and also get wing clusters separately
    bodyVoxels = [];
    wing1VoxelsTemp = [];
    wing2VoxelsTemp = [];
    for k=1:lengths
        if (Y(k) == a) || (Y(k) == b) % finds body as the big clusters
            bodyVoxels(size(bodyVoxels,1)+1,:) = voxels(k,:);
        elseif (Y(k) == c) % one wing
            wing1VoxelsTemp(size(wing1VoxelsTemp,1)+1,:) = voxels(k,:);
        elseif (Y(k) == d) % other wing
            wing2VoxelsTemp(size(wing2VoxelsTemp,1)+1,:) = voxels(k,:);
        end
    end
    
    % **************************************************
    % Tracking of body
    % **************************************************
    
    % find centroid of the body as means of voxel coordinates
    cBody = [mean(bodyVoxels(:,1)),mean(bodyVoxels(:,2)),mean(bodyVoxels(:,3))]; 
    xs = cBody(1);
    ys = cBody(2);
    zs = cBody(3);
    
    Coeffs = princomp(bodyVoxels); % decreasing variance of principal axes
    rollHat = Coeffs(:,1); % 1st axis = roll axis
    % make roll axis have positive z-component (meaning it points upward)
    if dot( rollHat, [0,0,1] ) < 0
        rollHat = -rollHat;
    end
    
	% psi is measured relative to the x-axis
    psi = atan2(rollHat(2),rollHat(1));
    % beta is measured between body axis vector (rollHat is called A in
	% text) and the horizontal plane
    beta = asin(rollHat(3));

    rhoProj = [rollHat(1),rollHat(2),0];
    rhoProj = rhoProj / norm(rhoProj);
    psiHat = [-sin(psi),cos(psi),0];
    betaHat = -rhoProj*sin(beta) + [0,0,1]*cos(beta);
   
    psis = psi;
    betas = beta;
            
    % **************************************************
    % Find the roll angle of the body
    % **************************************************
    % Get roll angle from position of head, thorax, abdomen (See text.)
    
    % perform clustering on body to get three body parts
    [groups,C] = kmeans(bodyVoxels,3,'maxiter',500,'replicates',4);
        
    part1 = bodyVoxels(find(groups==1),:);
    part2 = bodyVoxels(find(groups==2),:);
    part3 = bodyVoxels(find(groups==3),:);
            
    cpart1 = [mean(part1(:,1)),mean(part1(:,2)),mean(part1(:,3))];
    cpart2 = [mean(part2(:,1)),mean(part2(:,2)),mean(part2(:,3))];
    cpart3 = [mean(part3(:,1)),mean(part3(:,2)),mean(part3(:,3))];
    
    % get norm to rollHat that's in xy-plane
    planeRoll = cross( [0 0 1] , rollHat );
    planeRoll = planeRoll / norm(planeRoll);
    if dot( cross(planeRoll,rollHat) , [0 0 1] ) > 0
        planeRoll = -planeRoll;
    end
    thirdHat = cross(rollHat,planeRoll);
    thirdHat = thirdHat/norm(thirdHat);
    
    % get normal to "roll plane" of body
    normRoll = cross( cpart1 - cpart2 , cpart1 - cpart3 );
    normRoll = normRoll - dot(normRoll,rollHat)*rollHat';
    normRoll = normRoll / norm(normRoll);
    if dot(normRoll,thirdHat) < 0
        normRoll = -normRoll;
    end   
    
    % form roll angle, rho, as the angle between two vectors: 0 < rho < pi
    rho = atan2( norm(cross( normRoll , planeRoll )) , dot( normRoll , planeRoll ) );
    
	rhos = rho;
        
    % **************************************************
    % Identify which wing is left/right, make right = 1, left = 2
    % **************************************************
    
    wing1Voxels = [];
    wing2Voxels = [];
    c1 = [mean(wing1VoxelsTemp(:,1)),mean(wing1VoxelsTemp(:,2)),mean(wing1VoxelsTemp(:,3))];
    c2 = [mean(wing2VoxelsTemp(:,1)),mean(wing2VoxelsTemp(:,2)),mean(wing2VoxelsTemp(:,3))];
    if dot( cross( c1'-cBody' , rollHat) , [0,0,1]) < 0
        wing1Voxels = wing2VoxelsTemp; % switches wings to get right as wing 1
        wing2Voxels = wing1VoxelsTemp;
    else
        wing1Voxels = wing1VoxelsTemp;
        wing2Voxels = wing2VoxelsTemp;
    end
    
    % **************************************************
    % Tracking of wings
    % **************************************************
    
    %
    % analyze wing 1 = right wing
    %
    c1 = [mean(wing1Voxels(:,1)),mean(wing1Voxels(:,2)),mean(wing1Voxels(:,3))];
    x1s = c1(1);
    y1s = c1(2);
    z1s = c1(3);
    prinAxes1 = princomp(wing1Voxels); % decreasing variance of principal axes
    span1Hat = prinAxes1(:,1); % 1st axis
    second1Hat = prinAxes1(:,2); % 2nd axis
    
    % makes wing span point outward
    if dot(c1-cBody,span1Hat) < 0
        span1Hat = -span1Hat;
    end
        
    % wing 1 phi and theta
    phi1 = atan2(span1Hat(2),span1Hat(1));
    theta1 = asin(span1Hat(3));
    
    % wing 1 eta
    wing1Projs = [];
    % collect up all points in the wing that near (w/in delta of) mid-span
    delta = 2.5;
    for k = 1:size(wing1Voxels,1)
        if abs(dot( wing1Voxels(k,:)' - c1' ,span1Hat)) < delta
            wing1Projs(size(wing1Projs,1)+1,:) = wing1Voxels(k,:)' - c1' - span1Hat(:)*dot(wing1Voxels(k,:)'-c1',span1Hat(:));
        end
    end
    % form matrix of distances between all projected points
    mm = size(wing1Projs,1);
	[ii jj] = find(triu(ones(mm),1));
    E = zeros(mm,mm);
    E(ii+mm*(jj-1) ) = sqrt(sum(abs(wing1Projs(ii,:) - wing1Projs(jj,:)).^2,2));
    E(jj + mm*(ii-1)) = E(ii+mm*(jj-1));
    % locates maximum distance between all projected points and calls the
    % chord the connecting vector between the points
    chordRow = []; chordCol = [];
    [chordRow chordCol] = find(E(:,:) == max(max(E)));
    chord1Hat = wing1Projs(chordRow(1),:) - wing1Projs(chordCol(1),:);
    chord1Hat = chord1Hat/norm(chord1Hat);

    phi1Proj = [span1Hat(1),span1Hat(2),0];
    phi1Proj = phi1Proj / norm(phi1Proj);
    phi1Hat = [-sin(phi1),cos(phi1),0];
    theta1Hat = -phi1Proj*sin(theta1) + [0,0,1]*cos(theta1);
    chord1Hat = abs(dot(chord1Hat,phi1Hat))*phi1Hat + abs(dot(chord1Hat,theta1Hat))*theta1Hat; % chord vector
    eta1 = atan2( abs(dot(chord1Hat,theta1Hat)), abs(dot(chord1Hat,phi1Hat))); % in the first quadrant only
    
    %
    % analyze wing 2 = left wing
    %
    c2 = [mean(wing2Voxels(:,1)),mean(wing2Voxels(:,2)),mean(wing2Voxels(:,3))];
    x2s = c2(1);
    y2s = c2(2);
    z2s = c2(3);
    prinAxes2 = princomp(wing2Voxels); % decreasing variance of principal axes
    span2Hat = prinAxes2(:,1); % 1st axis
    second2Hat = prinAxes2(:,2); % 2nd axis
    
    % makes wing span point outward
    if dot(c2-cBody,span2Hat) < 0
        span2Hat = -span2Hat;
    end
    
    % wing 2 phi and theta
    phi2 = atan2(span2Hat(2),span2Hat(1));
    theta2 = asin(span2Hat(3));
    
    % wing 2 eta
    wing2Projs = [];
    % collect up all points in the wing that near (w/in delta of) mid-span
    for k = 1:size(wing2Voxels,1)
        if abs(dot( wing2Voxels(k,:)' - c2' ,span2Hat)) < delta
            wing2Projs(size(wing2Projs,1)+1,:) = wing2Voxels(k,:)' - c2' - span2Hat(:)*dot(wing2Voxels(k,:)'-c2',span2Hat(:));
        end
    end
    % form matrix of distances between all projected points
    mm = size(wing2Projs,1);
	[ii jj] = find(triu(ones(mm),1));
    E = zeros(mm,mm);
    E(ii+mm*(jj-1) ) = sqrt(sum(abs(wing2Projs(ii,:) - wing2Projs(jj,:)).^2,2));
    E(jj + mm*(ii-1)) = E(ii+mm*(jj-1));
    % locates maximum distance between all projected points and calls the
    % chord the connecting vector between the points
	chordRow = []; chordCol = [];    
    [chordRow chordCol] = find(E(:,:) == max(max(E)));
    chord2Hat = wing2Projs(chordRow(1),:) - wing2Projs(chordCol(1),:);
    chord2Hat = chord2Hat/norm(chord2Hat);
    
    phi2Proj = [span2Hat(1),span2Hat(2),0];
    phi2Proj = phi2Proj / norm(phi2Proj);
    phi2Hat = [-sin(phi2),cos(phi2),0];
    theta2Hat = -phi2Proj*sin(theta2) + [0,0,1]*cos(theta2);
    chord2Hat = -abs(dot(chord2Hat,phi2Hat))*phi2Hat + abs(dot(chord2Hat,theta2Hat))*theta2Hat; % chord vector
    eta2 = atan2( abs(dot(chord2Hat,theta2Hat)), abs(dot(chord2Hat,phi2Hat))); % in the first quadrant only
    
    % **************************************************
    % Compile all wing angles
    % **************************************************
    phi1s = phi1;
    theta1s = theta1;
    eta1s = eta1;
    
    phi2s = phi2;
    theta2s = theta2;
    eta2s = eta2;
    
    % **************************************************
    % Store variables for display later
    % **************************************************
    
    bodyPoints = bodyVoxels;
    wing1Points = wing1Voxels;
    wing2Points = wing2Voxels;
    
    rollHats(1:3) = rollHat;
    normRolls(1:3) = normRoll;
    psiHats(1:3) = psiHat;
    
    span1Hats(1:3) = span1Hat;
    chord1Hats(1:3) = chord1Hat;
    phi1Hats(1:3) = phi1Hat;
    
    span2Hats(1:3) = span2Hat;
    chord2Hats(1:3) = chord2Hat;
    phi2Hats(1:3) = phi2Hat;

pack

% form stroke vector that collects up all coordinates tracked
stroke(1) = xs;
stroke(2) = ys;
stroke(3) = zs;
stroke(4) = (180/pi)*psis;
stroke(5) = (180/pi)*betas;
stroke(6) = (180/pi)*rhos;

stroke(7) = x1s;
stroke(8) = y1s;
stroke(9) = z1s;
stroke(10) = (180/pi)*phi1s;
stroke(11) = (180/pi)*theta1s;
stroke(12) = (180/pi)*eta1s;

stroke(13) = x2s;
stroke(14) = y2s;
stroke(15) = z2s;
stroke(16) = (180/pi)*phi2s;
stroke(17) = (180/pi)*theta2s;
stroke(18) = (180/pi)*eta2s;

% **************************************************
% Plots to check the hull reconstruction and coordinate vectors
% **************************************************

figure(3), hold on, cla
plot3(bodyPoints(:,1),bodyPoints(:,2),bodyPoints(:,3),'c.'), hold on, axis equal

scale = 3;
cb = [mean(bodyPoints(:,1)),mean(bodyPoints(:,2)),mean(bodyPoints(:,3))];
rh = line([cb(1),cb(1)+scale*15*rollHats(1)],[cb(2),cb(2)+scale*15*rollHats(2)],[cb(3),cb(3)+scale*15*rollHats(3)],'Color','r','LineWidth',8);
nr = line([cb(1),cb(1)+scale*7*normRolls(1)],[cb(2),cb(2)+scale*7*normRolls(2)],[cb(3),cb(3)+scale*7*normRolls(3)],'Color','g','LineWidth',8);
ph = line([cb(1),cb(1)+scale*7*psiHats(1)],[cb(2),cb(2)+scale*7*psiHats(2)],[cb(3),cb(3)+scale*7*psiHats(3)],'Color','r','LineWidth',8);
    
plot3(wing1Points(:,1),wing1Points(:,2),wing1Points(:,3),'m.'), hold on, axis equal
cr = [mean(wing1Points(:,1)),mean(wing1Points(:,2)),mean(wing1Points(:,3))];
s1 = line([cr(1),cr(1)+scale*10*span1Hats(1)],[cr(2),cr(2)+scale*10*span1Hats(2)],[cr(3),cr(3)+scale*10*span1Hats(3)],'Color','r','LineWidth',4);
c1 = line([cr(1),cr(1)+scale*8*chord1Hats(1)],[cr(2),cr(2)+scale*8*chord1Hats(2)],[cr(3),cr(3)+scale*8*chord1Hats(3)],'Color','g','LineWidth',4);
p1h = line([cr(1),cr(1)+scale*8*phi1Hats(1)],[cr(2),cr(2)+scale*8*phi1Hats(2)],[cr(3),cr(3)+scale*8*phi1Hats(3)],'Color','r','LineWidth',4);
    
plot3(wing2Points(:,1),wing2Points(:,2),wing2Points(:,3),'b.'), hold on, axis equal
cl = [mean(wing2Points(:,1)),mean(wing2Points(:,2)),mean(wing2Points(:,3))];
s2 = line([cl(1),cl(1)+scale*10*span2Hats(1)],[cl(2),cl(2)+scale*10*span2Hats(2)],[cl(3),cl(3)+scale*10*span2Hats(3)],'Color','r','LineWidth',4);
c2 = line([cl(1),cl(1)+scale*8*chord2Hats(1)],[cl(2),cl(2)+scale*8*chord2Hats(2)],[cl(3),cl(3)+scale*8*chord2Hats(3)],'Color','g','LineWidth',4);
p2h = line([cl(1),cl(1)+scale*8*phi2Hats(1)],[cl(2),cl(2)+scale*8*phi2Hats(2)],[cl(3),cl(3)+scale*8*phi2Hats(3)],'Color','r','LineWidth',4);
    
rotate3d on