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 <limits.h>
00024 #include <math.h>
00025 #include "pathplan.h"
00026
00027 #ifdef DMALLOC
00028 #include "dmalloc.h"
00029 #endif
00030
00031 #define ISCCW 1
00032 #define ISCW 2
00033 #define ISON 3
00034
00035 #define DQ_FRONT 1
00036 #define DQ_BACK 2
00037
00038 #ifndef TRUE
00039 #define TRUE 1
00040 #define FALSE 0
00041 #endif
00042
00043 #define prerror(msg) \
00044 fprintf (stderr, "libpath/%s:%d: %s\n", __FILE__, __LINE__, (msg))
00045
00046 #define POINTSIZE sizeof (Ppoint_t)
00047
00048 typedef struct pointnlink_t {
00049 Ppoint_t *pp;
00050 struct pointnlink_t *link;
00051 } pointnlink_t;
00052
00053 #define POINTNLINKSIZE sizeof (pointnlink_t)
00054 #define POINTNLINKPSIZE sizeof (pointnlink_t *)
00055
00056 typedef struct tedge_t {
00057 pointnlink_t *pnl0p;
00058 pointnlink_t *pnl1p;
00059 struct triangle_t *ltp;
00060 struct triangle_t *rtp;
00061 } tedge_t;
00062
00063 typedef struct triangle_t {
00064 int mark;
00065 struct tedge_t e[3];
00066 } triangle_t;
00067
00068 #define TRIANGLESIZE sizeof (triangle_t)
00069
00070 typedef struct deque_t {
00071 pointnlink_t **pnlps;
00072 int pnlpn, fpnlpi, lpnlpi, apex;
00073 } deque_t;
00074
00075 static pointnlink_t *pnls, **pnlps;
00076 static int pnln, pnll;
00077
00078 static triangle_t *tris;
00079 static int trin, tril;
00080
00081 static deque_t dq;
00082
00083 static Ppoint_t *ops;
00084 static int opn;
00085
00086 static void triangulate(pointnlink_t **, int);
00087 static int isdiagonal(int, int, pointnlink_t **, int);
00088 static void loadtriangle(pointnlink_t *, pointnlink_t *, pointnlink_t *);
00089 static void connecttris(int, int);
00090 static int marktripath(int, int);
00091
00092 static void add2dq(int, pointnlink_t *);
00093 static void splitdq(int, int);
00094 static int finddqsplit(pointnlink_t *);
00095
00096 static int ccw(Ppoint_t *, Ppoint_t *, Ppoint_t *);
00097 static int intersects(Ppoint_t *, Ppoint_t *, Ppoint_t *, Ppoint_t *);
00098 static int between(Ppoint_t *, Ppoint_t *, Ppoint_t *);
00099 static int pointintri(int, Ppoint_t *);
00100
00101 static void growpnls(int);
00102 static void growtris(int);
00103 static void growdq(int);
00104 static void growops(int);
00105
00106 int Pshortestpath(Ppoly_t * polyp, Ppoint_t * eps, Ppolyline_t * output)
00107 {
00108 int pi, minpi;
00109 double minx;
00110 Ppoint_t p1, p2, p3;
00111 int trii, trij, ftrii, ltrii;
00112 int ei;
00113 pointnlink_t epnls[2], *lpnlp, *rpnlp, *pnlp;
00114 triangle_t *trip;
00115 int splitindex;
00116 #ifdef DEBUG
00117 int pnli;
00118 #endif
00119
00120
00121 growpnls(polyp->pn);
00122 pnll = 0;
00123 tril = 0;
00124 growdq(polyp->pn * 2);
00125 dq.fpnlpi = dq.pnlpn / 2, dq.lpnlpi = dq.fpnlpi - 1;
00126
00127
00128 for (pi = 0, minx = HUGE_VAL, minpi = -1; pi < polyp->pn; pi++) {
00129 if (minx > polyp->ps[pi].x)
00130 minx = polyp->ps[pi].x, minpi = pi;
00131 }
00132 p2 = polyp->ps[minpi];
00133 p1 = polyp->ps[((minpi == 0) ? polyp->pn - 1 : minpi - 1)];
00134 p3 = polyp->ps[((minpi == polyp->pn - 1) ? 0 : minpi + 1)];
00135 if (((p1.x == p2.x && p2.x == p3.x) && (p3.y > p2.y)) ||
00136 ccw(&p1, &p2, &p3) != ISCCW) {
00137 for (pi = polyp->pn - 1; pi >= 0; pi--) {
00138 if (pi < polyp->pn - 1
00139 && polyp->ps[pi].x == polyp->ps[pi + 1].x
00140 && polyp->ps[pi].y == polyp->ps[pi + 1].y)
00141 continue;
00142 pnls[pnll].pp = &polyp->ps[pi];
00143 pnls[pnll].link = &pnls[pnll % polyp->pn];
00144 pnlps[pnll] = &pnls[pnll];
00145 pnll++;
00146 }
00147 } else {
00148 for (pi = 0; pi < polyp->pn; pi++) {
00149 if (pi > 0 && polyp->ps[pi].x == polyp->ps[pi - 1].x &&
00150 polyp->ps[pi].y == polyp->ps[pi - 1].y)
00151 continue;
00152 pnls[pnll].pp = &polyp->ps[pi];
00153 pnls[pnll].link = &pnls[pnll % polyp->pn];
00154 pnlps[pnll] = &pnls[pnll];
00155 pnll++;
00156 }
00157 }
00158
00159 #if DEBUG >= 1
00160 fprintf(stderr, "points\n%d\n", pnll);
00161 for (pnli = 0; pnli < pnll; pnli++)
00162 fprintf(stderr, "%f %f\n", pnls[pnli].pp->x, pnls[pnli].pp->y);
00163 #endif
00164
00165
00166 triangulate(pnlps, pnll);
00167
00168 #if DEBUG >= 2
00169 fprintf(stderr, "triangles\n%d\n", tril);
00170 for (trii = 0; trii < tril; trii++)
00171 for (ei = 0; ei < 3; ei++)
00172 fprintf(stderr, "%f %f\n", tris[trii].e[ei].pnl0p->pp->x,
00173 tris[trii].e[ei].pnl0p->pp->y);
00174 #endif
00175
00176
00177 for (trii = 0; trii < tril; trii++)
00178 for (trij = trii + 1; trij < tril; trij++)
00179 connecttris(trii, trij);
00180
00181
00182 for (trii = 0; trii < tril; trii++)
00183 if (pointintri(trii, &eps[0]))
00184 break;
00185 if (trii == tril) {
00186 prerror("source point not in any triangle");
00187 return -1;
00188 }
00189 ftrii = trii;
00190 for (trii = 0; trii < tril; trii++)
00191 if (pointintri(trii, &eps[1]))
00192 break;
00193 if (trii == tril) {
00194 prerror("destination point not in any triangle");
00195 return -1;
00196 }
00197 ltrii = trii;
00198
00199
00200 if (!marktripath(ftrii, ltrii)) {
00201 prerror("cannot find triangle path");
00202 abort();
00203 }
00204
00205
00206 if (ftrii == ltrii) {
00207 growops(2);
00208 output->pn = 2;
00209 ops[0] = eps[0], ops[1] = eps[1];
00210 output->ps = ops;
00211 return 0;
00212 }
00213
00214
00215 epnls[0].pp = &eps[0], epnls[0].link = NULL;
00216 epnls[1].pp = &eps[1], epnls[1].link = NULL;
00217 add2dq(DQ_FRONT, &epnls[0]);
00218 dq.apex = dq.fpnlpi;
00219 trii = ftrii;
00220 while (trii != -1) {
00221 trip = &tris[trii];
00222 trip->mark = 2;
00223
00224
00225 for (ei = 0; ei < 3; ei++)
00226 if (trip->e[ei].rtp && trip->e[ei].rtp->mark == 1)
00227 break;
00228 if (ei == 3) {
00229 if (ccw(&eps[1], dq.pnlps[dq.fpnlpi]->pp,
00230 dq.pnlps[dq.lpnlpi]->pp) == ISCCW)
00231 lpnlp = dq.pnlps[dq.lpnlpi], rpnlp = &epnls[1];
00232 else
00233 lpnlp = &epnls[1], rpnlp = dq.pnlps[dq.lpnlpi];
00234 } else {
00235 pnlp = trip->e[(ei + 1) % 3].pnl1p;
00236 if (ccw(trip->e[ei].pnl0p->pp, pnlp->pp,
00237 trip->e[ei].pnl1p->pp) == ISCCW)
00238 lpnlp = trip->e[ei].pnl1p, rpnlp = trip->e[ei].pnl0p;
00239 else
00240 lpnlp = trip->e[ei].pnl0p, rpnlp = trip->e[ei].pnl1p;
00241 }
00242
00243
00244 if (trii == ftrii) {
00245 add2dq(DQ_BACK, lpnlp);
00246 add2dq(DQ_FRONT, rpnlp);
00247 } else {
00248 if (dq.pnlps[dq.fpnlpi] != rpnlp
00249 && dq.pnlps[dq.lpnlpi] != rpnlp) {
00250
00251 splitindex = finddqsplit(rpnlp);
00252 splitdq(DQ_BACK, splitindex);
00253 add2dq(DQ_FRONT, rpnlp);
00254
00255 if (splitindex > dq.apex)
00256 dq.apex = splitindex;
00257 } else {
00258
00259 splitindex = finddqsplit(lpnlp);
00260 splitdq(DQ_FRONT, splitindex);
00261 add2dq(DQ_BACK, lpnlp);
00262
00263 if (splitindex < dq.apex)
00264 dq.apex = splitindex;
00265 }
00266 }
00267 trii = -1;
00268 for (ei = 0; ei < 3; ei++)
00269 if (trip->e[ei].rtp && trip->e[ei].rtp->mark == 1) {
00270 trii = trip->e[ei].rtp - tris;
00271 break;
00272 }
00273 }
00274
00275 #if DEBUG >= 1
00276 fprintf(stderr, "polypath");
00277 for (pnlp = &epnls[1]; pnlp; pnlp = pnlp->link)
00278 fprintf(stderr, " %f %f", pnlp->pp->x, pnlp->pp->y);
00279 fprintf(stderr, "\n");
00280 #endif
00281
00282 for (pi = 0, pnlp = &epnls[1]; pnlp; pnlp = pnlp->link)
00283 pi++;
00284 growops(pi);
00285 output->pn = pi;
00286 for (pi = pi - 1, pnlp = &epnls[1]; pnlp; pi--, pnlp = pnlp->link)
00287 ops[pi] = *pnlp->pp;
00288 output->ps = ops;
00289
00290 return 0;
00291 }
00292
00293
00294 static void triangulate(pointnlink_t ** pnlps, int pnln)
00295 {
00296 int pnli, pnlip1, pnlip2;
00297
00298 if (pnln > 3) {
00299 for (pnli = 0; pnli < pnln; pnli++) {
00300 pnlip1 = (pnli + 1) % pnln;
00301 pnlip2 = (pnli + 2) % pnln;
00302 if (isdiagonal(pnli, pnlip2, pnlps, pnln)) {
00303 loadtriangle(pnlps[pnli], pnlps[pnlip1], pnlps[pnlip2]);
00304 for (pnli = pnlip1; pnli < pnln - 1; pnli++)
00305 pnlps[pnli] = pnlps[pnli + 1];
00306 triangulate(pnlps, pnln - 1);
00307 return;
00308 }
00309 }
00310 abort();
00311 } else
00312 loadtriangle(pnlps[0], pnlps[1], pnlps[2]);
00313 }
00314
00315
00316 static int isdiagonal(int pnli, int pnlip2, pointnlink_t ** pnlps,
00317 int pnln)
00318 {
00319 int pnlip1, pnlim1, pnlj, pnljp1, res;
00320
00321
00322 pnlip1 = (pnli + 1) % pnln;
00323 pnlim1 = (pnli + pnln - 1) % pnln;
00324
00325 if (ccw(pnlps[pnlim1]->pp, pnlps[pnli]->pp, pnlps[pnlip1]->pp) ==
00326 ISCCW)
00327 res =
00328 (ccw(pnlps[pnli]->pp, pnlps[pnlip2]->pp, pnlps[pnlim1]->pp) ==
00329 ISCCW)
00330 && (ccw(pnlps[pnlip2]->pp, pnlps[pnli]->pp, pnlps[pnlip1]->pp)
00331 == ISCCW);
00332
00333 else
00334 res = (ccw(pnlps[pnli]->pp, pnlps[pnlip2]->pp,
00335 pnlps[pnlip1]->pp) == ISCW);
00336 if (!res)
00337 return FALSE;
00338
00339
00340 for (pnlj = 0; pnlj < pnln; pnlj++) {
00341 pnljp1 = (pnlj + 1) % pnln;
00342 if (!((pnlj == pnli) || (pnljp1 == pnli) ||
00343 (pnlj == pnlip2) || (pnljp1 == pnlip2)))
00344 if (intersects(pnlps[pnli]->pp, pnlps[pnlip2]->pp,
00345 pnlps[pnlj]->pp, pnlps[pnljp1]->pp))
00346 return FALSE;
00347 }
00348 return TRUE;
00349 }
00350
00351 static void loadtriangle(pointnlink_t * pnlap, pointnlink_t * pnlbp,
00352 pointnlink_t * pnlcp)
00353 {
00354 triangle_t *trip;
00355 int ei;
00356
00357
00358 if (tril >= trin)
00359 growtris(trin + 20);
00360 trip = &tris[tril++];
00361 trip->mark = 0;
00362 trip->e[0].pnl0p = pnlap, trip->e[0].pnl1p = pnlbp, trip->e[0].rtp =
00363 NULL;
00364 trip->e[1].pnl0p = pnlbp, trip->e[1].pnl1p = pnlcp, trip->e[1].rtp =
00365 NULL;
00366 trip->e[2].pnl0p = pnlcp, trip->e[2].pnl1p = pnlap, trip->e[2].rtp =
00367 NULL;
00368 for (ei = 0; ei < 3; ei++)
00369 trip->e[ei].ltp = trip;
00370 }
00371
00372
00373 static void connecttris(int tri1, int tri2)
00374 {
00375 triangle_t *tri1p, *tri2p;
00376 int ei, ej;
00377
00378 for (ei = 0; ei < 3; ei++) {
00379 for (ej = 0; ej < 3; ej++) {
00380 tri1p = &tris[tri1], tri2p = &tris[tri2];
00381 if ((tri1p->e[ei].pnl0p->pp == tri2p->e[ej].pnl0p->pp &&
00382 tri1p->e[ei].pnl1p->pp == tri2p->e[ej].pnl1p->pp) ||
00383 (tri1p->e[ei].pnl0p->pp == tri2p->e[ej].pnl1p->pp &&
00384 tri1p->e[ei].pnl1p->pp == tri2p->e[ej].pnl0p->pp))
00385 tri1p->e[ei].rtp = tri2p, tri2p->e[ej].rtp = tri1p;
00386 }
00387 }
00388 }
00389
00390
00391 static int marktripath(int trii, int trij)
00392 {
00393 int ei;
00394
00395 if (tris[trii].mark)
00396 return FALSE;
00397 tris[trii].mark = 1;
00398 if (trii == trij)
00399 return TRUE;
00400 for (ei = 0; ei < 3; ei++)
00401 if (tris[trii].e[ei].rtp &&
00402 marktripath(tris[trii].e[ei].rtp - tris, trij))
00403 return TRUE;
00404 tris[trii].mark = 0;
00405 return FALSE;
00406 }
00407
00408
00409 static void add2dq(int side, pointnlink_t * pnlp)
00410 {
00411 if (side == DQ_FRONT) {
00412 if (dq.lpnlpi - dq.fpnlpi >= 0)
00413 pnlp->link = dq.pnlps[dq.fpnlpi];
00414 dq.fpnlpi--;
00415 dq.pnlps[dq.fpnlpi] = pnlp;
00416 } else {
00417 if (dq.lpnlpi - dq.fpnlpi >= 0)
00418 pnlp->link = dq.pnlps[dq.lpnlpi];
00419 dq.lpnlpi++;
00420 dq.pnlps[dq.lpnlpi] = pnlp;
00421 }
00422 }
00423
00424 static void splitdq(int side, int index)
00425 {
00426 if (side == DQ_FRONT)
00427 dq.lpnlpi = index;
00428 else
00429 dq.fpnlpi = index;
00430 }
00431
00432 static int finddqsplit(pointnlink_t * pnlp)
00433 {
00434 int index;
00435
00436 for (index = dq.fpnlpi; index < dq.apex; index++)
00437 if (ccw(dq.pnlps[index + 1]->pp, dq.pnlps[index]->pp, pnlp->pp) ==
00438 ISCCW)
00439 return index;
00440 for (index = dq.lpnlpi; index > dq.apex; index--)
00441 if (ccw(dq.pnlps[index - 1]->pp, dq.pnlps[index]->pp, pnlp->pp) ==
00442 ISCW)
00443 return index;
00444 return dq.apex;
00445 }
00446
00447
00448 static int ccw(Ppoint_t * p1p, Ppoint_t * p2p, Ppoint_t * p3p)
00449 {
00450 double d;
00451
00452 d = ((p1p->y - p2p->y) * (p3p->x - p2p->x)) -
00453 ((p3p->y - p2p->y) * (p1p->x - p2p->x));
00454 return (d > 0) ? ISCCW : ((d < 0) ? ISCW : ISON);
00455 }
00456
00457
00458 static int intersects(Ppoint_t * pap, Ppoint_t * pbp,
00459 Ppoint_t * pcp, Ppoint_t * pdp)
00460 {
00461 int ccw1, ccw2, ccw3, ccw4;
00462
00463 if (ccw(pap, pbp, pcp) == ISON || ccw(pap, pbp, pdp) == ISON ||
00464 ccw(pcp, pdp, pap) == ISON || ccw(pcp, pdp, pbp) == ISON) {
00465 if (between(pap, pbp, pcp) || between(pap, pbp, pdp) ||
00466 between(pcp, pdp, pap) || between(pcp, pdp, pbp))
00467 return TRUE;
00468 } else {
00469 ccw1 = (ccw(pap, pbp, pcp) == ISCCW) ? 1 : 0;
00470 ccw2 = (ccw(pap, pbp, pdp) == ISCCW) ? 1 : 0;
00471 ccw3 = (ccw(pcp, pdp, pap) == ISCCW) ? 1 : 0;
00472 ccw4 = (ccw(pcp, pdp, pbp) == ISCCW) ? 1 : 0;
00473 return (ccw1 ^ ccw2) && (ccw3 ^ ccw4);
00474 }
00475 return FALSE;
00476 }
00477
00478
00479 static int between(Ppoint_t * pap, Ppoint_t * pbp, Ppoint_t * pcp)
00480 {
00481 Ppoint_t p1, p2;
00482
00483 p1.x = pbp->x - pap->x, p1.y = pbp->y - pap->y;
00484 p2.x = pcp->x - pap->x, p2.y = pcp->y - pap->y;
00485 if (ccw(pap, pbp, pcp) != ISON)
00486 return FALSE;
00487 return (p2.x * p1.x + p2.y * p1.y >= 0) &&
00488 (p2.x * p2.x + p2.y * p2.y <= p1.x * p1.x + p1.y * p1.y);
00489 }
00490
00491 static int pointintri(int trii, Ppoint_t * pp)
00492 {
00493 int ei, sum;
00494
00495 for (ei = 0, sum = 0; ei < 3; ei++)
00496 if (ccw(tris[trii].e[ei].pnl0p->pp,
00497 tris[trii].e[ei].pnl1p->pp, pp) != ISCW)
00498 sum++;
00499 return (sum == 3 || sum == 0);
00500 }
00501
00502 static void growpnls(int newpnln)
00503 {
00504 if (newpnln <= pnln)
00505 return;
00506 if (!pnls) {
00507 if (!(pnls = (pointnlink_t *) malloc(POINTNLINKSIZE * newpnln))) {
00508 prerror("cannot malloc pnls");
00509 abort();
00510 }
00511 if (!(pnlps = (pointnlink_t **) malloc(POINTNLINKPSIZE * newpnln))) {
00512 prerror("cannot malloc pnlps");
00513 abort();
00514 }
00515 } else {
00516 if (!(pnls = (pointnlink_t *) realloc((void *) pnls,
00517 POINTNLINKSIZE * newpnln))) {
00518 prerror("cannot realloc pnls");
00519 abort();
00520 }
00521 if (!(pnlps = (pointnlink_t **) realloc((void *) pnlps,
00522 POINTNLINKPSIZE *
00523 newpnln))) {
00524 prerror("cannot realloc pnlps");
00525 abort();
00526 }
00527 }
00528 pnln = newpnln;
00529 }
00530
00531 static void growtris(int newtrin)
00532 {
00533 if (newtrin <= trin)
00534 return;
00535 if (!tris) {
00536 if (!(tris = (triangle_t *) malloc(TRIANGLESIZE * newtrin))) {
00537 prerror("cannot malloc tris");
00538 abort();
00539 }
00540 } else {
00541 if (!(tris = (triangle_t *) realloc((void *) tris,
00542 TRIANGLESIZE * newtrin))) {
00543 prerror("cannot realloc tris");
00544 abort();
00545 }
00546 }
00547 trin = newtrin;
00548 }
00549
00550 static void growdq(int newdqn)
00551 {
00552 if (newdqn <= dq.pnlpn)
00553 return;
00554 if (!dq.pnlps) {
00555 if (!
00556 (dq.pnlps =
00557 (pointnlink_t **) malloc(POINTNLINKPSIZE * newdqn))) {
00558 prerror("cannot malloc dq.pnls");
00559 abort();
00560 }
00561 } else {
00562 if (!(dq.pnlps = (pointnlink_t **) realloc((void *) dq.pnlps,
00563 POINTNLINKPSIZE *
00564 newdqn))) {
00565 prerror("cannot realloc dq.pnls");
00566 abort();
00567 }
00568 }
00569 dq.pnlpn = newdqn;
00570 }
00571
00572 static void growops(int newopn)
00573 {
00574 if (newopn <= opn)
00575 return;
00576 if (!ops) {
00577 if (!(ops = (Ppoint_t *) malloc(POINTSIZE * newopn))) {
00578 prerror("cannot malloc ops");
00579 abort();
00580 }
00581 } else {
00582 if (!(ops = (Ppoint_t *) realloc((void *) ops,
00583 POINTSIZE * newopn))) {
00584 prerror("cannot realloc ops");
00585 abort();
00586 }
00587 }
00588 opn = newopn;
00589 }