src/px/warp3d.c

/* Copyright (c) 1997 Regents of the University of California */

#ifndef lint
static char SCCSid[] = "$SunId$ LBL";
#endif

/*
 * 3D warping routines.
 */

#include <stdio.h>

#include <math.h>

#include "fvect.h"

#include "warp3d.h"

#define MIND    1e-5            /* minimum distance between input points */

typedef struct {
        GNDX    n;              /* index must be first */
        W3VEC   p;
} KEYDP;                /* key/data allocation pair */

#define fgetvec(f,v)    (fgetval(f,'f',v) > 0 && fgetval(f,'f',v+1) > 0 \
                                && fgetval(f,'f',v+2) > 0)

#define AHUNK   32              /* number of points to allocate at a time */

#ifndef malloc
extern char     *malloc(), *realloc();
#endif
extern void     free();


double
wpdist2(p1, p2)                 /* compute square of distance between points */
register W3VEC  p1, p2;
{
        double  d, d2;

        d = p1[0] - p2[0]; d2  = d*d;
        d = p1[1] - p2[1]; d2 += d*d;
        d = p1[2] - p2[2]; d2 += d*d;
        return(d2);
}


int
warp3d(po, pi, wp)              /* warp 3D point according to the given map */
W3VEC   po, pi;
register WARP3D *wp;
{
        int     rval = W3OK;
        GNDX    gi;
        W3VEC   gd;

        if (wp->grid.gn[0] == 0 && (rval = new3dwgrid(wp)) != W3OK)
                return(rval);
        rval |= gridpoint(gi, gd, pi, &wp->grid);
        if (wp->grid.flags & W3EXACT) {
                rval |= warp3dex(po, pi, wp);
                return(rval);
        }
        if (wp->grid.flags & W3FAST) {
                rval |= get3dgpt(po, gi, wp);
                return(rval);
        }
        rval |= get3dgin(po, gi, gd, wp);
        return(rval);
}


int
gridpoint(ndx, rem, ipt, gp)    /* compute grid position for ipt */
GNDX    ndx;
W3VEC   rem, ipt;
register struct grid3d  *gp;
{
        int     rval = W3OK;
        register int    i;

        for (i = 0; i < 3; i++) {
                rem[i] = (ipt[i] - gp->gmin[i])/gp->gstep[i];
                if (rem[i] < 0.) {
                        ndx[i] = 0;
                        rval = W3GAMUT;
                } else if ((int)rem[i] >= gp->gn[i]) {
                        ndx[i] = gp->gn[i] - 1;
                        rval = W3GAMUT;
                } else
                        ndx[i] = (int)rem[i];
                rem[i] -= (double)ndx[i];
        }
        return(rval);
}


int
get3dgpt(po, ndx, wp)           /* get value for voxel */
W3VEC   po;
GNDX    ndx;
register WARP3D *wp;
{
        W3VEC   gpt;
        register LUENT  *le;
        KEYDP   *kd;
        int     rval = W3OK;
        register int    i;

        le = lu_find(&wp->grid.gtab, (char *)ndx);
        if (le == NULL)
                return(W3ERROR);
        if (le->data == NULL) {
                if (le->key != NULL)
                        kd = (KEYDP *)le->key;
                else if ((kd = (KEYDP *)malloc(sizeof(KEYDP))) == NULL)
                        return(W3ERROR);
                for (i = 0; i < 3; i++) {
                        kd->n[i] = ndx[i];
                        gpt[i] = wp->grid.gmin[i] + ndx[i]*wp->grid.gstep[i];
                        if (wp->grid.flags & W3FAST)    /* on centers */
                                gpt[i] += .5*wp->grid.gstep[i];
                }
                rval |= warp3dex(kd->p, gpt, wp);
                le->key = (char *)kd->n;
                le->data = (char *)kd->p;
        }
        W3VCPY(po, (float *)le->data);
        return(rval);
}


