1 |
#include <assert.h> |
2 |
#include <stdio.h> |
3 |
#include <stdlib.h> |
4 |
#include "gsl/gsl_vector.h" |
5 |
#include "gsl/gsl_multiroots.h" |
6 |
#include "gsl/gsl_permutation.h" |
7 |
#include "gsl/gsl_linalg.h" |
8 |
|
9 |
/* #undef USE_DERIVATIVES */ |
10 |
/* #define SOLVER gsl_multiroot_fsolver_hybrids */ |
11 |
/* #define SOLVER gsl_multiroot_fsolver_hybrid */ |
12 |
/* #define SOLVER gsl_multiroot_fsolver_dnewton */ |
13 |
#define USE_DERIVATIVES |
14 |
/* #define SOLVER gsl_multiroot_fdfsolver_hybridsj */ |
15 |
/* #define SOLVER gsl_multiroot_fdfsolver_hybridj */ |
16 |
/* #define SOLVER gsl_multiroot_fdfsolver_newton */ |
17 |
#define SOLVER gsl_multiroot_fdfsolver_gnewton |
18 |
|
19 |
#define CONSTANT INT_MAX |
20 |
|
21 |
/** |
22 |
* trigrid_data -- parameters and results for a triangular grid fitting |
23 |
*/ |
24 |
|
25 |
struct trigrid_data { |
26 |
/* parameters */ |
27 |
unsigned int X, Y; |
28 |
double * A; |
29 |
|
30 |
/* results */ |
31 |
int status; |
32 |
unsigned int iter; |
33 |
#ifdef USE_DERIVATIVES |
34 |
gsl_multiroot_fdfsolver * s; |
35 |
#else |
36 |
gsl_multiroot_fsolver * s; |
37 |
#endif |
38 |
unsigned int xs_start, ys_start; |
39 |
gsl_vector * xs; |
40 |
}; |
41 |
|
42 |
|
43 |
void trigrid_fit(struct trigrid_data * data); |
44 |
int trigrid_f(const gsl_vector * xs, void * params, gsl_vector * f); |
45 |
int trigrid_df(const gsl_vector * xs, void * params, gsl_matrix * J); |
46 |
int trigrid_fdf(const gsl_vector * xs, void * params, gsl_vector * f, gsl_matrix * J); |
47 |
double trigrid_get_x(const struct trigrid_data * data, const gsl_vector * xs, |
48 |
unsigned int x, unsigned int y); |
49 |
double trigrid_get_y(const struct trigrid_data * data, const gsl_vector * xs, |
50 |
unsigned int x, unsigned int y); |
51 |
unsigned int trigrid_offset_x(const struct trigrid_data * data, |
52 |
unsigned int x, unsigned int y); |
53 |
unsigned int trigrid_offset_y(const struct trigrid_data * data, |
54 |
unsigned int x, unsigned int y); |
55 |
int main(void); |
56 |
void trigrid_print(struct trigrid_data * data); |
57 |
|
58 |
|
59 |
/** |
60 |
* trigrid_fit -- create a triangular grid with triangle areas A |
61 |
* |
62 |
* p struct trigrid_data with the following fields filled in: |
63 |
* X number of pairs of triangles in a row |
64 |
* Y number of rows of triangles |
65 |
* A target areas from bottom left by row, 2 * X * Y values |
66 |
* |
67 |
* p is updated with the result, which can be conveniently accessed |
68 |
* using trigrid_get_x and trigrid_get_y |
69 |
*/ |
70 |
|
71 |
void trigrid_fit(struct trigrid_data * data) |
72 |
{ |
73 |
unsigned int X = data->X, Y = data->Y; |
74 |
const size_t n = X * Y * 2; |
75 |
unsigned int x, y, i; |
76 |
|
77 |
int status; |
78 |
size_t iter = 0; |
79 |
|
80 |
|
81 |
#ifdef USE_DERIVATIVES |
82 |
gsl_multiroot_function_fdf f = {&trigrid_f, &trigrid_df, &trigrid_fdf, n, data}; |
83 |
#else |
84 |
gsl_multiroot_function f = {&trigrid_f, n, data}; |
85 |
#endif |
86 |
|
87 |
gsl_vector * xs = gsl_vector_alloc(n); |
88 |
data->xs = xs; |
89 |
|
90 |
i = 0; |
91 |
data->xs_start = i; |
92 |
for (y = 0; y != Y + 1; y++) |
93 |
for (x = 1; x != X; x++) |
94 |
gsl_vector_set(xs, i++, x + 0.1 * ((double) rand() / RAND_MAX - 0.5)); |
95 |
gsl_vector_set(xs, i++, X + 0.1 * ((double) rand() / RAND_MAX - 0.5)); |
96 |
data->ys_start = i; |
97 |
for (x = 0; x != X + 1; x++) |
98 |
for (y = 1; y != Y; y++) |
99 |
gsl_vector_set(xs, i++, y + 0.1 * ((double) rand() / RAND_MAX - 0.5)); |
100 |
gsl_vector_set(xs, i++, Y + 0.1 * ((double) rand() / RAND_MAX - 0.5)); |
101 |
assert(i == n); |
102 |
|
103 |
#ifdef USE_DERIVATIVES |
104 |
data->s = gsl_multiroot_fdfsolver_alloc(SOLVER, n); |
105 |
gsl_multiroot_fdfsolver_set(data->s, &f, xs); |
106 |
#else |
107 |
data->s = gsl_multiroot_fsolver_alloc(SOLVER, n); |
108 |
gsl_multiroot_fsolver_set(data->s, &f, xs); |
109 |
#endif |
110 |
|
111 |
/* trigrid_print(data); */ |
112 |
|
113 |
do { |
114 |
iter++; |
115 |
fprintf(stderr, "iter = %3u\n", iter); |
116 |
|
117 |
#ifdef USE_DERIVATIVES |
118 |
status = gsl_multiroot_fdfsolver_iterate(data->s); |
119 |
#else |
120 |
status = gsl_multiroot_fsolver_iterate(data->s); |
121 |
#endif |
122 |
|
123 |
/* trigrid_print(data); */ |
124 |
|
125 |
if (status) |
126 |
break; |
127 |
|
128 |
status = gsl_multiroot_test_residual(data->s->f, 0.01); |
129 |
} while (status == GSL_CONTINUE && iter != 1000); |
130 |
|
131 |
fprintf(stderr, "status = %s\n", gsl_strerror(status)); |
132 |
|
133 |
data->status = status; |
134 |
data->iter = iter; |
135 |
} |
136 |
|
137 |
|
138 |
/** |
139 |
* trigrid_f -- function evaluator for triangular grid fitter |
140 |
*/ |
141 |
|
142 |
int trigrid_f(const gsl_vector * xs, void * params, gsl_vector * f) |
143 |
{ |
144 |
unsigned int x, y, X, Y, i; |
145 |
struct trigrid_data * data = (struct trigrid_data *) params; |
146 |
|
147 |
X = data->X; |
148 |
Y = data->Y; |
149 |
|
150 |
for (i = 0, y = 0; y != Y; y++) { |
151 |
for (x = 0; x != X; x++) { |
152 |
double A, v; |
153 |
double ax, ay, bx, by, cx, cy, dx, dy; |
154 |
|
155 |
ax = trigrid_get_x(data, xs, x, y); |
156 |
ay = trigrid_get_y(data, xs, x, y); |
157 |
bx = trigrid_get_x(data, xs, x + 1, y + 1); |
158 |
by = trigrid_get_y(data, xs, x + 1, y + 1); |
159 |
cx = trigrid_get_x(data, xs, x, y + 1); |
160 |
cy = trigrid_get_y(data, xs, x, y + 1); |
161 |
dx = trigrid_get_x(data, xs, x + 1, y); |
162 |
dy = trigrid_get_y(data, xs, x + 1, y); |
163 |
|
164 |
/* printf("x = %u, y = %u, a = %g %g, b = %g %g, c = %g %g, d = %g %g\n", */ |
165 |
/* x, y, ax, ay, bx, by, cx, cy, dx, dy); */ |
166 |
|
167 |
/* upper triangle */ |
168 |
A = data->A[i]; |
169 |
v = 0.5 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by)); |
170 |
/* printf("upper A = %g, v = %g\n", A, v); */ |
171 |
|
172 |
gsl_vector_set(f, i++, v - A); |
173 |
|
174 |
/* lower triangle */ |
175 |
A = data->A[i]; |
176 |
v = 0.5 * (ax * (dy - by) + dx * (by - ay) + bx * (ay - dy)); |
177 |
/* printf("lower A = %g, v = %g\n\n", A, v); */ |
178 |
|
179 |
gsl_vector_set(f, i++, v - A); |
180 |
} |
181 |
} |
182 |
|
183 |
return GSL_SUCCESS; |
184 |
} |
185 |
|
186 |
|
187 |
/** |
188 |
* trigrid_df -- function derivative evaluator for triangular grid fitter |
189 |
*/ |
190 |
|
191 |
int trigrid_df(const gsl_vector * xs, void * params, gsl_matrix * J) |
192 |
{ |
193 |
unsigned int x, y, X, Y, i; |
194 |
struct trigrid_data * data = (struct trigrid_data *) params; |
195 |
|
196 |
X = data->X; |
197 |
Y = data->Y; |
198 |
gsl_matrix_set_zero(J); |
199 |
|
200 |
/* printf("xs =\n"); */ |
201 |
/* gsl_vector_fprintf(stdout, xs, "%g"); */ |
202 |
|
203 |
for (i = 0, y = 0; y != Y; y++) { |
204 |
for (x = 0; x != X; x++) { |
205 |
double ax, ay, bx, by, cx, cy, dx, dy; |
206 |
unsigned int axo, ayo, bxo, byo, cxo, cyo, dxo, dyo; |
207 |
|
208 |
ax = trigrid_get_x(data, xs, x, y); |
209 |
ay = trigrid_get_y(data, xs, x, y); |
210 |
bx = trigrid_get_x(data, xs, x + 1, y + 1); |
211 |
by = trigrid_get_y(data, xs, x + 1, y + 1); |
212 |
cx = trigrid_get_x(data, xs, x, y + 1); |
213 |
cy = trigrid_get_y(data, xs, x, y + 1); |
214 |
dx = trigrid_get_x(data, xs, x + 1, y); |
215 |
dy = trigrid_get_y(data, xs, x + 1, y); |
216 |
|
217 |
axo = trigrid_offset_x(data, x, y); |
218 |
ayo = trigrid_offset_y(data, x, y); |
219 |
bxo = trigrid_offset_x(data, x + 1, y + 1); |
220 |
byo = trigrid_offset_y(data, x + 1, y + 1); |
221 |
cxo = trigrid_offset_x(data, x, y + 1); |
222 |
cyo = trigrid_offset_y(data, x, y + 1); |
223 |
dxo = trigrid_offset_x(data, x + 1, y); |
224 |
dyo = trigrid_offset_y(data, x + 1, y); |
225 |
|
226 |
/* printf("x = %u, y = %u, a = %g %g, b = %g %g, c = %g %g, d = %g %g\n", */ |
227 |
/* x, y, ax, ay, bx, by, cx, cy, dx, dy); */ |
228 |
|
229 |
/* upper triangle */ |
230 |
/* v = 0.5 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by)); */ |
231 |
|
232 |
if (axo != CONSTANT) gsl_matrix_set(J, i, axo, 0.5 * (by - cy)); |
233 |
if (ayo != CONSTANT) gsl_matrix_set(J, i, ayo, 0.5 * (cx - bx)); |
234 |
if (bxo != CONSTANT) gsl_matrix_set(J, i, bxo, 0.5 * (cy - ay)); |
235 |
if (byo != CONSTANT) gsl_matrix_set(J, i, byo, 0.5 * (ax - cx)); |
236 |
if (cxo != CONSTANT) gsl_matrix_set(J, i, cxo, 0.5 * (ay - by)); |
237 |
if (cyo != CONSTANT) gsl_matrix_set(J, i, cyo, 0.5 * (bx - ax)); |
238 |
|
239 |
i++; |
240 |
|
241 |
/* lower triangle */ |
242 |
/* v = 0.5 * (ax * (dy - by) + dx * (by - ay) + bx * (ay - dy)); */ |
243 |
|
244 |
if (axo != CONSTANT) gsl_matrix_set(J, i, axo, 0.5 * (dy - by)); |
245 |
if (ayo != CONSTANT) gsl_matrix_set(J, i, ayo, 0.5 * (bx - dx)); |
246 |
if (dxo != CONSTANT) gsl_matrix_set(J, i, dxo, 0.5 * (by - ay)); |
247 |
if (dyo != CONSTANT) gsl_matrix_set(J, i, dyo, 0.5 * (ax - bx)); |
248 |
if (bxo != CONSTANT) gsl_matrix_set(J, i, bxo, 0.5 * (ay - dy)); |
249 |
if (byo != CONSTANT) gsl_matrix_set(J, i, byo, 0.5 * (dx - ax)); |
250 |
|
251 |
i++; |
252 |
} |
253 |
} |
254 |
|
255 |
/* printf("J =\n"); |
256 |
gsl_matrix_fprintf(stdout, J, "%f"); |
257 |
printf("\n"); |
258 |
*/ |
259 |
{ |
260 |
gsl_matrix * JJ = gsl_matrix_alloc(2 * data->X * data->Y, 2 * data->X * data->Y); |
261 |
gsl_permutation * p = gsl_permutation_alloc(2 * data->X * data->Y); |
262 |
int signum; |
263 |
|
264 |
gsl_matrix_memcpy(JJ, J); |
265 |
gsl_linalg_LU_decomp(JJ, p, &signum); |
266 |
fprintf(stderr, "det = %g\n", gsl_linalg_LU_det(JJ, signum)); |
267 |
} |
268 |
|
269 |
return GSL_SUCCESS; |
270 |
} |
271 |
|
272 |
|
273 |
int trigrid_fdf(const gsl_vector * xs, void * params, gsl_vector * f, gsl_matrix * J) |
274 |
{ |
275 |
trigrid_f(xs, params, f); |
276 |
trigrid_df(xs, params, J); |
277 |
|
278 |
return GSL_SUCCESS; |
279 |
} |
280 |
|
281 |
|
282 |
/** |
283 |
* trigrid_get_[xy] -- extract grid coordinates from a struct trigrid_data |
284 |
*/ |
285 |
|
286 |
double trigrid_get_x(const struct trigrid_data * data, const gsl_vector * xs, |
287 |
unsigned int x, unsigned int y) |
288 |
{ |
289 |
unsigned int i = trigrid_offset_x(data, x, y); |
290 |
if (i != CONSTANT) |
291 |
return gsl_vector_get(xs, i); |
292 |
if (x == 0) |
293 |
return 0; |
294 |
if (x == data->X) |
295 |
return data->X; |
296 |
assert(0); |
297 |
} |
298 |
|
299 |
unsigned int trigrid_offset_x(const struct trigrid_data * data, |
300 |
unsigned int x, unsigned int y) |
301 |
{ |
302 |
if ((x == data->X) && (y == data->Y)) |
303 |
/* top right corner */ |
304 |
return data->ys_start - 1; |
305 |
if (x == 0) |
306 |
return CONSTANT; |
307 |
if (x == data->X) |
308 |
return CONSTANT; |
309 |
return data->xs_start + y * (data->X - 1) + x - 1; |
310 |
} |
311 |
|
312 |
double trigrid_get_y(const struct trigrid_data * data, const gsl_vector * xs, |
313 |
unsigned int x, unsigned int y) |
314 |
{ |
315 |
unsigned int i = trigrid_offset_y(data, x, y); |
316 |
if (i != CONSTANT) |
317 |
return gsl_vector_get(xs, i); |
318 |
if (y == 0) |
319 |
return 0; |
320 |
if (y == data->Y) |
321 |
return data->Y; |
322 |
assert(0); |
323 |
} |
324 |
|
325 |
unsigned int trigrid_offset_y(const struct trigrid_data * data, |
326 |
unsigned int x, unsigned int y) |
327 |
{ |
328 |
if ((x == data->X) && (y == data->Y)) |
329 |
/* top right corner */ |
330 |
return data->ys_start + (data->X + 1) * (data->Y - 1); |
331 |
if (y == 0) |
332 |
return CONSTANT; |
333 |
if (y == data->Y) |
334 |
return CONSTANT; |
335 |
return data->ys_start + x * (data->Y - 1) + y - 1; |
336 |
} |
337 |
|
338 |
|
339 |
|
340 |
|
341 |
|
342 |
|
343 |
int main(void) |
344 |
{ |
345 |
unsigned int X = 10, Y = 10; |
346 |
double A[] = { |
347 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
348 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
349 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
350 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7, 0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
351 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7, 0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
352 |
|
353 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7, 0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
354 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.7,0.7, 0.7,0.7, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
355 |
0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
356 |
0.1,0.1, 0.1,0.1, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, |
357 |
0.1,0.1, 0.1,0.1, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5, 0.5,0.5 |
358 |
}; |
359 |
|
360 |
struct trigrid_data data = {X, Y, A, 0, 0, 0, 0, 0, 0}; |
361 |
trigrid_fit(&data); |
362 |
trigrid_print(&data); |
363 |
|
364 |
return 0; |
365 |
} |
366 |
|
367 |
|
368 |
void trigrid_print(struct trigrid_data * data) |
369 |
{ |
370 |
unsigned int x, y, i = 0; |
371 |
#ifdef USE_DERIVATIVES |
372 |
gsl_vector * root = gsl_multiroot_fdfsolver_root(data->s); |
373 |
#else |
374 |
gsl_vector * root = gsl_multiroot_fsolver_root(data->s); |
375 |
#endif |
376 |
|
377 |
fprintf(stderr, "root =\n"); |
378 |
gsl_vector_fprintf(stderr, root, "%g"); |
379 |
fprintf(stderr, "\n"); |
380 |
|
381 |
fprintf(stderr, "f =\n"); |
382 |
gsl_vector_fprintf(stderr, data->s->f, "%g"); |
383 |
fprintf(stderr, "\n"); |
384 |
|
385 |
printf("PROCClosedLines\n\n"); |
386 |
printf("PROCFont(\"Corpus.Medium\", 5, 80)\n"); |
387 |
|
388 |
for (y = 0; y != data->Y; y++) { |
389 |
for (x = 0; x != data->X; x++) { |
390 |
double ax, ay, bx, by, cx, cy, dx, dy; |
391 |
|
392 |
ax = trigrid_get_x(data, root, x, y); |
393 |
ay = trigrid_get_y(data, root, x, y); |
394 |
bx = trigrid_get_x(data, root, x + 1, y + 1); |
395 |
by = trigrid_get_y(data, root, x + 1, y + 1); |
396 |
cx = trigrid_get_x(data, root, x, y + 1); |
397 |
cy = trigrid_get_y(data, root, x, y + 1); |
398 |
dx = trigrid_get_x(data, root, x + 1, y); |
399 |
dy = trigrid_get_y(data, root, x + 1, y); |
400 |
|
401 |
/* printf("%.3f %.3f ", ax, ay);*/ |
402 |
|
403 |
printf("PROCGroup\n"); |
404 |
printf("PROCMove(%g, %g)\n", ax, ay); |
405 |
printf("PROCDraw(%g, %g)\n", bx, by); |
406 |
printf("PROCDraw(%g, %g)\n", cx, cy); |
407 |
printf("PROCPrint(\"%.3f\", %g, %g)\n", |
408 |
/*data->A[i] +*/ gsl_vector_get(data->s->f, i), |
409 |
(ax + bx + cx) / 3, (ay + by + cy) / 3); |
410 |
printf("PROCEndGroup\n\n"); |
411 |
i++; |
412 |
|
413 |
printf("PROCGroup\n"); |
414 |
printf("PROCMove(%g, %g)\n", ax, ay); |
415 |
printf("PROCDraw(%g, %g)\n", dx, dy); |
416 |
printf("PROCDraw(%g, %g)\n", bx, by); |
417 |
printf("PROCPrint(\"%.3f\", %g, %g)\n", |
418 |
/*data->A[i] +*/ gsl_vector_get(data->s->f, i), |
419 |
(ax + dx + bx) / 3, (ay + dy + by) / 3); |
420 |
printf("PROCEndGroup\n\n"); |
421 |
i++; |
422 |
} |
423 |
printf("\n"); |
424 |
} |
425 |
printf("\n"); |
426 |
|
427 |
printf("PROCPrint(\"'%s' solver, %u iterations, end status '%s'\", 0, %u)\n", |
428 |
#ifdef USE_DERIVATIVES |
429 |
gsl_multiroot_fdfsolver_name(data->s), |
430 |
#else |
431 |
gsl_multiroot_fsolver_name(data->s), |
432 |
#endif |
433 |
data->iter, |
434 |
gsl_strerror(data->status), |
435 |
data->Y + 1); |
436 |
} |