diff --git a/native/route_search.cpp b/native/route_search.cpp index da12c20..b023fe6 100644 --- a/native/route_search.cpp +++ b/native/route_search.cpp @@ -100,7 +100,7 @@ struct CurrentAreaSearch { std::vector startNodes; size_t startNodeCounter = 0; std::vector currentAreaSearchResults; - std::set utilizedNodes; + std::map> utilizedNodes; }; // Data @@ -801,18 +801,72 @@ AreaSearchResult continueAreaSearch() { } } - std::set path; - path.insert(farthestNode); + std::map> path; + path[farthestNode] = std::vector(); uint32_t currentNode = farthestNode; while (result.previous.find(currentNode) != result.previous.end()) { currentNode = result.previous.find(currentNode)->second; - path.insert(currentNode); + path[currentNode] = std::vector(); } // Check if our path overlaps too much with already travelled paths - std::vector overlap; - std::set_intersection(path.begin(), path.end(), currentAreaSearch.utilizedNodes.begin(), currentAreaSearch.utilizedNodes.end(), std::back_inserter(overlap)); - if (overlap.size() < 0.5 * path.size()) { + std::map> overlap; + auto inserterIterator = overlap.begin(); + std::set_intersection( + currentAreaSearch.utilizedNodes.begin(), + currentAreaSearch.utilizedNodes.end(), + path.begin(), + path.end(), + std::inserter(overlap, inserterIterator), + currentAreaSearch.utilizedNodes.value_comp() + ); + + std::map overlapCounts; + for (auto it = overlap.begin(); it != overlap.end(); it++) { + for (auto startingNodeIt = it->second.begin(); startingNodeIt != it->second.end(); startingNodeIt++) { + uint32_t startingNode = *startingNodeIt; + if (overlapCounts.find(startingNode) == overlapCounts.end()) { + overlapCounts[startingNode] = 0; + } + overlapCounts[startingNode] = overlapCounts[startingNode] + 1; + } + } + + auto overlapIterator = overlapCounts.begin(); + while (overlapIterator != overlapCounts.end()) { + uint32_t overlapCount = overlapIterator->second; + if (overlapCount < path.size() * 0.5) { + overlapIterator = overlapCounts.erase(overlapIterator); + continue; + } + + // In case we overlap more than 50% with another path, check if the current path is longer + uint32_t otherPath = overlapIterator->first; + bool isLongerThanOtherPath = true; + for (auto otherPathIterator = currentAreaSearch.currentAreaSearchResults.begin(); otherPathIterator != currentAreaSearch.currentAreaSearchResults.end(); otherPathIterator++) { + if (otherPathIterator->nodeId != otherPath) { + continue; + } + + if (farthestDistance > otherPathIterator->longestRoute) { + currentAreaSearch.currentAreaSearchResults.erase(otherPathIterator); + break; + } else { + isLongerThanOtherPath = false; + break; + } + } + + if (isLongerThanOtherPath) { + overlapIterator = overlapCounts.erase(overlapIterator); + continue; + } + + // If we got here, we found another path that shares at least 50% of or route points, that is longer than the current route + break; + } + + if (overlapCounts.size() == 0) { AreaSearchEntry searchEntry; searchEntry.nodeId = currentStartNode; searchEntry.positionX = startNode.position_x; @@ -829,8 +883,12 @@ AreaSearchResult continueAreaSearch() { searchEntry ); - for (uint32_t pathNode : path) { - currentAreaSearch.utilizedNodes.insert(pathNode); + for (std::pair> pathNodeEntry : path) { + uint32_t pathNode = pathNodeEntry.first; + if (currentAreaSearch.utilizedNodes.find(pathNode) == currentAreaSearch.utilizedNodes.end()) { + currentAreaSearch.utilizedNodes[pathNode] = std::vector(); + } + currentAreaSearch.utilizedNodes[pathNode].push_back(currentStartNode); } }