src/px/warp3d.c

#ifndef lint
static const char       RCSid[] = "$Id: warp3d.c,v 3.11 2019/12/28 18:05:14 greg Exp $";
#endif
/*
 * 3D warping routines.
 */

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

#include "rterror.h"
#include "rtio.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   v;
} KEYDP;                /* key/data allocation pair */

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

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


double wpdist2(W3VEC p1, W3VEC p2);
static int gridpoint(GNDX ndx, W3VEC rem, W3VEC ipt, struct grid3d *gp);
static int get3dgpt(W3VEC ov, GNDX ndx, WARP3D *wp);
static int get3dgin(W3VEC ov, GNDX ndx, W3VEC rem, WARP3D *wp);
static void l3interp(W3VEC vo, W3VEC *cl, W3VEC dv, int n);
static int warp3dex(W3VEC ov, W3VEC pi, WARP3D *wp);
static lut_hashf_t gridhash;
static int new3dgrid(WARP3D *wp);
static void done3dgrid(struct grid3d *gp);


double
wpdist2(                        /* compute square of distance between points */
        W3VEC   p1,
        W3VEC   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(         /* warp 3D point according to the given map */
        W3VEC   po,
        W3VEC   pi,
        WARP3D  *wp
)
{
        int     rval = W3OK;
        GNDX    gi;
        W3VEC   gd, ov;

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


static int
gridpoint(      /* compute grid position for ipt */
        GNDX    ndx,
        W3VEC   rem,
        W3VEC   ipt,
        struct grid3d   *gp
)
{
        int     rval = W3OK;
        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);
}


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

        le = lu_find(&wp->grid.gtab, 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->v, gpt, wp);
                le->key = (char *)kd->n;
                le->data = (char *)kd->v;
        }
        W3VCPY(ov, (float *)le->data);
        return(rval);
}


static int
get3dgin(       /* interpolate from warp grid */
        W3VEC   ov,
        GNDX    ndx,
        W3VEC   rem,
        WARP3D  *wp
)
{
        W3VEC   cv[8];
        GNDX    gi;
        int     rval = W3OK;
        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(ov, cv, rem, 3);       /* perform trilinear interpolation */
        return(rval);
}


static void
l3interp(               /* trilinear interpolation (recursive) */
        W3VEC   vo,
        W3VEC   *cl,
        W3VEC   dv,
        int     n
)
{
        W3VEC   v0, v1;
        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];
}


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

        vt[0] = vt[1] = vt[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;
                vt[0] += w*wp->ov[i][0];
                vt[1] += w*wp->ov[i][1];
                vt[2] += w*wp->ov[i][2];
                wsum += w;
        }
        if (wsum > 0.) {
                ov[0] = vt[0]/wsum;
                ov[1] = vt[1]/wsum;
                ov[2] = vt[2]/wsum;
        }
        return(W3OK);
}


WARP3D *
new3dw(                 /* allocate and initialize WARP3D struct */
        int     flgs
)
{
        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->ov = 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(                        /* 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(0)) == 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);
}


extern int
set3dwfl(               /* reset warp flags */
        WARP3D  *wp,
        int     flgs
)
{
        if (flgs == wp->grid.flags)
                return(0);
        if ((flgs & (W3EXACT|W3FAST)) == (W3EXACT|W3FAST)) {
                eputs("set3dwfl: only one of W3EXACT or W3FAST\n");
                return(-1);
        }
        wp->grid.flags = flgs;
        done3dgrid(&wp->grid);          /* old grid is invalid */
        return(0);
}


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

        if (wp->npts == 0) {                    /* initialize */
                wp->ip = (W3VEC *)malloc(AHUNK*sizeof(W3VEC));
                if (wp->ip == NULL) return(0);
                wp->ov = (W3VEC *)malloc(AHUNK*sizeof(W3VEC));
                if (wp->ov == 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((void *)wp->ip,
                                        (wp->npts+AHUNK)*sizeof(W3VEC));
                        if (na == NULL) return(0);
                        wp->ip = na;
                        na = (W3VEC *)realloc((void *)wp->ov,
                                        (wp->npts+AHUNK)*sizeof(W3VEC));
                        if (na == NULL) return(0);
                        wp->ov = 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 */
        wp->ov[wp->npts][0] = pto[0] - pti[0];
        wp->ov[wp->npts][1] = pto[1] - pti[1];
        wp->ov[wp->npts][2] = pto[2] - pti[2];
        done3dgrid(&wp->grid);          /* old grid is invalid */
        return(++wp->npts);
}


extern void
free3dw(                        /* free WARP3D data */
        WARP3D  *wp
)
{
        done3dgrid(&wp->grid);
        free(wp->ip);
        free(wp->ov);
        free(wp);
}


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


static int
new3dgrid(                      /* initialize interpolating grid for warp */
        WARP3D  *wp
)
{
        W3VEC   gmax;
        double  gridstep, d;
        int     n;
        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(gmax, wp->ulim);
        gridstep = sqrt(wp->d2min);
        for (i = 0; i < 3; i++) {
                wp->grid.gmin[i] -= .501*gridstep;
                gmax[i] += .501*gridstep;
        }
        if (wp->grid.flags & W3EXACT) {
                wp->grid.gn[0] = wp->grid.gn[1] = wp->grid.gn[2] = 1;
                wp->grid.gstep[0] = gmax[0] - wp->grid.gmin[0];
                wp->grid.gstep[1] = gmax[1] - wp->grid.gmin[1];
                wp->grid.gstep[2] = gmax[2] - wp->grid.gmin[2];
                return(W3OK);           /* no interpolation, so no grid */
        }
                                /* create grid */
        for (i = 0; i < 3; i++) {
                d = gmax[i] - wp->grid.gmin[i];
                n = d/gridstep + .5;
                if (n >= MAXGN-1)
                        n = MAXGN-2;
                wp->grid.gstep[i] = d / n;
                wp->grid.gn[i] = n;
        }
                                /* 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);
}


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