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