summaryrefslogtreecommitdiff
path: root/rspl/spline.c
blob: dabeb0fdf6eba41a03c448ba17b742c7e90cd059 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352

/* 
 * Argyll Color Correction System
 * Multi-dimensional regularized spline data structure
 *
 * Spline forward interpolation support.
 *
 * Author: Graeme W. Gill
 * Date:   12/10/98
 *
 * Copyright 1998, 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.
 */

/* TTBD:
	Get rid of error() calls - return status instead
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <math.h>
#include <time.h>
//#if defined(__IBMC__) && defined(_M_IX86)
//#include <float.h>
//#endif

#include "rspl_imp.h"
#include "numlib.h"

int spline_interp_rspl(rspl *ss, co *cp);

#undef DEBUG

#undef NEVER
#define ALWAYS

/* Convention is to use:
   i to index grid points u.a
   n to index data points d.a
   e to index position dimension di
   f to index output function dimension fdi
   j misc and cube corners
   k misc
 */

/* ====================================================== */

/* Init spline elements in rspl */
void
init_spline(rspl *s) {
	s->spline.nm = 0;
	s->spline.spline = 0;
	s->spline.magic = NULL;

	s->spline_interp = spline_interp_rspl;
}

/* Free up the spline interpolation info */
void free_spline(
rspl *s		/* Pointer to rspl grid */
) {
	if (s->spline.magic != NULL) {
		free(s->spline.magic);
	}
	s->spline.nm = 0;
	s->spline.spline = 0;
}

/* ====================================================== */
/* Setup functions first: */

/* Hermite spline, magic matrix */
/* Indexes are: param powers 0, 1, 2, 3; Offset from base vertex 0,1; Dimension mask 0,1 */
static double hmagic[4][2][2] = {
	{ { 1.0,  0.0}, { 0.0,  0.0} },
	{ { 0.0,  1.0}, { 0.0,  0.0} },
	{ {-3.0, -2.0}, { 3.0, -1.0} },
	{ { 2.0,  1.0}, {-2.0,  1.0} }
};

