Graphviz 13.1.3~dev.20250831.0023
Loading...
Searching...
No Matches
shortest.c
Go to the documentation of this file.
1/*************************************************************************
2 * Copyright (c) 2011 AT&T Intellectual Property
3 * All rights reserved. This program and the accompanying materials
4 * are made available under the terms of the Eclipse Public License v1.0
5 * which accompanies this distribution, and is available at
6 * https://www.eclipse.org/legal/epl-v10.html
7 *
8 * Contributors: Details at https://graphviz.org
9 *************************************************************************/
10
11#include <stdbool.h>
12#include <stdio.h>
13#include <stdint.h>
14#include <stdlib.h>
15#include <math.h>
16#include <pathplan/pathutil.h>
17#include <pathplan/tri.h>
18#include <util/list.h>
19#include <util/prisize_t.h>
20
21#define DQ_FRONT 1
22#define DQ_BACK 2
23
24#define prerror(msg) \
25 fprintf (stderr, "lib/pathplan/%s:%d: %s\n", __FILE__, __LINE__, (msg))
26
27#define POINTSIZE sizeof (Ppoint_t)
28
33
34#define POINTNLINKSIZE sizeof (pointnlink_t)
35#define POINTNLINKPSIZE sizeof (pointnlink_t *)
36
42
43typedef struct triangle_t {
44 int mark;
47
52
53static LIST(triangle_t) tris;
54
55static Ppoint_t *ops;
56static size_t opn;
57
58static int triangulate(pointnlink_t **, size_t);
60static void connecttris(size_t, size_t);
61static bool marktripath(size_t, size_t);
62
63static void add2dq(deque_t *dq, int, pointnlink_t*);
64static void splitdq(deque_t *dq, int, size_t);
65static size_t finddqsplit(const deque_t *dq, pointnlink_t*);
66
67static int pointintri(size_t, Ppoint_t *);
68
69static int growops(size_t);
70
71static Ppoint_t point_indexer(void *base, size_t index) {
72 pointnlink_t **b = base;
73 return *b[index]->pp;
74}
75
76/* Pshortestpath:
77 * Find a shortest path contained in the polygon polyp going between the
78 * points supplied in eps. The resulting polyline is stored in output.
79 * Return 0 on success, -1 on bad input, -2 on memory allocation problem.
80 */
81int Pshortestpath(Ppoly_t * polyp, Ppoint_t eps[2], Ppolyline_t * output)
82{
83 size_t pi, minpi;
84 double minx;
85 Ppoint_t p1, p2, p3;
86 size_t trii, trij, ftrii, ltrii;
87 int ei;
88 pointnlink_t epnls[2], *lpnlp, *rpnlp, *pnlp;
89 triangle_t *trip;
90
91 /* make space */
92 pointnlink_t *pnls = calloc(polyp->pn, sizeof(pnls[0]));
93 if (polyp->pn > 0 && pnls == NULL) {
94 prerror("cannot realloc pnls");
95 return -2;
96 }
97 pointnlink_t **pnlps = calloc(polyp->pn, sizeof(pnlps[0]));
98 if (polyp->pn > 0 && pnlps == NULL) {
99 prerror("cannot realloc pnlps");
100 free(pnls);
101 return -2;
102 }
103 size_t pnll = 0;
104 LIST_CLEAR(&tris);
105
106 deque_t dq = {.pnlpn = polyp->pn * 2};
107 dq.pnlps = calloc(dq.pnlpn, POINTNLINKPSIZE);
108 if (dq.pnlps == NULL) {
109 prerror("cannot realloc dq.pnls");
110 free(pnlps);
111 free(pnls);
112 return -2;
113 }
114 dq.fpnlpi = dq.pnlpn / 2;
115 dq.lpnlpi = dq.fpnlpi - 1;
116
117 /* make sure polygon is CCW and load pnls array */
118 for (pi = 0, minx = HUGE_VAL, minpi = SIZE_MAX; pi < polyp->pn; pi++) {
119 if (minx > polyp->ps[pi].x)
120 minx = polyp->ps[pi].x, minpi = pi;
121 }
122 p2 = polyp->ps[minpi];
123 p1 = polyp->ps[minpi == 0 ? polyp->pn - 1 : minpi - 1];
124 p3 = polyp->ps[(minpi == polyp->pn - 1) ? 0 : minpi + 1];
125 if ((p1.x == p2.x && p2.x == p3.x && p3.y > p2.y) ||
126 ccw(p1, p2, p3) != ISCCW) {
127 for (pi = polyp->pn - 1; polyp->pn > 0 && pi != SIZE_MAX; pi--) {
128 if (pi < polyp->pn - 1
129 && polyp->ps[pi].x == polyp->ps[pi + 1].x
130 && polyp->ps[pi].y == polyp->ps[pi + 1].y)
131 continue;
132 pnls[pnll].pp = &polyp->ps[pi];
133 pnls[pnll].link = &pnls[pnll % polyp->pn];
134 pnlps[pnll] = &pnls[pnll];
135 pnll++;
136 }
137 } else {
138 for (pi = 0; pi < polyp->pn; pi++) {
139 if (pi > 0 && polyp->ps[pi].x == polyp->ps[pi - 1].x &&
140 polyp->ps[pi].y == polyp->ps[pi - 1].y)
141 continue;
142 pnls[pnll].pp = &polyp->ps[pi];
143 pnls[pnll].link = &pnls[pnll % polyp->pn];
144 pnlps[pnll] = &pnls[pnll];
145 pnll++;
146 }
147 }
148
149#if defined(DEBUG) && DEBUG >= 1
150 fprintf(stderr, "points\n%" PRISIZE_T "\n", pnll);
151 for (size_t pnli = 0; pnli < pnll; pnli++)
152 fprintf(stderr, "%f %f\n", pnls[pnli].pp->x, pnls[pnli].pp->y);
153#endif
154
155 /* generate list of triangles */
156 if (triangulate(pnlps, pnll)) {
157 free(dq.pnlps);
158 free(pnlps);
159 free(pnls);
160 return -2;
161 }
162
163#if defined(DEBUG) && DEBUG >= 2
164 fprintf(stderr, "triangles\n%" PRISIZE_T "\n", LIST_SIZE(&tris));
165 for (trii = 0; trii < LIST_SIZE(&tris); trii++)
166 for (ei = 0; ei < 3; ei++)
167 fprintf(stderr, "%f %f\n", LIST_GET(&tris, trii).e[ei].pnl0p->pp->x,
168 LIST_GET(&tris, trii).e[ei].pnl0p->pp->y);
169#endif
170
171 /* connect all pairs of triangles that share an edge */
172 for (trii = 0; trii < LIST_SIZE(&tris); trii++)
173 for (trij = trii + 1; trij < LIST_SIZE(&tris); trij++)
174 connecttris(trii, trij);
175
176 /* find first and last triangles */
177 for (trii = 0; trii < LIST_SIZE(&tris); trii++)
178 if (pointintri(trii, &eps[0]))
179 break;
180 if (trii == LIST_SIZE(&tris)) {
181 prerror("source point not in any triangle");
182 free(dq.pnlps);
183 free(pnlps);
184 free(pnls);
185 return -1;
186 }
187 ftrii = trii;
188 for (trii = 0; trii < LIST_SIZE(&tris); trii++)
189 if (pointintri(trii, &eps[1]))
190 break;
191 if (trii == LIST_SIZE(&tris)) {
192 prerror("destination point not in any triangle");
193 free(dq.pnlps);
194 free(pnlps);
195 free(pnls);
196 return -1;
197 }
198 ltrii = trii;
199
200 /* mark the strip of triangles from eps[0] to eps[1] */
201 if (!marktripath(ftrii, ltrii)) {
202 prerror("cannot find triangle path");
203 free(dq.pnlps);
204 free(pnlps);
205 free(pnls);
206 /* a straight line is better than failing */
207 if (growops(2) != 0)
208 return -2;
209 output->pn = 2;
210 ops[0] = eps[0], ops[1] = eps[1];
211 output->ps = ops;
212 return 0;
213 }
214
215 /* if endpoints in same triangle, use a single line */
216 if (ftrii == ltrii) {
217 free(dq.pnlps);
218 free(pnlps);
219 free(pnls);
220 if (growops(2) != 0)
221 return -2;
222 output->pn = 2;
223 ops[0] = eps[0], ops[1] = eps[1];
224 output->ps = ops;
225 return 0;
226 }
227
228 /* build funnel and shortest path linked list (in add2dq) */
229 epnls[0].pp = &eps[0], epnls[0].link = NULL;
230 epnls[1].pp = &eps[1], epnls[1].link = NULL;
231 add2dq(&dq, DQ_FRONT, &epnls[0]);
232 dq.apex = dq.fpnlpi;
233 trii = ftrii;
234 while (trii != SIZE_MAX) {
235 trip = LIST_AT(&tris, trii);
236 trip->mark = 2;
237
238 /* find the left and right points of the exiting edge */
239 for (ei = 0; ei < 3; ei++)
240 if (trip->e[ei].right_index != SIZE_MAX && LIST_GET(&tris, trip->e[ei].right_index).mark == 1)
241 break;
242 if (ei == 3) { /* in last triangle */
243 if (ccw(eps[1], *dq.pnlps[dq.fpnlpi]->pp,
244 *dq.pnlps[dq.lpnlpi]->pp) == ISCCW)
245 lpnlp = dq.pnlps[dq.lpnlpi], rpnlp = &epnls[1];
246 else
247 lpnlp = &epnls[1], rpnlp = dq.pnlps[dq.lpnlpi];
248 } else {
249 pnlp = trip->e[(ei + 1) % 3].pnl1p;
250 if (ccw(*trip->e[ei].pnl0p->pp, *pnlp->pp,
251 *trip->e[ei].pnl1p->pp) == ISCCW)
252 lpnlp = trip->e[ei].pnl1p, rpnlp = trip->e[ei].pnl0p;
253 else
254 lpnlp = trip->e[ei].pnl0p, rpnlp = trip->e[ei].pnl1p;
255 }
256
257 /* update deque */
258 if (trii == ftrii) {
259 add2dq(&dq, DQ_BACK, lpnlp);
260 add2dq(&dq, DQ_FRONT, rpnlp);
261 } else {
262 if (dq.pnlps[dq.fpnlpi] != rpnlp
263 && dq.pnlps[dq.lpnlpi] != rpnlp) {
264 /* add right point to deque */
265 size_t splitindex = finddqsplit(&dq, rpnlp);
266 splitdq(&dq, DQ_BACK, splitindex);
267 add2dq(&dq, DQ_FRONT, rpnlp);
268 /* if the split is behind the apex, then reset apex */
269 if (splitindex > dq.apex)
270 dq.apex = splitindex;
271 } else {
272 /* add left point to deque */
273 size_t splitindex = finddqsplit(&dq, lpnlp);
274 splitdq(&dq, DQ_FRONT, splitindex);
275 add2dq(&dq, DQ_BACK, lpnlp);
276 /* if the split is in front of the apex, then reset apex */
277 if (splitindex < dq.apex)
278 dq.apex = splitindex;
279 }
280 }
281 trii = SIZE_MAX;
282 for (ei = 0; ei < 3; ei++)
283 if (trip->e[ei].right_index != SIZE_MAX && LIST_GET(&tris, trip->e[ei].right_index).mark == 1) {
284 trii = trip->e[ei].right_index;
285 break;
286 }
287 }
288
289#if defined(DEBUG) && DEBUG >= 1
290 fprintf(stderr, "polypath");
291 for (pnlp = &epnls[1]; pnlp; pnlp = pnlp->link)
292 fprintf(stderr, " %f %f", pnlp->pp->x, pnlp->pp->y);
293 fprintf(stderr, "\n");
294#endif
295
296 free(dq.pnlps);
297 size_t i;
298 for (i = 0, pnlp = &epnls[1]; pnlp; pnlp = pnlp->link)
299 i++;
300 if (growops(i) != 0) {
301 free(pnlps);
302 free(pnls);
303 return -2;
304 }
305 output->pn = i;
306 for (i = i - 1, pnlp = &epnls[1]; pnlp; i--, pnlp = pnlp->link)
307 ops[i] = *pnlp->pp;
308 output->ps = ops;
309 free(pnlps);
310 free(pnls);
311
312 return 0;
313}
314
315/* triangulate polygon */
316static int triangulate(pointnlink_t **points, size_t point_count) {
317 if (point_count > 3)
318 {
319 for (size_t pnli = 0; pnli < point_count; pnli++)
320 {
321 const size_t pnlip1 = (pnli + 1) % point_count;
322 const size_t pnlip2 = (pnli + 2) % point_count;
323 if (isdiagonal(pnli, pnlip2, points, point_count, point_indexer))
324 {
325 if (loadtriangle(points[pnli], points[pnlip1], points[pnlip2]) != 0)
326 return -1;
327 for (pnli = pnlip1; pnli < point_count - 1; pnli++)
328 points[pnli] = points[pnli + 1];
329 return triangulate(points, point_count - 1);
330 }
331 }
332 prerror("triangulation failed");
333 }
334 else {
335 if (loadtriangle(points[0], points[1], points[2]) != 0)
336 return -1;
337 }
338
339 return 0;
340}
341
342static int loadtriangle(pointnlink_t * pnlap, pointnlink_t * pnlbp,
343 pointnlink_t * pnlcp)
344{
345 triangle_t trip = {0};
346 trip.e[0].pnl0p = pnlap, trip.e[0].pnl1p = pnlbp, trip.e[0].right_index = SIZE_MAX;
347 trip.e[1].pnl0p = pnlbp, trip.e[1].pnl1p = pnlcp, trip.e[1].right_index = SIZE_MAX;
348 trip.e[2].pnl0p = pnlcp, trip.e[2].pnl1p = pnlap, trip.e[2].right_index = SIZE_MAX;
349
350 if (!LIST_TRY_APPEND(&tris, trip)) {
351 prerror("cannot realloc tris");
352 return -1;
353 }
354
355 return 0;
356}
357
358/* connect a pair of triangles at their common edge (if any) */
359static void connecttris(size_t tri1, size_t tri2) {
360 triangle_t *tri1p, *tri2p;
361 int ei, ej;
362
363 for (ei = 0; ei < 3; ei++) {
364 for (ej = 0; ej < 3; ej++) {
365 tri1p = LIST_AT(&tris, tri1);
366 tri2p = LIST_AT(&tris, tri2);
367 if ((tri1p->e[ei].pnl0p->pp == tri2p->e[ej].pnl0p->pp &&
368 tri1p->e[ei].pnl1p->pp == tri2p->e[ej].pnl1p->pp) ||
369 (tri1p->e[ei].pnl0p->pp == tri2p->e[ej].pnl1p->pp &&
370 tri1p->e[ei].pnl1p->pp == tri2p->e[ej].pnl0p->pp))
371 tri1p->e[ei].right_index = tri2, tri2p->e[ej].right_index = tri1;
372 }
373 }
374}
375
376/* find and mark path from trii, to trij */
377static bool marktripath(size_t trii, size_t trij) {
378 int ei;
379
380 if (LIST_GET(&tris, trii).mark)
381 return false;
382 LIST_AT(&tris, trii)->mark = 1;
383 if (trii == trij)
384 return true;
385 for (ei = 0; ei < 3; ei++)
386 if (LIST_GET(&tris, trii).e[ei].right_index != SIZE_MAX &&
387 marktripath(LIST_GET(&tris, trii).e[ei].right_index, trij))
388 return true;
389 LIST_AT(&tris, trii)->mark = 0;
390 return false;
391}
392
393/* add a new point to the deque, either front or back */
394static void add2dq(deque_t *dq, int side, pointnlink_t *pnlp) {
395 if (side == DQ_FRONT) {
396 if (dq->lpnlpi >= dq->fpnlpi)
397 pnlp->link = dq->pnlps[dq->fpnlpi]; /* shortest path links */
398 dq->fpnlpi--;
399 dq->pnlps[dq->fpnlpi] = pnlp;
400 } else {
401 if (dq->lpnlpi >= dq->fpnlpi)
402 pnlp->link = dq->pnlps[dq->lpnlpi]; /* shortest path links */
403 dq->lpnlpi++;
404 dq->pnlps[dq->lpnlpi] = pnlp;
405 }
406}
407
408static void splitdq(deque_t *dq, int side, size_t index) {
409 if (side == DQ_FRONT)
410 dq->lpnlpi = index;
411 else
412 dq->fpnlpi = index;
413}
414
415static size_t finddqsplit(const deque_t *dq, pointnlink_t *pnlp) {
416 for (size_t index = dq->fpnlpi; index < dq->apex; index++)
417 if (ccw(*dq->pnlps[index + 1]->pp, *dq->pnlps[index]->pp, *pnlp->pp) == ISCCW)
418 return index;
419 for (size_t index = dq->lpnlpi; index > dq->apex; index--)
420 if (ccw(*dq->pnlps[index - 1]->pp, *dq->pnlps[index]->pp, *pnlp->pp) == ISCW)
421 return index;
422 return dq->apex;
423}
424
425static int pointintri(size_t trii, Ppoint_t *pp) {
426 int ei, sum;
427
428 for (ei = 0, sum = 0; ei < 3; ei++)
429 if (ccw(*LIST_GET(&tris, trii).e[ei].pnl0p->pp,
430 *LIST_GET(&tris, trii).e[ei].pnl1p->pp, *pp) != ISCW)
431 sum++;
432 return sum == 3 || sum == 0;
433}
434
435static int growops(size_t newopn) {
436 if (newopn <= opn)
437 return 0;
438 Ppoint_t *new_ops = realloc(ops, POINTSIZE * newopn);
439 if (new_ops == NULL) {
440 prerror("cannot realloc ops");
441 return -1;
442 }
443 ops = new_ops;
444 opn = newopn;
445
446 return 0;
447}
void free(void *)
#define SIZE_MAX
Definition gmlscan.c:347
node NULL
Definition grammar.y:181
static gdPoint * points
static void mark(const stk_t *stk, Agnode_t *n)
set a mark on n
Definition ccomps.c:36
type-generic dynamically expanding list
#define LIST_AT(list, index)
Definition list.h:178
#define LIST(type)
Definition list.h:55
#define LIST_SIZE(list)
Definition list.h:80
#define LIST_CLEAR(list)
Definition list.h:249
#define LIST_GET(list, index)
Definition list.h:165
#define LIST_TRY_APPEND(list, item)
Definition list.h:106
#define PRISIZE_T
Definition prisize_t.h:25
static size_t opn
Definition route.c:34
static Ppoint_t * ops
Definition route.c:33
static size_t finddqsplit(const deque_t *dq, pointnlink_t *pnlp)
Definition shortest.c:415
static int triangulate(pointnlink_t **points, size_t point_count)
Definition shortest.c:316
static bool marktripath(size_t trii, size_t trij)
Definition shortest.c:377
#define DQ_FRONT
Definition shortest.c:21
#define POINTSIZE
Definition shortest.c:27
static void connecttris(size_t tri1, size_t tri2)
Definition shortest.c:359
static int pointintri(size_t trii, Ppoint_t *pp)
Definition shortest.c:425
static void splitdq(deque_t *dq, int side, size_t index)
Definition shortest.c:408
static void add2dq(deque_t *dq, int side, pointnlink_t *pnlp)
Definition shortest.c:394
int Pshortestpath(Ppoly_t *polyp, Ppoint_t eps[2], Ppolyline_t *output)
Definition shortest.c:81
#define DQ_BACK
Definition shortest.c:22
#define POINTNLINKPSIZE
Definition shortest.c:35
#define prerror(msg)
Definition shortest.c:24
static int growops(size_t newopn)
Definition shortest.c:435
static int loadtriangle(pointnlink_t *pnlap, pointnlink_t *pnlbp, pointnlink_t *pnlcp)
Definition shortest.c:342
size_t pn
Definition pathgeom.h:47
Ppoint_t * ps
Definition pathgeom.h:46
double x
Definition pathgeom.h:38
double y
Definition pathgeom.h:38
size_t lpnlpi
Definition shortest.c:50
size_t pnlpn
Definition shortest.c:50
size_t fpnlpi
Definition shortest.c:50
size_t apex
Definition shortest.c:50
pointnlink_t ** pnlps
Definition shortest.c:49
pointnlink_t * pnl0p
Definition shortest.c:38
size_t right_index
index into tris of the triangle to the right
Definition shortest.c:40
pointnlink_t * pnl1p
Definition shortest.c:39
tedge_t e[3]
Definition shortest.c:45
bool isdiagonal(size_t i, size_t ip2, void *pointp, size_t pointn, indexer_t indexer)
is (i, i + 2) a diagonal?
Definition triang.c:91
@ ISCW
clockwise
Definition tri.h:42
@ ISCCW
counter-clockwise
Definition tri.h:41
int ccw(Ppoint_t p1, Ppoint_t p2, Ppoint_t p3)
are the given points counter-clockwise, clockwise, or co-linear?
Definition triang.c:23
static Ppoint_t point_indexer(void *base, size_t index)
Definition triang.c:28