// ivgrid.cxx - read Open Inventor "iv" file format for 3-D models
// and extract from it a regular grid of depth and color samples,
// a rectangular array of points with the following attributes:
//	Z R G B
//
// Written specifically to parse Inventor files written by the
// Minolta Vivid 700 laser scanner,
// for 15-869 assignment 2: View Transformation
//
// some terminology comes from the paper:
//	Head-Tracked Stereoscopic Display Using Image Warping
//	Leonard McMillan and Gary Bishop
//	Stereoscopic Displays and Virtual Reality Systems II
//	SPIE Proceedings 2409, Feb.  1995. 
//
// IMPORTANT:
// for an explanation of coordinate systems, read all comments in
// transform_points() and extract_camera() below

// This program understands only a small subset of the Open Inventor
// file format, namely
//	Texture2 { image WIDTH HEIGHT 3 COLOR_IN_HEXADECIMAL ... }
//	TextureCoordinate2 { point[ X Y, ... X Y ] }
//	Coordinate3 { point [ X Y Z, ... X Y Z ] }
//	coordIndex [ I11, I12, I13, I14, -1,
//		     ...
//		     In1, In2, In3, In4, -1 ]
//	textureCoordIndex [ I11, I12, I13, I14, -1,
//		     ...
//		     In1, In2, In3, In4, -1 ]
// where all capitals means a numerical or string variable,
// and it ignores comments
//	# EVERYTHING_UP_TO_NEWLINE
// and any other tokens it doesn't recognize.
// Currently, comments within the Texture2, TextureCoordinate2, Coordinate3,
// coordIndex, and textureCoordIndex commands are not tolerated (a bug).

// Paul Heckbert
// version 1, 1pm 28 Sept 1999 - handles zoom 1 ok,
//	but doesn't do a great job on zoom>1
// version 2, 1pm 29 Sept 1999 - fixed portability problem that was causing
//	"Unknown error reading 'Texture2' command" error
// version 3, 3pm 30 Sept 1999 - added comments about the sign of w
// version 4, 3pm 1 Oct 1999 - fixed extract_camera to make |uvec|=|vvec|
// version 5, 4pm 3 Oct 1999 - now smart about zoom>1
//		scales the projected and texture coordinates much better now

#include <stdlib.h>
#include <string.h>	// for strcmp
#include <fstream.h>	// for file I/O, includes iostream.h
#include <iomanip.h>	// for ios::

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>

#include <pic.h>	// for color image, from libpicio

#include "grid.h"	// for Grid_data and Gpoint structures


typedef struct {float r, g, b;}	Pixelf_rgb;	// float color

struct Spoint {			// scanner point

			// data from IV file
    Point3 wo;			// position in world space
    float s, t;			// texture coordinates
    unsigned char r, g, b;	// color

			// data we compute
    Point3 gr;			// position in grid space
    // see comments in transform_points() below for explanation of coord sys
};

struct Line {
    float a, b, c;		// line equation ax+by+c=0

    void set(Point3 &p, Point3 &q, Point3 &r);
};

void Line::set(Point3 &p, Point3 &q, Point3 &r) {
    // initialize line so ax+by+c has value 0 at points p and q
    // and value 1 and r (a barycentric coordinate over the triangle)

    a = p.y-q.y;
    b = q.x-p.x;
    c = p.x*q.y-p.y*q.x;
    float s = a*r.x+b*r.y+c;	// = 2*(area of triangle pqr)
    s = 1./s;
    a *= s;			// scale so that a*rx+b*ry+c=1
    b *= s;
    c *= s;
}

struct Triangle {
    Spoint *v[3];		// pointers to three vertices
};

//----------------------------------------------------------------------

#define NMAX 40000	// because Vivid scans 200*200 3-D points, max

enum Quad_list_type {COORD_INDEX, TEXTURE_COORD_INDEX};
    // first or second batch of coordinate indices?

class Object {			// a 3-D object with shape and texture
  public:
    Pic *pic;			// texture

    Spoint pt[NMAX];		// scanner points
    int npt;			// number of scanner points
    float wxmin, wxmax, wymin, wymax, wzmin, wzmax;
				// bounding box in world space

    int zoom;			// Vivid lens zoom

    float pxmin, pxmax, pymin, pymax;
    // projected coordinates (-x/z,-y/z) lie within the range above

    Triangle tri[NMAX*2];	// triangles
    int ntri;			// number of triangles
    
