From 5d98ec36d66db8897f7e3e5120b43c50f379baea Mon Sep 17 00:00:00 2001 From: zheng_wenlong Date: Wed, 14 Nov 2018 12:50:56 +0900 Subject: Add tbtnavi for vertical mode Add tbtnavi for vertical mode. This application use waltham transmitter send video to horizontal mode. Change-Id: Icc8bb14f9c83439d90f28d0960e435a43a5e5245 Signed-off-by: zheng_wenlong --- .../tbtnavi_demo/include/mapbox/cheap_ruler.hpp | 358 +++++++++++++++++++++ 1 file changed, 358 insertions(+) create mode 100644 demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp (limited to 'demo3/vertical/tbtnavi_demo/include/mapbox/cheap_ruler.hpp') 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 + +#include +#include +#include +#include +#include + +namespace mapbox { +namespace cheap_ruler { + +using box = geometry::box; +using line_string = geometry::line_string; +using linear_ring = geometry::linear_ring; +using multi_line_string = geometry::multi_line_string; +using point = geometry::point; +using polygon = geometry::polygon; + +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 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 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 _pointOnLine(const line_string& line, point p) { + double minDist = std::numeric_limits::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 -- cgit 1.2.3-korg