Ticket #1279: routing2.diff

File routing2.diff, 14.9 KB (added by jandegr, 6 years ago)
  • route.c

     
    311311static void route_path_update(struct route *this, int cancel, int async);
    312312static int route_time_seg(struct vehicleprofile *profile, struct route_segment_data *over, struct route_traffic_distortion *dist);
    313313static void route_graph_flood(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb);
     314static void route_graph_flood_frugal(struct route_graph *this, struct route_info *dst, struct route_info *pos, struct vehicleprofile *profile, struct callback *cb);
    314315static void route_graph_reset(struct route_graph *this);
    315316
    316317
     
    807808                        this->link_path=1;
    808809                        this->current_dst=prev_dst;
    809810                        route_graph_reset(this->graph);
    810                         route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb);
     811/*                      route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb); */
     812                        route_graph_flood_frugal(this->graph, this->current_dst, this->pos, this->vehicleprofile, this->route_graph_flood_done_cb);
    811813                        return;
    812814                }
    813815                if (!new_graph && this->path2->updated)
     
    13311333                this->reached_destinations_count++;
    13321334                route_graph_reset(this->graph);
    13331335                this->current_dst = this->destinations->data;
    1334                 route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb);
     1336/*              route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, this->route_graph_flood_done_cb); */
     1337                route_graph_flood_frugal(this->graph, this->current_dst, this->pos, this->vehicleprofile,       this->route_graph_flood_done_cb);
    13351338        }
    13361339}
    13371340
     
    19161919 * @brief Returns the estimated speed on a segment
    19171920 *
    19181921 * This function returns the estimated speed to be driven on a segment, 0=not passable
     1922 * Uses route_weight, roundabout_weight and link_weight from the roadprofile in navit.xml to
     1923 * modify the speed or OSM maxspeed
    19191924 *
     1925 *
    19201926 * @param profile The routing preferences
    19211927 * @param over The segment which is passed
    19221928 * @param dist A traffic distortion if applicable
     
    19271933{
    19281934        struct roadprofile *roadprofile=vehicleprofile_get_roadprofile(profile, over->item.type);
    19291935        int speed,maxspeed;
    1930         if (!roadprofile || !roadprofile->route_weight)
    1931                 return 0;
    1932         /* maxspeed_handling: 0=always, 1 only if maxspeed restricts the speed, 2 never */
    1933         speed=roadprofile->route_weight;
    1934         if (profile->maxspeed_handling != 2) {
    1935                 if (over->flags & AF_SPEED_LIMIT) {
     1936        int route_averaging = 100;
     1937        if (!roadprofile || !roadprofile->speed)
     1938                        return 0;
     1939        /* maxspeed_handling: 0=always, 1 only if maxspeed restricts the speed,
     1940         *  2 never
     1941         *  */
     1942
     1943
     1944        if (roadprofile->route_weight)
     1945                route_averaging =roadprofile->route_weight;
     1946
     1947        if ((over->flags & AF_ROUNDABOUT) && roadprofile->roundabout_weight)
     1948                route_averaging = (route_averaging * roadprofile->roundabout_weight)/100;
     1949        else
     1950                if ((over->flags & AF_LINK) && roadprofile->link_weight)
     1951                        {
     1952                        route_averaging = (route_averaging * roadprofile->link_weight)/100;
     1953                /*      dbg (0,"link weight=%i\n",roadprofile->link_weight); */
     1954                        }
     1955        speed=(roadprofile->speed * route_averaging)/100 ;
     1956        if (profile->maxspeed_handling != 2)
     1957        {
     1958                if (over->flags & AF_SPEED_LIMIT)
     1959                {
    19361960                        maxspeed=RSD_MAXSPEED(over);
    1937                         if (!profile->maxspeed_handling)
    1938                                 speed=maxspeed;
    1939                 } else
     1961                        if (!profile->maxspeed_handling) /*always handle maxspeed*/
     1962                                maxspeed=(maxspeed * route_averaging)/100 ;
     1963                }
     1964                else
    19401965                        maxspeed=INT_MAX;
     1966
    19411967                if (dist && maxspeed > dist->maxspeed)
    19421968                        maxspeed=dist->maxspeed;
    1943                 if (maxspeed != INT_MAX && (profile->maxspeed_handling != 1 || maxspeed < speed))
     1969
     1970                /*below handling 0=always or 1=restricting */
     1971                if (maxspeed != INT_MAX && (profile->maxspeed_handling == 0 || maxspeed < speed))
    19441972                        speed=maxspeed;
    19451973        }
    1946         if (over->flags & AF_DANGEROUS_GOODS) {
     1974        else /* handling=2, don't use maxspeed*/
     1975                speed=(roadprofile->speed * route_averaging)/100 ;
     1976        if (over->flags & AF_DANGEROUS_GOODS)
     1977        {
    19471978                if (profile->dangerous_goods & RSD_DANGEROUS_GOODS(over))
    19481979                        return 0;
    19491980        }
     
    20492080                route_get_traffic_distortion(over, &dist) && dir != 2 && dir != -2) {
    20502081                        distp=&dist;
    20512082        }
    2052         ret=route_time_seg(profile, &over->data, distp);
     2083        if (profile->mode != 3)/*not new shortest*/
     2084                ret=route_time_seg(profile, &over->data, distp);
     2085        else ret = over->data.len; /*new shortest mode*/
     2086
    20532087        if (ret == INT_MAX)
    20542088                return ret;
    20552089        if (!route_through_traffic_allowed(profile, over) && from && route_through_traffic_allowed(profile, from->seg))
     
    23272361 *
    23282362 * This function uses Dijkstra's algorithm to do the routing. To understand it you should have a look
    23292363 * at this algorithm.
     2364 *
     2365 * This version uses the cost of an already found path as upper limit for further exploration
     2366 *
     2367 *
     2368 *
    23302369 */
    23312370static void
     2371route_graph_flood_frugal(struct route_graph *this, struct route_info *dst, struct route_info *pos, struct vehicleprofile *profile, struct callback *cb)
     2372{
     2373        struct route_graph_point *p_min;
     2374        struct route_graph_segment *s=NULL;
     2375        struct route_graph_segment *pos_segment=NULL;
     2376        int min,new,val;
     2377        struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs */
     2378        int edges_count=0;
     2379        int max_cost= INT_MAX;
     2380
     2381        dbg(0,"starting route_graph_flood_frugal\n");
     2382
     2383        pos_segment=route_graph_get_segment(this, pos->street, pos_segment);
     2384
     2385        heap = fh_makekeyheap();
     2386
     2387        while ((s=route_graph_get_segment(this, dst->street, s)))
     2388        {
     2389                val=route_value_seg(profile, NULL, s, -1);
     2390                if (val != INT_MAX)
     2391                {
     2392                        val=val*(100-dst->percent)/100;
     2393                        s->end->seg=s;
     2394                        s->end->value=val;
     2395                        s->end->el=fh_insertkey(heap, s->end->value, s->end);
     2396                }
     2397                val=route_value_seg(profile, NULL, s, 1);
     2398                if (val != INT_MAX)
     2399                {
     2400                        val=val*dst->percent/100;
     2401                        s->start->seg=s;
     2402                        s->start->value=val;
     2403                        s->start->el=fh_insertkey(heap, s->start->value, s->start);
     2404                }
     2405        }
     2406        for (;;)
     2407        {
     2408                p_min=fh_extractmin(heap); /* Starting Dijkstra by selecting the point with the minimum costs on the heap */
     2409                if (! p_min) /* There are no more points with temporarily calculated costs, Dijkstra has finished */
     2410                        break;
     2411                min=p_min->value;
     2412                dbg(lvl_debug,"extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
     2413                p_min->el=NULL; /* This point is permanently calculated now, we've taken it out of the heap */
     2414                s=p_min->start;
     2415                while (s)
     2416                { /* Iterating all the segments leading away from our point to update the points at their ends */
     2417                        edges_count ++;
     2418                        val=route_value_seg(profile, p_min, s, -1);
     2419                        if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item))
     2420                        {
     2421                                if (profile->turn_around_penalty2)
     2422                                        val+=profile->turn_around_penalty2;
     2423                                else
     2424                                        val=INT_MAX;
     2425                        }
     2426                        if (val != INT_MAX)
     2427                        {
     2428                                new=min+val;
     2429                                if (new <= max_cost) /*check if it is worth exploring */
     2430                                {
     2431                                        dbg(lvl_debug,"begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
     2432                                        if (new < s->end->value)
     2433                                        { /* We've found a less costly way to reach the end of s, update it */
     2434                                                s->end->value=new;
     2435                                                s->end->seg=s;
     2436                                                if (! s->end->el)
     2437                                                {
     2438                                                        dbg(lvl_debug,"insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
     2439                                                        s->end->el=fh_insertkey(heap, new, s->end);
     2440                                                }
     2441                                                else
     2442                                                {
     2443                                                        dbg(lvl_debug,"replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
     2444                                                        fh_replacekey(heap, s->end->el, new);
     2445                                                }
     2446                                        }
     2447                                        if (item_is_equal(pos_segment->data.item,s->data.item))
     2448                                        {
     2449                                                max_cost=new;
     2450                                                dbg(lvl_debug,"new shortest path cost = %i\n",new)
     2451                                        }
     2452                                }
     2453
     2454                        }
     2455                        s=s->start_next;
     2456                }
     2457
     2458
     2459                s=p_min->end;
     2460                while (s)
     2461                { /* Doing the same as above with the segments leading towards our point */
     2462                        edges_count ++;
     2463                        val=route_value_seg(profile, p_min, s, 1);
     2464                        if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item))
     2465                        {
     2466                                if (profile->turn_around_penalty2)
     2467                                        val+=profile->turn_around_penalty2;
     2468                                else
     2469                                        val=INT_MAX;
     2470                        }
     2471                        if (val != INT_MAX)
     2472                        {
     2473                                new=min+val;
     2474                                if (new <= max_cost)
     2475                                {
     2476                                        dbg(lvl_debug,"end %d len %d vs %d (0x%x,0x%x)\n",new,val,s->start->value,s->start->c.x, s->start->c.y);
     2477                                        if (new < s->start->value)
     2478                                        {
     2479                                                s->start->value=new;
     2480                                                s->start->seg=s;
     2481                                                if (! s->start->el)
     2482                                                {
     2483                                                        dbg(lvl_debug,"insert_start p=%p el=%p val=%d ", s->start, s->start->el, s->start->value);
     2484                                                        s->start->el=fh_insertkey(heap, new, s->start);
     2485                                                }
     2486                                                else
     2487                                                {
     2488                                                        dbg(lvl_debug,"replace_start p=%p el=%p val=%d\n", s->start, s->start->el, s->start->value);
     2489                                                        fh_replacekey(heap, s->start->el, new);
     2490                                                }
     2491                                        }
     2492                                        if (item_is_equal(pos_segment->data.item,s->data.item))
     2493                                        {
     2494                                                max_cost=new;
     2495                                                dbg(lvl_debug,"new shortest path cost = %i\n",new)
     2496                                        }
     2497                                }
     2498                        }
     2499                        s=s->end_next;
     2500                }
     2501        }
     2502        dbg(lvl_debug,"number of edges visited =%i\n",edges_count);
     2503        fh_deleteheap(heap);
     2504        callback_call_0(cb);
     2505        dbg(lvl_debug,"return\n");
     2506}
     2507
     2508
     2509/**
     2510 * @brief Calculates the routing costs for each point
     2511 *
     2512 * This function is the heart of routing. It assigns each point in the route graph a
     2513 * cost at which one can reach the destination from this point on. Additionally it assigns
     2514 * each point a segment one should follow from this point on to reach the destination at the
     2515 * stated costs.
     2516 *
     2517 * This function uses Dijkstra's algorithm to do the routing. To understand it you should have a look
     2518 * at this algorithm.
     2519 *
     2520 * This version calculates everything, even if the cost is higher than and already found path
     2521 *
     2522 */
     2523static void
    23322524route_graph_flood(struct route_graph *this, struct route_info *dst, struct vehicleprofile *profile, struct callback *cb)
    23332525{
    23342526        struct route_graph_point *p_min;
     
    23352527        struct route_graph_segment *s=NULL;
    23362528        int min,new,val;
    23372529        struct fibheap *heap; /* This heap will hold all points with "temporarily" calculated costs */
     2530        int edges_count=0;
    23382531
    23392532        heap = fh_makekeyheap();   
    23402533
    2341         while ((s=route_graph_get_segment(this, dst->street, s))) {
     2534        while ((s=route_graph_get_segment(this, dst->street, s)))
     2535        {
     2536
    23422537                val=route_value_seg(profile, NULL, s, -1);
    2343                 if (val != INT_MAX) {
     2538                if (val != INT_MAX)
     2539                {
    23442540                        val=val*(100-dst->percent)/100;
    23452541                        s->end->seg=s;
    23462542                        s->end->value=val;
     
    23472543                        s->end->el=fh_insertkey(heap, s->end->value, s->end);
    23482544                }
    23492545                val=route_value_seg(profile, NULL, s, 1);
    2350                 if (val != INT_MAX) {
     2546                if (val != INT_MAX)
     2547                {
    23512548                        val=val*dst->percent/100;
    23522549                        s->start->seg=s;
    23532550                        s->start->value=val;
     
    23632560                        printf("extract p=%p free el=%p min=%d, 0x%x, 0x%x\n", p_min, p_min->el, min, p_min->c.x, p_min->c.y);
    23642561                p_min->el=NULL; /* This point is permanently calculated now, we've taken it out of the heap */
    23652562                s=p_min->start;
    2366                 while (s) { /* Iterating all the segments leading away from our point to update the points at their ends */
     2563                while (s)
     2564                { /* Iterating all the segments leading away from our point to update the points at their ends */
     2565                        edges_count ++;
    23672566                        val=route_value_seg(profile, p_min, s, -1);
    2368                         if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
     2567                        if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item))
     2568                        {
    23692569                                if (profile->turn_around_penalty2)
    23702570                                        val+=profile->turn_around_penalty2;
    23712571                                else
    23722572                                        val=INT_MAX;
    23732573                        }
    2374                         if (val != INT_MAX) {
     2574                        if (val != INT_MAX)
     2575                        {
    23752576                                new=min+val;
    23762577                                if (debug_route)
    23772578                                        printf("begin %d len %d vs %d (0x%x,0x%x)\n",new,val,s->end->value, s->end->c.x, s->end->c.y);
     
    23782579                                if (new < s->end->value) { /* We've found a less costly way to reach the end of s, update it */
    23792580                                        s->end->value=new;
    23802581                                        s->end->seg=s;
    2381                                         if (! s->end->el) {
     2582                                        if (! s->end->el)
     2583                                        {
    23822584                                                if (debug_route)
    23832585                                                        printf("insert_end p=%p el=%p val=%d ", s->end, s->end->el, s->end->value);
    23842586                                                s->end->el=fh_insertkey(heap, new, s->end);
     
    23852587                                                if (debug_route)
    23862588                                                        printf("el new=%p\n", s->end->el);
    23872589                                        }
    2388                                         else {
     2590                                        else
     2591                                        {
    23892592                                                if (debug_route)
    23902593                                                        printf("replace_end p=%p el=%p val=%d\n", s->end, s->end->el, s->end->value);
    23912594                                                fh_replacekey(heap, s->end->el, new);
     
    23962599                        }
    23972600                        s=s->start_next;
    23982601                }
     2602
    23992603                s=p_min->end;
    24002604                while (s) { /* Doing the same as above with the segments leading towards our point */
     2605                        edges_count ++;
    24012606                        val=route_value_seg(profile, p_min, s, 1);
    24022607                        if (val != INT_MAX && item_is_equal(s->data.item,p_min->seg->data.item)) {
    24032608                                if (profile->turn_around_penalty2)
     
    24312636                        s=s->end_next;
    24322637                }
    24332638        }
     2639        dbg(0,"number of edges visited =%i\n",edges_count);
    24342640        fh_deleteheap(heap);
    24352641        callback_call_0(cb);
    24362642        dbg(lvl_debug,"return\n");
     
    25502756
    25512757        if (profile->mode == 2 || (profile->mode == 0 && pos->lenextra + dst->lenextra > transform_distance(map_projection(pos->street->item.map), &pos->c, &dst->c)))
    25522758                return route_path_new_offroad(this, pos, dst);
     2759        if (profile->mode != 3) /*not shortest on-road*/{
    25532760        while ((s=route_graph_get_segment(this, pos->street, s))) {
    25542761                val=route_value_seg(profile, NULL, s, 2);
    25552762                if (val != INT_MAX && s->end->value != INT_MAX) {
     
    25802787                        }
    25812788                }
    25822789        }
     2790        }
     2791        else /*experimental shortest route*/
     2792        {
     2793                dbg(lvl_debug,"experimental shortest route calc\n")
     2794                while ((s=route_graph_get_segment(this, pos->street, s)))
     2795                {
     2796                        val = s->data.len;
     2797                        if (val != INT_MAX && s->end->value != INT_MAX)
     2798                        {
     2799                                val=val*(100-pos->percent)/100;
     2800                                dbg(lvl_debug,"val1 %d\n",val);
     2801                                val1_new=s->end->value+val;
     2802                                dbg(lvl_debug,"val1 +%d=%d\n",s->end->value,val1_new);
     2803                                if (val1_new < val1)
     2804                                {
     2805                                        val1=val1_new;
     2806                                        s1=s;
     2807                                }
     2808                        }
     2809                        dbg(lvl_debug,"val=%i\n",val);
     2810                        val = s->data.len;
     2811                        if (val != INT_MAX && s->start->value != INT_MAX)
     2812                        {
     2813                                val=val*pos->percent/100;
     2814                                dbg(lvl_debug,"val2 %d\n",val);
     2815                                val2_new=s->start->value+val;
     2816                                dbg(lvl_debug,"val2 +%d=%d\n",s->start->value,val2_new);
     2817                                if (val2_new < val2)
     2818                                {
     2819                                        val2=val2_new;
     2820                                        s2=s;
     2821                                }
     2822                        }
     2823                }
     2824        }
     2825
     2826
    25832827        if (val1 == INT_MAX && val2 == INT_MAX) {
    25842828                dbg(lvl_error,"no route found, pos blocked\n");
    25852829                return NULL;
     
    26052849                        this->avoid_seg=s;
    26062850                        route_graph_set_traffic_distortion(this, this->avoid_seg, profile->turn_around_penalty);
    26072851                        route_graph_reset(this);
    2608                         route_graph_flood(this, dst, profile, NULL);
     2852        /*              route_graph_flood(this, dst, profile, NULL);*/
     2853                        route_graph_flood_frugal(this, dst, pos, profile, NULL);
    26092854                        return route_path_new(this, oldpath, pos, dst, profile);
    26102855                }
    26112856        }
     
    29003145static void
    29013146route_graph_update_done(struct route *this, struct callback *cb)
    29023147{
    2903         route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, cb);
     3148/*      route_graph_flood(this->graph, this->current_dst, this->vehicleprofile, cb); */
     3149        route_graph_flood_frugal(this->graph, this->current_dst, this->pos, this->vehicleprofile, cb);
    29043150}
    29053151
    29063152/**