00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #include <stdlib.h>
00019 #include <stdio.h>
00020 #ifdef HAVE_MALLOC_H
00021 #include <malloc.h>
00022 #endif
00023 #include <math.h>
00024 #include "pathplan.h"
00025 #include "solvers.h"
00026
00027 #ifdef DMALLOC
00028 #include "dmalloc.h"
00029 #endif
00030
00031 #define EPSILON1 1E-3
00032 #define EPSILON2 1E-6
00033
00034 #define ABS(a) ((a) >= 0 ? (a) : -(a))
00035
00036 typedef struct tna_t {
00037 double t;
00038 Ppoint_t a[2];
00039 } tna_t;
00040
00041 #define prerror(msg) \
00042 fprintf (stderr, "libpath/%s:%d: %s\n", __FILE__, __LINE__, (msg))
00043
00044 #define DISTSQ(a, b) ( \
00045 (((a).x - (b).x) * ((a).x - (b).x)) + (((a).y - (b).y) * ((a).y - (b).y)) \
00046 )
00047
00048 #define POINTSIZE sizeof (Ppoint_t)
00049
00050 #define LT(pa, pbp) ((pa.y > pbp->y) || ((pa.y == pbp->y) && (pa.x < pbp->x)))
00051 #define GT(pa, pbp) ((pa.y < pbp->y) || ((pa.y == pbp->y) && (pa.x > pbp->x)))
00052
00053 typedef struct p2e_t {
00054 Ppoint_t *pp;
00055 Pedge_t *ep;
00056 } p2e_t;
00057
00058 typedef struct elist_t {
00059 Pedge_t *ep;
00060 struct elist_t *next, *prev;
00061 } elist_t;
00062
00063 #if 0
00064 static p2e_t *p2es;
00065 static int p2en;
00066 #endif
00067
00068 #if 0
00069 static elist_t *elist;
00070 #endif
00071
00072 static Ppoint_t *ops;
00073 static int opn, opl;
00074
00075 static int reallyroutespline(Pedge_t *, int,
00076 Ppoint_t *, int, Ppoint_t, Ppoint_t);
00077 static int mkspline(Ppoint_t *, int, tna_t *, Ppoint_t, Ppoint_t,
00078 Ppoint_t *, Ppoint_t *, Ppoint_t *, Ppoint_t *);
00079 static int splinefits(Pedge_t *, int, Ppoint_t, Pvector_t, Ppoint_t,
00080 Pvector_t, Ppoint_t *, int);
00081 static int splineisinside(Pedge_t *, int, Ppoint_t *);
00082 static int splineintersectsline(Ppoint_t *, Ppoint_t *, double *);
00083 static void points2coeff(double, double, double, double, double *);
00084 static void addroot(double, double *, int *);
00085
00086 static Pvector_t normv(Pvector_t);
00087
00088 static void growops(int);
00089
00090 static Ppoint_t add(Ppoint_t, Ppoint_t);
00091 static Ppoint_t sub(Ppoint_t, Ppoint_t);
00092 static double dist(Ppoint_t, Ppoint_t);
00093 static Ppoint_t scale(Ppoint_t, double);
00094 static double dot(Ppoint_t, Ppoint_t);
00095 static double B0(double t);
00096 static double B1(double t);
00097 static double B2(double t);
00098 static double B3(double t);
00099 static double B01(double t);
00100 static double B23(double t);
00101 #if 0
00102 static int cmpp2efunc(const void *, const void *);
00103
00104 static void listdelete(Pedge_t *);
00105 static void listreplace(Pedge_t *, Pedge_t *);
00106 static void listinsert(Pedge_t *, Ppoint_t);
00107 #endif
00108
00109 int Proutespline(Pedge_t * edges, int edgen, Ppolyline_t input,
00110 Ppoint_t * evs, Ppolyline_t * output)
00111 {
00112 #if 0
00113 Ppoint_t p0, p1, p2, p3;
00114 Ppoint_t *pp;
00115 Pvector_t v1, v2, v12, v23;
00116 int ipi, opi;
00117 int ei, p2ei;
00118 Pedge_t *e0p, *e1p;
00119 #endif
00120 Ppoint_t *inps;
00121 int inpn;
00122
00123
00124 inps = input.ps;
00125 inpn = input.pn;
00126
00127 #if 0
00128 if (!(p2es = (p2e_t *) malloc(sizeof(p2e_t) * (p2en = edgen * 2)))) {
00129 prerror("cannot malloc p2es");
00130 abort();
00131 }
00132 for (ei = 0, p2ei = 0; ei < edgen; ei++) {
00133 if (edges[ei].a.x == edges[ei].b.x
00134 && edges[ei].a.y == edges[ei].b.y)
00135 continue;
00136 p2es[p2ei].pp = &edges[ei].a;
00137 p2es[p2ei++].ep = &edges[ei];
00138 p2es[p2ei].pp = &edges[ei].b;
00139 p2es[p2ei++].ep = &edges[ei];
00140 }
00141 p2en = p2ei;
00142 qsort(p2es, p2en, sizeof(p2e_t), cmpp2efunc);
00143 elist = NULL;
00144 for (p2ei = 0; p2ei < p2en; p2ei += 2) {
00145 pp = p2es[p2ei].pp;
00146 #if DEBUG >= 1
00147 fprintf(stderr, "point: %d %lf %lf\n", p2ei, pp->x, pp->y);
00148 #endif
00149 e0p = p2es[p2ei].ep;
00150 e1p = p2es[p2ei + 1].ep;
00151 p0 = (&e0p->a == p2es[p2ei].pp) ? e0p->b : e0p->a;
00152 p1 = (&e0p->a == p2es[p2ei + 1].pp) ? e1p->b : e1p->a;
00153 if (LT(p0, pp) && LT(p1, pp)) {
00154 listdelete(e0p), listdelete(e1p);
00155 } else if (GT(p0, pp) && GT(p1, pp)) {
00156 listinsert(e0p, *pp), listinsert(e1p, *pp);
00157 } else {
00158 if (LT(p0, pp))
00159 listreplace(e0p, e1p);
00160 else
00161 listreplace(e1p, e0p);
00162 }
00163 }
00164 #endif
00165
00166 evs[0] = normv(evs[0]);
00167 evs[1] = normv(evs[1]);
00168 opl = 0;
00169 growops(4);
00170 ops[opl++] = inps[0];
00171 if (reallyroutespline(edges, edgen, inps, inpn, evs[0], evs[1]) == -1)
00172 return -1;
00173 output->pn = opl;
00174 output->ps = ops;
00175
00176 #if 0
00177 fprintf(stderr, "edge\na\nb\n");
00178 fprintf(stderr, "points\n%d\n", inpn);
00179 for (ipi = 0; ipi < inpn; ipi++)
00180 fprintf(stderr, "%f %f\n", inps[ipi].x, inps[ipi].y);
00181 fprintf(stderr, "splpoints\n%d\n", opl);
00182 for (opi = 0; opi < opl; opi++)
00183 fprintf(stderr, "%f %f\n", ops[opi].x, ops[opi].y);
00184 #endif
00185
00186 return 0;
00187 }
00188
00189 static int reallyroutespline(Pedge_t * edges, int edgen,
00190 Ppoint_t * inps, int inpn, Ppoint_t ev0,
00191 Ppoint_t ev1)
00192 {
00193 Ppoint_t p1, p2, cp1, cp2, p;
00194 Pvector_t v1, v2, splitv, splitv1, splitv2;
00195 double maxd, d, t;
00196 int maxi, i, spliti;
00197
00198 static tna_t *tnas;
00199 static int tnan;
00200
00201 if (tnan < inpn) {
00202 if (!tnas) {
00203 if (!(tnas = malloc(sizeof(tna_t) * inpn)))
00204 return -1;
00205 } else {
00206 if (!(tnas = realloc(tnas, sizeof(tna_t) * inpn)))
00207 return -1;
00208 }
00209 tnan = inpn;
00210 }
00211 tnas[0].t = 0;
00212 for (i = 1; i < inpn; i++)
00213 tnas[i].t = tnas[i - 1].t + dist(inps[i], inps[i - 1]);
00214 for (i = 1; i < inpn; i++)
00215 tnas[i].t /= tnas[inpn - 1].t;
00216 for (i = 0; i < inpn; i++) {
00217 tnas[i].a[0] = scale(ev0, B1(tnas[i].t));
00218 tnas[i].a[1] = scale(ev1, B2(tnas[i].t));
00219 }
00220 if (mkspline(inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1)
00221 return -1;
00222 if (splinefits(edges, edgen, p1, v1, p2, v2, inps, inpn))
00223 return 0;
00224 cp1 = add(p1, scale(v1, 1 / 3.0));
00225 cp2 = sub(p2, scale(v2, 1 / 3.0));
00226 for (maxd = -1, maxi = -1, i = 1; i < inpn - 1; i++) {
00227 t = tnas[i].t;
00228 p.x = B0(t) * p1.x + B1(t) * cp1.x + B2(t) * cp2.x + B3(t) * p2.x;
00229 p.y = B0(t) * p1.y + B1(t) * cp1.y + B2(t) * cp2.y + B3(t) * p2.y;
00230 if ((d = dist(p, inps[i])) > maxd)
00231 maxd = d, maxi = i;
00232 }
00233 spliti = maxi;
00234 splitv1 = normv(sub(inps[spliti], inps[spliti - 1]));
00235 splitv2 = normv(sub(inps[spliti + 1], inps[spliti]));
00236 splitv = normv(add(splitv1, splitv2));
00237 reallyroutespline(edges, edgen, inps, spliti + 1, ev0, splitv);
00238 reallyroutespline(edges, edgen, &inps[spliti], inpn - spliti, splitv,
00239 ev1);
00240 return 0;
00241 }
00242
00243 static int mkspline(Ppoint_t * inps, int inpn, tna_t * tnas, Ppoint_t ev0,
00244 Ppoint_t ev1, Ppoint_t * sp0, Ppoint_t * sv0,
00245 Ppoint_t * sp1, Ppoint_t * sv1)
00246 {
00247 Ppoint_t tmp;
00248 double c[2][2], x[2], det01, det0X, detX1;
00249 double d01, scale0, scale3;
00250 int i;
00251
00252 scale0 = scale3 = 0.0;
00253 c[0][0] = c[0][1] = c[1][0] = c[1][1] = 0.0;
00254 x[0] = x[1] = 0.0;
00255 for (i = 0; i < inpn; i++) {
00256 c[0][0] += dot(tnas[i].a[0], tnas[i].a[0]);
00257 c[0][1] += dot(tnas[i].a[0], tnas[i].a[1]);
00258 c[1][0] = c[0][1];
00259 c[1][1] += dot(tnas[i].a[1], tnas[i].a[1]);
00260 tmp = sub(inps[i], add(scale(inps[0], B01(tnas[i].t)),
00261 scale(inps[inpn - 1], B23(tnas[i].t))));
00262 x[0] += dot(tnas[i].a[0], tmp);
00263 x[1] += dot(tnas[i].a[1], tmp);
00264 }
00265 det01 = c[0][0] * c[1][1] - c[1][0] * c[0][1];
00266 det0X = c[0][0] * x[1] - c[0][1] * x[0];
00267 detX1 = x[0] * c[1][1] - x[1] * c[0][1];
00268 if (ABS(det01) >= 1e-6) {
00269 scale0 = detX1 / det01;
00270 scale3 = det0X / det01;
00271 }
00272 if (ABS(det01) < 1e-6 || scale0 <= 0.0 || scale3 <= 0.0) {
00273 d01 = dist(inps[0], inps[inpn - 1]) / 3.0;
00274 scale0 = d01;
00275 scale3 = d01;
00276 }
00277 *sp0 = inps[0];
00278 *sv0 = scale(ev0, scale0);
00279 *sp1 = inps[inpn - 1];
00280 *sv1 = scale(ev1, scale3);
00281 return 0;
00282 }
00283
00284 static double dist_n(Ppoint_t * p, int n)
00285 {
00286 int i;
00287 double rv;
00288
00289 rv = 0.0;
00290 for (i = 1; i < n; i++) {
00291 rv +=
00292 sqrt((p[i].x - p[i - 1].x) * (p[i].x - p[i - 1].x) +
00293 (p[i].y - p[i - 1].y) * (p[i].y - p[i - 1].y));
00294 }
00295 return rv;
00296 }
00297
00298 static int splinefits(Pedge_t * edges, int edgen, Ppoint_t pa,
00299 Pvector_t va, Ppoint_t pb, Pvector_t vb,
00300 Ppoint_t * inps, int inpn)
00301 {
00302 Ppoint_t sps[4];
00303 double a, b;
00304 #if 0
00305 double d;
00306 #endif
00307 int pi;
00308 int forceflag;
00309 int first = 1;
00310
00311 forceflag = (inpn == 2 ? 1 : 0);
00312
00313 #if 0
00314 d = sqrt((pb.x - pa.x) * (pb.x - pa.x) +
00315 (pb.y - pa.y) * (pb.y - pa.y));
00316 a = d, b = d;
00317 #else
00318 a = b = 4;
00319 #endif
00320 for (;;) {
00321 sps[0].x = pa.x;
00322 sps[0].y = pa.y;
00323 sps[1].x = pa.x + a * va.x / 3.0;
00324 sps[1].y = pa.y + a * va.y / 3.0;
00325 sps[2].x = pb.x - b * vb.x / 3.0;
00326 sps[2].y = pb.y - b * vb.y / 3.0;
00327 sps[3].x = pb.x;
00328 sps[3].y = pb.y;
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338 if (first && (dist_n(sps, 4) < (dist_n(inps, inpn) - EPSILON1)))
00339 return 0;
00340 first = 0;
00341
00342 if (splineisinside(edges, edgen, &sps[0])) {
00343 growops(opl + 4);
00344 for (pi = 1; pi < 4; pi++)
00345 ops[opl].x = sps[pi].x, ops[opl++].y = sps[pi].y;
00346 #if DEBUG >= 1
00347 fprintf(stderr, "success: %f %f\n", a, b);
00348 #endif
00349 return 1;
00350 }
00351 if (a == 0 && b == 0) {
00352 if (forceflag) {
00353 growops(opl + 4);
00354 for (pi = 1; pi < 4; pi++)
00355 ops[opl].x = sps[pi].x, ops[opl++].y = sps[pi].y;
00356 #if DEBUG >= 1
00357 fprintf(stderr, "forced straight line: %f %f\n", a, b);
00358 #endif
00359 return 1;
00360 }
00361 break;
00362 }
00363 if (a > .01)
00364 a /= 2, b /= 2;
00365 else
00366 a = b = 0;
00367 }
00368 #if DEBUG >= 1
00369 fprintf(stderr, "failure\n");
00370 #endif
00371 return 0;
00372 }
00373
00374 static int splineisinside(Pedge_t * edges, int edgen, Ppoint_t * sps)
00375 {
00376 double roots[4];
00377 int rooti, rootn;
00378 int ei;
00379 Ppoint_t lps[2], ip;
00380 double t, ta, tb, tc, td;
00381
00382 for (ei = 0; ei < edgen; ei++) {
00383 lps[0] = edges[ei].a, lps[1] = edges[ei].b;
00384
00385
00386 if ((rootn = splineintersectsline(sps, lps, roots)) == 4)
00387 continue;
00388 for (rooti = 0; rooti < rootn; rooti++) {
00389 if (roots[rooti] < EPSILON2 || roots[rooti] > 1 - EPSILON2)
00390 continue;
00391 t = roots[rooti];
00392 td = t * t * t;
00393 tc = 3 * t * t * (1 - t);
00394 tb = 3 * t * (1 - t) * (1 - t);
00395 ta = (1 - t) * (1 - t) * (1 - t);
00396 ip.x = ta * sps[0].x + tb * sps[1].x +
00397 tc * sps[2].x + td * sps[3].x;
00398 ip.y = ta * sps[0].y + tb * sps[1].y +
00399 tc * sps[2].y + td * sps[3].y;
00400 if (DISTSQ(ip, lps[0]) < EPSILON1 ||
00401 DISTSQ(ip, lps[1]) < EPSILON1)
00402 continue;
00403 return 0;
00404 }
00405 }
00406 return 1;
00407 }
00408
00409 static int splineintersectsline(Ppoint_t * sps, Ppoint_t * lps,
00410 double *roots)
00411 {
00412 double scoeff[4], xcoeff[2], ycoeff[2];
00413 double xroots[3], yroots[3], tv, sv, rat;
00414 int rootn, xrootn, yrootn, i, j;
00415
00416 xcoeff[0] = lps[0].x;
00417 xcoeff[1] = lps[1].x - lps[0].x;
00418 ycoeff[0] = lps[0].y;
00419 ycoeff[1] = lps[1].y - lps[0].y;
00420 rootn = 0;
00421 if (xcoeff[1] == 0) {
00422 if (ycoeff[1] == 0) {
00423 points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
00424 scoeff[0] -= xcoeff[0];
00425 xrootn = solve3(scoeff, xroots);
00426 points2coeff(sps[0].y, sps[1].y, sps[2].y, sps[3].y, scoeff);
00427 scoeff[0] -= ycoeff[0];
00428 yrootn = solve3(scoeff, yroots);
00429 if (xrootn == 4)
00430 if (yrootn == 4)
00431 return 4;
00432 else
00433 for (j = 0; j < yrootn; j++)
00434 addroot(yroots[j], roots, &rootn);
00435 else if (yrootn == 4)
00436 for (i = 0; i < xrootn; i++)
00437 addroot(xroots[i], roots, &rootn);
00438 else
00439 for (i = 0; i < xrootn; i++)
00440 for (j = 0; j < yrootn; j++)
00441 if (xroots[i] == yroots[j])
00442 addroot(xroots[i], roots, &rootn);
00443 return rootn;
00444 } else {
00445 points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
00446 scoeff[0] -= xcoeff[0];
00447 xrootn = solve3(scoeff, xroots);
00448 if (xrootn == 4)
00449 return 4;
00450 for (i = 0; i < xrootn; i++) {
00451 tv = xroots[i];
00452 if (tv >= 0 && tv <= 1) {
00453 points2coeff(sps[0].y, sps[1].y, sps[2].y, sps[3].y,
00454 scoeff);
00455 sv = scoeff[0] + tv * (scoeff[1] + tv *
00456 (scoeff[2] + tv * scoeff[3]));
00457 sv = (sv - ycoeff[0]) / ycoeff[1];
00458 if ((0 <= sv) && (sv <= 1))
00459 addroot(tv, roots, &rootn);
00460 }
00461 }
00462 return rootn;
00463 }
00464 } else {
00465 rat = ycoeff[1] / xcoeff[1];
00466 points2coeff(sps[0].y - rat * sps[0].x, sps[1].y - rat * sps[1].x,
00467 sps[2].y - rat * sps[2].x, sps[3].y - rat * sps[3].x,
00468 scoeff);
00469 scoeff[0] += rat * xcoeff[0] - ycoeff[0];
00470 xrootn = solve3(scoeff, xroots);
00471 if (xrootn == 4)
00472 return 4;
00473 for (i = 0; i < xrootn; i++) {
00474 tv = xroots[i];
00475 if (tv >= 0 && tv <= 1) {
00476 points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x,
00477 scoeff);
00478 sv = scoeff[0] + tv * (scoeff[1] +
00479 tv * (scoeff[2] + tv * scoeff[3]));
00480 sv = (sv - xcoeff[0]) / xcoeff[1];
00481 if ((0 <= sv) && (sv <= 1))
00482 addroot(tv, roots, &rootn);
00483 }
00484 }
00485 return rootn;
00486 }
00487 }
00488
00489 static void points2coeff(double v0, double v1, double v2, double v3,
00490 double *coeff)
00491 {
00492 coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
00493 coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
00494 coeff[1] = 3 * (v1 - v0);
00495 coeff[0] = v0;
00496 }
00497
00498 static void addroot(double root, double *roots, int *rootnp)
00499 {
00500 if (root >= 0 && root <= 1)
00501 roots[*rootnp] = root, (*rootnp)++;
00502 }
00503
00504 static Pvector_t normv(Pvector_t v)
00505 {
00506 double d;
00507
00508 d = v.x * v.x + v.y * v.y;
00509 if (d > 1e-6) {
00510 d = sqrt(d);
00511 v.x /= d, v.y /= d;
00512 }
00513 return v;
00514 }
00515
00516 static void growops(int newopn)
00517 {
00518 if (newopn <= opn)
00519 return;
00520 if (!ops) {
00521 if (!(ops = (Ppoint_t *) malloc(POINTSIZE * newopn))) {
00522 prerror("cannot malloc ops");
00523 abort();
00524 }
00525 } else {
00526 if (!(ops = (Ppoint_t *) realloc((void *) ops,
00527 POINTSIZE * newopn))) {
00528 prerror("cannot realloc ops");
00529 abort();
00530 }
00531 }
00532 opn = newopn;
00533 }
00534
00535 static Ppoint_t add(Ppoint_t p1, Ppoint_t p2)
00536 {
00537 p1.x += p2.x, p1.y += p2.y;
00538 return p1;
00539 }
00540
00541 static Ppoint_t sub(Ppoint_t p1, Ppoint_t p2)
00542 {
00543 p1.x -= p2.x, p1.y -= p2.y;
00544 return p1;
00545 }
00546
00547 static double dist(Ppoint_t p1, Ppoint_t p2)
00548 {
00549 double dx, dy;
00550
00551 dx = p2.x - p1.x, dy = p2.y - p1.y;
00552 return sqrt(dx * dx + dy * dy);
00553 }
00554
00555 static Ppoint_t scale(Ppoint_t p, double c)
00556 {
00557 p.x *= c, p.y *= c;
00558 return p;
00559 }
00560
00561 static double dot(Ppoint_t p1, Ppoint_t p2)
00562 {
00563 return p1.x * p2.x + p1.y * p2.y;
00564 }
00565
00566 static double B0(double t)
00567 {
00568 double tmp = 1.0 - t;
00569 return tmp * tmp * tmp;
00570 }
00571
00572 static double B1(double t)
00573 {
00574 double tmp = 1.0 - t;
00575 return 3 * t * tmp * tmp;
00576 }
00577
00578 static double B2(double t)
00579 {
00580 double tmp = 1.0 - t;
00581 return 3 * t * t * tmp;
00582 }
00583
00584 static double B3(double t)
00585 {
00586 return t * t * t;
00587 }
00588
00589 static double B01(double t)
00590 {
00591 double tmp = 1.0 - t;
00592 return tmp * tmp * (tmp + 3 * t);
00593 }
00594
00595 static double B23(double t)
00596 {
00597 double tmp = 1.0 - t;
00598 return t * t * (3 * tmp + t);
00599 }
00600
00601 #if 0
00602 static int cmpp2efunc(const void *v0p, const void *v1p)
00603 {
00604 p2e_t *p2e0p, *p2e1p;
00605 double x0, x1;
00606
00607 p2e0p = (p2e_t *) v0p, p2e1p = (p2e_t *) v1p;
00608 if (p2e0p->pp->y > p2e1p->pp->y)
00609 return -1;
00610 else if (p2e0p->pp->y < p2e1p->pp->y)
00611 return 1;
00612 if (p2e0p->pp->x < p2e1p->pp->x)
00613 return -1;
00614 else if (p2e0p->pp->x > p2e1p->pp->x)
00615 return 1;
00616 x0 = (p2e0p->pp == &p2e0p->ep->a) ? p2e0p->ep->b.x : p2e0p->ep->a.x;
00617 x1 = (p2e1p->pp == &p2e1p->ep->a) ? p2e1p->ep->b.x : p2e1p->ep->a.x;
00618 if (x0 < x1)
00619 return -1;
00620 else if (x0 > x1)
00621 return 1;
00622 return 0;
00623 }
00624
00625 static void listdelete(Pedge_t * ep)
00626 {
00627 elist_t *lp;
00628
00629 for (lp = elist; lp; lp = lp->next) {
00630 if (lp->ep != ep)
00631 continue;
00632 if (lp->prev)
00633 lp->prev->next = lp->next;
00634 if (lp->next)
00635 lp->next->prev = lp->prev;
00636 if (elist == lp)
00637 elist = lp->next;
00638 free(lp);
00639 return;
00640 }
00641 if (!lp) {
00642 prerror("cannot find list element to delete");
00643 abort();
00644 }
00645 }
00646
00647 static void listreplace(Pedge_t * oldep, Pedge_t * newep)
00648 {
00649 elist_t *lp;
00650
00651 for (lp = elist; lp; lp = lp->next) {
00652 if (lp->ep != oldep)
00653 continue;
00654 lp->ep = newep;
00655 return;
00656 }
00657 if (!lp) {
00658 prerror("cannot find list element to replace");
00659 abort();
00660 }
00661 }
00662
00663 static void listinsert(Pedge_t * ep, Ppoint_t p)
00664 {
00665 elist_t *lp, *newlp, *lastlp;
00666 double lx;
00667
00668 if (!(newlp = (elist_t *) malloc(sizeof(elist_t)))) {
00669 prerror("cannot malloc newlp");
00670 abort();
00671 }
00672 newlp->ep = ep;
00673 newlp->next = newlp->prev = NULL;
00674 if (!elist) {
00675 elist = newlp;
00676 return;
00677 }
00678 for (lp = elist; lp; lp = lp->next) {
00679 lastlp = lp;
00680 lx = lp->ep->a.x + (lp->ep->b.x - lp->ep->a.x) * (p.y -
00681 lp->ep->a.y) /
00682 (lp->ep->b.y - lp->ep->a.y);
00683 if (lx <= p.x)
00684 continue;
00685 if (lp->prev)
00686 lp->prev->next = newlp;
00687 newlp->prev = lp->prev;
00688 newlp->next = lp;
00689 lp->prev = newlp;
00690 if (elist == lp)
00691 elist = newlp;
00692 return;
00693 }
00694 lastlp->next = newlp;
00695 newlp->prev = lastlp;
00696 if (!elist)
00697 elist = newlp;
00698 }
00699 #endif