int alert_is_on_route (roadmap_alerter_location_info *pInfo, int steering, roadmap_alert_provider* provider, int i ) {
int rc = 0;
int from;
int to;
RoadMapPosition from_position;
RoadMapPosition to_position;
int road_azymuth;
int delta;
BOOL is_alert_on_route = ((provider->is_on_route) && ((* (provider->is_on_route))((* (provider->get_id))(i))));
if (is_alert_on_route)
return TRUE;
roadmap_square_set_current (pInfo->square_id);
roadmap_line_points (pInfo->line_id, &from, &to);
roadmap_point_position (from, &from_position);
roadmap_point_position (to, &to_position);
road_azymuth = roadmap_math_azymuth (&from_position, &to_position);
delta = azymuth_delta (steering, road_azymuth);
if (delta >= -90 && delta < 90)
rc = navigate_is_line_on_route (pInfo->square_id, pInfo->line_id, from, to);
else
rc = navigate_is_line_on_route (pInfo->square_id, pInfo->line_id, to, from);
return rc;
}
static int alert_is_on_route (const RoadMapPosition *point_position, int steering) {
int count = 0;
int layers[128];
int layers_count;
RoadMapNeighbour neighbours;
int rc = 0;
int from;
int to;
RoadMapPosition from_position;
RoadMapPosition to_position;
int road_azymuth;
int delta;
int square_current = roadmap_square_active ();
layers_count = roadmap_layer_all_roads (layers, 128);
if (layers_count > 0) {
count = roadmap_street_get_closest
(point_position, 0, layers, layers_count, &neighbours, 1);
if (count > 0) {
roadmap_square_set_current (neighbours.line.square);
roadmap_line_points (neighbours.line.line_id, &from, &to);
roadmap_point_position (from, &from_position);
roadmap_point_position (to, &to_position);
road_azymuth = roadmap_math_azymuth (&from_position, &to_position);
delta = azymuth_delta (steering, road_azymuth);
if (delta >= -90 && delta < 90)
rc = navigate_is_line_on_route (neighbours.line.square, neighbours.line.line_id, from, to);
else
rc = navigate_is_line_on_route (neighbours.line.square, neighbours.line.line_id, to, from);
}
}
roadmap_square_set_current (square_current);
return rc;
}
/* 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);
}
//checks whether an alert is within range
static int is_alert_in_range(const RoadMapGpsPosition *gps_position, const PluginLine *line) {
int count;
int i;
int distance ;
int steering;
RoadMapPosition pos;
int azymuth;
int delta;
int j;
unsigned int speed;
RoadMapPosition gps_pos;
int square;
int squares[9];
int count_squares;
gps_pos.latitude = gps_position->latitude;
gps_pos.longitude = gps_position->longitude;
count_squares = roadmap_square_find_neighbours (&gps_pos, 0, squares);
for (square = 0; square < count_squares; square++) {
roadmap_square_set_current(squares[square]);
// loop alll prvidor for an alert
for (j = 0 ; j < RoadMapAlertProvidors.count; j++) {
count = (* (RoadMapAlertProvidors.providor[j]->count)) ();
// no alerts for this providor
if (count == 0) {
continue;
}
for (i=0; i<count; i++) {
// if the alert is not alertable, continue. (dummy speed cams, etc.)
if (!(* (RoadMapAlertProvidors.providor[j]->is_alertable))(i)) {
continue;
}
(* (RoadMapAlertProvidors.providor[j]->get_position)) (i, &pos, &steering);
// check that the alert is within alert distance
distance = roadmap_math_distance(&pos, &gps_pos);
if (distance > (*(RoadMapAlertProvidors.providor[j]->get_distance))(i)) {
continue;
}
// check if the alert is on the navigation route
if (!alert_is_on_route (&pos, steering)) {
// check that the alert is in the direction of driving
delta = azymuth_delta(gps_position->steering, steering);
if (delta > AZYMUTH_DELTA || delta < (0-AZYMUTH_DELTA)) {
continue;
}
// check that we didnt pass the alert
azymuth = roadmap_math_azymuth (&gps_pos, &pos);
delta = azymuth_delta(azymuth, steering);
if (delta > 90|| delta < (-90)) {
continue;
}
// check that the alert is on the same street
if (!check_same_street(line, &pos)) {
continue;
}
}
speed = (* (RoadMapAlertProvidors.providor[j]->get_speed))(i);
// check that the driving speed is over the allowed speed for that alert
if ((unsigned int)roadmap_math_to_speed_unit(gps_position->speed) < speed) {
the_active_alert.alert_type = WARN;
the_active_alert.distance_to_alert = distance;
the_active_alert.active_alert_id = (* (RoadMapAlertProvidors.providor[j]->get_id))(i);
the_active_alert.alert_providor = j;
return TRUE;
}
else {
the_active_alert.alert_type = ALERT;
the_active_alert.alert_providor = j;
the_active_alert.distance_to_alert = distance;
the_active_alert.active_alert_id = (* (RoadMapAlertProvidors.providor[j]->get_id))(i);
return TRUE;
}
}
}
}
return FALSE;
}
/*
* searches for relevant alerts in the given param provider - D.F.
*/
static BOOL is_alert_in_range_provider(roadmap_alert_provider* provider,
const RoadMapGpsPosition *gps_position, const PluginLine* line,
int * pAlert_index, int * pDistance, const char* cur_street_name){
int i;
int steering;
RoadMapPosition pos;
int azymuth;
int delta;
int square_current;
int count = 0;
const char *street_name;
roadmap_alerter_location_info location_info;
roadmap_alerter_location_info * pAlert_location_info = NULL;
RoadMapPosition gps_pos;
gps_pos.latitude = gps_position->latitude;
gps_pos.longitude = gps_position->longitude;
square_current = roadmap_square_active ();
count = (* (provider->count)) ();
// no alerts for this provider
if (count == 0) {
return FALSE;
}
for (i=0; i<count; i++) {
roadmap_square_set_current (square_current);
// if the alert is not alertable, continue. (dummy speed cams, etc.)
if (!(* (provider->is_alertable))(i)) {
continue;
}
(* (provider->get_position)) (i, &pos, &steering);
// check that the alert is within alert distance
* pDistance = roadmap_math_distance(&pos, &gps_pos);
if (*pDistance > (*(provider->get_distance))(i))
continue;
// Let the provider handle the Alert.
if ((* (provider->handle_event))((* (provider->get_id))(i)))
continue;
// get this alerts cached location info
pAlert_location_info = (*(provider->get_location_info))(i);
// retrieve the current location info for this alert ( square id, line id... )
if(!get_alert_location_info(&pos,&location_info,pAlert_location_info))
continue; // could not find relevant location.
// check if the alert is on the navigation route
if (!alert_is_on_route (&location_info, steering)) {
roadmap_square_set_current (square_current);
if ((* (provider->check_same_street))(i)){
// check that the alert is in the direction of driving
delta = azymuth_delta(gps_position->steering, steering);
if (delta > AZYMUTH_DELTA || delta < (0-AZYMUTH_DELTA)) {
continue;
}
// check that we didnt pass the alert
azymuth = roadmap_math_azymuth (&gps_pos, &pos);
delta = azymuth_delta(azymuth, steering);
if (delta > 90|| delta < (-90))
continue;
// get the street name of the alert
get_street_from_line(location_info.square_id,location_info.line_id, &street_name);
// if same street found an alert infront of us
if(strcmp(street_name,cur_street_name))
continue; // not the same street.
}
}
// if we got until here, then we found a valid alert
* pAlert_index = i;
roadmap_square_set_current (square_current);
return TRUE;
}
roadmap_square_set_current (square_current);
return FALSE;
}
/**
* [IN] alert_position - the position of the alert
* [IN] alert_location_info - The old location_info of the alert. Will be updated if necessary
* [OUT] pNew_Info - the location_info of this alert, to be calculated.
* returns TRUE iff successful in finding relevant location info for this alert
* - D.F.
*/
BOOL get_alert_location_info(const RoadMapPosition *alert_position, int alert_streering, roadmap_alerter_location_info *pNew_Info,
roadmap_alerter_location_info *alert_location_info){
int alert_square_time_stamp;
int layers_count;
int count;
int layers[128];
RoadMapNeighbour neighbours[2];
int square;
zoom_t context_save_zoom;
RoadMapPosition context_save_pos;
if (alert_location_info){
// check if this alert was tagged as invalid in the past - Could locate a street next to it, eventhough
// its tile already exists.
if (alert_location_info->square_id == ALERTER_SQUARE_ID_INVALID)
return FALSE;
// if alert info is already initialized and has an up to date timestamp, use its info.
if(alert_location_info->square_id!=ALERTER_SQUARE_ID_UNINITIALIZED){
alert_square_time_stamp = roadmap_square_version(alert_location_info->square_id);
if(alert_square_time_stamp==alert_location_info->time_stamp){
pNew_Info->line_id = alert_location_info->line_id;
pNew_Info->square_id = alert_location_info->square_id;
pNew_Info->time_stamp = alert_location_info->time_stamp;
return TRUE;
}
}
}
// did not return so far - so no cached valid location info was found, need to cacluate it.
layers_count = roadmap_layer_all_roads (layers, 128);
if (layers_count > 0) {
square = roadmap_tile_get_id_from_position(0,alert_position);
if(!roadmap_square_set_current(square)){
return FALSE; // do no have this alert's tile, return FALSE;
}
roadmap_math_get_context(&context_save_pos, &context_save_zoom);
roadmap_math_set_context((RoadMapPosition *)alert_position, 2);
count = roadmap_street_get_closest(alert_position, 0, layers, layers_count, 1, &neighbours[0], 2);
roadmap_math_set_context(&context_save_pos, context_save_zoom);
if(count>0&&neighbours[0].distance <= MAX_DISTANCE_FROM_ROAD){
if (count > 1){
int direction;
roadmap_square_set_current(neighbours[0].line.line_id);
direction = roadmap_line_route_get_direction (neighbours[0].line.line_id, ROUTE_CAR_ALLOWED);
if (direction == ROUTE_DIRECTION_WITH_LINE || direction == ROUTE_DIRECTION_AGAINST_LINE){
int line_azymuth;
int delta;
if (direction == ROUTE_DIRECTION_WITH_LINE)
line_azymuth = roadmap_math_azymuth (&neighbours[0].from, &neighbours[0].to);
else
line_azymuth = roadmap_math_azymuth (&neighbours[0].to, &neighbours[0].from);
delta = azymuth_delta(line_azymuth, alert_streering);
if (delta > AZYMUTH_DELTA || delta < (0-AZYMUTH_DELTA)) {
pNew_Info->line_id = neighbours[1].line.line_id;
pNew_Info->square_id = neighbours[1].line.square;
pNew_Info->time_stamp = roadmap_square_version(pNew_Info->square_id);
}
else{
pNew_Info->line_id = neighbours[0].line.line_id;
pNew_Info->square_id = neighbours[0].line.square;
pNew_Info->time_stamp = roadmap_square_version(pNew_Info->square_id);
}
}
else{
pNew_Info->line_id = neighbours[0].line.line_id;
pNew_Info->square_id = neighbours[0].line.square;
pNew_Info->time_stamp = roadmap_square_version(pNew_Info->square_id);
}
}
else{
// found valid information
pNew_Info->line_id = neighbours[0].line.line_id;
pNew_Info->square_id = neighbours[0].line.square;
pNew_Info->time_stamp = roadmap_square_version(pNew_Info->square_id);
}
}else{
// We have the alert's tile, yet still we could not locate it satisfactorily on
// a street. Tag this alert so it will not be checked in the future.
if(alert_location_info){
alert_location_info->square_id = ALERTER_SQUARE_ID_INVALID;
}
return FALSE;
}
}
if(alert_location_info){ // update the information of the alert, for future calls.
alert_location_info->line_id = pNew_Info->line_id;
alert_location_info->square_id = pNew_Info->square_id;
alert_location_info->time_stamp = pNew_Info->time_stamp;
}
return TRUE;
}
static int detect_turn(int point_id,
const RoadMapGpsPosition *gps_position,
TrackNewSegment *new_segment,
int max,
int road_type) {
static int last_straight_line_point = -1;
static int last_straight_azymuth = 0;
int middle1;
int middle2;
if (point_id == 0) return 0;
if (point_id == 1) {
last_straight_line_point = point_id;
last_straight_azymuth =
roadmap_math_azymuth
(track_point_pos (point_id-1),
track_point_pos (point_id));
return 0;
}
waze_assert (point_id > last_straight_line_point);
/* trivial case */
if ((last_straight_line_point == (point_id-1)) &&
(roadmap_math_delta_direction
(roadmap_math_azymuth
(track_point_pos (point_id - 1),
track_point_pos (point_id)),
last_straight_azymuth) > 60)) {
new_segment[0].type = road_type;
new_segment[0].point_id = point_id - 1;
last_straight_line_point = point_id;
last_straight_azymuth =
roadmap_math_azymuth
(track_point_pos (point_id-1),
track_point_pos (point_id));
return 1;
}
if (roadmap_math_delta_direction
(roadmap_math_azymuth
(track_point_pos (point_id - 1),
(RoadMapPosition *)gps_position),
roadmap_math_azymuth
(track_point_pos (point_id - 3),
track_point_pos (point_id - 2))) < 10) {
int this_straight_azymuth;
/* this is a straight line, save it */
this_straight_azymuth =
roadmap_math_azymuth
(track_point_pos (point_id - 3), track_point_pos (point_id));
if (last_straight_line_point == -1) {
last_straight_line_point = point_id;
last_straight_azymuth = this_straight_azymuth;
return 0;
}
if (!find_line_break
(last_straight_line_point, last_straight_azymuth,
point_id-3, &middle1, &middle2)) {
last_straight_line_point = point_id;
last_straight_azymuth = this_straight_azymuth;
return 0;
}
last_straight_line_point = point_id - middle2;
last_straight_azymuth = this_straight_azymuth;
waze_assert (middle1 > 0);
if (middle1 > 0) {
new_segment[0].type = road_type;
new_segment[0].point_id = middle1;
}
if (middle1 != middle2) {
/* make the curve a segment of its own */
new_segment[1].type = TRACK_ROAD_TURN;
new_segment[1].point_id = middle2;
return 2;
} else {
return 1;
}
}
return 0;
}
static int find_line_break (int last_point,
int last_azymuth,
int current_point,
int *middle1,
int *middle2) {
#define MAX_TURN_POINTS 50
angle_pair pairs[MAX_TURN_POINTS];
int i;
int j;
int max_azymuth_diff = 0;
int max_point_azymuth = 0;
int max_azymuth_point = -1;
int current_azymuth;
if ((current_point - last_point + 1) > MAX_TURN_POINTS) return 0;
j=0;
for (i=last_point; i<=current_point; i++) {
int diff;
int prev_azymuth;
current_azymuth =
roadmap_math_azymuth
(track_point_pos (i),
track_point_pos (i+1));
diff = roadmap_math_delta_direction (last_azymuth, current_azymuth);
if (diff > max_azymuth_diff) {
max_azymuth_diff = diff;
}
if (i == last_point) {
prev_azymuth = last_azymuth;
} else {
prev_azymuth =
roadmap_math_azymuth
(track_point_pos (i-1),
track_point_pos (i));
}
diff = roadmap_math_delta_direction (prev_azymuth, current_azymuth);
pairs[j].index = i;
pairs[j].angle = diff;
j++;
if (diff > max_point_azymuth) {
max_point_azymuth = diff;
max_azymuth_point = i;
}
}
qsort (pairs, j, sizeof(angle_pair), angle_pair_cmp);
if (max_azymuth_diff > 45) {
int angle_sum = pairs[0].angle;
int middle1_angle = pairs[0].angle;
int middle2_angle = pairs[0].angle;
*middle1 = pairs[0].index;
*middle2 = pairs[0].index;
j=1;
while ((100 * angle_sum / max_azymuth_diff) < 70) {
waze_assert (j<(current_point-last_point+1));
if (j>=(current_point-last_point+1)) break;
angle_sum += pairs[j].angle;
if (pairs[j].index < *middle1) {
*middle1 = pairs[j].index;
middle1_angle = pairs[j].angle;
} else if (pairs[j].index > *middle2) {
*middle2 = pairs[j].index;
middle2_angle = pairs[j].angle;
}
j++;
}
return 1;
}
return 0;
}