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;
}
