#include "morph.h"

/*
	Code for testing morph algorithm and create examples
	- Read a data file containing two land boundaries and control lines
	- Create two fractal surfaces, upsampled
	- Morph between the two fractal landscapes
	Create "lots" of debugging files in geom format.
	Create PovRay frames for rendering
*/

int npoly1 = 0,npoly2 = 0;
POINT2D poly1[MAXPOLY],poly2[MAXPOLY];

int nline = 0;
LINE2D line1[MAXLINE],line2[MAXLINE];
LINE2D linetmp[MAXLINE];

int DXY = 10; /* Dimensions of the world */
int NXY = 11; /* Initial grid resolution */
XYZ **grid1 = NULL,**grid2 = NULL;

int debug = FALSE;

int main(int argc,char **argv)
{
	int i,j,i1,j1,i2,j2,up,dt,line;
	int nline1,nline2;
	int outofbounds;
	POINT2D p,q,p1,q1,p2,q2,X,X1,X2,ptmp;
	POINT2D dsum1,dsum2;
	POINT2D X1final,X2final;
	double mu,mutmp,delta = 0.3,dist;
	double weightsum,weight,length;
	FILE *fptr;
	XYZ **gridtmp = NULL;
	double zmin=1e32,zmax=-1e32;
	double height1,height2;
	char fname[64];
	char cmd[64];
	
	if (argc < 2) {
		fprintf(stderr,"Usage: %s datafile\n",argv[0]);
		exit(-1);
	}

	/* Read in data file ************************************************/
	fprintf(stderr,"Reading data file\n");
	if ((fptr = fopen(argv[1],"r")) == NULL) {
		fprintf(stderr,"Failed to open \"%s\"\n",argv[1]);
		exit(-1);
	}
	while (fscanf(fptr,"%s",cmd) == 1) {
		if (strcmp(cmd,"poly1") == 0) {
			fscanf(fptr,"%d",&npoly1);
			for (i=0;i<npoly1;i++) {
				fscanf(fptr,"%lf %lf\n",&(poly1[i].x),&(poly1[i].y));
			}
		}
      if (strcmp(cmd,"poly2") == 0) {
         fscanf(fptr,"%d",&npoly2);
         for (i=0;i<npoly2;i++) {
            fscanf(fptr,"%lf %lf\n",&(poly2[i].x),&(poly2[i].y));
         }
      }
      if (strcmp(cmd,"line1") == 0) {
         fscanf(fptr,"%d",&nline1);
         for (i=0;i<nline1;i++) {
            fscanf(fptr,"%lf %lf %lf %lf\n",
					&(line1[i].p1.x),&(line1[i].p1.y),&(line1[i].p2.x),&(line1[i].p2.y));
         }
      }
      if (strcmp(cmd,"line2") == 0) {
         fscanf(fptr,"%d",&nline2);
         for (i=0;i<nline2;i++) {
            fscanf(fptr,"%lf %lf %lf %lf\n",
               &(line2[i].p1.x),&(line2[i].p1.y),&(line2[i].p2.x),&(line2[i].p2.y));
         }
      }
	}
   if (nline1 != nline2) {
      fprintf(stderr,"Number of control lines must be the same\n");
      exit(-1);
   }
	nline = nline1;

	/* Create geom files */
	if (debug) {
		WriteStructure(poly1,npoly1,line1,nline,"structure1.geom");
		WriteStructure(poly2,npoly2,line2,nline,"structure2.geom");
	}

	/* Create grids */
	CreateGrid(&grid1,NXY);
	CreateGrid(&grid2,NXY);

	/* Sample onto grid *************************************************/
	fprintf(stderr,"Sample onto grid\n");
	srand48(123);
	for (i=0;i<NXY;i++) {
		for (j=0;j<NXY;j++) {
			p.x = i;
			p.y = j;
			grid1[i][j].x = i;
			grid1[i][j].y = j; 
			if (InsidePolygon(poly1,npoly1,p)) 
				grid1[i][j].z = 0.1 + drand48() * ZSCALE;
			else 
				grid1[i][j].z = -0.1 - drand48() * ZSCALE;
         grid2[i][j].x = i;
         grid2[i][j].y = j;
         if (InsidePolygon(poly2,npoly2,p))
            grid2[i][j].z = 0.1 + drand48() * ZSCALE;
         else
            grid2[i][j].z = -0.1 - drand48() * ZSCALE;
		}
	}
	CalcZBounds(grid1,NXY,&zmin,&zmax);
	CalcZBounds(grid2,NXY,&zmin,&zmax); 

	if (debug) {
		WriteGeomGrid(grid1,NXY,"grid1_0.geom",zmin,zmax);
		WriteGeomGrid(grid2,NXY,"grid2_0.geom",zmin,zmax); 
	}

	/* Upsample *********************************************************/
	for (up=1;up<NSPLIT;up++) {

		fprintf(stderr,"Upsampling grid 1\n");
		FreeGrid(&gridtmp,NXY);
		CreateGrid(&gridtmp,NXY*2-1);

		for (i=0;i<NXY*2-1;i++) {
			for (j=0;j<NXY*2-1;j++) {
				gridtmp[i][j].x = i;
				gridtmp[i][j].y = j;
				gridtmp[i][j].z = 0;
				if (i % 2 == 0 && j % 2 == 0) {
					gridtmp[i][j].z = grid1[i/2][j/2].z;
				} else if (i % 2 == 0 && j % 2 == 1) {
					gridtmp[i][j].z = (grid1[i/2][j/2].z + grid1[i/2][j/2+1].z) / 2 + 
											(drand48()-0.5) * delta;
				} else if (i % 2 == 1 && j % 2 == 0) {
   	         gridtmp[i][j].z = (grid1[i/2][j/2].z + grid1[i/2+1][j/2].z) / 2 + 
											(drand48()-0.5) * delta;
				} else if (i % 2 == 1 && j % 2 == 1) {
   	         gridtmp[i][j].z = (grid1[i/2][j/2].z + grid1[i/2+1][j/2].z +
										   grid1[i/2][j/2+1].z + grid1[i/2+1][j/2+1].z)/4 + 
											(drand48()-0.5) * delta;
				}
			}
		}
		FreeGrid(&grid1,NXY);
		CreateGrid(&grid1,NXY*2-1);
		CopyGrid(gridtmp,grid1,NXY*2-1);

		fprintf(stderr,"Upsampling grid 2\n"); 
   	for (i=0;i<NXY*2-1;i++) {
   	   for (j=0;j<NXY*2-1;j++) {
   	      gridtmp[i][j].x = i;
   	      gridtmp[i][j].y = j;
   	      gridtmp[i][j].z = 0;
   	      if (i % 2 == 0 && j % 2 == 0) {
   	         gridtmp[i][j].z = grid2[i/2][j/2].z;
   	      } else if (i % 2 == 0 && j % 2 == 1) {
   	         gridtmp[i][j].z = (grid2[i/2][j/2].z + grid2[i/2][j/2+1].z) / 2 +
   	                           (drand48()-0.5) * delta;
   	      } else if (i % 2 == 1 && j % 2 == 0) {
   	         gridtmp[i][j].z = (grid2[i/2][j/2].z + grid2[i/2+1][j/2].z) / 2 +
   	                           (drand48()-0.5) * delta;
   	      } else if (i % 2 == 1 && j % 2 == 1) {
   	         gridtmp[i][j].z = (grid2[i/2][j/2].z + grid2[i/2+1][j/2].z +
   	                           grid2[i/2][j/2+1].z + grid2[i/2+1][j/2+1].z)/4 +
   	                           (drand48()-0.5) * delta;
   	      }
   	   }
   	}
		FreeGrid(&grid2,NXY);
		CreateGrid(&grid2,NXY*2-1);
		CopyGrid(gridtmp,grid2,NXY*2-1);
		NXY = NXY*2 - 1;

		zmin = 1e32;
		zmax = -1e32;
   	CalcZBounds(grid1,NXY,&zmin,&zmax);
   	CalcZBounds(grid2,NXY,&zmin,&zmax);

		delta /= 2;
	} /* up */

	/* Scale the grid so it is the same units as the lines 0->DXY */
	for (i=0;i<NXY;i++) {
		for (j=0;j<NXY;j++) {
			grid1[i][j].x *= (DXY / (double)(NXY-1));
         grid1[i][j].y *= (DXY / (double)(NXY-1));
         grid2[i][j].x *= (DXY / (double)(NXY-1));
         grid2[i][j].y *= (DXY / (double)(NXY-1));
		}
	}

	if (debug) {
   	WriteGeomGrid(grid1,NXY,"grid1_1.geom",zmin,zmax);
   	WriteGeomGrid(grid2,NXY,"grid2_1.geom",zmin,zmax);
	}

	/* Perform a simple blend ***********************************************/
   FreeGrid(&gridtmp,NXY);
   CreateGrid(&gridtmp,NXY);
	for (dt=0;dt<=NMORPH;dt++) {
		mu = dt / (double)NMORPH;
		for (i=0;i<NXY;i++) {
			for (j=0;j<NXY;j++) {
				gridtmp[i][j].x = grid1[i][j].x;
            gridtmp[i][j].y = grid1[i][j].y;
				gridtmp[i][j].z = grid1[i][j].z + mu*(grid2[i][j].z-grid1[i][j].z);
			}
		}
		if (debug)
			ReportRange(gridtmp,NXY);
		sprintf(fname,"blend_%04d",dt);
		WritePovray(gridtmp,NXY,fname,zmin,zmax);
	} 

   /* Perform the proper morph ***********************************************/
   FreeGrid(&gridtmp,NXY);
   CreateGrid(&gridtmp,NXY);
   for (dt=0;dt<=NMORPH;dt++) {
      mu = dt / (double)NMORPH;
		if (debug)
			fprintf(stderr,"Calculating morph step: %d\n",dt);
		
		/* Interpolate control lines */
		for (i=0;i<nline;i++) {
			linetmp[i].p1.x = line1[i].p1.x + mu * (line2[i].p1.x - line1[i].p1.x);
			linetmp[i].p1.y = line1[i].p1.y + mu * (line2[i].p1.y - line1[i].p1.y);
         linetmp[i].p2.x = line1[i].p2.x + mu * (line2[i].p2.x - line1[i].p2.x);
         linetmp[i].p2.y = line1[i].p2.y + mu * (line2[i].p2.y - line1[i].p2.y);
		}
		sprintf(fname,"structure_%04d.geom",dt);
		if (debug)
			WriteStructure(NULL,0,linetmp,nline,fname);

		/* Consider each pixel */
		outofbounds = 0;
      for (i=0;i<NXY;i++) {
         for (j=0;j<NXY;j++) {
            X.x = grid1[i][j].x;
            X.y = grid1[i][j].y;

				/* Consider each line */
				dsum1.x = 0; dsum1.y = 0;
				dsum2.x = 0; dsum2.y = 0;
				weightsum = 0;
				for (line=0;line<nline;line++) {
					p = linetmp[line].p1;            	/* Current frame */
					q = linetmp[line].p2;
					length = LineLength(p,q);
					dist = LineDist(X,p,q);
         		p1 = line1[line].p1;						/* Start frame */
         		q1 = line1[line].p2;
         		p2 = line2[line].p1;						/* End frame */
         		q2 = line2[line].p2;
					CalcUVX(p,q,X,p1,q1,&X1);           /* Corresponding point in start surface */
					CalcUVX(p,q,X,p2,q2,&X2);           /* Corresponding point in stop surface */ 
					weight = pow( pow(length,CONST1) / (CONST2 + dist) , CONST3);
					dsum1.x += (X1.x - X.x) * weight;
               dsum1.y += (X1.y - X.y) * weight;
					dsum2.x += (X2.x - X.x) * weight;
               dsum2.y += (X2.y - X.y) * weight;
					weightsum += weight;
				}

				/* Sample in start surface */
				X1final.x = X.x + dsum1.x / weightsum;
				X1final.y = X.y + dsum1.y / weightsum;

				/* Sample in stop surface */
         	X2final.x = X.x + dsum2.x / weightsum;
         	X2final.y = X.y + dsum2.y / weightsum;

				/* Turn the coordinates into indices */
				i1 = X1final.x * (NXY-1) / (double)DXY;
				j1 = X1final.y * (NXY-1) / (double)DXY;
				if (i1 >= 0 && i1 < NXY && j1 >= 0 && j1 < NXY) {
					height1 = grid1[i1][j1].z;
				} else {
					height1 = zmin;
					outofbounds++;
				}
         	i2 = X2final.x * (NXY-1) / (double)DXY;
         	j2 = X2final.y * (NXY-1) / (double)DXY;
         	if (i2 >= 0 && i2 < NXY && j2 >= 0 && j2 < NXY) {
					height2 = grid2[i2][j2].z;
				} else {
               height2 = zmin;
               outofbounds++;
				}

				/* Cross dissolve the heights */
				gridtmp[i][j].x = grid1[i][j].x;
				gridtmp[i][j].y = grid1[i][j].y;
				gridtmp[i][j].z = height1 * (1-mu) + height2 * mu;
			}
		}
		if (debug) {
			fprintf(stderr,"\tOut of bound pixels: %d\n",outofbounds);
			ReportRange(gridtmp,NXY);
		}
      sprintf(fname,"morph_%04d",dt);
      WritePovray(gridtmp,NXY,fname,zmin,zmax);
   } 

}

