Ticket #687: navit_position_projection.diff

File navit_position_projection.diff, 5.8 KB (added by tegzed, 11 years ago)

fixed road segment selection

  • attr_def.h

     
    210210ATTR(search_active)
    211211ATTR(unsuspend)
    212212ATTR(announce_on)
     213ATTR(position_projection)
    213214ATTR2(0x0002ffff,type_int_end)
    214215ATTR2(0x00030000,type_string_begin)
    215216ATTR(type)
  • navit.c

     
    142142        int flags;
    143143                 /* 1=No graphics ok */
    144144                 /* 2=No gui ok */
     145        int position_projection;
    145146};
    146147
    147148struct gui *main_loop_gui;
     
    721722        this_->tracking_flag=1;
    722723        this_->recentdest_count=10;
    723724        this_->osd_configuration=-1;
     725        this_->position_projection=0;
    724726
    725727        this_->center_timeout = 10;
    726728        this_->use_mousewheel = 1;
     
    15341536                attr_updated=(this_->autozoom_active != attr->u.num);
    15351537                this_->autozoom_active = attr->u.num;
    15361538                break;
     1539        case attr_position_projection:
     1540                this_->position_projection = attr->u.num;
     1541                break;
    15371542        case attr_center:
    15381543                transform_from_geo(transform_get_projection(this_->trans), attr->u.coord_geo, &co);
    15391544                dbg(1,"0x%x,0x%x\n",co.x,co.y);
     
    20262031static void
    20272032navit_vehicle_update(struct navit *this_, struct navit_vehicle *nv)
    20282033{
    2029         struct attr attr_valid, attr_dir, attr_speed, attr_pos;
     2034        struct attr attr_valid, attr_dir, attr_speed, attr_pos,attr_pos_no_tracking;
    20302035        struct pcoord cursor_pc;
    20312036        struct point cursor_pnt, *pnt=&cursor_pnt;
    20322037        struct tracking *tracking=NULL;
     
    20562061        if (get_attr(attr_object, attr_position_valid, &attr_valid, NULL))
    20572062                if (!attr_valid.u.num != attr_position_valid_invalid)
    20582063                        return;
     2064        vehicle_get_attr(nv->vehicle, attr_position_coord_geo, &attr_pos_no_tracking, NULL);
    20592065        if (! get_attr(attr_object, attr_position_direction, &attr_dir, NULL) ||
    20602066            ! get_attr(attr_object, attr_position_speed, &attr_speed, NULL) ||
    20612067            ! get_attr(attr_object, attr_position_coord_geo, &attr_pos, NULL)) {
     
    20642070        }
    20652071        nv->dir=*attr_dir.u.numd;
    20662072        nv->speed=*attr_speed.u.numd;
    2067         transform_from_geo(pro, attr_pos.u.coord_geo, &nv->coord);
     2073        if(!this_->position_projection || !tracking) {
     2074                transform_from_geo(pro, attr_pos.u.coord_geo, &nv->coord);
     2075        }
     2076        else {
     2077                transform_from_geo(pro, attr_pos_no_tracking.u.coord_geo, &nv->coord);
     2078                struct coord*coord_proj = tracking_get_pos_proj(tracking, &nv->coord);
     2079                nv->coord = *coord_proj;
     2080        }
    20682081        if (nv != this_->vehicle) {
    20692082                navit_vehicle_draw(this_, nv, NULL);
    20702083                profile(0,"return 3\n");
     
    20852098                navit_disable_suspend();
    20862099
    20872100        transform(this_->trans, pro, &nv->coord, &cursor_pnt, 1, 0, 0, NULL);
     2101
    20882102        if (this_->button_pressed != 1 && this_->follow_cursor && nv->follow_curr <= nv->follow &&
    20892103                (nv->follow_curr == 1 || !transform_within_border(this_->trans, &cursor_pnt, border)))
    20902104                navit_set_center_cursor_draw(this_);
  • track.c

     
    8787        struct tracking_line *lines;
    8888        struct tracking_line *curr_line;
    8989        int pos;
    90         struct coord curr[2], curr_in, curr_out;
     90        struct coord curr[2], curr_in, curr_out, curr_out_proj;
    9191        int curr_angle;
    9292        struct coord last[2], last_in, last_out;
    9393        struct cdf_data cdf;
     
    298298        return &tr->curr_out;
    299299}
    300300
     301struct coord *
     302tracking_get_pos_proj_last(struct tracking *tr)
     303{
     304        return &tr->curr_out_proj;
     305}
     306
     307struct coord *
     308tracking_get_pos_proj(struct tracking *tr, struct coord* pt)
     309{
     310        //calculate curr_out_proj
     311        struct coord a, b, c;           // viewing from c0
     312        struct coord a_, b_, c_;        // viewing from c1
     313        int base_idx = tr->pos;
     314        if(0<base_idx){         //check three consecutive segments (before, current, after)
     315                --base_idx;
     316        }
     317
     318        struct coord* coords = tr->curr_line->street->c;
     319        int count = tr->curr_line->street->count;
     320        int cnt = 0; //try only three segments
     321        int minx,miny,maxx,maxy;
     322        int dist = INT_MAX;
     323        int bFound = 0;
     324        do {
     325                struct coord c0 = tr->curr_line->street->c[base_idx];
     326                struct coord c1 = tr->curr_line->street->c[base_idx+1];
     327
     328                b.x = c1.x - c0.x;
     329                b.y = c1.y - c0.y;
     330                a.x = pt->x - c0.x;
     331                a.y = pt->y - c0.y;
     332
     333                int b_len_sq = b.x*b.x + b.y*b.y;
     334                int ab = a.x*b.x + a.y*b.y;
     335                if(b_len_sq) {
     336                        double b_factor = (double)ab / b_len_sq;
     337                        c.x = b.x*b_factor;
     338                        c.y = b.y*b_factor;
     339
     340                        c.x += c0.x;
     341                        c.y += c0.y;
     342                }
     343
     344                //calculate distance squeres from current segment
     345                int curr_dist;
     346                b_.x = c0.x - c1.x;
     347                b_.y = c0.y - c1.y;
     348                a_.x = pt->x - c1.x;
     349                a_.y = pt->y - c1.y;
     350                int ab_ = a_.x*b_.x + a_.y*b_.y;
     351                if(ab<0) {              // if /A/*/B/*cos(alpha) < 0  than take the distance sq from c0
     352                        curr_dist = (a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y);
     353                }
     354                else if(ab_<0) {        // if /A_/*/B_/*cos(alpha_) < 0  than take the distance sq from c1
     355                        curr_dist = (a_.x-b_.x)*(a_.x-b_.x) + (a_.y-b_.y)*(a_.y-b_.y);
     356                }
     357                else {                  //take the distance sq between the input point and the projected point
     358                        curr_dist = (a.x-c.x)*(a.x-c.x) + (a.y-c.y)*(a.y-c.y);
     359                        bFound = 1;
     360                }
     361                if(bFound || curr_dist<dist) {
     362                        dist=curr_dist;
     363                        tr->curr_out_proj = c;
     364                        tr->curr_out = c;
     365                }
     366                ++cnt;
     367                ++base_idx;
     368        } while( !bFound && cnt<=3 && (base_idx+1)<count);
     369
     370        return &tr->curr_out_proj;
     371}
     372
    301373int
    302374tracking_get_street_direction(struct tracking *tr)
    303375{
  • track.h

     
    3939struct vehicleprofile;
    4040int tracking_get_angle(struct tracking *tr);
    4141struct coord *tracking_get_pos(struct tracking *tr);
     42struct coord *tracking_get_pos_proj(struct tracking *tr, struct coord* pt);
     43struct coord *tracking_get_pos_proj_last(struct tracking *tr);
    4244int tracking_get_street_direction(struct tracking *tr);
    4345int tracking_get_segment_pos(struct tracking *tr);
    4446struct street_data *tracking_get_street_data(struct tracking *tr);