5e167b8745
This PR adds more advanced coordinate extraction, analysing the road to detect offsets due to OSM way modelling. In addition it improves the handling of bearings. Right now OSM reports bearings simply based on the very first coordinate along a way. With this PR, we store the bearings for a turn correctly, making the bearings for turns correct.
500 lines
22 KiB
C++
500 lines
22 KiB
C++
#ifndef GEOSPATIAL_QUERY_HPP
|
|
#define GEOSPATIAL_QUERY_HPP
|
|
|
|
#include "engine/phantom_node.hpp"
|
|
#include "util/bearing.hpp"
|
|
#include "util/coordinate_calculation.hpp"
|
|
#include "util/rectangle.hpp"
|
|
#include "util/typedefs.hpp"
|
|
#include "util/web_mercator.hpp"
|
|
|
|
#include "osrm/coordinate.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <memory>
|
|
#include <vector>
|
|
|
|
namespace osrm
|
|
{
|
|
namespace engine
|
|
{
|
|
|
|
inline std::pair<bool, bool> boolPairAnd(const std::pair<bool, bool> &A,
|
|
const std::pair<bool, bool> &B)
|
|
{
|
|
return std::make_pair(A.first && B.first, A.second && B.second);
|
|
}
|
|
|
|
// Implements complex queries on top of an RTree and builds PhantomNodes from it.
|
|
//
|
|
// Only holds a weak reference on the RTree and coordinates!
|
|
template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
|
|
{
|
|
using EdgeData = typename RTreeT::EdgeData;
|
|
using CoordinateList = typename RTreeT::CoordinateList;
|
|
using CandidateSegment = typename RTreeT::CandidateSegment;
|
|
|
|
public:
|
|
GeospatialQuery(RTreeT &rtree_, const CoordinateList &coordinates_, DataFacadeT &datafacade_)
|
|
: rtree(rtree_), coordinates(coordinates_), datafacade(datafacade_)
|
|
{
|
|
}
|
|
|
|
std::vector<EdgeData> Search(const util::RectangleInt2D &bbox)
|
|
{
|
|
return rtree.SearchInBox(bbox);
|
|
}
|
|
|
|
// Returns nearest PhantomNodes in the given bearing range within max_distance.
|
|
// Does not filter by small/big component!
|
|
std::vector<PhantomNodeWithDistance>
|
|
NearestPhantomNodesInRange(const util::Coordinate input_coordinate,
|
|
const double max_distance) const
|
|
{
|
|
auto results =
|
|
rtree.Nearest(input_coordinate,
|
|
[this](const CandidateSegment &segment) { return HasValidEdge(segment); },
|
|
[this, max_distance, input_coordinate](const std::size_t,
|
|
const CandidateSegment &segment) {
|
|
return CheckSegmentDistance(input_coordinate, segment, max_distance);
|
|
});
|
|
|
|
return MakePhantomNodes(input_coordinate, results);
|
|
}
|
|
|
|
// Returns nearest PhantomNodes in the given bearing range within max_distance.
|
|
// Does not filter by small/big component!
|
|
std::vector<PhantomNodeWithDistance>
|
|
NearestPhantomNodesInRange(const util::Coordinate input_coordinate,
|
|
const double max_distance,
|
|
const int bearing,
|
|
const int bearing_range) const
|
|
{
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, bearing, bearing_range, max_distance](const CandidateSegment &segment) {
|
|
return boolPairAnd(CheckSegmentBearing(segment, bearing, bearing_range),
|
|
HasValidEdge(segment));
|
|
},
|
|
[this, max_distance, input_coordinate](const std::size_t,
|
|
const CandidateSegment &segment) {
|
|
return CheckSegmentDistance(input_coordinate, segment, max_distance);
|
|
});
|
|
|
|
return MakePhantomNodes(input_coordinate, results);
|
|
}
|
|
|
|
// Returns max_results nearest PhantomNodes in the given bearing range.
|
|
// Does not filter by small/big component!
|
|
std::vector<PhantomNodeWithDistance>
|
|
NearestPhantomNodes(const util::Coordinate input_coordinate,
|
|
const unsigned max_results,
|
|
const int bearing,
|
|
const int bearing_range) const
|
|
{
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, bearing, bearing_range](const CandidateSegment &segment) {
|
|
return boolPairAnd(CheckSegmentBearing(segment, bearing, bearing_range),
|
|
HasValidEdge(segment));
|
|
},
|
|
[max_results](const std::size_t num_results, const CandidateSegment &) {
|
|
return num_results >= max_results;
|
|
});
|
|
|
|
return MakePhantomNodes(input_coordinate, results);
|
|
}
|
|
|
|
// Returns max_results nearest PhantomNodes in the given bearing range within the maximum
|
|
// distance.
|
|
// Does not filter by small/big component!
|
|
std::vector<PhantomNodeWithDistance>
|
|
NearestPhantomNodes(const util::Coordinate input_coordinate,
|
|
const unsigned max_results,
|
|
const double max_distance,
|
|
const int bearing,
|
|
const int bearing_range) const
|
|
{
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, bearing, bearing_range](const CandidateSegment &segment) {
|
|
return boolPairAnd(CheckSegmentBearing(segment, bearing, bearing_range),
|
|
HasValidEdge(segment));
|
|
},
|
|
[this, max_distance, max_results, input_coordinate](const std::size_t num_results,
|
|
const CandidateSegment &segment) {
|
|
return num_results >= max_results ||
|
|
CheckSegmentDistance(input_coordinate, segment, max_distance);
|
|
});
|
|
|
|
return MakePhantomNodes(input_coordinate, results);
|
|
}
|
|
|
|
// Returns max_results nearest PhantomNodes.
|
|
// Does not filter by small/big component!
|
|
std::vector<PhantomNodeWithDistance>
|
|
NearestPhantomNodes(const util::Coordinate input_coordinate, const unsigned max_results) const
|
|
{
|
|
auto results =
|
|
rtree.Nearest(input_coordinate,
|
|
[this](const CandidateSegment &segment) { return HasValidEdge(segment); },
|
|
[max_results](const std::size_t num_results, const CandidateSegment &) {
|
|
return num_results >= max_results;
|
|
});
|
|
|
|
return MakePhantomNodes(input_coordinate, results);
|
|
}
|
|
|
|
// Returns max_results nearest PhantomNodes in the given max distance.
|
|
// Does not filter by small/big component!
|
|
std::vector<PhantomNodeWithDistance>
|
|
NearestPhantomNodes(const util::Coordinate input_coordinate,
|
|
const unsigned max_results,
|
|
const double max_distance) const
|
|
{
|
|
auto results =
|
|
rtree.Nearest(input_coordinate,
|
|
[this](const CandidateSegment &segment) { return HasValidEdge(segment); },
|
|
[this, max_distance, max_results, input_coordinate](
|
|
const std::size_t num_results, const CandidateSegment &segment) {
|
|
return num_results >= max_results ||
|
|
CheckSegmentDistance(input_coordinate, segment, max_distance);
|
|
});
|
|
|
|
return MakePhantomNodes(input_coordinate, results);
|
|
}
|
|
|
|
// Returns the nearest phantom node. If this phantom node is not from a big component
|
|
// a second phantom node is return that is the nearest coordinate in a big component.
|
|
std::pair<PhantomNode, PhantomNode>
|
|
NearestPhantomNodeWithAlternativeFromBigComponent(const util::Coordinate input_coordinate,
|
|
const double max_distance) const
|
|
{
|
|
bool has_small_component = false;
|
|
bool has_big_component = false;
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, &has_big_component, &has_small_component](const CandidateSegment &segment) {
|
|
auto use_segment = (!has_small_component ||
|
|
(!has_big_component && !segment.data.component.is_tiny));
|
|
auto use_directions = std::make_pair(use_segment, use_segment);
|
|
const auto valid_edges = HasValidEdge(segment);
|
|
|
|
if (valid_edges.first || valid_edges.second)
|
|
{
|
|
has_big_component = has_big_component || !segment.data.component.is_tiny;
|
|
has_small_component = has_small_component || segment.data.component.is_tiny;
|
|
}
|
|
use_directions = boolPairAnd(use_directions, valid_edges);
|
|
return use_directions;
|
|
},
|
|
[this, &has_big_component, max_distance, input_coordinate](
|
|
const std::size_t num_results, const CandidateSegment &segment) {
|
|
return (num_results > 0 && has_big_component) ||
|
|
CheckSegmentDistance(input_coordinate, segment, max_distance);
|
|
});
|
|
|
|
if (results.size() == 0)
|
|
{
|
|
return std::make_pair(PhantomNode{}, PhantomNode{});
|
|
}
|
|
|
|
BOOST_ASSERT(results.size() == 1 || results.size() == 2);
|
|
return std::make_pair(MakePhantomNode(input_coordinate, results.front()).phantom_node,
|
|
MakePhantomNode(input_coordinate, results.back()).phantom_node);
|
|
}
|
|
|
|
// Returns the nearest phantom node. If this phantom node is not from a big component
|
|
// a second phantom node is return that is the nearest coordinate in a big component.
|
|
std::pair<PhantomNode, PhantomNode>
|
|
NearestPhantomNodeWithAlternativeFromBigComponent(const util::Coordinate input_coordinate) const
|
|
{
|
|
bool has_small_component = false;
|
|
bool has_big_component = false;
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, &has_big_component, &has_small_component](const CandidateSegment &segment) {
|
|
auto use_segment = (!has_small_component ||
|
|
(!has_big_component && !segment.data.component.is_tiny));
|
|
auto use_directions = std::make_pair(use_segment, use_segment);
|
|
if (!use_directions.first && !use_directions.second)
|
|
return use_directions;
|
|
const auto valid_edges = HasValidEdge(segment);
|
|
|
|
if (valid_edges.first || valid_edges.second)
|
|
{
|
|
|
|
has_big_component = has_big_component || !segment.data.component.is_tiny;
|
|
has_small_component = has_small_component || segment.data.component.is_tiny;
|
|
}
|
|
|
|
use_directions = boolPairAnd(use_directions, valid_edges);
|
|
return use_directions;
|
|
},
|
|
[&has_big_component](const std::size_t num_results, const CandidateSegment &) {
|
|
return num_results > 0 && has_big_component;
|
|
});
|
|
|
|
if (results.size() == 0)
|
|
{
|
|
return std::make_pair(PhantomNode{}, PhantomNode{});
|
|
}
|
|
|
|
BOOST_ASSERT(results.size() == 1 || results.size() == 2);
|
|
return std::make_pair(MakePhantomNode(input_coordinate, results.front()).phantom_node,
|
|
MakePhantomNode(input_coordinate, results.back()).phantom_node);
|
|
}
|
|
|
|
// Returns the nearest phantom node. If this phantom node is not from a big component
|
|
// a second phantom node is return that is the nearest coordinate in a big component.
|
|
std::pair<PhantomNode, PhantomNode> NearestPhantomNodeWithAlternativeFromBigComponent(
|
|
const util::Coordinate input_coordinate, const int bearing, const int bearing_range) const
|
|
{
|
|
bool has_small_component = false;
|
|
bool has_big_component = false;
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, bearing, bearing_range, &has_big_component, &has_small_component](
|
|
const CandidateSegment &segment) {
|
|
auto use_segment = (!has_small_component ||
|
|
(!has_big_component && !segment.data.component.is_tiny));
|
|
auto use_directions = std::make_pair(use_segment, use_segment);
|
|
use_directions = boolPairAnd(use_directions, HasValidEdge(segment));
|
|
|
|
if (use_segment)
|
|
{
|
|
use_directions =
|
|
boolPairAnd(CheckSegmentBearing(segment, bearing, bearing_range),
|
|
HasValidEdge(segment));
|
|
if (use_directions.first || use_directions.second)
|
|
{
|
|
has_big_component = has_big_component || !segment.data.component.is_tiny;
|
|
has_small_component = has_small_component || segment.data.component.is_tiny;
|
|
}
|
|
}
|
|
|
|
return use_directions;
|
|
},
|
|
[&has_big_component](const std::size_t num_results, const CandidateSegment &) {
|
|
return num_results > 0 && has_big_component;
|
|
});
|
|
|
|
if (results.size() == 0)
|
|
{
|
|
return std::make_pair(PhantomNode{}, PhantomNode{});
|
|
}
|
|
|
|
BOOST_ASSERT(results.size() > 0);
|
|
return std::make_pair(MakePhantomNode(input_coordinate, results.front()).phantom_node,
|
|
MakePhantomNode(input_coordinate, results.back()).phantom_node);
|
|
}
|
|
|
|
// Returns the nearest phantom node. If this phantom node is not from a big component
|
|
// a second phantom node is return that is the nearest coordinate in a big component.
|
|
std::pair<PhantomNode, PhantomNode>
|
|
NearestPhantomNodeWithAlternativeFromBigComponent(const util::Coordinate input_coordinate,
|
|
const double max_distance,
|
|
const int bearing,
|
|
const int bearing_range) const
|
|
{
|
|
bool has_small_component = false;
|
|
bool has_big_component = false;
|
|
auto results = rtree.Nearest(
|
|
input_coordinate,
|
|
[this, bearing, bearing_range, &has_big_component, &has_small_component](
|
|
const CandidateSegment &segment) {
|
|
auto use_segment = (!has_small_component ||
|
|
(!has_big_component && !segment.data.component.is_tiny));
|
|
auto use_directions = std::make_pair(use_segment, use_segment);
|
|
use_directions = boolPairAnd(use_directions, HasValidEdge(segment));
|
|
|
|
if (use_segment)
|
|
{
|
|
use_directions =
|
|
boolPairAnd(CheckSegmentBearing(segment, bearing, bearing_range),
|
|
HasValidEdge(segment));
|
|
if (use_directions.first || use_directions.second)
|
|
{
|
|
has_big_component = has_big_component || !segment.data.component.is_tiny;
|
|
has_small_component = has_small_component || segment.data.component.is_tiny;
|
|
}
|
|
}
|
|
|
|
return use_directions;
|
|
},
|
|
[this, &has_big_component, max_distance, input_coordinate](
|
|
const std::size_t num_results, const CandidateSegment &segment) {
|
|
return (num_results > 0 && has_big_component) ||
|
|
CheckSegmentDistance(input_coordinate, segment, max_distance);
|
|
});
|
|
|
|
if (results.size() == 0)
|
|
{
|
|
return std::make_pair(PhantomNode{}, PhantomNode{});
|
|
}
|
|
|
|
BOOST_ASSERT(results.size() > 0);
|
|
return std::make_pair(MakePhantomNode(input_coordinate, results.front()).phantom_node,
|
|
MakePhantomNode(input_coordinate, results.back()).phantom_node);
|
|
}
|
|
|
|
private:
|
|
std::vector<PhantomNodeWithDistance>
|
|
MakePhantomNodes(const util::Coordinate input_coordinate,
|
|
const std::vector<EdgeData> &results) const
|
|
{
|
|
std::vector<PhantomNodeWithDistance> distance_and_phantoms(results.size());
|
|
std::transform(results.begin(),
|
|
results.end(),
|
|
distance_and_phantoms.begin(),
|
|
[this, &input_coordinate](const EdgeData &data) {
|
|
return MakePhantomNode(input_coordinate, data);
|
|
});
|
|
return distance_and_phantoms;
|
|
}
|
|
|
|
PhantomNodeWithDistance MakePhantomNode(const util::Coordinate input_coordinate,
|
|
const EdgeData &data) const
|
|
{
|
|
util::Coordinate point_on_segment;
|
|
double ratio;
|
|
const auto current_perpendicular_distance =
|
|
util::coordinate_calculation::perpendicularDistance(coordinates[data.u],
|
|
coordinates[data.v],
|
|
input_coordinate,
|
|
point_on_segment,
|
|
ratio);
|
|
|
|
// Find the node-based-edge that this belongs to, and directly
|
|
// calculate the forward_weight, forward_offset, reverse_weight, reverse_offset
|
|
|
|
int forward_offset = 0, forward_weight = 0;
|
|
int reverse_offset = 0, reverse_weight = 0;
|
|
|
|
const std::vector<EdgeWeight> forward_weight_vector =
|
|
datafacade.GetUncompressedForwardWeights(data.packed_geometry_id);
|
|
const std::vector<EdgeWeight> reverse_weight_vector =
|
|
datafacade.GetUncompressedReverseWeights(data.packed_geometry_id);
|
|
|
|
for (std::size_t i = 0; i < data.fwd_segment_position; i++)
|
|
{
|
|
forward_offset += forward_weight_vector[i];
|
|
}
|
|
forward_weight = forward_weight_vector[data.fwd_segment_position];
|
|
|
|
BOOST_ASSERT(data.fwd_segment_position < reverse_weight_vector.size());
|
|
|
|
for (std::size_t i = 0; i < reverse_weight_vector.size() - data.fwd_segment_position - 1;
|
|
i++)
|
|
{
|
|
reverse_offset += reverse_weight_vector[i];
|
|
}
|
|
reverse_weight =
|
|
reverse_weight_vector[reverse_weight_vector.size() - data.fwd_segment_position - 1];
|
|
|
|
ratio = std::min(1.0, std::max(0.0, ratio));
|
|
if (data.forward_segment_id.id != SPECIAL_SEGMENTID)
|
|
{
|
|
forward_weight *= ratio;
|
|
}
|
|
if (data.reverse_segment_id.id != SPECIAL_SEGMENTID)
|
|
{
|
|
reverse_weight *= 1.0 - ratio;
|
|
}
|
|
|
|
auto transformed = PhantomNodeWithDistance{PhantomNode{data,
|
|
forward_weight,
|
|
forward_offset,
|
|
reverse_weight,
|
|
reverse_offset,
|
|
point_on_segment,
|
|
input_coordinate},
|
|
current_perpendicular_distance};
|
|
|
|
return transformed;
|
|
}
|
|
|
|
bool CheckSegmentDistance(const Coordinate input_coordinate,
|
|
const CandidateSegment &segment,
|
|
const double max_distance) const
|
|
{
|
|
BOOST_ASSERT(segment.data.forward_segment_id.id != SPECIAL_SEGMENTID ||
|
|
!segment.data.forward_segment_id.enabled);
|
|
BOOST_ASSERT(segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID ||
|
|
!segment.data.reverse_segment_id.enabled);
|
|
|
|
Coordinate wsg84_coordinate =
|
|
util::web_mercator::toWGS84(segment.fixed_projected_coordinate);
|
|
|
|
return util::coordinate_calculation::haversineDistance(input_coordinate, wsg84_coordinate) >
|
|
max_distance;
|
|
}
|
|
|
|
std::pair<bool, bool> CheckSegmentBearing(const CandidateSegment &segment,
|
|
const int filter_bearing,
|
|
const int filter_bearing_range) const
|
|
{
|
|
BOOST_ASSERT(segment.data.forward_segment_id.id != SPECIAL_SEGMENTID ||
|
|
!segment.data.forward_segment_id.enabled);
|
|
BOOST_ASSERT(segment.data.reverse_segment_id.id != SPECIAL_SEGMENTID ||
|
|
!segment.data.reverse_segment_id.enabled);
|
|
|
|
const double forward_edge_bearing = util::coordinate_calculation::bearing(
|
|
coordinates[segment.data.u], coordinates[segment.data.v]);
|
|
|
|
const double backward_edge_bearing = (forward_edge_bearing + 180) > 360
|
|
? (forward_edge_bearing - 180)
|
|
: (forward_edge_bearing + 180);
|
|
|
|
const bool forward_bearing_valid =
|
|
util::bearing::CheckInBounds(
|
|
std::round(forward_edge_bearing), filter_bearing, filter_bearing_range) &&
|
|
segment.data.forward_segment_id.enabled;
|
|
const bool backward_bearing_valid =
|
|
util::bearing::CheckInBounds(
|
|
std::round(backward_edge_bearing), filter_bearing, filter_bearing_range) &&
|
|
segment.data.reverse_segment_id.enabled;
|
|
return std::make_pair(forward_bearing_valid, backward_bearing_valid);
|
|
}
|
|
|
|
/**
|
|
* Checks to see if the edge weights are valid. We might have an edge,
|
|
* but a traffic update might set the speed to 0 (weight == INVALID_EDGE_WEIGHT).
|
|
* which means that this edge is not currently traversible. If this is the case,
|
|
* then we shouldn't snap to this edge.
|
|
*/
|
|
std::pair<bool, bool> HasValidEdge(const CandidateSegment &segment) const
|
|
{
|
|
|
|
bool forward_edge_valid = false;
|
|
bool reverse_edge_valid = false;
|
|
|
|
const std::vector<EdgeWeight> forward_weight_vector =
|
|
datafacade.GetUncompressedForwardWeights(segment.data.packed_geometry_id);
|
|
|
|
if (forward_weight_vector[segment.data.fwd_segment_position] != INVALID_EDGE_WEIGHT)
|
|
{
|
|
forward_edge_valid = segment.data.forward_segment_id.enabled;
|
|
}
|
|
|
|
const std::vector<EdgeWeight> reverse_weight_vector =
|
|
datafacade.GetUncompressedReverseWeights(segment.data.packed_geometry_id);
|
|
if (reverse_weight_vector[reverse_weight_vector.size() - segment.data.fwd_segment_position -
|
|
1] != INVALID_EDGE_WEIGHT)
|
|
{
|
|
reverse_edge_valid = segment.data.reverse_segment_id.enabled;
|
|
}
|
|
|
|
return std::make_pair(forward_edge_valid, reverse_edge_valid);
|
|
}
|
|
|
|
const RTreeT &rtree;
|
|
const CoordinateList &coordinates;
|
|
DataFacadeT &datafacade;
|
|
};
|
|
}
|
|
}
|
|
|
|
#endif
|