src/common/interp2d.c

#ifndef lint
static const char RCSid[] = "$Id: interp2d.c,v 2.16 2021/03/11 01:58:59 greg Exp $";
#endif
/*
 * General interpolation method for unstructured values on 2-D plane.
 *
 *      G.Ward Feb 2013
 */

#include "copyright.h"

/***************************************************************
 * This is a general method for 2-D interpolation similar to
 * radial basis functions but allowing for a good deal of local
 * anisotropy in the point distribution.  Each sample point
 * is examined to determine the closest neighboring samples in
 * each of NI2DIR surrounding directions.  To speed this
 * calculation, we sort the data into half-planes and apply
 * simple tests to see which neighbor is closest in each
 * angular slice.  Once we have our approximate neighborhood
 * for a sample, we can use it in a modified Gaussian weighting
 * with allowing local anisotropy.  Harmonic weighting is added
 * to reduce the influence of distant neighbors.  This yields a
 * smooth interpolation regardless of how the sample points are
 * initially distributed.  Evaluation is accelerated by use of
 * a fast approximation to the atan2(y,x) function and a low-res
 * map indicating where sample weights are significant.
 ****************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include "rtmath.h"
#include "interp2d.h"

#define DECODE_DIA(ip,ed)       ((ip)->dmin*(1. + .5*(ed)))
#define ENCODE_DIA(ip,d)        ((int)(2.*(d)/(ip)->dmin) - 2)

/* Sample order (private) */
typedef struct {
        int     si;             /* sample index */
        float   dm;             /* distance measure in this direction */
} SAMPORD;

/* private routine to encode sample diameter with range checks */
static int
encode_diameter(const INTERP2 *ip, double d)
{
        const int       ed = ENCODE_DIA(ip, d);

        if (ed <= 0)
                return(0);
        if (ed >= 0xffff)
                return(0xffff);
        return(ed);
}

/* Allocate a new set of interpolation samples (caller assigns spt[] array) */
INTERP2 *
interp2_alloc(int nsamps)
{
        INTERP2         *nip;

        if (nsamps <= 1)
                return(NULL);

        nip = (INTERP2 *)malloc(sizeof(INTERP2) + sizeof(float)*2*(nsamps-1));
        if (nip == NULL)
                return(NULL);

        nip->ns = nsamps;
        nip->dmin = 1;          /* default minimum diameter */
        nip->smf = NI2DSMF;     /* default smoothing factor */
        nip->c_data = NULL;
        nip->da = NULL;
                                /* caller must assign spt[] array */
        return(nip);
}

/* Resize interpolation array (caller must assign any new values) */
INTERP2 *
interp2_realloc(INTERP2 *ip, int nsamps)
{
        INTERP2 *old_ip = ip;

        if (ip == NULL)
                return(interp2_alloc(nsamps));
        if (nsamps <= 1) {
                interp2_free(ip);
                return(NULL);
        }
        if (nsamps == ip->ns)
                return(ip);
        if (ip->da != NULL) {   /* will need to recompute distribution */
                free(ip->da);
                ip->da = NULL;
        }
        ip = (INTERP2 *)realloc(ip, sizeof(INTERP2)+sizeof(float)*2*(nsamps-1));
        if (ip == NULL) {
                if (nsamps <= ip->ns) {
                        ip = old_ip;
                } else {
                        free(old_ip);
                        return(NULL);
                }
        }
        ip->ns = nsamps;
        return(ip);
}

/* Set minimum distance under which samples will start to merge */
void
interp2_spacing(INTERP2 *ip, double mind)
{
        if (mind <= 0)
                return;
        if ((.998*ip->dmin <= mind) & (mind <= 1.002*ip->dmin))
                return;
        if (ip->da != NULL) {   /* will need to recompute distribution */
                free(ip->da);
                ip->da = NULL;
        }
        ip->dmin = mind;
}

/* Compute unnormalized weight for a position relative to a sample */
double
interp2_wti(INTERP2 *ip, const int i, double x, double y)
{
        double          dir, rd, r2, d2;
        int             ri;
                                /* get relative direction */
        x -= ip->spt[i][0];
        y -= ip->spt[i][1];
        dir = atan2a(y, x);
        dir += 2.*PI*(dir < 0);
                                /* linear radius interpolation */
        rd = dir * (NI2DIR/2./PI);
        ri = (int)rd;
        rd -= (double)ri;
        rd = (1.-rd)*ip->da[i].dia[ri] + rd*ip->da[i].dia[(ri+1)%NI2DIR];
        rd = ip->smf * DECODE_DIA(ip, rd);
        r2 = 2.*rd*rd;
        d2 = x*x + y*y;
        if (d2 > 21.*r2)        /* result would be < 1e-9 */
                return(.0);
                                /* Gaussian times harmonic weighting */
        return( exp(-d2/r2) * ip->dmin/(ip->dmin + sqrt(d2)) );
}

