#include "heightfield.h"

static bool IsPowerOfTwo(Float x) {
	int i;
	for (i=0; i < 24; ++i) {
		if (x == (1<<i))
			return true;
	}
	return false;
}



Heightfield::Heightfield(const Transform &o2w, int xcount, int xrepeat,
		int ycount, int yrepeat, Float *zs)
	: Shape(o2w) {
	int i;
	int x, y;
	nxgrid = xcount;
	nygrid = ycount;

	/*
	** Setup repeat scaling
	**
	** For purposes of sampling the heightfield has grid dimensions
	**   of nxfullgrid by nyfullgrid, and the number of cells in
	**   each dimension is one less than the number of grids
	**   (nxfullcell by nyfullcell).  This is true whether the
	**   heightfield dimension is repeated or not.
	**
	** The size of the data grid is always nxgrid by nygrid.  If
	**   the heightfield dimension is not repeated (repeat == 1)
	**   then the number of data cells is one less than the number
	**   of grid locations (because the datafield doesn't wrap).
	**   If the dimension is repeated, the grid wraps, and the
	**   number of data cells in that dimension is equal to the
	**   number of grid points.
	*/
	if (xrepeat > 1) {
		if (! IsPowerOfTwo(nxgrid)) {
			Error("Heightfield: nx must be power of 2\n");
			exit(1);
		}
		nxcell = nxgrid;
		xmask = nxgrid - 1;
	}
	else {
		nxcell = nxgrid - 1;
		xmask = -1;
	}
	nxfullgrid = xrepeat * nxgrid;
	nxfullcell = nxfullgrid - 1;
	if (yrepeat > 1) {
		if (! IsPowerOfTwo(nygrid)) {
			Error("Heightfield: ny must be power of 2\n");
			exit(1);
		}
		nycell = nygrid;
		ymask = nygrid - 1;
	}
	else {
		nycell = nygrid - 1;
		ymask = -1;
	}
	nyfullgrid = yrepeat * nygrid;
	nyfullcell = nyfullgrid - 1;

	/*
	** Compute the size of the array needed to store the
	**   depth values.  Trivial if no tiling is done.  If
	**   storage is tiled, each dimension is one tile larger
	**   than might seem necessary, unless the dimension is
	**   an exact multiple of the tile dimension.  Portions
	**   of the last tile will never be accessed.
	*/
#ifdef TILED
	int arraysize = (1 << (2*TBITS)) *
		   ((nxgrid + ((1<<TBITS)-1)) >> TBITS) *
		   ((nygrid + ((1<<TBITS)-1)) >> TBITS);
#else
	int arraysize = nxgrid * nygrid;
#endif

	/*
	** Store the height field values in an array
	*/
	z = new Float[arraysize];
#ifdef TILED
	xmask &= NMASK;
	ymask &= NMASK;
	i = 0;
	for (y=0; y < nygrid; ++y) {
		for (x=0; x < nxgrid; ++x) {
			Z(x,y) = zs[i++];
		}
	}
#else
	memcpy(z, zs, arraysize * sizeof(Float));
#endif

#ifdef MINMAX
	/*
	** For each quad, find the location of the minimum and
	**   the maximum z values
	**
	** Name the locations as follows:
	**
	**	2    3
	**	+----+
	**	|   /| 
	**	|  / |       Y
	**	| /  |       |
	**	|/   |       |
	**	+----+       +---> X
	**	0    1
	**
	** Store the two 2-bit locations in a single packed char
	** XXX could store min/max indexes as MSBs of the z value itself ;-)
	*/
	zmm = new char[arraysize];
	for (y=0; y < nycell; ++y) {
		for (x=0; x < nxcell; ++x) {
			int imin = 0;
			int imax = 0;
			Float Zval = Z(x,y);
			Float Zmin = Zval;
			Float Zmax = Zval;
			Zval = Z(x+1,y);
			if (Zval < Zmin) {
				imin = 1;
				Zmin = Zval; 
			}
			else if (Zval > Zmax) {
				imax = 1;
				Zmax = Zval; 
			}
			Zval = Z(x,y+1);
			if (Zval < Zmin) {
				imin = 2;
				Zmin = Zval; 
			}
			else if (Zval > Zmax) {
				imax = 2;
				Zmax = Zval; 
			}
			Zval = Z(x+1,y+1);
			if (Zval < Zmin) {
				imin = 3;
				Zmin = Zval; 
			}
			else if (Zval > Zmax) {
				imax = 3;
				Zmax = Zval; 
			}
			ZMM(x,y) = imin + 4*imax;
		}
	}
#endif

	/*
	** Compute and store bounding box information
	*/
	zmin = z[0];
	zmax = z[0];
	for (i = 1; i < nxgrid*nygrid; ++i) {
		if (z[i] < zmin) zmin = z[i];
		if (z[i] > zmax) zmax = z[i];
	}
	Float height = zmax - zmin;
#ifdef FATBOX
	/*
	** Use this to test the effectiveness of the min/max test on
	**   performance.
	*/
	if (height > 0) {
		zmin -= height;
		zmax += height;
	}
	else {
		zmin -= 0.0001;
		zmax += 0.0001;
	}
#else
	/*
	** Bloat the box slightly to avoid sample errors at boundaries
	*/
	if (height > 0) {
		zmin -= 0.01 * height;
		zmax += 0.01 * height;
	}
	else {
		zmin -= 0.0001;
		zmax += 0.0001;
	}
#endif
	bbox = new BBox(Point(0,0,zmin), Point(1,1,zmax));
	printf("Heightfield min=%f, max=%f\n", zmin, zmax);
}



