/*****************************************************************************\
                         3D Spatial Evidence Accumulation
		          For Robot Sensor Interpretation
			           Hans Moravec
				      1992
\*****************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cmacs.h>
#define  is_volsense  /* switch for volsense.h */
#include "volsense.h"
   
MainInt LfP(MainFloat p)
 /*** 3D ***  Convert real probability to scaled integer log odds */
     {MainInt n, ip;
      p = LOG(fabs(p/(1.0-p)))*5.770780164;    /* log base 2^(1/4) */
      n = 0;  if (p<0) {p = -p; n = 1;}
      ip = p + 0.5;  if (ip>127) ip = 127;
      return(n?-ip:ip);
    }


MainInt ILOG2(MainInt i)
 /*** 3D *** Floor[Log2[i]] */
      { MainInt t, j;
	if ((j = i & ~BMx)) {t = 16; i=j;} else t = 0;
	if ((j = i & ~BM8)) {t += 8; i=j;};
	if ((j = i & ~BM4)) {t += 4; i=j;};
	if ((j = i & ~BM2)) {t += 2; i=j;};
	return ((i & ~BM1)?++t:t);
      }


MainInt ILOG2C(MainInt i)
 /*** 3D *** Ceil[Log2[i]] */
      { MainInt j;
	j = ILOG2(i);  return (((1<<j)<i)?++j:j);
      }


MainInt ILOG2R(MainInt i)
 /*** 3D *** Round[Log2[i]] -> Floor[Log2[i]+.5] -> Floor[Log2[i*Sqrt[2]]] */
      { return ILOG2(i + (i>>2) + (i>>3) + (i>>5) + (i>>7)); }


MainInt MakeMap3D(MainInt msize[3],  
		   MainFloat lov[3], MainFloat hiv[3],  Map3D *map)
 /*** 3D ***************************************************************\
 *	MakeMap3D(msize, lov, hiv, map)
 *	      Creates a new map "isize" wide by "jsize" deep by
 *             "ksize" high with co-ordinates
 *                       {lov:hiv [0:2], in meters}
 *             Each cell has dimensions  "(hiv-lov)/msize"
 \**********************************************************************/
     { MainInt i, js;
       js = 0;
       for (i=0; i<3; i++)
	 { (*map).msize[i] = msize[i];  js += ILOG2C(msize[i]);
	   (*map).lomv[i] = lov[i];   (*map).himv[i] = hiv[i]; };
       (*map).mapm = (Int16 *) calloc( 1<<js , sizeof(Int16) );
       return((MainInt) (*map).mapm);
     }


void FreeMap3D(Map3D map)
 /*** 3D ***************************************************************\
 *	FreeMap3D(map)
 *	      Release a previously allocated 3D map
 \**********************************************************************/
     { 
       free(map.mapm);
     }