/* private call to get grid flag index */
static int
interp2_flagpos(int fgi[2], INTERP2 *ip, double x, double y)
{
        int     inbounds = 0;

        if (ip == NULL)         /* paranoia */
                return(-1);
                                /* need to compute interpolant? */
        if (ip->da == NULL && !interp2_analyze(ip))
                return(-1);
                                /* get x & y grid positions */
        fgi[0] = (x - ip->smin[0]) * NI2DIM / (ip->smax[0] - ip->smin[0]);

        if (fgi[0] >= NI2DIM)
                fgi[0] = NI2DIM-1;
        else if (fgi[0] < 0)
                fgi[0] = 0;
        else
                ++inbounds;

        fgi[1] = (y - ip->smin[1]) * NI2DIM / (ip->smax[1] - ip->smin[1]);

        if (fgi[1] >= NI2DIM)
                fgi[1] = NI2DIM-1;
        else if (fgi[1] < 0)
                fgi[1] = 0;
        else
                ++inbounds;

        return(inbounds == 2);
}

#define setflg(fl,xi,yi)        ((fl)[yi] |= 1<<(xi))

#define chkflg(fl,xi,yi)        ((fl)[yi]>>(xi) & 1)

/* private flood function to determine sample influence */
static void
influence_flood(INTERP2 *ip, const int i, unsigned short visited[NI2DIM],
                        int xfi, int yfi)
{
        double  gx = (xfi+.5)*(1./NI2DIM)*(ip->smax[0] - ip->smin[0]) +
                                        ip->smin[0];
        double  gy = (yfi+.5)*(1./NI2DIM)*(ip->smax[1] - ip->smin[1]) +
                                        ip->smin[1];
        double  dx = gx - ip->spt[i][0];
        double  dy = gy - ip->spt[i][1];

        setflg(visited, xfi, yfi);

        if (dx*dx + dy*dy > 2.*ip->grid2 && interp2_wti(ip, i, gx, gy) <= 1e-7)
                return;

        setflg(ip->da[i].infl, xfi, yfi);

        if (xfi > 0 && !chkflg(visited, xfi-1, yfi))
                influence_flood(ip, i, visited, xfi-1, yfi);

        if (yfi > 0 && !chkflg(visited, xfi, yfi-1))
                influence_flood(ip, i, visited, xfi, yfi-1);

        if (xfi < NI2DIM-1 && !chkflg(visited, xfi+1, yfi))
                influence_flood(ip, i, visited, xfi+1, yfi);

        if (yfi < NI2DIM-1 && !chkflg(visited, xfi, yfi+1))
                influence_flood(ip, i, visited, xfi, yfi+1);
}

/* private call to compute sample influence maps */
static void
map_influence(INTERP2 *ip)
{
        unsigned short  visited[NI2DIM];
        int             fgi[2];
        int             i, j;

        for (i = ip->ns; i--; ) {
                for (j = NI2DIM; j--; ) {
                        ip->da[i].infl[j] = 0;
                        visited[j] = 0;
                }
                interp2_flagpos(fgi, ip, ip->spt[i][0], ip->spt[i][1]);

                influence_flood(ip, i, visited, fgi[0], fgi[1]);
        }
}

/* Modify smoothing parameter by the given factor */
void
interp2_smooth(INTERP2 *ip, double sf)
{
        float   old_smf = ip->smf;

        if ((ip->smf *= sf) < NI2DSMF)
                ip->smf = NI2DSMF;
                                        /* need to recompute influence maps? */
        if (ip->da != NULL && (old_smf*.85 > ip->smf) |
                                (ip->smf > old_smf*1.15))
                map_influence(ip);
}

/* private call-back to sort position index */
static int
cmp_spos(const void *p1, const void *p2)
{
        const SAMPORD   *so1 = (const SAMPORD *)p1;
        const SAMPORD   *so2 = (const SAMPORD *)p2;

        if (so1->dm > so2->dm)
                return 1;
        if (so1->dm < so2->dm)
                return -1;
        return 0;
}

/* private routine to order samples in a particular direction */
static void
sort_samples(SAMPORD *sord, const INTERP2 *ip, double ang)
{
        const double    cosd = cos(ang);
        const double    sind = sin(ang);
        int             i;

        for (i = ip->ns; i--; ) {
                sord[i].si = i;
                sord[i].dm = cosd*ip->spt[i][0] + sind*ip->spt[i][1];
        }
        qsort(sord, ip->ns, sizeof(SAMPORD), cmp_spos);
}