    Object();
    ~Object();

    void get_color_bilinear(float s, float t,
	unsigned char *r, unsigned char *g, unsigned char *b);
	    // do bilinear interpolation from texture

    // to read Open Inventor file format
    void inventor_read(istream &s, const char *streamname);
    int inventor_read(const char *file);

    void print_points();
    void print_transform();

    void transform_points(int nx, int ny);
    void extract_camera(Grid_data &gd);
    void grid_compute(Grid_data &gd);

  private:
    // to read Open Inventor file format, internals
    void inventor_read_texture2(istream &s, const char *streamname);
    void inventor_read_texturecoordinate2(istream &s, const char *streamname);
    void inventor_read_coordinate3(istream &s, const char *streamname);
    void inventor_read_coordindex(istream &s, const char *streamname,
	Quad_list_type which);
};

Object::Object() {
    pic = NULL;
    npt = ntri = 0;
    zoom = 0;		// flag to indicate that pxmin etc uninit
}

Object::~Object() {
    if (pic) pic_free(pic);
}

void Object::get_color_bilinear(float s, float t,
unsigned char *r, unsigned char *g, unsigned char *b) {
    // use bilinear interpolation to extract the color of the
    // texture pixel at (s,t) from pic

    float x = s*pic->nx;
    float y = t*pic->ny;

    // in case some of the (s,t)'s are outside [0,1] range
    if (x<0) x = 0;
    if (x>=pic->nx-1) x = pic->nx-1.001;
    if (y<0) y = 0;
    if (y>=pic->ny-1) y = pic->ny-1.001;

    int xi = (int)floor(x);	// integer part
    int yi = (int)floor(y);
    float xf = x-xi;	// fractional part
    float yf = y-yi;

    Pixel1_rgb *p00, *p10, *p01, *p11;
    Pixelf_rgb px0, px1, pxy;

    p00 = (Pixel1_rgb *)&PIC_PIXEL(pic, xi, yi, 0);
    p10 = p00+1;
    // note that we're reading unsigned chars below, but writing floats
    // linearly interpolate (lerp) between colors 00 and 10
    px0.r = p00->r + xf*(p10->r - p00->r);
    px0.g = p00->g + xf*(p10->g - p00->g);
    px0.b = p00->b + xf*(p10->b - p00->b);

    p01 = (Pixel1_rgb *)&PIC_PIXEL(pic, xi, yi+1, 0);
    p11 = p01+1;
    // lerp between colors 01 and 11
    px1.r = p01->r + xf*(p11->r - p01->r);
    px1.g = p01->g + xf*(p11->g - p01->g);
    px1.b = p01->b + xf*(p11->b - p01->b);

    // lerp between colors x0 and x1
    pxy.r = px0.r + yf*(px1.r - px0.r);
    pxy.g = px0.g + yf*(px1.g - px0.g);
    pxy.b = px0.b + yf*(px1.b - px0.b);
    // no need to check if in range since colors we started with were in range

    assert(pxy.r>=0 && pxy.r<256);
    assert(pxy.g>=0 && pxy.g<256);
    assert(pxy.b>=0 && pxy.b<256);

    // convert back to unsigned char
    *r = (unsigned char)pxy.r;
    *g = (unsigned char)pxy.g;
    *b = (unsigned char)pxy.b;
}

//----------------------------------------------------------------------
// Open Inventor parsing

#define TOKEN_SIZE 20

static int next_token_is_not(istream &s, const char *str) {
    // read a token from stream s, return whether token is not str
    static char tok[TOKEN_SIZE];
    s >> setw(sizeof tok) >> tok;
    return strcmp(tok, str);
}

static void expect_token(istream &s, const char *command, const char *str) {
    // read a token from stream s, and print an error and exit if it's not str
    char tok[TOKEN_SIZE];
    s >> setw(sizeof tok) >> tok;
    if (strcmp(tok, str)) {
	cerr << "Error reading '" << command << "' command, got '"
	    << tok << "', not '" << str << "', as expected" << endl;
	// would be  neat to print filename and line number of input file here
	exit(1);
    }
}

static void exit_if_trouble(istream &s, const char *command, int line) {
    if (s.fail()) {
	cerr << "Unknown error reading '" << command << "' command"
	<< " at line " << line << endl;
	exit(1);
    }
}

// warning: the following routines do a lousy job of checking for errors
// (e.g. braces not preceded by space)

