% proc3dmesh.m - get 3d info from a list of files

data_name = input('Base image filename? ([] to break)','s');

if isempty(data_name),
	return;
end

load calib_camera
load omlist

if ~exist('omegalist') | ~exist('fc') | ~exist('cc') | ~exist('kc'),
	disp('  ERROR - one or more necessary vars couldn''t be loaded from');
	disp('          omlist.mat or calib_camera.mat');
	return;
end

while 1,
	data_spacing = input('Spacing (in steps) between each data image? ');
	data_spacing = floor(data_spacing);
	if(data_spacing < 1) | (data_spacing > 100),
		disp(' INVALID - value must be between 1 and 100');
	else
		break;
	end
end

clear lastmeshlist;
pointlist = [0 ; 0 ; 0];
trilist = [0 ; 0 ; 0];
num=0;
omegandx = 1;
while 1,
	fname = strcat(data_name,sprintf('%d',num),'.bmp');
	clear im;
	if exist(fname,'file'),
		fprintf(1,'Processing %s...\n', fname);
		im = imread(fname,'bmp');
		im = double(im(:,:,1));
		[r,c]=size(im);
		[pointlist,meshlist] = getpointsmesh(pointlist,im,cc,fc,kc,omegalist(:,omegandx));
		if exist('lastmeshlist'),
			% generate meshes for this col and last
			for i=1:r-1,
				m1 = meshlist(i);
				m2 = meshlist(i+1);
				l1 = lastmeshlist(i);
				l2 = lastmeshlist(i+1);
				if (m1 > 0) & (m2 > 0),
					if (l1 > 0) & (l2 > 0),
						% vertices at all four corners of the square -
						% use shortest-diagonal method to decide how to add two triangles
						if getdistsq(pointlist(:,l1),pointlist(:,m2)) > getdistsq(pointlist(:,l2),pointlist(:,m1)),
							trilist = addtriangle(trilist,pointlist,m1,l1,l2);
							trilist = addtriangle(trilist,pointlist,m2,m1,l2);
						else
							trilist = addtriangle(trilist,pointlist,m2,l1,l2);
							trilist = addtriangle(trilist,pointlist,m2,m1,l1);
						end
					elseif (l1 > 0),
						trilist = addtriangle(trilist,pointlist,m2,m1,l1);
					elseif (l2 > 0),
						trilist = addtriangle(trilist,pointlist,m2,m1,l2);
					end
				elseif (m1 > 0),
					if (l1 > 0) & (l2 > 0),
						trilist = addtriangle(trilist,pointlist,m1,l1,l2);
					end
				elseif (m2 > 0),
					if (l1 > 0) & (l2 > 0),
						trilist = addtriangle(trilist,pointlist,m2,l1,l2);
					end
				end
			end
		end
		lastmeshlist = meshlist;
	else
		nfiles = num;
		break;
	end
	omegandx = omegandx + data_spacing;
	num = num+1;
end

[xx,yy]=size(pointlist);
[xx,zz]=size(trilist);
fprintf(1,'%d points and %d triangles generated.\n\n', yy, zz);

vrml_name = input('Name of VRML 2.0 mesh file to generate? ([] to skip)','s');

if ~isempty(vrml_name),
	fid = fopen(vrml_name,'w+');
	if fid ~= -1,
		fprintf(1,'Generating VRML file...');
		% generate vrml file
		% generate header
		fprintf(fid,'#VRML V2.0 utf8\n\n');
		fprintf(fid,'Transform {\n');
		fprintf(fid,'\tchildren Shape {\n');
		fprintf(fid,'\t\tappearance DEF A Appearance { material Material {} }\n');
		fprintf(fid,'\t\tgeometry DEF IFS IndexedFaceSet {\n');
		fprintf(fid,'\t\t\tcoord Coordinate {\n');
		fprintf(fid,'\t\t\tpoint [\n\n');
		% generate list of points
		[xx,numpts]=size(pointlist);
		n=4;
		fprintf(fid,'\t\t\t\t');
		for i=1:numpts,
			fprintf(fid,'%f %f %f,  ',pointlist(1,i),pointlist(2,i),pointlist(3,i));
			n=n-1;
			if n==0,
				fprintf(fid,'\n\t\t\t\t');
				n=4;
			end
		end
		% output end of point-list/beginning of vertex-list section
		fprintf(fid,'\n\t\t\t]\n\t\t\t}\n\t\t\tcoordIndex [\n');
		% generate list of triangles
		[xx,numtri]=size(trilist);
		n = 4;
		fprintf(fid,'\t\t\t\t');
		for i=2:numtri,		% first el of trilist is junk
			fprintf(fid,'%d %d %d -1,  ',trilist(1,i)-1,trilist(2,i)-1,trilist(3,i)-1);
			% NOTE: In VRML file, points are zero-referenced
			n=n-1;
			if n==0,
				fprintf(fid,'\n\t\t\t\t');
				n=4;
			end
		end
		% output end of file stuff
		fprintf(fid,'\n\t\t\t]\n\t\t}\n\t}\n}\n\n');
		fprintf(1,'done.\n');
		fclose(fid);
	else
		fprintf(1,'Error creating VRML file %s\n',vrml_name);
	end
end

figure;
plot3(pointlist(1,:),pointlist(3,:),-pointlist(2,:),'r.');