Root/ptrude/path.c

1/*
2 * path.c - 2D path operations
3 *
4 * Written 2011 by Werner Almesberger
5 * Copyright 2011 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#include <stdlib.h>
14#include <stdio.h>
15#include <string.h>
16#include <math.h>
17#include <assert.h>
18
19#include "ptrude.h"
20#include "path.h"
21
22
23#define alloc_type(t) ((t *) malloc(sizeof(t)))
24#define stralloc(s) strdup(s)
25
26
27static double deg(double rad)
28{
29    return rad/M_PI*180.0;
30}
31
32
33static struct path *alloc_path(void)
34{
35    struct path *path;
36
37    path = alloc_type(struct path);
38    path->vertices = NULL;
39    path->last = &path->vertices;
40    return path;
41}
42
43
44static struct vertex *alloc_vertex(void)
45{
46    struct vertex *v;
47
48    v = alloc_type(struct vertex);
49    v->r = 0;
50    v->d = 0;
51    v->tag = NULL;
52    v->next = NULL;
53    v->len = 0;
54    return v;
55}
56
57
58static void free_vertex(struct vertex *v)
59{
60    free(v);
61}
62
63
64void free_path(struct path *path)
65{
66    struct vertex *v, *next;
67
68    for (v = path->vertices; v; v = next) {
69        next = v->next;
70        free_vertex(v);
71    }
72    free(path);
73}
74
75
76static struct vertex *clone_vertex(const struct vertex *v)
77{
78    struct vertex *new;
79
80    new = alloc_type(struct vertex);
81    *new = *v;
82    new->next = NULL;
83    return new;
84}
85
86
87static void append_vertex(struct path *path, struct vertex *v)
88{
89    *path->last = v;
90    path->last = &v->next;
91}
92
93
94static struct vertex *add_vertex(struct path *path, double x, double y,
95    double r, double d, const char *tag)
96{
97    struct vertex *v;
98
99    v = alloc_vertex();
100    v->x = x;
101    v->y = y;
102    v->r = r;
103    v->d = d;
104    v->tag = tag;
105    append_vertex(path, v);
106    return v;
107}
108
109
110double path_set_length(struct path *path)
111{
112    struct vertex *v;
113    double sum = 0;
114
115    if (!path->vertices)
116        return 0;
117    for (v = path->vertices; v->next; v = v->next) {
118        v->len = hypot(v->x-v->next->x, v->y-v->next->y);
119        sum += v->len;
120    }
121    v->len = 0;
122    return sum;
123}
124
125
126static void adjust_length(struct vertex *from, struct vertex *to, double len)
127{
128    struct vertex *v;
129    double sum, f;
130
131    if (from == to)
132        return;
133    sum = 0;
134    for (v = from->next; v != to; v = v->next) {
135        v->len = hypot(v->x-v->next->x, v->y-v->next->y);
136        sum += v->len;
137    }
138
139    f = len/sum;
140    for (v = from->next; v != to; v = v->next)
141        v->len *= f;
142}
143
144
145/*
146 * "corner" replaces a corner with a ploygon if the corner is too sharp to be
147 * within distance "d" of the bend radius. This may change the point from
148 * where we resume drawing (originally the corner point, "b"). "corner"
149 * therefore returns the new end of the arc.
150 */
151
152static struct vertex *corner(struct path *path, struct vertex *a,
153    const struct vertex *b, const struct vertex *c, double r, double d)
154{
155    /* points to vectors */
156    double ax = b->x-a->x;
157    double ay = b->y-a->y;
158    double bx = c->x-b->x;
159    double by = c->y-b->y;
160
161    /* vector length */
162    double aa = hypot(ax, ay);
163    double bb = hypot(bx, by);
164
165    /* dot and cross product */
166    double dp = ax*bx+ay*by; /* a * b = a*b*cos 2t */
167    double cp = ax*by-ay*bx; /* |a x b| = a*b*sin 2t */
168    double dir = copysign(1, cp);
169
170    /* see corner.fig */
171    double dd; /* "d" of the given vectors */
172    double tt; /* tan t */
173    double s; /* distance between start of arc and corner */
174    double t2; /* angle, t*2 */
175
176    /* see arc.fig */
177    double p; /* half-angle of border side of border segment */
178    double q; /* half-angle of connecting segment */
179    double u; /* length of border side of border segment */
180    double v; /* half-length of connecting segment */
181    int n; /* number of connecting segments (0 if none) */
182
183    double f; /* scale factor; various uses */
184    double fa, fb; /* scale factors for first and last vertex */
185    double ang; /* current angle, for iteration */
186    double x, y; /* current position; for iteration */
187    int i; /* segment; for iteration */
188
189    struct vertex *v0; /* first vertex of arc */
190    struct vertex *v1; /* last vertex of arc */
191
192
193    /*
194     * http://en.wikipedia.org/wiki/Dot_product
195     * dp = a*b*cos 2t
196     *
197     * http://en.wikipedia.org/wiki/Cross_product
198     * cp = a*b*sin 2t
199     *
200     * http://en.wikipedia.org/wiki/Tangent_half-angle_formula
201     * tan t = sin 2t/(1+cos 2t)
202     */
203    tt = cp/(aa*bb+dp);
204
205    /*
206     * From s = r*tan t
207     */
208    s = fabs(r*tt);
209
210    /*
211     * From r^2+s^2 = (r+d)^2
212     */
213    dd = hypot(r, s)-r;
214
215    if (debug) {
216        fprintf(stderr, "a = (%g, %g)-(%g, %g) = (%g, %g); |a| = %g\n",
217            b->x, b->y, a->x, a->y, ax, ay, aa);
218        fprintf(stderr, "b = (%g, %g)-(%g, %g) = (%g, %g); |b| = %g\n",
219            c->x, c->y, b->x, b->y, bx, by, bb);
220        fprintf(stderr, "sin 2t = %g, cos 2t = %g, tan t = %g\n",
221            cp/aa/bb, dp/aa/bb, tt);
222        fprintf(stderr, "r = %g, d = %g, s = %g, dd = %g\n",
223            r, d, s, dd);
224    }
225
226    /*
227     * We only know how to make a rounded corner if two vectors are
228     * involved. They therefore have to be long enough to accommodate the
229     * entire arc, from beginning to end. Furthermore, we split the
230     * available length in half, one for the inbound arc, the other for the
231     * outbound arc.
232     */
233
234    /*
235     * @@@ Our error checking is a bit overzealous and doesn't provide
236     * enough information to debug any problems. Turn errors into warnings
237     * for now.
238     */
239    if (aa/2 < s) {
240        fprintf(stderr, "first vector is too short (%g/2 < %g)\n",
241            aa, s);
242// exit(1);
243    }
244    if (bb/2 < s) {
245        fprintf(stderr, "second vector is too short (%g/2 < %g)\n",
246            bb, s);
247// exit(1);
248    }
249
250    /*
251     * If the corner is already smooth enough, we just keep what we have.
252     */
253    if (dd <= d) {
254        v1 = clone_vertex(b);
255        append_vertex(path, v1);
256        return v1;
257    }
258
259    /* Step 1: determine the total angle (2*t) */
260
261    t2 = acos(dp/aa/bb);
262
263    /*
264     * Step 2: determine the maximum angle of the first and last segment.
265     *
266     * We use
267     * r*cos p = r-d
268     * cos p = 1-d/r
269     */
270
271    p = acos(1-d/r);
272
273    /*
274     * Step 3: determine the maximum angle of intermediate segments (if
275     * there are any).
276     *
277      * We use
278     * (r+d)*cos q = r-d
279     * cos q = r-q/(r+d)
280     */
281
282    q = acos((r-d)/(r+d));
283
284    if (debug)
285        fprintf(stderr, "t2 = %g, p(max) = %g, q(max) = %g\n",
286            deg(t2), deg(p), deg(q));
287
288    /*
289     * Step 4: emit the starting point of the arc
290     */
291
292    fa = s/aa;
293    x = b->x-fa*ax;
294    y = b->y-fa*ay;
295    v0 = add_vertex(path, x, y, b->r, b->d, b->tag);
296    v0->len = a->len*(1-fa);
297
298    /*
299     * Step 5: determine if we need intermediate points. If yes, how many,
300     * and then proceed to add them.
301     */
302
303    if (t2 > 2*p) {
304        n = (int) ceil((t2-2*(p+q))/(2*q));
305
306        /*
307         * We could evenly distribute the slack and try to pick a
308         * smaller value for d, but that seems difficult.
309         *
310         * A drawback of reducing p would be that we may make the
311         * corner unnecessarily sharp, possibly even turning against
312         * the general direction of the turn. We'd still respect the
313         * bend radius and the tolerance, but the result may look weird
314         * anyway.
315         *
316         * For now, we just center the polygon.
317         */
318        q = (t2/2-p)/(n+1);
319
320        if (n)
321            ang = p+q;
322        else {
323            ang = t2/2;
324            /*
325             * @@@ To do: adjust the radius such that we always hug
326             * the r-d circle (see arc.fig) and usually not the
327             * r+d circle. Right now, it's just the opposite.
328             */
329        }
330
331        u = tan(p)*(r-d);
332        v = tan(q)*(r-d);
333        f = (u+v)/aa;
334        for (i = 0; i <= n; i++) {
335            x += f*ax*cos(ang-q)-dir*f*ay*sin(ang-q);
336            y += dir*f*ax*sin(ang-q)+f*ay*cos(ang-q);
337            if (debug)
338                fprintf(stderr, " %d/%d: %g %g @ %g\n", i, n,
339                    x, y, deg(ang));
340            add_vertex(path, x, y, 0, 0, NULL);
341            ang += 2*q;
342            f = (2*v)/aa;
343        }
344    }
345
346    /*
347     * Step 6: emit the finishing point of the arc
348     */
349
350    fb = s/bb;
351    v1 = add_vertex(path, b->x+fb*bx, b->y+fb*by, 0, 0, NULL);
352    v1->len = b->len*(1-fb);
353
354    /*
355     * Step 7: adjust the nominal length of the segments
356     */
357
358    adjust_length(v0, v1, a->len*fa+b->len*fb);
359
360
361    return v1;
362}
363
364    
365struct path *round_path(const struct path *path, double r, double d)
366{
367    struct path *new;
368    struct vertex *prev;
369    const struct vertex *v;
370
371    new = alloc_path();
372    if (!path->vertices)
373        return new;
374
375    prev = clone_vertex(path->vertices);
376    append_vertex(new, prev);
377
378    if (!path->vertices->next)
379        return new;
380
381    if (prev->r)
382        r = prev->r;
383    if (prev->d)
384        d = prev->d;
385
386    for (v = path->vertices->next; v->next; v = v->next) {
387        if (v->r)
388            r = v->r;
389        if (v->d)
390            d = v->d;
391        prev = corner(new, prev, v, v->next, r, d);
392    }
393    append_vertex(new, clone_vertex(v));
394    return new;
395}
396
397
398static void move_vertex(struct path *path, const struct vertex *v,
399    double nx, double ny, double dist, double r)
400{
401    struct vertex *new;
402
403    new = clone_vertex(v);
404    new->x += nx*dist;
405    new->y += ny*dist;
406    new->r = r;
407    append_vertex(path, new);
408}
409
410
411struct path *stretch_path(const struct path *path, double dist, double r)
412{
413    struct path *new; /* new path */
414    const struct vertex *v; /* current vertex (for iteration) */
415        const struct vertex *a, *b, *c; /* previous, current, next vertex */
416        double nx, ny; /* 2D normals */
417        double f; /* factor for normalization */
418        double tx, ty; /* temporary 2D normals */
419
420    new = alloc_path();
421
422    a = path->vertices;
423    b = a->next;
424    nx = b->y-a->y;
425    ny = a->x-b->x;
426    f = hypot(nx, ny);
427    if (a->r)
428        r = a->r;
429    move_vertex(new, a, nx/f, ny/f, dist, r);
430
431    for (v = path->vertices->next; v->next; v = v->next) {
432        double tmp;
433
434        b = v;
435        c = v->next;
436
437        tx = b->y-a->y;
438        ty = a->x-b->x;
439        f = hypot(tx, ty);
440        nx = tx/f;
441        ny = ty/f;
442
443        tmp = f;
444
445        tx = c->y-b->y;
446        ty = b->x-c->x;
447        f = hypot(tx, ty);
448        nx += tx/f;
449        ny += ty/f;
450        if (b->r)
451            r = b->r;
452
453        f = hypot(nx, ny);
454        nx /= f;
455        ny /= f;
456
457        /*
458         * We have this far:
459         * nx, ny = normal on corner, normalized
460         * tmp = |a|, length of vector "a" (A -> B)
461         * dist = the distance by which we stretch
462         *
463         * As shown in stretch.fig, we the length we need is
464         * d' = d/cos(90-t)
465         *
466         * With
467         * http://en.wikipedia.org/wiki/Trigonometric_identities#Symmetry
468         * cos(90-t) = sin t = (n x a)/(|n|*|a|)
469         *
470         * Thus
471         * d' = d/sin(t) - d*(|n|*|a|)/(n x a)
472         * = d/sin(t) - d*|a|/(n x a)
473         */
474        tmp = dist*tmp/(nx*(b->y-a->y)-ny*(b->x-a->x));
475
476        move_vertex(new, b, nx, ny, tmp, r+dist);
477
478        a = v;
479    }
480
481    nx = v->y-a->y;
482    ny = a->x-v->x;
483    f = hypot(nx, ny);
484    if (v->r)
485        r = v->r;
486    move_vertex(new, v, nx/f, ny/f, dist, r);
487
488    return new;
489}
490
491
492struct path *load_path(FILE *file)
493{
494    struct path *path;
495    char buf[1100]; /* plenty :) */
496    char buf2[sizeof(buf)];
497    char *s;
498    float x, y, tmp;
499    float r = 0, d = 0;
500    const char *tag = NULL;
501
502    path = alloc_path();
503    while (fgets(buf, sizeof(buf),file)) {
504        s = strchr(buf, '\n');
505        if (s)
506            *s = 0;
507        if (sscanf(buf, "#r=%f", &tmp) == 1) {
508            r = tmp;
509            continue;
510        }
511        if (sscanf(buf, "#delta=%f", &tmp) == 1) {
512            d = tmp;
513            continue;
514        }
515        if (sscanf(buf, "#tag=%s", buf2) == 1) {
516            tag = stralloc(buf2);
517            continue;
518        }
519        if (*buf == '#')
520            continue;
521        if (sscanf(buf, "%f %f", &x, &y) != 2) {
522            fprintf(stderr, "can't parse \"%s\"\n", buf);
523            exit(1);
524        }
525
526        add_vertex(path, x, y, r, d, tag);
527
528        r = 0;
529        d = 0;
530        tag = NULL;
531    }
532
533    path_set_length(path);
534    return path;
535}
536
537
538void save_path(FILE *file, const struct path *path)
539{
540    const struct vertex *v;
541
542    for (v = path->vertices; v; v = v->next) {
543        if (v->r)
544            fprintf(file, "#r=%f\n", v->r);
545        if (v->d)
546            fprintf(file, "#delta=%f\n", v->d);
547        if (v->tag)
548            fprintf(file, "#delta=%f\n", v->d);
549        fprintf(file, "%f %f\n", v->x, v->y);
550    }
551}
552

Archive Download this file

Branches:
master



interactive