void Object::inventor_read_texture2(istream &s, const char *streamname) {
    // Texture2 { image WIDTH HEIGHT 3 COLOR_IN_HEXADECIMAL ... }
    // read a color image

    expect_token(s, "Texture2", "{");
    expect_token(s, "Texture2", "image");
    int nx, ny, x, y, color24;
    s >> nx;	// read width
    s >> ny;	// read height
    cout << nx << "x" << ny << " texture array" << endl;
    expect_token(s, "Texture2", "3");
    pic = pic_alloc(nx, ny, 3, NULL);	// allocate a picture array

    char tok[TOKEN_SIZE];
    for (y=0; y<ny; y++)
	for (x=0; x<nx; x++) {
	    // the following 3 lines work on SGI's but not Suns!  why?
	    // s.setf(ios::hex);	// use hexadecimal notation
	    // s >> color24;
	    // s.setf(ios::dec);	// back to decimal
	    s >> setw(sizeof tok) >> tok;
		// read in hexadecimal 24-bit color as a string
	    if (sscanf(tok, "%x", &color24) != 1) {
		cerr << "ERROR: sscanf expecting a hex color in Texture2"
		    << endl;
		exit(1);
	    }

	    exit_if_trouble(s, "Texture2", __LINE__);
	    // cout << "(" << x << "," << y << ")";

	    // get the address of pixel (x,y) in pic
	    Pixel1_rgb *p = (Pixel1_rgb *)&PIC_PIXEL(pic, x, y, 0);

	    p->r = color24>>16;	// save the bytes
	    p->g = color24>>8;
	    p->b = color24;
	    // printf("rgb=(%02x,%02x,%02x)=(%3d,%3d,%3d)\n",
		// p->r, p->g, p->b,
		// p->r, p->g, p->b);
	}
    expect_token(s, "Texture2", "}");
    exit_if_trouble(s, "Texture2", __LINE__);	// check that no stream error so far
}

void Object::inventor_read_texturecoordinate2
(istream &s, const char *streamname) {
    // TextureCoordinate2 { point[ X Y, ... X Y ] }
    // read a list of texture coordinates

    expect_token(s, "TextureCoordinate2", "{");
    expect_token(s, "TextureCoordinate2", "point[");
	// note! we're relying on the quirks of Minolta's VIVID software,
	// expecting no space between "point" and "[" here!

    int n;
    char tok[TOKEN_SIZE];

    for (n=0;;) {
	float x, y;
	s >> x >> y;	// read texture x and y (a.k.a. s and t)

	assert(n<NMAX);
	assert(pic);

	pt[n].s = x;
	pt[n].t = y;
	get_color_bilinear(pt[n].s, pt[n].t, &pt[n].r, &pt[n].g, &pt[n].b);

	// cout << "tex" << n << " ("
	    // << x << ","
	    // << y << ")" << endl;
	n++;

	s >> setw(sizeof tok) >> tok;
	if (strcmp(tok, ","))			// usually, token is a comma
	    if (!strcmp(tok, "]")) break;	// "]" means end of list
	    else {
		cerr << "Didn't find end of TextureCoordinate2 list in "
		    << streamname << endl;
		exit(1);
	    }
    }
    expect_token(s, "TextureCoordinate2", "}");
    exit_if_trouble(s, "TextureCoordinate2", __LINE__); // check that no error so far

    cout << "read " << n << " texture coordinates" << endl;
    npt = n;
}

