spOSMroute/mapsys/map_way.c

/***************************************************************************
 *            map_way.c
 *
 *  Copyright  2009 - Steffen Pohle
 *  steffen@gulpe.de
 ****************************************************************************/

/*
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include "map.h"
#include "memoryleak.h"
#include "system.h"
#include "vector.h"



/*****************************************************
 *
 *  Way handling
 *
 */

/* 
 * add the way without checking for it's size, but do an update
 */
int map_way_add_to_hash (struct map_hash *mh, struct map_way *mw) {
	int size;
	struct map_way *w;

	/* check if the hash pos and the waypos are valid */
	if (mw->p_cnt > 0) if (map_geo2igeo (mw->p[0].lon) != mh->pos.ilon || 
	                       map_geo2igeo (mw->p[0].lat) != mh->pos.ilat) {
							   d_printf ("p[0]:%d,%d  mhpos:%d,%d", __FUNCTION__, map_geo2igeo (mw->p[0].lon), map_geo2igeo (mw->p[0].lat), mh->pos.ilon, mh->pos.ilat); 
							   errorexit (-1);
						   }
	
	/* check if the first element is already the one we want to update */
	if (mh->ways && mh->ways->id == mw->id && mh->ways->subid == mw->subid) {
		mh->ways = mh->ways->next;
		mh->wayscnt--;
	}

	/* find possible update and go to the end */
	if (mh->ways) {
		/* find last way */	
		w = mh->ways;
		while (w && w->next) {
			if (w->next->id == mw->id && w->next->subid == mw->subid) {
				/* found old way.. just delete .. */
				w->next = w->next->next;
				mh->wayscnt--;
			}
			if (w->next) w = w->next;
		}
		w->next = (struct map_way*) map_hash_getfree_aligned (mh, POINTERALIGNMENT);
		w = w->next;
	}
	else {
		/* complete new ways */
		mh->ways = (struct map_way*) map_hash_getfree_aligned (mh, POINTERALIGNMENT);
		w = (struct map_way*) map_hash_getfree_aligned (mh, POINTERALIGNMENT);
	}
	
	/* copy the way data to the free entry
	 * calc size and set free entry to it's new place */
	size = map_way_getsize (mw);
	map_way_copy (w, mw);
	w->next = NULL;
	mh->free = mh->free + size;
	mh->wayscnt++;
	SETFLAG(mh->status, MS_changed);

	return 1;
};


/******************************************************************************
 * Add/Update way information
 * - load/ get hash table
 * - calculate size in bytes for this entry
 * - check if pointer in free is enought for this new entry
 *   if not: move old memory block out of the way and put a new data block inside.
 * - add new way
 */
int map_way_add (struct map_way *way, int loadflags) {
	struct map_hash *mh = NULL;
	int size;

	if (way->p == NULL || way->p_cnt == 0) return 0;
	size = map_way_getsize (way);
	mh = map_hash_get (way->p[0].lon, way->p[0].lat, loadflags);

	if (mh == NULL) {
		mh = map_hash_alloc (size + MAP_HASH_DEFAULTSIZE);
		mh->pos.ilon = map_geo2igeo (way->p[0].lon);
		mh->pos.ilat = map_geo2igeo (way->p[0].lat);
		map_hash_add (mh);
	}
	else if (size >= map_hash_getfree_size (mh)) {
		mh = map_hash_realloc (mh, mh->datasize + size + MAP_HASH_DEFAULTSIZE);
	}
	
	return map_way_add_to_hash (mh, way);
};


/*
 * will copy all the data from one way to another, this function will not
 * check if there are conflicts with the memory at all. Also the next field
 * will be keeped untouched.
 */