/*
	Malloc a 2D grid structure 
*/
int CreateGrid(XYZ ***g,int n)
{
	int i;

	if (debug)
		fprintf(stderr,"\tCreategrid(,%d)\n",n);

	if (*g != NULL)
		fprintf(stderr,"Warning - grid not free before malloc\n");

	*g = malloc(n*sizeof(XYZ *));
	for (i=0;i<n;i++) {
		(*g)[i] = malloc(n*sizeof(XYZ));
	}
	return(TRUE);
}

int FreeGrid(XYZ ***g,int n)
{
	int i;

	if (debug)
		fprintf(stderr,"\tFreegrid(,%d)\n",n);

	if (*g == NULL)
		return(FALSE);

	for (i=0;i<n;i++)
		free((*g)[i]);
	free(*g);
	*g = NULL;

	return(TRUE);
}

/*
	Write structural components in geom format a polygon
*/
void WriteStructure(POINT2D *p,int np,LINE2D *l,int nl,char *fname)
{
	int n = 0;
	int i,x,y;
	FILE *fptr;

	/* The polygon - red*/
	fptr = fopen(fname,"w");
	for (i=0;i<np;i++) {
		fprintf(fptr,"s %g %g 0 0.1 1 0 0\n",p[i].x,p[i].y);
		fprintf(fptr,"l %g %g 0 %g %g 0 1 0 0\n",
			p[i].x,p[i].y,p[(i+1)%np].x,p[(i+1)%np].y);
	}

	/* The lines - green */
	for (i=0;i<nl;i++) {
      fprintf(fptr,"l %g %g 0 %g %g 0 0 1 0\n",
         l[i].p1.x,l[i].p1.y,l[i].p2.x,l[i].p2.y);
	}

	/* The grid - grey */
	for (i=0;i<=DXY;i++) {
		fprintf(fptr,"l 0 %d 0 %d %d 0 0.75 0.75 0.75\n",i,DXY,i);
		fprintf(fptr,"l %d 0 0 %d %d 0 0.75 0.75 0.75\n",i,i,DXY); 
	}
	fclose(fptr);
}

