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

Fixed perpendicular distance calculation of segment endpoint is on equator

Browse Source
This commit is contained in:
Patrick Niklaus 2014-07-04 01:04:24 +02:00
parent 2a6585e664
commit b453a42f77
1 changed files with 37 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
DataStructures/Coordinate.cpp
View File

@ -159,10 +159,10 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
// initialize values // initialize values
const float x_value = lat2y(point.lat / COORDINATE_PRECISION); const float x_value = lat2y(point.lat / COORDINATE_PRECISION);
const float y_value = point.lon / COORDINATE_PRECISION; const float y_value = point.lon / COORDINATE_PRECISION;
const float a = lat2y(source_coordinate.lat / COORDINATE_PRECISION); float a = lat2y(source_coordinate.lat / COORDINATE_PRECISION);
const float b = source_coordinate.lon / COORDINATE_PRECISION; float b = source_coordinate.lon / COORDINATE_PRECISION;
const float c = lat2y(target_coordinate.lat / COORDINATE_PRECISION); float c = lat2y(target_coordinate.lat / COORDINATE_PRECISION);
const float d = target_coordinate.lon / COORDINATE_PRECISION; float d = target_coordinate.lon / COORDINATE_PRECISION;
float p, q; float p, q;
if (std::abs(a - c) > std::numeric_limits<float>::epsilon()) if (std::abs(a - c) > std::numeric_limits<float>::epsilon())
{ {
@ -178,15 +178,35 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
q = y_value; q = y_value;
} }
float nY = (d * p - c * q) / (a * d - b * c); float ratio;
// discretize the result to coordinate precision. it's a hack! bool inverse_ratio = false;
if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
// straight line segment on equator
if (std::abs(c) < std::numeric_limits<float>::epsilon() && std::abs(a) < std::numeric_limits<float>::epsilon())
{ {
nY = 0.f; ratio = (q - b) / (d - b);
}
else
{
if (std::abs(c) < std::numeric_limits<float>::epsilon())
{
// swap start/end
std::swap(a, c);
std::swap(b, d);
inverse_ratio = true;
}
float nY = (d * p - c * q) / (a * d - b * c);
// discretize the result to coordinate precision. it's a hack!
if (std::abs(nY) < (1.f / COORDINATE_PRECISION))
{
nY = 0.f;
}
// compute ratio
ratio = (p - nY * a) / c;
} }
// compute ratio
float ratio = (p - nY * a) / c;
if (std::isnan(ratio)) if (std::isnan(ratio))
{ {
ratio = (target_coordinate == point ? 1.f : 0.f); ratio = (target_coordinate == point ? 1.f : 0.f);
@ -200,6 +220,12 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
ratio = 1.f; ratio = 1.f;
} }
// we need to do this, if we switched start/end coordinates
if (inverse_ratio)
{
ratio = 1.0f - ratio;
}
//compute the nearest location //compute the nearest location
FixedPointCoordinate nearest_location; FixedPointCoordinate nearest_location;
BOOST_ASSERT(!std::isnan(ratio)); BOOST_ASSERT(!std::isnan(ratio));
@ -216,6 +242,7 @@ FixedPointCoordinate::ComputePerpendicularDistance(const FixedPointCoordinate &s
nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION); nearest_location.lat = static_cast<int>(y2lat(p) * COORDINATE_PRECISION);
nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION); nearest_location.lon = static_cast<int>(q * COORDINATE_PRECISION);
} }
BOOST_ASSERT(nearest_location.isValid()); BOOST_ASSERT(nearest_location.isValid());
return FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location); return FixedPointCoordinate::ApproximateEuclideanDistance(point, nearest_location);
} }