RoadMapFuzzy roadmap_fuzzy_connected (const RoadMapNeighbour *street, const RoadMapNeighbour *reference, int prev_direction, int direction, RoadMapPosition *connection) { /* The logic for the connection membership function is that * the line is the most connected to itself, is well connected * to lines to which it share a common point and is not well * connected to other lines. * The weight of each state is a matter fine tuning. */ RoadMapPosition line_point[2]; RoadMapPosition reference_point[2]; int allowed; int i, j; if (roadmap_plugin_same_db_line (&street->line, &reference->line)) { *connection = street->from; return (FUZZY_TRUTH_MAX * 4) / 4; } allowed = allowed_turn (reference, prev_direction, street, direction); if (allowed == 1) { //printf("==>found valid turn\n"); if (prev_direction == ROUTE_DIRECTION_AGAINST_LINE) *connection = street->from; else *connection = street->to; return (FUZZY_TRUTH_MAX * 4) / 4; } else if (allowed == -1) { //printf("--> found shared node\n"); if (prev_direction == ROUTE_DIRECTION_AGAINST_LINE) *connection = street->from; else *connection = street->to; return (FUZZY_TRUTH_MAX * 3) / 4; } roadmap_street_extend_line_ends (&street->line,&(line_point[0]), &(line_point[1]), FLAG_EXTEND_BOTH, NULL, NULL); roadmap_street_extend_line_ends (&reference->line,&(reference_point[0]), &(reference_point[1]), FLAG_EXTEND_BOTH, NULL, NULL); if (direction == ROUTE_DIRECTION_AGAINST_LINE) { i = 1; } else { i = 0; } if (prev_direction == ROUTE_DIRECTION_AGAINST_LINE) { j = 0; } else { j = 1; } if ((line_point[i].latitude == reference_point[j].latitude) && (line_point[i].longitude == reference_point[j].longitude)) { *connection = line_point[i]; return (FUZZY_TRUTH_MAX * 4) / 4; } else if ((line_point[!i].latitude == reference_point[j].latitude) && (line_point[!i].longitude == reference_point[j].longitude)) { *connection = line_point[!i]; return FUZZY_TRUTH_MAX / 2; } else { RoadMapPosition pos1; RoadMapPosition pos2; pos1 = line_point[i]; pos2 = reference_point[j]; if (roadmap_math_distance (&pos1, &pos2) < 50) { connection->latitude = 0; connection->longitude = 0; return FUZZY_TRUTH_MAX * 2 / 3; } } connection->latitude = 0; connection->longitude = 0; return FUZZY_TRUTH_MAX / 3; }
static int resolve_candidates (const RoadMapGpsPosition *gps_position, int point_id) { int count; int c; int found; RoadMapFuzzy best = roadmap_fuzzy_false (); RoadMapFuzzy second_best; int i; int j; for (c = KnownCandidatesCount - 1; c >= 0; c--) { RoadMapTracking new_street; KnownCandidateEntry *entry = KnownCandidates[c].entries + KnownCandidates[c].count - 1; count = editor_track_util_find_street (gps_position, &new_street, &entry->street, &entry->line, RoadMapNeighbourhood, ROADMAP_NEIGHBOURHOUD, &found, &best, &second_best); if (!roadmap_fuzzy_is_good (best)) { if (KnownCandidatesCount == 1) { /* This is the last path, force use it, but lower its confidence */ KnownCandidates[0].low_confidence = 1; return 0; } memmove (KnownCandidates + c, KnownCandidates + c + 1, (KnownCandidatesCount - c - 1) * sizeof (KnownCandidatePath)); KnownCandidatesCount--; continue; } if (roadmap_fuzzy_is_good (second_best) && !roadmap_plugin_same_db_line (&entry->line.line, &RoadMapNeighbourhood[found].line)) { /* We need to create new paths */ for (i = 0; i < count; ++i) { RoadMapTracking candidate; int result; /* We handle the best result below this block */ if (i == found) continue; result = roadmap_navigate_fuzzify (&candidate, &entry->street, &entry->line, RoadMapNeighbourhood+i, 0, gps_position->steering); if (roadmap_fuzzy_is_good (result)) { if (roadmap_plugin_same_line (&entry->line.line, &RoadMapNeighbourhood[i].line)) { /* Skip the current line */ continue; } if (KnownCandidatesCount == MAX_PATHS) return -1; memcpy (KnownCandidates + KnownCandidatesCount, KnownCandidates + c, sizeof (KnownCandidatePath)); resolve_add (&KnownCandidates[KnownCandidatesCount], point_id, &RoadMapNeighbourhood[i], result, &candidate); KnownCandidatesCount++; } } } resolve_add (&KnownCandidates[c], point_id, &RoadMapNeighbourhood[found], best, &new_street); } /* check for duplicates */ for (i = 0; i < KnownCandidatesCount-1; i++) { KnownCandidateEntry *entry1 = KnownCandidates[i].entries + KnownCandidates[i].count - 1; for (j = i+1; j < KnownCandidatesCount; j++) { KnownCandidateEntry *entry2 = KnownCandidates[j].entries + KnownCandidates[j].count - 1; if (roadmap_plugin_same_line (&entry2->line.line, &entry1->line.line)) { int entry1_score = 0; int entry2_score = 0; int do_switch = 0; int k; int entries_count; if (KnownCandidates[i].count == KnownCandidates[j].count) { /* don't count the last entry which is the same one */ entries_count = KnownCandidates[i].count - 1; } else if (KnownCandidates[i].count < KnownCandidates[j].count) { entries_count = KnownCandidates[i].count; } else { entries_count = KnownCandidates[j].count; } for (k=0; k<entries_count; k++) { entry1_score += KnownCandidates[i].entries[k].street.entry_fuzzyfied; entry2_score += KnownCandidates[j].entries[k].street.entry_fuzzyfied; } if (entries_count) { entry1_score /= entries_count; entry2_score /= entries_count; } if ((abs(entry1_score - entry2_score) < (roadmap_fuzzy_good () / 10)) && (KnownCandidates[i].count != KnownCandidates[j].count)) { if (KnownCandidates[j].count < KnownCandidates[i].count) { do_switch = 1; } } else if (entry2_score > entry1_score) { do_switch = 1; } if (do_switch) { memcpy (KnownCandidates + i, KnownCandidates + j, sizeof (KnownCandidatePath)); } /* remove the obsolete entry */ memmove (KnownCandidates + j, KnownCandidates + j + 1, (KnownCandidatesCount - j - 1) * sizeof (KnownCandidatePath)); KnownCandidatesCount--; j--; KnownCandidates[i].low_confidence = 1; } } } return 0; }