int InsidePolygon(POINT2D *polygon,int n,POINT2D p)
{
   int i;
   double angle=0;
   POINT2D p1,p2;

   for (i=0;i<n;i++) {
      p1.x = polygon[i].x - p.x;
      p1.y = polygon[i].y - p.y;
      p2.x = polygon[(i+1)%n].x - p.x;
      p2.y = polygon[(i+1)%n].y - p.y;
      angle += Angle2D(p1.x,p1.y,p2.x,p2.y);
   }

   if (ABS(angle) < PI)
      return(FALSE);
   else
      return(TRUE);
}

double Angle2D(double x1, double y1, double x2, double y2)
{
   double dtheta,theta1,theta2;

   theta1 = atan2(y1,x1);
   theta2 = atan2(y2,x2);
   dtheta = theta2 - theta1;
   while (dtheta > PI)
      dtheta -= TWOPI;
   while (dtheta < -PI)
      dtheta += TWOPI;

   return(dtheta);
}

/* 
	Write a grid as a geom file
*/
void WriteGeomGrid(XYZ **g,int n,char *fname,double zmin,double zmax)
{
	int i,j;
	FILE *fptr;
	COLOUR colour,blue = {0.0,0.0,1.0};

	if (debug)
		fprintf(stderr,"\tWriteGeomGrid(,%d,%s)\n",n,fname);

	fptr = fopen(fname,"w");
	for (i=0;i<n-1;i++) {
		for (j=0;j<n-1;j++) {
			if (g[i][j].z > 0)
				colour = GetColour(g[i][j].z,0.0,zmax);
			else
				colour = blue;
			fprintf(fptr,"f4 %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g\n",
				g[i][j].x,    g[i][j].y,    g[i][j].z,
            g[i+1][j].x,  g[i+1][j].y,  g[i+1][j].z,
            g[i+1][j+1].x,g[i+1][j+1].y,g[i+1][j+1].z,
            g[i][j+1].x,  g[i][j+1].y,  g[i][j+1].z,
				colour.r,colour.g,colour.b);
		}
	}
	fclose(fptr);
}

