/* 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);
}
}
