# KOSMIndoorMap

geomath.cpp
1/*
2 SPDX-FileCopyrightText: 2020 Volker Krause <vkrause@kde.org>
3
5*/
6
7#include "geomath.h"
8
9#include <QLineF>
10
11#include <limits>
12
13using namespace OSM;
14
15// see https://en.wikipedia.org/wiki/Haversine_formula
16double OSM::distance(double lat1, double lon1, double lat2, double lon2)
17{
18 constexpr const auto earthRadius = 6371000.0; // in meters
19
20 const auto d_lat = degToRad(lat1 - lat2);
21 const auto d_lon = degToRad(lon1 - lon2);
22
23 const auto a = pow(sin(d_lat / 2.0), 2) + cos(degToRad(lat1)) * cos(degToRad(lat2)) * pow(sin(d_lon / 2.0), 2);
24 return 2.0 * earthRadius * atan2(sqrt(a), sqrt(1.0 - a));
25}
26
27double OSM::distance(Coordinate coord1, Coordinate coord2)
28{
29 return distance(coord1.latF(), coord1.lonF(), coord2.latF(), coord2.lonF());
30}
31
33{
34 QLineF line(l1.lonF(), l1.latF(), l2.lonF(), l2.latF());
35 const auto len = line.length();
36 if (len == 0.0) {
37 return OSM::distance(l1, p);
38 }
39
40 // project p on a line extending the line segment given by @p l1 and @p l2, and clamp to that to the segment
41 QPointF pf(p.lonF(), p.latF());
42 const auto r = qBound(0.0, QPointF::dotProduct(pf - line.p1(), line.p2() - line.p1()) / (len*len), 1.0);
43 const auto intersection = line.p1() + r * (line.p2() - line.p1());
44 return OSM::distance(OSM::Coordinate(intersection.y(), intersection.x()), p);
45}
46
47double OSM::distance(const std::vector<const OSM::Node*> &path, OSM::Coordinate coord)
48{
49 if (path.empty()) {
50 return std::numeric_limits<double>::max();
51 }
52
53 if (path.size() == 1) {
54 return distance(path[0]->coordinate, coord);
55 }
56
57 auto dist = std::numeric_limits<double>::max();
58 OSM::Id firstNode = 0;
59 for (auto it = path.begin(); it != std::prev(path.end()) && it != path.end(); ++it) {
60 const auto nextIt = std::next(it);
61 if (firstNode == 0) { // starting a new loop
62 firstNode = (*it)->id;
63 }
64
65 // compute distance between line segment and coord
66 dist = std::min(dist, OSM::distance((*it)->coordinate, (*nextIt)->coordinate, coord));
67
68 if ((*nextIt)->id == firstNode) { // just closed a loop, so this is not a line on the path
69 firstNode = 0;
70 ++it;
71 }
72 }
73 return dist;
74}
Coordinate, stored as 1e7 * degree to avoid floating point precision issues, and offset to unsigned v...
Definition datatypes.h:37
Low-level types and functions to work with raw OSM data as efficiently as possible.
KOSM_EXPORT double distance(double lat1, double lon1, double lat2, double lon2)
Distance between two coordinates.
Definition geomath.cpp:16
int64_t Id
OSM element identifier.
Definition datatypes.h:30