void Object::inventor_read_coordinate3(istream &s, const char *streamname) {
    // Coordinate3 { point [ X Y Z, ... X Y Z ] }
    // read a list of xyz coordinates

    expect_token(s, "Coordinate3", "{");
    expect_token(s, "Coordinate3", "point");
    expect_token(s, "Coordinate3", "[");
    if (!pic) {
	// we should have encountered the Texture2 command by now
	cerr << "ERROR: Inventor file does not contain a texture image.\n"
	    << "Perhaps data wasn't saved properly." << endl;
	exit(1);
    }

    wxmin = HUGE;		// find 3-D bounding box
    wxmax = -HUGE;
    wymin = HUGE;
    wymax = -HUGE;
    wzmin = HUGE;
    wzmax = -HUGE;

    int n;
    char tok[TOKEN_SIZE];
    for (n=0;;) {
	float x, y, z;
	s >> x >> y >> z;	// read x, y, and z
	if (x<wxmin) wxmin = x;
	if (x>wxmax) wxmax = x;
	if (y<wymin) wymin = y;
	if (y>wymax) wymax = y;
	if (z<wzmin) wzmin = z;
	if (z>wzmax) wzmax = z;

	assert(n<NMAX);
	pt[n].wo.x = x;
	pt[n].wo.y = y;
	pt[n].wo.z = z;

	// cout << "pt" << n << " ("
	    // << x << ","
	    // << y << ","
	    // << z << ")" << endl;
	n++;

	s >> setw(sizeof tok) >> tok;
	if (strcmp(tok, ","))			// usually, token is a comma
	    if (!strcmp(tok, "]")) break;	// "]" means end of list
	    else {
		cerr << "Didn't find end of Coordinate3 list in "
		    << streamname << endl;
		exit(1);
	    }
    }
    expect_token(s, "Coordinate3", "}");
    exit_if_trouble(s, "Coordinate3", __LINE__); // check that no stream error so far

    cout << "read " << n << " 3-D points" << endl;
    cout << "    bounding box: "
	<< "x [" << wxmin << ":" << wxmax << "] "
	<< "y [" << wymin << ":" << wymax << "] "
	<< "z [" << wzmin << ":" << wzmax << "]" << endl;

    // check that the number of texture points and 3-D points are equal
    assert(n==npt);
}

void set_tri(Triangle &tri, Quad_list_type which,
Spoint *p, Spoint *q, Spoint *r) {
    if (which==COORD_INDEX) {
	tri.v[0] = p;
	tri.v[1] = q;
	tri.v[2] = r;
    }
    else {
	assert(tri.v[0]==p);
	assert(tri.v[1]==q);
	assert(tri.v[2]==r);
    }
}
    
void Object::inventor_read_coordindex(istream &s, const char *streamname,
Quad_list_type which) {
    // parse either
    //   coordIndex [ POLYLIST ]
    // or
    //   textureCoordIndex [ POLYLIST ]
    // where POLYLIST = POLY, POLY, ... POLY
    // and POLY =  I1, I2, I3, -1	(for a triangle)
    //		or I1, I2, I3, I4, -1	(for a quadrilateral)
    //          etc
    // (Vivid software creates only quadrilaterals and triangles, apparently)

    // read a list of polygons and triangulate them
    // if we're doing coordIndex, then build up the list
    // if we're doing textureCoordIndex, then check that list is identical

    char *command = which==COORD_INDEX ? "coordIndex" : "textureCoordIndex";
    expect_token(s, command, "[");
    char tok[TOKEN_SIZE];
    #define NSMAX 10
    int n, i, ns, ind[NSMAX+1];

    for (n=0;;) {
	for (ns=0; ns<NSMAX+1; ns++) {
	    s >> ind[ns];
	    if (ind[ns]<0) break;
	    assert(ind[ns]<npt);
	    expect_token(s, command, ",");
	}
	exit_if_trouble(s, command, __LINE__);	// check that no stream error so far
	assert(ns<=NSMAX);
	assert(ind[ns]==-1);

	// we triangulate, rather than storing it as an n-gon, because
	// we'll need triangles later anyway
	for (i=2; i<ns; i++) {
	    assert(n<NMAX*2);
	    set_tri(tri[n], which, &pt[ind[0]], &pt[ind[i-1]], &pt[ind[i]]);
	    n++;
	}

	s >> setw(sizeof tok) >> tok;
	if (strcmp(tok, ","))			// usually, token is a comma
	    if (!strcmp(tok, "]")) break;	// "]" means end of list
	    else {
		cerr << "Didn't find end of " << command << " list in "
		    << streamname << endl;
		exit(1);
	    }
    }
    if (which==COORD_INDEX) {
	ntri = n;
	cout << "read " << n << " triangles" << endl;
    }
    else assert(n==ntri);
}

void Object::inventor_read(istream &s, const char *streamname) {
    // read an Inventor file, returning a texture, a set of points,
    // and a list of triangles

    char tok[TOKEN_SIZE];

    cout << "reading " << streamname << endl;
    while (s >> setw(sizeof tok) >> tok) {
        // cout << "(" << tok << ")" << endl;
	if (!strcmp(tok, "#")) {
	    // gobble comment
	    s.ignore(1000, '\n');
	}
	else if (!strcmp(tok, "Texture2"))
	    inventor_read_texture2(s, streamname);
	else if (!strcmp(tok, "TextureCoordinate2"))
	    inventor_read_texturecoordinate2(s, streamname);
	else if (!strcmp(tok, "Coordinate3"))
	    inventor_read_coordinate3(s, streamname);
	else if (!strcmp(tok, "coordIndex"))
	    inventor_read_coordindex(s, streamname, COORD_INDEX);
	else if (!strcmp(tok, "textureCoordIndex"))
	    inventor_read_coordindex(s, streamname, TEXTURE_COORD_INDEX);
    }
    cout << "done with " << streamname << endl;
}

