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