summaryrefslogtreecommitdiffstats
path: root/demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp
diff options
context:
space:
mode:
Diffstat (limited to 'demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp')
-rw-r--r--demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp358
1 files changed, 358 insertions, 0 deletions
diff --git a/demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp b/demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp
new file mode 100644
index 0000000..ae82706
--- /dev/null
+++ b/demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp
@@ -0,0 +1,358 @@
+#pragma once
+
+#include <mapbox/geometry.hpp>
+
+#include <cmath>
+#include <cstdint>
+#include <limits>
+#include <tuple>
+#include <utility>
+
+namespace mapbox {
+namespace cheap_ruler {
+
+using box = geometry::box<double>;
+using line_string = geometry::line_string<double>;
+using linear_ring = geometry::linear_ring<double>;
+using multi_line_string = geometry::multi_line_string<double>;
+using point = geometry::point<double>;
+using polygon = geometry::polygon<double>;
+
+class CheapRuler {
+public:
+ enum Unit {
+ Kilometers,
+ Miles,
+ NauticalMiles,
+ Meters,
+ Metres = Meters,
+ Yards,
+ Feet,
+ Inches
+ };
+
+ //
+ // A collection of very fast approximations to common geodesic measurements. Useful
+ // for performance-sensitive code that measures things on a city scale. Point coordinates
+ // are in the [x = longitude, y = latitude] form.
+ //
+ explicit CheapRuler(double latitude, Unit unit = Kilometers) {
+ double m = 0.;
+
+ switch (unit) {
+ case Kilometers:
+ m = 1.;
+ break;
+ case Miles:
+ m = 1000. / 1609.344;
+ break;
+ case NauticalMiles:
+ m = 1000. / 1852.;
+ break;
+ case Meters:
+ m = 1000.;
+ break;
+ case Yards:
+ m = 1000. / 0.9144;
+ break;
+ case Feet:
+ m = 1000. / 0.3048;
+ break;
+ case Inches:
+ m = 1000. / 0.0254;
+ break;
+ }
+
+ auto cos = std::cos(latitude * M_PI / 180.);
+ auto cos2 = 2. * cos * cos - 1.;
+ auto cos3 = 2. * cos * cos2 - cos;
+ auto cos4 = 2. * cos * cos3 - cos2;
+ auto cos5 = 2. * cos * cos4 - cos3;
+
+ // multipliers for converting longitude and latitude
+ // degrees into distance (http://1.usa.gov/1Wb1bv7)
+ kx = m * (111.41513 * cos - 0.09455 * cos3 + 0.00012 * cos5);
+ ky = m * (111.13209 - 0.56605 * cos2 + 0.0012 * cos4);
+ }
+
+ static CheapRuler fromTile(uint32_t y, uint32_t z) {
+ double n = M_PI * (1. - 2. * (y + 0.5) / std::pow(2., z));
+ double latitude = std::atan(0.5 * (std::exp(n) - std::exp(-n))) * 180. / M_PI;
+
+ return CheapRuler(latitude);
+ }
+
+ //
+ // Given two points of the form [x = longitude, y = latitude], returns the distance.
+ //
+ double distance(point a, point b) {
+ auto dx = (a.x - b.x) * kx;
+ auto dy = (a.y - b.y) * ky;
+
+ return std::sqrt(dx * dx + dy * dy);
+ }
+
+ //
+ // Returns the bearing between two points in angles.
+ //
+ double bearing(point a, point b) {
+ auto dx = (b.x - a.x) * kx;
+ auto dy = (b.y - a.y) * ky;
+
+ if (!dx && !dy) {
+ return 0.;
+ }
+
+ auto bearing = std::atan2(-dy, dx) * 180. / M_PI + 90.;
+
+ if (bearing > 180.) {
+ bearing -= 360.;
+ }
+
+ return bearing;
+ }
+
+ //
+ // Returns a new point given distance and bearing from the starting point.
+ //
+ point destination(point origin, double dist, double bearing) {
+ auto a = (90. - bearing) * M_PI / 180.;
+
+ return offset(origin, std::cos(a) * dist, std::sin(a) * dist);
+ }
+
+ //
+ // Returns a new point given easting and northing offsets from the starting point.
+ //
+ point offset(point origin, double dx, double dy) {
+ return point(origin.x + dx / kx, origin.y + dy / ky);
+ }
+
+ //
+ // Given a line (an array of points), returns the total line distance.
+ //
+ double lineDistance(const line_string& points) {
+ double total = 0.;
+
+ for (unsigned i = 0; i < points.size() - 1; ++i) {
+ total += distance(points[i], points[i + 1]);
+ }
+
+ return total;
+ }
+
+ //
+ // Given a polygon (an array of rings, where each ring is an array of points),
+ // returns the area.
+ //
+ double area(polygon poly) {
+ double sum = 0.;
+
+ for (unsigned i = 0; i < poly.size(); ++i) {
+ auto& ring = poly[i];
+
+ for (unsigned j = 0, len = ring.size(), k = len - 1; j < len; k = j++) {
+ sum += (ring[j].x - ring[k].x) * (ring[j].y + ring[k].y) * (i ? -1. : 1.);
+ }
+ }
+
+ return (std::abs(sum) / 2.) * kx * ky;
+ }
+
+ //
+ // Returns the point at a specified distance along the line.
+ //
+ point along(const line_string& line, double dist) {
+ double sum = 0.;
+
+ if (dist <= 0.) {
+ return line[0];
+ }
+
+ for (unsigned i = 0; i < line.size() - 1; ++i) {
+ auto p0 = line[i];
+ auto p1 = line[i + 1];
+ auto d = distance(p0, p1);
+
+ sum += d;
+
+ if (sum > dist) {
+ return interpolate(p0, p1, (dist - (sum - d)) / d);
+ }
+ }
+
+ return line[line.size() - 1];
+ }
+
+ //
+ // Returns a pair of the form <point, index> where point is closest point on the line from
+ // the given point and index is the start index of the segment with the closest point.
+ //
+ std::pair<point, unsigned> pointOnLine(const line_string& line, point p) {
+ auto result = _pointOnLine(line, p);
+
+ return std::make_pair(std::get<0>(result), std::get<1>(result));
+ }
+
+ //
+ // Returns a part of the given line between the start and the stop points (or their closest
+ // points on the line).
+ //
+ line_string lineSlice(point start, point stop, const line_string& line) {
+ constexpr auto getPoint = [](auto tuple) { return std::get<0>(tuple); };
+ constexpr auto getIndex = [](auto tuple) { return std::get<1>(tuple); };
+ constexpr auto getT = [](auto tuple) { return std::get<2>(tuple); };
+
+ auto p1 = _pointOnLine(line, start);
+ auto p2 = _pointOnLine(line, stop);
+
+ if (getIndex(p1) > getIndex(p2) || (getIndex(p1) == getIndex(p2) && getT(p1) > getT(p2))) {
+ auto tmp = p1;
+ p1 = p2;
+ p2 = tmp;
+ }
+
+ line_string slice = { getPoint(p1) };
+
+ auto l = getIndex(p1) + 1;
+ auto r = getIndex(p2);
+
+ if (line[l] != slice[0] && l <= r) {
+ slice.push_back(line[l]);
+ }
+
+ for (unsigned i = l + 1; i <= r; ++i) {
+ slice.push_back(line[i]);
+ }
+
+ if (line[r] != getPoint(p2)) {
+ slice.push_back(getPoint(p2));
+ }
+
+ return slice;
+ };
+
+ //
+ // Returns a part of the given line between the start and the stop points
+ // indicated by distance along the line.
+ //
+ line_string lineSliceAlong(double start, double stop, const line_string& line) {
+ double sum = 0.;
+ line_string slice;
+
+ for (unsigned i = 0; i < line.size() - 1; ++i) {
+ auto p0 = line[i];
+ auto p1 = line[i + 1];
+ auto d = distance(p0, p1);
+
+ sum += d;
+
+ if (sum > start && slice.size() == 0) {
+ slice.push_back(interpolate(p0, p1, (start - (sum - d)) / d));
+ }
+
+ if (sum >= stop) {
+ slice.push_back(interpolate(p0, p1, (stop - (sum - d)) / d));
+ return slice;
+ }
+
+ if (sum > start) {
+ slice.push_back(p1);
+ }
+ }
+
+ return slice;
+ };
+
+ //
+ // Given a point, returns a bounding box object ([w, s, e, n])
+ // created from the given point buffered by a given distance.
+ //
+ box bufferPoint(point p, double buffer) {
+ auto v = buffer / ky;
+ auto h = buffer / kx;
+
+ return box(
+ point(p.x - h, p.y - v),
+ point(p.x + h, p.y + v)
+ );
+ }
+
+ //
+ // Given a bounding box, returns the box buffered by a given distance.
+ //
+ box bufferBBox(box bbox, double buffer) {
+ auto v = buffer / ky;
+ auto h = buffer / kx;
+
+ return box(
+ point(bbox.min.x - h, bbox.min.y - v),
+ point(bbox.max.x + h, bbox.max.y + v)
+ );
+ }
+
+ //
+ // Returns true if the given point is inside in the given bounding box, otherwise false.
+ //
+ bool insideBBox(point p, box bbox) {
+ return p.x >= bbox.min.x &&
+ p.x <= bbox.max.x &&
+ p.y >= bbox.min.y &&
+ p.y <= bbox.max.y;
+ }
+
+ static point interpolate(point a, point b, double t) {
+ double dx = b.x - a.x;
+ double dy = b.y - a.y;
+
+ return point(a.x + dx * t, a.y + dy * t);
+ }
+
+private:
+ std::tuple<point, unsigned, double> _pointOnLine(const line_string& line, point p) {
+ double minDist = std::numeric_limits<double>::infinity();
+ double minX = 0., minY = 0., minI = 0., minT = 0.;
+
+ for (unsigned i = 0; i < line.size() - 1; ++i) {
+ auto t = 0.;
+ auto x = line[i].x;
+ auto y = line[i].y;
+ auto dx = (line[i + 1].x - x) * kx;
+ auto dy = (line[i + 1].y - y) * ky;
+
+ if (dx != 0. || dy != 0.) {
+ t = ((p.x - x) * kx * dx + (p.y - y) * ky * dy) / (dx * dx + dy * dy);
+
+ if (t > 1) {
+ x = line[i + 1].x;
+ y = line[i + 1].y;
+
+ } else if (t > 0) {
+ x += (dx / kx) * t;
+ y += (dy / ky) * t;
+ }
+ }
+
+ dx = (p.x - x) * kx;
+ dy = (p.y - y) * ky;
+
+ auto sqDist = dx * dx + dy * dy;
+
+ if (sqDist < minDist) {
+ minDist = sqDist;
+ minX = x;
+ minY = y;
+ minI = i;
+ minT = t;
+ }
+ }
+
+ return std::make_tuple(point(minX, minY), minI, minT);
+ }
+
+ double ky;
+ double kx;
+};
+
+} // namespace cheap_ruler
+} // namespace mapbox