/* (Re)compute anisotropic basis function interpolant (normally automatic) */
int
interp2_analyze(INTERP2 *ip)
{
        SAMPORD *sortord;
        int     *rightrndx, *leftrndx, *endrndx;
        int     i, bd;
                                        /* sanity checks */
        if (ip == NULL)
                return(0);
        if (ip->da != NULL) {           /* free previous data if any */
                free(ip->da);
                ip->da = NULL;
        }
        if ((ip->ns <= 1) | (ip->dmin <= 0))
                return(0);
                                        /* compute sample domain */
        ip->smin[0] = ip->smin[1] = FHUGE;
        ip->smax[0] = ip->smax[1] = -FHUGE;
        for (i = ip->ns; i--; ) {
                if (ip->spt[i][0] < ip->smin[0]) ip->smin[0] = ip->spt[i][0];
                if (ip->spt[i][0] > ip->smax[0]) ip->smax[0] = ip->spt[i][0];
                if (ip->spt[i][1] < ip->smin[1]) ip->smin[1] = ip->spt[i][1];
                if (ip->spt[i][1] > ip->smax[1]) ip->smax[1] = ip->spt[i][1];
        }
        ip->grid2 = ((ip->smax[0]-ip->smin[0])*(ip->smax[0]-ip->smin[0]) +
                        (ip->smax[1]-ip->smin[1])*(ip->smax[1]-ip->smin[1])) *
                                (1./NI2DIM/NI2DIM);
        if (ip->grid2 <= FTINY*ip->dmin*ip->dmin)
                return(0);
                                        /* allocate analysis data */
        ip->da = (struct interp2_samp *)malloc(
                                sizeof(struct interp2_samp)*ip->ns );
        if (ip->da == NULL)
                return(0);
                                        /* allocate temporary arrays */
        sortord = (SAMPORD *)malloc(sizeof(SAMPORD)*ip->ns);
        rightrndx = (int *)malloc(sizeof(int)*ip->ns);
        leftrndx = (int *)malloc(sizeof(int)*ip->ns);
        endrndx = (int *)malloc(sizeof(int)*ip->ns);
        if ((sortord == NULL) | (rightrndx == NULL) |
                        (leftrndx == NULL) | (endrndx == NULL))
                return(0);
                                        /* run through bidirections */
        for (bd = 0; bd < NI2DIR/2; bd++) {
            const double        ang = 2.*PI/NI2DIR*bd;
            int                 *sptr;
                                        /* create right reverse index */
            if (bd) {                   /* re-use from previous iteration? */
                sptr = rightrndx;
                rightrndx = leftrndx;
                leftrndx = sptr;
            } else {                    /* else sort first half-plane */
                sort_samples(sortord, ip, PI/2. - PI/NI2DIR);
                for (i = ip->ns; i--; )
                    rightrndx[sortord[i].si] = i;
                                        /* & store reverse order for later */
                for (i = ip->ns; i--; )
                    endrndx[sortord[i].si] = ip->ns-1 - i;
            }
                                        /* create new left reverse index */
            if (bd == NI2DIR/2 - 1) {   /* use order from first iteration? */
                sptr = leftrndx;
                leftrndx = endrndx;
                endrndx = sptr;
            } else {                    /* else compute new half-plane */
                sort_samples(sortord, ip, ang + (PI/2. + PI/NI2DIR));
                for (i = ip->ns; i--; )
                    leftrndx[sortord[i].si] = i;
            }
                                        /* sort grid values in this direction */
            sort_samples(sortord, ip, ang);
                                        /* find nearest neighbors each side */
            for (i = ip->ns; i--; ) {
                const int       ii = sortord[i].si;
                int             j;
                                        /* preload with large radii */
                ip->da[ii].dia[bd] =
                ip->da[ii].dia[bd+NI2DIR/2] = encode_diameter(ip,
                                .5*(sortord[ip->ns-1].dm - sortord[0].dm));
                for (j = i; ++j < ip->ns; )     /* nearest above */
                    if (rightrndx[sortord[j].si] > rightrndx[ii] &&
                                    leftrndx[sortord[j].si] < leftrndx[ii]) {
                        ip->da[ii].dia[bd] = encode_diameter(ip,
                                                sortord[j].dm - sortord[i].dm);
                        break;
                    }
                for (j = i; j-- > 0; )          /* nearest below */
                    if (rightrndx[sortord[j].si] < rightrndx[ii] &&
                                    leftrndx[sortord[j].si] > leftrndx[ii]) {
                        ip->da[ii].dia[bd+NI2DIR/2] = encode_diameter(ip,
                                                sortord[i].dm - sortord[j].dm);
                        break;
                    }
            }
        }
        free(sortord);                  /* release temp arrays */
        free(rightrndx);
        free(leftrndx);
        free(endrndx);
                                        /* map sample influence areas */
        map_influence(ip);
        return(1);                      /* all done */
}