Heightfield::~Heightfield() {
	delete z;
#ifdef MINMAX
	delete zmm;
#endif
	delete bbox;
}



/*
** Return the (normalized) average of the four triangle normals at the
**   specified vertex
** Vertex order is Clockwise
*/
Normal Heightfield::VertNorm(int x, int y) const {
	Normal n;
	n  = TriNorm(x-1, y, x, y, x, y-1);
	n += TriNorm(x, y+1, x, y, x-1, y);
	n += TriNorm(x+1, y, x, y, x, y+1);
	n += TriNorm(x, y-1, x, y, x+1, y);
	return n.Hat();
}



/*
** Return the normal of the triangle specified by the integer coordinates
*/
Normal Heightfield::TriNorm(int x0, int y0, int x1, int y1, int x2, int y2)
    const {
	/*
	** Let normal calculations wrap if a dimension is repeated.
	** Otherwise clamp at edges
	*/
	if (nxgrid != nxcell) {
		if ((x0 < 0) || (x1 < 0) || (x2 < 0))
			return Normal(0, 0, 0);
		if ((x0 >= nxgrid) || (x1 >= nxgrid) || (x2 >= nxgrid))
			return Normal(0, 0, 0);
	}
	if (nygrid != nycell) {
		if ((y0 < 0) || (y1 < 0) || (y2 < 0))
			return Normal(0, 0, 0);
		if ((y0 >= nygrid) || (y1 >= nygrid) || (y2 >= nygrid))
			return Normal(0, 0, 0);
	}

	/*
	** Compute normal
	*/
	Point P0 = Point(x0 / Float(nxfullcell),
			 y0 / Float(nyfullcell),
			 Z(x0, y0));
	Point P1 = Point(x1 / Float(nxfullcell),
			 y1 / Float(nyfullcell),
			 Z(x1, y1));
	Point P2 = Point(x2 / Float(nxfullcell),
			 y2 / Float(nyfullcell),
			 Z(x2, y2));
	Vector E1 = P1 - P0;
	Vector E2 = P2 - P0;
	Normal N = Normal(Cross(E2, E1));
	return N.Hat();
}



BBox Heightfield::Bound() const {
	return *bbox;
}



BBox Heightfield::BoundWorldSpace() const {
	return ObjectToWorld(*bbox);
}