int Object::inventor_read(const char *file) {
    ifstream s(file, ios::in);
    if (!s) {
	cerr << "inventor_read: can't read " << file << endl;
	return 0;
    }
    inventor_read(s, file);
    s.close();
    return 1;
}

// end Open Inventor-specific code

//----------------------------------------------------------------------

#define EPS -1e-3
    // tolerance for roundoff error on point-in-triangle testing.
    // using floats (not doubles), it seems to be necessary to use an
    // epsilon, instead of testing against 0,
    // else some grid points fall through the cracks
    // (are considered to be outside all the triangles)

void interpolate(Spoint *v[3], Line edge[2], float x, float y, Gpoint &gp) {
    // test if point (x,y) is inside triangle with vertices v[0], v[1], v[2]
    // and edge equations edge[0], edge[1],
    // and interpolate grid point gp (z,r,g,b) there, if so

    int i;
    float bary[3];		// barycentric coordinates
    for (i=0; i<2; i++) {
	bary[i] = edge[i].a*x + edge[i].b*y + edge[i].c;
	if (bary[i] < EPS) return;	// if point was outside this edge
    }
    // optimization: compute bary[2] using invariant bary0+bary1+bary2=1
    // that way we don't need edge[2]
    bary[2] = 1-bary[0]-bary[1];
    if (bary[2] < EPS) return;		// if point was outside this edge
    // if we get to here, point (x,y) is on or in triangle

    // interpolate z,r,g,b at (x,y)
    // no need for range checking because colors we started with were within
    // range and this interpolation obeys the convex hull property
    gp.gz =  bary[0]*v[0]->gr.z + bary[1]*v[1]->gr.z + bary[2]*v[2]->gr.z;
    gp.r  = (unsigned char)
	    (bary[0]*v[0]->r    + bary[1]*v[1]->r    + bary[2]*v[2]->r);
    gp.g  = (unsigned char)
	    (bary[0]*v[0]->g    + bary[1]*v[1]->g    + bary[2]*v[2]->g);
    gp.b  = (unsigned char)
	    (bary[0]*v[0]->b    + bary[1]*v[1]->b    + bary[2]*v[2]->b);

    // printf("  (%2.0f,%2.0f) bary=(%5.3f,%5.3f,%5.3f) zrgb=%5.2f,%3d,%3d,%3d\n",
	// x, y, bary[0], bary[1], bary[2], gp.gz, gp.r, gp.g, gp.b);
}

void grid_resample_triangle(Spoint *p, Spoint *q, Spoint *r, Grid_data &gd) {
    // resample triangle pqr into grid,
    // find grid points that lie within it using a ray-tracing-like approach
    // printf("resample_triangle %d,%d,%d\n", p-pt, q-pt, r-pt);

    Spoint *v[3] = {p, q, r};	// vertices of triangle

    // find a bounding box around triangle in grid space
    int i;
    float gxmin = HUGE, gxmax = -HUGE, gymin = HUGE, gymax = -HUGE;
    for (i=0; i<3; i++) {
	if (v[i]->gr.x<gxmin) gxmin = v[i]->gr.x;
	if (v[i]->gr.x>gxmax) gxmax = v[i]->gr.x;
	if (v[i]->gr.y<gymin) gymin = v[i]->gr.y;
	if (v[i]->gr.y>gymax) gymax = v[i]->gr.y;
    }

    int gx0 = (int)ceil (gxmin);
    int gx1 = (int)floor(gxmax);
    int gy0 = (int)ceil (gymin);
    int gy1 = (int)floor(gymax);
    if (gx0<0) gx0 = 0;
    if (gx1>=gd.nx) gx1 = gd.nx-1;
    if (gy0<0) gy0 = 0;
    if (gy1>=gd.ny) gy1 = gd.ny-1;
    if (gx0>gx1 || gy0>gy1) return;	// bbox was entirely out of bounds

    // compute line equations for edges of triangle
    Line edge[2];
    edge[0].set(v[1]->gr, v[2]->gr, v[0]->gr);
    edge[1].set(v[2]->gr, v[0]->gr, v[1]->gr);
    // edge[2].set(v[0]->gr, v[1]->gr, v[2]->gr);  // we don't need it

    // interpolate at integer grid points inside triangle (if any)
    int gx, gy;
    for (gy=gy0; gy<=gy1; gy++)
	for (gx=gx0; gx<=gx1; gx++)
	    interpolate(v, edge, gx, gy, gd.grid[gy][gx]);
}

