% Test code to try to produce regular grids from vertices in obj format files.
% This one is for the F117 (only because the choice of angles
% theta, phi is tailored to this geometry) but should be more generally 
% adaptable.

% Note that at the moment, as z is treated as a function of x,y,  this 
% doesn't properly capture the wings, beyond the point at which 
% they separate from the body.

load       f117test.obj;  
vertices = f117test;    %f117test.obj ???
x=vertices(:,1);
y=vertices(:,2);
z=vertices(:,3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
tii=min(x):0.2:max(x);     % This is initial plot. Not used subsequently?
sii=min(y):0.2:max(y);      
[XI,YI] = meshgrid(tii,sii);
ZI = griddata(x,y,z,XI,YI);
%surf(XI,YI,ZI), hold
%plot3(x,y,z,'o'), hold off

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

% Now test rotation(s) of original axes, to find best "x" direction:
% for i=1:60,  
% theta=2*pi*(i-1)/59
% c=cos(theta);s=sin(theta);
%%  yy=c*y+s*z; zz=-s*y+c*z;  
%%  plot3(x,yy,zz,'o');pause;
% xx=c*x+s*y; yy=-s*x+c*y;  
% plot(xx,yy,'o');pause;
% end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%% ROTATE COORDINATES TO ALIGN F117 ROUGHLY WITH NEW X-AXIS

% Use theta=4.1533; try rotation of y and z:
theta=4.1533;
c=cos(theta);s=sin(theta);
xnew=c*x+s*y; ynew=-s*x+c*y; 

% Try centring w.r.t. (y,z,) plane:
ynew=ynew+1.2;
znew=z+1.1;

% This is find rotation angle w.r.t. y,z to make aircaft roughly horizontal:
% for i=1:90,  
% phi=-2*pi*(i-1)/59
% c=cos(phi);s=sin(phi);
%  yy=c*ynew+s*znew; zz=-s*ynew+c*znew;  
%  plot(yy,zz,'.');pause;hold on
% end

% Use phi=-0.9585, to get new Cartesian coordinates xfit, yfit, zfit
% with respect to which F117 is aligned:
% HOWEVER, unlike x itself, xfit is not monotonic.  Doesn't seem to matter 
% because the rectangular coordinate XI is monotonic.

phi=-0.9585
c=cos(phi);s=sin(phi);
 yfit=c*ynew+s*znew; zfit=-s*ynew+c*znew;  
xfit=xnew;
figure; plot3(xfit,yfit,zfit,'.');axis image

%%%%% THIS SHOWS F117 IN NEW COORDINATES 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%% NOW REDEFINE GEOMETRY TO USE REGULAR (X,Y)-GRID:

% Choose number of points to adjust resolution:

nxpts=123  % Number of points in range (plus 3?)
nypts=100  %  ...     and transverse directions

tii=min(xfit) + (max(xfit)-min(xfit))/(nxpts-0)*[0:nxpts-1] ;
sii=min(yfit) + (max(yfit)-min(yfit))/(nypts-1)*[0:nypts-1] ;

% (We've stopped tii slightly before end of aircraft because
%  it previously gave anomalies. Could use previous version of code
%  to get round this if necessary)

% Try griddata using new coordinates:
figure;
[XI,YI] = meshgrid(tii,sii);
ZI = griddata(xfit,yfit,zfit,XI,YI);
surf(XI,YI,ZI); axis image;hold on   
plot3(xfit,yfit,zfit,'o'), hold off;
title 'graph using new rectangular X,Y grid'

% So far this is fine, but the Y coordinates are on a rectangular
%  grid so that many Z points are represented as "NaN", corresponding
% to spatial points not on the aircraft.  Must now
% redefine the grid to remove these points and create the right number
% of points on the aircraft itself.  


sx=size(XI); % This contains range and transverse dimensions
nstart=4     % (To avoid initial `point' at front of aircraft)
XII=XI(:,nstart:sx(2));
YII=YI(:,nstart:sx(2));
ZII=ZI(:,nstart:sx(2));

sxn=size(XII); % This contains new transverse and range dimensions resp.

for ix=1:sxn(2),
   % Find subset of ZII at each range with finite (not NaN) values:
   Zsub=ZII(isfinite(ZII(:,ix)), ix );
   Ysub=YII(isfinite(ZII(:,ix)), ix ); 

   % Ysub, Zsub have dimension smaller than transverse dimension sx(1), 
   % so must now interpolate to produce same number of elements:
   % Try interp1?  First use HALF the number of points, as the 
   % rest will be needed later.
 
   Ymin=min(Ysub); Ymax=max(Ysub); 
   Ytop=Ymin+((Ymax-Ymin)/(sxn(1)-1)) *[0:2:sxn(1)-1] ;
   smid=length(Ytop) ; %   should be same as sxn(1)/2
   Ztop=interp1(Ysub,Zsub,Ytop);  

   % Now replace YII,ZII at this range with Yinterp,Zinterp etc:
   YII(1:smid, ix ) = Ytop';
   ZII(1:smid, ix ) = Ztop';


% So far this defines a "hollow" aircraft, in the sense that
% at each X, the vector (Y,Z) traces a path across the top of 
% the aircraft only.   Need to add a set of point going back across 
% the bottom surface.  Shouldn't be too difficult because the lower surface 
% is modelled as flat.  
   % This must use Y values in reverse order, and linearly interpolated Z:
   % (Be careful not to have repeated Y values at edges)


YII(smid+1:sxn(1), ix)= Ytop(smid) - (Ytop(smid)-Ytop(1))/(smid+1)*[1:smid]';
ZII(smid+1:sxn(1), ix)= Ztop(smid) - (Ztop(smid)-Ztop(1))/(smid+1)*[1:smid]';


end

% We should now have the F117 aircraft approximated in Cartesian coordinates
% (except that the intersection with any vertical (y,z)-plane is a
%  simply connected curve, so the wings aren't fully defined).
% Note again that these arrays have transverse and range dimensions
% respectively.  On close inspection, however, Y, Z are very jagged.

%figure
pause(1)
surf(XII,YII,ZII);axis image
title 'graph using regular X, with Y,Z wrapped around aircraft'
pause(1)


% TO SMOOTH OBJECT, TRY THE FOLLOWING FILTER (moving average
% over 4 elements, in the range direction): (maybe do YII only)
aa=1; bb=1/4*[1 1 1 1];

YIIsmooth=filter (bb,aa,YII'); YIIsmooth=YIIsmooth';
ZIIsmooth=filter (bb,aa,ZII'); ZIIsmooth=ZIIsmooth';


surf(XII,YIIsmooth,ZIIsmooth);axis image
title 'smoothed versions of regular X, with Y,Z wrapped around aircraft'

%%%%%        Reset to use smoothed versions:
YII=YIIsmooth;
ZII=ZIIsmooth;




% Find surface normals:
[NX,NY,NZ]=surfnorm(XII,YII,ZII);     % (To look at these, use quiver3)

% We'll also need the surface areas.  These can be obtained from a
% formula (see softsurfer.com, from search of google for "area of polygon"),
% under the assumption that the facets are PLANAR:
%   2*A = n.[(V2-V0)x(V3-V1)]  
%   ie the dot product of the normal with a cross-product
%   formed using opposite pairs of vertices.  I haven't checked this!

sizes=size(XII);
SIGMA=0*XII;
for i=1:sizes(2)-1,
    for j=1:sizes(1),
      j2=j+1;
        if j==sizes(1)
          j2=1;
        end
      V0=[XII(j,i) YII(j,i) ZII(j,i)];
      V1=[XII(j2,i) YII(j2,i) ZII(j2,i)];
      V2=[XII(j2,i+1) YII(j2,i+1) ZII(j2,i+1)];
      V3=[XII(j,i+1) YII(j,i+1) ZII(j,i+1)];
      NN=[NX(j,i) NY(j,i) NZ(j,i)];
      SIGMA(j,i)=.5*dot( NN, cross( (V2-V0),(V3-V1) ) );
    end
end
     SIGMA=abs(SIGMA) % In case we've got the sign wrong


% Save everything to ascii file, x,y,z,nx,ny,nz,sigma at one point on 
% each line:


ntot=sizes(1)*sizes(2);
X2=reshape(XII,ntot,1);
Y2=reshape(YII,ntot,1);
Z2=reshape(ZII,ntot,1);
NX2=reshape(NX,ntot,1);
NY2=reshape(NY,ntot,1);
NZ2=reshape(NZ,ntot,1);
SIGMA2=reshape(SIGMA,ntot,1);
COORDS = [X2 Y2 Z2 NX2 NY2 NZ2 SIGMA2];
save coords COORDS -ascii

