src/common/interp2d.c

#ifndef lint
static const char RCSid[] = "$Id: interp2d.c,v 2.1 2013/02/09 00:55:40 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 3 half-planes and
 * perform simple tests to see which neighbor is closest in
 * a each direction.  Once we have our approximate neighborhood
 * for a sample, we can use it in a Gaussian weighting scheme
 * with anisotropic surround.  This gives us a fairly smooth
 * interpolation however the sample points may be initially
 * distributed.  Evaluation is accelerated by use of a fast
 * approximation to the atan2(y,x) function.
 **************************************************************/

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

#define DECODE_RAD(ip,er)       ((ip)->rmin*(1. + .5*(er)))
#define ENCODE_RAD(ip,r)        ((int)(2.*(r)/(ip)->rmin) - 2)

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

/* 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->rmin = .5;         /* default radius minimum */
        nip->smf = NI2DSMF;     /* default smoothing factor */
        nip->ra = NULL;
                                /* caller must assign spt[] array */
        return(nip);
}

/* Resize interpolation array (caller must assign any new values) */
INTERP2 *
interp2_realloc(INTERP2 *ip, int nsamps)
{
        if (ip == NULL)
                return(interp2_alloc(nsamps));
        if (nsamps <= 1) {
                interp2_free(ip);
                return(NULL);
        }
        if (nsamps == ip->ns);
                return(ip);
        if (ip->ra != NULL) {   /* will need to recompute distribution */
                free(ip->ra);
                ip->ra = NULL;
        }
        ip = (INTERP2 *)realloc(ip, sizeof(INTERP2)+sizeof(float)*2*(nsamps-1));
        if (ip == NULL)
                return(NULL);
        ip->ns = nsamps;
        return(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);
}

/* private routine to encode radius with range checks */
static int
encode_radius(const INTERP2 *ip, double r)
{
        const int       er = ENCODE_RAD(ip, r);

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

/* (Re)compute anisotropic basis function interpolant (normally automatic) */
int
interp2_analyze(INTERP2 *ip)
{
        SAMPORD *sortord;
        int     *rightrndx, *leftrndx, *endrndx;
        int     bd;
                                        /* sanity checks */
        if (ip == NULL || (ip->ns <= 1) | (ip->rmin <= 0))
                return(0);
                                        /* need to allocate? */
        if (ip->ra == NULL) {
                ip->ra = (unsigned short (*)[NI2DIR])malloc(
                                sizeof(unsigned short)*NI2DIR*ip->ns);
                if (ip->ra == NULL)
                        return(0);
        }
                                        /* get 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;
            int                 i;
                                        /* 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       rpos = rightrndx[sortord[i].si];
                const int       lpos = leftrndx[sortord[i].si];
                int             j;
                                        /* preload with large radius */
                ip->ra[i][bd] = ip->ra[i][bd+NI2DIR/2] = encode_radius(ip,
                            .25*(sortord[ip->ns-1].dm - sortord[0].dm));
                for (j = i; ++j < ip->ns; )     /* nearest above */
                    if (rightrndx[sortord[j].si] > rpos &&
                                    leftrndx[sortord[j].si] < lpos) {
                        ip->ra[i][bd] = encode_radius(ip,
                                        .5*(sortord[j].dm - sortord[i].dm));
                        break;
                    }
                for (j = i; j-- > 0; )          /* nearest below */
                    if (rightrndx[sortord[j].si] < rpos &&
                                    leftrndx[sortord[j].si] > lpos) {
                        ip->ra[i][bd+NI2DIR/2] = encode_radius(ip,
                                        .5*(sortord[i].dm - sortord[j].dm));
                        break;
                    }
            }
        }
        free(sortord);                  /* clean up */
        free(rightrndx);
        free(leftrndx);
        free(endrndx);
        return(1);
}

/* private call returns log of raw weight for a particular sample */
static double
get_ln_wt(const INTERP2 *ip, const int i, double x, double y)
{
        double  dir, rd;
        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->ra[i][ri] + rd*ip->ra[i][(ri+1)%NI2DIR];
        rd = ip->smf * DECODE_RAD(ip, rd);
                                /* return log of Gaussian weight */
        return( (x*x + y*y) / (-2.*rd*rd) );
}