bool Heightfield::Intersect(const Ray &ray, DifferentialGeometry *dg) const {
	Ray oray = WorldToObject(ray);
	static int hfTests = 0, hfHits = 0;
	static int hfMMrejects = 0;
	if (hfTests == 0) {
		StatsRegisterRatio(STATS_DETAILED,
			"Heightfield", " Hits : Heightfield Ray Intersectionss",
			&hfHits, &hfTests);
		StatsRegisterRatio(STATS_DETAILED,
			"Heightfield", " Hits : Heightfield Min/Max Rejects",
			&hfHits, &hfMMrejects);
	}
	hfTests += 1;

	/*
	** Use Smits algorithm to test for ray/bbox intersection
	*/
	const Float xmin = 0;
	const Float xmax = 1;
	const Float ymin = 0;
	const Float ymax = 1;
	Point ptmin, ptmax;
	if (oray.D.x > 0) {
		ptmin.x = (xmin - oray.O.x) / oray.D.x;
		ptmax.x = (xmax - oray.O.x) / oray.D.x;
	} else {
		ptmin.x = (xmax - oray.O.x) / oray.D.x;
		ptmax.x = (xmin - oray.O.x) / oray.D.x;
	}
	if (oray.D.y > 0) {
		ptmin.y = (ymin - oray.O.y) / oray.D.y;
		ptmax.y = (ymax - oray.O.y) / oray.D.y;
	} else {
		ptmin.y = (ymax - oray.O.y) / oray.D.y;
		ptmax.y = (ymin - oray.O.y) / oray.D.y;
	}
	if (oray.D.z > 0) {
		ptmin.z = (zmin - oray.O.z) / oray.D.z;
		ptmax.z = (zmax - oray.O.z) / oray.D.z;
	} else {
		ptmin.z = (zmax - oray.O.z) / oray.D.z;
		ptmax.z = (zmin - oray.O.z) / oray.D.z;
	}
	Float t0 = max(oray.mint, max(ptmin.z, max(ptmin.y, ptmin.x)));
	Float t1 = min(oray.maxt, min(ptmax.z, min(ptmax.y, ptmax.x)));
	if (t0 >= t1)
		return false;

	/*
	** Compute start and end points for the ray intersection
	*/
	Point p0 = oray(t0);
	Point p1 = oray(t1);

	/*
	** Compute x and y cell indexes for t0 and t1
	** Check for out-of-bounds cell indexes due to floating point errors
	** XXX - assumes truncation when converting Float to int
	*/
#ifdef NOTDEF
	int X0 = max(0, min((nxfullcell-1), int(p0.x * nxfullcell)));
	int X1 = max(0, min((nxfullcell-1), int(p1.x * nxfullcell)));
	int Y0 = max(0, min((nyfullcell-1), int(p0.y * nyfullcell)));
	int Y1 = max(0, min((nyfullcell-1), int(p1.y * nyfullcell)));
#else
	int X0 = int(p0.x * nxfullcell);
	int X1 = int(p1.x * nxfullcell);
	int Y0 = int(p0.y * nyfullcell);
	int Y1 = int(p1.y * nyfullcell);
	if (X0 < 0)
		X0 = 0;
	else if (X0 > (nxfullcell-1))
		X0 = nxfullcell-1;
	if (X1 < 0)
		X1 = 0;
	else if (X1 > (nxfullcell-1))
		X1 = nxfullcell-1;
	if (Y0 < 0)
		Y0 = 0;
	else if (Y0 > (nyfullcell-1))
		Y0 = nyfullcell-1;
	if (Y1 < 0)
		Y1 = 0;
	else if (Y1 > (nyfullcell-1))
		Y1 = nyfullcell-1;
#endif

	/*
	** Initialize cell index counters
	*/
	int X = X0;
	int Y = Y0;

	/*
	** Compute all variables that are a function of the sign of
	**   the slope in x or y
	** Do a little extra work up front to avoid additional comparisons
	*/
	int dX, dY;
	int Xout, Yout;
	Float absdx, absdy;
	if (p1.x >= p0.x) {
		dX = 1;
		Xout = X0 + 1;
		absdx = p1.x - p0.x;
	}
	else {
		dX = -1;
		Xout = X0;
		absdx = p0.x - p1.x;
	}
	if (p1.y >= p0.y) {
		dY = 1;
		Yout = Y0 + 1;
		absdy = p1.y - p0.y;
	}
	else {
		dY = -1;
		Yout = Y0;
		absdy = p0.y - p1.y;
	}

#ifdef MINMAX
	Float zin = p0.z;
#define MAXLOOPCOUNT 1000000
	int loopcount = 0;
	/*
	** Handle easy cases
	*/
	if (X0 == X1) {
		/*
		** Handle y-major single-column of quads
		*/
		Float yout = Yout / Float(nyfullcell);
		Float dzdy = (oray.D.z / oray.D.y);
		Float zout = dzdy * (yout - oray.O.y) + oray.O.z;
		Float dzout = dzdy * (dY / Float(nyfullcell));
		if (dzout > 0) {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zin > Zmax) || (zout < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				if (Y == Y1)
					return false;
				Y += dY;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT0\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "Y=%d, dY = %d\n", Y, dY);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.y* = %17.8f, ",
						p0.y * nyfullcell);
					fprintf(stderr, "p1.y* = %17.8f\n",
						p1.y * nyfullcell);
					fprintf(stderr, "recomputed Y0=%d, ",
						int(p0.y * nyfullcell));
					fprintf(stderr, "recomputed Y1=%d\n",
						int(p1.y * nyfullcell));
					return false;
				}
			}
		}
		else {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zout > Zmax) || (zin < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				if (Y == Y1)
					return false;
				Y += dY;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT1\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "Y=%d, dY = %d\n", Y, dY);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.y* = %17.8f, ",
						p0.y * nyfullcell);
					fprintf(stderr, "p1.y* = %17.8f\n",
						p1.y * nyfullcell);
					fprintf(stderr, "recomputed Y0=%d, ",
						int(p0.y * nyfullcell));
					fprintf(stderr, "recomputed Y1=%d\n",
						int(p1.y * nyfullcell));
					return false;
				}
			}
		}
	}
	else if (Y0 == Y1) {
		/*
		** Handle x-major single-row of quads
		*/
		Float xout = Xout / Float(nxfullcell);
		Float dzdx = (oray.D.z / oray.D.x);
		Float zout = dzdx * (xout - oray.O.x) + oray.O.z;
		Float dzout = dzdx * (dX / Float(nxfullcell));
		if (dzout > 0) {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zin > Zmax) || (zout < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				if (X == X1)
					return false;
				X += dX;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT2\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "X=%d, dX = %d\n", X, dX);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.x* = %17.8f, ",
						p0.x * nxfullcell);
					fprintf(stderr, "p1.x* = %17.8f\n",
						p1.x * nxfullcell);
					fprintf(stderr, "recomputed X0=%d, ",
						int(p0.x * nxfullcell));
					fprintf(stderr, "recomputed X1=%d\n",
						int(p1.x * nxfullcell));
					return false;
				}
			}
		}
		else {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zout > Zmax) || (zin < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				if (X == X1)
					return false;
				X += dX;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT3\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "X=%d, dX = %d\n", X, dX);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.x* = %17.8f, ",
						p0.x * nxfullcell);
					fprintf(stderr, "p1.x* = %17.8f\n",
						p1.x * nxfullcell);
					fprintf(stderr, "recomputed X0=%d, ",
						int(p0.x * nxfullcell));
					fprintf(stderr, "recomputed X1=%d\n",
						int(p1.x * nxfullcell));
					return false;
				}
			}
		}
	}

	/*
	** Handle more difficult cases of sloping paths
	*/
	if ((absdy*nyfullcell) >= (absdx*nxfullcell)) {
		/*
		** Handle y-major line
		*/
		Float yout = Yout / Float(nyfullcell);
		Float dxdy = (oray.D.x / oray.D.y);
		Float dzdy = (oray.D.z / oray.D.y);
		Float xout = dxdy * (yout - oray.O.y) + oray.O.x;
		Float zout = dzdy * (yout - oray.O.y) + oray.O.z;
		Float dxout = dxdy * (dY / Float(nyfullcell));
		Float dzout = dzdy * (dY / Float(nyfullcell));
		if (dzout > 0) {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zin > Zmax) || (zout < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				Xout = int(xout * nxfullcell);
				if ((Xout != X) &&
					(Xout >= 0) && (Xout < nxfullcell)) {
					X += dX;
					// Assert(X == Xout); XXX
					index = ZMM(X,Y);
					Zmin = ZMIN(X,Y,index);
					Zmax = ZMAX(X,Y,index);
					if ((zin > Zmax) || (zout < Zmin)) {
						hfMMrejects += 1;
					}
					else if (TestQuad(ray, oray, dg, X, Y)){
						hfHits += 1;
						return true;
					}
				}
				if (Y == Y1)
					return false;
				Y += dY;
				xout += dxout;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT4\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "Y=%d, dY = %d\n", Y, dY);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.y* = %17.8f, ",
						p0.y * nyfullcell);
					fprintf(stderr, "p1.y* = %17.8f\n",
						p1.y * nyfullcell);
					fprintf(stderr, "recomputed Y0=%d, ",
						int(p0.y * nyfullcell));
					fprintf(stderr, "recomputed Y1=%d\n",
						int(p1.y * nyfullcell));
					return false;
				}
			}
		}
		else {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zout > Zmax) || (zin < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				Xout = int(xout * nxfullcell);
				if ((Xout != X) &&
					(Xout >= 0) && (Xout < nxfullcell)) {
					X += dX;
					// Assert(X == Xout); XXX
					index = ZMM(X,Y);
					Zmin = ZMIN(X,Y,index);
					Zmax = ZMAX(X,Y,index);
					if ((zout > Zmax) || (zin < Zmin)) {
						hfMMrejects += 1;
					}
					else if (TestQuad(ray, oray, dg, X, Y)){
						hfHits += 1;
						return true;
					}
				}
				if (Y == Y1)
					return false;
				Y += dY;
				xout += dxout;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT5\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "Y=%d, dY = %d\n", Y, dY);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.y* = %17.8f, ",
						p0.y * nyfullcell);
					fprintf(stderr, "p1.y* = %17.8f\n",
						p1.y * nyfullcell);
					fprintf(stderr, "recomputed Y0=%d, ",
						int(p0.y * nyfullcell));
					fprintf(stderr, "recomputed Y1=%d\n",
						int(p1.y * nyfullcell));
					return false;
				}
			}
		}
	}
	else {
		/*
		** Handle x-major line
		*/
		Float xout = Xout / Float(nxfullcell);
		Float dydx = (oray.D.y / oray.D.x);
		Float dzdx = (oray.D.z / oray.D.x);
		Float yout = dydx * (xout - oray.O.x) + oray.O.y;
		Float zout = dzdx * (xout - oray.O.x) + oray.O.z;
		Float dyout = dydx * (dX / Float(nxfullcell));
		Float dzout = dzdx * (dX / Float(nxfullcell));
		if (dzout > 0) {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zin > Zmax) || (zout < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				Yout = int(yout * nyfullcell);
				if ((Yout != Y) &&
					(Yout >= 0) && (Yout < nyfullcell)) {
					Y += dY;
					// Assert(Y == Yout); XXX
					index = ZMM(X,Y);
					Zmin = ZMIN(X,Y,index);
					Zmax = ZMAX(X,Y,index);
					if ((zin > Zmax) || (zout < Zmin)) {
						hfMMrejects += 1;
					}
					else if (TestQuad(ray, oray, dg, X, Y)){
						hfHits += 1;
						return true;
					}
				}
				if (X == X1)
					return false;
				X += dX;
				yout += dyout;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT6\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "X=%d, dX = %d\n", X, dX);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.x* = %17.8f, ",
						p0.x * nxfullcell);
					fprintf(stderr, "p1.x* = %17.8f\n",
						p1.x * nxfullcell);
					fprintf(stderr, "recomputed X0=%d, ",
						int(p0.x * nxfullcell));
					fprintf(stderr, "recomputed X1=%d\n",
						int(p1.x * nxfullcell));
					return false;
				}
			}
		}
		else {
			while (1) {
				int index = ZMM(X,Y);
				Float Zmin = ZMIN(X,Y,index);
				Float Zmax = ZMAX(X,Y,index);
				if ((zout > Zmax) || (zin < Zmin)) {
					hfMMrejects += 1;
				}
				else if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
				Yout = int(yout * nyfullcell);
				if ((Yout != Y) &&
					(Yout >= 0) && (Yout < nyfullcell)) {
					Y += dY;
					// Assert(Y == Yout); XXX
					index = ZMM(X,Y);
					Zmin = ZMIN(X,Y,index);
					Zmax = ZMAX(X,Y,index);
					if ((zout > Zmax) || (zin < Zmin)) {
						hfMMrejects += 1;
					}
					else if (TestQuad(ray, oray, dg, X, Y)){
						hfHits += 1;
						return true;
					}
				}
				if (X == X1)
					return false;
				X += dX;
				yout += dyout;
				zin = zout;
				zout += dzout;
				loopcount += 1;
				if (loopcount > MAXLOOPCOUNT) {
					fprintf(stderr, "\nLOOPCOUNT7\n");
					fprintf(stderr, "X0=%d, X1=%d\n", X0, X1);
					fprintf(stderr, "Y0=%d, Y1=%d\n", Y0, Y1);
					fprintf(stderr, "X=%d, dX = %d\n", X, dX);
					fprintf(stderr, "p0.x=%17.8f, p1.x=%17.8f\n",
						p0.x, p1.x);
					fprintf(stderr, "p0.y=%17.8f, p1.y=%17.8f\n",
						p0.y, p1.y);
					fprintf(stderr, "p0.z=%17.8f, p1.z=%17.8f\n",
						p0.z, p1.z);
					fprintf(stderr, "p0.x* = %17.8f, ",
						p0.x * nxfullcell);
					fprintf(stderr, "p1.x* = %17.8f\n",
						p1.x * nxfullcell);
					fprintf(stderr, "recomputed X0=%d, ",
						int(p0.x * nxfullcell));
					fprintf(stderr, "recomputed X1=%d\n",
						int(p1.x * nxfullcell));
					return false;
				}
			}
		}
	}
