Root/cameo/path.c

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

Archive Download this file

Branches:
master



interactive