← Back to branch summary

~ubuntu-branches/ubuntu/lucid/graphviz/lucid-security

« back to all changes in this revision

Viewing changes to pathplan/route.c

  • Committer: Bazaar Package Importer
  • Author(s): Stephen M Moraco
  • Date: 2002-02-05 18:52:12 UTC
  • Revision ID: james.westby@ubuntu.com-20020205185212-8i04c70te00rc40y
Tags: upstream-1.7.16
Import upstream version 1.7.16

Show diffs side-by-side

added added

removed removed

Lines of Context:
pathplan/route.c
 
1
/*
 
2
    This software may only be used by you under license from AT&T Corp.
 
3
    ("AT&T").  A copy of AT&T's Source Code Agreement is available at
 
4
    AT&T's Internet website having the URL:
 
5
    <http://www.research.att.com/sw/tools/graphviz/license/source.html>
 
6
    If you received this software without first entering into a license
 
7
    with AT&T, you have an infringing copy of this software and cannot use
 
8
    it without violating AT&T's intellectual property rights.
 
9
*/
 
10
#pragma prototyped
 
11
 
 
12
#include <stdlib.h>
 
13
#include <stdio.h>
 
14
#include <malloc.h>
 
15
#include <math.h>
 
16
#include "pathplan.h"
 
17
#include "solvers.h"
 
18
 
 
19
#ifdef DMALLOC
 
20
#include "dmalloc.h"
 
21
#endif
 
22
 
 
23
#define EPSILON1 1E-6
 
24
#define EPSILON2 1E-10
 
25
 
 
26
#define ABS(a) ((a) >= 0 ? (a) : -(a))
 
27
 
 
28
typedef struct tna_t {
 
29
    double t;
 
30
    Ppoint_t a[2];
 
31
} tna_t;
 
32
 
 
33
#define prerror(msg) \
 
34
        fprintf (stderr, "libpath/%s:%d: %s\n", __FILE__, __LINE__, (msg))
 
35
 
 
36
#define DISTSQ(a, b) ( \
 
37
    (((a).x - (b).x) * ((a).x - (b).x)) + (((a).y - (b).y) * ((a).y - (b).y)) \
 
38
)
 
39
 
 
40
#define POINTSIZE sizeof (Ppoint_t)
 
41
 
 
42
#define LT(pa, pbp) ((pa.y > pbp->y) || ((pa.y == pbp->y) && (pa.x < pbp->x)))
 
43
#define GT(pa, pbp) ((pa.y < pbp->y) || ((pa.y == pbp->y) && (pa.x > pbp->x)))
 
44
 
 
45
typedef struct p2e_t {
 
46
    Ppoint_t *pp;
 
47
    Pedge_t *ep;
 
48
} p2e_t;
 
49
 
 
50
typedef struct elist_t {
 
51
    Pedge_t *ep;
 
52
    struct elist_t *next, *prev;
 
53
} elist_t;
 
54
 
 
55
#if 0
 
56
static p2e_t *p2es;
 
57
static int p2en;
 
58
#endif
 
59
 
 
60
#if 0
 
61
static elist_t *elist;
 
62
#endif
 
63
 
 
64
static Ppoint_t *ops;
 
65
static int opn, opl;
 
66
 
 
67
static int reallyroutespline (Pedge_t *, int,
 
68
        Ppoint_t *, int, Ppoint_t, Ppoint_t);
 
69
static int mkspline (Ppoint_t *, int, tna_t *, Ppoint_t, Ppoint_t,
 
70
        Ppoint_t *, Ppoint_t *, Ppoint_t *, Ppoint_t *);
 
71
static int splinefits (Pedge_t *, int, Ppoint_t, Pvector_t, Ppoint_t, Pvector_t, Ppoint_t *, int);
 
72
static int splineisinside (Pedge_t *, int, Ppoint_t *);
 
73
static int splineintersectsline (Ppoint_t *, Ppoint_t *, double *);
 