void CopyGrid(XYZ **g1,XYZ **g2,int n)
{
	int i,j;

	if (debug)
		fprintf(stderr,"\tCopyGrid(,,&d)\n",n);

	for (i=0;i<n;i++)
		for (j=0;j<n;j++)
			g2[i][j] = g1[i][j];
}

void CalcZBounds(XYZ **g,int n,double *zmin,double *zmax)
{
	int i,j;

	for (i=0;i<n;i++) {
		for (j=0;j<n;j++) {
         *zmin = MIN(*zmin,g[i][j].z);
         *zmax = MAX(*zmax,g[i][j].z);
		}
	}
	if (debug)
		fprintf(stderr,"Z range: %g -> %g\n",*zmin,*zmax);
}

void WritePovray(XYZ **g,int n,char *basename, double zmin,double zmax)
{
	int i,j;
	double z;
	FILE *fptr;
	char fname[64];
	COLOUR colour;
	int ir,ig,ib;

	if (debug)
		fprintf(stderr,"Writing PovRay file \"%s\"\n",basename);

	sprintf(fname,"%s.ini",basename);
	if ((fptr = fopen(fname,"w")) == NULL) 
		return;
	fprintf(fptr,"Input_File_Name=%s.pov\n",basename);
	fprintf(fptr,"Output_File_Name=%s.tga\n",basename);
	fprintf(fptr,"Output_File_Type=T\n");
	fprintf(fptr,"Width=%d\nHeight=%d\n",WIDTH,HEIGHT);
	fprintf(fptr,"Antialias=on\nAntialias_Threshold=0.01\n");
	fprintf(fptr,"Display=on\n");
	fclose(fptr);

   sprintf(fname,"%s.pov",basename);
   if ((fptr = fopen(fname,"w")) == NULL)
      return;
	fprintf(fptr,"camera {\n");
   fprintf(fptr,"   location <9,9,9>\n");
   fprintf(fptr,"   up y\n");
   fprintf(fptr,"   right -%d*x/%d\n",WIDTH,HEIGHT);
   fprintf(fptr,"   angle 60\n");
   fprintf(fptr,"   sky <0,1,0>\n");
   fprintf(fptr,"   look_at <6,0,6>\n");
	fprintf(fptr,"}\n");
	fprintf(fptr,"global_settings {\n");
   fprintf(fptr,"   ambient_light\n");
   fprintf(fptr,"   rgb <1,1,1>\n");
	fprintf(fptr,"}\n");
	fprintf(fptr,"background {\n");
   fprintf(fptr,"   color rgb <0,0,0>\n");
	fprintf(fptr,"}\n");
	fprintf(fptr,"light_source {\n");
   fprintf(fptr,"   <10,10,0>\n");
   fprintf(fptr,"   color rgb <1,1,1>\n");
	fprintf(fptr,"}\n");
   fprintf(fptr,"#declare seatexture = texture {\n");
   fprintf(fptr,"   pigment {\n");
   fprintf(fptr,"      color rgb <0,0,1>\n");
   fprintf(fptr,"   }\n");
   fprintf(fptr,"}\n");
	fprintf(fptr,"polygon {\n");
	fprintf(fptr,"   5, <0,0,0>, <10,0,0>, <10,0,10>, <0,0,10>, <0,0,0>\n");
	fprintf(fptr,"   texture { seatexture }\n");
	fprintf(fptr,"}\n");
	fprintf(fptr,"#declare thenormal = normal {\n");
   fprintf(fptr,"   granite\n");
   fprintf(fptr,"   scale 0.1\n");
	fprintf(fptr,"}\n");
	fprintf(fptr,"#declare thefinish = finish {\n");
   fprintf(fptr,"	  ambient 0.2\n");
   fprintf(fptr,"	  diffuse 0.8\n");
   fprintf(fptr,"   specular 0.0\n");
   fprintf(fptr,"   roughness 0.1\n");
	fprintf(fptr,"}\n");
	for (ir=0;ir<256;ir+=8) {
		for (ig=0;ig<256;ig+=8) {
			for (ib=0;ib<256;ib+=8) {
				fprintf(fptr,"#declare land_%d_%d_%d = texture {\n",ir,ig,ib);
  	 			fprintf(fptr,"   pigment {\n");
      		fprintf(fptr,"      color rgb <%g,%g,%g>\n",
					ir/255.0,ig/255.0,ib/255.0);
   			fprintf(fptr,"   }\n");
   			fprintf(fptr,"   finish { thefinish }\n");
   			fprintf(fptr,"   /* normal { thenormal } */\n");
				fprintf(fptr,"}\n");
			}
		}
	}
	fprintf(fptr,"mesh {\n");
	for (i=0;i<n-1;i++) {
		for (j=0;j<n-1;j++) {
			if (g[i][j].z < 0 && g[i+1][j].z < 0 && 
				 g[i+1][j+1].z < 0 && g[i][j+1].z < 0)
				continue;

			z = (g[i][j].z+g[i+1][j].z+g[i+1][j+1].z)/3;
			colour = GetColour(z,0.0,zmax);
		   ir = colour.r * 255;
      	ig = colour.g * 255;
      	ib = colour.b * 255;
      	ir = (ir / 8) * 8;
      	ig = (ig / 8) * 8;
      	ib = (ib / 8) * 8;
			fprintf(fptr,"triangle {\n");
			fprintf(fptr,"   <%g,%g,%g>,\n",
				g[i][j].x,g[i][j].z,g[i][j].y);
         fprintf(fptr,"   <%g,%g,%g>,\n",
				g[i+1][j].x,g[i+1][j].z,g[i+1][j].y);
         fprintf(fptr,"   <%g,%g,%g> \n",
				g[i+1][j+1].x,g[i+1][j+1].z,g[i+1][j+1].y);
			if (z < 0)
				fprintf(fptr,"   texture { seatexture }\n");
			else
				fprintf(fptr,"   texture { land_%d_%d_%d }\n",ir,ig,ib);
			fprintf(fptr,"}\n");

			z = (g[i][j].z+g[i+1][j+1].z+g[i][j+1].z)/3;
         colour = GetColour(z,0.0,zmax);
         ir = colour.r * 255;
         ig = colour.g * 255;
         ib = colour.b * 255;
         ir = (ir / 8) * 8;
         ig = (ig / 8) * 8;
         ib = (ib / 8) * 8;
         fprintf(fptr,"triangle {\n");
         fprintf(fptr,"   <%g,%g,%g>,\n",
				g[i][j].x,g[i][j].z,g[i][j].y);
         fprintf(fptr,"   <%g,%g,%g>,\n",
				g[i+1][j+1].x,g[i+1][j+1].z,g[i+1][j+1].y);
         fprintf(fptr,"   <%g,%g,%g> \n",
				g[i][j+1].x,g[i][j+1].z,g[i][j+1].y);
         if (z < 0)
            fprintf(fptr,"   texture { seatexture }\n");
         else
            fprintf(fptr,"   texture { land_%d_%d_%d }\n",ir,ig,ib);
         fprintf(fptr,"}\n");
		}
	}
	fprintf(fptr,"}\n");
	fclose(fptr);
}

