1 /*
2 * Copyright 2014 Google Inc. All rights reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16 * Contributor: Raph Levien
17 */
18
19 #include <iostream>
20 #include <fstream>
21 #include <cmath>
22 #include <vector>
23 #include <algorithm>
24
25 using std::vector;
26
27 #define HALF_STEP 1
28
29 class Point {
30 public:
Point()31 Point() : x(0), y(0) { }
Point(double x,double y)32 Point(double x, double y) : x(x), y(y) { }
33 double x, y;
34 };
35
operator ==(const Point & p0,const Point & p1)36 bool operator==(const Point& p0, const Point& p1) {
37 return p0.x == p1.x && p0.y == p1.y;
38 }
39
operator <<(std::ostream & os,const Point & p)40 std::ostream& operator<<(std::ostream& os, const Point& p) {
41 os << "(" << p.x << ", " << p.y << ")";
42 return os;
43 }
44
dist(Point p0,Point p1)45 double dist(Point p0, Point p1) {
46 return std::hypot(p0.x - p1.x, p0.y - p1.y);
47 }
48
dist2(Point p0,Point p1)49 double dist2(Point p0, Point p1) {
50 double dx = p0.x - p1.x;
51 double dy = p0.y - p1.y;
52 return dx * dx + dy * dy;
53 }
54
lerp(double t,Point p0,Point p1)55 Point lerp(double t, Point p0, Point p1) {
56 return Point(p0.x + t * (p1.x - p0.x), p0.y + t * (p1.y - p0.y));
57 }
58
unitize(Point p)59 Point unitize(Point p) {
60 double scale = 1/std::hypot(p.x, p.y);
61 return Point(p.x * scale, p.y * scale);
62 }
63
round(Point p)64 Point round(Point p) {
65 return Point(std::round(p.x), std::round(p.y));
66 }
67
68 class Quad {
69 public:
Quad()70 Quad() : p() { }
Quad(Point p0,Point p1,Point p2)71 Quad(Point p0, Point p1, Point p2) : p() {
72 p[0] = p0;
73 p[1] = p1;
74 p[2] = p2;
75 }
76 Point p[3];
77 double arclen() const;
78 Point eval(double t) const;
79 bool isLine() const;
print(std::ostream & o) const80 void print(std::ostream& o) const {
81 o << p[0].x << " " << p[0].y << " " << p[1].x << " " << p[1].y << " "
82 << p[2].x << " " << p[2].y << std::endl;
83 }
84 };
85
isLine() const86 bool Quad::isLine() const {
87 return p[1] == lerp(0.5, p[0], p[2]);
88 }
89
90 // One step of a 4th-order Runge-Kutta numerical integration
91 template <size_t n, typename F>
rk4(double y[n],double x,double h,F & derivs)92 void rk4(double y[n], double x, double h, F& derivs) {
93 double dydx[n];
94 double dyt[n];
95 double dym[n];
96 double yt[n];
97 derivs(dydx, x, y);
98 double hh = h * .5;
99 double h6 = h * (1./6);
100 for (size_t i = 0; i < n; i++) {
101 yt[i] = y[i] + hh * dydx[i];
102 }
103 derivs(dyt, x + hh, yt);
104 for (size_t i = 0; i < n; i++) {
105 yt[i] = y[i] + hh * dyt[i];
106 }
107 derivs(dym, x + hh, yt);
108 for (size_t i = 0; i < n; i++) {
109 yt[i] = y[i] + h * dym[i];
110 dym[i] += dyt[i];
111 }
112 derivs(dyt, x + h, yt);
113 for (size_t i = 0; i < n; i++) {
114 y[i] += h6 * (dydx[i] + dyt[i] + 2 * dym[i]);
115 }
116 }
117
118 class ArclenFunctor {
119 public:
ArclenFunctor(const Quad & q)120 ArclenFunctor(const Quad& q)
121 : dx0(2 * (q.p[1].x - q.p[0].x))
122 , dy0(2 * (q.p[1].y - q.p[0].y))
123 , dx1(2 * (q.p[2].x - q.p[1].x))
124 , dy1(2 * (q.p[2].y - q.p[1].y)) { }
operator ()(double dydx[1],double t,const double y[1])125 void operator()(double dydx[1], double t, const double y[1]) {
126 Point p(deriv(t));
127 dydx[0] = std::hypot(p.x, p.y);
128 }
deriv(double t) const129 Point deriv(double t) const {
130 return Point(dx0 + t * (dx1 - dx0), dy0 + t * (dy1 - dy0));
131 }
132 private:
133 double dx0, dy0, dx1, dy1;
134 };
135
arclen() const136 double Quad::arclen() const {
137 ArclenFunctor derivs(*this);
138 const int n = 10;
139 double dt = 1./n;
140 double t = 0;
141 double y[1] = { 0 };
142 for (int i = 0; i < n; i++) {
143 rk4<1>(y, t, dt, derivs);
144 t += dt;
145 }
146 return y[0];
147 }
148
eval(double t) const149 Point Quad::eval(double t) const {
150 Point p01(lerp(t, p[0], p[1]));
151 Point p12(lerp(t, p[1], p[2]));
152 return lerp(t, p01, p12);
153 }
154
155 class Thetas {
156 public:
157 void init(const vector<Quad>& qs);
158 Point xy(double s) const;
159 Point dir(double s) const;
160 double arclen;
161 private:
162 vector<Point> xys;
163 vector<Point> dirs;
164 };
165
init(const vector<Quad> & qs)166 void Thetas::init(const vector<Quad>& qs) {
167 xys.clear();
168 dirs.clear();
169 double s = 0;
170 int ix = 0;
171 Point lastxy;
172 Point lastd;
173 double lasts = -1;
174 for (size_t i = 0; i < qs.size(); i++) {
175 const Quad& q = qs[i];
176 ArclenFunctor derivs(q);
177 const int n = 100;
178 double dt = 1./n;
179 double t = 0;
180 double y[1];
181 y[0] = s;
182 for (int j = 0; j < n; j++) {
183 Point thisxy(q.eval(t));
184 Point thisd(derivs.deriv(t));
185 while (ix <= y[0]) {
186 double u = (ix - lasts) / (y[0] - lasts);
187 xys.push_back(lerp(u, lastxy, thisxy));
188 dirs.push_back(unitize(lerp(u, lastd, thisd)));
189 ix++;
190 }
191 lasts = y[0];
192 rk4<1>(y, t, dt, derivs);
193 t += dt;
194 lastxy = thisxy;
195 lastd = thisd;
196 }
197 s = y[0];
198 }
199 const Quad& q = qs[qs.size() - 1];
200 Point thisxy(q.p[2]);
201 Point thisd(ArclenFunctor(q).deriv(1));
202 while (ix <= s + 1) {
203 double u = (ix - lasts) / (s - lasts);
204 xys.push_back(lerp(u, lastxy, thisxy));
205 dirs.push_back(unitize(lerp(u, lastd, thisd)));
206 ix++;
207 }
208 arclen = s;
209 }
210
xy(double s) const211 Point Thetas::xy(double s) const {
212 int bucket = (int)s;
213 double frac = s - bucket;
214 return lerp(frac, xys[bucket], xys[bucket + 1]);
215 }
216
dir(double s) const217 Point Thetas::dir(double s) const {
218 int bucket = (int)s;
219 double frac = s - bucket;
220 return lerp(frac, dirs[bucket], dirs[bucket + 1]);
221 }
222
223 #define NORM_LEVEL 2
224
225 // L1 angle norm, 2, L2 angle norm, 0.05
226 // L1 distance norm, 200
227 double dist_factor = .005;
228 double angle_factor = 5;
229
230
231 class MeasureFunctor {
232 public:
MeasureFunctor(const Thetas & curve,double s0,double ss,const ArclenFunctor & af,Quad q)233 MeasureFunctor(const Thetas& curve, double s0, double ss, const ArclenFunctor& af,
234 Quad q)
235 : curve(curve), s0(s0), ss(ss), af(af), q(q) { }
operator ()(double dydx[2],double t,const double y[2])236 void operator()(double dydx[2], double t, const double y[2]) {
237 Point dxy(af.deriv(t));
238 dydx[0] = std::hypot(dxy.x, dxy.y);
239
240 // distance error
241 Point curvexy = curve.xy(s0 + y[0] * ss);
242 #if NORM_LEVEL == 1
243 double disterr = dist(q.eval(t), curvexy);
244 #endif
245 #if NORM_LEVEL == 2
246 double disterr = dist2(q.eval(t), curvexy);
247 #endif
248 disterr *= dydx[0];
249
250 // angle error
251 Point dir = curve.dir(s0 + y[0] * ss);
252 double angleerr = dir.x * dxy.y - dir.y * dxy.x;
253 #if NORM_LEVEL == 1
254 angleerr = std::abs(angleerr);
255 #endif
256 #if NORM_LEVEL == 2
257 angleerr = (angleerr * angleerr) / dydx[0];
258 #endif
259
260 dydx[1] = dist_factor * disterr + angle_factor * angleerr;
261 }
262 private:
263 const Thetas& curve;
264 double s0;
265 double ss;
266 const ArclenFunctor& af;
267 Quad q;
268 };
269
270 // measure how closely the quad fits the section of curve, using L1 norm
271 // of angle mismatch
measureQuad(const Thetas & curve,double s0,double s1,const Quad & q)272 double measureQuad(const Thetas& curve, double s0, double s1, const Quad& q) {
273 ArclenFunctor derivs(q);
274 double ss = 0;
275 if (q.arclen() != 0)
276 ss = (s1 - s0) / q.arclen();
277 MeasureFunctor err(curve, s0, ss, derivs, q);
278 const int n = 10;
279 double dt = 1./n;
280 double t = 0;
281 double y[2] = { 0, 0 };
282 for (int i = 0; i < n; i++) {
283 rk4<2>(y, t, dt, err);
284 t += dt;
285 }
286 return y[1];
287 }
288
289 struct Break {
BreakBreak290 Break(double s, Point xy, Point dir) : s(s), xy(xy), dir(dir) { }
291 double s;
292 Point xy;
293 Point dir;
294 };
295
296 struct Statelet {
297 void combine(const Statelet* prev, double score, Quad q, double penalty);
298 const Statelet* prev;
299 double score;
300 Quad q;
301 };
302
combine(const Statelet * newprev,double newscore,Quad newq,double penalty)303 void Statelet::combine(const Statelet* newprev, double newscore, Quad newq, double penalty) {
304 prev = newprev;
305 double pmul = 2;
306 if (newq.isLine()) {
307 pmul = 1;
308 } else if (newprev != 0 && !newprev->q.isLine()
309 && lerp(0.5, newprev->q.p[1], newq.p[1]) == newq.p[0]) {
310 pmul = 1;
311 }
312 score = (newprev == 0 ? 0 : newprev->score) + penalty * pmul + newscore;
313 q = newq;
314 }
315
316 struct State {
317 void combine(const State* prev, double score, Quad q, double penalty);
318 vector<Statelet> sts;
319 bool init;
320 };
321
combine(const State * prev,double score,Quad q,double penalty)322 void State::combine(const State* prev, double score, Quad q, double penalty) {
323 const Statelet* prevsl = prev->sts.empty() ? 0 : &prev->sts[0];
324 if (prevsl == 0 && !prev->init) {
325 return;
326 }
327 Statelet sl;
328 sl.combine(prevsl, score, q, penalty);
329 if (sts.empty()) {
330 sts.push_back(sl);
331 } else {
332 if (sl.score < sts[0].score) {
333 sts[0] = sl;
334 }
335 }
336 }
337
isInt(double x)338 bool isInt(double x) {
339 return x == (int) x;
340 }
341
okForHalf(const State * prev,Quad q)342 bool okForHalf(const State* prev, Quad q) {
343 if (isInt(q.p[0].x) && isInt(q.p[0].y)) {
344 return true;
345 }
346 if (q.isLine()) {
347 return false;
348 }
349 const Statelet* prevsl = prev->sts.empty() ? 0 : &prev->sts[0];
350
351 if (prevsl == 0 || prevsl->q.isLine()) {
352 return false;
353 }
354 return lerp(0.5, prevsl->q.p[1], q.p[1]) == q.p[0];
355 }
356
findBreaks(vector<Break> * breaks,const Thetas & curve)357 void findBreaks(vector<Break>* breaks, const Thetas& curve) {
358 breaks->clear();
359 double lastd;
360 int n = round(10 * curve.arclen);
361 for (int i = 0; i <= n; i++) {
362 double s = curve.arclen * i / n;
363 Point origp = curve.xy(s);
364 #if HALF_STEP
365 Point p(.5 * std::round(2 * origp.x), .5 * std::round(2 * origp.y));
366 #else
367 Point p = round(origp);
368 #endif
369 double d = dist(p, origp);
370 if (i == 0 || !(p == (*breaks)[breaks->size() - 1].xy)) {
371 Break bk(s, p, curve.dir(s));
372 breaks->push_back(bk);
373 lastd = d;
374 } else if (d < lastd) {
375 (*breaks)[breaks->size() - 1] = Break(s, p, curve.dir(s));
376 lastd = d;
377 }
378 }
379 }
380
intersect(Point * result,Point p0,Point dir0,Point p1,Point dir1)381 bool intersect(Point* result, Point p0, Point dir0, Point p1, Point dir1) {
382 double det = dir0.x * dir1.y - dir0.y * dir1.x;
383 if (std::abs(det) < 1e-6 || std::isnan(det)) return false;
384 det = 1 / det;
385 double a = p0.y * dir0.x - p0.x * dir0.y;
386 double b = p1.y * dir1.x - p1.x * dir1.y;
387 result->x = (a * dir1.x - b * dir0.x) * det;
388 result->y = (a * dir1.y - b * dir0.y) * det;
389 return true;
390 }
391
tryQuad(const State * prev,State * st,const Thetas & curve,const Break & bk0,const Break & bk1,const Quad & q,double penalty)392 void tryQuad(const State* prev, State* st, const Thetas& curve,
393 const Break& bk0, const Break& bk1, const Quad& q, double penalty) {
394 double score = measureQuad(curve, bk0.s, bk1.s, q);
395 st->combine(prev, score, q, penalty);
396 }
397
tryLineQuad(const State * prev,State * st,const Thetas & curve,const Break & bk0,const Break & bk1,double penalty)398 void tryLineQuad(const State* prev, State* st, const Thetas& curve,
399 const Break& bk0, const Break& bk1, double penalty) {
400 if (isInt(bk0.xy.x) && isInt(bk0.xy.y)) {
401 Quad line(bk0.xy, lerp(0.5, bk0.xy, bk1.xy), bk1.xy);
402 tryQuad(prev, st, curve, bk0, bk1, line, penalty);
403 }
404 Point pmid;
405 if (intersect(&pmid, bk0.xy, bk0.dir, bk1.xy, bk1.dir)) {
406 Quad q(bk0.xy, round(pmid), bk1.xy);
407 if (okForHalf(prev, q)) {
408 tryQuad(prev, st, curve, bk0, bk1, q, penalty);
409 }
410 }
411 }
412
optimize(const Thetas & curve,double penalty=1)413 vector<Quad> optimize(const Thetas& curve, double penalty=1) {
414 vector<Break> breaks;
415 findBreaks(&breaks, curve);
416 int n = breaks.size() - 1;
417 vector<State> states;
418 states.resize(n + 1);
419 states[0].init = true;
420 tryLineQuad(&states[0], &states[n], curve, breaks[0], breaks[n], penalty);
421 if (states[n].sts[0].score <= 3 * penalty) {
422 goto done;
423 }
424 for (int i = 1; i < n; i++) {
425 tryLineQuad(&states[0], &states[i], curve, breaks[0], breaks[i], penalty);
426 tryLineQuad(&states[i], &states[n], curve, breaks[i], breaks[n], penalty);
427 }
428 if (states[n].sts[0].score <= 4 * penalty) {
429 goto done;
430 }
431 for (int i = 1; i <= n; i++) {
432 for (int j = i - 1; j >= 0; j--) {
433 tryLineQuad(&states[j], &states[i], curve, breaks[j], breaks[i], penalty);
434 }
435 }
436 done:
437 vector<Quad> result;
438 for (const Statelet* sl = &states[n].sts[0]; sl != 0; sl = sl->prev) {
439 result.push_back(sl->q);
440 }
441 std::reverse(result.begin(), result.end());
442 return result;
443 }
444
readBzs(vector<Quad> * result,std::istream & is)445 void readBzs(vector<Quad>* result, std::istream& is) {
446 double x0, y0, x1, y1, x2, y2;
447 while (is >> x0 >> y0 >> x1 >> y1 >> x2 >> y2) {
448 result->push_back(Quad(Point(x0, y0), Point(x1, y1), Point(x2, y2)));
449 }
450 // Round the endpoints, they must be on integers
451 result->front().p[0] = round(result->front().p[0]);
452 result->back().p[2] = round(result->back().p[2]);
453 }
454
main(int argc,char ** argv)455 int main(int argc, char** argv) {
456 if (argc != 3) {
457 std::cerr << "usage: quadopt in out\n";
458 return 1;
459 }
460 #if 0
461 Quad q(Point(100, 0), Point(0, 0), Point(0, 100));
462 std::cout.precision(8);
463 std::cout << q.arclen() << "\n";
464 #endif
465 vector<Quad> bzs;
466 std::ifstream is;
467 is.open(argv[1]);
468 readBzs(&bzs, is);
469 Thetas thetas;
470 thetas.init(bzs);
471 #if 0
472 for (int i = 0; i < thetas.arclen; i++) {
473 Point xy = thetas.dir(i);
474 std::cout << xy.x << " " << xy.y << std::endl;
475 }
476 #endif
477 vector<Quad> optbzs = optimize(thetas);
478 std::ofstream os;
479 os.open(argv[2]);
480 for (size_t i = 0; i < optbzs.size(); i++) {
481 optbzs[i].print(os);
482 }
483 return 0;
484 }
485