32 ((((a).x - (b).x) * ((a).x - (b).x)) + (((a).y - (b).y) * ((a).y - (b).y)))
46static void points2coeff(
double,
double,
double,
double,
double *);
47static void addroot(
double,
double *,
int *);
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);
74 inps = input_route.
ps;
75 assert(input_route.
pn <= INT_MAX);
76 inpn = (int)input_route.
pn;
79 endpoint_slopes[0] =
normv(endpoint_slopes[0]);
80 endpoint_slopes[1] =
normv(endpoint_slopes[1]);
86 endpoint_slopes[1]) == -1)
101 tna_t *
const tnas = calloc((
size_t)inpn,
sizeof(
tna_t));
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));
114 if (
mkspline(inps, inpn, tnas, ev0, ev1, &p1, &v1, &p2, &v2) == -1) {
118 int fit =
splinefits(edges, edgen, p1, v1, p2, v2, inps, inpn);
131 for (
int i = 1; i < inpn - 1; i++) {
132 const double t = tnas[i].
t;
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) {
145 const int spliti = maxi;
163 double c[2][2], x[2], det01, det0X, detX1;
164 double d01, scale0, scale3;
167 scale0 = scale3 = 0.0;
168 c[0][0] = c[0][1] = c[1][0] = c[1][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]);
174 c[1][1] +=
dot(tnas[i].a[1], tnas[i].a[1]);
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);
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;
187 if (fabs(det01) < 1e-6 || scale0 <= 0.0 || scale3 <= 0.0) {
188 d01 =
dist(inps[0], inps[inpn - 1]) / 3.0;
193 *sv0 =
scale(ev0, scale0);
194 *sp1 = inps[inpn - 1];
195 *sv1 =
scale(ev1, scale3);
204 for (i = 1; i < n; i++) {
205 rv += hypot(p[i].x - p[i - 1].x, p[i].y - p[i - 1].y);
218 forceflag = (inpn == 2 ? 1 : 0);
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;
244 for (pi = 1; pi < 4; pi++) {
249#if defined(DEBUG) && DEBUG >= 1
250 fprintf(stderr,
"success: %f %f\n", a, a);
258 for (pi = 1; pi < 4; pi++) {
263#if defined(DEBUG) && DEBUG >= 1
264 fprintf(stderr,
"forced straight line: %f %f\n", a, a);
275#if defined(DEBUG) && DEBUG >= 1
276 fprintf(stderr,
"failure\n");
285 double t, ta, tb, tc, td;
287 for (
size_t ei = 0; ei < edgen; ei++) {
288 lps[0] = edges[ei].
a, lps[1] = edges[ei].
b;
291 for (rooti = 0; rooti < rootn; rooti++) {
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;
310static bool eq(
double a,
double b) {
311 const double EPS = 1e-7;
312 return fabs(a - b) <
EPS;
317 double xroots[3], yroots[3], sv;
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};
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);
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);
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);
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);
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)
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);
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)
387 coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
388 coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
389 coeff[1] = 3 * (v1 - v0);
393static void addroot(
double root,
double *roots,
int *rootnp) {
394 if (root >= 0 && root <= 1)
395 roots[*rootnp] = root, (*rootnp)++;
401 d = v.
x * v.
x + v.
y * v.
y;
410 p1.
x += p2.
x, p1.
y += p2.
y;
415 p1.
x -= p2.
x, p1.
y -= p2.
y;
423 return hypot(
dx,
dy);
432 return p1.
x * p2.
x + p1.
y * p2.
y;
435static double B0(
double t) {
436 double tmp = 1.0 - t;
437 return tmp * tmp * tmp;
440static double B1(
double t) {
441 double tmp = 1.0 - t;
442 return 3 * t * tmp * tmp;
445static double B2(
double t) {
446 double tmp = 1.0 - t;
447 return 3 * t * t * tmp;
450static double B3(
double t) {
return t * t * t; }
452static double B01(
double t) {
453 double tmp = 1.0 - t;
454 return tmp * tmp * (tmp + 3 * t);
457static double B23(
double t) {
458 double tmp = 1.0 - t;
459 return t * t * (3 * tmp + t);
type-generic dynamically expanding list
#define LIST_TRY_APPEND(list, item)
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)
static double B23(double t)
static int splineintersectsline(Ppoint_t *sps, Ppoint_t *lps, double *roots)
static double B01(double t)
static double B1(double t)
static double B0(double t)
static double dist(Ppoint_t p1, Ppoint_t p2)
static double B3(double t)
static double dist_n(Ppoint_t *p, int n)
static Ppoint_t add(Ppoint_t p1, Ppoint_t p2)
static void addroot(double root, double *roots, int *rootnp)
static Ppoint_t scale(Ppoint_t p, double c)
static double B2(double t)
static bool eq(double a, double b)
a == b within a tolerance?
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)
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)
static Pvector_t normv(Pvector_t v)
static int reallyroutespline(Pedge_t *edges, size_t edgen, Ppoint_t *inps, int inpn, Ppoint_t ev0, Ppoint_t ev1)
static void points2coeff(double v0, double v1, double v2, double v3, double *coeff)
int solve3(double *coeff, double *roots)