|
|
@@ -0,0 +1,484 @@
|
|
|
+/*
|
|
|
+ * Copyright 2014 Google Inc. All rights reserved.
|
|
|
+ *
|
|
|
+ * Licensed under the Apache License, Version 2.0 (the "License");
|
|
|
+ * you may not use this file except in compliance with the License.
|
|
|
+ * You may obtain a copy of the License at
|
|
|
+ *
|
|
|
+ * http://www.apache.org/licenses/LICENSE-2.0
|
|
|
+ *
|
|
|
+ * Unless required by applicable law or agreed to in writing, software
|
|
|
+ * distributed under the License is distributed on an "AS IS" BASIS,
|
|
|
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
|
+ * See the License for the specific language governing permissions and
|
|
|
+ * limitations under the License.
|
|
|
+ *
|
|
|
+ * Contributor: Raph Levien
|
|
|
+ */
|
|
|
+
|
|
|
+#include <iostream>
|
|
|
+#include <fstream>
|
|
|
+#include <cmath>
|
|
|
+#include <vector>
|
|
|
+#include <algorithm>
|
|
|
+
|
|
|
+using std::vector;
|
|
|
+
|
|
|
+#define HALF_STEP 1
|
|
|
+
|
|
|
+class Point {
|
|
|
+public:
|
|
|
+ Point() : x(0), y(0) { }
|
|
|
+ Point(double x, double y) : x(x), y(y) { }
|
|
|
+ double x, y;
|
|
|
+};
|
|
|
+
|
|
|
+bool operator==(const Point& p0, const Point& p1) {
|
|
|
+ return p0.x == p1.x && p0.y == p1.y;
|
|
|
+}
|
|
|
+
|
|
|
+std::ostream& operator<<(std::ostream& os, const Point& p) {
|
|
|
+ os << "(" << p.x << ", " << p.y << ")";
|
|
|
+ return os;
|
|
|
+}
|
|
|
+
|
|
|
+double dist(Point p0, Point p1) {
|
|
|
+ return std::hypot(p0.x - p1.x, p0.y - p1.y);
|
|
|
+}
|
|
|
+
|
|
|
+double dist2(Point p0, Point p1) {
|
|
|
+ double dx = p0.x - p1.x;
|
|
|
+ double dy = p0.y - p1.y;
|
|
|
+ return dx * dx + dy * dy;
|
|
|
+}
|
|
|
+
|
|
|
+Point lerp(double t, Point p0, Point p1) {
|
|
|
+ return Point(p0.x + t * (p1.x - p0.x), p0.y + t * (p1.y - p0.y));
|
|
|
+}
|
|
|
+
|
|
|
+Point unitize(Point p) {
|
|
|
+ double scale = 1/std::hypot(p.x, p.y);
|
|
|
+ return Point(p.x * scale, p.y * scale);
|
|
|
+}
|
|
|
+
|
|
|
+Point round(Point p) {
|
|
|
+ return Point(std::round(p.x), std::round(p.y));
|
|
|
+}
|
|
|
+
|
|
|
+class Quad {
|
|
|
+public:
|
|
|
+ Quad() : p() { }
|
|
|
+ Quad(Point p0, Point p1, Point p2) : p() {
|
|
|
+ p[0] = p0;
|
|
|
+ p[1] = p1;
|
|
|
+ p[2] = p2;
|
|
|
+ }
|
|
|
+ Point p[3];
|
|
|
+ double arclen() const;
|
|
|
+ Point eval(double t) const;
|
|
|
+ bool isLine() const;
|
|
|
+ void print(std::ostream& o) const {
|
|
|
+ o << p[0].x << " " << p[0].y << " " << p[1].x << " " << p[1].y << " "
|
|
|
+ << p[2].x << " " << p[2].y << std::endl;
|
|
|
+ }
|
|
|
+};
|
|
|
+
|
|
|
+bool Quad::isLine() const {
|
|
|
+ return p[1] == lerp(0.5, p[0], p[2]);
|
|
|
+}
|
|
|
+
|
|
|
+// One step of a 4th-order Runge-Kutta numerical integration
|
|
|
+template <size_t n, typename F>
|
|
|
+void rk4(double y[n], double x, double h, F& derivs) {
|
|
|
+ double dydx[n];
|
|
|
+ double dyt[n];
|
|
|
+ double dym[n];
|
|
|
+ double yt[n];
|
|
|
+ derivs(dydx, x, y);
|
|
|
+ double hh = h * .5;
|
|
|
+ double h6 = h * (1./6);
|
|
|
+ for (size_t i = 0; i < n; i++) {
|
|
|
+ yt[i] = y[i] + hh * dydx[i];
|
|
|
+ }
|
|
|
+ derivs(dyt, x + hh, yt);
|
|
|
+ for (size_t i = 0; i < n; i++) {
|
|
|
+ yt[i] = y[i] + hh * dyt[i];
|
|
|
+ }
|
|
|
+ derivs(dym, x + hh, yt);
|
|
|
+ for (size_t i = 0; i < n; i++) {
|
|
|
+ yt[i] = y[i] + h * dym[i];
|
|
|
+ dym[i] += dyt[i];
|
|
|
+ }
|
|
|
+ derivs(dyt, x + h, yt);
|
|
|
+ for (size_t i = 0; i < n; i++) {
|
|
|
+ y[i] += h6 * (dydx[i] + dyt[i] + 2 * dym[i]);
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+class ArclenFunctor {
|
|
|
+public:
|
|
|
+ ArclenFunctor(const Quad& q)
|
|
|
+ : dx0(2 * (q.p[1].x - q.p[0].x))
|
|
|
+ , dx1(2 * (q.p[2].x - q.p[1].x))
|
|
|
+ , dy0(2 * (q.p[1].y - q.p[0].y))
|
|
|
+ , dy1(2 * (q.p[2].y - q.p[1].y)) { }
|
|
|
+ void operator()(double dydx[1], double t, const double y[1]) {
|
|
|
+ Point p(deriv(t));
|
|
|
+ dydx[0] = std::hypot(p.x, p.y);
|
|
|
+ }
|
|
|
+ Point deriv(double t) const {
|
|
|
+ return Point(dx0 + t * (dx1 - dx0), dy0 + t * (dy1 - dy0));
|
|
|
+ }
|
|
|
+private:
|
|
|
+ double dx0, dy0, dx1, dy1;
|
|
|
+};
|
|
|
+
|
|
|
+double Quad::arclen() const {
|
|
|
+ ArclenFunctor derivs(*this);
|
|
|
+ const int n = 10;
|
|
|
+ double dt = 1./n;
|
|
|
+ double t = 0;
|
|
|
+ double y[1] = { 0 };
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ rk4<1>(y, t, dt, derivs);
|
|
|
+ t += dt;
|
|
|
+ }
|
|
|
+ return y[0];
|
|
|
+}
|
|
|
+
|
|
|
+Point Quad::eval(double t) const {
|
|
|
+ Point p01(lerp(t, p[0], p[1]));
|
|
|
+ Point p12(lerp(t, p[1], p[2]));
|
|
|
+ return lerp(t, p01, p12);
|
|
|
+}
|
|
|
+
|
|
|
+class Thetas {
|
|
|
+public:
|
|
|
+ void init(const vector<Quad>& qs);
|
|
|
+ Point xy(double s) const;
|
|
|
+ Point dir(double s) const;
|
|
|
+ double arclen;
|
|
|
+private:
|
|
|
+ vector<Point> xys;
|
|
|
+ vector<Point> dirs;
|
|
|
+};
|
|
|
+
|
|
|
+void Thetas::init(const vector<Quad>& qs) {
|
|
|
+ xys.clear();
|
|
|
+ dirs.clear();
|
|
|
+ double s = 0;
|
|
|
+ int ix = 0;
|
|
|
+ Point lastxy;
|
|
|
+ Point lastd;
|
|
|
+ double lasts = -1;
|
|
|
+ for (size_t i = 0; i < qs.size(); i++) {
|
|
|
+ const Quad& q = qs[i];
|
|
|
+ ArclenFunctor derivs(q);
|
|
|
+ const int n = 100;
|
|
|
+ double dt = 1./n;
|
|
|
+ double t = 0;
|
|
|
+ double y[1];
|
|
|
+ y[0] = s;
|
|
|
+ for (int j = 0; j < n; j++) {
|
|
|
+ Point thisxy(q.eval(t));
|
|
|
+ Point thisd(derivs.deriv(t));
|
|
|
+ while (ix <= y[0]) {
|
|
|
+ double u = (ix - lasts) / (y[0] - lasts);
|
|
|
+ xys.push_back(lerp(u, lastxy, thisxy));
|
|
|
+ dirs.push_back(unitize(lerp(u, lastd, thisd)));
|
|
|
+ ix++;
|
|
|
+ }
|
|
|
+ lasts = y[0];
|
|
|
+ rk4<1>(y, t, dt, derivs);
|
|
|
+ t += dt;
|
|
|
+ lastxy = thisxy;
|
|
|
+ lastd = thisd;
|
|
|
+ }
|
|
|
+ s = y[0];
|
|
|
+ }
|
|
|
+ const Quad& q = qs[qs.size() - 1];
|
|
|
+ Point thisxy(q.p[2]);
|
|
|
+ Point thisd(ArclenFunctor(q).deriv(1));
|
|
|
+ while (ix <= s + 1) {
|
|
|
+ double u = (ix - lasts) / (s - lasts);
|
|
|
+ xys.push_back(lerp(u, lastxy, thisxy));
|
|
|
+ dirs.push_back(unitize(lerp(u, lastd, thisd)));
|
|
|
+ ix++;
|
|
|
+ }
|
|
|
+ arclen = s;
|
|
|
+}
|
|
|
+
|
|
|
+Point Thetas::xy(double s) const {
|
|
|
+ int bucket = (int)s;
|
|
|
+ double frac = s - bucket;
|
|
|
+ return lerp(frac, xys[bucket], xys[bucket + 1]);
|
|
|
+}
|
|
|
+
|
|
|
+Point Thetas::dir(double s) const {
|
|
|
+ int bucket = (int)s;
|
|
|
+ double frac = s - bucket;
|
|
|
+ return lerp(frac, dirs[bucket], dirs[bucket + 1]);
|
|
|
+}
|
|
|
+
|
|
|
+#define NORM_LEVEL 2
|
|
|
+
|
|
|
+// L1 angle norm, 2, L2 angle norm, 0.05
|
|
|
+// L1 distance norm, 200
|
|
|
+double penalty = 1;
|
|
|
+double dist_factor = .005;
|
|
|
+double angle_factor = 5;
|
|
|
+
|
|
|
+
|
|
|
+class MeasureFunctor {
|
|
|
+public:
|
|
|
+ MeasureFunctor(const Thetas& curve, double s0, double ss, const ArclenFunctor& af,
|
|
|
+ Quad q)
|
|
|
+ : curve(curve), s0(s0), ss(ss), af(af), q(q) { }
|
|
|
+ void operator()(double dydx[2], double t, const double y[2]) {
|
|
|
+ Point dxy(af.deriv(t));
|
|
|
+ dydx[0] = std::hypot(dxy.x, dxy.y);
|
|
|
+
|
|
|
+ // distance error
|
|
|
+ Point curvexy = curve.xy(s0 + y[0] * ss);
|
|
|
+#if NORM_LEVEL == 1
|
|
|
+ double disterr = dist(q.eval(t), curvexy);
|
|
|
+#endif
|
|
|
+#if NORM_LEVEL == 2
|
|
|
+ double disterr = dist2(q.eval(t), curvexy);
|
|
|
+#endif
|
|
|
+ disterr *= dydx[0];
|
|
|
+
|
|
|
+ // angle error
|
|
|
+ Point dir = curve.dir(s0 + y[0] * ss);
|
|
|
+ double angleerr = dir.x * dxy.y - dir.y * dxy.x;
|
|
|
+#if NORM_LEVEL == 1
|
|
|
+ angleerr = std::abs(angleerr);
|
|
|
+#endif
|
|
|
+#if NORM_LEVEL == 2
|
|
|
+ angleerr = (angleerr * angleerr) / dydx[0];
|
|
|
+#endif
|
|
|
+
|
|
|
+ dydx[1] = dist_factor * disterr + angle_factor * angleerr;
|
|
|
+ }
|
|
|
+private:
|
|
|
+ const Thetas& curve;
|
|
|
+ double s0;
|
|
|
+ double ss;
|
|
|
+ const ArclenFunctor& af;
|
|
|
+ Quad q;
|
|
|
+};
|
|
|
+
|
|
|
+// measure how closely the quad fits the section of curve, using L1 norm
|
|
|
+// of angle mismatch
|
|
|
+double measureQuad(const Thetas& curve, double s0, double s1, const Quad& q) {
|
|
|
+ ArclenFunctor derivs(q);
|
|
|
+ double ss = (s1 - s0) / q.arclen();
|
|
|
+ MeasureFunctor err(curve, s0, ss, derivs, q);
|
|
|
+ const int n = 10;
|
|
|
+ double dt = 1./n;
|
|
|
+ double t = 0;
|
|
|
+ double y[2] = { 0, 0 };
|
|
|
+ for (int i = 0; i < n; i++) {
|
|
|
+ rk4<2>(y, t, dt, err);
|
|
|
+ t += dt;
|
|
|
+ }
|
|
|
+ return y[1];
|
|
|
+}
|
|
|
+
|
|
|
+struct Break {
|
|
|
+ Break(double s, Point xy, Point dir) : s(s), xy(xy), dir(dir) { }
|
|
|
+ double s;
|
|
|
+ Point xy;
|
|
|
+ Point dir;
|
|
|
+};
|
|
|
+
|
|
|
+struct Statelet {
|
|
|
+ void combine(const Statelet* prev, double score, Quad q);
|
|
|
+ const Statelet* prev;
|
|
|
+ double score;
|
|
|
+ Quad q;
|
|
|
+};
|
|
|
+
|
|
|
+void Statelet::combine(const Statelet* newprev, double newscore, Quad newq) {
|
|
|
+ prev = newprev;
|
|
|
+ double pmul = 2;
|
|
|
+ if (newq.isLine()) {
|
|
|
+ pmul = 1;
|
|
|
+ } else if (newprev != 0 && !newprev->q.isLine()
|
|
|
+ && lerp(0.5, newprev->q.p[1], newq.p[1]) == newq.p[0]) {
|
|
|
+ pmul = 1;
|
|
|
+ }
|
|
|
+ score = (newprev == 0 ? 0 : newprev->score) + penalty * pmul + newscore;
|
|
|
+ q = newq;
|
|
|
+}
|
|
|
+
|
|
|
+struct State {
|
|
|
+ void combine(const State* prev, double score, Quad q);
|
|
|
+ vector<Statelet> sts;
|
|
|
+ bool init;
|
|
|
+};
|
|
|
+
|
|
|
+void State::combine(const State* prev, double score, Quad q) {
|
|
|
+ const Statelet* prevsl = prev->sts.empty() ? 0 : &prev->sts[0];
|
|
|
+ if (prevsl == 0 && !prev->init) {
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ Statelet sl;
|
|
|
+ sl.combine(prevsl, score, q);
|
|
|
+ if (sts.empty()) {
|
|
|
+ sts.push_back(sl);
|
|
|
+ } else {
|
|
|
+ if (sl.score < sts[0].score) {
|
|
|
+ sts[0] = sl;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+bool isInt(double x) {
|
|
|
+ return x == (int) x;
|
|
|
+}
|
|
|
+
|
|
|
+bool okForHalf(const State* prev, Quad q) {
|
|
|
+ if (isInt(q.p[0].x) && isInt(q.p[0].y)) {
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ if (q.isLine()) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ const Statelet* prevsl = prev->sts.empty() ? 0 : &prev->sts[0];
|
|
|
+
|
|
|
+ if (prevsl == 0 || prevsl->q.isLine()) {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return lerp(0.5, prevsl->q.p[1], q.p[1]) == q.p[0];
|
|
|
+}
|
|
|
+
|
|
|
+void findBreaks(vector<Break>* breaks, const Thetas& curve) {
|
|
|
+ breaks->clear();
|
|
|
+ double lastd;
|
|
|
+ int n = round(10 * curve.arclen);
|
|
|
+ for (int i = 0; i <= n; i++) {
|
|
|
+ double s = curve.arclen * i / n;
|
|
|
+ Point origp = curve.xy(s);
|
|
|
+#if HALF_STEP
|
|
|
+ Point p(.5 * std::round(2 * origp.x), .5 * std::round(2 * origp.y));
|
|
|
+#else
|
|
|
+ Point p = round(origp);
|
|
|
+#endif
|
|
|
+ double d = dist(p, origp);
|
|
|
+ if (i == 0 || !(p == (*breaks)[breaks->size() - 1].xy)) {
|
|
|
+ Break bk(s, p, curve.dir(s));
|
|
|
+ breaks->push_back(bk);
|
|
|
+ lastd = d;
|
|
|
+ } else if (d < lastd) {
|
|
|
+ (*breaks)[breaks->size() - 1] = Break(s, p, curve.dir(s));
|
|
|
+ lastd = d;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+bool intersect(Point* result, Point p0, Point dir0, Point p1, Point dir1) {
|
|
|
+ double det = dir0.x * dir1.y - dir0.y * dir1.x;
|
|
|
+ if (std::abs(det) < 1e-6) return false;
|
|
|
+ det = 1 / det;
|
|
|
+ double a = p0.y * dir0.x - p0.x * dir0.y;
|
|
|
+ double b = p1.y * dir1.x - p1.x * dir1.y;
|
|
|
+ result->x = (a * dir1.x - b * dir0.x) * det;
|
|
|
+ result->y = (a * dir1.y - b * dir0.y) * det;
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+void tryQuad(const State* prev, State* st, const Thetas& curve,
|
|
|
+ const Break& bk0, const Break& bk1, const Quad& q) {
|
|
|
+ double score = measureQuad(curve, bk0.s, bk1.s, q);
|
|
|
+ st->combine(prev, score, q);
|
|
|
+}
|
|
|
+
|
|
|
+void tryLineQuad(const State* prev, State* st, const Thetas& curve,
|
|
|
+ const Break& bk0, const Break& bk1) {
|
|
|
+ if (isInt(bk0.xy.x) && isInt(bk0.xy.y)) {
|
|
|
+ Quad line(bk0.xy, lerp(0.5, bk0.xy, bk1.xy), bk1.xy);
|
|
|
+ tryQuad(prev, st, curve, bk0, bk1, line);
|
|
|
+ }
|
|
|
+ Point pmid;
|
|
|
+ if (intersect(&pmid, bk0.xy, bk0.dir, bk1.xy, bk1.dir)) {
|
|
|
+ Quad q(bk0.xy, round(pmid), bk1.xy);
|
|
|
+ if (okForHalf(prev, q)) {
|
|
|
+ tryQuad(prev, st, curve, bk0, bk1, q);
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+vector<Quad> optimize(const Thetas& curve) {
|
|
|
+ vector<Break> breaks;
|
|
|
+ findBreaks(&breaks, curve);
|
|
|
+ int n = breaks.size() - 1;
|
|
|
+ vector<State> states;
|
|
|
+ states.resize(n + 1);
|
|
|
+ states[0].init = true;
|
|
|
+ tryLineQuad(&states[0], &states[n], curve, breaks[0], breaks[n]);
|
|
|
+ if (states[n].sts[0].score <= 3 * penalty) {
|
|
|
+ goto done;
|
|
|
+ }
|
|
|
+ for (int i = 1; i < n; i++) {
|
|
|
+ tryLineQuad(&states[0], &states[i], curve, breaks[0], breaks[i]);
|
|
|
+ tryLineQuad(&states[i], &states[n], curve, breaks[i], breaks[n]);
|
|
|
+ }
|
|
|
+ if (states[n].sts[0].score <= 4 * penalty) {
|
|
|
+ goto done;
|
|
|
+ }
|
|
|
+ for (int i = 1; i <= n; i++) {
|
|
|
+ for (int j = i - 1; j >= 0; j--) {
|
|
|
+ tryLineQuad(&states[j], &states[i], curve, breaks[j], breaks[i]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+done:
|
|
|
+ vector<Quad> result;
|
|
|
+ for (const Statelet* sl = &states[n].sts[0]; sl != 0; sl = sl->prev) {
|
|
|
+ result.push_back(sl->q);
|
|
|
+ }
|
|
|
+ std::reverse(result.begin(), result.end());
|
|
|
+ return result;
|
|
|
+}
|
|
|
+
|
|
|
+void readBzs(vector<Quad>* result, std::istream& is) {
|
|
|
+ double x0, y0, x1, y1, x2, y2;
|
|
|
+ while (is >> x0 >> y0 >> x1 >> y1 >> x2 >> y2) {
|
|
|
+ result->push_back(Quad(Point(x0, y0), Point(x1, y1), Point(x2, y2)));
|
|
|
+ }
|
|
|
+ // Round the endpoints, they must be on integers
|
|
|
+ (*result)[0].p[0] = round((*result)[0].p[0]);
|
|
|
+ Quad* lastq = &(*result)[(*result).size()];
|
|
|
+ lastq->p[2] = round(lastq->p[2]);
|
|
|
+}
|
|
|
+
|
|
|
+int main(int argc, char** argv) {
|
|
|
+ if (argc != 3) {
|
|
|
+ std::cerr << "usage: quadopt in out\n";
|
|
|
+ return 1;
|
|
|
+ }
|
|
|
+#if 0
|
|
|
+ Quad q(Point(100, 0), Point(0, 0), Point(0, 100));
|
|
|
+ std::cout.precision(8);
|
|
|
+ std::cout << q.arclen() << "\n";
|
|
|
+#endif
|
|
|
+ vector<Quad> bzs;
|
|
|
+ std::ifstream is;
|
|
|
+ is.open(argv[1]);
|
|
|
+ readBzs(&bzs, is);
|
|
|
+ Thetas thetas;
|
|
|
+ thetas.init(bzs);
|
|
|
+#if 0
|
|
|
+ for (int i = 0; i < thetas.arclen; i++) {
|
|
|
+ Point xy = thetas.dir(i);
|
|
|
+ std::cout << xy.x << " " << xy.y << std::endl;
|
|
|
+ }
|
|
|
+#endif
|
|
|
+ vector<Quad> optbzs = optimize(thetas);
|
|
|
+ std::ofstream os;
|
|
|
+ os.open(argv[2]);
|
|
|
+ for (size_t i = 0; i < optbzs.size(); i++) {
|
|
|
+ optbzs[i].print(os);
|
|
|
+ }
|
|
|
+ return 0;
|
|
|
+}
|
Roozbeh Pournader