Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

Fix mathematical assumptions in StaticRTree

Browse Source 
StaticRTree now uses projected coordinates internally. That means we can
use a euclidean distance measure (squared distance) for sorting the
query queue.
This commit is contained in:
Patrick Niklaus
2016-03-28 20:38:19 +02:00
parent 76d64f27cc
commit 2a103c4362
12 changed files with 717 additions and 570 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/engine/geospatial_query.hpp
+131 -100
View File
@@ -26,6 +26,7 @@ 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_,
@@ -45,15 +46,17 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
std::vector<PhantomNodeWithDistance>
NearestPhantomNodesInRange(const util::Coordinate input_coordinate, const double max_distance)
{
auto results = rtree.Nearest(input_coordinate,
[](const EdgeData &)
{
return std::make_pair(true, true);
},
[max_distance](const std::size_t, const double min_dist)
{
return min_dist > max_distance;
});
auto results =
rtree.Nearest(input_coordinate,
[](const CandidateSegment &)
{
return std::make_pair(true, true);
},
[this, max_distance, input_coordinate](const std::size_t,
const CandidateSegment &segment)
{
return checkSegmentDistance(input_coordinate, segment, max_distance);
});
return MakePhantomNodes(input_coordinate, results);
}
@@ -66,16 +69,17 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const int bearing,
const int bearing_range)
{
auto results =
rtree.Nearest(input_coordinate,
[this, bearing, bearing_range, max_distance](const EdgeData &data)
{
return checkSegmentBearing(data, bearing, bearing_range);
},
[max_distance](const std::size_t, const double min_dist)
{
return min_dist > max_distance;
});
auto results = rtree.Nearest(
input_coordinate,
[this, bearing, bearing_range, max_distance](const CandidateSegment &segment)
{
return checkSegmentBearing(segment, bearing, bearing_range);
},
[this, max_distance, input_coordinate](const std::size_t,
const CandidateSegment &segment)
{
return checkSegmentDistance(input_coordinate, segment, max_distance);
});
return MakePhantomNodes(input_coordinate, results);
}
@@ -88,15 +92,16 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const int bearing,
const int bearing_range)
{
auto results = rtree.Nearest(input_coordinate,
[this, bearing, bearing_range](const EdgeData &data)
{
return checkSegmentBearing(data, bearing, bearing_range);
},
[max_results](const std::size_t num_results, const double)
{
return num_results >= max_results;
});
auto results =
rtree.Nearest(input_coordinate,
[this, bearing, bearing_range](const CandidateSegment &segment)
{
return checkSegmentBearing(segment, bearing, bearing_range);
},
[max_results](const std::size_t num_results, const CandidateSegment &)
{
return num_results >= max_results;
});
return MakePhantomNodes(input_coordinate, results);
}
@@ -111,16 +116,18 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const int bearing,
const int bearing_range)
{
auto results = rtree.Nearest(
input_coordinate,
[this, bearing, bearing_range](const EdgeData &data)
{
return checkSegmentBearing(data, bearing, bearing_range);
},
[max_results, max_distance](const std::size_t num_results, const double min_dist)
{
return num_results >= max_results || min_dist > max_distance;
});
auto results =
rtree.Nearest(input_coordinate,
[this, bearing, bearing_range](const CandidateSegment &segment)
{
return checkSegmentBearing(segment, bearing, bearing_range);
},
[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);
}
@@ -130,15 +137,16 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
std::vector<PhantomNodeWithDistance>
NearestPhantomNodes(const util::Coordinate input_coordinate, const unsigned max_results)
{
auto results = rtree.Nearest(input_coordinate,
[](const EdgeData &)
{
return std::make_pair(true, true);
},
[max_results](const std::size_t num_results, const double)
{
return num_results >= max_results;
});
auto results =
rtree.Nearest(input_coordinate,
[](const CandidateSegment &)
{
return std::make_pair(true, true);
},
[max_results](const std::size_t num_results, const CandidateSegment &)
{
return num_results >= max_results;
});
return MakePhantomNodes(input_coordinate, results);
}
@@ -150,16 +158,18 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const unsigned max_results,
const double max_distance)
{
auto results = rtree.Nearest(
input_coordinate,
[](const EdgeData &)
{
return std::make_pair(true, true);
},
[max_results, max_distance](const std::size_t num_results, const double min_dist)
{
return num_results >= max_results || min_dist > max_distance;
});
auto results =
rtree.Nearest(input_coordinate,
[](const CandidateSegment &)
{
return std::make_pair(true, true);
},
[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);
}
@@ -174,20 +184,22 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
bool has_big_component = false;
auto results = rtree.Nearest(
input_coordinate,
[&has_big_component, &has_small_component](const EdgeData &data)
[&has_big_component, &has_small_component](const CandidateSegment &segment)
{
auto use_segment =
(!has_small_component || (!has_big_component && !data.component.is_tiny));
auto use_segment = (!has_small_component ||
(!has_big_component && !segment.data.component.is_tiny));
auto use_directions = std::make_pair(use_segment, use_segment);
has_big_component = has_big_component || !data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny;
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, max_distance](const std::size_t num_results, const double min_dist)
[this, &has_big_component, max_distance,
input_coordinate](const std::size_t num_results, const CandidateSegment &segment)
{
return (num_results > 0 && has_big_component) || min_dist > max_distance;
return (num_results > 0 && has_big_component) ||
checkSegmentDistance(input_coordinate, segment, max_distance);
});
if (results.size() == 0)
@@ -207,23 +219,23 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
{
bool has_small_component = false;
bool has_big_component = false;
auto results =
rtree.Nearest(input_coordinate,
[&has_big_component, &has_small_component](const EdgeData &data)
{
auto use_segment = (!has_small_component ||
(!has_big_component && !data.component.is_tiny));
auto use_directions = std::make_pair(use_segment, use_segment);
auto results = rtree.Nearest(
input_coordinate,
[&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);
has_big_component = has_big_component || !data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny;
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 double)
{
return num_results > 0 && has_big_component;
});
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)
{
@@ -245,25 +257,25 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
auto results = rtree.Nearest(
input_coordinate,
[this, bearing, bearing_range, &has_big_component,
&has_small_component](const EdgeData &data)
&has_small_component](const CandidateSegment &segment)
{
auto use_segment =
(!has_small_component || (!has_big_component && !data.component.is_tiny));
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_segment)
{
use_directions = checkSegmentBearing(data, bearing, bearing_range);
use_directions = checkSegmentBearing(segment, bearing, bearing_range);
if (use_directions.first || use_directions.second)
{
has_big_component = has_big_component || !data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny;
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 double)
[&has_big_component](const std::size_t num_results, const CandidateSegment &)
{
return num_results > 0 && has_big_component;
});
@@ -291,27 +303,29 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
auto results = rtree.Nearest(
input_coordinate,
[this, bearing, bearing_range, &has_big_component,
&has_small_component](const EdgeData &data)
&has_small_component](const CandidateSegment &segment)
{
auto use_segment =
(!has_small_component || (!has_big_component && !data.component.is_tiny));
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_segment)
{
use_directions = checkSegmentBearing(data, bearing, bearing_range);
use_directions = checkSegmentBearing(segment, bearing, bearing_range);
if (use_directions.first || use_directions.second)
{
has_big_component = has_big_component || !data.component.is_tiny;
has_small_component = has_small_component || data.component.is_tiny;
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, max_distance](const std::size_t num_results, const double min_dist)
[this, &has_big_component, max_distance,
input_coordinate](const std::size_t num_results, const CandidateSegment &segment)
{
return (num_results > 0 && has_big_component) || min_dist > max_distance;
return (num_results > 0 && has_big_component) ||
checkSegmentDistance(input_coordinate, segment, max_distance);
});
if (results.size() == 0)
@@ -401,15 +415,32 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
return transformed;
}
std::pair<bool, bool> checkSegmentBearing(const EdgeData &segment,
bool checkSegmentDistance(const Coordinate input_coordinate,
const CandidateSegment &segment,
const double max_distance)
{
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::coordinate_calculation::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)
{
BOOST_ASSERT(segment.forward_segment_id.id != SPECIAL_SEGMENTID || !segment.forward_segment_id.enabled);
BOOST_ASSERT(segment.reverse_segment_id.id != SPECIAL_SEGMENTID || !segment.reverse_segment_id.enabled);
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->at(segment.u), coordinates->at(segment.v));
coordinates->at(segment.data.u), coordinates->at(segment.data.v));
const double backward_edge_bearing = (forward_edge_bearing + 180) > 360
? (forward_edge_bearing - 180)
@@ -418,11 +449,11 @@ template <typename RTreeT, typename DataFacadeT> class GeospatialQuery
const bool forward_bearing_valid =
util::bearing::CheckInBounds(std::round(forward_edge_bearing), filter_bearing,
filter_bearing_range) &&
segment.forward_segment_id.enabled;
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.reverse_segment_id.enabled;
segment.data.reverse_segment_id.enabled;
return std::make_pair(forward_bearing_valid, backward_bearing_valid);
}
include/util/coordinate.hpp
+22
View File
@@ -78,6 +78,8 @@ inline FloatLongitude toFloating(const FixedLongitude fixed)
return FloatLongitude(floating);
}
struct FloatCoordinate;
// Coordinate encoded as longitude, latitude
struct Coordinate
{
@@ -85,6 +87,7 @@ struct Coordinate
FixedLatitude lat;
Coordinate();
Coordinate(const FloatCoordinate &other);
Coordinate(const FixedLongitude lon_, const FixedLatitude lat_);
Coordinate(const FloatLongitude lon_, const FloatLatitude lat_);
@@ -104,8 +107,27 @@ struct Coordinate
friend std::ostream &operator<<(std::ostream &out, const Coordinate coordinate);
};
// Coordinate encoded as longitude, latitude
struct FloatCoordinate
{
FloatLongitude lon;
FloatLatitude lat;
FloatCoordinate();
FloatCoordinate(const FixedLongitude lon_, const FixedLatitude lat_);
FloatCoordinate(const FloatLongitude lon_, const FloatLatitude lat_);
FloatCoordinate(const Coordinate other);
bool IsValid() const;
friend bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs);
friend bool operator!=(const FloatCoordinate lhs, const FloatCoordinate rhs);
friend std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate);
};
bool operator==(const Coordinate lhs, const Coordinate rhs);
bool operator==(const FloatCoordinate lhs, const FloatCoordinate rhs);
std::ostream &operator<<(std::ostream &out, const Coordinate coordinate);
std::ostream &operator<<(std::ostream &out, const FloatCoordinate coordinate);
}
}
include/util/coordinate_calculation.hpp
+21 -25
View File
@@ -24,22 +24,26 @@ const constexpr double EARTH_RADIUS_WGS84 = 6378137.0;
namespace detail
{
// earth circumference devided by 2
const constexpr double MAXEXTENT = EARTH_RADIUS_WGS84 * boost::math::constants::pi<double>();
// ^ math functions are not constexpr since they have side-effects (setting errno) :(
const double MAX_LATITUDE = RAD_TO_DEGREE * (2.0 * std::atan(std::exp(180.0 * DEGREE_TO_RAD)) - boost::math::constants::half_pi<double>());
const constexpr double MAX_LONGITUDE = 180.0;
// earth circumference devided by 2
const constexpr double MAXEXTENT = EARTH_RADIUS_WGS84 * boost::math::constants::pi<double>();
// ^ math functions are not constexpr since they have side-effects (setting errno) :(
const double MAX_LATITUDE = RAD_TO_DEGREE * (2.0 * std::atan(std::exp(180.0 * DEGREE_TO_RAD)) -
boost::math::constants::half_pi<double>());
const constexpr double MAX_LONGITUDE = 180.0;
}
//! Projects both coordinates and takes the euclidean distance of the projected points
// Does not return meters!
double euclideanDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
//! Takes the squared euclidean distance of the input coordinates. Does not return meters!
double squaredEuclideanDistance(const FloatCoordinate &lhs, const FloatCoordinate &rhs);
double haversineDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
double greatCircleDistance(const Coordinate first_coordinate, const Coordinate second_coordinate);
std::pair<double, FloatCoordinate>
projectPointOnSegment(const FloatCoordinate &projected_xy_source,
const FloatCoordinate &projected_xy_target,
const FloatCoordinate &projected_xy_coordinate);
double perpendicularDistance(const Coordinate segment_source,
const Coordinate segment_target,
const Coordinate query_location);
@@ -50,20 +54,6 @@ double perpendicularDistance(const Coordinate segment_source,
Coordinate &nearest_location,
double &ratio);
double perpendicularDistanceFromProjectedCoordinate(
const Coordinate segment_source,
const Coordinate segment_target,
const Coordinate query_location,
const std::pair<double, double> projected_xy_coordinate);
double perpendicularDistanceFromProjectedCoordinate(
const Coordinate segment_source,
const Coordinate segment_target,
const Coordinate query_location,
const std::pair<double, double> projected_xy_coordinate,
Coordinate &nearest_location,
double &ratio);
Coordinate centroid(const Coordinate lhs, const Coordinate rhs);
double bearing(const Coordinate first_coordinate, const Coordinate second_coordinate);
@@ -86,8 +76,14 @@ double degreeToPixel(FloatLatitude lat, unsigned zoom);
double degreeToPixel(FloatLongitude lon, unsigned zoom);
FloatLatitude yToLat(const double value);
double latToY(const FloatLatitude latitude);
void xyzToMercator(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy);
void xyzToWSG84(const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy);
FloatCoordinate fromWGS84(const FloatCoordinate &wgs84_coordinate);
FloatCoordinate toWGS84(const FloatCoordinate &mercator_coordinate);
void xyzToMercator(
const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy);
void xyzToWGS84(
const int x, const int y, const int z, double &minx, double &miny, double &maxx, double &maxy);
} // ns mercator
} // ns coordinate_calculation
} // ns util
include/util/rectangle.hpp
+12 -40
View File
@@ -80,7 +80,10 @@ struct RectangleInt2D
min_lat > other.max_lat);
}
double GetMinDist(const Coordinate location) const
// This code assumes that we are operating in euclidean space!
// That means if you just put unprojected lat/lon in here you will
// get invalid results.
double GetMinSquaredDist(const Coordinate location) const
{
const bool is_contained = Contains(location);
if (is_contained)
@@ -117,36 +120,36 @@ struct RectangleInt2D
switch (d)
{
case NORTH:
min_dist = coordinate_calculation::greatCircleDistance(
min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(location.lon, max_lat));
break;
case SOUTH:
min_dist = coordinate_calculation::greatCircleDistance(
min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(location.lon, min_lat));
break;
case WEST:
min_dist = coordinate_calculation::greatCircleDistance(
min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(min_lon, location.lat));
break;
case EAST:
min_dist = coordinate_calculation::greatCircleDistance(
min_dist = coordinate_calculation::squaredEuclideanDistance(
location, Coordinate(max_lon, location.lat));
break;
case NORTH_EAST:
min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(max_lon, max_lat));
coordinate_calculation::squaredEuclideanDistance(location, Coordinate(max_lon, max_lat));
break;
case NORTH_WEST:
min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(min_lon, max_lat));
coordinate_calculation::squaredEuclideanDistance(location, Coordinate(min_lon, max_lat));
break;
case SOUTH_EAST:
min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(max_lon, min_lat));
coordinate_calculation::squaredEuclideanDistance(location, Coordinate(max_lon, min_lat));
break;
case SOUTH_WEST:
min_dist =
coordinate_calculation::greatCircleDistance(location, Coordinate(min_lon, min_lat));
coordinate_calculation::squaredEuclideanDistance(location, Coordinate(min_lon, min_lat));
break;
default:
break;
@@ -157,37 +160,6 @@ struct RectangleInt2D
return min_dist;
}
double GetMinMaxDist(const Coordinate location) const
{
double min_max_dist = std::numeric_limits<double>::max();
// Get minmax distance to each of the four sides
const Coordinate upper_left(min_lon, max_lat);
const Coordinate upper_right(max_lon, max_lat);
const Coordinate lower_right(max_lon, min_lat);
const Coordinate lower_left(min_lon, min_lat);
min_max_dist =
std::min(min_max_dist,
std::max(coordinate_calculation::greatCircleDistance(location, upper_left),
coordinate_calculation::greatCircleDistance(location, upper_right)));
min_max_dist =
std::min(min_max_dist,
std::max(coordinate_calculation::greatCircleDistance(location, upper_right),
coordinate_calculation::greatCircleDistance(location, lower_right)));
min_max_dist =
std::min(min_max_dist,
std::max(coordinate_calculation::greatCircleDistance(location, lower_right),
coordinate_calculation::greatCircleDistance(location, lower_left)));
min_max_dist =
std::min(min_max_dist,
std::max(coordinate_calculation::greatCircleDistance(location, lower_left),
coordinate_calculation::greatCircleDistance(location, upper_left)));
return min_max_dist;
}
bool Contains(const Coordinate location) const
{
const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon);
include/util/static_rtree.hpp
+75 -45
View File
@@ -36,6 +36,8 @@ namespace util
{
// Static RTree for serving nearest neighbour queries
// All coordinates are pojected first to Web Mercator before the bounding boxes
// are computed, this means the internal distance metric doesn not represent meters!
template <class EdgeDataT,
class CoordinateListT = std::vector<Coordinate>,
bool UseSharedMemory = false,
@@ -48,7 +50,11 @@ class StaticRTree
using EdgeData = EdgeDataT;
using CoordinateList = CoordinateListT;
static constexpr std::size_t MAX_CHECKED_ELEMENTS = 4 * LEAF_NODE_SIZE;
struct CandidateSegment
{
Coordinate fixed_projected_coordinate;
EdgeDataT data;
};
struct TreeNode
{
@@ -86,16 +92,16 @@ class StaticRTree
}
};
using QueryNodeType = mapbox::util::variant<TreeNode, EdgeDataT>;
using QueryNodeType = mapbox::util::variant<TreeNode, CandidateSegment>;
struct QueryCandidate
{
inline bool operator<(const QueryCandidate &other) const
{
// Attn: this is reversed order. std::pq is a max pq!
return other.min_dist < min_dist;
return other.squared_min_dist < squared_min_dist;
}
float min_dist;
double squared_min_dist;
QueryNodeType node;
};
@@ -315,9 +321,16 @@ class StaticRTree
leaves_stream.read((char *)&m_element_count, sizeof(uint64_t));
}
/* Returns all features inside the bounding box */
/* Returns all features inside the bounding box.
Rectangle needs to be projected!*/
std::vector<EdgeDataT> SearchInBox(const Rectangle &search_rectangle)
{
const Rectangle projected_rectangle{
search_rectangle.min_lon, search_rectangle.max_lon,
toFixed(FloatLatitude{coordinate_calculation::mercator::latToY(
toFloating(FixedLatitude(search_rectangle.min_lat)))}),
toFixed(FloatLatitude{coordinate_calculation::mercator::latToY(
toFloating(FixedLatitude(search_rectangle.max_lat)))})};
std::vector<EdgeDataT> results;
std::queue<TreeNode> traversal_queue;
@@ -377,11 +390,11 @@ class StaticRTree
std::vector<EdgeDataT> Nearest(const Coordinate input_coordinate, const std::size_t max_results)
{
return Nearest(input_coordinate,
[](const EdgeDataT &)
[](const CandidateSegment &)
{
return std::make_pair(true, true);
},
[max_results](const std::size_t num_results, const float)
[max_results](const std::size_t num_results, const CandidateSegment &)
{
return num_results >= max_results;
});
@@ -393,9 +406,8 @@ class StaticRTree
Nearest(const Coordinate input_coordinate, const FilterT filter, const TerminationT terminate)
{
std::vector<EdgeDataT> results;
std::pair<double, double> projected_coordinate = {
static_cast<double>(toFloating(input_coordinate.lon)),
coordinate_calculation::mercator::latToY(toFloating(input_coordinate.lat))};
auto projected_coordinate = coordinate_calculation::mercator::fromWGS84(input_coordinate);
Coordinate fixed_projected_coordinate{projected_coordinate};
// initialize queue with root element
std::priority_queue<QueryCandidate> traversal_queue;
@@ -403,13 +415,7 @@ class StaticRTree
while (!traversal_queue.empty())
{
const QueryCandidate current_query_node = traversal_queue.top();
if (terminate(results.size(), current_query_node.min_dist))
{
traversal_queue = std::priority_queue<QueryCandidate>{};
break;
}
QueryCandidate current_query_node = traversal_queue.top();
traversal_queue.pop();
if (current_query_node.node.template is<TreeNode>())
@@ -418,30 +424,34 @@ class StaticRTree
current_query_node.node.template get<TreeNode>();
if (current_tree_node.child_is_on_disk)
{
ExploreLeafNode(current_tree_node.children[0], input_coordinate,
projected_coordinate, traversal_queue);
ExploreLeafNode(current_tree_node.children[0], projected_coordinate,
traversal_queue);
}
else
{
ExploreTreeNode(current_tree_node, input_coordinate, traversal_queue);
ExploreTreeNode(current_tree_node, fixed_projected_coordinate, traversal_queue);
}
}
else
{
// inspecting an actual road segment
const auto &current_segment = current_query_node.node.template get<EdgeDataT>();
auto &current_candidate = current_query_node.node.template get<CandidateSegment>();
if (terminate(results.size(), current_candidate))
{
traversal_queue = std::priority_queue<QueryCandidate>{};
break;
}
auto use_segment = filter(current_segment);
auto use_segment = filter(current_candidate);
if (!use_segment.first && !use_segment.second)
{
continue;
}
current_candidate.data.forward_segment_id.enabled &= use_segment.first;
current_candidate.data.reverse_segment_id.enabled &= use_segment.second;
// store phantom node in result vector
results.push_back(std::move(current_segment));
results.back().forward_segment_id.enabled &= use_segment.first;
results.back().reverse_segment_id.enabled &= use_segment.second;
results.push_back(std::move(current_candidate.data));
}
}
@@ -451,8 +461,7 @@ class StaticRTree
private:
template <typename QueueT>
void ExploreLeafNode(const std::uint32_t leaf_id,
const Coordinate input_coordinate,
const std::pair<double, double> &projected_coordinate,
const FloatCoordinate &projected_input_coordinate,
QueueT &traversal_queue)
{
LeafNode current_leaf_node;
@@ -462,21 +471,30 @@ class StaticRTree
for (const auto i : irange(0u, current_leaf_node.object_count))
{
auto &current_edge = current_leaf_node.objects[i];
const float current_perpendicular_distance =
coordinate_calculation::perpendicularDistanceFromProjectedCoordinate(
m_coordinate_list->at(current_edge.u), m_coordinate_list->at(current_edge.v),
input_coordinate, projected_coordinate);
auto projected_u =
coordinate_calculation::mercator::fromWGS84((*m_coordinate_list)[current_edge.u]);
auto projected_v =
coordinate_calculation::mercator::fromWGS84((*m_coordinate_list)[current_edge.v]);
FloatCoordinate projected_nearest;
std::tie(std::ignore, projected_nearest) =
coordinate_calculation::projectPointOnSegment(projected_u, projected_v,
projected_input_coordinate);
auto squared_distance = coordinate_calculation::squaredEuclideanDistance(
projected_input_coordinate, projected_nearest);
// distance must be non-negative
BOOST_ASSERT(0.f <= current_perpendicular_distance);
BOOST_ASSERT(0. <= squared_distance);
traversal_queue.push(
QueryCandidate{current_perpendicular_distance, std::move(current_edge)});
QueryCandidate{squared_distance, CandidateSegment{Coordinate{projected_nearest},
std::move(current_edge)}});
}
}
template <class QueueT>
void ExploreTreeNode(const TreeNode &parent,
const Coordinate input_coordinate,
const Coordinate fixed_projected_input_coordinate,
QueueT &traversal_queue)
{
for (std::uint32_t i = 0; i < parent.child_count; ++i)
@@ -484,8 +502,10 @@ class StaticRTree
const std::int32_t child_id = parent.children[i];
const auto &child_tree_node = m_search_tree[child_id];
const auto &child_rectangle = child_tree_node.minimum_bounding_rectangle;
const float lower_bound_to_element = child_rectangle.GetMinDist(input_coordinate);
traversal_queue.push(QueryCandidate{lower_bound_to_element, m_search_tree[child_id]});
const auto squared_lower_bound_to_element =
child_rectangle.GetMinSquaredDist(fixed_projected_input_coordinate);
traversal_queue.push(
QueryCandidate{squared_lower_bound_to_element, m_search_tree[child_id]});
}
}
@@ -517,19 +537,29 @@ class StaticRTree
BOOST_ASSERT(objects[i].u < coordinate_list.size());
BOOST_ASSERT(objects[i].v < coordinate_list.size());
Coordinate projected_u{coordinate_calculation::mercator::fromWGS84(
Coordinate{coordinate_list[objects[i].u]})};
Coordinate projected_v{coordinate_calculation::mercator::fromWGS84(
Coordinate{coordinate_list[objects[i].v]})};
BOOST_ASSERT(toFloating(projected_u.lon) <= FloatLongitude(180.));
BOOST_ASSERT(toFloating(projected_u.lon) >= FloatLongitude(-180.));
BOOST_ASSERT(toFloating(projected_u.lat) <= FloatLatitude(180.));
BOOST_ASSERT(toFloating(projected_u.lat) >= FloatLatitude(-180.));
BOOST_ASSERT(toFloating(projected_v.lon) <= FloatLongitude(180.));
BOOST_ASSERT(toFloating(projected_v.lon) >= FloatLongitude(-180.));
BOOST_ASSERT(toFloating(projected_v.lat) <= FloatLatitude(180.));
BOOST_ASSERT(toFloating(projected_v.lat) >= FloatLatitude(-180.));
rectangle.min_lon =
std::min(rectangle.min_lon, std::min(coordinate_list[objects[i].u].lon,
coordinate_list[objects[i].v].lon));
std::min(rectangle.min_lon, std::min(projected_u.lon, projected_v.lon));
rectangle.max_lon =
std::max(rectangle.max_lon, std::max(coordinate_list[objects[i].u].lon,
coordinate_list[objects[i].v].lon));
std::max(rectangle.max_lon, std::max(projected_u.lon, projected_v.lon));
rectangle.min_lat =
std::min(rectangle.min_lat, std::min(coordinate_list[objects[i].u].lat,
coordinate_list[objects[i].v].lat));
std::min(rectangle.min_lat, std::min(projected_u.lat, projected_v.lat));
rectangle.max_lat =
std::max(rectangle.max_lat, std::max(coordinate_list[objects[i].u].lat,
coordinate_list[objects[i].v].lat));
std::max(rectangle.max_lat, std::max(projected_u.lat, projected_v.lat));
}
BOOST_ASSERT(rectangle.min_lon != FixedLongitude(std::numeric_limits<int>::min()));
BOOST_ASSERT(rectangle.min_lat != FixedLatitude(std::numeric_limits<int>::min()));