/* Detect roundabouts */ static int is_roundabout (int first_point, int last_point) { //RoadMapPosition middle; int length; //int i; length = editor_track_util_length (first_point, last_point); if ((length < editor_track_point_distance ()*3) || (length > editor_track_point_distance ()*20)) { return 0; } return 1; #if 0 middle.longitude = middle.latitude = 0; for (i=first_point; i<last_point; i++) { middle.longitude += GpsPoints[i].longitude; middle.latitude += GpsPoints[i].latitude; } middle.longitude /= (last_point - first_point); middle.latitude /= (last_point - first_point); /* Fix the end of the segment before the roundabout */ GpsPoints[first_point-1].longitude = middle.longitude; GpsPoints[first_point-1].latitude = middle.latitude; #endif }
/* Find if the current point matches a point on the current new line. * (Detect loops) */ static int match_distance_from_current (const RoadMapGpsPosition *gps_point, int last_point, RoadMapPosition *best_intersection, int *match_point) { int i; int distance; int smallest_distance; int azymuth = 0; RoadMapPosition intersection; smallest_distance = 0x7fffffff; for (i = 0; i < (last_point-1); i++) { distance = roadmap_math_get_distance_from_segment ((RoadMapPosition *)gps_point, track_point_pos (i), track_point_pos (i+1), &intersection, NULL); if (distance < smallest_distance) { smallest_distance = distance; *best_intersection = intersection; *match_point = i; azymuth = roadmap_math_azymuth (track_point_pos (i), track_point_pos (i+1)); } } return (smallest_distance < editor_track_point_distance ()) && (roadmap_math_delta_direction (azymuth, gps_point->steering) < 10); }
int editor_track_known_locate_point (int point_id, const RoadMapGpsPosition *gps_position, RoadMapTracking *confirmed_street, RoadMapNeighbour *confirmed_line, RoadMapTracking *new_street, RoadMapNeighbour *new_line) { int found; int count; int current_fuzzy; RoadMapFuzzy best = roadmap_fuzzy_false (); RoadMapFuzzy second_best; const RoadMapPosition *position = track_point_pos (point_id); RoadMapFuzzy before; if (KnownCandidatesCount > 1) { if ((resolve_candidates (gps_position, point_id) == -1) || !KnownCandidatesCount) { KnownCandidatesCount = 0; editor_track_reset (); return 0; } if (KnownCandidatesCount > 1) { return 0; } } if (KnownCandidatesCount == 1) { KnownCandidateEntry *entry = KnownCandidates[0].entries; if (!--KnownCandidates[0].count) { KnownCandidatesCount = 0; } *new_line = entry->line; *new_street = entry->street; if (KnownCandidates[0].low_confidence) new_street->entry_fuzzyfied = roadmap_fuzzy_acceptable (); point_id = entry->point_id; memmove (entry, entry+1, KnownCandidates[0].count * sizeof(KnownCandidateEntry)); if (!point_id) { if (confirmed_street->valid && roadmap_plugin_same_line (&confirmed_line->line, &new_line->line)) { return 0; } *confirmed_street = *new_street; *confirmed_line = *new_line; confirmed_street->valid = 1; if (confirmed_street->valid) { /* This is probably a GPS jump or an error in resolve */ return -1; } } return point_id; } if (confirmed_street->valid) { /* We have an existing street match: check if it is still valid. */ before = confirmed_street->cur_fuzzyfied; if (!editor_track_util_get_distance (position, &confirmed_line->line, confirmed_line)) { current_fuzzy = 0; confirmed_line->distance = MAX_DISTANCE; } else { current_fuzzy = roadmap_navigate_fuzzify (new_street, confirmed_street, confirmed_line, confirmed_line, 0, gps_position->steering); } if (current_fuzzy && (new_street->line_direction == confirmed_street->line_direction) && ((current_fuzzy >= before) || roadmap_fuzzy_is_certain(current_fuzzy))) { confirmed_street->cur_fuzzyfied = current_fuzzy; return 0; /* We are on the same street. */ } } /* We must search again for the best street match. */ count = editor_track_util_find_street (gps_position, new_street, confirmed_street, confirmed_line, RoadMapNeighbourhood, ROADMAP_NEIGHBOURHOUD, &found, &best, &second_best); #if 0 if (count && (RoadMapNeighbourhood[found].distance < 30) && !roadmap_fuzzy_is_acceptable (best)) { /* if we are on a known street, we don't want to start a new road * if we are still close to it. In this case we ignore the fuzzy */ } else #endif if (!roadmap_fuzzy_is_good (best) && roadmap_fuzzy_is_acceptable (best) && confirmed_street->valid) { /* We're not sure that the new line is a real match. * Delay the decision if we're still close to the previous road. */ if (confirmed_line->distance < editor_track_point_distance ()) { RoadMapTracking candidate; if (roadmap_plugin_same_line (&RoadMapNeighbourhood[found].line, &confirmed_line->line)) { /* We don't have any candidates other than the current line */ return 0; } /* We have two candidates here */ /* current line */ KnownCandidates[0].entries[0].street = *confirmed_street; KnownCandidates[0].entries[0].line = *confirmed_line; KnownCandidates[0].entries[0].point_id = 0; KnownCandidates[0].count = 1; KnownCandidates[0].low_confidence = 0; /* new line */ candidate.entry_fuzzyfied = roadmap_navigate_fuzzify (&candidate, confirmed_street, confirmed_line, RoadMapNeighbourhood+found, 0, gps_position->steering); candidate.cur_fuzzyfied = candidate.entry_fuzzyfied; candidate.valid = 1; KnownCandidates[1].entries[0].street = candidate; KnownCandidates[1].entries[0].line = RoadMapNeighbourhood[found]; KnownCandidates[1].entries[0].point_id = point_id; KnownCandidates[1].count = 1; KnownCandidates[1].low_confidence = 0; KnownCandidatesCount = 2; return 0; } } if ((roadmap_fuzzy_is_good (best) && roadmap_fuzzy_is_good (second_best)) || (!roadmap_fuzzy_is_good (best) && roadmap_fuzzy_is_acceptable (second_best))) { /* We got too many options. Wait before deciding. */ int i; RoadMapFuzzy result; int use_acceptable = 0; if (!roadmap_fuzzy_is_good (best)) use_acceptable = 1; for (i = 0; i < count; ++i) { RoadMapTracking candidate; result = roadmap_navigate_fuzzify (&candidate, confirmed_street, confirmed_line, RoadMapNeighbourhood+i, 0, gps_position->steering); if (roadmap_fuzzy_is_good (result) || (use_acceptable && roadmap_fuzzy_is_acceptable (result))) { if (confirmed_street->valid && roadmap_plugin_same_line (&confirmed_line->line, &RoadMapNeighbourhood[i].line)) { if (result < best) { continue; } else { /* delay the decision as the current line is still good */ KnownCandidatesCount = 0; return 0; } } candidate.valid = 1; candidate.entry_fuzzyfied = candidate.cur_fuzzyfied = result; KnownCandidates[KnownCandidatesCount].entries[0].street = candidate; KnownCandidates[KnownCandidatesCount].entries[0].line = RoadMapNeighbourhood[i]; KnownCandidates[KnownCandidatesCount].entries[0].point_id = point_id; KnownCandidates[KnownCandidatesCount].count = 1; KnownCandidates[KnownCandidatesCount].low_confidence = 0; KnownCandidatesCount++; if (KnownCandidatesCount == MAX_PATHS) { roadmap_log (ROADMAP_ERROR, "ResolveCandidates - Reached max entries!"); break; } } } if (KnownCandidatesCount > 1) { return 0; } else { /* We only got one candidate so fall through to use it */ KnownCandidatesCount = 0; } } if (roadmap_fuzzy_is_acceptable (best)) { if (confirmed_street->valid && (!roadmap_plugin_same_line (&confirmed_line->line, &RoadMapNeighbourhood[found].line) || (confirmed_street->opposite_street_direction != new_street->opposite_street_direction) || (confirmed_street->line_direction != new_street->line_direction))) { *new_line = RoadMapNeighbourhood[found]; new_street->valid = 1; new_street->entry_fuzzyfied = new_street->cur_fuzzyfied = best; return point_id; } *confirmed_line = RoadMapNeighbourhood[found]; if (!confirmed_street->valid) confirmed_street->entry_fuzzyfied = best; new_street->entry_fuzzyfied = confirmed_street->entry_fuzzyfied; *confirmed_street = *new_street; confirmed_street->cur_fuzzyfied = best; confirmed_street->valid = 1; return 0; } else { new_street->valid = 0; return point_id; } }
int editor_track_known_end_segment (PluginLine *previous_line, int last_point_id, PluginLine *line, RoadMapTracking *street, int is_new_track) { //TODO: add stuff //Notice that previous_line may not be valid (only at first) // //check for low confidence & in static update, do not use a trkseg with low confidence! RoadMapPosition from; RoadMapPosition to; RoadMapPosition *current; int trkseg; int trkseg_line_id; int trkseg_plugin_id; int trkseg_square; int line_length; int segment_length; int percentage; int flags = 0; editor_log_push ("editor_track_end_known_segment"); assert (last_point_id != 0); if (!last_point_id) return 0; if (editor_db_activate (line->fips) == -1) { editor_db_create (line->fips); if (editor_db_activate (line->fips) == -1) { editor_log_pop (); return 0; } } roadmap_street_extend_line_ends (line, &from, &to, FLAG_EXTEND_BOTH, NULL, NULL); trkseg_plugin_id = roadmap_plugin_get_id (line); trkseg_line_id = roadmap_plugin_get_line_id (line); trkseg_square = roadmap_plugin_get_square (line); line_length = editor_track_util_get_line_length (line); segment_length = editor_track_util_length (0, last_point_id); editor_log (ROADMAP_INFO, "Ending line %d (plugin_id:%d). Line length:%d, Segment length:%d", trkseg_line_id, trkseg_plugin_id, line_length, segment_length); /* End current segment if we really passed through it * and not only touched a part of it. */ /*SRUL*: avoid this problem, see above comment assert (line_length > 0); if (line_length == 0) { editor_log (ROADMAP_ERROR, "line %d (plugin_id:%d) has length of zero.", trkseg_line_id, trkseg_plugin_id); editor_log_pop (); return 0; } */ if (line_length <= 0) line_length = 1; current = track_point_pos (last_point_id); if (roadmap_math_distance (current, &to) > roadmap_math_distance (current, &from)) { flags = ED_TRKSEG_OPPOSITE_DIR; #if DEBUG_TRACKS printf ("Closing trkseg from %d.%06d,%d.%06d to %d.%06d,%d.%06d", to.longitude / 1000000, to.longitude % 1000000, to.latitude / 1000000, to.latitude % 1000000, from.longitude / 1000000, from.longitude % 1000000, from.latitude / 1000000, from.latitude % 1000000); } else { printf ("Closing trkseg from %d.%06d,%d.%06d to %d.%06d,%d.%06d", from.longitude / 1000000, from.longitude % 1000000, from.latitude / 1000000, from.latitude % 1000000, to.longitude / 1000000, to.longitude % 1000000, to.latitude / 1000000, to.latitude % 1000000); #endif } #if DEBUG_TRACKS printf (" %d/%d", segment_length, line_length); #endif if (is_new_track) { flags |= ED_TRKSEG_NEW_TRACK; #if DEBUG_TRACKS printf (" NEW"); #endif } if (!editor_ignore_new_roads ()) { flags |= ED_TRKSEG_RECORDING_ON; } percentage = 100 * segment_length / line_length; if (percentage < 70) { editor_log (ROADMAP_INFO, "segment is too small to consider: %d%%", percentage); if (segment_length > (editor_track_point_distance ()*1.5)) { trkseg = editor_track_util_create_trkseg (trkseg_square, trkseg_line_id, trkseg_plugin_id, 0, last_point_id, flags|ED_TRKSEG_LOW_CONFID); editor_track_add_trkseg (line, trkseg, ROUTE_DIRECTION_NONE, ROUTE_CAR_ALLOWED); editor_log_pop (); #if DEBUG_TRACKS printf (" LOW (percentage)\n"); #endif return 1; } else { trkseg = editor_track_util_create_trkseg (trkseg_square, trkseg_line_id, trkseg_plugin_id, 0, last_point_id, flags|ED_TRKSEG_IGNORE); editor_track_add_trkseg (line, trkseg, ROUTE_DIRECTION_NONE, ROUTE_CAR_ALLOWED); editor_log_pop (); #if DEBUG_TRACKS printf (" IGNORE\n"); #endif return 0; } } #if 0 if (!roadmap_fuzzy_is_good (street->entry_fuzzyfied) || !roadmap_fuzzy_is_good (street->cur_fuzzyfied)) { flags |= ED_TRKSEG_LOW_CONFID; #if DEBUG_TRACKS printf (" LOW"); if (!roadmap_fuzzy_is_good (street->entry_fuzzyfied)) printf (" (entry)"); if (!roadmap_fuzzy_is_good (street->cur_fuzzyfied)) printf (" (cur)"); #endif } #endif if (trkseg_plugin_id != ROADMAP_PLUGIN_ID) flags |= ED_TRKSEG_LOW_CONFID; trkseg = editor_track_util_create_trkseg (trkseg_square, trkseg_line_id, trkseg_plugin_id, 0, last_point_id, flags); if (flags & ED_TRKSEG_OPPOSITE_DIR) { editor_log (ROADMAP_INFO, "Updating route direction: to -> from"); editor_track_add_trkseg (line, trkseg, ROUTE_DIRECTION_AGAINST_LINE, ROUTE_CAR_ALLOWED); } else { editor_log (ROADMAP_INFO, "Updating route direction: from -> to"); editor_track_add_trkseg (line, trkseg, ROUTE_DIRECTION_WITH_LINE, ROUTE_CAR_ALLOWED); } editor_log_pop (); #if DEBUG_TRACKS printf ("\n"); #endif return 1; }
static void end_unknown_segments (TrackNewSegment *new_segments, int count) { int i; int start_point = 0; NodeNeighbour end_node = NODE_NEIGHBOUR_NULL; editor_track_util_create_db (track_point_pos (start_point)); for (i=0; i<count; i++) { int type = new_segments[i].type; int end_point = new_segments[i].point_id; int end_node_id = -1; switch (type) { case TRACK_ROAD_TURN: if (editor_track_util_length (start_point, end_point) < (editor_track_point_distance () * 2)) { RoadMapPosition pos; RoadMapPosition *pos1; RoadMapPosition *pos2; pos1 = track_point_pos (start_point); pos2 = track_point_pos (end_point); pos.longitude = (pos1->longitude + pos2->longitude) / 2; pos.latitude = (pos1->latitude + pos2->latitude) / 2; if (cur_node.plugin_id == ROADMAP_PLUGIN_ID) { cur_node.id = editor_track_util_roadmap_node_to_editor (&cur_node); cur_node.plugin_id = EditorPluginID; } else { editor_point_set_pos (cur_node.id, &pos); } start_point = end_point; continue; } break; case TRACK_ROAD_ROUNDABOUT: if (cur_node.plugin_id == ROADMAP_PLUGIN_ID) { cur_node.id = editor_track_util_roadmap_node_to_editor (&cur_node); cur_node.plugin_id = EditorPluginID; } create_new_line (start_point, end_point, cur_node.id, cur_node.id, ROADMAP_ROAD_STREET); start_point = end_point; continue; break; } if ((i == (count - 1)) && (TrackConfirmedStreet.valid)) { /* we are connecting to a known road */ end_point = editor_track_util_new_road_end (&TrackConfirmedLine, track_point_pos (end_point), end_point, TrackConfirmedStreet.line_direction, &end_node); if (end_node.plugin_id == ROADMAP_PLUGIN_ID) { end_node.id = editor_track_util_roadmap_node_to_editor (&end_node); end_node.plugin_id = EditorPluginID; } end_node_id = end_node.id; } if ((i < (count -1)) || (start_point != (end_point -1))) { int line_id = create_new_line (start_point, end_point, -1, end_node_id, ROADMAP_ROAD_STREET); if ((line_id != -1) && ((type == TRACK_ROAD_CONNECTION) || !EditorAllowNewRoads)) { editor_line_set_flag (line_id, ED_LINE_CONNECTION); } } start_point = end_point; } track_reset_points (start_point); }
int editor_track_unknown_locate_point (int point_id, const RoadMapGpsPosition *gps_position, RoadMapTracking *confirmed_street, RoadMapNeighbour *confirmed_line, TrackNewSegment *new_segment, int max_segments, int point_type) { RoadMapFuzzy best; RoadMapFuzzy second_best; RoadMapTracking nominated; int found; int count; if (point_type & POINT_UNKNOWN) { /* this point is already known to be unknown */ return detect_road_segment (point_id, gps_position, new_segment, max_segments, point_type); } /* let's see if we got to a known street line */ count = editor_track_util_find_street (gps_position, &nominated, confirmed_street, confirmed_line, RoadMapNeighbourhood, ROADMAP_NEIGHBOURHOUD, &found, &best, &second_best); if (roadmap_fuzzy_is_good (best) || confirmed_street->valid) { /* we found an existing road, let's close the new road */ /* delay ending the line until we find the best location to end it */ if (roadmap_fuzzy_is_good (best) && ( !confirmed_street->valid || (best > confirmed_street->cur_fuzzyfied))) { // RoadMapNeighbourhood[found].distance < // confirmed_line->distance)) { *confirmed_line = RoadMapNeighbourhood[found]; *confirmed_street = nominated; confirmed_street->valid = 1; confirmed_street->entry_fuzzyfied = best; confirmed_street->cur_fuzzyfied = best; /* if (!roadmap_fuzzy_is_certain (best)) { return 0; } */ if (confirmed_line->distance > editor_track_point_distance ()) { return 0; } } if (point_type & POINT_GAP) { new_segment->type = TRACK_ROAD_CONNECTION; } else { new_segment->type = TRACK_ROAD_REG; } new_segment->point_id = point_id; if (best != confirmed_street->cur_fuzzyfied) { new_segment->point_id--; } return 1; } else { /* point is unknown, see if we can detect a road segment */ return detect_road_segment (point_id, gps_position, new_segment, max_segments, point_type); } }