| 62 | } |
| 63 | |
| 64 | std::vector<size_t> Waynet::findWay(const WaynetInstance& waynet, size_t start, size_t end) |
| 65 | { |
| 66 | // FIXME: This is not a very fast implementation. Improve! |
| 67 | // Simple Dijkstra-Implementation. Totally non-optimized. |
| 68 | |
| 69 | //LogInfo() << "Entered function: " << start; |
| 70 | |
| 71 | // Give all other nodes a distance of infinity |
| 72 | std::vector<float> distances(waynet.waypoints.size(), FLT_MAX); |
| 73 | std::vector<size_t> prev(waynet.waypoints.size(), static_cast<size_t>(-1)); |
| 74 | std::set<size_t> unvisitedSet; |
| 75 | |
| 76 | for (size_t i = 0; i < waynet.waypoints.size(); i++) |
| 77 | unvisitedSet.insert(i); |
| 78 | |
| 79 | // Init startnode with a distance of 0 |
| 80 | distances[start] = 0.0f; |
| 81 | size_t cn = start; |
| 82 | |
| 83 | //LogInfo() << "Starting: " << cn; |
| 84 | |
| 85 | do |
| 86 | { |
| 87 | for (size_t e : waynet.waypoints[cn].edges) |
| 88 | { |
| 89 | if (unvisitedSet.find(e) != unvisitedSet.end()) |
| 90 | { |
| 91 | // Check if this actually was a shorter path |
| 92 | float tentativeDist = |
| 93 | distances[cn] + (waynet.waypoints[cn].position - waynet.waypoints[e].position).lengthSquared(); |
| 94 | if (distances[e] > tentativeDist) |
| 95 | { |
| 96 | distances[e] = tentativeDist; |
| 97 | prev[e] = cn; |
| 98 | } |
| 99 | } |
| 100 | } |
| 101 | |
| 102 | //LogInfo() << "Visited: " << cn; |
| 103 | |
| 104 | unvisitedSet.erase(cn); |
| 105 | |
| 106 | if (!unvisitedSet.empty()) |
| 107 | { |
| 108 | size_t smallest = *unvisitedSet.begin(); |
| 109 | for (size_t n : unvisitedSet) |
| 110 | { |
| 111 | if (distances[smallest] > distances[n]) |
| 112 | { |
| 113 | smallest = n; |
| 114 | } |
| 115 | } |
| 116 | |
| 117 | cn = smallest; |
| 118 | } |
| 119 | |
| 120 | } while (unvisitedSet.find(end) != unvisitedSet.end() && cn != static_cast<size_t>(-1) && cn != end); |
| 121 | |