Ben NanoNote 3D scans
Sign in or create your account | Project List | Help
Ben NanoNote 3D scans Git Source Tree
Root/
Source at commit f37441a7c2078ad61bd6c88dae45707dce2d6ba1 created 13 years 6 months ago. By Werner Almesberger, ben-lcdframe-front-100um is done. | |
---|---|
1 | /* |
2 | * overlap.c - Overlap two parallel faces |
3 | * |
4 | * Written 2010 by Werner Almesberger |
5 | * Copyright 2010 by Werner Almesberger |
6 | * |
7 | * This program is free software; you can redistribute it and/or modify |
8 | * it under the terms of the GNU General Public License as published by |
9 | * the Free Software Foundation; either version 2 of the License, or |
10 | * (at your option) any later version. |
11 | */ |
12 | |
13 | |
14 | #include <stdlib.h> |
15 | #include <stdio.h> |
16 | #include <math.h> |
17 | #include <limits.h> |
18 | #include <gtk/gtk.h> |
19 | |
20 | #include "util.h" |
21 | #include "face.h" |
22 | #include "solid.h" |
23 | #include "style.h" |
24 | #include "overlap.h" |
25 | |
26 | |
27 | #define UNDEF_F HUGE_VAL |
28 | |
29 | |
30 | static int has_osd; |
31 | |
32 | |
33 | static int sx(const struct solid *s) |
34 | { |
35 | return (s->a->sx > s->b->sx ? s->a->sx : s->b->sx)+2*OVERLAP_BORDER; |
36 | } |
37 | |
38 | |
39 | static int sy(const struct solid *s) |
40 | { |
41 | return (s->a->sy > s->b->sy ? s->a->sy : s->b->sy)+2*OVERLAP_BORDER; |
42 | } |
43 | |
44 | |
45 | static double r_center(const struct solid *s) |
46 | { |
47 | return hypot(sx(s), sy(s))/OVERLAP_CENTER_DIV; |
48 | } |
49 | |
50 | |
51 | static double ramp(int z0, double w0, int z1, double w1) |
52 | { |
53 | if (z0 != UNDEF && z1 != UNDEF) |
54 | return z0*w0+z1*w1; |
55 | if (z0 == UNDEF && z0 == UNDEF) |
56 | return UNDEF_F; |
57 | if (z0 == UNDEF && w0 < w1) |
58 | return z1; |
59 | if (z1 == UNDEF && w0 > w1) |
60 | return z0; |
61 | return UNDEF_F; |
62 | } |
63 | |
64 | |
65 | static double zmix(struct face *f, double x, double y) |
66 | { |
67 | int xa, xb, ya, yb; |
68 | double zx0, zx1; |
69 | |
70 | xa = floor(x); |
71 | xb = xa+1; |
72 | ya = floor(y); |
73 | yb = ya+1; |
74 | |
75 | zx0 = ramp( |
76 | get_bounded(f->a, xa, ya), yb-y, |
77 | get_bounded(f->a, xa, yb), y-ya); |
78 | zx1 = ramp( |
79 | get_bounded(f->a, xb, ya), yb-y, |
80 | get_bounded(f->a, xb, yb), y-ya); |
81 | |
82 | return ramp(zx0, xb-x, zx1, x-xa); |
83 | } |
84 | |
85 | |
86 | /* |
87 | * Coordinate transformations, on the example of the x coordinate: |
88 | * |
89 | * - the x coordinate runs from 0 to sx(s)-1 |
90 | * - since we work relative to the screen center, this becomes x-sx(s)/2 |
91 | * This is what we perform the coordinate transform on. |
92 | * - our model runs from min_x to max_x. Its center is at cx. |
93 | */ |
94 | |
95 | static void point(const struct solid *s, int x, int y, guchar *p, |
96 | const struct matrix *ma, const struct matrix *mb) |
97 | { |
98 | double za, zb, z; |
99 | double xaf, xbf, yaf, ybf; |
100 | |
101 | matrix_map(x, y, ma, &xaf, &yaf); |
102 | matrix_map(x, y, mb, &xbf, &ybf); |
103 | |
104 | za = zmix(s->a, xaf, yaf); |
105 | zb = zmix(s->b, xbf, ybf); |
106 | |
107 | if (za == UNDEF_F && zb == UNDEF_F) |
108 | return; |
109 | |
110 | if (za == UNDEF_F) { |
111 | z = 128.0*(zb-s->b->a->min_z)/(s->b->a->max_z-s->b->a->min_z); |
112 | if (z < 0) |
113 | z = 0; |
114 | if (z > 255) |
115 | z = 255; |
116 | p[0] = 255; |
117 | p[1] = z; |
118 | p[2] = z; |
119 | return; |
120 | } |
121 | if (zb == UNDEF_F) { |
122 | z = 128.0*(za-s->a->a->min_z)/(s->a->a->max_z-s->a->a->min_z); |
123 | if (z < 0) |
124 | z = 0; |
125 | if (z > 255) |
126 | z = 255; |
127 | p[0] = z; |
128 | p[1] = 255; |
129 | p[2] = z; |
130 | return; |
131 | } |
132 | |
133 | z = za; |
134 | za -= face_z0(s->a, xaf, yaf); |
135 | zb -= face_z0(s->b, xbf, ybf); |
136 | |
137 | if (za+zb < -s->dist) { |
138 | p[0] = 0; |
139 | p[1] = 0; |
140 | p[2] = 255; |
141 | return; |
142 | } |
143 | |
144 | z = 256.0*(z-s->a->a->min_z)/(s->a->a->max_z-s->a->a->min_z); |
145 | if (z < 0) |
146 | z = 0; |
147 | if (z > 255) |
148 | z = 255; |
149 | p[0] = z; |
150 | p[1] = z; |
151 | p[2] = z; |
152 | } |
153 | |
154 | |
155 | static void merge_matrix(struct matrix *m, const struct solid *s, |
156 | const struct face *f) |
157 | { |
158 | double tm[2][2], tm2[2][2]; |
159 | double tv[2]; |
160 | double f_x, f_y; |
161 | |
162 | /* |
163 | * Finally, we convert to model matrix coordinates. |
164 | * |
165 | * v' = v+c |
166 | */ |
167 | |
168 | m->b[0] += f->cx; |
169 | m->b[1] += f->cy; |
170 | |
171 | /* |
172 | * Apply shrinkage caused by rotation out of z0. |
173 | * We need to divide by x = cos a. We have f = tan a. |
174 | * With sin^2 a + cos^2 a = 1, we get |
175 | * |
176 | * f = sqrt(1-cos^2 a)/cos a |
177 | * = sqrt(1-x^2)/x |
178 | * f^2 = 1/x^2-1 |
179 | * 1/(f^2+1) = x^2 |
180 | * cos a = sqrt(1/(f^2+1)) |
181 | */ |
182 | |
183 | f_x = sqrt(f->fx*f->fx+1); |
184 | f_y = sqrt(f->fy*f->fy+1); |
185 | |
186 | m->a[0][0] *= f_x; |
187 | m->a[0][1] *= f_x; |
188 | m->b[0] *= f_x; |
189 | m->a[1][0] *= f_y; |
190 | m->a[1][1] *= f_y; |
191 | m->b[1] *= f_y; |
192 | |
193 | /* |
194 | * The transformation matrix f->m describes a transformation of |
195 | * (centered) model coordinates. We therefore have to reverse it: |
196 | * |
197 | * v = v'A+b |
198 | * v-b = v'A |
199 | * (v-b)A^-1 = v' |
200 | * vA^-1-bA^-1 = v' |
201 | */ |
202 | |
203 | matrix_invert(f->m.a, tm); |
204 | matrix_multv(f->m.b, tm, tv); |
205 | tv[0] = -tv[0]; |
206 | tv[1] = -tv[1]; |
207 | |
208 | /* |
209 | * Merge with the transformations we have so far: |
210 | * |
211 | * v' = vA1+b1 the transformation we have so far |
212 | * v'' = v'A2+b2 the transformation we apply |
213 | * |
214 | * v'' = (vA1+b1)A2+b2 |
215 | * v'' = vA1A2+b1A2+b2 |
216 | */ |
217 | |
218 | /* |
219 | * So far, the theory. To make it really work, we have to calculate |
220 | * v'' = vA1A2+b1+b2 |
221 | * duh ?!? |
222 | */ |
223 | |
224 | matrix_mult(m->a, tm, tm2); /* A1A2 */ |
225 | matrix_copy(tm2, m->a); |
226 | // matrix_multv(m->b, tm, m->b); /* b1A2 */ |
227 | m->b[0] += tv[0]; /* b2 */ |
228 | m->b[1] += tv[1]; |
229 | |
230 | /* |
231 | * Our input is a screen coordinate, its origin is in a corner so we |
232 | * first have to make it center-based: |
233 | * |
234 | * v' = (v-s/2)A+b |
235 | * v' = vA+(b-s/2*A) |
236 | */ |
237 | |
238 | tv[0] = sx(s)/2; |
239 | tv[1] = sy(s)/2; |
240 | matrix_multv(tv, m->a, tv); |
241 | m->b[0] -= tv[0]; |
242 | m->b[1] -= tv[1]; |
243 | } |
244 | |
245 | |
246 | static void draw_map(GtkWidget *widget, struct solid *s) |
247 | { |
248 | guchar *rgbbuf, *p; |
249 | int x, y; |
250 | struct matrix ma = { |
251 | .a = { { 1, 0 }, { 0, 1 } }, |
252 | .b = { 0, 0 }, |
253 | }; |
254 | struct matrix mb = { |
255 | .a = { { -1, 0 }, { 0, 1 } }, |
256 | .b = { 0, 0 }, |
257 | }; |
258 | |
259 | rgbbuf = p = calloc(sx(s)*sy(s), 3); |
260 | if (!rgbbuf) { |
261 | perror("calloc"); |
262 | exit(1); |
263 | } |
264 | |
265 | merge_matrix(&ma, s, s->a); |
266 | merge_matrix(&mb, s, s->b); |
267 | |
268 | for (y = sy(s)-1; y >= 0; y--) |
269 | for (x = 0; x != sx(s) ; x++) { |
270 | point(s, x, y, p, &ma, &mb); |
271 | p += 3; |
272 | } |
273 | gdk_draw_rgb_image(widget->window, |
274 | widget->style->fg_gc[GTK_STATE_NORMAL], |
275 | 0, 0, sx(s), sy(s), GDK_RGB_DITHER_MAX, rgbbuf, sx(s)*3); |
276 | free(rgbbuf); |
277 | } |
278 | |
279 | |
280 | static void draw_image(GtkWidget *widget, struct solid *s, int osd) |
281 | { |
282 | int cx = sx(s)/2; |
283 | int cy = sy(s)/2; |
284 | int p; |
285 | |
286 | draw_map(widget, s); |
287 | has_osd = osd; |
288 | if (!osd) |
289 | return; |
290 | draw_circle(widget->window, gc_osd, cx, cy, r_center(s)); |
291 | p = r_center(s)/sqrt(2); |
292 | gdk_draw_line(widget->window, gc_osd, cx-p, cy-p, cx+p, cy+p); |
293 | gdk_draw_line(widget->window, gc_osd, cx-p, cy+p, cx+p, cy-p); |
294 | } |
295 | |
296 | |
297 | /* |
298 | * Rotate such that a point at distance "r" moves one unit. Rotate |
299 | * counter-clockwise for r > 1, clockwise for r < 0. |
300 | */ |
301 | |
302 | static void rotate(struct matrix *m, double r) |
303 | { |
304 | struct matrix t; |
305 | double s, c; |
306 | |
307 | s = 1/r; |
308 | c = sqrt(1-s*s); |
309 | t.a[0][0] = m->a[0][0]*c-m->a[1][0]*s; |
310 | t.a[0][1] = m->a[0][1]*c-m->a[1][1]*s; |
311 | t.a[1][0] = m->a[1][0]*c+m->a[0][0]*s; |
312 | t.a[1][1] = m->a[1][1]*c+m->a[0][1]*s; |
313 | t.b[0] = m->b[0]*c-m->b[1]*s; |
314 | t.b[1] = m->b[0]*s+m->b[1]*c; |
315 | *m = t; |
316 | } |
317 | |
318 | |
319 | static void do_shift(struct matrix *m, double dx, double dy) |
320 | { |
321 | m->b[0] += dx; |
322 | m->b[1] += dy; |
323 | } |
324 | |
325 | |
326 | static void shift(struct matrix *m, int dx, int dy, double dist) |
327 | { |
328 | /* |
329 | * Wheeling "up" in each quadrant shifts in the respective direction, |
330 | * wheeling "down" in the opposite direction. |
331 | * |
332 | * No rule without exception: we treat the "down" quadrant like the |
333 | * "up" quadrant, because it would be extremely counter-intuitive to |
334 | * wheel "up" to move "down". |
335 | */ |
336 | |
337 | if (dx > 0 && dy < dx && dy > -dx) |
338 | do_shift(m, dist, 0); |
339 | if (dx < 0 && dy < -dx && dy > dx) |
340 | do_shift(m, -dist, 0); |
341 | if (dy > 0 && dx < dy && dx > -dy) |
342 | do_shift(m, 0, dist); |
343 | if (dy < 0 && dx < -dy && dx > dy) |
344 | do_shift(m, 0, dist); /* exception ! */ |
345 | } |
346 | |
347 | |
348 | static int osd_proximity(const struct solid *s, int dx, int dy) |
349 | { |
350 | double r = hypot(dx, dy); |
351 | double rc = r_center(s); |
352 | |
353 | if (fabs(r-rc) < OSD_PROXIMITY) |
354 | return 1; |
355 | if (r > rc) |
356 | return 0; |
357 | if (abs(abs(dx)-abs(dy)) < OSD_PROXIMITY) |
358 | return 1; |
359 | return 0; |
360 | } |
361 | |
362 | |
363 | static gboolean scroll_event(GtkWidget *widget, GdkEventScroll *event, |
364 | gpointer data) |
365 | { |
366 | GtkWidget *darea = gtk_bin_get_child(GTK_BIN(widget)); |
367 | struct solid *s = data; |
368 | int dx = event->x-sx(s)/2; |
369 | int dy = event->y-sy(s)/2; |
370 | double r = hypot(dx, dy); |
371 | double rc = r_center(s); |
372 | double rs, rot, dist; |
373 | int center = r < rc; |
374 | int osd = osd_proximity(s, dx, dy); |
375 | |
376 | if (r < 1) |
377 | return TRUE; |
378 | |
379 | /* |
380 | * rot goes exponentially from SLOWEST_ROT*rs to FASTEST_ROT for |
381 | * r = rc to rs, with rs being half the canvas diagonal. |
382 | * |
383 | * The values are picked such that we achieve sufficient precision at |
384 | * a reasonably large distance from the circle (for accidently entering |
385 | * the circle would change the mode) but can also spin quickly, e.g., |
386 | * when a 180 degrees rotation is needed. |
387 | * |
388 | * First, normalize to 0 ... 1 |
389 | * Then, we start at exp(0) and end at |
390 | * exp(ln(SLOWEST_ROT*rs/FASTEST_ROT))) |
391 | */ |
392 | rs = hypot(sx(s), sy(s))/2; |
393 | rot = (r-rc)/(rs-rc); |
394 | rot = SLOWEST_ROT*rs*exp(-rot*log(SLOWEST_ROT*rs/FASTEST_ROT)); |
395 | |
396 | /* |
397 | * dist stays at 1 from 0...rc/DIST_STEPS, then linearly goes up to |
398 | * DIST_STEPS from rc/DIST_STEPS...rc |
399 | */ |
400 | dist = r/rc*DIST_STEPS; |
401 | if (dist < 0) |
402 | dist = 1; |
403 | |
404 | switch (event->direction) { |
405 | case GDK_SCROLL_UP: |
406 | if (center) |
407 | shift(&s->a->m, dx, dy, dist); |
408 | else |
409 | rotate(&s->a->m, dx > 0 ? rot : -rot); |
410 | draw_image(darea, s, osd); |
411 | break; |
412 | case GDK_SCROLL_DOWN: |
413 | if (center) |
414 | shift(&s->a->m, dx, dy, -dist); |
415 | else |
416 | rotate(&s->a->m, dx > 0 ? -rot : rot); |
417 | draw_image(darea, s, osd); |
418 | break; |
419 | default: |
420 | /* ignore */; |
421 | } |
422 | return TRUE; |
423 | } |
424 | |
425 | |
426 | static gboolean expose_event(GtkWidget *widget, GdkEventExpose *event, |
427 | gpointer user_data) |
428 | { |
429 | draw_image(widget, user_data, has_osd); |
430 | return TRUE; |
431 | } |
432 | |
433 | |
434 | static gboolean motion_notify_event(GtkWidget *widget, GdkEventMotion *event, |
435 | gpointer data) |
436 | { |
437 | struct solid *s = data; |
438 | int dx = event->x-sx(s)/2; |
439 | int dy = event->y-sy(s)/2; |
440 | int osd = osd_proximity(s, dx, dy); |
441 | |
442 | if (osd != has_osd) |
443 | draw_image(widget, s, osd); |
444 | return FALSE; |
445 | } |
446 | |
447 | |
448 | void overlap(GtkWidget *canvas, struct solid *s) |
449 | { |
450 | GtkWidget *evbox, *darea; |
451 | |
452 | evbox = gtk_event_box_new(); |
453 | darea = gtk_drawing_area_new(); |
454 | |
455 | gtk_widget_set_events(darea, |
456 | GDK_EXPOSE | GDK_KEY_PRESS_MASK | |
457 | GDK_BUTTON_PRESS_MASK | GDK_BUTTON_RELEASE_MASK | |
458 | GDK_SCROLL | |
459 | GDK_POINTER_MOTION_MASK); |
460 | |
461 | gtk_widget_set_size_request(darea, sx(s), sy(s)); |
462 | gtk_container_add(GTK_CONTAINER(canvas), evbox); |
463 | gtk_container_add(GTK_CONTAINER(evbox), darea); |
464 | |
465 | draw_image(darea, s, 0); |
466 | |
467 | g_signal_connect(G_OBJECT(evbox), "scroll-event", |
468 | G_CALLBACK(scroll_event), s); |
469 | g_signal_connect(G_OBJECT(darea), "expose-event", |
470 | G_CALLBACK(expose_event), s); |
471 | g_signal_connect(G_OBJECT(darea), "motion-notify-event", |
472 | G_CALLBACK(motion_notify_event), s); |
473 | |
474 | if (0) { |
475 | int i; |
476 | long t0 = time(NULL); |
477 | gtk_widget_show_all(canvas); |
478 | for (i = 0; i != 1000; i++) { |
479 | rotate(&s->a->m, 100); |
480 | draw_image(darea, s, 0); |
481 | while (gtk_events_pending()) |
482 | gtk_main_iteration(); |
483 | } |
484 | fprintf(stderr, "%lu\n", time(NULL)-t0); |
485 | } |
486 | |
487 | } |
488 |
Branches:
master