74
static void points2coeff (double, double, double, double, double *);
 
75
static void addroot (double, double *, int *);
 
76
 
 
77
static Pvector_t normv (Pvector_t);
 
78
 
 
79
static void growops (int);
 
80
 
 
81
static Ppoint_t add (Ppoint_t, Ppoint_t);
 
82
static Ppoint_t sub (Ppoint_t, Ppoint_t);
 
83
static double dist (Ppoint_t, Ppoint_t);
 
84
static Ppoint_t scale (Ppoint_t, double);
 
85
static double dot (Ppoint_t, Ppoint_t);
 
86
static double B0 (double t);
 
87
static double B1 (double t);
 
88
static double B2 (double t);
 
89
static double B3 (double t);
 
90
static double B01 (double t);
 
91
static double B23 (double t);
 
92
#if 0
 
93
static int cmpp2efunc (const void *, const void *);
 
94
 
 
95
static void listdelete (Pedge_t *);
 
96
static void listreplace (Pedge_t *, Pedge_t *);
 
97
static void listinsert (Pedge_t *, Ppoint_t);
 
98
#endif
 
99
 
 
100
int Proutespline (Pedge_t *edges, int edgen, Ppolyline_t input,
 
101
        Ppoint_t *evs, Ppolyline_t *output) {
 
102
#if 0
 
103
    Ppoint_t p0, p1, p2, p3;
 
104
    Ppoint_t *pp;
 
105
    Pvector_t v1, v2, v12, v23;
 
106
    int ipi, opi;
 
107
    int ei, p2ei;
 
108
    Pedge_t *e0p, *e1p;
 
109
#endif
 
110
        Ppoint_t        *inps;
 
111
        int                     inpn;
 
112
 
 
113
        /* unpack into previous format rather than modify legacy code */
 
114
        inps = input.ps;
 
115
        inpn = input.pn;
 
116
 
 
117
#if 0
 
118
    if (!(p2es = (p2e_t *) malloc (sizeof (p2e_t) * (p2en = edgen * 2)))) {
 
119
        prerror ("cannot malloc p2es");
 
120
        abort ();
 
121
    }
 
122
    for (ei = 0, p2ei = 0; ei < edgen; ei++) {
 
123
        if (edges[ei].a.x == edges[ei].b.x && edges[ei].a.y == edges[ei].b.y)
 
124
            continue;
 
125
        p2es[p2ei].pp = &edges[ei].a;
 
126
        p2es[p2ei++].ep = &edges[ei];
 
127
        p2es[p2ei].pp = &edges[ei].b;
 
128
        p2es[p2ei++].ep = &edges[ei];
 
129
    }
 
130
    p2en = p2ei;
 
131
    qsort (p2es, p2en, sizeof (p2e_t), cmpp2efunc);
 
132
    elist = NULL;
 
133
    for (p2ei = 0; p2ei < p2en; p2ei += 2) {
 
134
        pp = p2es[p2ei].pp;
 
135
#if DEBUG >= 1
 
136
fprintf (stderr, "point: %d %lf %lf\n", p2ei, pp->x, pp->y);
 
137
#endif
 
138
        e0p = p2es[p2ei].ep;
 
139
        e1p = p2es[p2ei + 1].ep;
 
140
        p0 = (&e0p->a == p2es[p2ei].pp) ? e0p->b : e0p->a;
 
141
        p1 = (&e0p->a == p2es[p2ei + 1].pp) ? e1p->b : e1p->a;
 
142
        if (LT (p0, pp) && LT (p1, pp)) {
 
143
            listdelete (e0p), listdelete (e1p);
 
144
        } else if (GT (p0, pp) && GT (p1, pp)) {
 
145
            listinsert (e0p, *pp), listinsert (e1p, *pp);
 
146
        } else {
 
147
            if (LT (p0, pp))
 
148
                listreplace (e0p, e1p);
 
149
            else
 
150
                listreplace (e1p, e0p);
 
151
        }
 
152
    }
 
153
#endif
 
154
    /* generate the splines */
 
155
    evs[0] = normv (evs[0]);
 
156
    evs[1] = normv (evs[1]);
 
157
    opl = 0;
 
158
    growops (4);
 
159
    ops[opl++] = inps[0];
 
160
    if (reallyroutespline (edges, edgen, inps, inpn, evs[0], evs[1]) == -1)
 
161
        return -1;
 
162
        output->pn = opl;
 
163
        output->ps = ops;
 
164
 
 
165
#if 0
 
166
    fprintf (stderr, "edge\na\nb\n");
 
167
    fprintf (stderr, "points\n%d\n", inpn);
 
168
    for (ipi = 0; ipi < inpn; ipi++)
 
169
        fprintf (stderr, "%f %f\n", inps[ipi].x, inps[ipi].y);
 
170
    fprintf (stderr, "splpoints\n%d\n", opl);
 
171
    for (opi = 0; opi < opl; opi++)
 
172
        fprintf (stderr, "%f %f\n", ops[opi].x, ops[opi].y);
 
173
#endif
 
174
 
 
175
    return 0;
 
176
}
 