/* 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     i;

        if ((wtv == NULL) | (ip == NULL))
                return(0);
                                        /* need to compute interpolant? */
        if (ip->ra == NULL && !interp2_analyze(ip))
                return(0);

        wnorm = 0;                      /* compute raw weights */
        for (i = ip->ns; i--; ) {
                double  wt = get_ln_wt(ip, i, x, y);
                if (wt < -21.) {
                        wtv[i] = 0;     /* ignore weights < 1e-9 */
                        continue;
                }
                wt = exp(wt);           /* Gaussian weight */
                wtv[i] = wt;
                wnorm += wt;
        }
        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;
        double  wnorm;
        int     i, j;

        if ((n <= 0) | (wt == NULL) | (si == NULL) | (ip == NULL))
                return(0);
                                        /* need to compute interpolant? */
        if (ip->ra == NULL && !interp2_analyze(ip))
                return(0);
                                        /* identify top n weights */
        for (i = ip->ns; i--; ) {
                const double    lnwt = get_ln_wt(ip, i, x, y);
                for (j = nn; j > 0; j--) {
                        if (wt[j-1] >= lnwt)
                                break;
                        if (j < n) {
                                wt[j] = wt[j-1];
                                si[j] = si[j-1];
                        }
                }
                if (j < n) {            /* add/insert sample */
                        wt[j] = lnwt;
                        si[j] = i;
                        nn += (nn < n);
                }
        }
        wnorm = 0;                      /* exponentiate and normalize */
        for (j = nn; j--; ) {
                double  dwt = exp(wt[j]);
                wt[j] = dwt;
                wnorm += dwt;
        }
        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->ra != NULL)
                free(ip->ra);
        free(ip);
}
Revision:	2.2
Committed:	Sat Feb 9 17:39:21 2013 UTC (11 years, 3 months ago) by greg
Content type:	text/plain
Branch:	MAIN
Changes since 2.1:	+69 -37 lines
Log Message:	Added interp2_realloc() call, exposed interp2_analyze() and code improvements
#	User	Rev	Content
1	greg	2.1	#ifndef lint
2	greg	2.2	static const char RCSid[] = "$Id: interp2d.c,v 2.1 2013/02/09 00:55:40 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			/*************************************************************
13			* 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			* calculation, we sort the data into 3 half-planes and
19			* perform simple tests to see which neighbor is closest in
20			* a each direction. Once we have our approximate neighborhood
21			* for a sample, we can use it in a Gaussian weighting scheme
22			* with anisotropic surround. This gives us a fairly smooth
23			* interpolation however the sample points may be initially
24			* distributed. Evaluation is accelerated by use of a fast
25			* approximation to the atan2(y,x) function.
26			**************************************************************/
27
28			#include <stdio.h>
29			#include <stdlib.h>
30			#include "rtmath.h"
31			#include "interp2d.h"
32
33			#define DECODE_RAD(ip,er) ((ip)->rmin(1. + .5(er)))
34			#define ENCODE_RAD(ip,r) ((int)(2.*(r)/(ip)->rmin) - 2)
35
36			/* Sample order (private) */
37			typedef struct {
38			int si; /* sample index */
39			float dm; /* distance measure in this direction */
40			} SAMPORD;
41
42	greg	2.2	/* Allocate a new set of interpolation samples (caller assigns spt[] array) */
43	greg	2.1	INTERP2 *
44			interp2_alloc(int nsamps)
45			{
46			INTERP2 *nip;
47
48			if (nsamps <= 1)
49			return(NULL);
50
51			nip = (INTERP2 )malloc(sizeof(INTERP2) + sizeof(float)2*(nsamps-1));
52			if (nip == NULL)
53			return(NULL);
54
55			nip->ns = nsamps;
56			nip->rmin = .5; /* default radius minimum */
57			nip->smf = NI2DSMF; /* default smoothing factor */
58			nip->ra = NULL;
59			/* caller must assign spt[] array */
60			return(nip);
61			}
62
63	greg	2.2	/* Resize interpolation array (caller must assign any new values) */
64			INTERP2 *
65			interp2_realloc(INTERP2 *ip, int nsamps)
66			{
67			if (ip == NULL)
68			return(interp2_alloc(nsamps));
69			if (nsamps <= 1) {
70			interp2_free(ip);
71			return(NULL);
72			}
73			if (nsamps == ip->ns);
74			return(ip);
75			if (ip->ra != NULL) { /* will need to recompute distribution */
76			free(ip->ra);
77			ip->ra = NULL;
78			}
79			ip = (INTERP2 )realloc(ip, sizeof(INTERP2)+sizeof(float)2*(nsamps-1));
80			if (ip == NULL)
81			return(NULL);
82			ip->ns = nsamps;
83			return(ip);
84			}
85
86	greg	2.1	/* private call-back to sort position index */
87			static int
88			cmp_spos(const void p1, const void p2)
89			{
90			const SAMPORD so1 = (const SAMPORD )p1;
91			const SAMPORD so2 = (const SAMPORD )p2;
92
93			if (so1->dm > so2->dm)
94			return 1;
95			if (so1->dm < so2->dm)
96			return -1;
97			return 0;
98			}
99
100	greg	2.2	/* private routine to order samples in a particular direction */
101			static void
102			sort_samples(SAMPORD sord, const INTERP2 ip, double ang)
103			{
104			const double cosd = cos(ang);
105			const double sind = sin(ang);
106			int i;
107
108			for (i = ip->ns; i--; ) {
109			sord[i].si = i;
110			sord[i].dm = cosdip->spt[i][0] + sindip->spt[i][1];
111			}
112			qsort(sord, ip->ns, sizeof(SAMPORD), &cmp_spos);
113			}
114
115	greg	2.1	/* private routine to encode radius with range checks */
116			static int
117			encode_radius(const INTERP2 *ip, double r)
118			{
119			const int er = ENCODE_RAD(ip, r);
120
121			if (er <= 0)
122			return(0);
123			if (er >= 0xffff)
124			return(0xffff);
125			return(er);
126			}
127
128	greg	2.2	/* (Re)compute anisotropic basis function interpolant (normally automatic) */
129			int
130			interp2_analyze(INTERP2 *ip)
131	greg	2.1	{
132			SAMPORD *sortord;
133	greg	2.2	int rightrndx, leftrndx, *endrndx;
134	greg	2.1	int bd;
135			/* sanity checks */
136			if (ip == NULL \|\| (ip->ns <= 1) \| (ip->rmin <= 0))
137			return(0);
138			/* need to allocate? */
139			if (ip->ra == NULL) {
140			ip->ra = (unsigned short (*)[NI2DIR])malloc(
141			sizeof(unsigned short)NI2DIRip->ns);
142			if (ip->ra == NULL)
143			return(0);
144			}
145			/* get temporary arrays */
146			sortord = (SAMPORD )malloc(sizeof(SAMPORD)ip->ns);
147			rightrndx = (int )malloc(sizeof(int)ip->ns);
148			leftrndx = (int )malloc(sizeof(int)ip->ns);
149	greg	2.2	endrndx = (int )malloc(sizeof(int)ip->ns);
150			if ((sortord == NULL) \| (rightrndx == NULL) \|
151			(leftrndx == NULL) \| (endrndx == NULL))
152	greg	2.1	return(0);
153			/* run through bidirections */
154			for (bd = 0; bd < NI2DIR/2; bd++) {
155			const double ang = 2.PI/NI2DIRbd;
156	greg	2.2	int *sptr;
157	greg	2.1	int i;
158			/* create right reverse index */
159	greg	2.2	if (bd) { /* re-use from previous iteration? */
160			sptr = rightrndx;
161	greg	2.1	rightrndx = leftrndx;
162			leftrndx = sptr;
163	greg	2.2	} else { /* else sort first half-plane */
164			sort_samples(sortord, ip, PI/2. - PI/NI2DIR);
165			for (i = ip->ns; i--; )
166	greg	2.1	rightrndx[sortord[i].si] = i;
167	greg	2.2	/* & store reverse order for later */
168			for (i = ip->ns; i--; )
169			endrndx[sortord[i].si] = ip->ns-1 - i;
170	greg	2.1	}
171			/* create new left reverse index */
172	greg	2.2	if (bd == NI2DIR/2 - 1) { /* use order from first iteration? */
173			sptr = leftrndx;
174			leftrndx = endrndx;
175			endrndx = sptr;
176			} else { /* else compute new half-plane */
177			sort_samples(sortord, ip, ang + (PI/2. + PI/NI2DIR));
178			for (i = ip->ns; i--; )
179			leftrndx[sortord[i].si] = i;
180	greg	2.1	}
181			/* sort grid values in this direction */
182	greg	2.2	sort_samples(sortord, ip, ang);
183	greg	2.1	/* find nearest neighbors each side */
184	greg	2.2	for (i = ip->ns; i--; ) {
185	greg	2.1	const int rpos = rightrndx[sortord[i].si];
186			const int lpos = leftrndx[sortord[i].si];
187			int j;
188			/* preload with large radius */
189			ip->ra[i][bd] = ip->ra[i][bd+NI2DIR/2] = encode_radius(ip,
190			.25*(sortord[ip->ns-1].dm - sortord[0].dm));
191			for (j = i; ++j < ip->ns; ) /* nearest above */
192			if (rightrndx[sortord[j].si] > rpos &&
193			leftrndx[sortord[j].si] < lpos) {
194			ip->ra[i][bd] = encode_radius(ip,
195			.5*(sortord[j].dm - sortord[i].dm));
196			break;
197			}
198			for (j = i; j-- > 0; ) /* nearest below */
199			if (rightrndx[sortord[j].si] < rpos &&
200			leftrndx[sortord[j].si] > lpos) {
201			ip->ra[i][bd+NI2DIR/2] = encode_radius(ip,
202			.5*(sortord[i].dm - sortord[j].dm));
203			break;
204			}
205			}
206			}
207			free(sortord); /* clean up */
208			free(rightrndx);
209			free(leftrndx);
210	greg	2.2	free(endrndx);
211	greg	2.1	return(1);
212			}
213
214			/* private call returns log of raw weight for a particular sample */
215			static double
216			get_ln_wt(const INTERP2 *ip, const int i, double x, double y)
217			{
218			double dir, rd;
219			int ri;
220			/* get relative direction */
221			x -= ip->spt[i][0];
222			y -= ip->spt[i][1];
223			dir = atan2a(y, x);
224			dir += 2.PI(dir < 0);
225			/* linear radius interpolation */
226			rd = dir * (NI2DIR/2./PI);
227			ri = (int)rd;
228			rd -= (double)ri;
229			rd = (1.-rd)ip->ra[i][ri] + rdip->ra[i][(ri+1)%NI2DIR];
230			rd = ip->smf * DECODE_RAD(ip, rd);
231			/* return log of Gaussian weight */
232			return( (xx + yy) / (-2.rdrd) );
233			}
234
235			/* Assign full set of normalized weights to interpolate the given position */
236			int
237			interp2_weights(float wtv[], INTERP2 *ip, double x, double y)
238			{
239			double wnorm;
240			int i;
241
242			if ((wtv == NULL) \| (ip == NULL))
243			return(0);
244			/* need to compute interpolant? */
245	greg	2.2	if (ip->ra == NULL && !interp2_analyze(ip))
246	greg	2.1	return(0);
247
248			wnorm = 0; /* compute raw weights */
249			for (i = ip->ns; i--; ) {
250			double wt = get_ln_wt(ip, i, x, y);
251			if (wt < -21.) {
252			wtv[i] = 0; /* ignore weights < 1e-9 */
253			continue;
254			}
255			wt = exp(wt); /* Gaussian weight */
256			wtv[i] = wt;
257			wnorm += wt;
258			}
259			if (wnorm <= 0) /* too far from all our samples! */
260			return(0);
261			wnorm = 1./wnorm; /* normalize weights */
262			for (i = ip->ns; i--; )
263			wtv[i] *= wnorm;
264			return(ip->ns); /* all done */
265			}
266
267
268			/* Get normalized weights and indexes for n best samples in descending order */
269			int
270			interp2_topsamp(float wt[], int si[], const int n, INTERP2 *ip, double x, double y)
271			{
272			int nn = 0;
273			double wnorm;
274			int i, j;
275
276			if ((n <= 0) \| (wt == NULL) \| (si == NULL) \| (ip == NULL))
277			return(0);
278			/* need to compute interpolant? */
279	greg	2.2	if (ip->ra == NULL && !interp2_analyze(ip))
280	greg	2.1	return(0);
281			/* identify top n weights */
282			for (i = ip->ns; i--; ) {
283			const double lnwt = get_ln_wt(ip, i, x, y);
284			for (j = nn; j > 0; j--) {
285			if (wt[j-1] >= lnwt)
286			break;
287			if (j < n) {
288			wt[j] = wt[j-1];
289			si[j] = si[j-1];
290			}
291			}
292			if (j < n) { /* add/insert sample */
293			wt[j] = lnwt;
294			si[j] = i;
295			nn += (nn < n);
296			}
297			}
298			wnorm = 0; /* exponentiate and normalize */
299			for (j = nn; j--; ) {
300			double dwt = exp(wt[j]);
301			wt[j] = dwt;
302			wnorm += dwt;
303			}
304			if (wnorm <= 0)
305			return(0);
306			wnorm = 1./wnorm;
307			for (j = nn; j--; )
308			wt[j] *= wnorm;
309			return(nn); /* return actual # samples */
310			}
311
312			/* Free interpolant */
313			void
314			interp2_free(INTERP2 *ip)
315			{
316			if (ip == NULL)
317			return;
318			if (ip->ra != NULL)
319			free(ip->ra);
320			free(ip);
321			}