/* Assign full set of normalized weights to interpolate the given position */
int
interp2_weights(float wtv[], INTERP2 *ip, double x, double y)
{
        double  wnorm;
        int     fgi[2];
        int     i;

        if (wtv == NULL)
                return(0);
                                        /* get flag position */
        if (interp2_flagpos(fgi, ip, x, y) < 0)
                return(0);

        wnorm = 0;                      /* compute raw weights */
        for (i = ip->ns; i--; )
            if (chkflg(ip->da[i].infl, fgi[0], fgi[1])) {
                double  wt = interp2_wti(ip, i, x, y);
                wtv[i] = wt;
                wnorm += wt;
            } else
                wtv[i] = 0;
        if (wnorm <= 0)                 /* too far from all our samples! */
                return(0);
        wnorm = 1./wnorm;               /* normalize weights */
        for (i = ip->ns; i--; )
                wtv[i] *= wnorm;
        return(ip->ns);                 /* all done */
}


/* Get normalized weights and indexes for n best samples in descending order */
int
interp2_topsamp(float wt[], int si[], const int n, INTERP2 *ip, double x, double y)
{
        int     nn = 0;
        int     fgi[2];
        double  wnorm;
        int     i, j;

        if ((n <= 0) | (wt == NULL) | (si == NULL))
                return(0);
                                        /* get flag position */
        if (interp2_flagpos(fgi, ip, x, y) < 0)
                return(0);
                                        /* identify top n weights */
        for (i = ip->ns; i--; )
            if (chkflg(ip->da[i].infl, fgi[0], fgi[1])) {
                const double    wti = interp2_wti(ip, i, x, y);
                for (j = nn; j > 0; j--) {
                        if (wt[j-1] >= wti)
                                break;
                        if (j < n) {
                                wt[j] = wt[j-1];
                                si[j] = si[j-1];
                        }
                }
                if (j < n) {            /* add/insert sample */
                        wt[j] = wti;
                        si[j] = i;
                        nn += (nn < n);
                }
            }
        wnorm = 0;                      /* normalize sample weights */
        for (j = nn; j--; )
                wnorm += wt[j];
        if (wnorm <= 0)
                return(0);
        wnorm = 1./wnorm;
        for (j = nn; j--; )
                wt[j] *= wnorm;
        return(nn);                     /* return actual # samples */
}

