Source code: com/port80/graph/dot/impl/RouteSpline.java
1 //
2 // Copyright(c) 2002, Chris Leung
3 //
4
5 package com.port80.graph.dot.impl;
6
7 import java.awt.geom.*;
8 import com.port80.util.*;
9 import com.port80.util.struct.*;
10
11 /** Spline generation and fitting.
12 */
13 public class RouteSpline {
14
15 ////////////////////////////////////////////////////////////////////////
16
17 private static final String NAME = "RouteSpline";
18
19 public static final double EPSILON0 = 1E-1;
20 public static final double EPSILON1 = 1E-2;
21 public static final double EPSILON2 = 1E-3;
22 public static final boolean DEBUG = false;
23 public static final boolean VERBOSE = true;
24
25 public static RouteSpline instance = null;
26
27 ////////////////////////////////////////////////////////////////////////
28
29 static class BezierPt {
30 double t;
31 DotPoint a;
32 DotPoint b;
33 }
34
35 ////////////////////////////////////////////////////////////////////////
36
37 BezierPt[] cpts = null;
38
39 // Static methods //////////////////////////////////////////////////////
40 //
41 public static RouteSpline getInstance() {
42 if (instance == null)
43 instance = new RouteSpline();
44 return instance;
45 }
46
47 // Instance methods ////////////////////////////////////////////////////
48 //
49
50 /** Route spline to approx. the polyline 'input' within the
51 * bounding polygon 'bound'.
52 *
53 * @return true if route success and 'ret' holds the spline vertices.
54 *
55 * @param bound the closed (bound[bound.size-1]=bound[0]) bounding polygon.
56 * @param input polyline to be approx.
57 * @param tangents tangents at the two end points.
58 * @param output polyline to hold the result spline.
59 *
60 * @see DotPolyline
61 */
62 public boolean routeSpline(
63 DotPolyline ret,
64 DotPolyline bound,
65 DotPolyline input,
66 DotPoint tangent0,
67 DotPoint tangent3) {
68 // Estimated control points.
69 if (cpts == null || cpts.length < input.size) {
70 cpts = new BezierPt[input.size];
71 for (int i = 0; i < input.size; ++i)
72 cpts[i] = new BezierPt();
73 }
74 // Make tangents unit length.
75 tangent0 = normalize(tangent0);
76 tangent3 = normalize(tangent3);
77 ret.add(input.pts[0]);
78 return routeSpline1(ret, bound, input.pts, 0, input.size, tangent0, tangent3);
79 }
80
81 /** Route spline with recursive splitting.
82 *
83 * @return true if route success and 'ret' holds the spline vertices.
84 *
85 * @param bound the bounding envelope polygon.
86 * @param ipts polyline points to be approx.
87 * @param istart start index to the 'ipts' array.
88 * @param isize number of 'ipts' vertices to be used.
89 */
90 private boolean routeSpline1(
91 DotPolyline ret,
92 DotPolyline bound,
93 DotPoint[] ipts,
94 int istart,
95 int isize,
96 DotPoint tangent0,
97 DotPoint tangent1) {
98 // Create approx. control points.
99 cpts[0].t = 0;
100 for (int n = 1, i = istart + 1; n < isize; ++i, ++n)
101 cpts[n].t = cpts[n - 1].t + dist(ipts[i], ipts[i - 1]);
102 for (int n = 1; n < isize; n++)
103 cpts[n].t /= cpts[isize - 1].t;
104 for (int n = 0; n < isize; n++) {
105 cpts[n].a = scale(tangent0, B1(cpts[n].t));
106 cpts[n].b = scale(tangent1, B2(cpts[n].t));
107 }
108
109 // Find spline that fit the polyline vertices.
110 DoublePair scales = makeSpline(ipts, istart, isize, cpts);
111 // Result curve sometimes have a sharp turn into a long
112 // straight segment due the a very large scale. Limit scale
113 // to min. of entering and leaving segment lengths.
114 DotPoint p1 = new DotPoint(ipts[istart]);
115 DotPoint p2 = new DotPoint(ipts[istart + isize - 1]);
116 DotPoint v1 = scale(tangent0, scales.first);
117 DotPoint v2 = scale(tangent1, scales.second);
118 if (splineFits(ret, bound, ipts, istart, isize, p1, v1, p2, v2))
119 return true;
120
121 // Split at vertex with max. error.
122 DotPoint cp1 = add(p1, scale(v1, 1 / 3.0));
123 DotPoint cp2 = sub(p2, scale(v2, 1 / 3.0));
124 DotPoint p = new DotPoint();
125 double d;
126 int maxi = -1;
127 double maxd = -1;
128 for (int n = 1, i = istart + 1; n < isize - 1; ++i, ++n) {
129 double t = cpts[n].t;
130 p.x = B0(t) * p1.x + B1(t) * cp1.x + B2(t) * cp2.x + B3(t) * p2.x;
131 p.y = B0(t) * p1.y + B1(t) * cp1.y + B2(t) * cp2.y + B3(t) * p2.y;
132 if ((d = dist(p, ipts[i])) > maxd) {
133 maxd = d;
134 maxi = i;
135 }
136 }
137
138 // Split polyline and try curve fitting again.
139 v1 = normalize(sub(ipts[maxi], ipts[maxi - 1]));
140 v2 = normalize(sub(ipts[maxi + 1], ipts[maxi]));
141 p = normalize(add(v1, v2));
142 if (!routeSpline1(ret, bound, ipts, istart, maxi - istart + 1, tangent0, p))
143 return false;
144 if (!routeSpline1(ret, bound, ipts, maxi, istart + isize - maxi, p, tangent1))
145 return false;
146 return true;
147 }
148
149 /** Interpolate the two middle control points of the Bezier curve.
150 */
151 private DoublePair makeSpline(DotPoint[] ipts, int istart, int isize, BezierPt[] cpts) {
152 DotPoint tmp;
153 double[] x = { 0.0, 0.0 };
154 double det01, det0X, detX1;
155 double d01, scale0, scale3;
156 double[][] c = { { 0.0, 0.0 }, {
157 0.0, 0.0 }
158 };
159 scale0 = scale3 = 0.0;
160 //x[0]=x[1]=0.0;
161 //c[0][0]=c[0][1]=c[1][0]=c[1][1]=0.0;
162 for (int n = 0, i = istart; n < isize; ++n, ++i) {
163 c[0][0] += dot(cpts[n].a, cpts[n].a);
164 c[0][1] += dot(cpts[n].a, cpts[n].b);
165 c[1][0] = c[0][1];
166 c[1][1] += dot(cpts[n].b, cpts[n].b);
167 tmp =
168 sub(
169 ipts[i],
170 add(
171 scale(ipts[istart], B01(cpts[n].t)),
172 scale(ipts[istart + isize - 1], B23(cpts[n].t))));
173 x[0] += dot(cpts[n].a, tmp);
174 x[1] += dot(cpts[n].b, tmp);
175 }
176 det01 = c[0][0] * c[1][1] - c[1][0] * c[0][1];
177 det0X = c[0][0] * x[1] - c[0][1] * x[0];
178 detX1 = x[0] * c[1][1] - x[1] * c[0][1];
179 if (Math.abs(det01) < 1e-4)
180 det01 = 1e-4;
181 if (det01 != 0.0) {
182 scale0 = detX1 / det01;
183 scale3 = det0X / det01;
184 }
185 // NOTE: If 1e-6 is used, some route with very large x/y aspect ratio would get a knot
186 // because one of the control point goes too far.
187 // if (Math.abs(det01) < 1e-6 || scale0 <= 0.0 || scale3 <= 0.0) {
188 if (scale0 <= 0.0 || scale3 <= 0.0) {
189 d01 = dist(ipts[istart], ipts[istart + isize - 1]) / 3.0;
190 scale0 = d01;
191 scale3 = d01;
192 }
193 if (DEBUG)
194 msg.println(NAME + ".makeSpine(): scale0=" + scale0 + ", scale3=" + scale3);
195 return new DoublePair(scale0, scale3);
196 }
197
198 /** Check if spline given by 'p0','v0','p3','v3' fit inside the
199 * bounding polygon with 4/3,2/3 and 0 curvature.
200 *
201 * @return p1,p2,p3 in 'ret' for the first spline that fit.
202 *
203 * //TELLME: why not start with less curvature.
204 */
205 private boolean splineFits(
206 DotPolyline ret,
207 DotPolyline bound,
208 DotPoint[] ipts,
209 int istart,
210 int isize,
211 DotPoint p0,
212 DotPoint tan0,
213 DotPoint p3,
214 DotPoint tan3) {
215 if (DEBUG)
216 msg.println(NAME + ".splineFits(): p0=" + p0 + " tan0=" + tan0 + " p3=" + p3 + " tan3=" + tan3);
217 boolean first = true;
218 boolean success = false;
219 double a = 4.0;
220 double b = 4.0;
221 DotPoint[] pts = new DotPoint[4];
222 DotPoint[] savepts = new DotPoint[4];
223 for (int i = 0; i < 4; ++i)
224 pts[i] = new DotPoint();
225 pts[0].x = p0.x;
226 pts[0].y = p0.y;
227 pts[3].x = p3.x;
228 pts[3].y = p3.y;
229 for (;;) {
230 pts[1].x = p0.x + tan0.x * a / 3.0;
231 pts[1].y = p0.y + tan0.y * a / 3.0;
232 pts[2].x = p3.x - tan3.x * b / 3.0;
233 pts[2].y = p3.y - tan3.y * b / 3.0;
234 if (DEBUG) {
235 msg.print(NAME + ".splineFits(): a=" + a + ", b=" + b + ": ");
236 for (int i = 0; i < 4; ++i)
237 msg.print(" " + pts[i].toString());
238 msg.println("");
239 }
240 /* shortcuts(paths shorter than the shortest path) not
241 * allowed - they must be outside the constraint polygon.
242 * this can happen if the candidate spline intersects the
243 * constraint polygon exactly on sides or vertices. maybe
244 * this could be more elegant,but it solves the immediate
245 * problem. we could also try jittering the constraint
246 * polygon,or computing the candidate spline more
247 * carefully, for example using the path. SCN */
248
249 if (first && (lineLength(pts, 0, 4) < (lineLength(ipts, istart, isize) - EPSILON1)))
250 return false;
251 first = false;
252
253 if (splineIsInside(bound, pts)) {
254 success = true;
255 for (int pi = 1; pi < 4; pi++)
256 savepts[pi] = new DotPoint(pts[pi]);
257 if (DEBUG)
258 msg.println(NAME + ".splineFits(): success: a=" + a + ",b=" + b);
259 } else {
260 if (success) {
261 for (int pi = 1; pi < 4; pi++)
262 ret.add(savepts[pi]);
263 return true;
264 }
265 }
266 // Lower the limit for straighter line and shaper turns.
267 if (a < 2.5) {
268 if (success) {
269 for (int pi = 1; pi < 4; pi++)
270 ret.add(savepts[pi]);
271 return true;
272 }
273 }
274 if (a > 0.75) {
275 a /= 2;
276 b /= 2;
277 } else {
278 //if(a==0 && b==0) {
279 // If there are only two input points, there can be no
280 // more splitting. So just take the straight line as a
281 // fit.
282 if (isize == 2) {
283 // Instead of having p1=p0 and p2=p3, we put p1,p2
284 // on evenly separated on the line from p0 to p3.
285 // double dx=(p3.x-p0.x)/3;
286 // double dy=(p3.y-p0.y)/3;
287 // pts[1].x=p0.x+dx;
288 // pts[1].y=p0.y+dy;
289 // pts[2].x=p3.x-dx;
290 // pts[2].y=p3.y-dy;
291 for (int i = 1; i < 4; ++i)
292 ret.add(pts[i]);
293 if (DEBUG)
294 msg.println(NAME + ".splineFits(): forced straight line");
295 return true;
296 }
297 break;
298 }
299 //else {a=0; b=0;}
300 }
301 if (DEBUG)
302 msg.println(NAME + ".splineFits(): failed");
303 return false;
304 }
305
306 /** Check if spline given by 'pts' lies inside the bounding
307 * polygon by looking for intersections of the spline with the
308 * polygon. There are any intersections besides the vertices of
309 * the bounding polygon, then the spline crossed the boundary and
310 * considered not inside the bounds.
311 */
312 private boolean splineIsInside(DotPolyline bound, DotPoint[] pts) {
313 DotLine line = new DotLine();
314 double t, ta, tb, tc, td;
315 int nroot;
316 DotPoint pt = new DotPoint();
317 double[] roots = new double[4];
318
319 for (int ei = 0; ei < bound.size - 1; ei++) {
320 line.set(bound.pts[ei], bound.pts[ei + 1]);
321 /* if((nroot=splineIntersectsLine(pts,line,roots))==4) return 1; */
322 nroot = splineIntersectsLine(pts, line, roots);
323 if (DEBUG)
324 msg.println(NAME + ".splineIsInside(): ei=" + ei + ", nroot=" + nroot);
325 if (nroot == 4)
326 continue;
327 for (int i = 0; i < nroot; i++) {
328 if (DEBUG)
329 msg.println(NAME + ".splineIsInside(): i=" + i + ", root=" + roots[i]);
330 if (roots[i] < EPSILON2 || roots[i] > (1 - EPSILON2))
331 continue;
332 t = roots[i];
333 td = t * t * t;
334 tc = 3 * t * t * (1 - t);
335 tb = 3 * t * (1 - t) * (1 - t);
336 ta = (1 - t) * (1 - t) * (1 - t);
337 pt.x = ta * pts[0].x + tb * pts[1].x + tc * pts[2].x + td * pts[3].x;
338 pt.y = ta * pts[0].y + tb * pts[1].y + tc * pts[2].y + td * pts[3].y;
339 if (DEBUG)
340 msg.println(
341 NAME
342 + ".splineIsInside(): pt="
343 + pt.toString("%.4f,%.4f, pts[ei]=")
344 + bound.pts[ei].toString("%.4f,%.4f, pts[ei+1]=")
345 + bound.pts[ei
346 + 1].toString("%.4f,%.4f"));
347 if (distsq(pt, bound.pts[ei]) < EPSILON0 || distsq(pt, bound.pts[ei + 1]) < EPSILON0)
348 continue;
349 return false;
350 }
351 }
352 return true;
353 }
354
355 //FIXME: check sign of y.
356 private int splineIntersectsLine(DotPoint[] pts, DotLine line, double[] roots) {
357 double[] scoeff = new double[4];
358 double[] xcoeff = new double[2];
359 double[] ycoeff = new double[2];
360 double[] xroots = new double[3];
361 double[] yroots = new double[3];
362 double tv, sv;
363 int xrootn, yrootn;
364
365 xcoeff[0] = line.x1;
366 xcoeff[1] = line.x2 - line.x1;
367 ycoeff[0] = line.y1;
368 ycoeff[1] = line.y2 - line.y1;
369 int nroot = 0;
370 if (xcoeff[1] == 0) {
371 if (ycoeff[1] == 0) {
372 points2coeff(pts[0].x, pts[1].x, pts[2].x, pts[3].x, scoeff);
373 scoeff[0] -= xcoeff[0];
374 xrootn = CubicSolver.solve3(scoeff, xroots);
375 points2coeff(pts[0].y, pts[1].y, pts[2].y, pts[3].y, scoeff);
376 scoeff[0] -= ycoeff[0];
377 yrootn = CubicSolver.solve3(scoeff, yroots);
378 if (xrootn == 4)
379 if (yrootn == 4)
380 return 4;
381 else
382 for (int j = 0; j < yrootn; j++) {
383 nroot = addroot(roots, nroot, yroots[j]);
384 } else if (yrootn == 4)
385 for (int i = 0; i < xrootn; i++) {
386 nroot = addroot(roots, nroot, xroots[i]);
387 } else {
388 for (int i = 0; i < xrootn; i++)
389 for (int j = 0; j < yrootn; j++)
390 if (xroots[i] == yroots[j])
391 nroot = addroot(roots, nroot, xroots[i]);
392 }
393 return nroot;
394 } else {
395 points2coeff(pts[0].x, pts[1].x, pts[2].x, pts[3].x, scoeff);
396 scoeff[0] -= xcoeff[0];
397 xrootn = CubicSolver.solve3(scoeff, xroots);
398 if (xrootn == 4)
399 return 4;
400 for (int i = 0; i < xrootn; i++) {
401 tv = xroots[i];
402 if (tv >= 0 && tv <= 1) {
403 points2coeff(pts[0].y, pts[1].y, pts[2].y, pts[3].y, scoeff);
404 sv = scoeff[0] + tv * (scoeff[1] + tv * (scoeff[2] + tv * scoeff[3]));
405 sv = (sv - ycoeff[0]) / ycoeff[1];
406 if ((0 <= sv) && (sv <= 1))
407 nroot = addroot(roots, nroot, tv);
408 }
409 }
410 return nroot;
411 }
412 } else {
413 double ratio = ycoeff[1] / xcoeff[1];
414 points2coeff(
415 pts[0].y - ratio * pts[0].x,
416 pts[1].y - ratio * pts[1].x,
417 pts[2].y - ratio * pts[2].x,
418 pts[3].y - ratio * pts[3].x,
419 scoeff);
420 scoeff[0] += ratio * xcoeff[0] - ycoeff[0];
421 xrootn = CubicSolver.solve3(scoeff, xroots);
422 if (xrootn == 4)
423 return 4;
424 for (int i = 0; i < xrootn; i++) {
425 tv = xroots[i];
426 if (tv >= 0 && tv <= 1) {
427 points2coeff(pts[0].x, pts[1].x, pts[2].x, pts[3].x, scoeff);
428 sv = scoeff[0] + tv * (scoeff[1] + tv * (scoeff[2] + tv * scoeff[3]));
429 sv = (sv - xcoeff[0]) / xcoeff[1];
430 if (0 <= sv && sv <= 1)
431 nroot = addroot(roots, nroot, tv);
432 }
433 }
434 return nroot;
435 }
436 }
437
438 ////////////////////////////////////////////////////////////////////////
439
440 /** Determine total length of polyline p[istart]..p[istart+n]. */
441 private double lineLength(DotPoint[] p, int start, int size) {
442 double ret = 0.0;
443 for (int i = start + 1, max = start + size; i < max; ++i) {
444 ret
445 += Math.sqrt(
446 (p[i].x - p[i - 1].x) * (p[i].x - p[i - 1].x)
447 + (p[i].y - p[i - 1].y) * (p[i].y - p[i - 1].y));
448 }
449 return ret;
450 }
451
452 private void points2coeff(double v0, double v1, double v2, double v3, double[] coeff) {
453 coeff[3] = v3 + 3 * v1 - (v0 + 3 * v2);
454 coeff[2] = 3 * v0 + 3 * v2 - 6 * v1;
455 coeff[1] = 3 * (v1 - v0);
456 coeff[0] = v0;
457 }
458
459 private int addroot(double[] roots, int rootn, double root) {
460 if (root >= 0 && root <= 1)
461 roots[rootn++] = root;
462 return rootn;
463 }
464
465 /** Normalize vector to 'v' to unit length. */
466 private DotPoint normalize(DotPoint v) {
467 double d = Math.sqrt(v.x * v.x + v.y * v.y);
468 if (d != 0) {
469 v.x /= d;
470 v.y /= d;
471 }
472 return v;
473 }
474
475 ////////////////////////////////////////////////////////////////////////
476
477 private DotPoint add(DotPoint p1, DotPoint p2) {
478 return new DotPoint(p1.x + p2.x, p1.y + p2.y);
479 }
480
481 private DotPoint sub(DotPoint p1, DotPoint p2) {
482 return new DotPoint(p1.x - p2.x, p1.y - p2.y);
483 }
484
485 private DotPoint scale(DotPoint p, double c) {
486 return new DotPoint(p.x * c, p.y * c);
487 }
488
489 ////////////////////////////////////////////////////////////////////////
490
491 private double distsq(DotPoint p1, DotPoint p2) {
492 double dx = p2.x - p1.x;
493 double dy = p2.y - p1.y;
494 return dx * dx + dy * dy;
495 }
496
497 private double dist(DotPoint p1, DotPoint p2) {
498 double dx = p2.x - p1.x;
499 double dy = p2.y - p1.y;
500 return Math.sqrt(dx * dx + dy * dy);
501 }
502
503 private double dot(DotPoint p1, DotPoint p2) {
504 return p1.x * p2.x + p1.y * p2.y;
505 }
506
507 ////////////////////////////////////////////////////////////////////////
508
509 private double B0(double t) {
510 double tmp = 1.0 - t;
511 return tmp * tmp * tmp;
512 }
513
514 private double B1(double t) {
515 double tmp = 1.0 - t;
516 return 3 * t * tmp * tmp;
517 }
518
519 private double B2(double t) {
520 double tmp = 1.0 - t;
521 return 3 * t * t * tmp;
522 }
523
524 private double B3(double t) {
525 return t * t * t;
526 }
527
528 private double B01(double t) {
529 double tmp = 1.0 - t;
530 return tmp * tmp * (tmp + 3 * t);
531 }
532
533 private double B23(double t) {
534 double tmp = 1.0 - t;
535 return t * t * (3 * tmp + t);
536 }
537
538 ////////////////////////////////////////////////////////////////////////
539
540 public static void main(String[] args) {
541 new RouteSpline().test();
542 }
543
544 public void test() {
545 test1();
546 }
547
548 /** A simple test with single curve. */
549 public void test1() {
550 DotPolyline bounds = new DotPolyline(20);
551 bounds.add(0, 600);
552 bounds.add(0, 500);
553 bounds.add(300, 500);
554 bounds.add(300, 400);
555 bounds.add(0, 400);
556 bounds.add(0, 0);
557 bounds.add(500, 0);
558 bounds.add(500, 100);
559 bounds.add(200, 100);
560 bounds.add(200, 200);
561 bounds.add(500, 200);
562 bounds.add(500, 600);
563 bounds.close();
564 DotPolyline input = new DotPolyline(10);
565 input.add(100, 600);
566 input.add(400, 450);
567 input.add(400, 350);
568 input.add(100, 150);
569 input.add(400, 0);
570 DotPoint tangent0 = new DotPoint(100, -30);
571 DotPoint tangent3 = new DotPoint(-100, -30);
572 DotPolyline ret = new DotPolyline(10);
573 getInstance().routeSpline(ret, bounds, input, tangent0, tangent3);
574 StringBuffer buf = new StringBuffer("// Result:\n");
575 sprintBound(buf, bounds);
576 sprintPath(buf, input);
577 sprintCurve(buf, ret);
578 msg.println(buf.toString());
579 }
580
581 /** Print bounding polygon as GeneralPath code. */
582 private void sprintBound(StringBuffer ret, DotPolyline bounds) {
583 for (int i = 0; i < bounds.size; ++i) {
584 if (i == 0)
585 ret.append("// Bounds:\ncurve.moveTo");
586 else
587 ret.append("curve.lineTo");
588 ret.append(sprint.f("(%.1ff,%.1ff);\n").a(bounds.pts[i].x).a(bounds.pts[i].y).end());
589 }
590 ret.append("curve.closePath();\n");
591 }
592
593 /** Print path to be approximated.*/
594 private void sprintPath(StringBuffer ret, DotPolyline path) {
595 for (int i = 0; i < path.size; ++i) {
596 if (i == 0)
597 ret.append("// Path\ncurve.moveTo");
598 else
599 ret.append("curve.lineTo");
600 ret.append(sprint.f("(%.1ff,%.1ff);\n").a(path.pts[i].x).a(path.pts[i].y).end());
601 }
602 }
603
604 private void sprintCurve(StringBuffer ret, DotPolyline spline) {
605 ret.append(
606 sprint
607 .f("// Spline\ncurve.moveTo(%.1ff,%.1ff);\n")
608 .a(spline.pts[0].x)
609 .a(spline.pts[0].y)
610 .end());
611 for (int i = 1; i < spline.size; i += 3) {
612 ret.append(
613 sprint
614 .f("curve.curveTo(%.1ff,%.1ff,%.1ff,%.1ff,%.1ff,%.1ff);\n")
615 .a(spline.pts[i].x)
616 .a(spline.pts[i].y)
617 .a(spline.pts[i + 1].x)
618 .a(spline.pts[i + 1].y)
619 .a(spline.pts[i + 2].x)
620 .a(spline.pts[i + 2].y)
621 .end());
622 }
623 }
624
625 ////////////////////////////////////////////////////////////////////////
626
627 }