Graphviz 16.1.3~dev.20261121.1645
Loading...
Searching...
No Matches
route.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 v2.0
5 * which accompanies this distribution, and is available at
6 * https://www.eclipse.org/org/documents/epl-2.0/EPL-2.0.html
7 *
8 * Contributors: Details at https://graphviz.org
9 *************************************************************************/
10
11#include "config.h"
12
13#include <assert.h>
14#include <limits.h>
15#include <math.h>
16#include <pathplan/pathutil.h>
17#include <pathplan/solvers.h>
18#include <stdbool.h>
19#include <stdio.h>
20#include <stdlib.h>
21#include <util/list.h>
22
23#define EPSILON1 1E-3
24#define EPSILON2 1E-6
25
26typedef struct tna_t {
27 double t;
30
31#define DISTSQ(a, b) \
32 ((((a).x - (b).x) * ((a).x - (b).x)) + (((a).y - (b).y) * ((a).y - (b).y)))
33
34typedef LIST(Ppoint_t) ops_t;
35
36static ops_t ops;
37
38static int reallyroutespline(Pedge_t *, size_t, Ppoint_t *, int, Ppoint_t,
39 Ppoint_t);
40static int mkspline(Ppoint_t *, int, const tna_t *, Ppoint_t, Ppoint_t,
42static int splinefits(Pedge_t *, size_t, Ppoint_t, Pvector_t, Ppoint_t,
43 Pvector_t, Ppoint_t *, int);
44static int splineisinside(Pedge_t *, size_t, Ppoint_t *);
45static int splineintersectsline(Ppoint_t *, Ppoint_t *, double *);
46static void points2coeff(double, double, double, double, double *);
47static void addroot(double, double *, int *);
48
50
53static double dist(Ppoint_t, Ppoint_t);
54static Ppoint_t scale(Ppoint_t, double);
55static double dot(Ppoint_t, Ppoint_t);
56static double B0(double t);
57static double B1(double t);
58static double B2(double t);
59static double B3(double t);
60static double B01(double t);
61static double B23(double t);
62
63/* Given a set of barrier line segments edges as obstacles, a template
64 * path input_route, and endpoint vectors endpoint_slopes, construct a spline
65 * fitting the input and endpoint vectors, and return in output_route.
66 * Return 0 on success and -1 on failure, including no memory.
67 */
68int Proutespline(Pedge_t *barriers, size_t n_barriers, Ppolyline_t input_route,
69 Ppoint_t endpoint_slopes[2], Ppolyline_t *output_route) {
70 Ppoint_t *inps;
71 int inpn;
72
73 /* unpack into previous format rather than modify legacy code */
74 inps = input_route.ps;
75 assert(input_route.pn <= INT_MAX);
76 inpn = (int)input_route.pn;
77
78 /* generate the splines */
79 endpoint_slopes[0] = normv(endpoint_slopes[0]);
80 endpoint_slopes[1] = normv(endpoint_slopes[1]);
81 LIST_CLEAR(&ops);
82 if (!LIST_TRY_APPEND(&ops, inps[0])) {
83 return -1;
84 }
85 if (reallyroutespline(barriers, n_barriers, inps, inpn, endpoint_slopes[0],
86 endpoint_slopes[1]) == -1)
87 return -1;
88 output_route->pn = LIST_SIZE(&ops);
89 output_route->ps = LIST_FRONT(&ops);
90
91 return 0;
92}
93
94static int reallyroutespline(Pedge_t *edges, size_t edgen, Ppoint_t *inps,
95 int inpn, Ppoint_t ev0, Ppoint_t ev1) {
96 Ppoint_t p1, p2;
97 Pvector_t v1, v2;
98 double d;
99
100 assert(inpn > 0);
101 tna_t *const tnas = calloc((size_t)inpn, sizeof(tna_t));
102 if (tnas == NULL) {
103 return -1;
104 }
105 tnas[0].t = 0;
106 for (int i = 1; i < inpn; i++)
107 tnas[i].t = tnas[i - 1].t + dist(inps[i], inps[i - 1]);
108 for (int i = 1; i < inpn; i++)
109 tnas[i].t /= tnas[inpn - 1].t;
110 for (int i = 0; i < inpn; i++) {
111 tnas[i].a[0] = scale(ev0, B1(tnas[i].t));
112 tnas[i].a[1] = scale(ev1, B2(tnas[i].t));
113 }
114 if (mkspline(inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1) {
115 free(tnas);
116 return -1;
117 }
118 int fit = splinefits(edges, edgen, p1, v1, p2, v2, inps, inpn);
119 if (fit > 0) {
120 free(tnas);
121 return 0;
122 }
123 if (fit < 0) {
124 free(tnas);
125 return -1;
126 }
127 const Ppoint_t cp1 = add(p1, scale(v1, 1 / 3.0));
128 const Ppoint_t cp2 = sub(p2, scale(v2, 1 / 3.0));
129 int maxi = -1;
130 double maxd = -1;
131 for (int i = 1; i < inpn - 1; i++) {
132 const double t = tnas[i].t;
133 const Ppoint_t p = {
134 .x = B0(t) * p1.x + B1(t) * cp1.x + B2(t) * cp2.x + B3(t) * p2.x,
135 .y = B0(t) * p1.y + B1(t) * cp1.y + B2(t) * cp2.y + B3(t) * p2.y};
136 if ((d = dist(p, inps[i])) > maxd) {
137 maxd = d;
138 maxi = i;
139 }
140 }
141 free(tnas);
142 if (maxi < 0) {
143 return -1;
144 }
145 const int spliti = maxi;
146 const Pvector_t splitv1 = normv(sub(inps[spliti], inps[spliti - 1]));
147 const Pvector_t splitv2 = normv(sub(inps[spliti + 1], inps[spliti]));
148 const Pvector_t splitv = normv(add(splitv1, splitv2));
149 if (reallyroutespline(edges, edgen, inps, spliti + 1, ev0, splitv) < 0) {
150 return -1;
151 }
152 if (reallyroutespline(edges, edgen, &inps[spliti], inpn - spliti, splitv,
153 ev1) < 0) {
154 return -1;
155 }
156 return 0;
157}
158
159static int mkspline(Ppoint_t *inps, int inpn, const tna_t *tnas, Ppoint_t ev0,
160 Ppoint_t ev1, Ppoint_t *sp0, Ppoint_t *sv0, Ppoint_t *sp1,
161 Ppoint_t *sv1) {
162 Ppoint_t tmp;
163 double c[2][2], x[2], det01, det0X, detX1;
164 double d01, scale0, scale3;
165 int i;
166
167 scale0 = scale3 = 0.0;
168 c[0][0] = c[0][1] = c[1][0] = c[1][1] = 0.0;
169 x[0] = x[1] = 0.0;
170 for (i = 0; i < inpn; i++) {
171 c[0][0] += dot(tnas[i].a[0], tnas[i].a[0]);
172 c[0][1] += dot(tnas[i].a[0], tnas[i].a[1]);
173 c[1][0] = c[0][1];
174 c[1][1] += dot(tnas[i].a[1], tnas[i].a[1]);
175 tmp = sub(inps[i], add(scale(inps[0], B01(tnas[i].t)),
176 scale(inps[inpn - 1], B23(tnas[i].t))));
177 x[0] += dot(tnas[i].a[0], tmp);
178 x[1] += dot(tnas[i].a[1], tmp);
179 }
180 det01 = c[0][0] * c[1][1] - c[1][0] * c[0][1];
181 det0X = c[0][0] * x[1] - c[0][1] * x[0];
182 detX1 = x[0] * c[1][1] - x[1] * c[0][1];
183 if (fabs(det01) >= 1e-6) {
184 scale0 = detX1 / det01;
185 scale3 = det0X / det01;
186 }
187 if (fabs(det01) < 1e-6 || scale0 <= 0.0 || scale3 <= 0.0) {
188 d01 = dist(inps[0], inps[inpn - 1]) / 3.0;
189 scale0 = d01;
190 scale3 = d01;
191 }
192 *sp0 = inps[0];
193 *sv0 = scale(ev0, scale0);
194 *sp1 = inps[inpn - 1];
195 *sv1 = scale(ev1, scale3);
196 return 0;
197}
198
199static double dist_n(Ppoint_t *p, int n) {
200 int i;
201 double rv;
202
203 rv = 0.0;
204 for (i = 1; i < n; i++) {
205 rv += hypot(p[i].x - p[i - 1].x, p[i].y - p[i - 1].y);
206 }
207 return rv;
208}
209
210static int splinefits(Pedge_t *edges, size_t edgen, Ppoint_t pa, Pvector_t va,
211 Ppoint_t pb, Pvector_t vb, Ppoint_t *inps, int inpn) {
212 Ppoint_t sps[4];
213 double a;
214 int pi;
215 int forceflag;
216 int first = 1;
217
218 forceflag = (inpn == 2 ? 1 : 0);
219
220 a = 4;
221 for (;;) {
222 sps[0].x = pa.x;
223 sps[0].y = pa.y;
224 sps[1].x = pa.x + a * va.x / 3.0;
225 sps[1].y = pa.y + a * va.y / 3.0;
226 sps[2].x = pb.x - a * vb.x / 3.0;
227 sps[2].y = pb.y - a * vb.y / 3.0;
228 sps[3].x = pb.x;
229 sps[3].y = pb.y;
230
231 /* shortcuts (paths shorter than the shortest path) not allowed -
232 * they must be outside the constraint polygon. this can happen
233 * if the candidate spline intersects the constraint polygon exactly
234 * on sides or vertices. maybe this could be more elegant, but
235 * it solves the immediate problem. we could also try jittering the
236 * constraint polygon, or computing the candidate spline more carefully,
237 * for example using the path. SCN */
238
239 if (first && (dist_n(sps, 4) < (dist_n(inps, inpn) - EPSILON1)))
240 return 0;
241 first = 0;
242
243 if (splineisinside(edges, edgen, &sps[0])) {
244 for (pi = 1; pi < 4; pi++) {
245 if (!LIST_TRY_APPEND(&ops, sps[pi])) {
246 return -1;
247 }
248 }
249#if defined(DEBUG) && DEBUG >= 1
250 fprintf(stderr, "success: %f %f\n", a, a);
251#endif
252 return 1;
253 }
254 // is `a` 0, accounting for the precision with which it was computed (on the
255 // last loop iteration) below?
256 if (a < 0.005) {
257 if (forceflag) {
258 for (pi = 1; pi < 4; pi++) {
259 if (!LIST_TRY_APPEND(&ops, sps[pi])) {
260 return -1;
261 }
262 }
263#if defined(DEBUG) && DEBUG >= 1
264 fprintf(stderr, "forced straight line: %f %f\n", a, a);
265#endif
266 return 1;
267 }
268 break;
269 }
270 if (a > .01)
271 a /= 2;
272 else
273 a = 0;
274 }
275#if defined(DEBUG) && DEBUG >= 1
276 fprintf(stderr, "failure\n");
277#endif
278 return 0;
279}
280
281static int splineisinside(Pedge_t *edges, size_t edgen, Ppoint_t *sps) {
282 double roots[4];
283 int rooti, rootn;
284 Ppoint_t lps[2], ip;
285 double t, ta, tb, tc, td;
286
287 for (size_t ei = 0; ei < edgen; ei++) {
288 lps[0] = edges[ei].a, lps[1] = edges[ei].b;
289 if ((rootn = splineintersectsline(sps, lps, roots)) == 4)
290 continue;
291 for (rooti = 0; rooti < rootn; rooti++) {
292 if (roots[rooti] < EPSILON2 || roots[rooti] > 1 - EPSILON2)
293 continue;
294 t = roots[rooti];
295 td = t * t * t;
296 tc = 3 * t * t * (1 - t);
297 tb = 3 * t * (1 - t) * (1 - t);
298 ta = (1 - t) * (1 - t) * (1 - t);
299 ip.x = ta * sps[0].x + tb * sps[1].x + tc * sps[2].x + td * sps[3].x;
300 ip.y = ta * sps[0].y + tb * sps[1].y + tc * sps[2].y + td * sps[3].y;
301 if (DISTSQ(ip, lps[0]) < EPSILON1 || DISTSQ(ip, lps[1]) < EPSILON1)
302 continue;
303 return 0;
304 }
305 }
306 return 1;
307}
308
310static bool eq(double a, double b) {
311 const double EPS = 1e-7; // same tolerance as `solve3`
312 return fabs(a - b) < EPS;
313}
314
315static int splineintersectsline(Ppoint_t *sps, Ppoint_t *lps, double *roots) {
316 double scoeff[4];
317 double xroots[3], yroots[3], sv;
318
319 const double xcoeff[] = {lps[0].x, lps[1].x - lps[0].x};
320 const double ycoeff[] = {lps[0].y, lps[1].y - lps[0].y};
321 int rootn = 0;
322 if (eq(xcoeff[1], 0)) {
323 if (eq(ycoeff[1], 0)) {
324 points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
325 scoeff[0] -= xcoeff[0];
326 const int xrootn = solve3(scoeff, xroots);
327 points2coeff(sps[0].y, sps[1].y, sps[2].y, sps[3].y, scoeff);
328 scoeff[0] -= ycoeff[0];
329 const int yrootn = solve3(scoeff, yroots);
330 if (xrootn == 4)
331 if (yrootn == 4)
332 return 4;
333 else
334 for (int j = 0; j < yrootn; j++)
335 addroot(yroots[j], roots, &rootn);
336 else if (yrootn == 4)
337 for (int i = 0; i < xrootn; i++)
338 addroot(xroots[i], roots, &rootn);
339 else
340 for (int i = 0; i < xrootn; i++)
341 for (int j = 0; j < yrootn; j++)
342 if (eq(xroots[i], yroots[j]))
343 addroot(xroots[i], roots, &rootn);
344 return rootn;
345 } else {
346 points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
347 scoeff[0] -= xcoeff[0];
348 const int xrootn = solve3(scoeff, xroots);
349 if (xrootn == 4)
350 return 4;
351 for (int i = 0; i < xrootn; i++) {
352 const double tv = xroots[i];
353 if (tv >= 0 && tv <= 1) {
354 points2coeff(sps[0].y, sps[1].y, sps[2].y, sps[3].y, scoeff);
355 sv = scoeff[0] + tv * (scoeff[1] + tv * (scoeff[2] + tv * scoeff[3]));
356 sv = (sv - ycoeff[0]) / ycoeff[1];
357 if (0 <= sv && sv <= 1)
358 addroot(tv, roots, &rootn);
359 }
360 }
361 return rootn;
362 }
363 } else {
364 const double rat = ycoeff[1] / xcoeff[1];
365 points2coeff(sps[0].y - rat * sps[0].x, sps[1].y - rat * sps[1].x,
366 sps[2].y - rat * sps[2].x, sps[3].y - rat * sps[3].x, scoeff);
367 scoeff[0] += rat * xcoeff[0] - ycoeff[0];
368 const int xrootn = solve3(scoeff, xroots);
369 if (xrootn == 4)
370 return 4;
371 for (int i = 0; i < xrootn; i++) {
372 const double tv = xroots[i];
373 if (tv >= 0 && tv <= 1) {
374 points2coeff(sps[0].x, sps[1].x, sps[2].x, sps[3].x, scoeff);
375 sv = scoeff[0] + tv * (scoeff[1] + tv * (scoeff[2] + tv * scoeff[3]));
376 sv = (sv - xcoeff[0]) / xcoeff[1];
377 if (0 <= sv && sv <= 1)
378 addroot(tv, roots, &rootn);
379 }
380 }
381 return rootn;
382 }
383}
384
385static void points2coeff(double v0, double v1, double v2, double v3,
386 double *coeff) {
387 coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
388 coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
389 coeff[1] = 3 * (v1 - v0);
390 coeff[0] = v0;
391}
392
393static void addroot(double root, double *roots, int *rootnp) {
394 if (root >= 0 && root <= 1)
395 roots[*rootnp] = root, (*rootnp)++;
396}
397
399 double d;
400
401 d = v.x * v.x + v.y * v.y;
402 if (d > 1e-6) {
403 d = sqrt(d);
404 v.x /= d, v.y /= d;
405 }
406 return v;
407}
408
410 p1.x += p2.x, p1.y += p2.y;
411 return p1;
412}
413
415 p1.x -= p2.x, p1.y -= p2.y;
416 return p1;
417}
418
419static double dist(Ppoint_t p1, Ppoint_t p2) {
420 double dx, dy;
421
422 dx = p2.x - p1.x, dy = p2.y - p1.y;
423 return hypot(dx, dy);
424}
425
426static Ppoint_t scale(Ppoint_t p, double c) {
427 p.x *= c, p.y *= c;
428 return p;
429}
430
431static double dot(Ppoint_t p1, Ppoint_t p2) {
432 return p1.x * p2.x + p1.y * p2.y;
433}
434
435static double B0(double t) {
436 double tmp = 1.0 - t;
437 return tmp * tmp * tmp;
438}
439
440static double B1(double t) {
441 double tmp = 1.0 - t;
442 return 3 * t * tmp * tmp;
443}
444
445static double B2(double t) {
446 double tmp = 1.0 - t;
447 return 3 * t * t * tmp;
448}
449
450static double B3(double t) { return t * t * t; }
451
452static double B01(double t) {
453 double tmp = 1.0 - t;
454 return tmp * tmp * (tmp + 3 * t);
455}
456
457static double B23(double t) {
458 double tmp = 1.0 - t;
459 return t * t * (3 * tmp + t);
460}
#define sub(h, i)
Definition closest.c:62
static float dy
Definition draw.c:43
static float dx
Definition draw.c:42
#define EPS
Definition emit.c:437
#define dot(v, w)
Definition geom.c:191
void free(void *)
node NULL
Definition grammar.y:181
type-generic dynamically expanding list
#define LIST(type)
Definition list.h:66
#define LIST_SIZE(list)
Definition list.h:92
#define LIST_CLEAR(list)
Definition list.h:307
#define LIST_FRONT(list)
Definition list.h:222
#define LIST_TRY_APPEND(list, item)
Definition list.h:118
int Proutespline(Pedge_t *barriers, size_t n_barriers, Ppolyline_t input_route, Pvector_t endpoint_slopes[2], Ppolyline_t *output_route)
static int splineisinside(Pedge_t *edges, size_t edgen, Ppoint_t *sps)
Definition route.c:281
#define EPSILON2
Definition route.c:24
static double B23(double t)
Definition route.c:457
static int splineintersectsline(Ppoint_t *sps, Ppoint_t *lps, double *roots)
Definition route.c:315
#define DISTSQ(a, b)
Definition route.c:31
static double B01(double t)
Definition route.c:452
static double B1(double t)
Definition route.c:440
static double B0(double t)
Definition route.c:435
static double dist(Ppoint_t p1, Ppoint_t p2)
Definition route.c:419
static double B3(double t)
Definition route.c:450
static double dist_n(Ppoint_t *p, int n)
Definition route.c:199
static Ppoint_t add(Ppoint_t p1, Ppoint_t p2)
Definition route.c:409
static void addroot(double root, double *roots, int *rootnp)
Definition route.c:393
static Ppoint_t scale(Ppoint_t p, double c)
Definition route.c:426
static double B2(double t)
Definition route.c:445
static bool eq(double a, double b)
a == b within a tolerance?
Definition route.c:310
static int splinefits(Pedge_t *edges, size_t edgen, Ppoint_t pa, Pvector_t va, Ppoint_t pb, Pvector_t vb, Ppoint_t *inps, int inpn)
Definition route.c:210
#define EPSILON1
Definition route.c:23
static int mkspline(Ppoint_t *inps, int inpn, const tna_t *tnas, Ppoint_t ev0, Ppoint_t ev1, Ppoint_t *sp0, Ppoint_t *sv0, Ppoint_t *sp1, Ppoint_t *sv1)
Definition route.c:159
static Pvector_t normv(Pvector_t v)
Definition route.c:398
static int reallyroutespline(Pedge_t *edges, size_t edgen, Ppoint_t *inps, int inpn, Ppoint_t ev0, Ppoint_t ev1)
Definition route.c:94
static void points2coeff(double v0, double v1, double v2, double v3, double *coeff)
Definition route.c:385
int solve3(double *coeff, double *roots)
Definition solvers.c:29
Ppoint_t b
Definition pathgeom.h:53
Ppoint_t a
Definition pathgeom.h:53
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
Definition route.c:26
double t
Definition route.c:27
Ppoint_t a[2]
Definition route.c:28