steffen
/
spOSMroute
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'e93500869a'
${ noResults }
spOSMroute/main/routing.c

996 lines
30 KiB
Raw Blame History

/* $Id: routing.c,v 1.4 2014/02/12 21:23:06 steffen Exp $ */
/***************************************************************************
* routing.c
*
* Copyright 2011 - Steffen Pohle
* steffen@gulpe.de
****************************************************************************/
#include <math.h>
#include "osmroute.h"
#include "gps.h"
#include "routing.h"
#include "system.h"
#include "memoryleak.h"
#include "map.h"
#include "vector.h"
struct rtdata *route = NULL;
void route_free (struct rtdata *rt);
struct rtdata *route_alloc (struct rtdata *rt, int max, int stmax);
void route_calc (struct rtdata *r);
void route_onway (struct rtdata *r);
int route_checkway (struct rtdata *r, struct rtway *rtw);
int route_calc_way (struct rtdata *r, struct rtway *rtw, struct map_way *mw, struct rtway *rtwnew, struct map_way *mwnew);
int route_optway (struct rtdata *r, struct rtway *rtw);
int route_optrefsconnectways (struct rtdata *r, struct rtrefway from, struct rtrefway to, struct rtref *ref);
void route_optrefs (struct rtdata *r, struct rtref *ref);
struct rtwaylist *route_getstack (struct rtdata *r, int waytype);
int rtl_find (struct rtwaylist *rtl, struct rtwayid *id);
int rtl_realloc (struct rtwaylist *rtl, int max);
int rtl_fifo_push (struct rtwaylist *rtl, struct rtway *rtw);
int rtl_lifo_push (struct rtwaylist *rtl, struct rtway *rtw);
int rtl_pop (struct rtwaylist *rtl, struct rtway *rtw);
int rtl_addsorted (struct rtwaylist *rtl, struct rtway *rtw);
void rtl_debug (struct rtwaylist *rtl);
void rtl_delete (struct rtwaylist *rtl, int nr);
void rtrlrefs_alloc (struct rtreflist *rtrl, int cnt);
void rtrl_addway (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw);
void rtrl_addwayname (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw, char *name);
void rtrefways_realloc (struct rtref *rtref, int cnt);
void rtrl_recalculate (struct rtdata *r, struct map_way *mw, struct rtwayid from);
/****************************************************************************
* references and names ways
*/
void rtrlrefs_alloc (struct rtreflist *rtrl, int cnt) {
int i;
if (rtrl == NULL) return;
if (cnt == 0) {
/* free all data */
for (i = 0; i < rtrl->cnt && rtrl->refs != NULL; i++) {
if (rtrl->refs[i].ways) ml_free (rtrl->refs[i].ways);
rtrl->refs[i].ways = NULL;
}
if (rtrl->refs) ml_free (rtrl->refs);
rtrl->refs = NULL;
rtrl->max = 0;
rtrl->cnt = 0;
}
else if (cnt <= rtrl->max) return;
else {
/* allocate more memory */
rtrl->refs = (struct rtref *) ml_realloc (rtrl->refs, cnt * sizeof (struct rtref));
for (i = rtrl->cnt; i < cnt; i++) {
/* clear reset data */
rtrl->refs[i].refname[0] = 0;
rtrl->refs[i].cnt = 0;
rtrl->refs[i].max = 0;
rtrl->refs[i].ways = NULL;
rtrl->refs[i].lastnr = 0;
}
rtrl->max = cnt;
}
};
/* add the giving way to the stack list, before check for double entry and
* set the priority to 0 */
void rtrl_recalculate (struct rtdata *r, struct map_way *mw, struct rtwayid from) {
int nr, i;
struct rtwaylist *stack;
struct rtwayid wid;
struct rtway rtw;
if (mw == NULL) {
d_printf ("%s:%d %s: mw == NULL abort", __FILE__, __LINE__, __FUNCTION__);
return;
}
wid.id = mw->id;
wid.sid = mw->subid;
wid.type = mw->type;
/* check if this way is already calculated.. */
stack = &r->ways;
if ((nr = rtl_find (stack, &wid)) >= 0) {
d_printf ("%s:%d mw[%lld:%d] is already calculated.", __FILE__, __LINE__, wid.id, wid.sid);
return;
}
/* find this way inside the stacks to set the priority to 0.0 */
stack = route_getstack (r, wid.type);
if ((nr = rtl_find (stack, &wid)) >= 0) {
rtw = stack->array[nr];
rtl_delete (stack, nr);
rtw.priority = 0.0;
rtw.cycle2 = r->cycle;
}
/* rtw has to be created from the begining */
else {
/* search from inside n */
for (i = mw->n_cnt-1; (i >= 0 && (from.id != mw->n[i].d_id || from.sid != mw->n[i].d_subid)); i--);
rtw.way = wid;
rtw.pos = mw->p[0];
if (i >= 0) rtw.pnr = mw->n[i].pnr;
else rtw.pnr = -1;
rtw.dist2dest = -1.0;
rtw.dist2pos = -1.0;
rtw.step = -1; /* indicate we know nothing.. just need this on top of the list..
* so it will be ignored as long as there is nothing known .. */
rtw.priority = 0.0;
rtw.cycle1 = r->cycle;
rtw.cycle2 = r->cycle;
rtw.deltadist = 0.0;
}
rtl_addsorted (stack, &rtw);
};
/* add reference to the list of references, also recheck if we have two not
* connected references, abort at 1.5times distance */
#define RTRL_MAXWAYS 512
void rtrl_addwayname (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw, char *name) {
int ir; // index refnames
int nr; // segment number
int i, n;
struct rtref *ref; // pointer to reference data
// static struct rtwayid _rtrl_wlist [RTRL_MAXWAYS]; // only create this once, saves time..
int _rtrl_wlist_cnt = 0; // re-init with zero elements
d_printf (" rtrl_addwayname '%s' id:%lld:%d mw->ref:%s", name, mw->id, mw->subid, mw->ref);
for (ir = rtrl->cnt; (ir >= 0 && strcmp (name, rtrl->refs[ir].refname) != 0); ir--);
if (ir < 0) {
/* nothing found add new reference */
if (rtrl->cnt >= rtrl->max || rtrl->refs)
rtrlrefs_alloc (rtrl, rtrl->max + RT_ALLOCREFS);
ref = &rtrl->refs[rtrl->cnt];
strncpy (ref->refname, name, MAP_W_NAMELEN);
rtrl->cnt++;
}
else {
ref = &rtrl->refs[ir];
}
/* add to ref the right way..
* first check if we have one big segment or if we enter the road again */
for (nr = -1, i = 0; i < ref->cnt; i++) {
/* check way, if found return to caller */
if (mw->id == ref->ways[i].way.id
&& mw->subid == ref->ways[i].way.sid) return;
/* check nodes */
for (n = 0; n < mw->n_cnt; n++) {
if (mw->n[n].d_id == ref->ways[i].way.id
&& mw->n[n].d_subid == ref->ways[i].way.sid)
nr = ref->ways[i].nr;
}
}
/* nothing found? */
if (nr < 0) {
float dist = -1.0;
float f;
/* find closest way to current position */
d_printf (" searching way to position. cnt:%d, ref:'%s'", ref->cnt, ref->refname);
for (nr = -1, i = 0; i < ref->cnt; i++) {
/* search shortest distance.. */
f = map_way_getdistance (mw, &ref->ways[i].pos, NULL);
if (dist < 0.0 || f < dist) {
dist = f;
nr = i;
}
}
/* add ways to the stacks...
* as long as recalc waydist < 1.5times dist
* and (mwleft || right) != &ref->ways[nr] */
if (nr >= 0) {
/* go to the left and to the right until 1.5time dist */
struct map_hash *mhtmp = NULL;
struct map_way *mwtmp = NULL;
struct rtwayid fromdir[2]; /* 0.. left, 1.. right */
struct map_way *mwdir[2] = {NULL, NULL};
// int pnrdir[2] = {-1, -1}; /* pnr: where we entered the way */
int dir;
d_printf (" found closest way nr:%d way:%lld:%d dist:%f",
nr, ref->ways[nr].way.id, ref->ways[nr].way.sid, dist);
/*
* start checking: prepare the variables mh/mw left and right
*/
fromdir[0].id = fromdir[1].id = ref->ways[nr].way.id;
fromdir[0].sid = fromdir[1].sid = ref->ways[nr].way.sid;
mhtmp = map_hash_get (ref->ways[nr].pos.lon, ref->ways[nr].pos.lat, MHLOAD_RELOAD);
mwtmp = map_way_find (mhtmp, ref->ways[nr].way.id, ref->ways[nr].way.sid);
if (mwtmp == NULL) {
d_printf ("%s:%d %s: could not find way: %lld:%d",
__FILE__, __LINE__, __FUNCTION__, ref->ways[nr].way.id,
ref->ways[nr].way.sid);
return;
}
for (i = 0; i < mwtmp->n_cnt; i++) {
struct map_hash *mhtmp1 = NULL;
struct map_way *mwtmp1 = NULL;
mhtmp1 = map_hash_get (mwtmp->n[i].d.lon, mwtmp->n[i].d.lat, MHLOAD_RELOAD);
mwtmp1 = map_way_find (mhtmp1, mwtmp->n[i].d_id, mwtmp->n[i].d_subid);
if (!mwtmp1) {
d_printf ("%s:%d %s: could not find way: %lld:%d pos:%f,%f",
__FILE__, __LINE__, __FUNCTION__, mwtmp->n[i].d_id, mwtmp->n[i].d_subid);
return;
}
d_printf ("check name[%s] == [%s]", name, mwtmp1->ref);
if (map_way_checkref (mwtmp1->ref, name)) {
/* found way */
if (mwdir[0] == NULL) {
mwdir[0]= mwtmp1;
// pnrdir[0] = mwtmp->n[i].d_pnr;
}
else if (mwdir[1] == NULL) {
mwdir[1]= mwtmp1;
// pnrdir[1] = mwtmp->n[i].d_pnr;
}
else {
d_printf ("%s:%d %s: why there are three connecting referneces? ignoring way:%lld:%d",
__FILE__, __LINE__, __FUNCTION__, mw->n[i].d_id, mw->n[i].d_subid);
}
}
}
while ((mwdir[0] != NULL || mwdir[1] != NULL) && _rtrl_wlist_cnt++ < RTRL_MAXWAYS) {
struct map_hash *mhtmp1 = NULL;
struct map_way *mwtmp1 = NULL;
for (dir = 0; dir < 2; dir++) if (mwdir[dir]) {
d_printf (" mwdir[%d]: %lld:%d fromdir:%lld:%d", dir, mwdir[dir]->id, mwdir[dir]->subid, fromdir[dir].id, fromdir[dir].sid);
for (i = 0; i < mwdir[dir]->n_cnt; i++) {
mhtmp1 = map_hash_get (mwdir[dir]->n[i].d.lon, mwdir[dir]->n[i].d.lat, MHLOAD_RELOAD);
mwtmp1 = map_way_find (mhtmp1, mwdir[dir]->n[i].d_id, mwdir[dir]->n[i].d_subid);
if (!mwtmp1) {
d_printf ("%s:%d %s: could not find way: %lld:%d pos:%f,%f",
__FILE__, __LINE__, __FUNCTION__, mwdir[dir]->n[i].d_id, mwdir[dir]->n[i].d_subid);
return;
}
if (map_way_checkref (mwtmp1->ref, name)
&& (mwtmp1->id != fromdir[dir].id || mwtmp1->subid != fromdir[dir].sid))
break;
}
if (i < mwdir[dir]->n_cnt) {
/* found next way.. */
if (map_way_getdistance (mwtmp1, &ref->ways[nr].pos, NULL) * 1.5 < dist) {
fromdir[dir].id = mwdir[dir]->id;
fromdir[dir].sid = mwdir[dir]->subid;
// pnrdir[dir] = mwdir[dir]->n[i].d_pnr;
mwdir[dir] = mwtmp1;
}
else mwdir[dir] = NULL;
}
else mwdir[dir] = NULL;
}
}
/* add to stack list */
for (dir = 0; dir < 2; dir++)
if (mwdir[dir]) rtrl_recalculate (r, mwdir[dir], fromdir[dir]);
}
else
d_printf (" found nothing..");
/* no connection way found... need to add new segment.. */
ref->lastnr++;
nr = ref->lastnr;
}
/* save data */
if (ref->cnt+1 >= ref->max) rtrefways_realloc (ref, ref->max+RT_ALLOCREFS);
ref->ways[ref->cnt].way.id = mw->id;
ref->ways[ref->cnt].way.sid = mw->subid;
ref->ways[ref->cnt].way.type = mw->type;
ref->ways[ref->cnt].pos = mw->p[0];
ref->ways[ref->cnt].nr = nr;
ref->cnt++;
d_printf (" cnt:%d lastnr:%d nr:%d", ref->cnt, ref->lastnr, nr);
};
void rtrl_addway (struct rtdata *r, struct rtreflist *rtrl, struct map_way *mw) {
int i, j;
char n[MAP_W_NAMELEN];
/* add name to refrence list
* adding streetnames is not a good idea, exist at different locations */
// if (strlen (mw->name) > 0) rtrl_addwayname (rtrl, mw, mw->name);
/* add references to reference list */
for (n[0] = 0, j = 0, i = 0; i < strlen (mw->ref); i++) {
if (j >= MAP_W_NAMELEN-1) {
d_printf ("%s:%d (%s) something went very wrong", __FILE__, __LINE__, __FUNCTION__);
break;
}
if (mw->ref[i] == ';') {
rtrl_addwayname (r, rtrl, mw, n);
j = 0;
n[0] = 0;
}
else if (mw->ref[i] != ' ' ) {
n[j++] = mw->ref[i];
n[j] = 0;
}
}
if (strlen (n) > 0) rtrl_addwayname (r, rtrl, mw, n);
};
void rtrefways_realloc (struct rtref *rtref, int cnt) {
int i;
if (cnt == 0) {
/* free data */
if (rtref->ways != NULL) ml_free (rtref->ways);
rtref->ways = NULL;
rtref->cnt = 0;
rtref->max = 0;
}
else if (cnt < rtref->max) return;
else {
rtref->ways = (struct rtrefway *) ml_realloc (rtref->ways, cnt * sizeof (struct rtrefway));
for (i = rtref->max-1; i >= cnt; i--) {
rtref->ways[i].way.id = 0;
rtref->ways[i].way.sid = 0;
rtref->ways[i].nr = -1;
rtref->ways[i].pos.lon = 0.0;
rtref->ways[i].pos.lat = 0.0;
}
rtref->max = cnt;
}
};
/****************************************************************************
* routing memory management
*/
void rtl_debug (struct rtwaylist *rtl) {
int i;
d_printf ("debug rtwaylist:");
d_printf ("================");
for (i = 0; i < rtl->cnt; i++) {
d_printf (" %-4d: %lld:%d", i, rtl->array[i].way.id, rtl->array[i].way.sid);
}
};
/*
* alloc or realloc data from memory
*/
struct rtdata *route_alloc (struct rtdata *rt, int max, int stmax) {
struct rtdata *r = rt;
d_printf ("route alloc: %p max:%d ig:%d sel:%d st:%d", rt, max, stmax);
/* new route */
if (r == NULL) {
d_printf ("route alloc: new alloc");
r = (struct rtdata *) ml_malloc (sizeof (struct rtdata));
r->pos.lon = 0.0;
r->pos.lat = 0.0;
r->cycle = 0;
r->ways.array = NULL;
r->ways.cnt = 0;
r->ways.max = 0;
r->stack1.array = NULL;
r->stack1.cnt = 0;
r->stack1.max = 0;
r->stack2.array = NULL;
r->stack2.cnt = 0;
r->stack2.max = 0;
r->stack3.array = NULL;
r->stack3.cnt = 0;
r->stack3.max = 0;
r->route.array = NULL;
r->route.cnt = 0;
r->route.max = 0;
r->state = ROUTING_NONE;
r->reflist.cnt = 0;
r->reflist.refs = NULL;
r->reflist.max = 0;
}
rtrlrefs_alloc (&r->reflist, RT_ALLOCREFS);
rtl_realloc (&r->ways, max);
rtl_realloc (&r->stack1, stmax);
rtl_realloc (&r->stack2, stmax);
rtl_realloc (&r->stack3, stmax);
r->stack1.array[0].priority = 0.0;
r->stack2.array[0].priority = 0.0;
r->stack3.array[0].priority = 0.0;
r->ways.array[0].priority = 0.0;
return r;
};
/****************************************************************************
* routing data functions
*/
/*
* find the given element inside the givven array */
int rtl_find (struct rtwaylist *rtl, struct rtwayid *id) {
int i;
for (i = rtl->cnt-1; i >= 0; i--) {
if (rtl->array[i].way.id == id->id && rtl->array[i].way.sid == id->sid)
break;
}
return i;
};
/*
* alloc or free memory
*/
int rtl_realloc (struct rtwaylist *rtl, int max) {
void *ptr;
d_printf ("realloc: %p, size:%d neu:%d", rtl->array, rtl->max, max);
ptr = ml_realloc (rtl->array, sizeof (struct rtway) * max);
if (ptr == NULL && max > 0) {
d_printf ("%s:%d (%s) ptr == %p", __FILE__, __LINE__, __FUNCTION__, ptr);
errorexit (-1);
}
rtl->array = ptr;
rtl->max = max;
if (rtl->max < rtl->cnt) rtl->cnt = max;
return 1;
};
/*
* first in and first out push.. element will be put on the first place
*/
int rtl_fifo_push (struct rtwaylist *stack, struct rtway *rtw) {
if ((stack->cnt+1) >= stack->max) rtl_realloc (stack, stack->max+RT_ALLOCSTACK);
memmove (&stack->array[1], &stack->array[0], sizeof (struct rtway) * stack->cnt);
stack->array[0] = *rtw;
stack->cnt++;
return 1;
};
/*
* first in and first out push.. element will be put on the first place
*/
int rtl_lifo_push (struct rtwaylist *stack, struct rtway *rtw) {
if ((stack->cnt+1) >= stack->max) rtl_realloc (stack, stack->max+RT_ALLOCSTACK);
stack->array[stack->cnt] = *rtw;
stack->cnt++;
return 1;
};
/*
* last element will be removed from list.
*/
int rtl_pop (struct rtwaylist *stack, struct rtway *rtw) {
if (stack->cnt > 0) {
stack->cnt--;
*rtw = stack->array[stack->cnt];
return 1;
}
d_printf ("ERROR: rtl_pop() cnt==0");
return 0;
};
/*
* add element to the list. In sorted order.
*/
int rtl_addsorted (struct rtwaylist *rtl, struct rtway *rtw) {
int i = 0;
if (!rtl) return 0;
// d_printf (" rtl_addsorted: %lld:%d", rtw->way.id, rtw->way.sid);
if ((rtl->cnt+1) >= rtl->max) rtl_realloc (rtl, rtl->max+RT_ALLOCSTACK);
i = 0;
for (i = 0; i < rtl->cnt; i++) {
if (rtl->array[i].priority < rtw->priority) break;
}
if (i < rtl->cnt)
memmove (&rtl->array[i+1], &rtl->array[i], sizeof (struct rtway) * (rtl->cnt-i));
rtl->array[i] = *rtw;
rtl->cnt++;
return 1;
};
/*
* delete entry from the list
*/
void rtl_delete (struct rtwaylist *rtl, int nr) {
if (nr < rtl->cnt-1 && nr >= 0 && rtl->cnt > 0) {
rtl->cnt--;
if (nr < rtl->cnt) memmove (&rtl->array[nr], &rtl->array[nr+1], sizeof (struct rtway) * (rtl->cnt-nr));
}
};
/****************************************************************************
* routing function itself
*/
/*
* free route data from memory
*/
void route_free (struct rtdata *rt) {
if (rt == NULL) return;
d_printf ("route free: %p", rt);
rtrlrefs_alloc (&rt->reflist, 0);
rtl_realloc (&rt->ways, 0);
rtl_realloc (&rt->stack1, 0);
rtl_realloc (&rt->stack2, 0);
rtl_realloc (&rt->stack3, 0);
rtl_realloc (&rt->route, 0);
ml_free (rt);
};
int route_calc_way (struct rtdata *r, struct rtway *rtw, struct map_way *mw, struct rtway *rtwnew, struct map_way *mwnew) {
float deltadist = 0.0;
fPoint v1, v2, v3, v4;
float f, ideal = 0.0;
int i, step, deltastep;
r->cycle++;
rtwnew->cycle1 = r->cycle;
rtwnew->cycle2 = -1;
/*
* check the distance to the ideal line, also look for the closest distacne
*/
v1.x = r->pos_dest.lon;
v1.y = r->pos_dest.lat;
for (i = 0; i < mwnew->n_cnt; i++) {
v2.x = map_lon2km (r->dest.lon - mwnew->p[mwnew->n[i].pnr].lon, mwnew->p[mwnew->n[i].pnr].lat);
v2.y = map_lat2km (r->dest.lat - mwnew->p[mwnew->n[i].pnr].lat);
v3 = vec_normale (v1, v2);
/* check if the normale vector is on the v1 vector,
* if not we just get the distance */
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);
}
else f = sqrt (v2.x * v2.x + v2.y * v2.y);
/* check ideal */
if (f < ideal || i == 0) ideal = f;
ideal = fabsf (ideal);
f = map_getdistance (mwnew->p[mwnew->n[i].pnr], r->pos);
/* check distance */
if (f < r->closest_distance || r->closest_distance < 0.0) { // for debugging and status information
r->closest_distance = f;
r->closest_lon = mwnew->p[0].lon;
r->closest_lat = mwnew->p[0].lat;
}
if (f < rtwnew->dist2pos || i == 0) {
rtwnew->dist2pos = f;
}
}
/*
* calculate the delta between distance on this way since start (deltaway)
* calculate delta step and calculate delta distance to dest. (deltadist)
*/
deltadist = rtwnew->dist2pos - rtw->dist2pos;
rtwnew->deltadist = deltadist;
deltastep = rtwnew->step - rtw->step;
if (deltastep) {
step = rtw->step;
}
else {
step = rtw->step-1;
}
/*
* calclating priority
*/
// until now the best way to calculate..
// rtwnew->priority = (powf (1.0 + 4.0 * ideal, 2.0) + powf (1.0 + rtwnew->dist2pos, 2.0) + rtwnew->dist2dest) * (1.0 + step/5.0);
rtwnew->priority = (powf (1.0 + 4.0 * ideal, 2.0) + powf (1.0 + rtwnew->dist2pos, 2.0) + rtwnew->dist2dest) * (1.0 + step/5.0);
if (isinf(rtwnew->priority) || isnan(rtwnew->priority)) {
rtwnew->priority = fabsf (rtw->priority)*0.9;
}
/*
* only a way no connection nodes.. check this route right away
*/
if ((mwnew->n_cnt <= 2 && mwnew->p_cnt < 2) || deltadist == 0.0 || (mwnew->flags & MWAY_F_junction)) {
rtwnew->priority = 0.0;
}
return 1;
};
int route_checkway (struct rtdata *r, struct rtway *rtw) {
struct map_hash *mh = map_hash_get (rtw->pos.lon, rtw->pos.lat, MHLOAD_RELOAD);
struct map_way *mw = map_way_find (mh, rtw->way.id, rtw->way.sid);
struct map_hash *mhto;
struct map_way *mwto;
struct rtway rtwnew, *rtwtmp;
struct rtwaylist *rtltmp;
int i, j, nr, w;
// d_printf ("route_checkway: rtw:%lld:%d step:%d dist2pos:%g priority:%g", rtw->way.id, rtw->way.sid, rtw->step, rtw->dist2pos, rtw->priority);
r->cycle++;
rtw->cycle2 = r->cycle;
if (mw == NULL) {
d_printf ("%s: could not find the way with id: %lld:%d at %f,%f", __FUNCTION__, rtw->way.id, rtw->way.sid, rtw->pos.lon, rtw->pos.lat);
return 0;
}
/* ignore the following ways */
if (mw->type == MWAY_railway) return 0;
if (mw->type == MWAY_unknown_railway) return 0;
if (mw->type == MWAY_cycleway) return 0;
if (mw->type == MWAY_footway) return 0;
if (mw->type == MWAY_service) return 0;
// d_printf (" way %lld:%d pnr:%d name:%s ref:%s flags:%d", mw->id, mw->subid, rtw->pnr, mw->name, mw->ref, mw->flags);
/* check all dnodes and add them to the list if they aren't already inside */
if ((mw->flags & (MWAY_F_oneway + MWAY_F_roundabout)) == MWAY_F_oneway) {
nr = rtw->pnr;
}
else {
nr = 0;
}
/* add to reference list */
rtrl_addway (r, &r->reflist, mw);
for (i = 0; i < mw->n_cnt; i++) if (mw->id != mw->n[i].d_id || mw->subid != mw->n[i].d_subid) {
rtwnew.way.id = mw->n[i].d_id;
rtwnew.way.sid = mw->n[i].d_subid;
rtwnew.pos = mw->n[i].d;
rtwnew.pnr = mw->n[i].d_pnr;
mhto = map_hash_get (rtwnew.pos.lon, rtwnew.pos.lat, MHLOAD_RELOAD);
mwto = map_way_find (mhto, rtwnew.way.id, rtwnew.way.sid);
if (mwto == NULL) {
continue;
}
if (((mw->flags & (MWAY_F_oneway+MWAY_F_roundabout)) == MWAY_F_oneway) &&
((mw->id != mwto->id && mw->n[i].pnr > nr) || (mw->id == mwto->id && mw->subid < mwto->subid))) {
continue; // prevent us from going backwards on oneways
}
if (mwto->id == mw->id && mwto->subid == mw->subid) {
continue;
}
/* increase step */
j = map_way_checkref (mwto->ref, mw->ref);
if (rtwnew.way.id == rtw->way.id || j > 0)
rtwnew.step = rtw->step;
else
rtwnew.step = rtw->step + 1;
/* calc distance */
rtwnew.dist2dest = rtw->dist2dest;
rtwnew.dist2dest += map_way_getlenght (mw, rtw->pnr, mw->n[i].pnr);
route_calc_way (r, rtw, mw, &rtwnew, mwto);
rtwtmp = NULL;
rtltmp = NULL;
if ((w = rtl_find(&r->ways, &rtwnew.way)) >= 0) {
rtwtmp = &r->ways.array[w];
rtltmp = &r->ways;
}
else {
rtltmp = route_getstack(r, mwto->type);
if ((w = rtl_find(rtltmp, &rtwnew.way)) >= 0)
rtwtmp = &rtltmp->array[w];
}
/* make sure we are not rechecking ways with only one p_cnt
* (will happen if the last point is inside a different hash) */
if (rtwtmp && rtltmp == &r->ways && mw->p_cnt == 1) continue;
if (rtwtmp) {
/* we found the element */
// d_printf (" add_sorted overwrite: %lld:%d", rtwtmp->way.id, rtwtmp->way.sid);
// d_printf (" dist2dest:rtwtmp:%f > rtwnew:%f (%d) prio:rtwtmp:%f > rtwnew:%f (%d)",
// rtwtmp->dist2dest, rtwnew.dist2dest, rtwtmp->priority, rtwnew.priority,
// rtwtmp->dist2dest > rtwnew.dist2dest , rtwtmp->priority > rtwnew.priority);
// d_printf (" step:rtwtmp:%d , rtwnew:%d rtw->pnr:%d mw->n_cnt:%d p_cnt:%d", rtwtmp->step,
// rtwnew.step, rtw->pnr, mw->n_cnt, mw->p_cnt);
if (rtwtmp->step < rtwnew.step) rtwnew.step = rtwtmp->step;
if (rtwtmp->dist2dest > rtwnew.dist2dest || rtwtmp->priority > rtwnew.priority) {
rtl_delete (rtltmp, w); // delete old data
rtl_addsorted (route_getstack(r, mwto->type), &rtwnew);
}
}
else
rtl_addsorted (route_getstack(r, mwto->type), &rtwnew);
}
if (mw->id == r->pos_way_id && mw->subid == r->pos_way_sid) {
d_printf ("** routing: way found go to the next step");
rtl_lifo_push (&r->route, rtw);
rtl_debug (&r->route);
r->state = ROUTING_OPTIMIZE;
}
return 1;
};
/*
* read data from stack
*/
#define FACTOR_HI 2.0
#define FACTOR_IA 3.0
#define FACTOR_HA FACTOR_HI*FACTOR_IA
int route_popstack (struct rtdata *r, struct rtway *rtw) {
int selstack = 0;
int i;
int res = 0;
/* check if we have some need to calculate routes.. i.e. just ways without connections */
if (r->stack1.cnt > 0 && r->stack1.array[route->stack1.cnt-1].priority == 0.0)
res = rtl_pop (&r->stack1, rtw);
else if (r->stack2.cnt > 0 && r->stack2.array[route->stack2.cnt-1].priority == 0.0)
res = rtl_pop (&r->stack2, rtw);
else if (r->stack3.cnt > 0 && r->stack3.array[route->stack3.cnt-1].priority == 0.0)
res = rtl_pop (&r->stack3, rtw);
/* check if we are on the way inside the stack */
if (res <= 0)
for (i = 0; i < r->stack1.cnt; i++) if (r->stack1.array[i].way.id == r->pos_way_id && r->stack1.array[i].way.sid == r->pos_way_sid) {
*rtw = r->stack1.array[i];
res = 1;
rtl_delete (&r->stack1, i);
break;
}
if (res <= 0)
for (i = 0; i < r->stack2.cnt; i++) if (r->stack2.array[i].way.id == r->pos_way_id && r->stack2.array[i].way.sid == r->pos_way_sid) {
*rtw = r->stack2.array[i];
res = 1;
rtl_delete (&r->stack2, i);
break;
}
if (res <= 0)
for (i = 0; i < r->stack3.cnt; i++) if (r->stack3.array[i].way.id == r->pos_way_id && r->stack3.array[i].way.sid == r->pos_way_sid) {
*rtw = r->stack3.array[i];
res = 1;
rtl_delete (&r->stack3, i);
break;
}
if (res <= 0) {
if (r->stack1.cnt > 0) {
selstack = 1;
if (r->stack2.cnt > 0 && r->stack1.array[route->stack1.cnt-1].priority/FACTOR_HI > r->stack2.array[route->stack2.cnt-1].priority) selstack = 2;
if (r->stack3.cnt > 0 && r->stack2.cnt == 0 && r->stack1.array[route->stack1.cnt-1].priority/FACTOR_HA > r->stack3.array[route->stack3.cnt-1].priority) selstack = 3;
if (r->stack3.cnt > 0 && r->stack2.cnt > 0 && r->stack2.array[route->stack2.cnt-1].priority/FACTOR_IA > r->stack3.array[route->stack3.cnt-1].priority) selstack = 3;
}
if (selstack == 0 && r->stack2.cnt > 0) {
selstack = 2;
if (r->stack3.cnt > 0 && r->stack2.array[route->stack2.cnt-1].priority/2.0 > r->stack3.array[route->stack3.cnt-1].priority) selstack = 3;
}
if (selstack == 0 && r->stack3.cnt > 0) {
selstack = 3;
}
if (selstack == 1)
res = rtl_pop (&r->stack1, rtw);
else if (selstack == 2)
res = rtl_pop (&r->stack2, rtw);
else if (selstack == 3)
res = rtl_pop (&r->stack3, rtw);
}
// if (res >= 0) {
// d_printf ("route_popstack: %lld:%d", rtw->way.id, rtw->way.sid);
// }
return res;
};
/*
* calculating the route..
* 1. reading unchecked ways from stack1 (motor/highway), stack2 (tertiary)
* or stack3 (residential/unknown ways).
* 2. check these ways and add connected unknown ways to the stack
*/
void route_calc (struct rtdata *r) {
struct rtway rtw;
int res;
if ((res = route_popstack (r, &rtw)) == 1) {
d_printf ("route_calc: rtw: %lld:%d", rtw.way.id, rtw.way.sid);
if (route_checkway (r, &rtw))
rtl_fifo_push (&r->ways, &rtw);
}
else if (r->state == ROUTING_INIT) {
d_printf ("route_calc: processing from destination way");
rtw.way.id = r->dest_way_id;
rtw.way.sid = r->dest_way_sid;
rtw.way.type = MWAY_unknown;
r->start_way_id = drawgps_on_way_id;
r->start_way_sid = drawgps_on_way_sid;
rtw.pos = r->dest;
rtw.step = 0;
rtw.pnr = 0;
d_printf ("route_calc %s:%d FIX ME: nr must be set to the closest node.. ", __FILE__, __LINE__);
if (drawgps_on_way_id != 0) {
rtl_lifo_push (&r->stack3, &rtw);
r->state = ROUTING_CALC;
}
}
else {
d_printf ("something went very wrong. cannot find ways which are connected to this area. (state:%d res:%d)", r->state, res);
r->state = ROUTING_ABORT;
}
};
/* return pointer to current way stack, depending on the type of way */
struct rtwaylist *route_getstack (struct rtdata *r, int waytype) {
if (!r) return NULL;
if (waytype == MWAY_highway) return &r->stack1;
if (waytype == MWAY_tertiary) return &r->stack2;
else return &r->stack3;
};
void route_onway (struct rtdata *r) {
static int _once = 1;
if (_once) d_printf ("route_onway: we are on the way... ");
_once = 0;
};
/* optimize the route and create the route from current position */
void route_optimize (struct rtdata *r) {
if (r->state == ROUTING_OPTIMIZE) r->state = ROUTING_ONWAY;
};
int route_optway (struct rtdata *r, struct rtway *rtw) {
return 0;
};
/***************************************************************************
* start stop and loop functions for routing
*/
void route_start (struct map_pos dest) {
static int _called = 0;
struct map_pos pos;
struct gps_data *gpsdata;
int i, calcto;
if (_called) return;
_called = 1;
if (route != NULL) {
route_free (route);
route = NULL;
}
route = route_alloc (NULL, RT_ALLOC, RT_ALLOCSTACK);
d_printf ("*********************************************");
d_printf ("route_start to: %f,%f", dest.lon, dest.lat);
route->state = ROUTING_NONE;
route->dest = dest;
/* find closest way */
i = map_way_findclosest (&route->dest, &route->dest_way_pos, 0, route->dest_way_name, MAP_W_NAMELEN, &route->dest_way_id, &route->dest_way_sid);
d_printf (" wayid: %lld:%d", route->dest_way_id, route->dest_way_sid);
d_printf (" name: %s", route->dest_way_name);
route->closest_distance = -1.0;
route->closest_lon = dest.lon;
route->closest_lat = dest.lat;
route->state = ROUTING_INIT;
do {
if (gps_isrunning ()) {
gpsdata = gps_getposition ();
if (gpsdata != NULL && gpsdata->valid) {
pos.lon = gpsdata->lon;
pos.lat = gpsdata->lat;
route->pos_dest.lon = map_lon2km (route->dest.lon - pos.lon, pos.lat);
route->pos_dest.lat = map_lat2km (route->dest.lat - pos.lat);
route->pos_way_id = drawgps_on_way_id;
route->pos_way_sid = drawgps_on_way_sid;
route->pos = pos;
route->distance = sqrtf (route->pos_dest.lon * route->pos_dest.lon + route->pos_dest.lat * route->pos_dest.lat);
route->angle = (180.0 / M_PI) * atan2f(route->pos_dest.lon, route->pos_dest.lat);
if (route->angle < 0.0) route->angle += 360.0;
if (route->angle > 360.0) route->angle -= 360.0;
if (route->state == ROUTING_INIT) {
route->start = pos;
route_calc (route);
route->state = ROUTING_CALC;
}
else if (route->state == ROUTING_INIT)
for (i = 0; i < route->ways.cnt; i++) if (route->ways.array[i].way.id == route->pos_way_id && route->ways.array[i].way.sid == route->pos_way_sid) {
d_printf ("** routing: pos found inside ways go to the next step");
route->state = ROUTING_OPTIMIZE;
}
}
if (route->state == ROUTING_CALC) {
route_calc (route);
}
else if (route->state == ROUTING_OPTIMIZE) {
route_optimize (route);
}
else if (route->state == ROUTING_ONWAY) {
route_onway (route);
}
}
if (app.status == APPSTATUS_quit) route_stop ();
if (route->state == ROUTING_CALC) {
if ((++calcto) % 200 == 0) main_wnd_update ();
}
else
main_wnd_update ();
} while (route && route->state != ROUTING_NONE && route->state != ROUTING_ABORT);
d_printf ("*********************************************");
d_printf ("route_start: finished or aborted");
_called = 0;
return;
};
void route_stop () {
d_printf ("*********************************************");
d_printf ("route_stop");
if (route) route->state = ROUTING_ABORT;
};
Reference in new issue View Git Blame Copy Permalink