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

consolidate duplicated distance calculations

Browse Source
This commit is contained in:
Dennis Luxen 2014-04-30 14:32:56 +02:00
parent 99a47ae87b
commit e3cc896a42
6 changed files with 335 additions and 356 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

203
Algorithms/DouglasPeucker.cpp
View File

@ -25,173 +25,96 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <osrm/Coordinate.h>
#include "DouglasPeucker.h"
#include "../DataStructures/SegmentInformation.h"
#include "../Util/MercatorUtil.h"
#include <boost/assert.hpp>
#include <limits>
//These thresholds are more or less heuristically chosen.
static double DouglasPeuckerThresholds[19] = {
262144., //z0
131072., //z1
65536., //z2
32768., //z3
16384., //z4
8192., //z5
4096., //z6
2048., //z7
960., //z8
480., //z9
240., //z10
90., //z11
50., //z12
25., //z13
15., //z14
5., //z15
.65, //z16
.5, //z17
.35 //z18
// These thresholds are more or less heuristically chosen.
static double DouglasPeuckerThresholds[19] = {262144., // z0
131072., // z1
65536., // z2
32768., // z3
16384., // z4
8192., // z5
4096., // z6
2048., // z7
960., // z8
480., // z9
240., // z10
90., // z11
50., // z12
25., // z13
15., // z14
5., // z15
.65, // z16
.5, // z17
.35 // z18
};
/**
* Yuck! Code duplication. This function is also in EgdeBasedNode.h
*/
double DouglasPeucker::ComputeDistance(
const FixedPointCoordinate& point,
const FixedPointCoordinate& segA,
const FixedPointCoordinate& segB
) const {
const double x = lat2y(point.lat/COORDINATE_PRECISION);
const double y = point.lon/COORDINATE_PRECISION;
const double a = lat2y(segA.lat/COORDINATE_PRECISION);
const double b = segA.lon/COORDINATE_PRECISION;
const double c = lat2y(segB.lat/COORDINATE_PRECISION);
const double d = segB.lon/COORDINATE_PRECISION;
double p,q,nY;
if( std::abs(a-c) > std::numeric_limits<double>::epsilon() ){
const double m = (d-b)/(c-a); // slope
// Projection of (x,y) on line joining (a,b) and (c,d)
p = ((x + (m*y)) + (m*m*a - m*b))/(1. + m*m);
q = b + m*(p - a);
} else {
p = c;
q = y;
}
nY = (d*p - c*q)/(a*d - b*c);
//discretize the result to coordinate precision. it's a hack!
if( std::abs(nY) < (1./COORDINATE_PRECISION) ) {
nY = 0.;
}
double r = (p - nY*a)/c;
if( std::isnan(r) ) {
r = ((segB.lat == point.lat) && (segB.lon == point.lon)) ? 1. : 0.;
} else if( std::abs(r) <= std::numeric_limits<double>::epsilon() ) {
r = 0.;
} else if( std::abs(r-1.) <= std::numeric_limits<double>::epsilon() ) {
r = 1.;
}
FixedPointCoordinate nearest_location;
BOOST_ASSERT( !std::isnan(r) );
if( r <= 0. ){
nearest_location.lat = segA.lat;
nearest_location.lon = segA.lon;
} else if( r >= 1. ){
nearest_location.lat = segB.lat;
nearest_location.lon = segB.lon;
} else { // point lies in between
nearest_location.lat = y2lat(p)*COORDINATE_PRECISION;
nearest_location.lon = q*COORDINATE_PRECISION;
}
BOOST_ASSERT( nearest_location.isValid() );
const double approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance(
point,
nearest_location
);
BOOST_ASSERT( 0. <= approximated_distance );
return approximated_distance;
}
void DouglasPeucker::Run(
std::vector<SegmentInformation> & input_geometry,
const unsigned zoom_level
) {
void DouglasPeucker::Run(std::vector<SegmentInformation> &input_geometry, const unsigned zoom_level)
{
{
BOOST_ASSERT_MSG(zoom_level < 19, "unsupported zoom level");
BOOST_ASSERT_MSG(1 < input_geometry.size(), "geometry invalid");
std::size_t left_border = 0;
std::size_t right_border = 1;
//Sweep over array and identify those ranges that need to be checked
do {
BOOST_ASSERT_MSG(
input_geometry[left_border].necessary,
"left border must be necessary"
);
BOOST_ASSERT_MSG(
input_geometry.back().necessary,
"right border must be necessary"
);
unsigned left_border = 0;
unsigned right_border = 1;
// Sweep over array and identify those ranges that need to be checked
do
{
BOOST_ASSERT_MSG(input_geometry[left_border].necessary,
"left border must be necessary");
BOOST_ASSERT_MSG(input_geometry.back().necessary, "right border must be necessary");
if(input_geometry[right_border].necessary) {
if (input_geometry[right_border].necessary)
{
recursion_stack.push(std::make_pair(left_border, right_border));
left_border = right_border;
}
++right_border;
} while( right_border < input_geometry.size());
} while (right_border < input_geometry.size());
}
while( !recursion_stack.empty() ) {
//pop next element
const PairOfPoints pair = recursion_stack.top();
while (!recursion_stack.empty())
{
// pop next element
const GeometryRange pair = recursion_stack.top();
recursion_stack.pop();
BOOST_ASSERT_MSG(
input_geometry[pair.first].necessary,
"left border mus be necessary"
);
BOOST_ASSERT_MSG(
input_geometry[pair.second].necessary,
"right border must be necessary"
);
BOOST_ASSERT_MSG(
pair.second < input_geometry.size(),
"right border outside of geometry"
);
BOOST_ASSERT_MSG(
pair.first < pair.second,
"left border on the wrong side"
);
BOOST_ASSERT_MSG(input_geometry[pair.first].necessary, "left border mus be necessary");
BOOST_ASSERT_MSG(input_geometry[pair.second].necessary, "right border must be necessary");
BOOST_ASSERT_MSG(pair.second < input_geometry.size(), "right border outside of geometry");
BOOST_ASSERT_MSG(pair.first < pair.second, "left border on the wrong side");
double max_distance = std::numeric_limits<double>::min();
std::size_t farthest_element_index = pair.second;
//find index idx of element with max_distance
for(std::size_t i = pair.first+1; i < pair.second; ++i){
const int temp_dist = ComputeDistance(
input_geometry[i].location,
input_geometry[pair.first].location,
input_geometry[pair.second].location
);
unsigned farthest_element_index = pair.second;
// find index idx of element with max_distance
for (unsigned i = pair.first + 1; i < pair.second; ++i)
{
const int temp_dist = FixedPointCoordinate::ComputePerpendicularDistance(
input_geometry[i].location,
input_geometry[pair.first].location,
input_geometry[pair.second].location);
const double distance = std::abs(temp_dist);
if(
distance > DouglasPeuckerThresholds[zoom_level] &&
distance > max_distance
) {
if (distance > DouglasPeuckerThresholds[zoom_level] && distance > max_distance)
{
farthest_element_index = i;
max_distance = distance;
}
}
if (max_distance > DouglasPeuckerThresholds[zoom_level]) {
if (max_distance > DouglasPeuckerThresholds[zoom_level])
{
// mark idx as necessary
input_geometry[farthest_element_index].necessary = true;
if (1 < (farthest_element_index - pair.first) ) {
recursion_stack.push(
std::make_pair(pair.first, farthest_element_index)
);
if (1 < (farthest_element_index - pair.first))
{
recursion_stack.push(std::make_pair(pair.first, farthest_element_index));
}
if (1 < (pair.second - farthest_element_index) ) {
recursion_stack.push(
std::make_pair(farthest_element_index, pair.second)
);
if (1 < (pair.second - farthest_element_index))
{
recursion_stack.push(std::make_pair(farthest_element_index, pair.second));
}
}
}

37
Algorithms/DouglasPeucker.h
View File

@ -28,16 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef DOUGLASPEUCKER_H_
#define DOUGLASPEUCKER_H_
#include "../Util/SimpleLogger.h"
#include <osrm/Coordinate.h>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <cmath>
#include <limits>
#include <stack>
#include <vector>
@ -48,25 +38,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* bit indicating if the points is present in the generalization.
* Note: points may also be pre-selected*/
struct FixedPointCoordinate;
struct SegmentInformation;
class DouglasPeucker {
private:
typedef std::pair<std::size_t, std::size_t> PairOfPoints;
//Stack to simulate the recursion
std::stack<PairOfPoints> recursion_stack;
class DouglasPeucker
{
private:
typedef std::pair<unsigned, unsigned> GeometryRange;
// Stack to simulate the recursion
std::stack<GeometryRange> recursion_stack;
double ComputeDistance(
const FixedPointCoordinate& point,
const FixedPointCoordinate& segA,
const FixedPointCoordinate& segB
) const;
public:
void Run(
std::vector<SegmentInformation> & input_geometry,
const unsigned zoom_level
);
double ComputeDistance(const FixedPointCoordinate &point,
const FixedPointCoordinate &segA,
const FixedPointCoordinate &segB) const;
public:
void Run(std::vector<SegmentInformation> &input_geometry, const unsigned zoom_level);
};
#endif /* DOUGLASPEUCKER_H_ */

316
DataStructures/Coordinate.cpp
View File

@ -26,6 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <osrm/Coordinate.h>
#include "../Util/MercatorUtil.h"
#include "../Util/SimpleLogger.h"
#include "../Util/StringUtil.h"
@ -38,21 +39,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <limits>
FixedPointCoordinate::FixedPointCoordinate()
: lat(std::numeric_limits<int>::min()),
lon(std::numeric_limits<int>::min())
{ }
: lat(std::numeric_limits<int>::min()), lon(std::numeric_limits<int>::min())
{
}
FixedPointCoordinate::FixedPointCoordinate(int lat, int lon)
: lat(lat),
lon(lon)
FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) : lat(lat), lon(lon)
{
#ifndef NDEBUG
if(0 != (std::abs(lat) >> 30))
if (0 != (std::abs(lat) >> 30))
{
std::bitset<32> y(lat);
SimpleLogger().Write(logDEBUG) << "broken lat: " << lat << ", bits: " << y;
}
if(0 != (std::abs(lon) >> 30))
if (0 != (std::abs(lon) >> 30))
{
std::bitset<32> x(lon);
SimpleLogger().Write(logDEBUG) << "broken lon: " << lon << ", bits: " << x;
@ -60,124 +59,265 @@ FixedPointCoordinate::FixedPointCoordinate(int lat, int lon)
#endif
}
void FixedPointCoordinate::Reset() {
void FixedPointCoordinate::Reset()
{
lat = std::numeric_limits<int>::min();
lon = std::numeric_limits<int>::min();
}
bool FixedPointCoordinate::isSet() const {
return (std::numeric_limits<int>::min() != lat) &&
(std::numeric_limits<int>::min() != lon);
bool FixedPointCoordinate::isSet() const
{
return (std::numeric_limits<int>::min() != lat) && (std::numeric_limits<int>::min() != lon);
}
bool FixedPointCoordinate::isValid() const {
if (lat > 90*COORDINATE_PRECISION ||
lat < -90*COORDINATE_PRECISION ||
lon > 180*COORDINATE_PRECISION ||
lon < -180*COORDINATE_PRECISION)
bool FixedPointCoordinate::isValid() const
{
if (lat > 90 * COORDINATE_PRECISION || lat < -90 * COORDINATE_PRECISION ||
lon > 180 * COORDINATE_PRECISION || lon < -180 * COORDINATE_PRECISION)
{
return false;
}
return true;
}
bool FixedPointCoordinate::operator==(const FixedPointCoordinate & other) const {
bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const
{
return lat == other.lat && lon == other.lon;
}
double FixedPointCoordinate::ApproximateDistance(
const int lat1,
const int lon1,
const int lat2,
const int lon2
) {
double FixedPointCoordinate::ApproximateDistance(const int lat1,
const int lon1,
const int lat2,
const int lon2)
{
BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
double RAD = 0.017453292519943295769236907684886;
double lt1 = lat1/COORDINATE_PRECISION;
double ln1 = lon1/COORDINATE_PRECISION;
double lt2 = lat2/COORDINATE_PRECISION;
double ln2 = lon2/COORDINATE_PRECISION;
double dlat1=lt1*(RAD);
double lt1 = lat1 / COORDINATE_PRECISION;
double ln1 = lon1 / COORDINATE_PRECISION;
double lt2 = lat2 / COORDINATE_PRECISION;
double ln2 = lon2 / COORDINATE_PRECISION;
double dlat1 = lt1 * (RAD);
double dlong1=ln1*(RAD);
double dlat2=lt2*(RAD);
double dlong2=ln2*(RAD);
double dlong1 = ln1 * (RAD);
double dlat2 = lt2 * (RAD);
double dlong2 = ln2 * (RAD);
double dLong=dlong1-dlong2;
double dLat=dlat1-dlat2;
double dLong = dlong1 - dlong2;
double dLat = dlat1 - dlat2;
double aHarv= pow(sin(dLat/2.0),2.0)+cos(dlat1)*cos(dlat2)*pow(sin(dLong/2.),2);
double cHarv=2.*atan2(sqrt(aHarv),sqrt(1.0-aHarv));
//earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
//The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const double earth=6372797.560856;
return earth*cHarv;
double aHarv = pow(sin(dLat / 2.0), 2.0) + cos(dlat1) * cos(dlat2) * pow(sin(dLong / 2.), 2);
double cHarv = 2. * atan2(sqrt(aHarv), sqrt(1.0 - aHarv));
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const double earth = 6372797.560856;
return earth * cHarv;
}
double FixedPointCoordinate::ApproximateDistance(
const FixedPointCoordinate &c1,
const FixedPointCoordinate &c2
) {
double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate &c2)
{
return ApproximateDistance(c1.lat, c1.lon, c2.lat, c2.lon);
}
double FixedPointCoordinate::ApproximateEuclideanDistance(
const FixedPointCoordinate &c1,
const FixedPointCoordinate &c2
) {
double FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate &c2)
{
BOOST_ASSERT(c1.lat != std::numeric_limits<int>::min());
BOOST_ASSERT(c1.lon != std::numeric_limits<int>::min());
BOOST_ASSERT(c2.lat != std::numeric_limits<int>::min());
BOOST_ASSERT(c2.lon != std::numeric_limits<int>::min());
const double RAD = 0.017453292519943295769236907684886;
const double lat1 = (c1.lat/COORDINATE_PRECISION)*RAD;
const double lon1 = (c1.lon/COORDINATE_PRECISION)*RAD;
const double lat2 = (c2.lat/COORDINATE_PRECISION)*RAD;
const double lon2 = (c2.lon/COORDINATE_PRECISION)*RAD;
const double lat1 = (c1.lat / COORDINATE_PRECISION) * RAD;
const double lon1 = (c1.lon / COORDINATE_PRECISION) * RAD;
const double lat2 = (c2.lat / COORDINATE_PRECISION) * RAD;
const double lon2 = (c2.lon / COORDINATE_PRECISION) * RAD;
const double x = (lon2-lon1) * cos((lat1+lat2)/2.);
const double y = (lat2-lat1);
const double x = (lon2 - lon1) * cos((lat1 + lat2) / 2.);
const double y = (lat2 - lat1);
const double earthRadius = 6372797.560856;
return sqrt(x*x + y*y) * earthRadius;
return sqrt(x * x + y * y) * earthRadius;
}
void FixedPointCoordinate::convertInternalLatLonToString(
const int value,
std::string & output
) {
// Yuck! Code duplication. This function is also in EgdeBasedNode.h
double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &point,
const FixedPointCoordinate &segA,
const FixedPointCoordinate &segB)
{
const double x = lat2y(point.lat / COORDINATE_PRECISION);
const double y = point.lon / COORDINATE_PRECISION;
const double a = lat2y(segA.lat / COORDINATE_PRECISION);
const double b = segA.lon / COORDINATE_PRECISION;
const double c = lat2y(segB.lat / COORDINATE_PRECISION);
const double d = segB.lon / COORDINATE_PRECISION;
double p, q, nY;
if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
{
const double m = (d - b) / (c - a); // slope
// Projection of (x,y) on line joining (a,b) and (c,d)
p = ((x + (m * y)) + (m * m * a - m * b)) / (1. + m * m);
q = b + m * (p - a);
}
else
{
p = c;
q = y;
}
nY = (d * p - c * q) / (a * d - b * c);
// discretize the result to coordinate precision. it's a hack!
if (std::abs(nY) < (1. / COORDINATE_PRECISION))
{
nY = 0.;
}
double r = (p - nY * a) / c;
if (std::isnan(r))
{
r = ((segB.lat == point.lat) && (segB.lon == point.lon)) ? 1. : 0.;
}
else if (std::abs(r) <= std::numeric_limits<double>::epsilon())
{
r = 0.;
}
else if (std::abs(r - 1.) <= std::numeric_limits<double>::epsilon())
{
r = 1.;
}
FixedPointCoordinate nearest_location;
BOOST_ASSERT(!std::isnan(r));
if (r <= 0.)
{ // point is "left" of edge
nearest_location.lat = segA.lat;
nearest_location.lon = segA.lon;
}
else if (r >= 1.)
{ // point is "right" of edge
nearest_location.lat = segB.lat;
nearest_location.lon = segB.lon;
}
else
{ // point lies in between
nearest_location.lat = y2lat(p) * COORDINATE_PRECISION;
nearest_location.lon = q * COORDINATE_PRECISION;
}
BOOST_ASSERT(nearest_location.isValid());
const double approximated_distance =
FixedPointCoordinate::ApproximateDistance(point, nearest_location);
BOOST_ASSERT(0. <= approximated_distance);
return approximated_distance;
}
double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &coord_a,
const FixedPointCoordinate &coord_b,
const FixedPointCoordinate &query_location,
FixedPointCoordinate &nearest_location,
double &r)
{
BOOST_ASSERT(query_location.isValid());
const double x = lat2y(query_location.lat / COORDINATE_PRECISION);
const double y = query_location.lon / COORDINATE_PRECISION;
const double a = lat2y(coord_a.lat / COORDINATE_PRECISION);
const double b = coord_a.lon / COORDINATE_PRECISION;
const double c = lat2y(coord_b.lat / COORDINATE_PRECISION);
const double d = coord_b.lon / COORDINATE_PRECISION;
double p, q /*,mX*/, nY;
if (std::abs(a - c) > std::numeric_limits<double>::epsilon())
{
const double m = (d - b) / (c - a); // slope
// Projection of (x,y) on line joining (a,b) and (c,d)
p = ((x + (m * y)) + (m * m * a - m * b)) / (1. + m * m);
q = b + m * (p - a);
}
else
{
p = c;
q = y;
}
nY = (d * p - c * q) / (a * d - b * c);
// discretize the result to coordinate precision. it's a hack!
if (std::abs(nY) < (1. / COORDINATE_PRECISION))
{
nY = 0.;
}
r = (p - nY * a) / c; // These values are actually n/m+n and m/m+n , we need
// not calculate the explicit values of m an n as we
// are just interested in the ratio
if (std::isnan(r))
{
r = ((coord_b.lat == query_location.lat) && (coord_b.lon == query_location.lon)) ? 1. : 0.;
}
else if (std::abs(r) <= std::numeric_limits<double>::epsilon())
{
r = 0.;
}
else if (std::abs(r - 1.) <= std::numeric_limits<double>::epsilon())
{
r = 1.;
}
BOOST_ASSERT(!std::isnan(r));
if (r <= 0.)
{
nearest_location.lat = coord_a.lat;
nearest_location.lon = coord_a.lon;
}
else if (r >= 1.)
{
nearest_location.lat = coord_b.lat;
nearest_location.lon = coord_b.lon;
}
else
{
// point lies in between
nearest_location.lat = y2lat(p) * COORDINATE_PRECISION;
nearest_location.lon = q * COORDINATE_PRECISION;
}
BOOST_ASSERT(nearest_location.isValid());
// TODO: Replace with euclidean approximation when k-NN search is done
// const double approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance(
const double approximated_distance =
FixedPointCoordinate::ApproximateDistance(query_location, nearest_location);
BOOST_ASSERT(0. <= approximated_distance);
return approximated_distance;
}
void FixedPointCoordinate::convertInternalLatLonToString(const int value, std::string &output)
{
char buffer[100];
buffer[11] = 0; // zero termination
char* string = printInt< 11, 6 >( buffer, value );
char *string = printInt<11, 6>(buffer, value);
output = string;
}
void FixedPointCoordinate::convertInternalCoordinateToString(
const FixedPointCoordinate & coord,
std::string & output
) {
std::string tmp;
tmp.reserve(23);
convertInternalLatLonToString(coord.lon, tmp);
output = tmp;
output += ",";
convertInternalLatLonToString(coord.lat, tmp);
output += tmp;
}
void FixedPointCoordinate::convertInternalReversedCoordinateToString(
const FixedPointCoordinate & coord,
std::string & output
) {
std::string tmp;
tmp.reserve(23);
convertInternalLatLonToString(coord.lat, tmp);
output = tmp;
output += ",";
convertInternalLatLonToString(coord.lon, tmp);
output += tmp;
}
void FixedPointCoordinate::Output(std::ostream & out) const
void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord,
std::string &output)
{
out << "(" << lat/COORDINATE_PRECISION << "," << lon/COORDINATE_PRECISION << ")";
std::string tmp;
tmp.reserve(23);
convertInternalLatLonToString(coord.lon, tmp);
output = tmp;
output += ",";
convertInternalLatLonToString(coord.lat, tmp);
output += tmp;
}
void
FixedPointCoordinate::convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord,
std::string &output)
{
std::string tmp;
tmp.reserve(23);
convertInternalLatLonToString(coord.lat, tmp);
output = tmp;
output += ",";
convertInternalLatLonToString(coord.lon, tmp);
output += tmp;
}
void FixedPointCoordinate::Output(std::ostream &out) const
{
out << "(" << lat / COORDINATE_PRECISION << "," << lon / COORDINATE_PRECISION << ")";
}

72
DataStructures/EdgeBasedNode.h
View File

@ -1,7 +1,6 @@
#ifndef EDGE_BASED_NODE_H
#define EDGE_BASED_NODE_H
#include "../Util/MercatorUtil.h"
#include "../Util/SimpleLogger.h"
#include "../typedefs.h"
@ -61,81 +60,14 @@ struct EdgeBasedNode {
);
}
inline static double ComputePerpendicularDistance(
const FixedPointCoordinate & coord_a,
const FixedPointCoordinate & coord_b,
const FixedPointCoordinate & query_location,
FixedPointCoordinate & nearest_location,
double & r
) {
BOOST_ASSERT( query_location.isValid() );
const double x = lat2y(query_location.lat/COORDINATE_PRECISION);
const double y = query_location.lon/COORDINATE_PRECISION;
const double a = lat2y(coord_a.lat/COORDINATE_PRECISION);
const double b = coord_a.lon/COORDINATE_PRECISION;
const double c = lat2y(coord_b.lat/COORDINATE_PRECISION);
const double d = coord_b.lon/COORDINATE_PRECISION;
double p,q/*,mX*/,nY;
if( std::abs(a-c) > std::numeric_limits<double>::epsilon() ){
const double m = (d-b)/(c-a); // slope
// Projection of (x,y) on line joining (a,b) and (c,d)
p = ((x + (m*y)) + (m*m*a - m*b))/(1. + m*m);
q = b + m*(p - a);
} else {
p = c;
q = y;
}
nY = (d*p - c*q)/(a*d - b*c);
//discretize the result to coordinate precision. it's a hack!
if( std::abs(nY) < (1./COORDINATE_PRECISION) ) {
nY = 0.;
}
r = (p - nY*a)/c;// These values are actually n/m+n and m/m+n , we need
// not calculate the explicit values of m an n as we
// are just interested in the ratio
if( std::isnan(r) ) {
r = ((coord_b.lat == query_location.lat) && (coord_b.lon == query_location.lon)) ? 1. : 0.;
} else if( std::abs(r) <= std::numeric_limits<double>::epsilon() ) {
r = 0.;
} else if( std::abs(r-1.) <= std::numeric_limits<double>::epsilon() ) {
r = 1.;
}
BOOST_ASSERT( !std::isnan(r) );
if( r <= 0. ){
nearest_location.lat = coord_a.lat;
nearest_location.lon = coord_a.lon;
} else if( r >= 1. ){
nearest_location.lat = coord_b.lat;
nearest_location.lon = coord_b.lon;
} else {
// point lies in between
nearest_location.lat = y2lat(p)*COORDINATE_PRECISION;
nearest_location.lon = q*COORDINATE_PRECISION;
}
BOOST_ASSERT( nearest_location.isValid() );
// TODO: Replace with euclidean approximation when k-NN search is done
// const double approximated_distance = FixedPointCoordinate::ApproximateEuclideanDistance(
const double approximated_distance = FixedPointCoordinate::ApproximateDistance(
query_location,
nearest_location
);
BOOST_ASSERT( 0. <= approximated_distance );
return approximated_distance;
}
static inline FixedPointCoordinate Centroid(
const FixedPointCoordinate & a,
const FixedPointCoordinate & b
) {
FixedPointCoordinate centroid;
//The coordinates of the midpoint are given by:
//x = (x1 + x2) /2 and y = (y1 + y2) /2.
centroid.lon = (std::min(a.lon, b.lon) + std::max(a.lon, b.lon))/2;
centroid.lat = (std::min(a.lat, b.lat) + std::max(a.lat, b.lat))/2;
centroid.lat = (a.lat + b.lat)/2;
centroid.lon = (a.lon + b.lon)/2;
return centroid;
}

2
DataStructures/StaticRTree.h
View File

@ -672,7 +672,7 @@ public:
}
double current_ratio = 0.;
const double current_perpendicular_distance = current_edge.ComputePerpendicularDistance(
const double current_perpendicular_distance = FixedPointCoordinate::ComputePerpendicularDistance(
m_coordinate_list->at(current_edge.u),
m_coordinate_list->at(current_edge.v),
input_coordinate,

61
Include/osrm/Coordinate.h
View File

@ -28,57 +28,54 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef FIXED_POINT_COORDINATE_H_
#define FIXED_POINT_COORDINATE_H_
#include <iosfwd> //for std::ostream
#include <iosfwd> //for std::ostream
static const double COORDINATE_PRECISION = 1000000.;
struct FixedPointCoordinate {
struct FixedPointCoordinate
{
int lat;
int lon;
FixedPointCoordinate();
explicit FixedPointCoordinate( int lat, int lon);
explicit FixedPointCoordinate(int lat, int lon);
void Reset();
bool isSet() const;
bool isValid() const;
bool operator==(const FixedPointCoordinate & other) const;
bool operator==(const FixedPointCoordinate &other) const;
static double ApproximateDistance(
const int lat1,
const int lon1,
const int lat2,
const int lon2
);
static double
ApproximateDistance(const int lat1, const int lon1, const int lat2, const int lon2);
static double ApproximateDistance(
const FixedPointCoordinate & c1,
const FixedPointCoordinate & c2
);
static double ApproximateDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate &c2);
static double ApproximateEuclideanDistance(
const FixedPointCoordinate & c1,
const FixedPointCoordinate & c2
);
static double ApproximateEuclideanDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate &c2);
static void convertInternalLatLonToString(
const int value,
std::string & output
);
static void convertInternalLatLonToString(const int value, std::string &output);
static void convertInternalCoordinateToString(
const FixedPointCoordinate & coord,
std::string & output
);
static void convertInternalCoordinateToString(const FixedPointCoordinate &coord,
std::string &output);
static void convertInternalReversedCoordinateToString(
const FixedPointCoordinate & coord,
std::string & output
);
static void convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord,
std::string &output);
void Output(std::ostream & out) const;
static double ComputePerpendicularDistance(const FixedPointCoordinate &point,
const FixedPointCoordinate &segA,
const FixedPointCoordinate &segB);
static double ComputePerpendicularDistance(const FixedPointCoordinate &coord_a,
const FixedPointCoordinate &coord_b,
const FixedPointCoordinate &query_location,
FixedPointCoordinate &nearest_location,
double &r);
void Output(std::ostream &out) const;
};
inline std::ostream& operator<<(std::ostream& o, FixedPointCoordinate const & c){
inline std::ostream &operator<<(std::ostream &o, FixedPointCoordinate const &c)
{
c.Output(o);
return o;
}