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