Back to index

radiance  4R0+20100331
warp3d.c
Go to the documentation of this file.
00001 #ifndef lint
00002 static const char    RCSid[] = "$Id: warp3d.c,v 3.8 2004/05/25 22:04:14 greg Exp $";
00003 #endif
00004 /*
00005  * 3D warping routines.
00006  */
00007 
00008 #include <stdio.h>
00009 #include <stdlib.h>
00010 #include <math.h>
00011 
00012 #include "rterror.h"
00013 #include "rtio.h"
00014 #include "fvect.h"
00015 #include "warp3d.h"
00016 
00017 #define MIND  1e-5          /* minimum distance between input points */
00018 
00019 typedef struct {
00020        GNDX   n;            /* index must be first */
00021        W3VEC  v;
00022 } KEYDP;             /* key/data allocation pair */
00023 
00024 #define fgetvec(p,v) (fgetval(p,'f',v) > 0 && fgetval(p,'f',v+1) > 0 \
00025                             && fgetval(p,'f',v+2) > 0)
00026 
00027 #define AHUNK 24            /* number of points to allocate at a time */
00028 
00029 
00030 double wpdist2(W3VEC p1, W3VEC p2);
00031 static int gridpoint(GNDX ndx, W3VEC rem, W3VEC ipt, struct grid3d *gp);
00032 static int get3dgpt(W3VEC ov, GNDX ndx, WARP3D *wp);
00033 static int get3dgin(W3VEC ov, GNDX ndx, W3VEC rem, WARP3D *wp);
00034 static void l3interp(W3VEC vo, W3VEC *cl, W3VEC dv, int n);
00035 static int warp3dex(W3VEC ov, W3VEC pi, WARP3D *wp);
00036 //static unsigned long gridhash(const void *gp);
00037 static lut_hashf_t gridhash;
00038 static int new3dgrid(WARP3D *wp);
00039 static void done3dgrid(struct grid3d *gp);
00040 
00041 
00042 double
00043 wpdist2(                    /* compute square of distance between points */
00044        register W3VEC       p1,
00045        register W3VEC       p2
00046 )
00047 {
00048        double d, d2;
00049 
00050        d = p1[0] - p2[0]; d2  = d*d;
00051        d = p1[1] - p2[1]; d2 += d*d;
00052        d = p1[2] - p2[2]; d2 += d*d;
00053        return(d2);
00054 }
00055 
00056 
00057 int
00058 warp3d(              /* warp 3D point according to the given map */
00059        W3VEC  po,
00060        W3VEC  pi,
00061        register WARP3D      *wp
00062 )
00063 {
00064        int    rval = W3OK;
00065        GNDX   gi;
00066        W3VEC  gd, ov;
00067 
00068        if (wp->grid.gn[0] == 0 && (rval = new3dgrid(wp)) != W3OK)
00069               return(rval);
00070        rval |= gridpoint(gi, gd, pi, &wp->grid);
00071        if (wp->grid.flags & W3EXACT)
00072               rval |= warp3dex(ov, pi, wp);
00073        else if (wp->grid.flags & W3FAST)
00074               rval |= get3dgpt(ov, gi, wp);
00075        else
00076               rval |= get3dgin(ov, gi, gd, wp);
00077        po[0] = pi[0] + ov[0];
00078        po[1] = pi[1] + ov[1];
00079        po[2] = pi[2] + ov[2];
00080        return(rval);
00081 }
00082 
00083 
00084 static int
00085 gridpoint(    /* compute grid position for ipt */
00086        GNDX   ndx,
00087        W3VEC  rem,
00088        W3VEC  ipt,
00089        register struct grid3d      *gp
00090 )
00091 {
00092        int    rval = W3OK;
00093        register int  i;
00094 
00095        for (i = 0; i < 3; i++) {
00096               rem[i] = (ipt[i] - gp->gmin[i])/gp->gstep[i];
00097               if (rem[i] < 0.) {
00098                      ndx[i] = 0;
00099                      rval = W3GAMUT;
00100               } else if ((int)rem[i] >= gp->gn[i]) {
00101                      ndx[i] = gp->gn[i] - 1;
00102                      rval = W3GAMUT;
00103               } else
00104                      ndx[i] = (int)rem[i];
00105               rem[i] -= (double)ndx[i];
00106        }
00107        return(rval);
00108 }
00109 
00110 
00111 static int
00112 get3dgpt(            /* get value for voxel */
00113        W3VEC  ov,
00114        GNDX   ndx,
00115        register WARP3D      *wp
00116 )
00117 {
00118        W3VEC  gpt;
00119        register LUENT       *le;
00120        KEYDP  *kd;
00121        int    rval = W3OK;
00122        register int  i;
00123 
00124        le = lu_find(&wp->grid.gtab, ndx);
00125        if (le == NULL)
00126               return(W3ERROR);
00127        if (le->data == NULL) {
00128               if (le->key != NULL)
00129                      kd = (KEYDP *)le->key;
00130               else if ((kd = (KEYDP *)malloc(sizeof(KEYDP))) == NULL)
00131                      return(W3ERROR);
00132               for (i = 0; i < 3; i++) {
00133                      kd->n[i] = ndx[i];
00134                      gpt[i] = wp->grid.gmin[i] + ndx[i]*wp->grid.gstep[i];
00135                      if (wp->grid.flags & W3FAST)       /* on centers */
00136                             gpt[i] += .5*wp->grid.gstep[i];
00137               }
00138               rval = warp3dex(kd->v, gpt, wp);
00139               le->key = (char *)kd->n;
00140               le->data = (char *)kd->v;
00141        }
00142        W3VCPY(ov, (float *)le->data);
00143        return(rval);
00144 }
00145 
00146 
00147 static int
00148 get3dgin(     /* interpolate from warp grid */
00149        W3VEC  ov,
00150        GNDX   ndx,
00151        W3VEC  rem,
00152        WARP3D *wp
00153 )
00154 {
00155        W3VEC  cv[8];
00156        GNDX   gi;
00157        int    rval = W3OK;
00158        register int  i;
00159                                    /* get corner values */
00160        for (i = 0; i < 8; i++) {
00161               gi[0] = ndx[0] + (i & 1);
00162               gi[1] = ndx[1] + (i>>1 & 1);
00163               gi[2] = ndx[2] + (i>>2);
00164               rval |= get3dgpt(cv[i], gi, wp);
00165        }
00166        if (rval & W3ERROR)
00167               return(rval);
00168        l3interp(ov, cv, rem, 3);   /* perform trilinear interpolation */
00169        return(rval);
00170 }
00171 
00172 
00173 static void
00174 l3interp(            /* trilinear interpolation (recursive) */
00175        W3VEC  vo,
00176        W3VEC  *cl,
00177        W3VEC  dv,
00178        int    n
00179 )
00180 {
00181        W3VEC  v0, v1;
00182        register int  i;
00183 
00184        if (--n) {
00185               l3interp(v0, cl, dv, n);
00186               l3interp(v1, cl+(1<<n), dv, n);
00187        } else {
00188               W3VCPY(v0, cl[0]);
00189               W3VCPY(v1, cl[1]);
00190        }
00191        for (i = 0; i < 3; i++)
00192               vo[i] = (1.-dv[n])*v0[i] + dv[n]*v1[i];
00193 }
00194 
00195 
00196 static int
00197 warp3dex(            /* compute warp using 1/r^2 weighted avg. */
00198        W3VEC  ov,
00199        W3VEC  pi,
00200        register WARP3D      *wp
00201 )
00202 {
00203        double d2, w, wsum;
00204        W3VEC  vt;
00205        register int  i;
00206 
00207        vt[0] = vt[1] = vt[2] = 0.;
00208        wsum = 0.;
00209        for (i = wp->npts; i--; ) {
00210               d2 = wpdist2(pi, wp->ip[i]);
00211               if (d2 <= MIND*MIND) w = 1./(MIND*MIND);
00212               else w = 1./d2;
00213               vt[0] += w*wp->ov[i][0];
00214               vt[1] += w*wp->ov[i][1];
00215               vt[2] += w*wp->ov[i][2];
00216               wsum += w;
00217        }
00218        if (wsum > 0.) {
00219               ov[0] = vt[0]/wsum;
00220               ov[1] = vt[1]/wsum;
00221               ov[2] = vt[2]/wsum;
00222        }
00223        return(W3OK);
00224 }
00225 
00226 
00227 WARP3D *
00228 new3dw(                     /* allocate and initialize WARP3D struct */
00229        int    flgs
00230 )
00231 {
00232        register WARP3D  *wp;
00233 
00234        if ((flgs & (W3EXACT|W3FAST)) == (W3EXACT|W3FAST)) {
00235               eputs("new3dw: only one of W3EXACT or W3FAST\n");
00236               return(NULL);
00237        }
00238        if ((wp = (WARP3D *)malloc(sizeof(WARP3D))) == NULL) {
00239               eputs("new3dw: no memory\n");
00240               return(NULL);
00241        }
00242        wp->ip = wp->ov = NULL;
00243        wp->npts = 0;
00244        wp->grid.flags = flgs;
00245        wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 0;
00246        return(wp);
00247 }
00248 
00249 
00250 WARP3D *
00251 load3dw(                    /* load 3D warp from file */
00252        char   *fn,
00253        WARP3D *wp
00254 )
00255 {
00256        FILE   *fp;
00257        W3VEC  inp, outp;
00258 
00259        if ((fp = fopen(fn, "r")) == NULL) {
00260               eputs(fn);
00261               eputs(": cannot open\n");
00262               return(NULL);
00263        }
00264        if (wp == NULL && (wp = new3dw(0)) == NULL)
00265               goto memerr;
00266        while (fgetvec(fp, inp) && fgetvec(fp, outp))
00267               if (!add3dpt(wp, inp, outp))
00268                      goto memerr;
00269        if (!feof(fp)) {
00270               wputs(fn);
00271               wputs(": non-number in 3D warp file\n");
00272        }
00273        goto cleanup;
00274 memerr:
00275        eputs("load3dw: out of memory\n");
00276 cleanup:
00277        fclose(fp);
00278        return(wp);
00279 }
00280 
00281 
00282 extern int
00283 set3dwfl(            /* reset warp flags */
00284        register WARP3D      *wp,
00285        int    flgs
00286 )
00287 {
00288        if (flgs == wp->grid.flags)
00289               return(0);
00290        if ((flgs & (W3EXACT|W3FAST)) == (W3EXACT|W3FAST)) {
00291               eputs("set3dwfl: only one of W3EXACT or W3FAST\n");
00292               return(-1);
00293        }
00294        wp->grid.flags = flgs;
00295        done3dgrid(&wp->grid);             /* old grid is invalid */
00296        return(0);
00297 }
00298 
00299 
00300 int
00301 add3dpt(             /* add 3D point pair to warp structure */
00302        register WARP3D      *wp,
00303        W3VEC  pti,
00304        W3VEC  pto
00305 )
00306 {
00307        double d2;
00308        register W3VEC       *na;
00309        register int  i;
00310 
00311        if (wp->npts == 0) {               /* initialize */
00312               wp->ip = (W3VEC *)malloc(AHUNK*sizeof(W3VEC));
00313               if (wp->ip == NULL) return(0);
00314               wp->ov = (W3VEC *)malloc(AHUNK*sizeof(W3VEC));
00315               if (wp->ov == NULL) return(0);
00316               wp->d2min = 1e10;
00317               wp->d2max = 0.;
00318               W3VCPY(wp->llim, pti);
00319               W3VCPY(wp->ulim, pti);
00320        } else {
00321               if (wp->npts % AHUNK == 0) {       /* allocate another hunk */
00322                      na = (W3VEC *)realloc((void *)wp->ip,
00323                                    (wp->npts+AHUNK)*sizeof(W3VEC));
00324                      if (na == NULL) return(0);
00325                      wp->ip = na;
00326                      na = (W3VEC *)realloc((void *)wp->ov,
00327                                    (wp->npts+AHUNK)*sizeof(W3VEC));
00328                      if (na == NULL) return(0);
00329                      wp->ov = na;
00330               }
00331               for (i = 0; i < 3; i++)            /* check boundaries */
00332                      if (pti[i] < wp->llim[i])
00333                             wp->llim[i] = pti[i];
00334                      else if (pti[i] > wp->ulim[i])
00335                             wp->ulim[i] = pti[i];
00336               for (i = wp->npts; i--; ) { /* check distances */
00337                      d2 = wpdist2(pti, wp->ip[i]);
00338                      if (d2 < MIND*MIND)  /* value is too close */
00339                             return(wp->npts);
00340                      if (d2 < wp->d2min)
00341                             wp->d2min = d2;
00342                      if (d2 > wp->d2max)
00343                             wp->d2max = d2;
00344               }
00345        }
00346        W3VCPY(wp->ip[wp->npts], pti);     /* add new point */
00347        wp->ov[wp->npts][0] = pto[0] - pti[0];
00348        wp->ov[wp->npts][1] = pto[1] - pti[1];
00349        wp->ov[wp->npts][2] = pto[2] - pti[2];
00350        done3dgrid(&wp->grid);             /* old grid is invalid */
00351        return(++wp->npts);
00352 }
00353 
00354 
00355 extern void
00356 free3dw(                    /* free WARP3D data */
00357        register WARP3D      *wp
00358 )
00359 {
00360        done3dgrid(&wp->grid);
00361        free((void *)wp->ip);
00362        free((void *)wp->ov);
00363        free((void *)wp);
00364 }
00365 
00366 
00367 static unsigned long
00368 gridhash(                   /* hash a grid point index */
00369        //GNDX gp
00370        const void    *gp
00371 )
00372 {
00373        //return(((unsigned long)gp[0]<<GNBITS | gp[1])<<GNBITS | gp[2]);
00374        return(((unsigned long)((const unsigned char*)gp)[0]<<GNBITS | ((const unsigned char*)gp)[1])<<GNBITS | ((const unsigned char*)gp)[2]);
00375 }
00376 
00377 
00378 static int
00379 new3dgrid(                  /* initialize interpolating grid for warp */
00380        register WARP3D      *wp
00381 )
00382 {
00383        W3VEC  gmax;
00384        double gridstep, d;
00385        int    n;
00386        register int  i;
00387                             /* free old grid (if any) */
00388        done3dgrid(&wp->grid);
00389                             /* check parameters */
00390        if (wp->npts < 2)
00391               return(W3BADMAP);    /* undefined for less than 2 points */
00392        if (wp->d2max < MIND)
00393               return(W3BADMAP);    /* not enough disparity */
00394                             /* compute gamut */
00395        W3VCPY(wp->grid.gmin, wp->llim);
00396        W3VCPY(gmax, wp->ulim);
00397        gridstep = sqrt(wp->d2min);
00398        for (i = 0; i < 3; i++) {
00399               wp->grid.gmin[i] -= .501*gridstep;
00400               gmax[i] += .501*gridstep;
00401        }
00402        if (wp->grid.flags & W3EXACT) {
00403               wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 1;
00404               wp->grid.gstep[0] = gmax[0] - wp->grid.gmin[0];
00405               wp->grid.gstep[1] = gmax[1] - wp->grid.gmin[1];
00406               wp->grid.gstep[2] = gmax[2] - wp->grid.gmin[2];
00407               return(W3OK);        /* no interpolation, so no grid */
00408        }
00409                             /* create grid */
00410        for (i = 0; i < 3; i++) {
00411               d = gmax[i] - wp->grid.gmin[i];
00412               n = d/gridstep + .5;
00413               if (n >= MAXGN-1)
00414                      n = MAXGN-2;
00415               wp->grid.gstep[i] = d / n;
00416               wp->grid.gn[i] = n;
00417        }
00418                             /* initialize lookup table */
00419        wp->grid.gtab.hashf = gridhash;
00420        wp->grid.gtab.keycmp = NULL;
00421        wp->grid.gtab.freek = free;
00422        wp->grid.gtab.freed = NULL;
00423        return(lu_init(&wp->grid.gtab, 1024) ? W3OK : W3ERROR);
00424 }
00425 
00426 
00427 static void
00428 done3dgrid(                 /* free interpolating grid for warp */
00429        register struct grid3d      *gp
00430 )
00431 {
00432        if (gp->gn[0] == 0)
00433               return;
00434        lu_done(&gp->gtab);
00435        gp->gn[0] = gp->gn[1] = gp->gn[2] = 0;
00436 }