/*
	Calculate X1, it's relation ship to the line p1,q1
	being the same as the relationship of X to p,q
*/
void CalcUVX(POINT2D p,POINT2D q,POINT2D X,POINT2D p1,POINT2D q1,POINT2D *X1)
{
	double u,v;
	POINT2D xmp,qmp,perp;
	double l2;

	xmp.x = X.x - p.x;
	xmp.y = X.y - p.y;
	qmp.x = q.x - p.x;
	qmp.y = q.y - p.y;
	l2 = qmp.x*qmp.x + qmp.y*qmp.y;
	perp.x = qmp.y;
	perp.y = - qmp.x;

	u = (xmp.x * qmp.x + xmp.y * qmp.y) / l2;
	v = (xmp.x * perp.x + xmp.y * perp.y) / sqrt(l2);

   qmp.x = q1.x - p1.x;
   qmp.y = q1.y - p1.y;
	l2 = sqrt(qmp.x*qmp.x + qmp.y*qmp.y);
	perp.x = qmp.y;
	perp.y = - qmp.x;
	X1->x = p1.x + u * qmp.x + v * perp.x / l2;
	X1->y = p1.y + u * qmp.y + v * perp.y / l2;
}

double LineDist(POINT2D p3,POINT2D p1,POINT2D p2)
{
   double dx,dy,dd,mu;
	POINT2D close;

   dx = p2.x - p1.x;
   dy = p2.y - p1.y;
   dd = dx * dx + dy * dy;

   mu = ((p3.x - p1.x) * dx + (p3.y - p1.y) * dy) / dd;
   close.x = p1.x + mu * dx;
   close.y = p1.y + mu * dy;
   dx = p3.x - close.x;
   dy = p3.y - close.y;

   return(sqrt(dx * dx + dy * dy));
}