int
get3dgin(po, ndx, rem, wp)      /* interpolate from warp grid */
W3VEC   po, rem;
GNDX    ndx;
WARP3D  *wp;
{
        W3VEC   cv[8];
        int     rval;
        GNDX    gi;
        register int    i;
                                        /* get corner values */
        for (i = 0; i < 8; i++) {
                gi[0] = ndx[0] + (i & 1);
                gi[1] = ndx[1] + (i>>1 & 1);
                gi[2] = ndx[2] + (i>>2);
                rval |= get3dgpt(cv[i], gi, wp);
        }
        if (rval & W3ERROR)
                return(rval);
        l3interp(po, cv, rem, 3);       /* perform trilinear interpolation */
        return(rval);
}


l3interp(vo, cl, dv, n)         /* trilinear interpolation (recursive) */
W3VEC   vo, *cl, dv;
int     n;
{
        W3VEC   v0, v1;
        register int    i;

        if (--n) {
                l3interp(v0, cl, dv, n);
                l3interp(v1, cl+(1<<n), dv, n);
        } else {
                W3VCPY(v0, cl[0]);
                W3VCPY(v1, cl[1]);
        }
        for (i = 0; i < 3; i++)
                vo[i] = (1.-dv[n])*v0[i] + dv[n]*v1[i];
}


int
warp3dex(po, pi, wp)            /* compute warp using 1/r^2 weighted avg. */
W3VEC   po, pi;
register WARP3D *wp;
{
        double  d2, w, wsum;
        W3VEC   pt;
        register int    i;

        pt[0] = pt[1] = pt[2] = 0.;
        wsum = 0.;
        for (i = wp->npts; i--; ) {
                d2 = wpdist2(pi, wp->ip[i]);
                if (d2 <= MIND*MIND) w = 1./(MIND*MIND);
                else w = 1./d2;
                pt[0] += w*wp->op[i][0];
                pt[1] += w*wp->op[i][1];
                pt[2] += w*wp->op[i][2];
                wsum += w;
        }
        if (wsum > 0.) {
                po[0] = pt[0]/wsum;
                po[1] = pt[1]/wsum;
                po[2] = pt[2]/wsum;
        }
        return(W3OK);
}


WARP3D *
new3dw(flgs)                    /* allocate and initialize WARP3D struct */
int     flgs;
{
        register WARP3D  *wp;

        if ((flgs & (W3EXACT|W3FAST)) == (W3EXACT|W3FAST)) {
                eputs("new3dw: only one of W3EXACT or W3FAST\n");
                return(NULL);
        }
        if ((wp = (WARP3D *)malloc(sizeof(WARP3D))) == NULL) {
                eputs("new3dw: no memory\n");
                return(NULL);
        }
        wp->ip = wp->op = NULL;
        wp->npts = 0;
        wp->grid.flags = flgs;
        wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 0;
        return(wp);
}


WARP3D *
load3dw(fn, wp)                 /* load 3D warp from file */
char    *fn;
WARP3D  *wp;
{
        FILE    *fp;
        W3VEC   inp, outp;

        if ((fp = fopen(fn, "r")) == NULL) {
                eputs(fn);
                eputs(": cannot open\n");
                return(NULL);
        }
        if (wp == NULL && (wp = new3dw()) == NULL)
                goto memerr;
        while (fgetvec(fp, inp) && fgetvec(fp, outp))
                if (!add3dpt(wp, inp, outp))
                        goto memerr;
        if (!feof(fp)) {
                wputs(fn);
                wputs(": non-number in 3D warp file\n");
        }
        goto cleanup;
memerr:
        eputs("load3dw: out of memory\n");
cleanup:
        fclose(fp);
        return(wp);
}


