summaryrefslogtreecommitdiff
path: root/rspl/stest.c
diff options
context:
space:
mode:
Diffstat (limited to 'rspl/stest.c')
-rw-r--r--rspl/stest.c654
1 files changed, 654 insertions, 0 deletions
diff --git a/rspl/stest.c b/rspl/stest.c
new file mode 100644
index 0000000..d1cf2da
--- /dev/null
+++ b/rspl/stest.c
@@ -0,0 +1,654 @@
+
+/*
+ * Scattered Data Interpolation
+ * with multilevel B-splines
+ * research.
+ *
+ * This is from the paper
+ * "Scattered Data Interpolation with Multilevel B-Splines"
+ * by Seungyong Lee, George Wolberg and Sung Yong Shin,
+ */
+
+/*
+ * Author: Graeme W. Gill
+ * Date: 2001/1/1
+ *
+ * Copyright 2000 - 2001 Graeme W. Gill
+ * All rights reserved.
+ *
+ * This material is licenced under the GNU AFFERO GENERAL PUBLIC LICENSE Version 3 :-
+ * see the License.txt file for licencing details.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <fcntl.h>
+#include <string.h>
+#include <math.h>
+#include "../numeric/numlib.h"
+
+
+#ifndef NUMSUP_H
+void error(char *fmt, ...), warning(char *fmt, ...);
+#endif
+
+#define MXDI 4
+#define MXDO 4
+
+/* A control point */
+typedef struct {
+ double p[MXDI];
+ double v[MXDO];
+} co;
+
+/* Neighborhood latice cache data */
+typedef struct {
+ int c[MXDI]; /* Coordinate */
+ int xo; /* Offset into srbs latice */
+ double w; /* B-spline basis weight */
+} neigh;
+
+/* Structure that represents a resolution level of B-splines */
+struct _srbs {
+ struct _mrbs *p; /* Parent structure */
+ int res; /* Basic resolution */
+ int coi[MXDI]; /* Double increment for each input dimension into latice */
+ double *lat; /* Control latice, extending from +/- 1 from 0..res-1 */
+ double *_lat; /* Allocation base of lat */
+ int lsize, loff; /* Number of doubles in _lat, offset of lat from _lat */
+ double w[MXDI]; /* Input data cell width */
+ neigh *n; /* Neighborhood latice cache */
+ int nsize; /* Number of n entries */
+}; typedef struct _srbs srbs;
+
+/* Structure that represents the whole scattered interpolation state */
+struct _mrbs {
+ int di; /* Input dimensions */
+ int fdi; /* Output dimensions */
+ int tres; /* Target resolution */
+ double smf; /* Smoothing factor */
+ int npts; /* Number of data points */
+ co *pts; /* Coordinate points */
+ double l[MXDI], h[MXDI]; /* Input data range, cell width */
+ srbs *s; /* Current B-spline latice */
+}; typedef struct _mrbs mrbs;
+
+static void delete_srbs(srbs *s);
+
+/* Create a new empty mrbs */
+static mrbs *new_mrbs(
+int di, /* Input dimensionality */
+int fdi, /* Output dimesionality */
+int res, /* Target resolution */
+double smf /* Smoothing factor */
+) {
+ mrbs *p;
+ if ((p = (mrbs *)malloc(sizeof(mrbs))) == NULL)
+ error("Malloc mrbs failed");
+
+ p->di = di;
+ p->fdi = fdi;
+ p->tres = res;
+ p->smf = smf;
+ p->s = NULL;
+
+ return p;
+}
+
+static void delete_mrbs(mrbs *p) {
+
+ if (p != NULL) {
+ delete_srbs(p->s);
+ free(p);
+ }
+}
+
+/* Create a new empty srbs */
+static srbs *new_srbs(
+mrbs *p, /* Parent mrbs */
+int res /* Resolution of this srbs */
+) {
+ srbs *s;
+ int e, f;
+ double *_lat, *lat; /* Latice base address */
+ int ix, oe, oo[MXDI]; /* Neighborhood offset index, counter */
+
+ if ((s = (srbs *)malloc(sizeof(srbs))) == NULL)
+ error("Malloc srbs failed");
+
+ s->p = p;
+ s->res = res;
+
+ for (s->lsize = p->fdi, s->nsize = 1, e = 0; e < p->di; e++) {
+ s->coi[e] = s->lsize; /* (double) increment in this input dimension */
+ s->lsize *= (res + 2); /* Latice in 1D +/- 1 */
+ s->nsize *= 4; /* Neighborhood of 4 */
+ }
+
+ if ((s->_lat = (double *)malloc(s->lsize * sizeof(double))) == NULL)
+ error("Malloc srbs latice failed");
+
+ /* Compute the base address */
+ for (s->loff = 0, e = 0; e < p->di; e++) {
+ s->loff += s->coi[e]; /* Offset by 1 in each input dimension */
+ }
+ s->lat = s->_lat + s->loff;
+
+ /* Figure the cell width */
+ for (e = 0; e < p->di; e++)
+ s->w[e] = (p->h[e] - p->l[e])/(res-1.0);
+
+ /* Setup neighborhood cache info */
+ if ((s->n = (neigh *)malloc(s->nsize * sizeof(neigh))) == NULL)
+ error("Malloc srbs neighborhood failed");
+
+ for (oe = 0; oe < p->di; oe++)
+ oo[oe] = 0;
+
+ for(ix = oe = 0; oe < p->di; ix++) {
+ int xo;
+ for (xo = e = 0; e < p->di; e++) {
+ s->n[ix].c[e] = oo[e];
+ xo += s->coi[e] * oo[e]; /* Accumulate latice offset */
+ }
+ s->n[ix].xo = xo;
+ s->n[ix].w = 0.0;
+
+ /* Increment destination offset counter */
+ for (oe = 0; oe < p->di; oe++) {
+ if (++oo[oe] <= 3) /* Counting from 0 ... 3 */
+ break;
+ oo[oe] = 0;
+ }
+ }
+
+ return s;
+}
+
+/* Destroy a srbs */
+static void delete_srbs(srbs *s) {
+ if (s != NULL) {
+ free(s->_lat);
+ free(s->n);
+ free(s);
+ }
+}
+
+/* Dump the 2D -> 1D contents of an srbs */
+static void dump_srbs(srbs *s) {
+ int e, f;
+ int ce, co[MXDI]; /* latice counter */
+ mrbs *p = s->p; /* Parent object */
+
+ /* Init the counter */
+ for (ce = 0; ce < p->di; ce++)
+ co[ce] = -1;
+ ce = 0;
+
+ f = 0;
+ while(ce < p->di) {
+ double v;
+ int off = 0; /* Latice offset */
+ for (e = 0; e < p->di; e++) {
+ off += co[e] * s->coi[e]; /* Accumulate latice offset */
+ }
+ v = s->lat[off + f]; /* Value of this latice point */
+
+ printf("Latice at [%d][%d] = %f\n",co[1],co[0],v);
+
+ /* Increment the latice counter */
+ for (ce = 0; ce < p->di; ce++) {
+ if (++co[ce] <= s->res) /* Counting from -1 ... s->res */
+ break;
+ co[ce] = -1;
+ }
+ }
+}
+
+/* Initialise an srbs with a linear approximation to the scattered data */
+static void linear_srbs(
+srbs *s
+) {
+ int i, e, f;
+ mrbs *p = s->p; /* Parent object */
+ double **A; /* A matrix holding scattered data points */
+ double *B; /* B matrix holding RHS & solution */
+
+ /* Allocate the matricies */
+ B = dvector(0, p->npts-1);
+ A = dmatrix(0, p->npts-1, 0, p->di);
+
+ /* For each output dimension, solve the linear equation coeficients */
+ for (f = 0; f < p->fdi; f++) {
+ int ce, co[MXDI]; /* latice counter */
+
+ /* Init A[][] with the scattered data points positions */
+ /* Also init B[] with the value for this output dimension */
+ for (i = 0; i < p->npts; i++) {
+ for (e = 0; e < p->di; e++)
+ A[i][e] = p->pts[i].p[e];
+ A[i][e] = 1.0;
+ B[i] = p->pts[i].v[f];
+ }
+
+ /* Solve the equation A.x = b using SVD */
+ /* (The w[] values are thresholded for best accuracy) */
+ /* Return non-zero if no solution found */
+ if (svdsolve(A, B, p->npts, p->di+1) != 0)
+ error("SVD least squares failed");
+ /* A[][] will have been changed, and B[] holds the p->di+1 coefficients */
+
+ /* Use the coefficients to initialise the srbs values */
+ for (ce = 0; ce < p->di; ce++)
+ co[ce] = -1;
+ ce = 0;
+
+ while(ce < p->di) {
+ double v = B[p->di]; /* Constant */
+ int off = 0; /* Latice offset */
+ for (e = 0; e < p->di; e++) {
+ double lv;
+ lv = p->l[e] + s->w[e] * co[e]; /* Input value for this latice location */
+ v += B[e] * lv;
+ off += co[e] * s->coi[e]; /* Accumulate latice offset */
+ }
+ s->lat[off + f] = v; /* Value of this latice point */
+
+ /* Increment the latice counter */
+ for (ce = 0; ce < p->di; ce++) {
+ if (++co[ce] <= s->res) /* Counting from -1 ... s->res */
+ break;
+ co[ce] = -1;
+ }
+ }
+ }
+ free_dmatrix(A, 0, p->npts-1, 0, p->di);
+ free_dvector(B, 0, p->npts-1);
+}
+
+/* Do a latice refinement - upsample the current */
+/* source latice to the destination latice. */
+static void refine_srbs(
+srbs *ds, /* Destination srbs */
+srbs *ss /* Source srbs */
+) {
+ mrbs *p = ss->p; /* Parent object */
+ int ce, co[MXDI]; /* Source coordinate counter */
+ int six; /* Source index */
+ int dix; /* destination index */
+ static double _wt[5] = { 1.0/8.0, 4.0/8.0, 6.0/8.0, 4.0/8.0, 1.0/8.0 };
+ static double *wt = &_wt[2]; /* 1D Distribution weighting */
+
+ /* Zero the destination latice before accumulating values */
+ for (dix = 0; dix < ds->lsize; dix++)
+ ds->_lat[dix] = 0.0;
+
+ /* Now for each source latice entry, add weighted portions */
+ /* to the associated destination points */
+
+ /* Init the source coordinate counter */
+ for (ce = 0; ce < p->di; ce++)
+ co[ce] = -1;
+ ce = 0;
+ six = -ss->loff;
+
+ while(ce < p->di) {
+ int oe, oo[MXDI]; /* Destination offset counter */
+
+//printf("Source coord %d %d, offset %d, value %f\n",co[0], co[1], six, ss->lat[six]);
+ /* calc destination index, and init offest counter */
+ for (dix = oe = 0; oe < p->di; oe++) {
+ oo[oe] = -2;
+ dix += co[oe] * 2 * ds->coi[oe]; /* Accumulate dest offset */
+ }
+ oe = 0;
+
+//printf("Dest coord %d %d\n",co[0] * 2, co[1] * 2);
+ /* For all the offsets from the destination point */
+ while(oe < p->di) {
+ int e, f, dixo; /* Destination index offset */
+ double w = 1.0; /* Weighting */
+
+//printf("dest offset %d %d\n",oo[0], oo[1]);
+ /* Compute dest index offset, and check that we are not outside the destination */
+ for (dixo = e = 0; e < p->di; e++) {
+ int x = co[e] * 2 + oo[e]; /* dest coord */
+ dixo += oo[e] * ds->coi[e]; /* Accumulate dest offset */
+//printf("x[%d] = %d\n",e, x);
+ w *= wt[oo[e]]; /* Compute distribution weighting */
+ if (x < -1 || x > ds->res)
+ break; /* No good */
+ }
+ if (e >= p->di) { /* We are within the destination latice */
+//if ((co[0] * 2 + oo[0]) == 0 && (co[1] * 2 + oo[1]) == 0) {
+//printf("Source coord %d %d, offset %d, value %f\n",co[0], co[1], six, ss->lat[six]);
+//printf("Dest coord %d %d ix %d, weight %f\n",co[0] * 2 + oo[0], co[1] * 2 + oo[1], dix+dixo, w);
+//}
+
+ for (f = 0; f < p->fdi; f++) { /* Distribute weighted values */
+ double v = ss->lat[six + f];
+//if ((co[0] * 2 + oo[0]) == 0 && (co[1] * 2 + oo[1]) == 0)
+//printf("Value being dist %f, weighted value %f\n", v, v * w);
+ ds->lat[dix + dixo + f] += v * w;
+ }
+ }
+
+ /* Increment destination offset counter */
+ for (oe = 0; oe < p->di; oe++) {
+ if (++oo[oe] <= 2) /* Counting from -2 ... +2 */
+ break;
+ oo[oe] = -2;
+ }
+ }
+
+ /* Increment the source index and coordinat counter */
+ six += p->fdi;
+ for (ce = 0; ce < p->di; ce++) {
+ if (++co[ce] <= ss->res) /* Counting from -1 ... ss->res */
+ break;
+ co[ce] = -1;
+ }
+ }
+}
+
+/* Compute the Cubic B-spline weightings for a given t */
+void basis(double b[4], double t) {
+ double _t3, _t2, _t1, _3t3, _3t2, _3t1, _6t2;
+
+ _t1 = t/6.0;
+ _t2 = _t1 * _t1;
+ _t3 = _t2 * _t1;
+ _3t1 = 3.0 * _t1;
+ _3t2 = 3.0 * _t2;
+ _3t3 = 3.0 * _t3;
+ _6t2 = 6.0 * _t2;
+
+ b[0] = - _t3 + _3t2 - _3t1 + 1.0/6.0;
+ b[1] = _3t3 - _6t2 + 4.0/6.0;
+ b[2] = -_3t3 + _3t2 + _3t1 + 1.0/6.0;
+ b[3] = _t3;
+}
+
+
+/* Improve an srbs to make it closer to the scattered data */
+static void improve_srbs(
+srbs *s
+) {
+ int i, e, f;
+ mrbs *p = s->p; /* Parent object */
+ double *delta; /* Delta accumulation */
+ double *omega; /* Omega accumulation */
+
+ /* Allocate temporary accumulation arrays */
+ if ((delta = (double *)calloc(sizeof(double), s->lsize)) == NULL)
+ error("Malloc srbs temp latice failed");
+ delta += s->loff;
+ if ((omega = (double *)calloc(sizeof(double), s->lsize)) == NULL)
+ error("Malloc srbs temp latice failed");
+ omega += s->loff;
+
+ /* For each scattered data point */
+ for (i = 0; i < p->npts; i++) {
+ int ix; /* Latice index of base of neighborhood */
+ double b[MXDI][4]; /* B-spline basis factors for each dimension */
+ double sws; /* Sum of all the basis factors squared */
+ double ve[MXDO]; /* Current output value error */
+ int nn; /* Neighbor counter */
+
+ /* Figure out our neighborhood */
+ for (ix = e = 0; e < p->di; e++) {
+ int x;
+ double t, sp, fp;
+ sp = (p->pts[i].p[e] - p->l[e])/s->w[e]; /* Scaled position */
+ fp = floor(sp);
+ x = (int)(fp - 1.0); /* Grid coordinate */
+ ix += s->coi[e] * x; /* Accume latice offset */
+ t = sp - fp; /* Spline parameter */
+ basis(b[e], t); /* Compute basis function values */
+ }
+
+ /* Compute the grid basis weight functions, */
+ /* the sum of the weights squared, and the current */
+ /* output value estimate. */
+ for (f = 0; f < p->fdi; f++)
+ ve[f] = p->pts[i].v[f]; /* Target output value */
+ for (sws = 0.0, nn = 0; nn < s->nsize; nn++) {
+ double w;
+ for (w = 1.0, e = 0; e < p->di; e++)
+ w *= b[e][s->n[nn].c[e]];
+ s->n[nn].w = w; /* cache weighting */
+ sws += w * w;
+ for (f = 0; f < p->fdi; f++)
+ ve[f] -= w * s->lat[ix + s->n[nn].xo + f]; /* Subtract current aprox value */
+ }
+printf("Error at point %d = %f\n",i,ve[0]);
+
+ /* Accumulate the delta and omega factors */
+ /* for this resolutions improvement. */
+ for (nn = 0; nn < s->nsize; nn++) {
+ double ws, ww, w = s->n[nn].w;
+ int xo = ix + s->n[nn].xo; /* Latice offset */
+ ww = w * w;
+ ws = ww * w/sws; /* Scale factor for delta */
+ omega[xo] += ww; /* Accumulate omega */
+ for (f = 0; f < p->fdi; f++)
+ delta[xo + f] += ws * ve[f]; /* Accumulate delta */
+printf("Distributing omega %f to %d %d\n",ww,s->n[nn].c[0],s->n[nn].c[1]);
+printf("Distributing delta %f to %d %d\n",ws * ve[0],s->n[nn].c[0],s->n[nn].c[1]);
+ }
+ }
+
+ omega -= s->loff; /* Base them back to -1 corner */
+ delta -= s->loff;
+
+ /* Go through the delta and omega arrays, */
+ /* compute and add the refinements to the current */
+ /* B-spline control latice. */
+ for (i = 0; i < s->lsize; i++) {
+ double om = omega[i];
+ if (om != 0.0) {
+ for (f = 0; f < p->fdi; f++)
+ s->_lat[i] += delta[i + f]/om;
+printf("Adjusting latice index %d by %f to give %f\n",i, delta[i]/om, s->_lat[i]);
+ }
+ }
+
+ /* Done with temporary arrays */
+ free(omega);
+ free(delta);
+}
+
+/* Return the interpolated value for a given point */
+/* Return NZ if input point is out of range */
+static int lookup_srbs(
+mrbs *p,
+co *c /* Point to interpolate */
+) {
+ srbs *s = p->s;
+ int e, f;
+ int ix; /* Latice index of base of neighborhood */
+ double b[MXDI][4]; /* B-spline basis factors for each dimension */
+ int nn; /* Neighbor counter */
+
+ /* Figure out our neighborhood */
+ for (ix = e = 0; e < p->di; e++) {
+ int x;
+ double t, sp, fp;
+ sp = c->p[e];
+ if (sp < p->l[e] || sp > p->h[e])
+ return 1;
+ sp = (sp - p->l[e])/s->w[e]; /* Scaled position */
+ fp = floor(sp);
+ x = (int)(fp - 1.0); /* Grid coordinate */
+ ix += s->coi[e] * x; /* Accume latice offset */
+ t = sp - fp; /* Spline parameter */
+ basis(b[e], t); /* Compute basis function values */
+ }
+
+ /* Compute the the current output value. */
+ for (f = 0; f < p->fdi; f++)
+ c->v[f] = 0.0;
+ for (nn = 0; nn < s->nsize; nn++) {
+ double w;
+ for (w = 1.0, e = 0; e < p->di; e++)
+ w *= b[e][s->n[nn].c[e]];
+ for (f = 0; f < p->fdi; f++)
+ c->v[f] += w * s->lat[ix + s->n[nn].xo + f]; /* Accume spline value */
+ }
+
+ return 0;
+}
+
+/* Take a list of scattered data points, */
+/* and setup the mrbs. */
+void set_mrbs(
+mrbs *p, /* mrbs to set up */
+co *pts, /* scattered data points (taken) */
+int npts, /* number of scattered data points */
+double *l, /* Input data range, low (May be NULL) */
+double *h /* Input data range, high (May be NULL) */
+) {
+ int res;
+ int i, e, f;
+ srbs *s0 = NULL, *s1;
+
+ /* Establish the input data range */
+ for (e = 0; e < p->di; e++) {
+ if (l == NULL)
+ p->l[e] = 1e60;
+ else
+ p->l[e] = l[e];
+ if (h == NULL)
+ p->h[e] = -1e60;
+ else
+ p->h[e] = h[e];
+ }
+ for (i = 0; i < npts; i++) {
+ for (e = 0; e < p->di; e++) {
+ if (pts[i].p[e] < p->l[e])
+ p->l[e] = pts[i].p[e];
+ if (pts[i].p[e] > p->h[e])
+ p->h[e] = pts[i].p[e];
+ }
+ }
+
+ /* Take ownership the data */
+ p->pts = pts;
+ p->npts = npts;
+
+ /* Create an initial srbs */
+ res = 2;
+ if ((s1 = new_srbs(p, 2)) == NULL)
+ error("new_srbs failed");
+
+ /* Set it up with a linear first approximation */
+ linear_srbs(s1);
+dump_srbs(s1);
+
+ /* Build up the resolution */
+ for (res = 2 * res -1; res < p->tres; res = 2 * res -1) {
+
+printf("~1 doing resolution %d\n",res);
+ delete_srbs(s0);
+ s0 = s1;
+ if ((s1 = new_srbs(p, res)) == NULL)
+ error("new_srbs failed");
+
+ refine_srbs(s1, s0);
+dump_srbs(s1);
+ improve_srbs(s1);
+ }
+
+ delete_srbs(s0);
+ p->s = s1; /* Final resolution */
+}
+
+
+/* - - - - - - - - - - - - - - - - */
+
+/* Some test points */
+#define NP 5
+co tpts[NP] = {
+ { { 1, 5 }, { 3 } },
+ { { 2, 9 }, { 6 } },
+ { { 8, 0 }, { 1 } },
+ { { 5, 4 }, { 10 } },
+ { { 3, 9 }, { 4 } }
+};
+
+#define FRES 33
+
+int
+main(void) {
+ double ll[MXDI], hh[MXDI];
+ mrbs *p;
+ int i;
+
+#ifdef NEVER /* Make points be on a plane */
+{
+for (i = 0; i < NP; i++) {
+ tpts[i].v[0] = 0.5 * tpts[i].p[0] + 0.7 * tpts[i].p[1] + 3.0;
+printf("%f, %f -> %f\n", tpts[i].p[0], tpts[i].p[1], tpts[i].v[0]);
+}
+}
+#endif
+ error_program = "stest";
+
+ printf("Starting test\n");
+ p = new_mrbs(2, 1, FRES, 1.0);
+
+ ll[0] = 0.0;
+ ll[1] = 0.0;
+ hh[0] = 10.0;
+ hh[1] = 10.0;
+ set_mrbs(p, tpts, NP, ll, hh);
+
+ printf("Interpolation setup\n");
+
+ /* Check out the accuracy */
+ for (i = 0; i < NP; i++) {
+ co tp;
+ tp.p[0] = tpts[i].p[0];
+ tp.p[1] = tpts[i].p[1];
+ if (lookup_srbs(p, &tp))
+ printf("Point %d, %f %f outside domain\n",i,tpts[i].p[0],tpts[i].p[1]);
+ else {
+ printf("Point %d has value %f, should be %f\n",i,tp.v[0], tpts[i].v[0]);
+ }
+ }
+
+ printf("Test done\n");
+ return 0;
+}
+
+#ifndef NUMSUP_H
+/* Basic printf type error() and warning() routines */
+
+void
+error(char *fmt, ...)
+{
+ va_list args;
+
+ fprintf(stderr,"stest: Error - ");
+ va_start(args, fmt);
+ vfprintf(stderr, fmt, args);
+ va_end(args);
+ fprintf(stderr, "\n");
+ exit (-1);
+}
+
+void
+warning(char *fmt, ...)
+{
+ va_list args;
+
+ fprintf(stderr,"stest: Warning - ");
+ va_start(args, fmt);
+ vfprintf(stderr, fmt, args);
+ va_end(args);
+ fprintf(stderr, "\n");
+}
+
+#endif /* NUMSUP_H */
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-18 00:09:50 +0000