[OSM-dev] Retrieve Nearest Way

[Richard Atterer]

Hi Ian,

On Wed, Apr 14, 2010 at 11:27:37AM -0500, Ian Dees wrote:
> I'm trying to write a mockup for simple editing tool and one of the steps 
> needed is to have a fairly quick "find nearest way" call. Can anyone 
> recommend some reading or pseudocode to find the nearest way given a 
> lat/lon?

The following is from my "Leadme" editor/project, 
<http://atterer.net/leadme>. GPL v2 or v3. All coordinates are assumed to 
be in a cartesian coordinate system, so you need to do a Mercator transform 
first, and arguably the code will not find the nearest way in all cases 
because of that - but it should be OK unless you edit a pole. It *is* 
rather accurate in that it actually calculates the distance in 3D (not 2D), 
and it also takes into account the (semi-)spheres around the endpoints of 
each way segment.

Cheers,

  Richard


const MapWay* Map::findNearestWay(const Point *p, double maxDistance,
		double* dist) const {
	double d = maxDistance;
	const MapWay* result = 0;
	for (Ways::const_iterator i = ways.begin(), e = ways.end(); i != e; ++i) {
		//cerr << "Map::findNearestWay quickcheck " << &*i << " d=" << d <<endl;
		if (i->nodes.size() < 2)
			continue; // Actually, such ways are forbidden

		/* First create a bbox for the entire way, to quickly eliminate
		 * ways which are too far away. */
		MapWay::Nodes::const_iterator j = i->nodes.begin(), je = i->nodes.end();
		double wayminx = (*j)->x, waymaxx = wayminx;
		double wayminy = (*j)->y, waymaxy = wayminy;
		while (++j != je) {
			wayminx = min(wayminx, (*j)->x);
			waymaxx = max(waymaxx, (*j)->x);
			wayminy = min(wayminy, (*j)->y);
			waymaxy = max(waymaxy, (*j)->y);
		}
		if (p->x + d < wayminx || p->x - d > waymaxx
				|| p->y + d < wayminy || p->y - d > waymaxy) continue;

		//cerr << "Map::findNearestWay detailcheck " << &*i << " d=" << d << endl;
		/* Bboxes overlap - do the slow, accurate check. Calculate the distance
		 * from each segment of the way. */
		j = i->nodes.begin();
		double dd = distance(*p, **j); /* Check half-sphere around first node */
		if (dd < d) {
			d = dd; result = &*i;
			//cerr << "  near 1st node d=" << d << endl;
		}
		while (true) {
			const MapNode* o = *j; // old node
			++j;
			if (j == je) break;
			const MapNode* c = *j; // current node
			/* t is the normalized direction vector of the way segment */
			double tx = c->x - o->x, ty = c->y - o->y, tz = c->z - o->z;
			double segmentLen = sqrt(tx*tx + ty*ty + tz*tz);
			tx /= segmentLen; ty /= segmentLen; tz /= segmentLen;
			/* projLen = length of projection of p onto the segment o c */
			double hesseDist = o->x * tx + o->y * ty + o->z * tz;
			double projLen = p->x * tx + p->y * ty + p->z * tz - hesseDist;
			//cerr << "  proj=" << projLen << " segment=" << segmentLen << endl;
			if (projLen < 0 || projLen > segmentLen + d/2) continue;
			if (projLen > segmentLen) {
				/* p may be inside half-sphere at endpoint c */
				double dd = distance(*p, *c);
				if (dd < d) {
					d = dd; result = &*i;
					//cerr << "  near n-th node, dd=" << dd << endl;
				}
			} else { /* or inside the cylinder around the segment */
				double distpo = distance(*p, *o);
				double dd = sqrt(distpo*distpo - projLen*projLen);
				if (dd < d) {
					d = dd; result = &*i;
					//cerr << "  near segment, dd=" << dd << endl;
				}
			}
		}
	}
	if (result != 0 && dist != 0)
		*dist = d;
	return result;
}

-- 
  __   ,
  | ) /|  Richard Atterer     |  GnuPG key: 888354F7
  | \/ |  http://atterer.net  |  08A9 7B7D 3D13 3EF2 3D25  D157 79E6 F6DC 8883 54F7