#else
	/*
	** Handle easy cases
	*/
	if (X0 == X1) {
		/*
		** Handle y-major single-column of quads
		*/
		while (1) {
			if (TestQuad(ray, oray, dg, X, Y)) {
				hfHits += 1;
				return true;
			}
			if (Y == Y1)
				return false;
			Y += dY;
		}
	}
	else if (Y0 == Y1) {
		/*
		** Handle x-major single-row of quads
		*/
		while (1) {
			if (TestQuad(ray, oray, dg, X, Y)) {
				hfHits += 1;
				return true;
			}
			if (X == X1)
				return false;
			X += dX;
		}
	}

	/*
	** Handle more difficult cases of sloping paths
	*/
	if ((absdy*nyfullcell) >= (absdx*nxfullcell)) {
		/*
		** Handle y-major line
		*/
		Float yout = Yout / Float(nyfullcell);
		Float dxdy = (oray.D.x / oray.D.y);
		Float xout = dxdy * (yout - oray.O.y) + oray.O.x;
		Float dxout = dxdy * (dY / Float(nyfullcell));
		while (1) {
			if (TestQuad(ray, oray, dg, X, Y)) {
				hfHits += 1;
				return true;
			}
			Xout = int(xout * nxfullcell);
			if ((Xout != X) && (Xout >= 0) && (Xout < nxfullcell)) {
				X += dX;
				Assert(X == Xout);
				if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
			}
			if (Y == Y1)
				return false;
			Y += dY;
			xout += dxout;
		}
	}
	else {
		/*
		** Handle x-major line
		*/
		Float xout = Xout / Float(nxfullcell);
		Float dydx = (oray.D.y / oray.D.x);
		Float yout = dydx * (xout - oray.O.x) + oray.O.y;
		Float dyout = dydx * (dX / Float(nxfullcell));
		while (1) {
			if (TestQuad(ray, oray, dg, X, Y)) {
				hfHits += 1;
				return true;
			}
			Yout = int(yout * nyfullcell);
			if ((Yout != Y) && (Yout >= 0) && (Yout < nyfullcell)) {
				Y += dY;
				Assert(Y == Yout);
				if (TestQuad(ray, oray, dg, X, Y)) {
					hfHits += 1;
					return true;
				}
			}
			if (X == X1)
				return false;
			X += dX;
			yout += dyout;
		}
	}