void map_way_copy (struct map_way *dest, struct map_way *src) {
	int i;

	strncpy (dest->name, src->name, MAP_W_NAMELEN);
	strncpy (dest->ref, src->ref, MAP_W_NAMELEN);
	dest->id = src->id;
	dest->subid = src->subid;
	dest->type = src->type;
	dest->flags = src->flags;
	dest->p_cnt = src->p_cnt;
	dest->n_cnt = src->n_cnt;
	dest->p = (struct map_pos*) (dest->data);
	dest->n = (struct map_waynode*) (dest->data+(sizeof (struct map_pos)*dest->p_cnt));
	for (i = 0; (i < dest->p_cnt || i < dest->n_cnt); i++) {
		if (i < dest->p_cnt) dest->p[i] = src->p[i];
		if (i < dest->n_cnt) dest->n[i] = src->n[i];
	}
};


struct map_way *map_way_find (struct map_hash *mh, unsigned long long int id, unsigned short int subid) {
	struct map_way *mw;
	if (mh == NULL) return NULL;

	mw = mh->ways;
	while (mw) {
		if (mw->id == id && subid == mw->subid) return mw;
		mw = mw->next;
	}
	d_printf ("%s: could not find way %lld:%d ipos:%d,%d", __FUNCTION__, id, subid, mh->pos.ilon, mh->pos.ilat);

	return NULL;
};


int map_way_getsize (struct map_way *way) {
	return (sizeof (struct map_way) + way->n_cnt * sizeof (struct map_waynode) + way->p_cnt * sizeof (struct map_pos));
};


/************************************************************
 * find closest way to the givin position
 * idh: distance to check (igeo distance)
 * if name is not NULL and namelen above zero the name of
 * the road will be copied into name
 */
int map_way_findclosest (struct map_pos *pos, struct map_pos *waypos, int idh, char *name, 
    					int namelen, unsigned long long int *closest_id, unsigned short int *closest_sid) {
	struct map_hash *mh;
	struct map_hashpos ipos;
	struct map_way *mw;
	float dist_closest = 40000.0, f;

	ipos.ilon = map_geo2igeo (pos->lon);
	ipos.ilat = map_geo2igeo (pos->lat);

	mh = map_hash_geti (ipos.ilon, ipos.ilat, MHLOAD_RELOAD);
	if (mh == NULL) return 0;

	mw = mh->ways;
	while (mw) {
		f = map_way_getdistance (mw, pos, waypos);
		if (dist_closest > f) {
			dist_closest = f;
			if (name != NULL && namelen > 0) {
				if (mw->name != NULL && mw->name[0] != 0)
					strncpy (name, mw->name, namelen); 
				else if (mw->ref != NULL && mw->ref[0] != 0)
					strncpy (name, mw->ref, namelen); 
			}
			if (closest_id != NULL)	*closest_id = mw->id;
			if (closest_sid != NULL) *closest_sid = mw->subid;
		}
		mw = mw->next;
	}

	return 1;
};


/*
 * return the closest distance to the way. If waypos is set also return the 
 * point of the way.
 */