/* Free interpolant */
void
interp2_free(INTERP2 *ip)
{
        if (ip == NULL)
                return;
        if (ip->da != NULL)
                free(ip->da);
        free(ip);
}
Revision:	2.17
Committed:	Fri Mar 19 21:16:15 2021 UTC (3 years, 1 month ago) by greg
Content type:	text/plain
Branch:	MAIN
CVS Tags:	rad5R4, HEAD
Changes since 2.16:	+5 -3 lines
Log Message:	fix: free memory on failure
#	User	Rev	Content
1	greg	2.1	#ifndef lint
2	greg	2.17	static const char RCSid[] = "$Id: interp2d.c,v 2.16 2021/03/11 01:58:59 greg Exp $";
3	greg	2.1	#endif
4			/*
5			* General interpolation method for unstructured values on 2-D plane.
6			*
7			* G.Ward Feb 2013
8			*/
9
10			#include "copyright.h"
11
12	greg	2.4	/***************************************************************
13	greg	2.1	* This is a general method for 2-D interpolation similar to
14			* radial basis functions but allowing for a good deal of local
15			* anisotropy in the point distribution. Each sample point
16			* is examined to determine the closest neighboring samples in
17			* each of NI2DIR surrounding directions. To speed this
18	greg	2.4	* calculation, we sort the data into half-planes and apply
19			* simple tests to see which neighbor is closest in each
20	greg	2.7	* angular slice. Once we have our approximate neighborhood
21	greg	2.3	* for a sample, we can use it in a modified Gaussian weighting
22	greg	2.4	* with allowing local anisotropy. Harmonic weighting is added
23	greg	2.3	* to reduce the influence of distant neighbors. This yields a
24			* smooth interpolation regardless of how the sample points are
25	greg	2.4	* initially distributed. Evaluation is accelerated by use of
26	greg	2.13	* a fast approximation to the atan2(y,x) function and a low-res
27			* map indicating where sample weights are significant.
28	greg	2.4	****************************************************************/
29	greg	2.1
30			#include <stdio.h>
31			#include <stdlib.h>
32			#include "rtmath.h"
33			#include "interp2d.h"
34
35	greg	2.4	#define DECODE_DIA(ip,ed) ((ip)->dmin(1. + .5(ed)))
36			#define ENCODE_DIA(ip,d) ((int)(2.*(d)/(ip)->dmin) - 2)
37	greg	2.1
38			/* Sample order (private) */
39			typedef struct {
40			int si; /* sample index */
41			float dm; /* distance measure in this direction */
42			} SAMPORD;
43
44	greg	2.13	/* private routine to encode sample diameter with range checks */
45			static int
46			encode_diameter(const INTERP2 *ip, double d)
47			{
48			const int ed = ENCODE_DIA(ip, d);
49
50			if (ed <= 0)
51			return(0);
52			if (ed >= 0xffff)
53			return(0xffff);
54			return(ed);
55			}
56
57	greg	2.2	/* Allocate a new set of interpolation samples (caller assigns spt[] array) */
58	greg	2.1	INTERP2 *
59			interp2_alloc(int nsamps)
60			{
61			INTERP2 *nip;
62
63			if (nsamps <= 1)
64			return(NULL);
65
66			nip = (INTERP2 )malloc(sizeof(INTERP2) + sizeof(float)2*(nsamps-1));
67			if (nip == NULL)
68			return(NULL);
69
70			nip->ns = nsamps;
71	greg	2.4	nip->dmin = 1; /* default minimum diameter */
72	greg	2.1	nip->smf = NI2DSMF; /* default smoothing factor */
73	greg	2.15	nip->c_data = NULL;
74	greg	2.4	nip->da = NULL;
75	greg	2.1	/* caller must assign spt[] array */
76			return(nip);
77			}
78
79	greg	2.2	/* Resize interpolation array (caller must assign any new values) */
80			INTERP2 *
81			interp2_realloc(INTERP2 *ip, int nsamps)
82			{
83	greg	2.16	INTERP2 *old_ip = ip;
84
85	greg	2.2	if (ip == NULL)
86			return(interp2_alloc(nsamps));
87			if (nsamps <= 1) {
88			interp2_free(ip);
89			return(NULL);
90			}
91	greg	2.8	if (nsamps == ip->ns)
92	greg	2.2	return(ip);
93	greg	2.4	if (ip->da != NULL) { /* will need to recompute distribution */
94			free(ip->da);
95			ip->da = NULL;
96	greg	2.2	}
97			ip = (INTERP2 )realloc(ip, sizeof(INTERP2)+sizeof(float)2*(nsamps-1));
98	greg	2.16	if (ip == NULL) {
99	greg	2.17	if (nsamps <= ip->ns) {
100	greg	2.16	ip = old_ip;
101	greg	2.17	} else {
102			free(old_ip);
103	greg	2.16	return(NULL);
104	greg	2.17	}
105	greg	2.16	}
106	greg	2.2	ip->ns = nsamps;
107			return(ip);
108			}
109
110	greg	2.5	/* Set minimum distance under which samples will start to merge */
111			void
112			interp2_spacing(INTERP2 *ip, double mind)
113			{
114			if (mind <= 0)
115			return;
116	greg	2.7	if ((.998ip->dmin <= mind) & (mind <= 1.002ip->dmin))
117	greg	2.5	return;
118			if (ip->da != NULL) { /* will need to recompute distribution */
119			free(ip->da);
120			ip->da = NULL;
121			}
122			ip->dmin = mind;
123			}
124
125	greg	2.12	/* Compute unnormalized weight for a position relative to a sample */
126			double
127			interp2_wti(INTERP2 *ip, const int i, double x, double y)
128			{
129			double dir, rd, r2, d2;
130			int ri;
131			/* get relative direction */
132			x -= ip->spt[i][0];
133			y -= ip->spt[i][1];
134			dir = atan2a(y, x);
135			dir += 2.PI(dir < 0);
136			/* linear radius interpolation */
137			rd = dir * (NI2DIR/2./PI);
138			ri = (int)rd;
139			rd -= (double)ri;
140			rd = (1.-rd)ip->da[i].dia[ri] + rdip->da[i].dia[(ri+1)%NI2DIR];
141			rd = ip->smf * DECODE_DIA(ip, rd);
142			r2 = 2.rdrd;
143			d2 = xx + yy;
144			if (d2 > 21.r2) / result would be < 1e-9 */
145			return(.0);
146			/* Gaussian times harmonic weighting */
147			return( exp(-d2/r2) * ip->dmin/(ip->dmin + sqrt(d2)) );
148			}
149
150			/* private call to get grid flag index */
151			static int
152			interp2_flagpos(int fgi[2], INTERP2 *ip, double x, double y)
153			{
154			int inbounds = 0;
155
156			if (ip == NULL) /* paranoia */
157			return(-1);
158			/* need to compute interpolant? */
159			if (ip->da == NULL && !interp2_analyze(ip))
160			return(-1);
161			/* get x & y grid positions */
162			fgi[0] = (x - ip->smin[0]) * NI2DIM / (ip->smax[0] - ip->smin[0]);
163
164			if (fgi[0] >= NI2DIM)
165			fgi[0] = NI2DIM-1;
166			else if (fgi[0] < 0)
167			fgi[0] = 0;
168			else
169			++inbounds;
170
171			fgi[1] = (y - ip->smin[1]) * NI2DIM / (ip->smax[1] - ip->smin[1]);
172
173			if (fgi[1] >= NI2DIM)
174			fgi[1] = NI2DIM-1;
175			else if (fgi[1] < 0)
176			fgi[1] = 0;
177			else
178			++inbounds;
179
180			return(inbounds == 2);
181			}
182
183			#define setflg(fl,xi,yi) ((fl)[yi] \|= 1<<(xi))
184
185			#define chkflg(fl,xi,yi) ((fl)[yi]>>(xi) & 1)
186
187			/* private flood function to determine sample influence */
188			static void
189			influence_flood(INTERP2 *ip, const int i, unsigned short visited[NI2DIM],
190			int xfi, int yfi)
191			{
192			double gx = (xfi+.5)(1./NI2DIM)(ip->smax[0] - ip->smin[0]) +
193			ip->smin[0];
194			double gy = (yfi+.5)(1./NI2DIM)(ip->smax[1] - ip->smin[1]) +
195			ip->smin[1];
196			double dx = gx - ip->spt[i][0];
197			double dy = gy - ip->spt[i][1];
198
199			setflg(visited, xfi, yfi);
200
201			if (dxdx + dydy > 2.*ip->grid2 && interp2_wti(ip, i, gx, gy) <= 1e-7)
202			return;
203
204			setflg(ip->da[i].infl, xfi, yfi);
205
206			if (xfi > 0 && !chkflg(visited, xfi-1, yfi))
207			influence_flood(ip, i, visited, xfi-1, yfi);
208
209			if (yfi > 0 && !chkflg(visited, xfi, yfi-1))
210			influence_flood(ip, i, visited, xfi, yfi-1);
211
212			if (xfi < NI2DIM-1 && !chkflg(visited, xfi+1, yfi))
213			influence_flood(ip, i, visited, xfi+1, yfi);
214
215			if (yfi < NI2DIM-1 && !chkflg(visited, xfi, yfi+1))
216			influence_flood(ip, i, visited, xfi, yfi+1);
217			}
218
219	greg	2.13	/* private call to compute sample influence maps */
220			static void
221			map_influence(INTERP2 *ip)
222			{
223			unsigned short visited[NI2DIM];
224			int fgi[2];
225			int i, j;
226
227			for (i = ip->ns; i--; ) {
228			for (j = NI2DIM; j--; ) {
229			ip->da[i].infl[j] = 0;
230			visited[j] = 0;
231			}
232			interp2_flagpos(fgi, ip, ip->spt[i][0], ip->spt[i][1]);
233
234			influence_flood(ip, i, visited, fgi[0], fgi[1]);
235			}
236			}
237
238			/* Modify smoothing parameter by the given factor */
239			void
240			interp2_smooth(INTERP2 *ip, double sf)
241			{
242			float old_smf = ip->smf;
243
244			if ((ip->smf *= sf) < NI2DSMF)
245			ip->smf = NI2DSMF;
246			/* need to recompute influence maps? */
247			if (ip->da != NULL && (old_smf*.85 > ip->smf) \|
248			(ip->smf > old_smf*1.15))
249			map_influence(ip);
250			}
251
252			/* private call-back to sort position index */
253			static int
254			cmp_spos(const void p1, const void p2)
255			{
256			const SAMPORD so1 = (const SAMPORD )p1;
257			const SAMPORD so2 = (const SAMPORD )p2;
258
259			if (so1->dm > so2->dm)
260			return 1;
261			if (so1->dm < so2->dm)
262			return -1;
263			return 0;
264			}
265
266			/* private routine to order samples in a particular direction */
267			static void
268			sort_samples(SAMPORD sord, const INTERP2 ip, double ang)
269			{
270			const double cosd = cos(ang);
271			const double sind = sin(ang);
272			int i;
273
274			for (i = ip->ns; i--; ) {
275			sord[i].si = i;
276			sord[i].dm = cosdip->spt[i][0] + sindip->spt[i][1];
277			}
278	greg	2.14	qsort(sord, ip->ns, sizeof(SAMPORD), cmp_spos);
279	greg	2.13	}
280
281	greg	2.2	/* (Re)compute anisotropic basis function interpolant (normally automatic) */
282			int
283			interp2_analyze(INTERP2 *ip)
284	greg	2.1	{
285			SAMPORD *sortord;
286	greg	2.2	int rightrndx, leftrndx, *endrndx;
287	greg	2.13	int i, bd;
288	greg	2.1	/* sanity checks */
289	greg	2.10	if (ip == NULL)
290			return(0);
291			if (ip->da != NULL) { /* free previous data if any */
292			free(ip->da);
293			ip->da = NULL;
294			}
295			if ((ip->ns <= 1) \| (ip->dmin <= 0))
296	greg	2.1	return(0);
297	greg	2.10	/* compute sample domain */
298			ip->smin[0] = ip->smin[1] = FHUGE;
299	greg	2.11	ip->smax[0] = ip->smax[1] = -FHUGE;
300	greg	2.10	for (i = ip->ns; i--; ) {
301	greg	2.12	if (ip->spt[i][0] < ip->smin[0]) ip->smin[0] = ip->spt[i][0];
302			if (ip->spt[i][0] > ip->smax[0]) ip->smax[0] = ip->spt[i][0];
303			if (ip->spt[i][1] < ip->smin[1]) ip->smin[1] = ip->spt[i][1];
304			if (ip->spt[i][1] > ip->smax[1]) ip->smax[1] = ip->spt[i][1];
305	greg	2.1	}
306	greg	2.11	ip->grid2 = ((ip->smax[0]-ip->smin[0])*(ip->smax[0]-ip->smin[0]) +
307			(ip->smax[1]-ip->smin[1])(ip->smax[1]-ip->smin[1]))
308			(1./NI2DIM/NI2DIM);
309	greg	2.10	if (ip->grid2 <= FTINYip->dminip->dmin)
310			return(0);
311			/* allocate analysis data */
312	greg	2.13	ip->da = (struct interp2_samp *)malloc(
313			sizeof(struct interp2_samp)*ip->ns );
314	greg	2.10	if (ip->da == NULL)
315			return(0);
316	greg	2.12	/* allocate temporary arrays */
317	greg	2.1	sortord = (SAMPORD )malloc(sizeof(SAMPORD)ip->ns);
318			rightrndx = (int )malloc(sizeof(int)ip->ns);
319			leftrndx = (int )malloc(sizeof(int)ip->ns);
320	greg	2.2	endrndx = (int )malloc(sizeof(int)ip->ns);
321			if ((sortord == NULL) \| (rightrndx == NULL) \|
322			(leftrndx == NULL) \| (endrndx == NULL))
323	greg	2.1	return(0);
324			/* run through bidirections */
325			for (bd = 0; bd < NI2DIR/2; bd++) {
326			const double ang = 2.PI/NI2DIRbd;
327	greg	2.2	int *sptr;
328	greg	2.1	/* create right reverse index */
329	greg	2.2	if (bd) { /* re-use from previous iteration? */
330			sptr = rightrndx;
331	greg	2.1	rightrndx = leftrndx;
332			leftrndx = sptr;
333	greg	2.2	} else { /* else sort first half-plane */
334			sort_samples(sortord, ip, PI/2. - PI/NI2DIR);
335			for (i = ip->ns; i--; )
336	greg	2.1	rightrndx[sortord[i].si] = i;
337	greg	2.2	/* & store reverse order for later */
338			for (i = ip->ns; i--; )
339			endrndx[sortord[i].si] = ip->ns-1 - i;
340	greg	2.1	}
341			/* create new left reverse index */
342	greg	2.2	if (bd == NI2DIR/2 - 1) { /* use order from first iteration? */
343			sptr = leftrndx;
344			leftrndx = endrndx;
345			endrndx = sptr;
346			} else { /* else compute new half-plane */
347			sort_samples(sortord, ip, ang + (PI/2. + PI/NI2DIR));
348			for (i = ip->ns; i--; )
349			leftrndx[sortord[i].si] = i;
350	greg	2.1	}
351			/* sort grid values in this direction */
352	greg	2.2	sort_samples(sortord, ip, ang);
353	greg	2.1	/* find nearest neighbors each side */
354	greg	2.2	for (i = ip->ns; i--; ) {
355	greg	2.3	const int ii = sortord[i].si;
356	greg	2.13	int j;
357	greg	2.3	/* preload with large radii */
358	greg	2.10	ip->da[ii].dia[bd] =
359			ip->da[ii].dia[bd+NI2DIR/2] = encode_diameter(ip,
360			.5*(sortord[ip->ns-1].dm - sortord[0].dm));
361	greg	2.1	for (j = i; ++j < ip->ns; ) /* nearest above */
362	greg	2.3	if (rightrndx[sortord[j].si] > rightrndx[ii] &&
363			leftrndx[sortord[j].si] < leftrndx[ii]) {
364	greg	2.10	ip->da[ii].dia[bd] = encode_diameter(ip,
365	greg	2.4	sortord[j].dm - sortord[i].dm);
366	greg	2.1	break;
367			}
368			for (j = i; j-- > 0; ) /* nearest below */
369	greg	2.3	if (rightrndx[sortord[j].si] < rightrndx[ii] &&
370			leftrndx[sortord[j].si] > leftrndx[ii]) {
371	greg	2.10	ip->da[ii].dia[bd+NI2DIR/2] = encode_diameter(ip,
372	greg	2.4	sortord[i].dm - sortord[j].dm);
373	greg	2.1	break;
374			}
375			}
376			}
377	greg	2.12	free(sortord); /* release temp arrays */
378	greg	2.1	free(rightrndx);
379			free(leftrndx);
380	greg	2.2	free(endrndx);
381	greg	2.13	/* map sample influence areas */
382			map_influence(ip);
383	greg	2.12	return(1); /* all done */
384	greg	2.11	}
385
386	greg	2.1	/* Assign full set of normalized weights to interpolate the given position */
387			int
388			interp2_weights(float wtv[], INTERP2 *ip, double x, double y)
389			{
390			double wnorm;
391	greg	2.11	int fgi[2];
392	greg	2.1	int i;
393
394	greg	2.11	if (wtv == NULL)
395			return(0);
396			/* get flag position */
397	greg	2.12	if (interp2_flagpos(fgi, ip, x, y) < 0)
398	greg	2.1	return(0);
399
400			wnorm = 0; /* compute raw weights */
401	greg	2.11	for (i = ip->ns; i--; )
402	greg	2.12	if (chkflg(ip->da[i].infl, fgi[0], fgi[1])) {
403	greg	2.10	double wt = interp2_wti(ip, i, x, y);
404	greg	2.1	wtv[i] = wt;
405			wnorm += wt;
406	greg	2.12	} else
407			wtv[i] = 0;
408	greg	2.1	if (wnorm <= 0) /* too far from all our samples! */
409			return(0);
410			wnorm = 1./wnorm; /* normalize weights */
411			for (i = ip->ns; i--; )
412			wtv[i] *= wnorm;
413			return(ip->ns); /* all done */
414			}
415
416
417			/* Get normalized weights and indexes for n best samples in descending order */
418			int
419			interp2_topsamp(float wt[], int si[], const int n, INTERP2 *ip, double x, double y)
420			{
421			int nn = 0;
422	greg	2.11	int fgi[2];
423	greg	2.1	double wnorm;
424			int i, j;
425
426	greg	2.11	if ((n <= 0) \| (wt == NULL) \| (si == NULL))
427			return(0);
428			/* get flag position */
429	greg	2.12	if (interp2_flagpos(fgi, ip, x, y) < 0)
430	greg	2.1	return(0);
431			/* identify top n weights */
432	greg	2.11	for (i = ip->ns; i--; )
433	greg	2.12	if (chkflg(ip->da[i].infl, fgi[0], fgi[1])) {
434	greg	2.10	const double wti = interp2_wti(ip, i, x, y);
435	greg	2.1	for (j = nn; j > 0; j--) {
436	greg	2.3	if (wt[j-1] >= wti)
437	greg	2.1	break;
438			if (j < n) {
439			wt[j] = wt[j-1];
440			si[j] = si[j-1];
441			}
442			}
443			if (j < n) { /* add/insert sample */
444	greg	2.3	wt[j] = wti;
445	greg	2.1	si[j] = i;
446			nn += (nn < n);
447			}
448	greg	2.11	}
449	greg	2.3	wnorm = 0; /* normalize sample weights */
450			for (j = nn; j--; )
451			wnorm += wt[j];
452	greg	2.1	if (wnorm <= 0)
453			return(0);
454			wnorm = 1./wnorm;
455			for (j = nn; j--; )
456			wt[j] *= wnorm;
457			return(nn); /* return actual # samples */
458			}
459
460			/* Free interpolant */
461			void
462			interp2_free(INTERP2 *ip)
463			{
464			if (ip == NULL)
465			return;
466	greg	2.4	if (ip->da != NULL)
467			free(ip->da);
468	greg	2.1	free(ip);
469			}