void Object::transform_points(int nx, int ny) {

    // coordinate systems:
    //
    //   world space is the coordinates in the Inventor file,
    //     its origin is at the camera, so McMillan's cdot is (0,0,0)
    //     stored in (x,y,z) in Spoint struct
    //
    //   projected space is (px,py,pz)=(-x/z,-y/z,-1/z)
    //
    //   grid space is (gx,gy,gz) where 0<=gx<nx, 0<=gy<ny, and
    //     this is the screen space of the reference image
    //     px = pxmin+gx/(nx-1.)*(pxmax-pxmin)
    //     py = pymin+gy/(ny-1.)*(pymax-pymin)
    //	   pz = gz
    //     it is sampled on an integer grid

    if (zoom<=0) {
	cerr << "you forgot to call extract_camera()?" << endl;
	exit(1);
    }
    float xscale = (nx-1.)/(pxmax-pxmin);
    float yscale = (ny-1.)/(pymax-pymin);

    // transform world space to grid space
    int i;
    for (i=0; i<npt; i++) {
	pt[i].gr.x = (-pt[i].wo.x/pt[i].wo.z-pxmin)*xscale;
	pt[i].gr.y = (-pt[i].wo.y/pt[i].wo.z-pymin)*yscale;
	pt[i].gr.z = -1/pt[i].wo.z;
    }
}

struct Cam_info {float pmax, slope;};
    // info about the parameters of a zoom setting of the lens
    //   * px and py each range from [-pmax:pmax]
    //   * the correspondence between (s,t) and (px,py) is approximately
    //        px = slope*(s-.5)
    //        py = slope*(t-.5)

#define NZOOM 8

// parameters of the Vivid's 8 zoom settings, determined empirically

Cam_info cam[NZOOM+1] = {
    1, 1,
    .216, .429, // zoom 1
    .181, .360, // zoom 2
    .145, .294, // zoom 3
    .116, .234, // zoom 4
    .091, .185, // zoom 5
    .072, .146, // zoom 6
    .058, .118, // zoom 7
    .047, .097, // zoom 8
};

void Object::extract_camera(Grid_data &gd) {
    // extract camera parameters from point set in Object,
    // and save them in Grid_data

    // To guess the Vivid's lens zoom,
    // find the linear fit px(s) = slope*s+offset by least squares.
    // Computing slope is enough to identify the zoom.

    double S = npt, Ss = 0, Sx = 0, Sss = 0, Ssx = 0;
    int i;
    for (i=0; i<npt; i++) {
	float px;
	px = -pt[i].wo.x/pt[i].wo.z;
	Ss += pt[i].s;
	Sx += px;
	Sss += (double)pt[i].s*pt[i].s;
	Ssx += (double)pt[i].s*px;
    }

    double slope = (Ssx*S - Ss*Sx)/(Sss*S-Ss*Ss);
    cout << "slope=" << slope << ", ";

    // try to guess the zoom of the Vivid lens
    // by finding the one with the closest slope
    zoom = 0;
    float dist = HUGE;
    for (i=1; i<=NZOOM; i++)
	if (fabs(cam[i].slope - slope) < dist) {
	    zoom = i;
	    dist = fabs(cam[i].slope - slope);
	}
    assert(zoom>0);
    cout << "zoom was " << zoom << endl;
    pxmin = -cam[zoom].pmax;
    pxmax =  cam[zoom].pmax;
    pymin = -cam[zoom].pmax;
    pymax =  cam[zoom].pmax;

    // compute vectors o, u, and v of McMillan's paper

    gd.cam = Point3(0., 0., 0.);
	// world space position of camera (Vivid's convention)

    gd.ovec = Point3(pxmin, pymin, -1.);
	// world space direction of grid space point (0,0) from camera

    gd.uvec = Point3((pxmax-pxmin)/(gd.nx-1.), 0.,  0.);
	// world space direction of grid space vector (1,0)

    gd.vvec = Point3(0., (pymax-pymin)/(gd.ny-1.), 0.);
	// world space direction of grid space vector (0,1)

	// IMPORTANT NOTE: this vvec points up (origin in lower left of image)
	// whereas McMillan's points down (origin in upper left of image)
	// so you'll need to make some changes to the algorithm to
	// compensate.  This flips the sign of w, for instance.

    // The coordinate system used by the Vivid has world z pointing
    // out of the picture, not in. Consequently, ALL THE GZ'S IN THE
    // GRID WILL BE NEGATIVE.

    // By setting ovec.z=-1 and uvec.z=vvec.z=0 above, we give the
    // projection plane an (arbitrary) depth of -1.
    // This choice along with the formula gz=pz=-1/z
    // means that McMillan's generalized disparity
    //   delta = len(ovec+gx*uvec+gy*vvec) / sqrt(x*x+y*y+z*z)
    // and our
    //   gz = -1/z
    // are exactly equal, by similar triangles!

    // In other words, McMillan's "generalized disparity" is just a special
    // case of "perspective depth", as defined in, say,
    // Newman & Sproull, "Principles of Interactive Computer Graphics",
    // 1979, p. 356.
}

