Root/cameo/path.c

1/*
2 * path.c - Toolpath operations
3 *
4 * Written 2010-2012 by Werner Almesberger
5 * Copyright 2010-2012 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 <stdio.h>
15#include <stdlib.h>
16#include <math.h>
17#include <assert.h>
18
19#include "util.h"
20#include "shape.h"
21#include "poly2d.h"
22#include "path.h"
23
24
25/*
26 * We allow for a bit of tolerance, to absorb the rounding errors KiCad
27 * produces with designs using a metric grid.
28 */
29
30#define EPSILON_MM 0.006 /* 6 um */
31
32
33static void free_points(struct point *points)
34{
35    struct point *next;
36
37    while (points) {
38        next = points->next;
39        free(points);
40        points = next;
41    }
42}
43
44
45void path_free(struct path *path)
46{
47    free_points(path->first);
48    free((void *) path->id);
49    free(path);
50}
51
52
53struct path *path_new(double r_tool, const char *id)
54{
55    struct path *path;
56
57    path = alloc_type(struct path);
58    path->r_tool = r_tool;
59    path->outside = 0;
60    path->notch = 0;
61    path->id = id ? stralloc(id) : NULL;
62    path->first = path->last = NULL;
63    path->next = NULL;
64    return path;
65}
66
67
68static struct path *path_from(const struct path *old)
69{
70    struct path *new;
71
72    new = path_new(old->r_tool, old->id);
73    new->outside = old->outside;
74    new->notch = old->notch;
75    return new;
76}
77
78
79static struct point *clone_point(const struct point *p)
80{
81    struct point *n;
82
83    n = alloc_type(struct point);
84    n->x = p->x;
85    n->y = p->y;
86    n->z = p->z;
87    n->next = NULL;
88    return n;
89}
90
91
92static int points_eq(const struct point *a, const struct point *b)
93{
94    if (hypot(a->x-b->x, a->y-b->y) > EPSILON_MM)
95        return 0;
96    if (fabs(a->z-b->z) > EPSILON_MM)
97        return 0;
98    return 1;
99}
100
101
102int path_is_closed(const struct path *path)
103{
104    if (path->first == path->last)
105        return 1;
106    return points_eq(path->first, path->last);
107}
108
109
110static void assert_path_is_closed(const struct path *path)
111{
112    if (path_is_closed(path))
113        return;
114    fprintf(stderr, "path from (%g, %g, %g) to (%g, %g, %g) is open\n",
115        path->first->x, path->first->y, path->first->z,
116        path->last->x, path->last->y, path->last->z);
117    abort();
118}
119
120
121static void path_add_point(struct path *path, struct point *p)
122{
123    if (path->last &&
124        path->last->x == p->x && path->last->y == p->y &&
125        path->last->z == p->z) {
126        free(p);
127        return;
128    }
129    p->next = NULL;
130    if (path->last)
131        path->last->next = p;
132    else
133        path->first = p;
134    path->last = p;
135}
136
137
138void path_add(struct path *path, double x, double y, double z)
139{
140    struct point *p;
141
142    p = alloc_type(struct point);
143    p->x = x;
144    p->y = y;
145    p->z = z;
146    return path_add_point(path, p);
147}
148
149
150struct path *path_reverse(const struct path *path)
151{
152    struct path *new;
153    const struct point *p;
154    struct point *n;
155
156    new = path_from(path);
157    for (p = path->first; p; p = p->next) {
158        n = alloc_type(struct point);
159        n->x = p->x;
160        n->y = p->y;
161        n->z = p->z;
162        n->next = new->first;
163        if (!new->last)
164            new->last = n;
165        new->first = n;
166    }
167    return new;
168}
169
170
171struct path *path_clone(const struct path *path)
172{
173    struct path *new;
174    const struct point *p;
175    struct point *n;
176
177    new = path_from(path);
178    for (p = path->first; p; p = p->next) {
179        n = clone_point(p);
180        if (new->first)
181            new->last->next = n;
182        else
183            new->first = n;
184        new->last = n;
185    }
186    return new;
187}
188
189
190static void path_reverse_inplace(struct path *path)
191{
192    struct point *points = NULL, *p, *next;
193
194    path->last = path->first;
195    for (p = path->first; p; p = next) {
196        next = p->next;
197        p->next = points;
198        points = p;
199    }
200    path->first = points;
201}
202
203
204static struct point *offset_point(const struct point *a, const struct point *b,
205    const struct point *c, double off, int left)
206{
207    double ax, ay, bx, by;
208    double aa, bb;
209    double nx, ny;
210    double angle, f;
211    struct point *p;
212
213    ax = b->x-a->x;
214    ay = b->y-a->y;
215    bx = c->x-b->x;
216    by = c->y-b->y;
217
218    aa = hypot(ax, ay);
219    bb = hypot(bx, by);
220
221    if (left) {
222        nx = -(ay/aa+by/bb);
223        ny = ax/aa+bx/bb;
224    } else {
225        nx = ay/aa+by/bb;
226        ny = -(ax/aa+bx/bb);
227    }
228
229    /* angle between AB and BC */
230    angle = acos(-(ax*bx+ay*by)/aa/bb);
231
232    /* multiplier for combination of normal vectors */
233    f = off/sin(angle/2);
234    f /= hypot(nx, ny);
235
236    nx *= f;
237    ny *= f;
238
239    p = alloc_type(struct point);
240    p->x = b->x+nx;
241    p->y = b->y+ny;
242    p->z = b->z;
243    p->next = NULL;
244
245    return p;
246}
247
248
249static int left_turn(const struct point *a, const struct point *b,
250    const struct point *c)
251{
252    double ax, ay, bx, by;
253
254    ax = b->x-a->x;
255    ay = b->y-a->y;
256    bx = c->x-b->x;
257    by = c->y-b->y;
258
259    return (ax*by-ay*bx) >= 0;
260}
261
262
263/*
264 * Angle in counter-clockwise direction to turn at point B when coming from A
265 * in order to face towards C.
266 */
267
268static double angle_3(const struct point *a, const struct point *b,
269    const struct point *c)
270{
271    double ax, ay, bx, by;
272    double aa, bb;
273    double angle;
274
275    ax = b->x-a->x;
276    ay = b->y-a->y;
277    bx = c->x-b->x;
278    by = c->y-b->y;
279
280    aa = hypot(ax, ay);
281    bb = hypot(bx, by);
282
283    angle = acos((ax*bx+ay*by)/aa/bb)/M_PI*180.0;
284
285    return (ax*by-ay*bx) >= 0 ? angle : -angle;
286}
287
288
289/*
290 * If we predominantly turn to the right, then the tool must be on the
291 * left-hand side. Otherwise, it's on the right.
292 */
293
294int path_tool_is_left(const struct path *path)
295{
296    const struct point *prev, *p, *next;
297    double a = 0;
298
299    
300    assert_path_is_closed(path);
301    prev = path->first;
302    for (p = path->first->next; p; p = p->next) {
303        next = p->next ? p->next : path->first->next;
304        a += angle_3(prev, p, next);
305        prev = p;
306    }
307    return a < 0;
308}
309
310
311/*
312 * http://www.makecnc.com/bones.jpg
313 */
314
315
316static struct point *dog_point(const struct point *edge,
317    const struct point *tool, double off)
318{
319    double vx, vy, v;
320    struct point *p;
321
322    vx = edge->x-tool->x;
323    vy = edge->y-tool->y;
324    v = hypot(vx, vy);
325
326    vx *= 1-off/v;
327    vy *= 1-off/v;
328
329    p = alloc_type(struct point);
330    p->x = tool->x+vx;
331    p->y = tool->y+vy;
332    p->z = tool->z;
333    p->next = NULL;
334
335    return p;
336}
337
338
339/*
340 * The tool is on the "left" side of the path. E.g., if the path goes from
341 * 6 o'clock to 12 o'clock, the tool would be at 9 o'clock.
342 */
343
344struct path *path_offset(const struct path *path, int left, int notch)
345{
346    struct path *new;
347    const struct point *prev, *p, *next;
348    struct point *n, *n2;
349    int dog;
350
351    assert_path_is_closed(path);
352    if (path->first == path->last)
353        return circle(path->first->x, path->first->y, path->first->z,
354            path->r_tool, path->r_tool, 0.1, path->id);
355    new = path_from(path);
356    prev = path->first;
357    for (p = path->first->next; p; p = p->next) {
358        next = p->next ? p->next : path->first->next;
359        n = offset_point(prev, p, next, path->r_tool, left);
360        dog = notch && left_turn(prev, p, next) == left;
361        if (dog)
362            n2 = clone_point(n);
363        path_add_point(new, n);
364        if (dog) {
365            path_add_point(new, dog_point(p, n2, path->r_tool));
366            path_add_point(new, n2);
367        }
368        prev = p;
369    }
370    path_add_point(new, clone_point(new->first));
371    return new;
372}
373
374
375const struct path *path_find_leftmost(const struct path *path)
376{
377    const struct point *p;
378    const struct path *best = NULL;
379    double best_x = HUGE_VAL;
380
381    while (path) {
382        for (p = path->first; p; p = p->next)
383            if (p->x < best_x) {
384                best = path;
385                best_x = p->x;
386                break;
387            }
388        path = path->next;
389    }
390    return best;
391}
392
393
394int path_is_inside(const struct path *a, const struct path *b)
395{
396    struct p2d *pa, *pb;
397    int res;
398
399    pa = path_to_poly(a);
400    pb = path_to_poly(b);
401    res = p2d_contains_poly(pb, pa);
402    p2d_free(pa);
403    p2d_free(pb);
404
405    return res == 1; /* 0 if they intersect */
406}
407
408
409static int attr_eq(const struct path *a, const struct path *b)
410{
411    return a->r_tool == b->r_tool && a->outside == b->outside &&
412        a->notch == b->notch;
413}
414
415
416struct path *path_connect(struct path *path)
417{
418    struct path **a, **b;
419    struct path *tmp;
420
421again:
422    for (a = &path; *a; a = &(*a)->next)
423        for (b = &(*a)->next; *b; b = &(*b)->next) {
424            if (!attr_eq(*a, *b))
425                continue;
426            if (points_eq((*a)->last, (*b)->last))
427                path_reverse_inplace(*b);
428            if (points_eq((*a)->last, (*b)->first)) {
429                (*a)->last->next = (*b)->first->next;
430                (*a)->last = (*b)->last;
431                free((*b)->first);
432                tmp = *b;
433                *b = tmp->next;
434                free(tmp);
435                goto again;
436            }
437            if (points_eq((*a)->first, (*b)->first))
438                path_reverse_inplace(*b);
439            if (points_eq((*a)->first, (*b)->last)) {
440                (*b)->last->next = (*a)->first->next;
441                (*b)->last = (*a)->last;
442                free((*a)->first);
443                tmp = *a;
444                *a = tmp->next;
445                free(tmp);
446                goto again;
447            }
448        }
449    return path;
450}
451
452
453void path_stats(const struct path *path)
454{
455    int paths = 0, segs = 0;
456    double len = 0;
457    const struct point *p;
458
459    while (path) {
460        paths++;
461        for (p = path->first; p; p = p->next) {
462            if (!p->next)
463                continue;
464            segs++;
465            len += hypot(hypot(p->x-p->next->x, p->y-p->next->y),
466                p->z-p->next->z);
467        }
468        path = path->next;
469    }
470    fprintf(stderr, "%d path%s, %d segment%s, %f mm\n",
471        paths, paths == 1 ? "" : "s", segs, segs == 1 ? "" : "s", len);
472}
473

Archive Download this file

Branches:
master



interactive