File indexing completed on 2024-12-01 07:25:50
0001 /* 0002 SPDX-FileCopyrightText: 2020 Volker Krause <vkrause@kde.org> 0003 0004 SPDX-License-Identifier: LGPL-2.0-or-later 0005 */ 0006 0007 #include "geomath.h" 0008 0009 #include <QLineF> 0010 0011 #include <limits> 0012 0013 using namespace OSM; 0014 0015 // see https://en.wikipedia.org/wiki/Haversine_formula 0016 double OSM::distance(double lat1, double lon1, double lat2, double lon2) 0017 { 0018 constexpr const auto earthRadius = 6371000.0; // in meters 0019 0020 const auto d_lat = degToRad(lat1 - lat2); 0021 const auto d_lon = degToRad(lon1 - lon2); 0022 0023 const auto a = pow(sin(d_lat / 2.0), 2) + cos(degToRad(lat1)) * cos(degToRad(lat2)) * pow(sin(d_lon / 2.0), 2); 0024 return 2.0 * earthRadius * atan2(sqrt(a), sqrt(1.0 - a)); 0025 } 0026 0027 double OSM::distance(Coordinate coord1, Coordinate coord2) 0028 { 0029 return distance(coord1.latF(), coord1.lonF(), coord2.latF(), coord2.lonF()); 0030 } 0031 0032 double OSM::distance(OSM::Coordinate l1, OSM::Coordinate l2, OSM::Coordinate p) 0033 { 0034 QLineF line(l1.lonF(), l1.latF(), l2.lonF(), l2.latF()); 0035 const auto len = line.length(); 0036 if (len == 0.0) { 0037 return OSM::distance(l1, p); 0038 } 0039 0040 // project p on a line extending the line segment given by @p l1 and @p l2, and clamp to that to the segment 0041 QPointF pf(p.lonF(), p.latF()); 0042 const auto r = qBound(0.0, QPointF::dotProduct(pf - line.p1(), line.p2() - line.p1()) / (len*len), 1.0); 0043 const auto intersection = line.p1() + r * (line.p2() - line.p1()); 0044 return OSM::distance(OSM::Coordinate(intersection.y(), intersection.x()), p); 0045 } 0046 0047 double OSM::distance(const std::vector<const OSM::Node*> &path, OSM::Coordinate coord) 0048 { 0049 if (path.empty()) { 0050 return std::numeric_limits<double>::max(); 0051 } 0052 0053 if (path.size() == 1) { 0054 return distance(path[0]->coordinate, coord); 0055 } 0056 0057 auto dist = std::numeric_limits<double>::max(); 0058 OSM::Id firstNode = 0; 0059 for (auto it = path.begin(); it != std::prev(path.end()) && it != path.end(); ++it) { 0060 const auto nextIt = std::next(it); 0061 if (firstNode == 0) { // starting a new loop 0062 firstNode = (*it)->id; 0063 } 0064 0065 // compute distance between line segment and coord 0066 dist = std::min(dist, OSM::distance((*it)->coordinate, (*nextIt)->coordinate, coord)); 0067 0068 if ((*nextIt)->id == firstNode) { // just closed a loop, so this is not a line on the path 0069 firstNode = 0; 0070 ++it; 0071 } 0072 } 0073 return dist; 0074 }