void AddCylReading(MainFloat range, MainFloat Pos[3], MainFloat Dir[3],
		   CylSensorModelArray smodels,  Map3D map)
 /*** 3D ***************************************************************\
 *	AddCylReading(range, Pos, Dir, smodel, map)
 *	  Adds a reading of range "range" recorded at "Pos" oriented
 *	  in direction "Dir", using sensor model array "smodel"
 \**********************************************************************/
     {
      BestFloat slor, slod, shid, shir;  	/* sensor model parameters */
      MainInt   srsize, sdsize, srdsize, srlog;	/* sensor model index bounds */
      MainInt   srdmask, srdoff;
      Int8 *smodelm;				/* sensor model array */
      Int16 *sprofile;				/* sensor model silhouette */
      BestFloat srscale, sdscale;
      BestFloat mlox, mloy, mloz, mhix, mhiy, mhiz;	/* map parameters */
      MainInt   mxsize, mysize, mzsize, mxyzsize, mxyzmask;
      Int16  *mmapm;				/* map array */
      BestFloat mxscale, myscale, mzscale;
      MainInt   mindex[3], mindx, mindy, mindz; /* for permuted indexing */
      MainInt lx, ly, lz, i, nm, nmt, nmh;
      BestFloat rangefactor, tx, ty, tz, td, t, x, y, d, d1, dx1, dy1, y2;
      BestFloat px, py, pz,  dx, dy, dz, adz, dxdx, dydy, dzdz;
      BestFloat elldx, elldy, elldz, zlo, zhi;
      BestFloat ys, yf, surc, surf, tyf, tslor, tslors, tslod;
      MainInt izlo, izhi, ellxhd, ellyhd, ellxsz, ellysz;
      MainInt ix, iy, iz, gy, ir, id, itdd, tmzpos, mxpos, mypos;
      MainInt  ip, ipi, ipf, j, ixlo, ixhi, did, dl, dh, dv;
      Int32 epx, epy, itx, ity, idd, idinc;
      Int32 *ellrdtab, *ellxytab, *ellp, *ellpinit, ellpmax;
      Float32 *xx;     Int16 *rv; 

      /* binary search for sensor model nearest to this range */
      nm = 1; nmh = smodels.nmodels-2;
      while (nm<=nmh) if ((smodels.models[nmt = (nm+nmh)>>1]).rangem < range)
	nm=nmt+1; else nmh=nmt-1;

      rangefactor = range/smodels.models[nm].rangem; 
      /* nudge array to exact range */
      slor = rangefactor*smodels.models[nm].lorv; /* should be zero */
      shir = rangefactor*smodels.models[nm].hirv;	
      tslod = slod = rangefactor*smodels.models[nm].lodv;
      shid = rangefactor*smodels.models[nm].hidv;
      srsize = smodels.models[nm].rsize;
      sdsize = smodels.models[nm].dsize;
      srlog = ILOG2C(srsize);      srdsize = sdsize<<srlog;
      srdoff = ((dl = 1<<ILOG2C(sdsize)) - sdsize)<<srlog;
      srdmask = -(dl<<srlog);
      smodelm = smodels.models[nm].modelm;
      sprofile = smodels.models[nm].profile;
      srscale = (shir-slor)/srsize;
      sdscale = (shid-slod)/sdsize;
      
      /* general mapping between real co-ordinates and a grid:
	 each dimension of grid is defined by three numbers:
	 Glo, Ghi, Gsize. Glo and Ghi are the real coords of
	 the outer bounds of the lowest and highest numbered
	 cells.  Gsize is the number of cells in the dimension.
	 The cell co-ordinates range from 0 to Gsize-1.
	 Gscale, the real extent of a single cell, can be
	 calculated:    Gscale = (Ghi-Glo)/Gsize.

	 Cell index for a particular real co-ord Gp is given by
	        Gindex = Floor((Gp-Glo)/Gscale)
	 real co-ordinate for center of cell Gindex is found by
	        Gp = (Gindex+0.5)*Gscale+Glo			*/

      /* permute axes so that |dz| >= |dx| >= |dy|.  Maximum
	 |dz| is essential; minimum |dy| is slight efficiency. */

      tx = ABS(Dir[0]);  ty = ABS(Dir[1]);  tz = ABS(Dir[2]);
      if (tz>((lz=(ty>tx)?1:0)?ty:tx)) lz = 2;
      if (ABS(Dir[lx=(lz+1)%3])<ABS(Dir[ly=(lz+2)%3])) SWAPi(lx,ly);

      px = Pos[lx];  py = Pos[ly];  pz = Pos[lz];
      dx = Dir[lx];  dy = Dir[ly];  dz = Dir[lz];

      t = SQRT(dx*dx+dy*dy+dz*dz); dx /= t; dy /= t; dz /= t; adz = ABS(dz);
      dxdx = dx*dx;  dydy = dy*dy;  dzdz = dz*dz;

      mindx = ILOG2C(map.msize[0]);   mindy = ILOG2C(map.msize[1]);
      mindz = ILOG2C(map.msize[2]);   mxyzsize = 1<<(mindx+mindy+mindz);
      mxyzmask = -mxyzsize;
      mindex[0] = 0;    mindex[1] = mindx;    mindex[2] = mindx + mindy;
      mindx = mindex[lx];    mindy = mindex[ly];    mindz = mindex[lz];
      mlox = map.lomv[lx];  mloy = map.lomv[ly];  mloz = map.lomv[lz];
      mhix = map.himv[lx];  mhiy = map.himv[ly];  mhiz = map.himv[lz];
      mxsize = map.msize[lx]; mysize = map.msize[ly]; mzsize = map.msize[lz];
      mmapm = map.mapm;
      mxscale = (mhix-mlox)/mxsize;   myscale = (mhiy-mloy)/mysize;
      mzscale = (mhiz-mloz)/mzsize;

      /* Construct intersection ellipse addressing prototype, 
	 as a list of pairs of relative (x,y) co-ordinates in map,
	 and corresponding (r,d) coordinates in sensor model.
	 The cylinder axis passes through middle of the center grid
         cell of the ellipse so defined.

	 Let R be the cylinder radius.  R = shir in the sensor model.
	 In shifted co-ordinates where the evidence ray starts
	 at the origin, the circular disk of the cylinder where
	 d=0 is bounded by

		      x = +/-  R*Sqrt(1 - dx^2)
		      y = +/-  R*Sqrt(1 - dy^2)
		      z = +/-  R*Sqrt(1 - dz^2)

	 The ellipse of intersection of the cylinder with z=0 has
	 a mapping from (x,y) to (r,d) co-ordinates defined by:
	 
	     d = dx*x + dy*y        r^2 = x^2 + y^2 - d^2

	 The extent of this intersection ellipse in x and y is:

             x = +/-  elldx =  R*Sqrt(dx^2/dz^2 + 1)
             y = +/-  elldy =  R*Sqrt(dy^2/dz^2 + 1)

	  Expressing the ellipse as x = f(y), ellipse midline in x is

	           xm  =  y * dx*dy/(1-dx^2)   

          then:   d   =    y * dy/(1-dx^2)    +  (x-xm)   * dx
	          r^2 =  y^2 * dz^2/(1-dx^2)  +  (x-xm)^2 * (1-dx^2)

	 at a particular y value, the ellipse x extent is given by

             x =  xm  +/- Sqrt(R^2/(1-dx^2) - (y * dz/(1-dx^2))^2 )

      */

      elldx = shir*SQRT(dxdx/dzdz + 1.0); /* max x radius of ellipse */
      elldy = shir*SQRT(dydy/dzdz + 1.0); /* max y radius of ellipse */
      ellxhd = elldx/mxscale;  ellyhd = elldy/myscale; /* radius in pixels */
      ellxsz = 1+ellxhd+ellxhd; ellysz = 1+ellyhd+ellyhd; /* diam in pixels */ 
      
      if (ellxsz==1 && ellysz==1)  /* very thin ray - go real fast! */
	{
	  if (ABS(dx)>1e-7)  /* clip by x limits */
	    { zlo = (mlox+(t=mxscale*1e-4)-px)/dx;  zhi = (mhix-t-px)/dx;
	      if (zlo>zhi) SWAPf(zlo,zhi);
	      if (zlo>tslod) tslod=zlo;  if (zhi<shid) shid=zhi;
	      if (tslod>shid) return; }
	  else if (px<mlox || px>mhix) return;
	  
	  if (ABS(dy)>1e-7) /* clip by y limits */
	    { zlo = (mloy+(t=myscale*1e-4)-py)/dy;  zhi = (mhiy-t-py)/dy;
	      if (zlo>zhi) SWAPf(zlo,zhi);
	      if (zlo>tslod) tslod=zlo;   if (zhi<shid) shid=zhi;
	      if (tslod>shid) return; }
	  else if (py<mloy || py>mhiy) return;

	  zlo = pz+tslod*dz;   zhi = pz+shid*dz;  /* clip by z limits */  
	  if (zlo>zhi) SWAPf(zlo,zhi);
	  izlo=(zlo-mloz)/mzscale+0.5;  izhi=(zhi-mloz)/mzscale+0.5;
	  if (izlo<0) izlo=0;  if (izhi>=mzsize) izhi=mzsize-1;   /* clip */
	  if (izlo>=izhi) return;

	  d   = ((izlo+0.5)*mzscale+mloz-pz)/dz;  td   = mzscale/dz;
	  idd = FXP((d-slod)/sdscale);         itdd = FXP(td/sdscale);
	  epx = FXP((px+d*dx-mlox)/mxscale);   itx  = FXP(td*dx/mxscale);
	  epy = FXP((py+d*dy-mloy)/myscale);   ity  = FXP(td*dy/myscale);
	  tmzpos = 1<<mindz;   izlo = izlo<<mindz;   izhi = izhi<<mindz;

	  /* Step through Z planes between ray Z bounds, adding evidence */
	  if (srlog==0)
	    if(mindx==0)
	      for(iz=izlo;iz<izhi;iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
		mmapm[(epx>>16)+((epy>>16)<<mindy)+iz] += smodelm[idd>>16];
	    else if (mindy==0)
	      for(iz=izlo;iz<izhi;iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
		mmapm[((epx>>16)<<mindx)+(epy>>16)+iz] += smodelm[idd>>16];
	    else
	      for(iz=izlo;iz<izhi;iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
	      mmapm[((epx>>16)<<mindx)+((epy>>16)<<mindy)+iz]+=smodelm[idd>>16];
	  else
	    if(mindx==0)
	      for(iz=izlo;iz<izhi;iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
		mmapm[(epx>>16)+((epy>>16)<<mindy)+iz]
		  += smodelm[(idd>>16)<<srlog];
	    else if (mindy==0)
	      for(iz=izlo;iz<izhi;iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
		mmapm[((epx>>16)<<mindx)+(epy>>16)+iz]
		  += smodelm[(idd>>16)<<srlog];
	    else
	      for(iz=izlo;iz<izhi;iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
		mmapm[((epx>>16)<<mindx)+((epy>>16)<<mindy)+iz]
		  += smodelm[(idd>>16)<<srlog];
	}
      else /* we must paint fat rays the old fashioned way */
	{
	  /* determine z and d protrusion of ray profile beyond its axis */
	  
	  if (ABS(dx)>1e-7)  /* clip by x limits */
	    { zlo = (mlox-elldx-px)/dx;  zhi = (mhix+elldx-px)/dx;
	      if (zlo>zhi) SWAPf(zlo,zhi);
	      if (zlo>tslod) tslod=zlo;  if (zhi<shid) shid=zhi;
	      if (tslod>shid) return; }
	  else if (px<mlox-elldx || px>mhix+elldx) return;
	  
	  if (ABS(dy)>1e-7) /* clip by y limits */
	    { zlo = (mloy-elldy-py)/dy;  zhi = (mhiy+elldy-py)/dy;
	      if (zlo>zhi) SWAPf(zlo,zhi);
	      if (zlo>tslod) tslod=zlo;   if (zhi<shid) shid=zhi;
	      if (tslod>shid) return; }
	  else if (py<mloy-elldy || py>mhiy+elldy) return;
	  
	  elldz = SQRT(1.0-dzdz);  d1 = sdscale*adz;  tyf = srscale*elldz;
	  dv = shir*elldz/d1;       dv += 2;

	  rv = (Int16 *) calloc((1+dv+dv),sizeof(Int16));
	  ir = slor/srscale+2;  j = (i = dv)*srsize;
	  while ((rv[dv-i] = rv[dv+i] = j/dv-ir)>0) {i--; j -= srsize;}
	  rv[dv-i] = rv[dv+i] = 0;   ip = dv<sdsize?dv:sdsize-1;
	  dl = sprofile[0];   dh = sprofile[sdsize-1];
	  for (id = 1; id <= ip; id++)
	    { if ((i = sprofile[id]-(j=rv[dv-id])) > dl)   dl=i;
	      if ((i = sprofile[sdsize-1-id]-j)    > dh)   dh=i; }

	  zlo = pz+tslod*dz;  zhi = pz+shid*dz;
	  if (zlo>zhi) {SWAPf(zlo,zhi); SWAPi(dl,dh);}
	  zlo -= dl*tyf;    zhi += dh*tyf;
	  izlo=(zlo-mloz)/mzscale+0.5;   izhi=(zhi-mloz)/mzscale+0.5;
	  if (izlo<0) izlo=0; if (izhi>=mzsize) izhi=mzsize-1;  /* clip */
	  if (izlo>=izhi) return;

	  /* Create pointers for ellipse, bucket sorted by radius.
	     Bucket for each radius has enough room for the number of cells
	     that might be occupied in raster by circle of that radius, plus
	     four cells, to avoid collisions between adjacent buckets.      */
	  
	  iz = ellxsz*MAX(srsize,ellysz);
	  ellxytab = (Int32 *)malloc(i=(ellpmax=iz+(srsize<<2))*sizeof(Int32));
	  ellrdtab = (Int32 *) malloc(i);
	  ellpinit = (Int32 *) malloc(i=(srsize+1)*sizeof(Int32));
	  ellp     = (Int32 *) malloc(i);
	  t = srsize;  t = iz/(t*(t+1.0));
	  for (i=0; i<=srsize; i++) ellpinit[i] = ellp[i] = (t*i)*(i+1)+(i<<2);
	  
	  /* set up constants for ellipse (x,y) -> sensor model (r,d) map */
	  
	  x = -ellxhd*(tx=mxscale/srscale);
	  y = -ellyhd*(ty=myscale/srscale);  td = srscale/sdscale;
	  d = d1 = dy*y + dx*x;    dx1 = dx*tx;   dy1 = dy*ty;
	  yf = surf = (surc = 1.0/(dydy+dzdz))/tx;
	  t = shir/mxscale;     surc *= t*t;
	  surf *= dzdz*surf;     tyf = (yf *= dx*dy)*ty;
	  did = ((ellysz-1)<<mindy) | ((ellxsz-1)<<mindx);
	  ys = y*yf + (ellxhd + 1);
	  xx = (Float32 *) malloc(ellxsz*sizeof(Float32)); xx[ellxhd] = 0.0;
	  for (ix=0; ix<ellxhd; ++ix, x+=tx) xx[ix] = xx[ellxsz-ix-1] = x*x;
	  
	  if (srsize<ISQRTSZ && slor==0.0) /* the usual condition */
	    {
	      for (iy=0; iy<ellyhd; ++iy, d1+=dy1, ys+=tyf, y+=ty)
		{ gy = iy<<mindy;
		  ixlo = ys-(t=SQRT(surc-surf*(y2=y*y))); ixhi=ys+t;
		  d = d1+ixlo*dx1;
		  for (ix=ixlo; ix<ixhi; ++ix, d += dx1)
		  { ir = ISQRT[(MainInt) (xx[ix] + y2 - d*d)];
		     printf ("xx[ix] + y2 - d*d = %f, (MainInt) (<-) = %d, ISQRT[<-] = %d, ISQRT[2] = %d, ir = %d\n, ISQRT = 0x%x",
			     xx[ix] + y2 - d*d, (MainInt) (xx[ix] + y2 - d*d),
			     ISQRT[(MainInt) (xx[ix] + y2 - d*d)],
			     ISQRT[2], ir, ISQRT);
		      ellxytab[i = ellp[ir]++] = j = gy | (ix<<mindx);
		      ellrdtab[i] =  (id = ((MainInt) ROUND(d*td))<<srlog) | ir;
		      ellxytab[i = ellp[ir]++] = did-j;
		      ellrdtab[i] = (-id)|ir; }
		}
	      gy = ellyhd<<mindy;
	      ixlo=ellxhd-(i=SQRT(surc)); ixhi=ellxhd+i; d= -i*dx1;
	      for (ix=ixlo; ix<=ixhi; ++ix, d += dx1)
		{ ir = ISQRT[(MainInt) (xx[ix] - d*d)];
		  ellxytab[i = ellp[ir]++] = j = gy | (ix<<mindx);
		  ellrdtab[i] =  (id = ((MainInt) ROUND(d*td))<<srlog) | ir; }
	    }
	  else   /* we must do more tests */
	    {
	      tslor = slor/srscale;  tslors = tslor*tslor-1e-7;
	      for (iy=0; iy<ellyhd; ++iy, d1 += dy1, ys += tyf, y += ty)
		{ gy = iy<<mindy;
		  ixlo = ys-(t=SQRT(surc-surf*(y2=y*y))); ixhi=ys+t;
		  d = d1+ixlo*dx1;
		  for (ix=ixlo; ix<ixhi; ++ix, d += dx1)
		    if ((t=xx[ix]+y2-d*d)>tslors)
		      { ir = (t<ISQRTSZ2)?ISQRT[(MainInt) (t)]:SQRT(t);
			ellxytab[i = ellp[ir]++] = j = gy | (ix<<mindx);
			ellrdtab[i] =  (id = ((MainInt) ROUND(d*td))<<srlog) | ir;
			ellxytab[i = ellp[ir]++] = did-j;
			ellrdtab[i] = (-id)|ir;  };
		};
	      gy = ellyhd<<mindy;
	      ixlo=ellxhd-(i=SQRT(surc)); ixhi=ellxhd+i; d= -i*dx1;
	      for (ix=ixlo; ix<=ixhi; ++ix, d += dx1)
		if ((t=xx[ix]-d*d)>tslors)
		  { ir = (t<ISQRTSZ2)?ISQRT[(MainInt) (t)]:SQRT(t);
		    ellxytab[i = ellp[ir]++] = j = gy | (ix<<mindx);
		    ellrdtab[i] =  (id = ((MainInt) ROUND(d*td))<<srlog) | ir;  };
	    };
	  free(xx);
	  
	  /* compactify the radius-ordered lists */
	  ip = ellp[0];
	  for (i=1; i<srsize; i++)
	    { ipi = ellpinit[i];  ipf = ellp[i];
	      ellpinit[i] = ip;
	      for (j=ipi; j<ipf; j++)
		{ ellxytab[ip] = ellxytab[j];
		  ellrdtab[ip++] = ellrdtab[j]; }
	    }
	  ellpinit[srsize]=ip;
	  free(ellp); 
	  
	  /*   Step through constant Z planes between ray Z bounds,
	       and add evidence to elliptical cross-sections.
	   !! Can step out of x and y bounds by ray diameter !! */
	  
	  d = ((izlo+0.5)*mzscale+mloz-pz)/dz;   td = mzscale/dz;
	  idd = FXP((d-slod)/sdscale);        itdd = FXP(td/sdscale);
	  epx = FXP((px+d*dx-mlox)/mxscale);  itx  = FXP(td*dx/mxscale);
	  epy = FXP((py+d*dy-mloy)/myscale);  ity  = FXP(td*dy/myscale);
	  tmzpos = 1<<mindz;   izlo = izlo<<mindz;   izhi = izhi<<mindz;

	  if (srdoff==0) /* fast d bounds test if sdsize is 2^n */
	    for (iz=izlo; iz<izhi; iz+=tmzpos,epx+=itx,epy+=ity,idd+=itdd)
	      {	mypos = (((epx>>16)-ellxhd)<<mindx)
		       +(((epy>>16)-ellyhd)<<mindy)+iz;
		idinc = ((dh=(idd>>16))<<srlog);
		
		/* determine max r in current V wedge */
		ir = (dh>=0&&dh<sdsize)?sprofile[dh]:0;
		if ((i=dl=dh-dv)<0) dl=0;  if ((dh += dv)>=sdsize) dh=sdsize-1;
		for (id = dh; id >= dl; id--)
		  if ((j=sprofile[id])>ir && j>=rv[id-i]) ir=j;
		
		ipf = ellpinit[ir];
		for (ip = 0; ip < ipf; ip++)
		  if( ((id = ellrdtab[ip]+idinc)&srdmask)==0 &&
		     (i = smodelm[id]) != 0 &&
		     ((mxpos = ellxytab[ip]+mypos)&mxyzmask)==0 )
		    mmapm[mxpos] +=  i;
	      }
	  else        /* slightly hairier bounds test on d */
	    for (iz=izlo; iz<izhi; iz += tmzpos,epx+=itx,epy+=ity,idd+=itdd)
	      {	mypos = (((epx>>16)-ellxhd)<<mindx)
		       +(((epy>>16)-ellyhd)<<mindy)+iz;
		idinc = ((dh=(idd>>16))<<srlog);
		
		/* determine max r in current V wedge */
		ir = (dh>=0&&dh<sdsize)?sprofile[dh]:0;
		if ((i=dl=dh-dv)<0) dl=0;  if ((dh += dv)>=sdsize) dh=sdsize-1;
		for (id = dh; id >= dl; id--)
		  if ((j=sprofile[id])>ir && j>=rv[id-i]) ir=j;
		
		ipf = ellpinit[ir];
		for (ip = 0; ip < ipf; ip++)
		  if( ((((id = ellrdtab[ip]+idinc)+srdoff)|id)&srdmask)==0 &&
		     (i = smodelm[id]) != 0 &&
		     ((mxpos = ellxytab[ip]+mypos)&mxyzmask)==0 )
		    mmapm[mxpos] +=  i;
	      };

	  free(ellpinit); free(ellrdtab); free(ellxytab); free(rv);
	}

    }


void ReadCylModel(char *sfilename,  CylSensorModelArray *smodels)
 /*** 3D ***************************************************************\
 *		ReadCylModel(smodelfile,&smodels)
 *		creates a set of sensor models in "smodels" from "file"
 \**********************************************************************/
     {MainInt i, rs, ds, ri, di, log2rs, it, n;  FILE *sfile;

      sfile=fopen(sfilename,"r");
      fscanf(sfile,"%d\n",&(*smodels).nmodels);

      (*smodels).models = (CylSensorModel *) 
	calloc((*smodels).nmodels,sizeof(CylSensorModel));

      for (i=0; i<(*smodels).nmodels; i++)
	{fscanf(sfile,"%f%d%d%f%f%f%f\n",
		&((*smodels).models[i].rangem),
		&((*smodels).models[i].rsize),
		&((*smodels).models[i].dsize),
		&((*smodels).models[i].lorv),
		&((*smodels).models[i].lodv),
		&((*smodels).models[i].hirv),
		&((*smodels).models[i].hidv));

	 rs = (*smodels).models[i].rsize;
	 ds = (*smodels).models[i].dsize;
	 log2rs = ILOG2C(rs);  /* pad r direction to power of 2 */
	 (*smodels).models[i].modelm = 
	   (Int8 *) calloc((1<<log2rs)*ds,sizeof(Int8));   /* The grid */
	 (*smodels).models[i].profile =
	   (Int16 *) calloc(ds,sizeof(Int16));   /* The silhouette */
	 n = 0;
	 for (di = 0; di<ds; ++di)
	   {
	     for (ri = 0; ri < rs; ++ri) 
	       {fscanf(sfile,"%d ",&it);
		(*smodels).models[i].modelm[n++] = it; }
	     n += (1<<log2rs)-rs;
	     fscanf(sfile,"\n");
	   }
	 for (di = 0; di<ds; ++di)
	   { fscanf(sfile,"%d ",&it);
		(*smodels).models[i].profile[di] = it; }
	 fscanf(sfile,"\n");
       }
      fclose(sfile);
    }


void  WriteCylModel(CylSensorModelArray smodels,   char *sfilename)
 /*** 3D ***************************************************************\
 *		WriteCylModel(smodels,smodelfile)
 *		writes a set of sensor models in "smodels" to "file"
 \**********************************************************************/
     {MainInt i, rs, ds, ri, di, n, log2rs;  FILE *sfile;

      sfile=fopen(sfilename,"w");
      fprintf(sfile,"%d\n",smodels.nmodels);

      for (i=0; i<smodels.nmodels; i++)
	{fprintf(sfile,"%g %d %d %g %g %g %g\n",
		 smodels.models[i].rangem,
		 smodels.models[i].rsize,
		 smodels.models[i].dsize,
		 smodels.models[i].lorv, smodels.models[i].lodv,
		 smodels.models[i].hirv, smodels.models[i].hidv);

	 rs = smodels.models[i].rsize;
	 ds = smodels.models[i].dsize;
	 log2rs = ILOG2C(rs);

	 n = 0;
	 for (di = 0; di<ds; ++di)
	   {
	     for (ri = 0; ri < rs; ++ri) 
	       {fprintf(sfile,"%d ",smodels.models[i].modelm[n++]); }
	     n += (1<<log2rs)-rs;
	     fprintf(sfile,"\n");
	   }
	 for (di = 0; di<ds; ++di)
	   { fprintf(sfile,"%d ",smodels.models[i].profile[di]); }
	 fprintf(sfile,"\n");
       }
     fclose(sfile);
   }


void TraceCylModel(CylSensorModelArray smodels,   char *sfilename)
/*** 3D ***************************************************************\
*		TraceCylModel(smodels,smodelfile)
*		writes a human-readable trace 
*		of sensor models in "smodels" to "file"
\**********************************************************************/
    {MainInt i, rs, ds, ri, di, log2rs, t;  FILE *sfile;
     
     sfile=fopen(sfilename,"w");
     fprintf(sfile,"%d Models\n",smodels.nmodels);
     
     for (i=0; i<smodels.nmodels; i++)
       {fprintf(sfile,"range %g  size %d %d  bounds (%g %g) : (%g %g)\n",
		smodels.models[i].rangem,
		rs = smodels.models[i].rsize, ds = smodels.models[i].dsize,
		smodels.models[i].lorv, smodels.models[i].lodv,
		smodels.models[i].hirv, smodels.models[i].hidv);
	
	fprintf(sfile, "Profile:\n");
	for (di = 0; di < ds; ++di)
	  if ((ri=smodels.models[i].profile[di]/10))
	    fprintf(sfile,"%2d",ri); else fprintf(sfile,"  ");
	fprintf(sfile,"\n");
	for (di = 0; di < ds; ++di)
	  fprintf(sfile,"%2d",smodels.models[i].profile[di]%10);
	fprintf(sfile,"\n\n");

	log2rs = ILOG2C(rs);
	for (ri = 0; ri < rs; ++ri)
	  {
	    for (di = 0; di < ds; ++di)
	      {
		t = smodels.models[i].modelm[(di<<log2rs)+ri];
		fprintf(sfile,"%c%c",t<0?(t<-35?'=':'-'):t>0?(t>35?'#':'+'):' ',
			" 123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ"[(t<0?-t:t)%36]);
	      }
	    fprintf(sfile,"\n");
	  }
      }
     fclose(sfile);
   }


void ClearMap3D(Map3D map)
/*** 3D ***************************************************************\
*      ClearMap3D(map)  -      erase map array
\**********************************************************************/
    { MainInt k, km;
      km = 1<<(ILOG2C(map.msize[0])+ILOG2C(map.msize[1])+ILOG2C(map.msize[2]));
      for (k=0; k<km; k++) map.mapm[k] = 0;
    }


void WriteMap3D(Map3D map, char *title, char *footer, char *mfilename)
/*** 3D ***************************************************************\
*      WriteMap3D(map, title, footer, mfilename)
*		writes a map into a file
\**********************************************************************/
    { MainInt ix, iy, iz, mx, my, mz, lx, ly;  FILE *mfile;
      
      mfile=fopen(mfilename,"w");
      fprintf(mfile,"%s\n",title);
      fprintf(mfile,"%s\n",footer);
      
      fprintf(mfile,"%d %d %d %g %g %g %g %g %g\n",
	      mx = map.msize[0], my = map.msize[1], mz = map.msize[2],
	      map.lomv[0],  map.lomv[1],  map.lomv[2],
	      map.himv[0],  map.himv[1],  map.himv[2] );
      lx = ILOG2C(mx);
      ly = ILOG2C(my)+lx;
      
      for (iz = 0; iz < mz; ++iz)
	{
	  for (iy = 0; iy < my; ++iy)
	    {
	      for (ix = 0; ix < mx; ++ix)
		fprintf(mfile,"%d ",map.mapm[(iz<<ly)+(iy<<lx)+ix]);
	      fprintf(mfile,"\n");
	    }
	  fprintf(mfile,"\n");
	}
      fclose(mfile);
    }


MainInt ReadMap3D(char *mfilename, Map3D *map, char *title, char *footer)
/*** 3D ***************************************************************\
*      ReadMap3D(mfilename, map, title, footer)
*		reads a map from a file
\**********************************************************************/
    { MainInt j, lx, ly, lz, ix, iy, iz;  FILE *mfile;
      Float32 tf0, tf1, tf2, tf3, tf4, tf5;  MainInt it, mx, my, mz;
      
      if(!(mfile=fopen(mfilename,"r"))) return(0);
      fgets(title,100,mfile); j = strlen(title)-1;
      if (title[j]=='\n') title[j]='\0';
      fgets(footer,100,mfile);  j = strlen(footer)-1;
      if (footer[j]=='\n') footer[j]='\0';
      
      fscanf(mfile,"%d%d%d%f%f%f%f%f%f", &mx, &my, &mz,
	     &tf0, &tf1, &tf2, &tf3, &tf4, &tf5);
      
      (*map).msize[0] = mx;  (*map).msize[1] = my;  (*map).msize[2] = mz;
      (*map).lomv[0] = tf0;  (*map).lomv[1] = tf1;  (*map).lomv[2] = tf2;
      (*map).himv[0] = tf3;  (*map).himv[1] = tf4;  (*map).himv[2] = tf5;
      lx = ILOG2C(mx);
      ly = ILOG2C(my)+lx;
      lz = ILOG2C(mz)+ly;
      
      (*map).mapm = (Int16 *) calloc((1<<lz), sizeof(Int16));
           
      for (iz = 0; iz < mz; ++iz)
	for (iy = 0; iy < my; ++iy)
	  for (ix = 0; ix < mx; ++ix)
	    { fscanf(mfile,"%d",&it);
	      (*map).mapm[(iz<<ly)+(iy<<lx)+ix] = it;
	    }
      fclose(mfile);
     return((MainInt) (*map).mapm);
    }


void WriteMap3Db(Map3D map, char *title, char *footer, char *mfilename)
/*** 3D ***************************************************************\
*      WriteMap3D(map, title, footer, mfilename)
*		writes a map into a file, binary mode
\**********************************************************************/
    { MainInt mx, my, mz, lx, ly, lz;  FILE *mfile;
      
      mfile=fopen(mfilename,"w");
      fprintf(mfile,"%s\n",title);
      fprintf(mfile,"%s\n",footer);
      
      fprintf(mfile,"%d %d %d %g %g %g %g %g %g\n",
	      mx = map.msize[0], my = map.msize[1], mz = map.msize[2],
	      map.lomv[0],  map.lomv[1],  map.lomv[2],
	      map.himv[0],  map.himv[1],  map.himv[2] );
      lx = ILOG2C(mx);
      ly = ILOG2C(my)+lx;
      lz = ILOG2C(mz)+ly;

      fwrite((char *)map.mapm,1,sizeof(Int16)<<lz,mfile);

      fclose(mfile);
    }


MainInt ReadMap3Db(char *mfilename, Map3D *map, char *title, char *footer)
/*** 3D ***************************************************************\
*      ReadMap3D(mfilename, map, title, footer)
*		reads a map from a file, binary mode
\**********************************************************************/
    { MainInt j, lx, ly, lz;  FILE *mfile;
      Float32 tf0, tf1, tf2, tf3, tf4, tf5;  MainInt mx, my, mz;
      
      if(!(mfile=fopen(mfilename,"r"))) return(0);
      fgets(title,100,mfile); j = strlen(title)-1;
      if (title[j]=='\n') title[j]='\0';
      fgets(footer,100,mfile);  j = strlen(footer)-1;
      if (footer[j]=='\n') footer[j]='\0';
      
      fscanf(mfile,"%d%d%d%f%f%f%f%f%f\n", &mx, &my, &mz,
	     &tf0, &tf1, &tf2, &tf3, &tf4, &tf5);
      
      (*map).msize[0] = mx;  (*map).msize[1] = my;  (*map).msize[2] = mz;
      (*map).lomv[0] = tf0;  (*map).lomv[1] = tf1;  (*map).lomv[2] = tf2;
      (*map).himv[0] = tf3;  (*map).himv[1] = tf4;  (*map).himv[2] = tf5;
      lx = ILOG2C(mx);
      ly = ILOG2C(my)+lx;
      lz = ILOG2C(mz)+ly;
      
      (*map).mapm = (Int16 *) calloc((1<<lz), sizeof(Int16));
      fread((char *)(*map).mapm,1,sizeof(Int16)<<lz,mfile);

      fclose(mfile);
     return((MainInt) (*map).mapm);
    }

MainInt ReReadMap3Db(char *mfilename, Map3D *map, char *title, char *footer)
/*** 3D ***************************************************************\
*      ReReadMap3D(mfilename, map, title, footer)
*		reads a map from a file, without allocating space
\**********************************************************************/
    { MainInt j, lx, ly, lz;  FILE *mfile;
      Float32 tf0, tf1, tf2, tf3, tf4, tf5;  MainInt mx, my, mz;
      
      if(!(mfile=fopen(mfilename,"r"))) return(0);
      fgets(title,100,mfile); j = strlen(title)-1;
      if (title[j]=='\n') title[j]='\0';
      fgets(footer,100,mfile);  j = strlen(footer)-1;
      if (footer[j]=='\n') footer[j]='\0';
      
      fscanf(mfile,"%d%d%d%f%f%f%f%f%f\n", &mx, &my, &mz,
	     &tf0, &tf1, &tf2, &tf3, &tf4, &tf5);
      
      (*map).msize[0] = mx;  (*map).msize[1] = my;  (*map).msize[2] = mz;
      (*map).lomv[0] = tf0;  (*map).lomv[1] = tf1;  (*map).lomv[2] = tf2;
      (*map).himv[0] = tf3;  (*map).himv[1] = tf4;  (*map).himv[2] = tf5;
      lx = ILOG2C(mx);
      ly = ILOG2C(my)+lx;
      lz = ILOG2C(mz)+ly;
      
      fread((char *)(*map).mapm,1,sizeof(Int16)<<lz,mfile);

      fclose(mfile);
     return((MainInt) (*map).mapm);
    }


void InitCylModelParams(void)
/*** 3D ***************************************************************\
*   InitCylModelParams()
*   Set up default parameters of analytic sensor evidence function
\**********************************************************************/
     {MainInt i;
       /* "neutral" values */
         P[em0] = 0.01;	 	P[emI] = 0.05;	 	P[ems] = 0.05;
	 P[oc0] = 0.99;		P[ocI] = 0.95;	 	P[ocs] = 0.05;
	 P[an0] = 0.1;		P[anI] = .01;		P[ans] = 0.05;
	 P[ru0] = 0.1;	 	P[ruI] = 1.0;	 	P[rus] = 0.05;
	 P[ro0] = 0.0;	 	P[roI] = 0.0;	 	P[ros] = 0.05;

      /* em: "empty volume" has probability .01 at mouth, rising to
	 .05 with scale 20 distance units (I think of them as meters,
	 but don't let that stop you from calling them centimeters or
	 anything else.  The scale is set by the units used in "range"
	 when calling AddCylReading) */

      /* oc: "occupied ridge" has height .99 for very short readings,
	 .95 for very long ones, changing with scale 20 meters.  This
	 parameter only affects the range ridge itself. */

      /* an: "beam angle" narrows from 11.4 degrees (2*atan(0.1)) at
	 mouth to 1.15 degrees at infinity, with scale 20 meters */

      /* ru: "occupied ridge" thickness (to model range uncertainty,
	 controlled by a 1/(ru^2+1) formula that approximates a normal
	 curve) varies from 0.1 meters for very short readings to 1
	 meter for very long ones, changing with characteristic length
	 20 meters */

      /* ro: "occupied ridge" range offset, to compensate for
	 systematic, range dependent range errors.  Set to 0 for short
	 readings and long ones, with a 20 meter scale for the
	 transition, as if that mattered */

       /* lower and upper limits */
         for (i=0; i<NP; i++) {Pl[i] = 0.0;  Ph[i] = 1.0;};
	 Ph[em0] = Ph[emI] =  0.5;  /* can't have full empty */
	 Pl[oc0] = Pl[ocI] =  0.5;  /* can't have empty occupation */
	 Pl[ro0] = Pl[roI] = -1.0;  /* range offset can be + or - */
       }


void MakeCylModel(MainInt nmods, MainFloat cellsize, MainInt rszmax, MainInt dszmax,
		  MainFloat rngl, MainFloat rngh, CylSensorModelArray *smodels)
/*** 3D ***************************************************************\
*	MakeCylModel(nmods, cellsize, 
*                            rszmax, dszmax, rngl, rngh, smodels)
*	creates nmods sensor models with cell resolution cellsize,
*	limited by maximum array sizes rszmax by dszmax
*       and range from rngl to rngh in equi-proportional steps
\**********************************************************************/
    {MainFloat rratio;
     BestFloat lr, ld, hr, hd, r, r0, dr, d, dd, anratio, dbulge, t;
     BestFloat  rb, rr, ru, oc, po, pe, pc, p, h, rd;
     BestFloat  Dem, Doc, Dan, Dru, Dro;  /* parameter differences */
     MainInt rsz, dsz, ir, id, k, gi, lrsz;
     Int8 *grid; Int16 *prof;

     /* in below note that 1/(p^n+1) roughly approximates Exp[-p] */

     if (P[anI]<0.0001) P[anI]=0.0001;
     if (P[an0]<0.0) P[an0]=0.0; if (P[ans]<0.0001) P[ans]=0.0001;

     Dem = P[em0]-P[emI];    Doc = P[oc0]-P[ocI];
     Dan = P[an0]-P[anI];    Dru = P[ru0]-P[ruI];
     Dro = P[ro0]-P[roI];

     /* beam profile is   r = d*(anI+(an0-anI)/((ans*d)^2+1))
       find d at bulge (if any) of this function */

     anratio = P[an0]/P[anI];
     dbulge = (anratio-9.0)*(anratio-1.0);
     if (dbulge>=0)  dbulge = anratio-3.0-SQRT(dbulge);
     if (dbulge>=0)  dbulge = SQRT(dbulge*0.5)/P[ans];

     if (rngl<=0.0) rngl=rngh*0.02; if (nmods<=1) nmods=2;
     (*smodels).nmodels = nmods;
     (*smodels).models = (CylSensorModel *) calloc(nmods,sizeof(CylSensorModel));

     rratio = EXP(LOG(fabs(rngh/rngl))/((MainFloat) (nmods-1)));
     for (k=0; k<nmods; k++)
       { ld = lr = 0.0;
	 rr = rngl+P[roI]+Dro/(rngl*P[ros]+1.0); /* adj. range */
	 ru = P[ruI] + Dru/(rngl*P[rus]+1.0); /* range uncertainty */
	 oc = P[ocI] + Doc/(rngl*P[ocs]+1.0); /* occ peak height */
	 hd = rr;   if((t = 2.0*oc-1.0)>0.0) hd += ru*SQRT(t);

	 t = hd*P[ans];
	 hr = hd*(P[anI]+Dan/(t*t + 1.0));
	 hr *= hd/HYPOT(hd,hr);     /* cut corner */
	 if (dbulge>=0.0 && dbulge<=hd)
	   { t = dbulge*P[ans];
	     t = dbulge*(P[anI]+Dan/(t*t + 1.0));
	     if (t>hr) hr=t;
	   }
	 dsz = (hd-ld)/cellsize+0.5;  if (dsz>dszmax) dsz=dszmax;
	 rsz = (hr-lr)/cellsize;  rsz++;  if (rsz>rszmax) rsz=rszmax; 
	 lrsz = ILOG2C(rsz);  /* for r, lr is center, not edge, of row 0 */

	 (*smodels).models[k].rangem = rngl;
	 (*smodels).models[k].rsize = rsz;  (*smodels).models[k].dsize = dsz;
	 (*smodels).models[k].lorv = lr;  (*smodels).models[k].lodv = ld;
	 (*smodels).models[k].hirv = hr;  (*smodels).models[k].hidv = hd;
	 (*smodels).models[k].modelm = grid =
	   (Int8 *) calloc(dsz<<lrsz,sizeof(Int8));   /* The grid */
	 (*smodels).models[k].profile = prof =
	   (Int16 *) calloc(dsz,sizeof(Int16));   /* radius silhouette */

	 dd = (hd-ld)/dsz;    d = 0.5*dd + ld;
	 dr = (hr-lr)/rsz;    r0 = lr - dr;  /* lr is midline of row 0 */
	 if (rsz==1 && lr==0.0)  /* thin ray! zoom zoom */
	   for (id=0; id<dsz; ++id)
	     { prof[id]=0;
	       rd = (rr-d)/ru;   rd = rd*rd+1.0;
	       po = oc/rd;
	       pe = d>rr?0.5:P[emI] + Dem/(d*P[ems]+1.0);
	       p = po>pe?po:pe;
	       if (p==0.5) {grid[id]=0; continue;}
	       if (p<0.0001) {grid[id] = -54;  continue;};
	       if (p>0.9999) {grid[id] =  54;  continue;};
	       p = LOG(p/(1.0-p))*5.770780164;    /* log base 2^(1/4) */
	       if ((grid[id] = ROUND(p))) prof[id]=1; /* if nonzero! */
	       d += dd;
	     }
	 else /* fat ray: more work */
	   for (id=0; id<dsz; ++id)
	     { t = d*P[ans];
	       rb = d*(P[anI] + Dan/(t*t+1.0)); /* beam radius at d*/
	       r = r0;  gi = (id << lrsz) - 1;  prof[id]=0;
	       for (ir=0; ir<rsz; ++ir)
		 { r += dr;  gi++;
		   if (r>rb) {grid[gi]=0; continue;}
		   h = r==0.0?d:HYPOT(r,d);
		   rd = (rr-h)/ru;   rd = rd*rd+1.0;
		   po = oc/rd;
		   pe = h>rr?0.5:P[emI] + Dem/(h*P[ems]+1.0);
		   pc = po>pe?po:pe;  t = r/rb;
		   p = 1.0-pc+(2.0*pc-1.0)/(t*t+1.0) ;
		   if (p==0.5) {grid[gi]=0; continue;}
		   if (p<0.0001) {grid[gi] = -54;  continue;};
		   if (p>0.9999) {grid[gi] =  54;  continue;};
		   p = LOG(p/(1.0-p))*5.770780164;    /* log base 2^(1/4) */
		   if ((grid[gi] = ROUND(p))) prof[id]=ir+1; /* if nonzero! */
		 }
	       d += dd;
	     }
	 rngl *= rratio;}
   }


void MakeCylModelStereo(MainInt PicWidth, MainFloat FocalLength, 
			MainFloat CamSeparation, MainFloat Cellsize,
			MainFloat MaxRange, MainInt  EvLo, MainInt EvHi,
			CylSensorModelArray *smodels)
/*** 3D ***************************************************************\
*	MakeCylModelStereo(PicWidth, FocalLength,
*                 CamSeparation, GridResolution,ProbLo, ProbHi)
*	sensor model for stereoscopic ranging
*	PicWidth  image width, in pixels   FocalLength  in pixels
*       CamSeparation  stereo pair camera separation, in meters
*       Cellsize  cell to cell grid spacing, in meters
*       MaxRange is the length of the longest ray to paint
*       EvLo  lowest "empty" evidence (log odds) to use
*       EvHi  highest "occupied" evidence to use
*	smodels holds the resulting model
\**********************************************************************/
    {
     BestFloat lr, ld, hr, hd, r, d, dd, r0, rng, rngmax, rngmin, dr, t;
     BestFloat  rb;
     MainInt rsz, dsz, ir, id, k, gi, lrsz, nmods, displacement, EvHid;
     Int8 *grid; Int16 *prof;

#define broad 4.0 /* beam broadening fudge factor (4 = wide as feature windows) */

     nmods=PicWidth/2;
     (*smodels).nmodels = nmods;
     (*smodels).models = (CylSensorModel *) calloc(nmods,sizeof(CylSensorModel));

     for (k=0; k<nmods; k++)
       { 
	   ld = lr = 0;
	   displacement = PicWidth/2-k;
	   rng = CamSeparation*FocalLength/displacement;
	   rngmax = CamSeparation*FocalLength/(MAX(displacement,2)-0.5);
	   rngmin = CamSeparation*FocalLength/(displacement+0.5);
	   if ((t=rngmax-rngmin) < Cellsize)
	     {
		 t = (Cellsize-t)/2;
		 rngmin -= t;  rngmax += t;
	     };

	   hd = MIN(rngmax,MaxRange);
	   hr = broad*hd/FocalLength;

	   dsz = (hd-ld)/Cellsize+0.5;
	   rsz = (hr-lr)/Cellsize;  rsz++;
	   lrsz = ILOG2C(rsz);  /* for r, lr is center, not edge, of row 0 */

	   EvHid = EvHi*Cellsize/(rsz*rsz*(rngmax-rngmin)) + 0.9;

	   /* spread occupied evidence over position uncertainty */
	   if (EvHid>127)  EvHid  =  127;
	   if (EvHid<-127) EvHid  = -127;
	   if (EvLo>127)  EvLo  =  127;
	   if (EvLo<-127) EvLo  = -127;
	   
	   (*smodels).models[k].rangem = rng;
	   (*smodels).models[k].rsize = rsz;  (*smodels).models[k].dsize = dsz;
	   (*smodels).models[k].lorv = lr;  (*smodels).models[k].lodv = ld;
	   (*smodels).models[k].hirv = hr;  (*smodels).models[k].hidv = hd;
	   (*smodels).models[k].modelm = grid =
	     (Int8 *) calloc(dsz<<lrsz,sizeof(Int8));   /* The grid */
	   (*smodels).models[k].profile = prof =
	     (Int16 *) calloc(dsz,sizeof(Int16));   /* radius silhouette */

	   dd = (hd-ld)/dsz;    d = 0.5*dd + ld;
	   dr = (hr-lr)/rsz;    r0 = lr - dr;  /* lr is midline of row 0 */
	   if (rsz==1 && lr==0.0)  /* thin ray! zoom zoom */
	     for (id=0; id<dsz; ++id)
	       {
		   prof[id]=0;
		   grid[id] = d<rngmin?EvLo:(d<=rngmax?EvHid:0);
		   d += dd;
	       }
	   else /* fat ray: more work */
	     for (id=0; id<dsz; ++id)
	       { 
		   rb = broad*d/FocalLength; /* beam radius at d */
		   r = r0;  gi = (id << lrsz) - 1;  prof[id]=0;
		   for (ir=0; ir<rsz; ++ir)
		     { 
			 r += dr;  gi++;
			 if (r>rb) grid[gi]=0; else
			   grid[gi] = d<rngmin?EvLo:(d<=rngmax?EvHid:0);
			 if ((grid[gi])) prof[id]=ir+1; /* if nonzero! */
		     }
		   d += dd;
	       };
       }
 }


void TrimCylModel(CylSensorModelArray smodels)
/*** 3D ***************************************************************\
*		TrimCylModel(smodels)
*		trims boundary zeros from each of a set of sensor models
\**********************************************************************/
    {MainInt   k, l, r, d, rs, ds, rl, rh, dl, dh, rsn, rsnx, log2rs;
     BestFloat lr, ld, hr, hd; 
     
     for (k=0; k<smodels.nmodels; k++)
       {
	 rs = smodels.models[k].rsize;   ds = smodels.models[k].dsize;
	 lr = smodels.models[k].lorv ;   ld = smodels.models[k].lodv ;
	 hr = smodels.models[k].hirv ;   hd = smodels.models[k].hidv ;
	 log2rs = ILOG2C(rs);
	 
	 /* find bounds of nonzero area */
	 dl = ds-1;  dh = 0;  rl = rs-1;  rh = 0;
	 
	 for (d=0; d<ds; ++d)  for (r=0; r<rs; ++r)
	   if (smodels.models[k].modelm[(d<<log2rs)+r]!=0)
	     {if (r<rl) rl=r;  if (r>rh) rh=r;
	      if (d<dl) dl=d;  if (d>dh) dh=d;}
	 if (rl>rh || dl>dh) {rl=rh; dl=dh;};  /* if sensor model empty */
	 if (rl>0)
	   {printf("TrimSensor rl=%d  dl=%d (rh=%d  dh=%d)\n",rl,dl,rh,dh);
	    rl=0;}
	 rsn = rh-rl+1;
	 rsnx = 1<<ILOG2C(rsn);
	 
	 l = 0;
	 for (d=dl; d<=dh; ++d) 
	   { for (r=rl; r<=rh; ++r) smodels.models[k].modelm[l++] = 
	                        smodels.models[k].modelm[(d<<log2rs)+r];
	     for (r=rsn; r<rsnx; ++r) smodels.models[k].modelm[l++] = 0;
	     smodels.models[k].profile[d-dl] = smodels.models[k].profile[d]-rl;
	   }

	 smodels.models[k].rsize = rsn;
	 smodels.models[k].dsize = dh-dl+1;
	 smodels.models[k].lorv  += (rl*(hr-lr))/rs;
	 smodels.models[k].lodv  += (dl*(hd-ld))/ds;
	 smodels.models[k].hirv  -= ((rs-1-rh)*(hr-lr))/rs;
	 smodels.models[k].hidv  -= ((ds-1-dh)*(hd-ld))/ds;
       }
   }


void FreeCylModel(CylSensorModelArray smodels)
/*** 3D ***************************************************************\
*		FreeCylModel(smodels)
*		deletes and frees up the memory of set of sensor models
\**********************************************************************/
    {MainInt   k;
     
     for (k=0; k<smodels.nmodels; k++)
       {
	   free(smodels.models[k].modelm);
	   free(smodels.models[k].profile);
       }
     free(smodels.models);
   }


MainFloat MatchMap3D(Map3D map1, Map3D map2)
/*** 3D ***************************************************************\
*	MatchMap3D(map1, map2)
*	compare map1 and map2 and return log probability of match
*       goodness
\**********************************************************************/
    {MainInt kx1, ky1, kz1, kx2, ky2, kz2, lx1, ly1, lx2, ly2;
     MainInt izd, iyd, izh, iyh, ixh;
     MainInt t1, t2, t3, ix, iy, iz, match;
     Int16 *grid1, *grid2;   MainFloat t;

     kx1 = map1.msize[0]; lx1 = ILOG2C(kx1);
     ky1 = map1.msize[1]; ly1 = ILOG2C(ky1);
     kz1 = map1.msize[2];

     kx2 = map2.msize[0]; lx2 = ILOG2C(kx2);
     ky2 = map2.msize[1]; ly2 = ILOG2C(ky2);
     kz2 = map2.msize[2];

     if (kx2<kx1) kx1 = kx2;  t = kx1;
     if (ky2<ky1) ky1 = ky2;  t *= ky1;
     if (kz2<kz1) kz1 = kz2;  t *= kz1;

     grid1 = map1.mapm;  grid2 = map2.mapm;

     /* computes Match = Sum(Log2[p1*p2+(1-p1)*(1-p2)]).
	In maps, p is represented by logodds: l = Log2[p/(1-p)].
	Match can then be given by Sum(LP1(l1+l2)-LP1(l1)-LP1(l2)),
	where LP1(i) = Log2[2^i+1].  (Scaling factors omitted)    */

     match=0;
     if (lx1==lx2 && ly1==ly2)
       { izd = 1<<(lx1+ly1);   iyd = 1<<lx1;   izh = kz1<<(lx1+ly1); 
	 for (iz = 0; iz < izh; iz+=izd)
	   { iyh = iz+(ky1<<lx1);   for (iy = iz; iy < iyh; iy+=iyd)
	       { ixh = iy+kx1;   for (ix = iy; ix < ixh; ix++)
		   { t3=(t1=grid1[ix])+(t2=grid2[ix]);
		     match += LP1(t3)-LP1(t2)-LP1(t1); }; } } }
     else
       for (iz = 0; iz < kz1; iz++) for (iy = 0; iy < ky1; iy++)
	   for (ix = 0; ix < kx1; ix++)
	     { t3 = (t1 = grid1[(((iz<<ly1)+iy)<<lx1)+ix]) +
		    (t2 = grid2[(((iz<<ly2)+iy)<<lx2)+ix]) ;
	       match += LP1(t3)-LP1(t2)-LP1(t1); };

     return((MainFloat) (t+match/1024.0));
    }


int SimulSolve(MainFloat a[], MainInt n)
/*** 3D ***************************************************************\
*	SimulSolve(a,n)  solve n simultaneous linear equations
*  a is an [n * (n+1)] array with rhs constants in rightmost column.
*  Solved values are returned in a[0:n-1]. Rest of a is a mess. 
\**********************************************************************/
    { MainInt i, ii, j, jj, k, it;  MainFloat t, tt;
      MainInt *perm;

      /* initialize pivot permutation */
      perm = (MainInt *) malloc(n*sizeof(MainInt));
      perm[0]=0;  for (i=1; i<n; i++) perm[i] = perm[i-1]+n+1;
      
      for (ii=0; ii<n; ii++)  /* elimination loop */
        { if ((tt = a[perm[ii]+ii])<0.0) tt = -tt;
          for (i=ii+1; i<n; i++)  /* pivot */
            { if ((t = a[perm[i]+ii])<0.0) t = -t;
                if (t>tt)
               {it = perm[ii]; perm[ii] = perm[i]; perm[i] = it; tt=t;}; }
          i = perm[ii];
	  if ((t=a[i+ii]) == 0.0) {free(perm); return(0);}; /* singular */
          if (t != 1.0)
            { t = 1.0/t;  /* eliminate variable ii */
              for (k=ii+1; k<=n; k++) a[i+k] *= t; }
          for (jj=ii+1; jj<n; jj++)
             { j = perm[jj]; if ((t=a[j+ii])!=0.0)
                for (k=ii+1; k<=n; k++) a[j+k] -= t*a[i+k]; }
         }

       for (ii=n-1; ii>0; ii--)   /* backsolve */
         { i = perm[ii]; if ((t=a[i+n]) != 0.0)
              for (jj=0; jj<ii; jj++)  
                {j = perm[jj]; a[j+n] -= a[j+ii]*t;}
         }
        
       /* unscramble pivot permutation */
       for (ii=0; ii<n; ii++) a[ii]=a[perm[ii]+n];
       free(perm);  return(1);
    }

void InitLST(MainFloat a[], MainInt n)
 /* initialize array for least squares tangent hperplane fit:
    these routines fit a least squares quadratic to an n-dimensional
    point set, then find the tangent plane (i.e. slope) at a point */
    { MainInt i, nt;
      nt = n*(n+1);
      for (i=0; i<nt; i++) a[i]=0.0; }

/* accumulate crossproduct sums for l.s.t. hyperplane fit */
void AddLST(MainFloat a[], MainInt n, MainFloat x[])
/* a coefficient array;  n # of equations;  x variables, value */
     { MainInt i, j, ii;
       for (i=0; i<n; i++)
	 { ii = i*(n+1); for (j=i; j<=n; j++) a[ii+j] += x[i]*x[j]; }
     }

void ScaleLST(MainFloat a[], MainInt n, MainFloat w)  /* scale existing sums */
/* a coefficient array;  n # of equations;  w scale factor */
     { MainInt i, j, ii;
       for (i=0; i<n; i++)
	 { ii = i*(n+1); for (j=i; j<=n; j++) a[ii+j] *= w; }
     }

/* accumulate crossproduct sums, weighted */
void AddLSTw(MainFloat a[], MainInt n, MainFloat x[], MainFloat w)
/* a: coefficient array; n: # of equations; x: variables, value; w weight; */
     { MainInt i, j, ii;
       for (i=0; i<n; i++)
	 { ii = i*(n+1); for (j=i; j<=n; j++) a[ii+j] += w*x[i]*x[j]; }
     }

int SolveLST(MainFloat a[], MainInt n) /* solve for hyperplane coefficients */
/* a coefficient array, becomes solution;  n number of equations */
     { MainInt i, j, np;
       np = n+1; /* matrix is symmetric */
       for (i=1; i<n; i++) for (j=0; j<i; j++) a[i*np+j] = a[j*np+i];
     return(SimulSolve(a,n));     /* solve resulting equations */
     }