177
 
 
178
static int reallyroutespline (Pedge_t *edges, int edgen,
 
179
        Ppoint_t *inps, int inpn, Ppoint_t ev0, Ppoint_t ev1) {
 
180
    Ppoint_t p1, p2, cp1, cp2, p;
 
181
    Pvector_t v1, v2, splitv, splitv1, splitv2;
 
182
    double maxd, d, t;
 
183
    int maxi, i, spliti;
 
184
 
 
185
    static tna_t *tnas;
 
186
    static int tnan;
 
187
 
 
188
    if (tnan < inpn) {
 
189
        if (!tnas) {
 
190
            if (!(tnas = malloc (sizeof (tna_t) * inpn)))
 
191
                return -1;
 
192
        } else {
 
193
            if (!(tnas = realloc (tnas, sizeof (tna_t) * inpn)))
 
194
                return -1;
 
195
        }
 
196
        tnan = inpn;
 
197
    }
 
198
    tnas[0].t = 0;
 
199
    for (i = 1; i < inpn; i++)
 
200
        tnas[i].t = tnas[i - 1].t + dist (inps[i], inps[i - 1]);
 
201
    for (i = 1; i < inpn; i++)
 
202
        tnas[i].t /= tnas[inpn - 1].t;
 
203
    for (i = 0; i < inpn; i++) {
 
204
        tnas[i].a[0] = scale (ev0, B1 (tnas[i].t));
 
205
        tnas[i].a[1] = scale (ev1, B2 (tnas[i].t));
 
206
    }
 
207
    if (mkspline (inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1)
 
208
        return -1;
 
209
    if (splinefits (edges, edgen, p1, v1, p2, v2, inps, inpn))
 
210
        return 0;
 
211
    cp1 = add (p1, scale (v1, 1 / 3.0));
 
212
    cp2 = sub (p2, scale (v2, 1 / 3.0));
 
213
    for (maxd = -1, maxi = -1, i = 1; i < inpn - 1; i++) {
 
214
        t = tnas[i].t;
 
215
        p.x = B0 (t) * p1.x + B1 (t) * cp1.x +
 
216
                B2 (t) * cp2.x + B3 (t) * p2.x;
 
217
        p.y = B0 (t) * p1.y + B1 (t) * cp1.y +
 
218
                B2 (t) * cp2.y + B3 (t) * p2.y;
 
219
        if ((d = dist (p, inps[i])) > maxd)
 
220
            maxd = d, maxi = i;
 
221
    }
 
222
    spliti = maxi;
 
223
    splitv1 = normv (sub (inps[spliti], inps[spliti - 1]));
 
224
    splitv2 = normv (sub (inps[spliti + 1], inps[spliti]));
 
225
    splitv = normv (add (splitv1, splitv2));
 
226
    reallyroutespline (edges, edgen, inps, spliti + 1, ev0, splitv);
 
227
    reallyroutespline (edges, edgen, &inps[spliti], inpn - spliti, splitv, ev1);
 
228
    return 0;
 
229
}
 
230
 
 
231
static int mkspline (Ppoint_t *inps, int inpn, tna_t *tnas, Ppoint_t ev0, Ppoint_t ev1,
 
232
        Ppoint_t *sp0, Ppoint_t *sv0, Ppoint_t *sp1, Ppoint_t *sv1) {
 
233
    Ppoint_t tmp;
 
234
    double c[2][2], x[2], det01, det0X, detX1;
 
235
    double d01, scale0, scale3;
 
236
    int i;
 
237
 
 
238
    scale0 = scale3 = 0.0;
 
239
    c[0][0] = c[0][1] = c[1][0] = c[1][1] = 0.0;
 
240
    x[0] = x[1] = 0.0;
 
241
    for (i = 0; i < inpn; i++) {
 
242
        c[0][0] += dot (tnas[i].a[0], tnas[i].a[0]);
 
243
        c[0][1] += dot (tnas[i].a[0], tnas[i].a[1]);
 
244
        c[1][0] = c[0][1];
 
245
        c[1][1] += dot (tnas[i].a[1], tnas[i].a[1]);
 
246
        tmp = sub (inps[i], add (scale (inps[0], B01 (tnas[i].t)),
 
247
                scale (inps[inpn - 1], B23 (tnas[i].t))));
 
248
        x[0] += dot (tnas[i].a[0], tmp);
 
249
        x[1] += dot (tnas[i].a[1], tmp);
 
250
    }
 
251
    det01 = c[0][0] * c[1][1] - c[1][0] * c[0][1];
 
252
    det0X = c[0][0] * x[1] - c[0][1] * x[0];
 
253
    detX1 = x[0] * c[1][1] - x[1] * c[0][1];
 
254
    if (det01 != 0.0) {
 
255
        scale0 = detX1 / det01;
 
256
        scale3 = det0X / det01;
 
257
    }
 
258
    if (ABS (det01) < 1e-6 || scale0 <= 0.0 || scale3 <= 0.0) {
 
259
        d01 = dist (inps[0], inps[inpn - 1]) / 3.0;
 
260
        scale0 = d01;
 
261
        scale3 = d01;
 
262
    }
 
263
    *sp0 = inps[0];
 
264
    *sv0 = scale (ev0, scale0);
 
265
    *sp1 = inps[inpn - 1];
 
266
    *sv1 = scale (ev1, scale3);
 
267
    return 0;
 
268
}
 
269
 
 
270
static double dist_n(Ppoint_t *p, int n)
 
271
{
 
272
        int             i;
 
273
        double  rv;
 
274
 
 
275
        rv = 0.0;
 
276
        for (i = 1; i < n; i++) {
 
277
                rv += sqrt((p[i].x - p[i-1].x)*(p[i].x - p[i-1].x) + (p[i].y - p[i-1].y)*(p[i].y - p[i-1].y));
 
278
        }
 
279
        return rv;
 
280
}
 
281
 
 
282
static int splinefits (Pedge_t *edges, int edgen, Ppoint_t pa, Pvector_t va,
 
283
        Ppoint_t pb, Pvector_t vb, Ppoint_t *inps, int inpn) {
 
284
    Ppoint_t sps[4];
 
285
    double a, b;
 
286
#if 0
 
287
    double d;
 
288
#endif
 
289
    int pi;
 
290
        int     forceflag;
 
291
        int first = 1;
 
292
 
 
293
        forceflag = (inpn == 2 ? 1 : 0);
 
294
 
 
295
#if 0
 
296
    d = sqrt ((pb.x - pa.x) * (pb.x - pa.x) + (pb.y - pa.y) * (pb.y - pa.y));
 
297
    a = d, b = d;
 
298
#else
 
299
    a = b = 4;
 
300
#endif
 
301
    for ( ;; ) {
 
302
        sps[0].x = pa.x;
 
303
        sps[0].y = pa.y;
 
304
        sps[1].x = pa.x + a * va.x / 3.0;
 
305
        sps[1].y = pa.y + a * va.y / 3.0;
 
306
        sps[2].x = pb.x - b * vb.x / 3.0;
 
307
        sps[2].y = pb.y - b * vb.y / 3.0;
 
308
        sps[3].x = pb.x;
 
309
        sps[3].y = pb.y;
 
310
 
 
311
                /* shortcuts (paths shorter than the shortest path) not allowed -
 
312
                 * they must be outside the constraint polygon.  this can happen
 
313
                 * if the candidate spline intersects the constraint polygon exactly
 
314
                 * on sides or vertices.  maybe this could be more elegant, but
 
315
                 * it solves the immediate problem. we could also try jittering the
 
316
                 * constraint polygon, or computing the candidate spline more carefully,
 
317
                 * for example using the path. SCN */
 
318
 
 
319
                if (first && (dist_n(sps,4) < (dist_n(inps,inpn) - EPSILON1))) return 0;
 
320
                first = 0;
 
321
 
 
322
        if (splineisinside (edges, edgen, &sps[0])) {
 
323
            growops (opl + 4);
 
324
            for (pi = 1; pi < 4; pi++)
 
325
                ops[opl].x = sps[pi].x, ops[opl++].y = sps[pi].y;
 
326
#if DEBUG >= 1
 
327
fprintf (stderr, "success: %f %f\n", a, b);
 
328
#endif
 
329
            return 1;
 
330
        }
 
331
        if (a == 0 && b == 0) {
 
332
            if (forceflag) {
 
333
                growops (opl + 4);
 
334
                for (pi = 1; pi < 4; pi++)
 
335
                    ops[opl].x = sps[pi].x, ops[opl++].y = sps[pi].y;
 
336
#if DEBUG >= 1
 
337
fprintf (stderr, "forced straight line: %f %f\n", a, b);
 
338
#endif
 
339
                return 1;
 
340
            }
 
341
            break;
 
342
        }
 
343
        if (a > .01)
 
344
            a /= 2, b /= 2;
 
345
        else
 
346
            a = b = 0;
 
347
    }
 
348
#if DEBUG >= 1
 
349
fprintf (stderr, "failure\n");
 
350
#endif
 
351
    return 0;
 
352
}
 
353
 
 
354
static int splineisinside (Pedge_t *edges, int edgen, Ppoint_t *sps) {
 
355
    double roots[4];
 
356
    int rooti, rootn;
 
357
    int ei;
 
358
    Ppoint_t lps[2], ip;
 
359
    double t, ta, tb, tc, td;
 
360
 
 
361
    for (ei = 0; ei < edgen; ei++) {
 
362
        lps[0] = edges[ei].a, lps[1] = edges[ei].b;
 
363
        /* if ((rootn = splineintersectsline (sps, lps, roots)) == 4)
 
364
            return 1; */
 
365
        if ((rootn = splineintersectsline (sps, lps, roots)) == 4)
 
366
            continue;
 
367
        for (rooti = 0; rooti < rootn; rooti++) {
 
368
            if (roots[rooti] < EPSILON2 || roots[rooti] > 1 - EPSILON2)
 
369
                continue;
 
370
            t = roots[rooti];
 
371
            td = t * t * t;
 
372
            tc = 3 * t * t * (1 - t);
 
373
            tb = 3 * t * (1 - t) * (1 - t);
 
374
            ta = (1 - t) * (1 - t) * (1 - t);
 
375
            ip.x = ta * sps[0].x + tb * sps[1].x +
 
376
                    tc * sps[2].x + td * sps[3].x;
 
377
            ip.y = ta * sps[0].y + tb * sps[1].y +
 
378
                    tc * sps[2].y + td * sps[3].y;
 
379
            if (DISTSQ (ip, lps[0]) < EPSILON1 ||
 
380
                    DISTSQ (ip, lps[1]) < EPSILON1)
 
381
                continue;
 
382
            return 0;
 
383
        }
 
384
    }
 
385
    return 1;
 
386
}
 
387
 
 
388
static int splineintersectsline (Ppoint_t *sps, Ppoint_t *lps,
 
389
        double *roots) {
 
390
    double scoeff[4], xcoeff[2], ycoeff[2];
 
391
    double xroots[3], yroots[3], tv, sv, rat;
 
392
    int rootn, xrootn, yrootn, i, j;
 
393
 
 
394
    xcoeff[0] = lps[0].x;
 
395
    xcoeff[1] = lps[1].x - lps[0].x;
 
396
    ycoeff[0] = lps[0].y;
 
397
    ycoeff[1] = lps[1].y - lps[0].y;
 
398
    rootn = 0;
 
399
    if (xcoeff[1] == 0) {
 
400
        if (ycoeff[1] == 0) {
 
401
            points2coeff (sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
 
402
            scoeff[0] -= xcoeff[0];
 
403
            xrootn = solve3 (scoeff, xroots);
 
404
            points2coeff (sps[0].y, sps[1].y, sps[2].y, sps[3].y, scoeff);
 
405
            scoeff[0] -= ycoeff[0];
 
406
            yrootn = solve3 (scoeff, yroots);
 
407
            if (xrootn == 4)
 
408
                if (yrootn == 4)
 
409
                    return 4;
 
410
                else
 
411
                    for (j = 0; j < yrootn; j++)
 
412
                        addroot (yroots[j], roots, &rootn);
 
413
            else if (yrootn == 4)
 
414
                for (i = 0; i < xrootn; i++)
 
415
                    addroot (xroots[i], roots, &rootn);
 
416
            else
 
417
                for (i = 0; i < xrootn; i++)
 
418
                    for (j = 0; j < yrootn; j++)
 
419
                        if (xroots[i] == yroots[j])
 
420
                            addroot (xroots[i], roots, &rootn);
 
421
            return rootn;
 
422
        } else {
 
423
            points2coeff (sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
 
424
            scoeff[0] -= xcoeff[0];
 
425
            xrootn = solve3 (scoeff, xroots);
 
426
            if (xrootn == 4)
 
427
                return 4;
 
428
            for (i = 0; i < xrootn; i++) {
 
429
                tv = xroots[i];
 
430
                if (tv >= 0 && tv <= 1) {
 
431
                    points2coeff (sps[0].y, sps[1].y, sps[2].y, sps[3].y,
 
432
                            scoeff);
 
433
                    sv = scoeff[0] + tv * (scoeff[1] + tv *
 
434
                        (scoeff[2] + tv * scoeff[3]));
 
435
                    sv = (sv - ycoeff[0]) / ycoeff[1];
 
436
                    if ((0 <= sv) && (sv <= 1))
 
437
                        addroot (tv, roots, &rootn);
 
438
                }
 
439
            }
 
440
            return rootn;
 
441
        }
 
442
    } else {
 
443
        rat = ycoeff[1] / xcoeff[1];
 
444
        points2coeff (sps[0].y - rat * sps[0].x, sps[1].y - rat * sps[1].x,
 
445
                sps[2].y - rat * sps[2].x, sps[3].y - rat * sps[3].x, scoeff);
 
446
        scoeff[0] += rat * xcoeff[0] - ycoeff[0];
 
447
        xrootn = solve3 (scoeff, xroots);
 
448
        if (xrootn == 4)
 
449
            return 4;
 
450
        for (i = 0; i < xrootn; i++) {
 
451
            tv = xroots[i];
 
452
            if (tv >= 0 && tv <= 1) {
 
453
                points2coeff (sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
 
454
                sv = scoeff[0] + tv * (scoeff[1] + tv *
 
455
                    (scoeff[2] + tv * scoeff[3]));
 
456
                sv = (sv - xcoeff[0]) / xcoeff[1];
 
457
                if ((0 <= sv) && (sv <= 1))
 
458
                    addroot (tv, roots, &rootn);
 
459
            }
 
460
        }
 
461
        return rootn;
 
462
    }
 
463
}
 
464
 
 
465
static void points2coeff (double v0, double v1, double v2, double v3,
 
466
        double *coeff) {
 
467
    coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
 
468
    coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
 
469
    coeff[1] = 3 * (v1 - v0);
 
470
    coeff[0] = v0;
 
471
}
 
472
 
 
473
static void addroot (double root, double *roots, int *rootnp) {
 
474
    if (root >= 0 && root <= 1)
 
475
        roots[*rootnp] = root, (*rootnp)++;
 
476
}
 
477
 
 
478
static Pvector_t normv (Pvector_t v) {
 
479
    double d;
 
480
 
 
481
    d = sqrt (v.x * v.x + v.y * v.y);
 
482
    if (d != 0)
 
483
        v.x /= d, v.y /= d;
 
484
    return v;
 
485
}
 
486
 
 
487
static void growops (int newopn) {
 
488
    if (newopn <= opn)
 
489
        return;
 
490
    if (!ops) {
 
491
        if (!(ops = (Ppoint_t *) malloc (POINTSIZE * newopn))) {
 
492
            prerror ("cannot malloc ops");
 
493
            abort ();
 
494
        }
 
495
    } else {
 
496
        if (!(ops = (Ppoint_t *) realloc ((void *) ops,
 
497
                POINTSIZE * newopn))) {
 
498
            prerror ("cannot realloc ops");
 
499
            abort ();
 
500
        }
 
501
    }
 
502
    opn = newopn;
 
503
}
 
504
 
 
505
static Ppoint_t add (Ppoint_t p1, Ppoint_t p2) {
 
506
    p1.x += p2.x, p1.y += p2.y;
 
507
    return p1;
 
508
}
 
509
 
 
510
static Ppoint_t sub (Ppoint_t p1, Ppoint_t p2) {
 
511
    p1.x -= p2.x, p1.y -= p2.y;
 
512
    return p1;
 
513
}
 
514
 
 
515
static double dist (Ppoint_t p1, Ppoint_t p2) {
 
516
    double dx, dy;
 
517
 
 
518
    dx = p2.x - p1.x, dy = p2.y - p1.y;
 
519
    return sqrt (dx * dx + dy * dy);
 
520
}
 
521
 
 
522
static Ppoint_t scale (Ppoint_t p, double c) {
 
523
    p.x *= c, p.y *= c;
 
524
    return p;
 
525
}
 
526
 
 
527
static double dot (Ppoint_t p1, Ppoint_t p2) {
 
528
    return p1.x * p2.x + p1.y * p2.y;
 
529
}
 
530
 
 
531
static double B0 (double t)
 
532
{
 
533
    double tmp = 1.0 - t;
 
534
    return tmp * tmp * tmp;
 
535
}
 
536
 
 
537
static double B1 (double t)
 
538
{
 
539
    double tmp = 1.0 - t;
 
540
    return 3 * t * tmp * tmp;
 
541
}
 
542
 
 
543
static double B2 (double t)
 
544
{
 
545
    double tmp = 1.0 - t;
 
546
    return 3 * t * t * tmp;
 
547
}
 
548
 
 
549
static double B3 (double t)
 
550
{
 
551
    return t * t * t;
 
552
}
 
553
 
 
554
static double B01 (double t)
 
555
{
 
556
    double tmp = 1.0 - t;
 
557
    return tmp * tmp * (tmp + 3 * t);
 
558
}
 
559
 
 
560
static double B23 (double t)
 
561
{
 
562
    double tmp = 1.0 - t;
 
563
    return t * t * (3 * tmp + t);
 
564
}
 
565
 
 
566
#if 0
 
567
static int cmpp2efunc (const void *v0p, const void *v1p) {
 
568
    p2e_t *p2e0p, *p2e1p;
 
569
    double x0, x1;
 
570
 
 
571
    p2e0p = (p2e_t *) v0p, p2e1p = (p2e_t *) v1p;
 
572
    if (p2e0p->pp->y > p2e1p->pp->y)
 
573
        return -1;
 
574
    else if (p2e0p->pp->y < p2e1p->pp->y)
 
575
        return 1;
 
576
    if (p2e0p->pp->x < p2e1p->pp->x)
 
577
        return -1;
 
578
    else if (p2e0p->pp->x > p2e1p->pp->x)
 
579
        return 1;
 
580
    x0 = (p2e0p->pp == &p2e0p->ep->a) ? p2e0p->ep->b.x : p2e0p->ep->a.x;
 
581
    x1 = (p2e1p->pp == &p2e1p->ep->a) ? p2e1p->ep->b.x : p2e1p->ep->a.x;
 
582
    if (x0 < x1)
 
583
        return -1;
 
584
    else if (x0 > x1)
 
585
        return 1;
 
586
    return 0;
 
587
}
 
588
 
 
589
static void listdelete (Pedge_t *ep) {
 
590
    elist_t *lp;
 
591
 
 
592
    for (lp = elist; lp; lp = lp->next) {
 
593
        if (lp->ep != ep)
 
594
            continue;
 
595
        if (lp->prev)
 
596
            lp->prev->next = lp->next;
 
597
        if (lp->next)
 
598
            lp->next->prev = lp->prev;
 
599
        if (elist == lp)
 
600
            elist = lp->next;
 
601
        free (lp);
 
602
        return;
 
603
    }
 
604
    if (!lp) {
 
605
        prerror ("cannot find list element to delete");
 
606
        abort ();
 
607
    }
 
608
}
 
609
 
 
610
static void listreplace (Pedge_t *oldep, Pedge_t *newep) {
 
611
    elist_t *lp;
 
612
 
 
613
    for (lp = elist; lp; lp = lp->next) {
 
614
        if (lp->ep != oldep)
 
615
            continue;
 
616
        lp->ep = newep;
 
617
        return;
 
618
    }
 
619
    if (!lp) {
 
620
        prerror ("cannot find list element to replace");
 
621
        abort ();
 
622
    }
 
623
}
 
624
 
 
625
static void listinsert (Pedge_t *ep, Ppoint_t p) {
 
626
    elist_t *lp, *newlp, *lastlp;
 
627
    double lx;
 
628
 
 
629
    if (!(newlp = (elist_t *) malloc (sizeof (elist_t)))) {
 
630
        prerror ("cannot malloc newlp");
 
631
        abort ();
 
632
    }
 
633
    newlp->ep = ep;
 
634
    newlp->next = newlp->prev = NULL;
 
635
    if (!elist) {
 
636
        elist = newlp;
 
637
        return;
 
638
    }
 
639
    for (lp = elist; lp; lp = lp->next) {
 
640
        lastlp = lp;
 
641
        lx = lp->ep->a.x + (lp->ep->b.x - lp->ep->a.x) * (p.y - lp->ep->a.y) /
 
642
                (lp->ep->b.y - lp->ep->a.y);
 
643
        if (lx <= p.x)
 
644
            continue;
 
645
        if (lp->prev)
 
646
            lp->prev->next = newlp;
 
647
        newlp->prev = lp->prev;
 
648
        newlp->next = lp;
 
649
        lp->prev = newlp;
 
650
        if (elist == lp)
 
651
            elist = newlp;
 
652
        return;
 
653
    }
 
654
    lastlp->next = newlp;
 
655
    newlp->prev = lastlp;
 
656
    if (!elist)
 
657
        elist = newlp;
 
658
}
 
659
#endif