int
add3dpt(wp, pti, pto)           /* add 3D point pair to warp structure */
register WARP3D *wp;
W3VEC   pti, pto;
{
        double  d2;
        register W3VEC  *na;
        register int    i;

        if (wp->npts == 0) {                    /* initialize */
                wp->ip = (W3VEC *)malloc(AHUNK*sizeof(W3VEC));
                if (wp->ip == NULL) return(0);
                wp->op = (W3VEC *)malloc(AHUNK*sizeof(W3VEC));
                if (wp->op == NULL) return(0);
                wp->d2min = 1e10;
                wp->d2max = 0.;
                W3VCPY(wp->llim, pti);
                W3VCPY(wp->ulim, pti);
        } else {
                if (wp->npts % AHUNK == 0) {    /* allocate another hunk */
                        na = (W3VEC *)realloc((char *)wp->ip,
                                        (wp->npts+AHUNK)*sizeof(W3VEC));
                        if (na == NULL) return(0);
                        wp->ip = na;
                        na = (W3VEC *)realloc((char *)wp->op,
                                        (wp->npts+AHUNK)*sizeof(W3VEC));
                        if (na == NULL) return(0);
                        wp->op = na;
                }
                for (i = 0; i < 3; i++)         /* check boundaries */
                        if (pti[i] < wp->llim[i])
                                wp->llim[i] = pti[i];
                        else if (pti[i] > wp->ulim[i])
                                wp->ulim[i] = pti[i];
                for (i = wp->npts; i--; ) {     /* check distances */
                        d2 = wpdist2(pti, wp->ip[i]);
                        if (d2 < MIND*MIND)     /* value is too close */
                                return(wp->npts);
                        if (d2 < wp->d2min)
                                wp->d2min = d2;
                        if (d2 > wp->d2max)
                                wp->d2max = d2;
                }
        }
        W3VCPY(wp->ip[wp->npts], pti);  /* add new point */
        W3VCPY(wp->op[wp->npts], pto);
        done3dgrid(&wp->grid);          /* old grid is invalid */
        return(++wp->npts);
}


free3dw(wp)                     /* free WARP3D data */
register WARP3D *wp;
{
        done3dgrid(&wp->grid);
        free((char *)wp->ip);
        free((char *)wp->op);
        free((char *)wp);
}


long
gridhash(gp)                    /* hash a grid point index */
GNDX    gp;
{
        return(((unsigned long)gp[0]<<GNBITS | gp[1])<<GNBITS | gp[2]);
}


int
new3dwgrid(wp)                  /* initialize interpolating grid for warp */
register WARP3D *wp;
{
        double  gridstep, d;
        register int    i;
                                /* free old grid (if any) */
        done3dgrid(&wp->grid);
                                /* check parameters */
        if (wp->npts < 2)
                return(W3BADMAP);       /* undefined for less than 2 points */
        if (wp->d2max < MIND)
                return(W3BADMAP);       /* not enough disparity */
                                /* compute gamut */
        W3VCPY(wp->grid.gmin, wp->llim);
        W3VCPY(wp->grid.gmax, wp->ulim);
        gridstep = sqrt(wp->d2min);
        for (i = 0; i < 3; i++) {
                wp->grid.gmin[i] -= .501*gridstep;
                wp->grid.gmax[i] += .501*gridstep;
        }
        if (wp->grid.flags & W3EXACT) {
                wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 1;
                return(W3OK);           /* no interpolation, so no grid */
        }
                                /* create grid */
        for (i = 0; i < 3; i++) {
                d = wp->grid.gmax[i] - wp->grid.gmin[i];
                wp->grid.gn[i] = d/gridstep + .5;
                if (wp->grid.gn[i] >= MAXGN)
                        wp->grid.gn[i] = MAXGN-1;
                wp->grid.gstep[i] = d / wp->grid.gn[i];
        }
                                /* initialize lookup table */
        wp->grid.gtab.hashf = gridhash;
        wp->grid.gtab.keycmp = NULL;
        wp->grid.gtab.freek = free;
        wp->grid.gtab.freed = NULL;
        return(lu_init(&wp->grid.gtab, 1024) ? W3OK : W3ERROR);
}


