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 "DouglasPeucker.h"
#include "../DataStructures/SegmentInformation.h" #include "../DataStructures/SegmentInformation.h"
#include "../Util/MercatorUtil.h"
#include <boost/assert.hpp>
#include <limits> #include <limits>
//These thresholds are more or less heuristically chosen. // These thresholds are more or less heuristically chosen.
static double DouglasPeuckerThresholds[19] = { static double DouglasPeuckerThresholds[19] = {262144., // z0
262144., //z0 131072., // z1
131072., //z1 65536., // z2
65536., //z2 32768., // z3
32768., //z3 16384., // z4
16384., //z4 8192., // z5
8192., //z5 4096., // z6
4096., //z6 2048., // z7
2048., //z7 960., // z8
960., //z8 480., // z9
480., //z9 240., // z10
240., //z10 90., // z11
90., //z11 50., // z12
50., //z12 25., // z13
25., //z13 15., // z14
15., //z14 5., // z15
5., //z15 .65, // z16
.65, //z16 .5, // z17
.5, //z17 .35 // z18
.35 //z18
}; };
/** void DouglasPeucker::Run(std::vector<SegmentInformation> &input_geometry, const unsigned zoom_level)
* 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
) {
{ {
BOOST_ASSERT_MSG(zoom_level < 19, "unsupported zoom level"); BOOST_ASSERT_MSG(zoom_level < 19, "unsupported zoom level");
BOOST_ASSERT_MSG(1 < input_geometry.size(), "geometry invalid"); BOOST_ASSERT_MSG(1 < input_geometry.size(), "geometry invalid");
std::size_t left_border = 0; unsigned left_border = 0;
std::size_t right_border = 1; unsigned right_border = 1;
//Sweep over array and identify those ranges that need to be checked // Sweep over array and identify those ranges that need to be checked
do { do
BOOST_ASSERT_MSG( {
input_geometry[left_border].necessary, BOOST_ASSERT_MSG(input_geometry[left_border].necessary,
"left border must be necessary" "left border must be necessary");
); BOOST_ASSERT_MSG(input_geometry.back().necessary, "right 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)); recursion_stack.push(std::make_pair(left_border, right_border));
left_border = right_border; left_border = right_border;
} }
++right_border; ++right_border;
} while( right_border < input_geometry.size()); } while (right_border < input_geometry.size());
} }
while( !recursion_stack.empty() ) { while (!recursion_stack.empty())
//pop next element {
const PairOfPoints pair = recursion_stack.top(); // pop next element
const GeometryRange pair = recursion_stack.top();
recursion_stack.pop(); recursion_stack.pop();
BOOST_ASSERT_MSG( BOOST_ASSERT_MSG(input_geometry[pair.first].necessary, "left border mus be necessary");
input_geometry[pair.first].necessary, BOOST_ASSERT_MSG(input_geometry[pair.second].necessary, "right border must be necessary");
"left border mus 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.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(); double max_distance = std::numeric_limits<double>::min();
std::size_t farthest_element_index = pair.second; unsigned farthest_element_index = pair.second;
//find index idx of element with max_distance // find index idx of element with max_distance
for(std::size_t i = pair.first+1; i < pair.second; ++i){ for (unsigned i = pair.first + 1; i < pair.second; ++i)
const int temp_dist = ComputeDistance( {
input_geometry[i].location, const int temp_dist = FixedPointCoordinate::ComputePerpendicularDistance(
input_geometry[pair.first].location, input_geometry[i].location,
input_geometry[pair.second].location input_geometry[pair.first].location,
); input_geometry[pair.second].location);
const double distance = std::abs(temp_dist); const double distance = std::abs(temp_dist);
if( if (distance > DouglasPeuckerThresholds[zoom_level] && distance > max_distance)
distance > DouglasPeuckerThresholds[zoom_level] && {
distance > max_distance
) {
farthest_element_index = i; farthest_element_index = i;
max_distance = distance; max_distance = distance;
} }
} }
if (max_distance > DouglasPeuckerThresholds[zoom_level]) { if (max_distance > DouglasPeuckerThresholds[zoom_level])
{
// mark idx as necessary // mark idx as necessary
input_geometry[farthest_element_index].necessary = true; input_geometry[farthest_element_index].necessary = true;
if (1 < (farthest_element_index - pair.first) ) { if (1 < (farthest_element_index - pair.first))
recursion_stack.push( {
std::make_pair(pair.first, farthest_element_index) recursion_stack.push(std::make_pair(pair.first, farthest_element_index));
);
} }
if (1 < (pair.second - farthest_element_index) ) { if (1 < (pair.second - farthest_element_index))
recursion_stack.push( {
std::make_pair(farthest_element_index, pair.second) 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_ #ifndef DOUGLASPEUCKER_H_
#define 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 <stack>
#include <vector> #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. * bit indicating if the points is present in the generalization.
* Note: points may also be pre-selected*/ * Note: points may also be pre-selected*/
struct FixedPointCoordinate;
struct SegmentInformation; struct SegmentInformation;
class DouglasPeucker { class DouglasPeucker
private: {
typedef std::pair<std::size_t, std::size_t> PairOfPoints; private:
//Stack to simulate the recursion typedef std::pair<unsigned, unsigned> GeometryRange;
std::stack<PairOfPoints> recursion_stack; // Stack to simulate the recursion
std::stack<GeometryRange> recursion_stack;
double ComputeDistance( double ComputeDistance(const FixedPointCoordinate &point,
const FixedPointCoordinate& point, const FixedPointCoordinate &segA,
const FixedPointCoordinate& segA, const FixedPointCoordinate &segB) const;
const FixedPointCoordinate& segB
) const;
public:
void Run(
std::vector<SegmentInformation> & input_geometry,
const unsigned zoom_level
);
public:
void Run(std::vector<SegmentInformation> &input_geometry, const unsigned zoom_level);
}; };
#endif /* DOUGLASPEUCKER_H_ */ #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 <osrm/Coordinate.h>
#include "../Util/MercatorUtil.h"
#include "../Util/SimpleLogger.h" #include "../Util/SimpleLogger.h"
#include "../Util/StringUtil.h" #include "../Util/StringUtil.h"
@ -38,21 +39,19 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <limits> #include <limits>
FixedPointCoordinate::FixedPointCoordinate() FixedPointCoordinate::FixedPointCoordinate()
: lat(std::numeric_limits<int>::min()), : lat(std::numeric_limits<int>::min()), lon(std::numeric_limits<int>::min())
lon(std::numeric_limits<int>::min()) {
{ } }
FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) FixedPointCoordinate::FixedPointCoordinate(int lat, int lon) : lat(lat), lon(lon)
: lat(lat),
lon(lon)
{ {
#ifndef NDEBUG #ifndef NDEBUG
if(0 != (std::abs(lat) >> 30)) if (0 != (std::abs(lat) >> 30))
{ {
std::bitset<32> y(lat); std::bitset<32> y(lat);
SimpleLogger().Write(logDEBUG) << "broken lat: " << lat << ", bits: " << y; 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); std::bitset<32> x(lon);
SimpleLogger().Write(logDEBUG) << "broken lon: " << lon << ", bits: " << x; SimpleLogger().Write(logDEBUG) << "broken lon: " << lon << ", bits: " << x;
@ -60,124 +59,265 @@ FixedPointCoordinate::FixedPointCoordinate(int lat, int lon)
#endif #endif
} }
void FixedPointCoordinate::Reset() { void FixedPointCoordinate::Reset()
{
lat = std::numeric_limits<int>::min(); lat = std::numeric_limits<int>::min();
lon = std::numeric_limits<int>::min(); lon = std::numeric_limits<int>::min();
} }
bool FixedPointCoordinate::isSet() const { bool FixedPointCoordinate::isSet() const
return (std::numeric_limits<int>::min() != lat) && {
(std::numeric_limits<int>::min() != lon); return (std::numeric_limits<int>::min() != lat) && (std::numeric_limits<int>::min() != lon);
} }
bool FixedPointCoordinate::isValid() const { bool FixedPointCoordinate::isValid() const
if (lat > 90*COORDINATE_PRECISION || {
lat < -90*COORDINATE_PRECISION || if (lat > 90 * COORDINATE_PRECISION || lat < -90 * COORDINATE_PRECISION ||
lon > 180*COORDINATE_PRECISION || lon > 180 * COORDINATE_PRECISION || lon < -180 * COORDINATE_PRECISION)
lon < -180*COORDINATE_PRECISION)
{ {
return false; return false;
} }
return true; return true;
} }
bool FixedPointCoordinate::operator==(const FixedPointCoordinate & other) const { bool FixedPointCoordinate::operator==(const FixedPointCoordinate &other) const
{
return lat == other.lat && lon == other.lon; return lat == other.lat && lon == other.lon;
} }
double FixedPointCoordinate::ApproximateDistance( double FixedPointCoordinate::ApproximateDistance(const int lat1,
const int lat1, const int lon1,
const int lon1, const int lat2,
const int lat2, const int lon2)
const int lon2 {
) {
BOOST_ASSERT(lat1 != std::numeric_limits<int>::min()); BOOST_ASSERT(lat1 != std::numeric_limits<int>::min());
BOOST_ASSERT(lon1 != std::numeric_limits<int>::min()); BOOST_ASSERT(lon1 != std::numeric_limits<int>::min());
BOOST_ASSERT(lat2 != std::numeric_limits<int>::min()); BOOST_ASSERT(lat2 != std::numeric_limits<int>::min());
BOOST_ASSERT(lon2 != std::numeric_limits<int>::min()); BOOST_ASSERT(lon2 != std::numeric_limits<int>::min());
double RAD = 0.017453292519943295769236907684886; double RAD = 0.017453292519943295769236907684886;
double lt1 = lat1/COORDINATE_PRECISION; double lt1 = lat1 / COORDINATE_PRECISION;
double ln1 = lon1/COORDINATE_PRECISION; double ln1 = lon1 / COORDINATE_PRECISION;
double lt2 = lat2/COORDINATE_PRECISION; double lt2 = lat2 / COORDINATE_PRECISION;
double ln2 = lon2/COORDINATE_PRECISION; double ln2 = lon2 / COORDINATE_PRECISION;
double dlat1=lt1*(RAD); double dlat1 = lt1 * (RAD);
double dlong1=ln1*(RAD); double dlong1 = ln1 * (RAD);
double dlat2=lt2*(RAD); double dlat2 = lt2 * (RAD);
double dlong2=ln2*(RAD); double dlong2 = ln2 * (RAD);
double dLong=dlong1-dlong2; double dLong = dlong1 - dlong2;
double dLat=dlat1-dlat2; double dLat = dlat1 - dlat2;
double aHarv= pow(sin(dLat/2.0),2.0)+cos(dlat1)*cos(dlat2)*pow(sin(dLong/2.),2); 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)); 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) // 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) // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const double earth=6372797.560856; const double earth = 6372797.560856;
return earth*cHarv; return earth * cHarv;
} }
double FixedPointCoordinate::ApproximateDistance( double FixedPointCoordinate::ApproximateDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate &c1, const FixedPointCoordinate &c2)
const FixedPointCoordinate &c2 {
) {
return ApproximateDistance(c1.lat, c1.lon, c2.lat, c2.lon); return ApproximateDistance(c1.lat, c1.lon, c2.lat, c2.lon);
} }
double FixedPointCoordinate::ApproximateEuclideanDistance( double FixedPointCoordinate::ApproximateEuclideanDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate &c1, const FixedPointCoordinate &c2)
const FixedPointCoordinate &c2 {
) {
BOOST_ASSERT(c1.lat != std::numeric_limits<int>::min()); BOOST_ASSERT(c1.lat != std::numeric_limits<int>::min());
BOOST_ASSERT(c1.lon != 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.lat != std::numeric_limits<int>::min());
BOOST_ASSERT(c2.lon != std::numeric_limits<int>::min()); BOOST_ASSERT(c2.lon != std::numeric_limits<int>::min());
const double RAD = 0.017453292519943295769236907684886; const double RAD = 0.017453292519943295769236907684886;
const double lat1 = (c1.lat/COORDINATE_PRECISION)*RAD; const double lat1 = (c1.lat / COORDINATE_PRECISION) * RAD;
const double lon1 = (c1.lon/COORDINATE_PRECISION)*RAD; const double lon1 = (c1.lon / COORDINATE_PRECISION) * RAD;
const double lat2 = (c2.lat/COORDINATE_PRECISION)*RAD; const double lat2 = (c2.lat / COORDINATE_PRECISION) * RAD;
const double lon2 = (c2.lon/COORDINATE_PRECISION)*RAD; const double lon2 = (c2.lon / COORDINATE_PRECISION) * RAD;
const double x = (lon2-lon1) * cos((lat1+lat2)/2.); const double x = (lon2 - lon1) * cos((lat1 + lat2) / 2.);
const double y = (lat2-lat1); const double y = (lat2 - lat1);
const double earthRadius = 6372797.560856; const double earthRadius = 6372797.560856;
return sqrt(x*x + y*y) * earthRadius; return sqrt(x * x + y * y) * earthRadius;
} }
void FixedPointCoordinate::convertInternalLatLonToString( // Yuck! Code duplication. This function is also in EgdeBasedNode.h
const int value, double FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &point,
std::string & output 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]; char buffer[100];
buffer[11] = 0; // zero termination buffer[11] = 0; // zero termination
char* string = printInt< 11, 6 >( buffer, value ); char *string = printInt<11, 6>(buffer, value);
output = string; output = string;
} }
void FixedPointCoordinate::convertInternalCoordinateToString( void FixedPointCoordinate::convertInternalCoordinateToString(const FixedPointCoordinate &coord,
const FixedPointCoordinate & coord, std::string &output)
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
{ {
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 #ifndef EDGE_BASED_NODE_H
#define EDGE_BASED_NODE_H #define EDGE_BASED_NODE_H
#include "../Util/MercatorUtil.h"
#include "../Util/SimpleLogger.h" #include "../Util/SimpleLogger.h"
#include "../typedefs.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( static inline FixedPointCoordinate Centroid(
const FixedPointCoordinate & a, const FixedPointCoordinate & a,
const FixedPointCoordinate & b const FixedPointCoordinate & b
) { ) {
FixedPointCoordinate centroid; FixedPointCoordinate centroid;
//The coordinates of the midpoint are given by: //The coordinates of the midpoint are given by:
//x = (x1 + x2) /2 and y = (y1 + y2) /2. centroid.lat = (a.lat + b.lat)/2;
centroid.lon = (std::min(a.lon, b.lon) + std::max(a.lon, b.lon))/2; centroid.lon = (a.lon + b.lon)/2;
centroid.lat = (std::min(a.lat, b.lat) + std::max(a.lat, b.lat))/2;
return centroid; return centroid;
} }

2
DataStructures/StaticRTree.h
View File

@ -672,7 +672,7 @@ public:
} }
double current_ratio = 0.; 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.u),
m_coordinate_list->at(current_edge.v), m_coordinate_list->at(current_edge.v),
input_coordinate, 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_ #ifndef FIXED_POINT_COORDINATE_H_
#define 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.; static const double COORDINATE_PRECISION = 1000000.;
struct FixedPointCoordinate { struct FixedPointCoordinate
{
int lat; int lat;
int lon; int lon;
FixedPointCoordinate(); FixedPointCoordinate();
explicit FixedPointCoordinate( int lat, int lon); explicit FixedPointCoordinate(int lat, int lon);
void Reset(); void Reset();
bool isSet() const; bool isSet() const;
bool isValid() const; bool isValid() const;
bool operator==(const FixedPointCoordinate & other) const; bool operator==(const FixedPointCoordinate &other) const;
static double ApproximateDistance( static double
const int lat1, ApproximateDistance(const int lat1, const int lon1, const int lat2, const int lon2);
const int lon1,
const int lat2,
const int lon2
);
static double ApproximateDistance( static double ApproximateDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate & c1, const FixedPointCoordinate &c2);
const FixedPointCoordinate & c2
);
static double ApproximateEuclideanDistance( static double ApproximateEuclideanDistance(const FixedPointCoordinate &c1,
const FixedPointCoordinate & c1, const FixedPointCoordinate &c2);
const FixedPointCoordinate & c2
);
static void convertInternalLatLonToString( static void convertInternalLatLonToString(const int value, std::string &output);
const int value,
std::string & output
);
static void convertInternalCoordinateToString( static void convertInternalCoordinateToString(const FixedPointCoordinate &coord,
const FixedPointCoordinate & coord, std::string &output);
std::string & output
);
static void convertInternalReversedCoordinateToString( static void convertInternalReversedCoordinateToString(const FixedPointCoordinate &coord,
const FixedPointCoordinate & coord, std::string &output);
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); c.Output(o);
return o; return o;
} }