#endif
}



// #define EPSILON 0.0000001
#define EPSILON 0.0
#define TRIA 0
#define TRIB 1
/*
** Check both triangles in a cell for intersection
**  - Always check both, even if the first intersects.  The second
**    might intersect closer to the view point.
**  - XXX Assume a fixed decomposition for now
**
**	1b   2
**	+----+
**	|   /|
**	|  / |
**	| /  |
**	|/   |
**	+----+
**	0   1a
**
**  - Triangle A is (0, 1a, 2)
**  - Triangle B is (0, 1b, 2)
*/
bool Heightfield::TestQuad(const Ray &ray, const Ray &oray,
	DifferentialGeometry *dg, int X, int Y) const {
	static int quadTests = 0;
	static int quadHits = 0;
	if (quadTests == 0) {
		StatsRegisterRatio(STATS_DETAILED,
			"Heightfield", " Hits : Quad Ray Intersections",
			&quadHits, &quadTests);
	}
	quadTests += 1;

	bool hit = false;

	/*
	** Compute object coordinate vertexes of the quad
	*/
	Float inv_nx_minus_one = 1.0 / Float(nxfullcell);
	Float inv_ny_minus_one = 1.0 / Float(nyfullcell);
	Point vert0 = Point(Float(X) * inv_nx_minus_one,
			      Float(Y) * inv_ny_minus_one, 
			      Z(X, Y));
	Point vert1a = Point(Float(X+1) * inv_nx_minus_one,
		 	       Float(Y) * inv_ny_minus_one, 
			       Z(X+1, Y));
	Point vert1b = Point(Float(X) * inv_nx_minus_one,
			      Float(Y+1) * inv_ny_minus_one, 
			      Z(X, Y+1));
	Point vert2 = Point(Float(X+1) * inv_nx_minus_one,
			      Float(Y+1) * inv_ny_minus_one, 
			      Z(X+1, Y+1));

	/*
	** Compute edge parameters
	*/
	Vector edge1a = vert1a - vert0;
	Vector edge1b = vert1b - vert0;
	Vector edge2 = vert2 - vert0;

	/*
	** Do complex intersection calculations
	*/
	Vector pvec = Cross(oray.D, edge2);
	Float deta = Dot(edge1a, pvec);
	Float detb = Dot(edge1b, pvec);
	Float ua, va, ta;
	Float ub, vb, tb;
	if ((deta > -EPSILON) && (deta < EPSILON)) {
		// only triangle b remains
		fprintf(stderr, "EPSILON!\n");	// XXX
		if ((detb > -EPSILON) && (detb < EPSILON)) {
			return false;
		}
		Float inv_detb = 1.0 / detb;
		Vector tvec = oray.O - vert0;
		ub = Dot(tvec, pvec) * inv_detb;
		if ((ub < 0.0) || (ub > 1.0)) {
			return false;
		}
		Vector qvecb = Cross(tvec, edge1b);
		vb = Dot(oray.D, qvecb) * inv_detb;
		if ((vb < 0.0) || ((ub+vb) > 1.0)) {
			return false;
		}
		tb = Dot(edge2, qvecb) * inv_detb;
		hit = SetDG(ray, dg, edge2, X, Y, TRIB, ub, vb, tb);
	}
	else if ((detb > -EPSILON) && (detb < EPSILON)) {
		// only triangle a remains
		fprintf(stderr, "EPSILON!\n");	// XXX
		Float inv_deta = 1.0 / deta;
		Vector tvec = oray.O - vert0;
		ua = Dot(tvec, pvec) * inv_deta;
		if ((ua < 0.0) || (ua > 1.0)) {
			return false;
		}
		Vector qveca = Cross(tvec, edge1a);
		va = Dot(oray.D, qveca) * inv_deta;
		if ((va < 0.0) || ((ua+va) > 1.0)) {
			return false;
		}
		ta = Dot(edge2, qveca) * inv_deta;
		hit = SetDG(ray, dg, edge2, X, Y, TRIA, ua, va, ta);
	}
	else {
		// both triangles remain
		Float inv_deta = 1.0 / deta;
		Float inv_detb = 1.0 / detb;
		Vector tvec = oray.O - vert0;
		Float tdotp = Dot(tvec, pvec);
		ua = tdotp * inv_deta;
		ub = tdotp * inv_detb;
		if ((ua < 0.0) || (ua > 1.0)) {
			// only triangle b remains
			if ((ub < 0.0) || (ub > 1.0)) {
				return false;
			}
			Vector qvecb = Cross(tvec, edge1b);
			vb = Dot(oray.D, qvecb) * inv_detb;
			if ((vb < 0.0) || ((ub+vb) > 1.0)) {
				return false;
			}
			// ray intersects triangle b
			tb = Dot(edge2, qvecb) * inv_detb;
			hit = SetDG(ray, dg, edge2, X, Y, TRIB, ub, vb, tb);
		}
		else if ((ub < 0.0) || (ub > 1.0)) {
			// only triangle a remains
			Vector qveca = Cross(tvec, edge1a);
			va = Dot(oray.D, qveca) * inv_deta;
			if ((va < 0.0) || ((ua+va) > 1.0)) {
				return false;
			}
			// ray intersects triangle a
			ta = Dot(edge2, qveca) * inv_deta;
			hit = SetDG(ray, dg, edge2, X, Y, TRIA, ua, va, ta);
		}
		else {
			// both triangles remain
			Vector qveca = Cross(tvec, edge1a);
			Vector qvecb = Cross(tvec, edge1b);
			va = Dot(oray.D, qveca) * inv_deta;
			vb = Dot(oray.D, qvecb) * inv_detb;
			if ((va < 0.0) || ((ua+va) > 1.0)) {
				if ((vb < 0.0) || ((ub+vb) > 1.0)) {
					return false;
				}
				// ray intersects triangle b
				tb = Dot(edge2, qvecb) * inv_detb;
				hit = SetDG(ray, dg, edge2, X, Y,
						TRIB, ub, vb, tb);
			}
			else if ((vb < 0.0) || ((ub+vb) > 1.0)) {
				// ray intersects triangle a
				ta = Dot(edge2, qveca) * inv_deta;
				hit = SetDG(ray, dg, edge2, X, Y,
						TRIA, ua, va, ta);
			}
			else {
				// ray intersects both triangles
				ta = Dot(edge2, qveca) * inv_deta;
				tb = Dot(edge2, qvecb) * inv_detb;
				if (ta < tb)
					hit = SetDG(ray, dg, edge2, X, Y,
							TRIA, ua, va, ta);
				else
					hit = SetDG(ray, dg, edge2, X, Y,
							TRIB, ub, vb, tb);
			}
		}	
	}
	if (hit) {
		quadHits += 1;
	}
	return hit;
}