#ifdef OLD // ----------------------------------- for data gathering

void Object::old_extract_camera(Grid_data &gd) {
    // extract camera parameters from point set in Object,
    // and save them in Grid_data

    // to guess the Vivid's lens zoom, compute a bounding box of
    // projected coordinates (-x/z,-y/z)
    // 'd' for data

    // also find the linear fit px(s) = a*s+b, py(t) = c*t+d
    // by least squares

    float dxmin = HUGE, dxmax = -HUGE, dymin = HUGE, dymax = -HUGE;
    double S = npt, Ss = 0, St = 0, Sx = 0, Sy = 0,
	Sss = 0, Stt = 0, Ssx = 0, Sty = 0;
    int i;
    for (i=0; i<npt; i++) {
	float px, py;
	px = -pt[i].wo.x/pt[i].wo.z;
	py = -pt[i].wo.y/pt[i].wo.z;
	// cout << pt[i].s << " " << px << " " << pt[i].t << " " << py << endl;
	if (px<dxmin) dxmin = px;
	if (px>dxmax) dxmax = px;
	if (py<dymin) dymin = py;
	if (py>dymax) dymax = py;

	Ss += pt[i].s;
	Sx += px;
	Sss += (double)pt[i].s*pt[i].s;
	Ssx += (double)pt[i].s*px;

	St += pt[i].t;
	Sy += py;
	Stt += (double)pt[i].t*pt[i].t;
	Sty += (double)pt[i].t*py;
    }
    float dmax = -dxmin > dxmax ? -dxmin : dxmax;
    if (-dymin > dmax) dmax = -dymin;
    if (dymax > dmax) dmax = dymax;

    cout << "bbox of projected: "
	<< "x [" << dxmin << ":" << dxmax << "] "
	<< "y [" << dymin << ":" << dymax << "] "
	<< "dmax=" << dmax << endl;

    double a = (Ssx*S - Ss*Sx)/(Sss*S-Ss*Ss);
    double b = (Sss*Sx-Ssx*Ss)/(Sss*S-Ss*Ss);
    double c = (Sty*S - St*Sy)/(Stt*S-St*St);
    double d = (Stt*Sy-Sty*St)/(Stt*S-St*St);
    cout << "px = " << a << " * s + " << b << endl;
    cout << "py = " << c << " * t + " << d << endl;

    double Sex = 0, Sey = 0;
    for (i=0; i<npt; i++) {
	float px, py, ex, ey;
	px = -pt[i].wo.x/pt[i].wo.z;
	py = -pt[i].wo.y/pt[i].wo.z;
	ex = a*pt[i].s+b - px;
	ey = c*pt[i].t+d - py;
	// cout << "predicted px=" << ex+px << " actual=" << px
	    // << " err=" << ex << endl;
	// cout << "predicted py=" << ey+py << " actual=" << py
	    // << " err=" << ey << endl;
	Sex += ex*ex;
	Sey += ey*ey;
    }
    cout << "rms err:  x=" << sqrt(Sex/npt) << " y=" << sqrt(Sey/npt) << endl;
    exit(1);

    ...
}

#endif // OLD ----------------------------------- end data gathering

