bool SearchSpace::removeNode(geometry_msgs::Pose nodePose)
{
SearchSpaceNode *temp = searchspace;
// Empty
if(!temp)
return false;
// if it's the first node, simply remove it
if(samePosition(nodePose,temp->location))
{
searchspace = temp->next;
delete temp;
return true;
}
while (temp->next != NULL)
{
if(samePosition(nodePose,temp->next->location))
{
SearchSpaceNode *toDelete = temp->next;
temp->next = temp->next->next;
delete toDelete;
return true;
}
temp = temp->next;
}
// Not found in the list
return false;
}
SearchSpaceNode * SearchSpace::nodeExists(geometry_msgs::Pose nodePose)
{
SearchSpaceNode *temp = searchspace;
while (temp != NULL)
{
if(samePosition(nodePose,temp->location))
return temp;
temp = temp->next;
}
// node with the given location does not exist
return NULL;
}
int LostInJingAnTemple::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QMainWindow::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: on_actionImportData_triggered(); break;
case 1: on_actionConvert_To_Weighted_Graph_triggered(); break;
case 2: on_actionFindShortestPath_triggered(); break;
case 3: on_actionExportData_triggered(); break;
case 4: on_actionAbout_triggered(); break;
case 5: { float _r = distanceBetweenTwoPoints((*reinterpret_cast< Point(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])));
if (_a[0]) *reinterpret_cast< float*>(_a[0]) = _r; } break;
case 6: { bool _r = onTheBuildingEdge((*reinterpret_cast< Building(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 7: { bool _r = isTheBuildingEdge((*reinterpret_cast< Building(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 8: { bool _r = crossTheBuilding((*reinterpret_cast< Building(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 9: { bool _r = crossTheBuildings((*reinterpret_cast< vector<Building>(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 10: { bool _r = crossLines((*reinterpret_cast< Point(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])),(*reinterpret_cast< Point(*)>(_a[4])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 11: { bool _r = samePosition((*reinterpret_cast< Point(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 12: { bool _r = isPointInsideBuilding((*reinterpret_cast< Building(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 13: { bool _r = isLineInsideBuilding((*reinterpret_cast< Building(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 14: { bool _r = isLineFullInsideBuilding((*reinterpret_cast< Building(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 15: { bool _r = isTwoLinesParalleled((*reinterpret_cast< Point(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])),(*reinterpret_cast< Point(*)>(_a[4])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
case 16: { bool _r = isOnLine((*reinterpret_cast< Point(*)>(_a[1])),(*reinterpret_cast< Point(*)>(_a[2])),(*reinterpret_cast< Point(*)>(_a[3])));
if (_a[0]) *reinterpret_cast< bool*>(_a[0]) = _r; } break;
default: ;
}
_id -= 17;
}
return _id;
}
/**
* Finds a line ID given 2 GPS points
* @param pTrafficInfo - pointer to traffic info
* @param start - first node point
* @param end - second node point
* @param Line - point to line to return the found line
* @param allowedDeviation - allowed deviation when searching the nodes
* @return TRUE - If a line is found
*/
static BOOL RTTrafficInfo_Get_LineId(RTTrafficInfo *pTrafficInfo, RoadMapPosition *start, RoadMapPosition *end, char *name, PluginLine *Line, int allowedDeviation){
RoadMapNeighbour neighbours_start[6], neighbours_end[6];
int count_start, count_end = 0;
int layers[128];
int layers_count;
RoadMapPosition context_save_pos;
int context_save_zoom;
RoadMapPosition from;
RoadMapPosition to;
int i;
roadmap_math_get_context(&context_save_pos, &context_save_zoom);
layers_count = roadmap_layer_all_roads(layers, 128);
roadmap_math_set_context(start, 20);
count_start = roadmap_street_get_closest(start, 0, layers, layers_count,
1, &neighbours_start[0], 5);
// first try to find it in the same segment
for (i=0; i<count_start; i++){
RoadMapStreetProperties properties;
const char *street_name;
roadmap_square_set_current(neighbours_start[i].line.square);
roadmap_street_get_properties(neighbours_start[i].line.line_id, &properties);
street_name = roadmap_street_get_street_fename(&properties);
if (strcmp(street_name, pTrafficInfo->sStreet))
continue;
roadmap_plugin_line_from (&neighbours_start[i].line, &from);
roadmap_plugin_line_to (&neighbours_start[i].line, &to);
if ( ( samePosition(&from, start,allowedDeviation) && samePosition(&to, end,allowedDeviation)) || (samePosition(&from, end,allowedDeviation) && samePosition(&to, start,allowedDeviation)) ){
if (samePosition(&from, start,allowedDeviation))
pTrafficInfo->iDirection = ROUTE_DIRECTION_WITH_LINE;
else
pTrafficInfo->iDirection = ROUTE_DIRECTION_AGAINST_LINE;
roadmap_math_set_context(&context_save_pos, context_save_zoom);
*Line = neighbours_start[i].line;
return TRUE;
}
}
//if not try to extend the line on the starting point
for (i=0; i<count_start; i++){
RoadMapStreetProperties properties;
const char *street_name;
roadmap_square_set_current(neighbours_start[i].line.square);
roadmap_street_get_properties(neighbours_start[i].line.line_id, &properties);
street_name = roadmap_street_get_street_fename(&properties);
if (strcmp(street_name, pTrafficInfo->sStreet)){
continue;
}
roadmap_street_extend_line_ends (&neighbours_start[i].line, &from, &to, FLAG_EXTEND_BOTH, NULL, NULL);
if ( ( samePosition(&from, start,allowedDeviation) && samePosition(&to, end,allowedDeviation)) || (samePosition(&from, end,allowedDeviation) && samePosition(&to, start,allowedDeviation)) ){
if (samePosition(&from, start,allowedDeviation))
pTrafficInfo->iDirection = ROUTE_DIRECTION_WITH_LINE;
else
pTrafficInfo->iDirection = ROUTE_DIRECTION_AGAINST_LINE;
roadmap_street_extend_line_ends (&neighbours_start[i].line, &from, &to, FLAG_EXTEND_BOTH, extendCallBack, (void *)pTrafficInfo);
roadmap_math_set_context(&context_save_pos, context_save_zoom);
*Line = neighbours_start[i].line;
return TRUE;
}
}
roadmap_math_set_context(end, 20);
count_end = roadmap_street_get_closest(end, 0, layers, layers_count,
1, &neighbours_end[0], 4);
for (i=0; i<count_end; i++){
RoadMapStreetProperties properties;
const char *street_name;
roadmap_square_set_current(neighbours_end[i].line.square);
roadmap_street_get_properties(neighbours_end[i].line.line_id, &properties);
street_name = roadmap_street_get_street_fename(&properties);
if (strcmp(street_name, pTrafficInfo->sStreet)){
continue;
}
roadmap_street_extend_line_ends (&neighbours_end[i].line, &from, &to, FLAG_EXTEND_BOTH, NULL, NULL);
if ( ( samePosition(&from, start,allowedDeviation) && samePosition(&to, end,allowedDeviation)) || (samePosition(&from, end,allowedDeviation) && samePosition(&to, start,allowedDeviation)) ){
if (samePosition(&to, end,allowedDeviation))
pTrafficInfo->iDirection = ROUTE_DIRECTION_WITH_LINE;
else
pTrafficInfo->iDirection = ROUTE_DIRECTION_AGAINST_LINE;
roadmap_street_extend_line_ends (&neighbours_end[i].line, &from, &to, FLAG_EXTEND_BOTH, extendCallBack, (void *)pTrafficInfo);
roadmap_math_set_context(&context_save_pos, context_save_zoom);
*Line = neighbours_end[i].line;
return TRUE;
}
}
roadmap_math_set_context(&context_save_pos, context_save_zoom);
return FALSE;
}
