File indexing completed on 2024-10-27 13:24:19

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 }