void Object::print_transform() {
    printf("transform\n");
    int i;
    for (i=0; i<npt; i++)
	printf("  pt[%2d] wo=(%g,%g,%g) pr=(%g,%g) gr=(%g,%g,%g)\n",
	    i,
	    pt[i].wo.x, pt[i].wo.y, pt[i].wo.z,
	    -pt[i].wo.x/pt[i].wo.z, -pt[i].wo.y/pt[i].wo.z,
	    pt[i].gr.x, pt[i].gr.y, pt[i].gr.z);
}

void Object::grid_compute(Grid_data &gd) {
    // Compute a grid that resamples the triangles.
    // This is necessary because we want a regular grid of points, but the
    // point set output by the Vivid is not on a regular grid (points deviate
    // from a regular grid in a scene-dependent way, apparently related to
    // surface orientation).
    // Also, the point set in the inventor file often has missing points
    // because of unknown depths.  We fill in these missing points using a
    // large depth value.

    float gzmin = -1/wzmin;	// transform from world to grid space
    float gzmax = -1/wzmax;
    float zunk = gzmin + .05*(gzmin-gzmax);
	// depth to assign to grid points of unknown depth
	// (chosen to be just a bit beyond the deepest known point)
    cout << "gz range: [" << gzmin << ":" << gzmax << "]"
	<< " zunk=" << zunk << endl;

    // clear the depths to the far value and set all the colors
    int gx, gy;
    for (gy=0; gy<gd.ny; gy++)
	for (gx=0; gx<gd.nx; gx++) {
	    gd.grid[gy][gx].gz = zunk;
	    get_color_bilinear(gx/(gd.nx-1.), gy/(gd.ny-1.),
		&gd.grid[gy][gx].r, &gd.grid[gy][gx].g, &gd.grid[gy][gx].b);
		// no lens distortion correction here.
		// the colors we compute here are overwritten
		// at grid points where depth is known, in code below
    }

    // transform all points from world space to grid space
    extract_camera(gd);			// fill in gd.ovec, uvec, vvec, zoom
    transform_points(gd.nx, gd.ny);	// fill in v[i].gr

    // interpolate depths and colors from the triangles, in grid space
    int i;
    for (i=0; i<ntri; i++)
	grid_resample_triangle(tri[i].v[0], tri[i].v[1], tri[i].v[2], gd);
}

void Object::print_points() {
    int i;
    printf("\n%d points\n# X Y Z R G B\n", npt);
    for (i=0; i<npt; i++)
	printf("%g %g %g %d %d %d\n",
	    pt[i].wo.x, pt[i].wo.y, pt[i].wo.z,
	    pt[i].r, pt[i].g, pt[i].b);
}

//----------------------------------------------------------------------

Grid_data::Grid_data(int nx_, int ny_) {
    // allocate a grid of nx_ by ny_ points
    nx = nx_;
    ny = ny_;
    Gpoint *garray = new Gpoint[nx*ny];
    grid = new Gpoint *[ny];
    int y;
    for (y=0; y<ny; y++)
	grid[y] = &garray[y*nx];	// ptr to beginning of row y
}

Grid_data::~Grid_data() {
    assert(nx>0 && ny>0);
    delete grid[0];	// delete the array of Gpoints
    delete grid;	// delete the array of row pointers
}

void Grid_data::print() {
    printf("\ncam=(%g,%g,%g) ovec=(%g,%g,%g)\n",
	cam.x, cam.y, cam.z, ovec.x, ovec.y, ovec.z);
    printf("uvec=(%g,%g,%g) vvec=(%g,%g,%g)\n",
	uvec.x, uvec.y, uvec.z, vvec.x, vvec.y, vvec.z);

    int gx, gy;
    printf("# x y gz(delta) r g b\n");
    for (gy=0; gy<ny; gy++)
	for (gx=0; gx<nx; gx++) {
	    // printf("(%2d,%2d) gz=%7.5f rgb=(%3d,%3d,%3d)\n",
	    printf("%3d %3d %10.8f %3d %3d %3d\n",
		gx, gy, grid[gy][gx].gz,
		grid[gy][gx].r, grid[gy][gx].g, grid[gy][gx].b);
	}
}

int Grid_data::inventor_read_and_resample(char *filename) {
    // read Open Inventor file and create grid
    Object ob;
    if (!ob.inventor_read(filename))
	return 0;
    // ob.print_points();
    // ob.print_grid();

    ob.grid_compute(*this);
    return 1;
}