/*
**  Set u,v to x,y (reasonable approximation for a unit-size height field)
**  Compute interpolated N,S,T based on Phong-like arithmetic
*/
bool Heightfield::SetDG(const Ray &ray, DifferentialGeometry *dg,
	const Vector &edge,
	int X, int Y, int tri, Float u, Float v, Float t) const {

	static int normalFlips = 0;
	static int first = true;
	if (first) {
		StatsRegisterCounter(STATS_DETAILED,
			"Geometry", "Normal Flips", &normalFlips);
		first = false;
	}

	/*
	** Check to see if the new intersection is closer
	*/
	if ((t < ray.mint) || (t > ray.maxt)) {
		return false;
	}
	ray.maxt = t;

	/*
	** Compute the actual intersection point in world coordinates
	*/
	dg->P = ray(t);

	/*
	** Set texture coordinates to x,y location in the unit-size
	**   height field.
	*/
	Point P = WorldToObject(dg->P);
	dg->u = P.x;
	dg->v = P.y;

	/*
	** Set normal to the weighted average of triangle's vertex normals
	** XXX depends on order of specification of vertexes (see above)
	*/
	Normal TriN;
	Normal N = (1.0 - u - v) * VertNorm(X, Y);
	N += v * VertNorm(X+1, Y+1);
	if (tri == TRIA) {
		TriN = TriNorm(X, Y, X+1, Y+1, X+1, Y);
		N += u * VertNorm(X+1, Y);
	}
	else {
		TriN = TriNorm(X, Y, X, Y+1, X+1, Y+1);
		N += u * VertNorm(X, Y+1);
	}
	N = ObjectToWorld(N).Hat();

	/*
	** Invert normal if facet normal is in hemisphere of
	**   ray direction
	*/
	TriN = ObjectToWorld(TriN);
	if (Dot(TriN, ray.D) > 0) {
		N = -N;
		normalFlips += 1;
	}

	/*
	** To avoid the situation where Normal interpolation results in
	**   a normal that faces away from the Eye vector, we clamp the
	**   dot product of N and Eye to a small value, rotating N toward
	**   Eye in the plane defined by N and Eye.
	*/
	const Float angle = 2.0;
	const Float dotangle = 0.034899;
	Vector Eye = (-ray.D).Hat();
	if (Dot(N, Eye) < dotangle) {
		Vector Axis = Cross(Vector(N), Eye);
		Vector Nperp = Cross(Eye, Axis);
		Transform Rot = Rotate(angle, Axis);
		N = Normal(Rot(Nperp).Hat());
	}

	/*
	** Use cheap trick to get S and T
	** XXX assumes "reasonable" change to normal due to Phong interpolation
	*/
	dg->N = N;
	Vector S2 = Vector(edge.x, edge.y, edge.z).Hat();
	S2 = ObjectToWorld(S2).Hat();
	dg->S = Cross(Vector(S2), Vector(N)).Hat();
	dg->T = Cross(Vector(N), dg->S).Hat();

	return true;
}



/*
** XXX not optimized!!!
*/
bool Heightfield::IntersectP(const Ray &ray) const {
	// printf("IntersectP called!\n");
	DifferentialGeometry dg;
	return Intersect(ray, &dg);
}