/* Allocate and initialize tangency information for each grid point */
static void make_tang(
rspl *s		/* Pointer to rspl grid */
) {
	int i,p,j;
	int di  = s->di;
	int fdi = s->fdi;
	int nig = s->g.no;
	float *tp;		/* Pointer to tangent values */
	int nim, mix;	/* Number in magic, magic index */
	float *tang_alloc, *tang;	/* Tangency info */
	float *gt;		/* Working grid point */
	
//printf("~~make_tang called\n");
	/* Organized as: tang[[grid]][di combs.][fdi] */
	/* Allocate space for tangency info */
	if ((tang_alloc = (float *) malloc(sizeof(float) * nig * (((1 << di) * fdi)+G_XTRA))) == NULL)
		error("rspl malloc failed - tangecy points");
	tang = tang_alloc + G_XTRA;	/* Offset for flags and non-mono error */

	/* For all grid points */
	for (tp = tang, gt = s->g.a, i = 0; i < nig; i++, gt += s->g.pss, tp += G_XTRA) {
		int ee;
/* printf("\n~~ grid point %d\n",i); */

		/* Look at surrounding grid points in combinations of +- 1 all dimensions */
		for (ee = 0; ee < (1 << di); ee++) {
			double av[MXRO];	/* average */
			int nia = 0;		/* Number in average */
			int f, ec;

/* printf("Dim combo %d\n",ee); */
			/* special case - base value */
			if (ee == 0) { 
				*((int *)(tp-2)) = *((int *)(gt-2));	/* Copy flags */
				tp[-1] = gt[-1];						/* Copy ink limit function value */
				for (f = 0; f < fdi; f++) {
					*tp++ = gt[f];
/* printf("Tang value out %d = %f\n",f,tp[-1]); */
				}
				continue;
			}
			for (f = 0; f < fdi; f++)
				av[f] = 0.0;	/* Init average */
			/* For all surroundin grid points in this combination */
			for (ec = 0; ec < (1 << di); ec++) {
				int xo, io, sgn, e, ex;
/* printf("~~checking out surrounding combo %d\n",ec); */
				if (ec & ~ee) {
/* printf("~~being skipped\n"); */
					continue;	/* Skip invalid combo */
				}
				xo = io = 0;		/* Grid float offset */
				sgn = 1;			/* Sign */
				ex = 0;				/* Flag - No extrapolation */
				for (e = 0; e < di; e++) {	/* For each dimension */
/* printf("~~checking  dimension %d\n",e); */
					if (!(ee & (1 << e))) {
/* printf("~~dimension not active\n"); */
						continue;	/* Dimension is not active */
					}
					if (ec & (1 << e)) {
						/* If + dimension is valid */
						if (((G_FL(gt,e) & 3) > 0) || (G_FL(gt,e) & 0x4)) {
							int to = s->g.fci[e];	/* +1 in dimension */
							io += to;				/* real/pivot point */
							xo += to;				/* reflected point */
						} else {
							ex = 1;					/* Use extrapolation */
							xo -= s->g.fci[e];		/* -1 in dimension */
						}
					} else {
						sgn = -sgn;			/* Reverse sign */
						/* If - dimension is valid */
						if (((G_FL(gt,e) & 3) > 0) || !(G_FL(gt,e) & 0x4)) {
							int to = -s->g.fci[e];	/* -1 in dimension */
							io += to;				/* real/pivot point */
							xo += to;				/* reflected point */
						} else {
							ex = 1;					/* Use extrapolation */
							xo += s->g.fci[e];		/* +1 in dimension */
						}
					}
				}
				/* Add surrounding grid points value into the average */
				if (!ex) {
					for (f = 0; f < fdi; f++) 
						av[f] += (double)sgn * gt[io + f];
				} else {	/* Extrapolate point beyond edge */
							/* Use an extrapolation that tries to maintain curvature */
					for (f = 0; f < fdi; f++)  {
						double v0,v1,v2;
						v0 = gt[io + f];			/* Pivot point */
						v1 = gt[xo + f];			/* Reflection of target in pivot */
						v2 = gt[2 * xo - io + f];	/* Reflection +2 */
						av[f] += (double)sgn * (3.0 * (v0 - v1) + v2);
					}
				}
				nia++;
			}
			for (f = 0; f < fdi; f++) {
				*tp++ = (float)(av[f]/(double)nia);
/* printf("Tang value out %d = %f, average of %d\n",f,tp[-1],nia); */
			}
		}	/* Next dimension combination */
	}	/* Next grid point */

	/* Create a full sized hermite magic matrix */
	/* Organized as: magic[4^di][2^di][2^di] */
	/* = [param power combos][cube vertex index][di combos], */
	/* but then only store non-zero weight values. */
	for (i = 0, nim = 1; i < di; nim *= 10, i++);	/* Number of entries needed */
	if (s->spline.magic == NULL) { /* Allocate space for magic matrix info */
		if ((s->spline.magic = (magic_data *) malloc(sizeof(magic_data) * nim)) == NULL)
			error("rspl malloc failed - hermite magic matrix data");
	}

	mix = 0;
	for (p = 0; p < (1 << (2 * di)); p++) {		/* For all combinations of parameter powers */
		for (i = 0; i < (1 << di); i++)	{		/* For all corners of cube */
			for (j = 0; j < (1 << di); j++)	{	/* For all dimension combinations */
				int ii;
				double wgt = 1.0;
				for (ii = 0; ii < di; ii++) {
					wgt *= hmagic[3&(p>>(2*ii))][1&(i>>ii)][1&(j>>ii)];
				}
				if (wgt != 0.0) {	/* record non-zero weight value */
					s->spline.magic[mix].p = p;
					s->spline.magic[mix].i = i;
					s->spline.magic[mix].j = fdi * j;	/* Pre-scale */
					s->spline.magic[mix].wgt = (float)wgt;
					mix++;
				}
			}
		}
	}
	/* mix should == nim! */
	s->spline.nm = nim;

	/* Free basic grid info, and substitute tangency enhanced version */
	/* ~~~~!! need to free any other structures in rspl that depend on */
	/* ~~~~!! g.pss size, ie. rev stuff ??? */
	if (s->g.alloc != NULL)
		free((void *)s->g.alloc);

	s->g.alloc  = tang_alloc;
	s->g.a      = tang;

	/* Adjust index tables */
	s->g.pss = (1 << di) * fdi + G_XTRA;
	for (i = 0; i < di; i++)
		s->g.fci[i] = s->g.ci[i] * s->g.pss;	/* In floats */
	for (i = 0; i < (1 << di); i++)
		s->g.fhi[i] = s->g.hi[i] * s->g.pss;	/* In floats */

	s->spline.spline = 1;

//printf("~~make_tang finished\n");
}