double LineLength(POINT2D p,POINT2D q)
{
	double dx,dy;

	dx = p.x-q.x;
	dy = p.y-q.y;
	return(sqrt(dx*dx + dy*dy));
}

void ReportRange(XYZ **g,int n)
{
	int i,j;
   double xmin=1e32,xmax=-1e32;
   double ymin=1e32,ymax=-1e32;
	double zmin=1e32,zmax=-1e32;

	for (i=0;i<n;i++) {
		for (j=0;j<n;j++) {
         xmin = MIN(xmin,g[i][j].x);
         xmax = MAX(xmax,g[i][j].x);
         ymin = MIN(ymin,g[i][j].y);
         ymax = MAX(ymax,g[i][j].y);
			zmin = MIN(zmin,g[i][j].z);
			zmax = MAX(zmax,g[i][j].z);
		}
	}
   fprintf(stderr,"\tX range: %g -> %g\n",xmin,xmax);
   fprintf(stderr,"\tY range: %g -> %g\n",ymin,ymax);
	fprintf(stderr,"\tZ range: %g -> %g\n",zmin,zmax);
}

COLOUR GetColour(double v,double vmin,double vmax)
{
   double dv,vmid;
   COLOUR c = {1.0,1.0,1.0};
   COLOUR c1,c2,c3;
   double ratio;

   if (v < vmin)
      v = vmin;
   if (v > vmax)
      v = vmax;
   dv = vmax - vmin;

   c1.r =   0 / 255.0; c1.g = 160 / 255.0; c1.b =   0 / 255.0;
   c2.r = 180 / 255.0; c2.g = 220 / 255.0; c2.b =   0 / 255.0;
   c3.r = 250 / 255.0; c3.g = 220 / 255.0; c3.b = 170 / 255.0;
   ratio = 0.3;
   vmid = vmin + ratio * dv;
   if (v < vmid) {
      c.r = (c2.r - c1.r) * (v - vmin) / (ratio*dv) + c1.r;
      c.g = (c2.g - c1.g) * (v - vmin) / (ratio*dv) + c1.g;
      c.b = (c2.b - c1.b) * (v - vmin) / (ratio*dv) + c1.b;
   } else {
      c.r = (c3.r - c2.r) * (v - vmid) / ((1-ratio)*dv) + c2.r;
      c.g = (c3.g - c2.g) * (v - vmid) / ((1-ratio)*dv) + c2.g;
      c.b = (c3.b - c2.b) * (v - vmid) / ((1-ratio)*dv) + c2.b;
   }
	return(c);
}