done3dgrid(gp)                  /* free interpolating grid for warp */
register struct grid3d  *gp;
{
        if (gp->gn[0] == 0)
                return;
        lu_done(&gp->gtab);
        gp->gn[0] = gp->gn[1] = gp->gn[2] = 0;
}
Revision:	3.1
Committed:	Tue Feb 4 16:03:48 1997 UTC (28 years, 8 months ago) by greg
Content type:	text/plain
Branch:	MAIN
Log Message:	Initial revision
#	User	Rev	Content
1	greg	3.1	/* Copyright (c) 1997 Regents of the University of California */
2
3			#ifndef lint
4			static char SCCSid[] = "$SunId$ LBL";
5			#endif
6
7			/*
8			* 3D warping routines.
9			*/
10
11			#include <stdio.h>
12
13			#include <math.h>
14
15			#include "fvect.h"
16
17			#include "warp3d.h"
18
19			#define MIND 1e-5 /* minimum distance between input points */
20
21			typedef struct {
22			GNDX n; /* index must be first */
23			W3VEC p;
24			} KEYDP; /* key/data allocation pair */
25
26			#define fgetvec(f,v) (fgetval(f,'f',v) > 0 && fgetval(f,'f',v+1) > 0 \
27			&& fgetval(f,'f',v+2) > 0)
28
29			#define AHUNK 32 /* number of points to allocate at a time */
30
31			#ifndef malloc
32			extern char malloc(), realloc();
33			#endif
34			extern void free();
35
36
37			double
38			wpdist2(p1, p2) /* compute square of distance between points */
39			register W3VEC p1, p2;
40			{
41			double d, d2;
42
43			d = p1[0] - p2[0]; d2 = d*d;
44			d = p1[1] - p2[1]; d2 += d*d;
45			d = p1[2] - p2[2]; d2 += d*d;
46			return(d2);
47			}
48
49
50			int
51			warp3d(po, pi, wp) /* warp 3D point according to the given map */
52			W3VEC po, pi;
53			register WARP3D *wp;
54			{
55			int rval = W3OK;
56			GNDX gi;
57			W3VEC gd;
58
59			if (wp->grid.gn[0] == 0 && (rval = new3dwgrid(wp)) != W3OK)
60			return(rval);
61			rval \|= gridpoint(gi, gd, pi, &wp->grid);
62			if (wp->grid.flags & W3EXACT) {
63			rval \|= warp3dex(po, pi, wp);
64			return(rval);
65			}
66			if (wp->grid.flags & W3FAST) {
67			rval \|= get3dgpt(po, gi, wp);
68			return(rval);
69			}
70			rval \|= get3dgin(po, gi, gd, wp);
71			return(rval);
72			}
73
74
75			int
76			gridpoint(ndx, rem, ipt, gp) /* compute grid position for ipt */
77			GNDX ndx;
78			W3VEC rem, ipt;
79			register struct grid3d *gp;
80			{
81			int rval = W3OK;
82			register int i;
83
84			for (i = 0; i < 3; i++) {
85			rem[i] = (ipt[i] - gp->gmin[i])/gp->gstep[i];
86			if (rem[i] < 0.) {
87			ndx[i] = 0;
88			rval = W3GAMUT;
89			} else if ((int)rem[i] >= gp->gn[i]) {
90			ndx[i] = gp->gn[i] - 1;
91			rval = W3GAMUT;
92			} else
93			ndx[i] = (int)rem[i];
94			rem[i] -= (double)ndx[i];
95			}
96			return(rval);
97			}
98
99
100			int
101			get3dgpt(po, ndx, wp) /* get value for voxel */
102			W3VEC po;
103			GNDX ndx;
104			register WARP3D *wp;
105			{
106			W3VEC gpt;
107			register LUENT *le;
108			KEYDP *kd;
109			int rval = W3OK;
110			register int i;
111
112			le = lu_find(&wp->grid.gtab, (char *)ndx);
113			if (le == NULL)
114			return(W3ERROR);
115			if (le->data == NULL) {
116			if (le->key != NULL)
117			kd = (KEYDP *)le->key;
118			else if ((kd = (KEYDP *)malloc(sizeof(KEYDP))) == NULL)
119			return(W3ERROR);
120			for (i = 0; i < 3; i++) {
121			kd->n[i] = ndx[i];
122			gpt[i] = wp->grid.gmin[i] + ndx[i]*wp->grid.gstep[i];
123			if (wp->grid.flags & W3FAST) /* on centers */
124			gpt[i] += .5*wp->grid.gstep[i];
125			}
126			rval \|= warp3dex(kd->p, gpt, wp);
127			le->key = (char *)kd->n;
128			le->data = (char *)kd->p;
129			}
130			W3VCPY(po, (float *)le->data);
131			return(rval);
132			}
133
134
135			int
136			get3dgin(po, ndx, rem, wp) /* interpolate from warp grid */
137			W3VEC po, rem;
138			GNDX ndx;
139			WARP3D *wp;
140			{
141			W3VEC cv[8];
142			int rval;
143			GNDX gi;
144			register int i;
145			/* get corner values */
146			for (i = 0; i < 8; i++) {
147			gi[0] = ndx[0] + (i & 1);
148			gi[1] = ndx[1] + (i>>1 & 1);
149			gi[2] = ndx[2] + (i>>2);
150			rval \|= get3dgpt(cv[i], gi, wp);
151			}
152			if (rval & W3ERROR)
153			return(rval);
154			l3interp(po, cv, rem, 3); /* perform trilinear interpolation */
155			return(rval);
156			}
157
158
159			l3interp(vo, cl, dv, n) /* trilinear interpolation (recursive) */
160			W3VEC vo, *cl, dv;
161			int n;
162			{
163			W3VEC v0, v1;
164			register int i;
165
166			if (--n) {
167			l3interp(v0, cl, dv, n);
168			l3interp(v1, cl+(1<<n), dv, n);
169			} else {
170			W3VCPY(v0, cl[0]);
171			W3VCPY(v1, cl[1]);
172			}
173			for (i = 0; i < 3; i++)
174			vo[i] = (1.-dv[n])v0[i] + dv[n]v1[i];
175			}
176
177
178			int
179			warp3dex(po, pi, wp) /* compute warp using 1/r^2 weighted avg. */
180			W3VEC po, pi;
181			register WARP3D *wp;
182			{
183			double d2, w, wsum;
184			W3VEC pt;
185			register int i;
186
187			pt[0] = pt[1] = pt[2] = 0.;
188			wsum = 0.;
189			for (i = wp->npts; i--; ) {
190			d2 = wpdist2(pi, wp->ip[i]);
191			if (d2 <= MINDMIND) w = 1./(MINDMIND);
192			else w = 1./d2;
193			pt[0] += w*wp->op[i][0];
194			pt[1] += w*wp->op[i][1];
195			pt[2] += w*wp->op[i][2];
196			wsum += w;
197			}
198			if (wsum > 0.) {
199			po[0] = pt[0]/wsum;
200			po[1] = pt[1]/wsum;
201			po[2] = pt[2]/wsum;
202			}
203			return(W3OK);
204			}
205
206
207			WARP3D *
208			new3dw(flgs) /* allocate and initialize WARP3D struct */
209			int flgs;
210			{
211			register WARP3D *wp;
212
213			if ((flgs & (W3EXACT\|W3FAST)) == (W3EXACT\|W3FAST)) {
214			eputs("new3dw: only one of W3EXACT or W3FAST\n");
215			return(NULL);
216			}
217			if ((wp = (WARP3D *)malloc(sizeof(WARP3D))) == NULL) {
218			eputs("new3dw: no memory\n");
219			return(NULL);
220			}
221			wp->ip = wp->op = NULL;
222			wp->npts = 0;
223			wp->grid.flags = flgs;
224			wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 0;
225			return(wp);
226			}
227
228
229			WARP3D *
230			load3dw(fn, wp) /* load 3D warp from file */
231			char *fn;
232			WARP3D *wp;
233			{
234			FILE *fp;
235			W3VEC inp, outp;
236
237			if ((fp = fopen(fn, "r")) == NULL) {
238			eputs(fn);
239			eputs(": cannot open\n");
240			return(NULL);
241			}
242			if (wp == NULL && (wp = new3dw()) == NULL)
243			goto memerr;
244			while (fgetvec(fp, inp) && fgetvec(fp, outp))
245			if (!add3dpt(wp, inp, outp))
246			goto memerr;
247			if (!feof(fp)) {
248			wputs(fn);
249			wputs(": non-number in 3D warp file\n");
250			}
251			goto cleanup;
252			memerr:
253			eputs("load3dw: out of memory\n");
254			cleanup:
255			fclose(fp);
256			return(wp);
257			}
258
259
260			int
261			add3dpt(wp, pti, pto) /* add 3D point pair to warp structure */
262			register WARP3D *wp;
263			W3VEC pti, pto;
264			{
265			double d2;
266			register W3VEC *na;
267			register int i;
268
269			if (wp->npts == 0) { /* initialize */
270			wp->ip = (W3VEC )malloc(AHUNKsizeof(W3VEC));
271			if (wp->ip == NULL) return(0);
272			wp->op = (W3VEC )malloc(AHUNKsizeof(W3VEC));
273			if (wp->op == NULL) return(0);
274			wp->d2min = 1e10;
275			wp->d2max = 0.;
276			W3VCPY(wp->llim, pti);
277			W3VCPY(wp->ulim, pti);
278			} else {
279			if (wp->npts % AHUNK == 0) { /* allocate another hunk */
280			na = (W3VEC )realloc((char )wp->ip,
281			(wp->npts+AHUNK)*sizeof(W3VEC));
282			if (na == NULL) return(0);
283			wp->ip = na;
284			na = (W3VEC )realloc((char )wp->op,
285			(wp->npts+AHUNK)*sizeof(W3VEC));
286			if (na == NULL) return(0);
287			wp->op = na;
288			}
289			for (i = 0; i < 3; i++) /* check boundaries */
290			if (pti[i] < wp->llim[i])
291			wp->llim[i] = pti[i];
292			else if (pti[i] > wp->ulim[i])
293			wp->ulim[i] = pti[i];
294			for (i = wp->npts; i--; ) { /* check distances */
295			d2 = wpdist2(pti, wp->ip[i]);
296			if (d2 < MINDMIND) / value is too close */
297			return(wp->npts);
298			if (d2 < wp->d2min)
299			wp->d2min = d2;
300			if (d2 > wp->d2max)
301			wp->d2max = d2;
302			}
303			}
304			W3VCPY(wp->ip[wp->npts], pti); /* add new point */
305			W3VCPY(wp->op[wp->npts], pto);
306			done3dgrid(&wp->grid); /* old grid is invalid */
307			return(++wp->npts);
308			}
309
310
311			free3dw(wp) /* free WARP3D data */
312			register WARP3D *wp;
313			{
314			done3dgrid(&wp->grid);
315			free((char *)wp->ip);
316			free((char *)wp->op);
317			free((char *)wp);
318			}
319
320
321			long
322			gridhash(gp) /* hash a grid point index */
323			GNDX gp;
324			{
325			return(((unsigned long)gp[0]<<GNBITS \| gp[1])<<GNBITS \| gp[2]);
326			}
327
328
329			int
330			new3dwgrid(wp) /* initialize interpolating grid for warp */
331			register WARP3D *wp;
332			{
333			double gridstep, d;
334			register int i;
335			/* free old grid (if any) */
336			done3dgrid(&wp->grid);
337			/* check parameters */
338			if (wp->npts < 2)
339			return(W3BADMAP); /* undefined for less than 2 points */
340			if (wp->d2max < MIND)
341			return(W3BADMAP); /* not enough disparity */
342			/* compute gamut */
343			W3VCPY(wp->grid.gmin, wp->llim);
344			W3VCPY(wp->grid.gmax, wp->ulim);
345			gridstep = sqrt(wp->d2min);
346			for (i = 0; i < 3; i++) {
347			wp->grid.gmin[i] -= .501*gridstep;
348			wp->grid.gmax[i] += .501*gridstep;
349			}
350			if (wp->grid.flags & W3EXACT) {
351			wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 1;
352			return(W3OK); /* no interpolation, so no grid */
353			}
354			/* create grid */
355			for (i = 0; i < 3; i++) {
356			d = wp->grid.gmax[i] - wp->grid.gmin[i];
357			wp->grid.gn[i] = d/gridstep + .5;
358			if (wp->grid.gn[i] >= MAXGN)
359			wp->grid.gn[i] = MAXGN-1;
360			wp->grid.gstep[i] = d / wp->grid.gn[i];
361			}
362			/* initialize lookup table */
363			wp->grid.gtab.hashf = gridhash;
364			wp->grid.gtab.keycmp = NULL;
365			wp->grid.gtab.freek = free;
366			wp->grid.gtab.freed = NULL;
367			return(lu_init(&wp->grid.gtab, 1024) ? W3OK : W3ERROR);
368			}
369
370
371			done3dgrid(gp) /* free interpolating grid for warp */
372			register struct grid3d *gp;
373			{
374			if (gp->gn[0] == 0)
375			return;
376			lu_done(&gp->gtab);
377			gp->gn[0] = gp->gn[1] = gp->gn[2] = 0;
378			}