25 fprintf (stderr, "lib/pathplan/%s:%d: %s\n", __FILE__, __LINE__, (msg))
27#define POINTSIZE sizeof (Ppoint_t)
34#define POINTNLINKSIZE sizeof (pointnlink_t)
35#define POINTNLINKPSIZE sizeof (pointnlink_t *)
86 size_t trii, trij, ftrii, ltrii;
93 if (polyp->
pn > 0 && pnls ==
NULL) {
98 if (polyp->
pn > 0 && pnlps ==
NULL) {
99 prerror(
"cannot realloc pnlps");
109 prerror(
"cannot realloc dq.pnls");
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;
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) ||
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)
132 pnls[pnll].
pp = &polyp->
ps[pi];
133 pnls[pnll].
link = &pnls[pnll % polyp->
pn];
134 pnlps[pnll] = &pnls[pnll];
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)
142 pnls[pnll].
pp = &polyp->
ps[pi];
143 pnls[pnll].
link = &pnls[pnll % polyp->
pn];
144 pnlps[pnll] = &pnls[pnll];
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);
163#if defined(DEBUG) && DEBUG >= 2
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);
172 for (trii = 0; trii <
LIST_SIZE(&tris); trii++)
173 for (trij = trii + 1; trij <
LIST_SIZE(&tris); trij++)
177 for (trii = 0; trii <
LIST_SIZE(&tris); trii++)
181 prerror(
"source point not in any triangle");
188 for (trii = 0; trii <
LIST_SIZE(&tris); trii++)
192 prerror(
"destination point not in any triangle");
202 prerror(
"cannot find triangle path");
210 ops[0] = eps[0],
ops[1] = eps[1];
216 if (ftrii == ltrii) {
223 ops[0] = eps[0],
ops[1] = eps[1];
239 for (ei = 0; ei < 3; ei++)
249 pnlp = trip->
e[(ei + 1) % 3].pnl1p;
269 if (splitindex > dq.
apex)
270 dq.
apex = splitindex;
277 if (splitindex < dq.
apex)
278 dq.
apex = splitindex;
282 for (ei = 0; ei < 3; ei++)
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");
298 for (i = 0, pnlp = &epnls[1]; pnlp; pnlp = pnlp->
link)
306 for (i = i - 1, pnlp = &epnls[1]; pnlp; i--, pnlp = pnlp->
link)
319 for (
size_t pnli = 0; pnli < point_count; pnli++)
321 const size_t pnlip1 = (pnli + 1) % point_count;
322 const size_t pnlip2 = (pnli + 2) % point_count;
327 for (pnli = pnlip1; pnli < point_count - 1; pnli++)
332 prerror(
"triangulation failed");
351 prerror(
"cannot realloc tris");
363 for (ei = 0; ei < 3; ei++) {
364 for (ej = 0; ej < 3; ej++) {
382 LIST_AT(&tris, trii)->mark = 1;
385 for (ei = 0; ei < 3; ei++)
389 LIST_AT(&tris, trii)->mark = 0;
416 for (
size_t index = dq->
fpnlpi; index < dq->
apex; index++)
419 for (
size_t index = dq->
lpnlpi; index > dq->
apex; index--)
428 for (ei = 0, sum = 0; ei < 3; ei++)
432 return sum == 3 || sum == 0;
439 if (new_ops ==
NULL) {
static void mark(const stk_t *stk, Agnode_t *n)
set a mark on n
type-generic dynamically expanding list
#define LIST_AT(list, index)
#define LIST_GET(list, index)
#define LIST_TRY_APPEND(list, item)
static size_t finddqsplit(const deque_t *dq, pointnlink_t *pnlp)
static int triangulate(pointnlink_t **points, size_t point_count)
static bool marktripath(size_t trii, size_t trij)
static void connecttris(size_t tri1, size_t tri2)
static int pointintri(size_t trii, Ppoint_t *pp)
static void splitdq(deque_t *dq, int side, size_t index)
static void add2dq(deque_t *dq, int side, pointnlink_t *pnlp)
int Pshortestpath(Ppoly_t *polyp, Ppoint_t eps[2], Ppolyline_t *output)
static int growops(size_t newopn)
static int loadtriangle(pointnlink_t *pnlap, pointnlink_t *pnlbp, pointnlink_t *pnlcp)
struct pointnlink_t * link
size_t right_index
index into tris of the triangle to the right
bool isdiagonal(size_t i, size_t ip2, void *pointp, size_t pointn, indexer_t indexer)
is (i, i + 2) a diagonal?
int ccw(Ppoint_t p1, Ppoint_t p2, Ppoint_t p3)
are the given points counter-clockwise, clockwise, or co-linear?
static Ppoint_t point_indexer(void *base, size_t index)