float map_way_getdistance (struct map_way *mw, struct map_pos *pos, struct map_pos *waypos) {
	int i, j;
	float f, dist_closest;
	fPoint v1, v2, v3, v4;
	fPoint p1, p2, p3, p4;

	if (!mw || mw->p_cnt == 0) return (-1.0);

	/* check first point.. */
	p3.x = map_lon2km (0.0, pos->lat);
	p3.y = map_lat2km (0.0);
	p2.x = map_lon2km (pos->lon - mw->p[0].lon, mw->p[0].lat);
	p2.y = map_lat2km (pos->lat - mw->p[0].lat);
	v2 = vec_sub (p3, p2);
	dist_closest = sqrt (v2.x * v2.x + v2.y * v2.y);
	if (waypos) {
		waypos->lat = mw->p[0].lat;
		waypos->lon = mw->p[0].lon;
	}

	/* check the other nodes, also the way between */
	for (i = 1; i < (mw->p_cnt + mw->n_cnt); i++) {
		/* mw->p... must be converted to km first. */
		if (i >= mw->p_cnt) {
			/* check nodes */
			j = i - mw->p_cnt;
			p1.x = map_lon2km (pos->lon - mw->p[mw->n[j].pnr].lon, mw->p[mw->n[j].pnr].lat);
			p1.y = map_lat2km (pos->lat - mw->p[mw->n[j].pnr].lat);
			p2.x = map_lon2km (pos->lon - mw->n[j].d.lon, mw->n[j].d.lat);
			p2.y = map_lat2km (pos->lat - mw->n[j].d.lat);
		}
		else {
			/* check waysegments */
			p2.x = map_lon2km (pos->lon - mw->p[i].lon, mw->p[i].lat);
			p2.y = map_lat2km (pos->lat - mw->p[i].lat);
			p1.x = map_lon2km (pos->lon - mw->p[i-1].lon, mw->p[i-1].lat);
			p1.y = map_lat2km (pos->lat - mw->p[i-1].lat);
		}
			
		v1 = vec_sub (p1, p2);
		v2 = vec_sub (p3, p2);
		v3 = vec_normale (v1, v2);

		/* need to check the range f the new vec.. */
		if     (((v3.x <= 0.0 && v1.x <= 0.0) || (v3.x >= 0.0 && v1.x >= 0.0) 
		         || (v3.y <= 0.0 && v1.y <= 0.0) || (v3.y >= 0.0 && v1.y >= 0.0))
		        && fabs(v3.x) <= fabs(v1.x) && fabs(v3.y) <= fabs(v1.y)) {
		
			v4 = vec_sub (v3, v2);
			f = sqrt (v4.x * v4.x + v4.y * v4.y);
		}

		/* if we are out or range just check the distance */
		else {
			f = sqrt (v2.x * v2.x + v2.y * v2.y);
			v3 = v2;
		}

		if (f < dist_closest) {
			dist_closest = f;
			if (waypos) {
				p4 = vec_add (v3, p2);
				waypos->lat = pos->lat - map_km2lat (p4.y);
				waypos->lon = pos->lon - map_km2lon (p4.x, waypos->lat);
			}
		}
	}

	return dist_closest;
};


/***************************************************************************
 * distance calculation
 */
float map_way_getlenght (struct map_way *mw, int start, int end) {
	float ret = 0.0;
	int i;

	if (start == -1 || end == -1) {
		start = 0;
		end = mw->p_cnt;
	}

	if (start > end) {
		i = end;
		end = start;
		start = i;
	}
	
	for (i = start + 1; (i < mw->p_cnt && i <= end); i++) {
		ret += map_getdistance (mw->p[i-1], mw->p[i]);
	}

	return ret;
};


/***************************************************************************
 * Check References if some are identical (seperation with ';')
 */
int map_way_checkref (char *ref1, char *ref2) {
	char ref1_cpy[MAP_W_REFS*MAP_W_NAMELEN];
	char ref2_cpy[MAP_W_REFS*MAP_W_NAMELEN];
	int res = 0, i, j;
	char *next = NULL;
	char *cur = NULL;


	if (ref1 == NULL || ref2 == NULL) return 0;
	if (ref1[0] == 0 || ref2[0] == 0) return 0;
	
	/* make a copy of ref1 and ref2 but filter out all unneeded spaces */
	for (i = 0, j = 0; i < strlen (ref1); i++)
		if (ref1[i] != ' ') ref1_cpy[j++] = ref1[i];
	ref1_cpy[j] = 0;

	for (i = 0, j = 0; i < strlen (ref2); i++)
		if (ref2[i] != ' ') ref2_cpy[j++] = ref2[i];
	ref2_cpy[j] = 0;

	cur = ref1_cpy;
	do {
		next = strchr (cur, ';');
		if (next != NULL) {
			next[0] = 0;
			next++;
		}

		if (strstr (ref2_cpy, cur) != NULL) res++;
	}
	while ((cur = next) != NULL);

	return res;
};