/* Do a Hermite spline smooth interpolation based on the finest grid */
/* (To do this more accurately, the data point interpolation within */
/*  the grid itteration should be of the same order. This increases */
/*  itteration complexity quite a bit, so we won't bother for the moment.) */
/* This code is not optimised for speed. */
/* Return 0 if OK, 1 if input was clipped to grid */
int spline_interp_rspl(
rspl *s,
co *cp			/* Input value and returned function value */
) {
	int e,f,p,i;
	int di  = s->di;
	int fdi = s->fdi;
	double ppw[MXRI][4];	/* Parameter powers of 0, 1, 2, 3 */
	float  *ga[POW2MXRI];	/* Pointers to grid cubes data in tang[] */
	magic_data *tp;			/* Pointer to items in magic matrix */
	int rv = 0;
	
/* printf("~~smooth interp called\n"); */

	/* This is a restricted size function */
	if (di > MXRI)
		error("rspl: spline can't handle di = %d",di);
	if (fdi > MXRO)
		error("rspl: spline can't handle fdi = %d",fdi);

	if (s->spline.spline == 0) 	/* Compute tangent info if it doesn't exist */
		make_tang(s);

	/* Locate grid base point, and position with base cube */
	ga[0] = s->g.a;	/* Base pointer of cube */
	for (e = 0; e < di; e++) {
		double t, pe;
		int mi, gres_1 = s->g.res[e]-1;

		pe = cp->p[e];
		if (pe < s->g.l[e]) {		/* Clip to grid */
			pe = s->g.l[e];
			rv = 1;
		}
		if (pe > s->g.h[e]) {
			pe = s->g.h[e];
			rv = 1;
		}
		t = (pe - s->g.l[e])/s->g.w[e];
		mi = (int)floor(t);			/* Grid coordinate */
		if (mi < 0)					/* Limit to valid cube base index range */
			mi = 0;
		else if (mi >= gres_1)
			mi = gres_1-1;
		ga[0] += s->g.fci[e] * mi;	/* Add offset in dimen */
		t = t - (double)mi;;		/* sub-cube offset = parameter in dimension e */
		ppw[e][0] = 1.0;			/* Powers of parameter */
		ppw[e][1] = t;
		ppw[e][2] = t * t;
		ppw[e][3] = t * t * t; 
	}

	/* Compute indexes into cube corners in tangent array */
	for (i = 1; i < (1 << di); i++)
		ga[i] = ga[0] + s->g.fhi[i];

	/* Now compute the output values */
	for (f = 0; f < fdi; f++)		/* Zero output value sums */
		cp->v[f] = 0.0;
	
	/* For all non-zero combinations of parameter powers */
	{
		double ppc = -1000.0;			/* Parameter power combination */
		for (tp = s->spline.magic, p = -1; tp < &s->spline.magic[s->spline.nm]; tp++) {
			double wgt;			/* Magic matrix weight */
			float *gp;			/* Pointer to vertex data */

			if (p != tp->p) {		/* Param power needs re-calculating */
				int pp;
				p = tp->p;
				for (ppc = 1.0, pp = 0; pp < di; pp++)
					ppc *= ppw[pp][3&(p>>(2*pp))];		/* comb. of param powers value */
			}

			wgt = tp->wgt * ppc;		/* matrix times parameter */
			gp = ga[tp->i] + tp->j;		/* Point to base of vertex data */
			for (f = 0; f < fdi; f++) 	/* For all output values */
				cp->v[f] += wgt * gp[f];
		}
	}
/* printf("~~smooth interp finished\n"); */
	return rv;
}