I'm working on a game prototype. I need to associate N starting positions to N ending positions while minimizing the sum of each distance.
I'm currently on a 2D projet, but this problem also applies to 3D (or higher dimension).
In university, I learned an algorithm that do more or less that. It's called Branch and Bound (wikipedia article).
The algorithm is perfect in term of giving the good result. I was skeptical at first, but it works. But, my implementation is to slow for realtime usage. For a N of 10, the algorithm take about 8 to 10 seconds to perform. I don't know why it is that slow and I want to optimise it.
Here is my test implementation in C++
#include <iostream>
#include <utility>
#include <vector>
#include <algorithm>
#include <limits>
#include <queue>
#include <tuple>
#include <cmath>
const float Epsilon = 0.0005f;
class HashMap {
private:
size_t* map;
size_t size;
public:
explicit HashMap(size_t size) {
this->size = size;
map = new size_t[size];
for (auto i = 0; i < size; ++i) {
map[i] = -1;
}
}
~HashMap() {
delete map;
}
void clear() {
for (auto i = 0; i < size; ++i) {
map[i] = -1;
}
}
void insert(size_t key, size_t value) {
map[key] = value;
}
size_t get(size_t key) {
return map[key];
}
void remove(size_t key) {
map[key] = -1;
}
};
struct Vector2 {
float x, y; // Assuming a 2D vector
};
float distance(const Vector2& v1, const Vector2& v2) {
float dx = v1.x - v2.x;
float dy = v1.y - v2.y;
return std::sqrt(dx * dx + dy * dy);
}
float getBestLineSolution(HashMap& usedValues, size_t size, const float* distances, size_t line) {
auto best = std::numeric_limits<float>::max();
for (auto i = 0; i < size; i++) {
if (usedValues.get(i) == -1 && distances[line * size + i] < best) {
best = distances[line * size + i];
}
}
return best;
}
float getRemainingScore(HashMap& usedValues, size_t size, const float* distances, size_t line) {
auto remainingScore = 0.0f;
for (auto i = line; i < size; i++) {
remainingScore += getBestLineSolution(usedValues, size, distances, i);
}
return remainingScore;
}
float getTotalHeuristic(std::tuple<float, float, std::vector<size_t>> a) {
// 0 is the true weight up until now and 1 is the heuristics
return std::get<0>(a) + std::get<1>(a);
}
struct CustomCompare {
bool operator()(std::tuple<float, float, std::vector<size_t>> a, std::tuple<float, float, std::vector<size_t>> b) {
// Sort by cost
return getTotalHeuristic(std::move(a)) > getTotalHeuristic(std::move(b));
}
};
std::vector<size_t> branchAndBoundIterative(const std::vector<Vector2>& currentPosition, std::vector<Vector2>& finalPositions) {
auto size = currentPosition.size();
auto usedPosition = HashMap(size);
float distances[size * size];
auto minHeap = std::priority_queue<std::tuple<float, float, std::vector<size_t>>,
std::vector<std::tuple<float, float, std::vector<size_t>>>, CustomCompare>();
// Initialize the matrix with the distances
for (auto i = 0; i < size; i++) {
for (auto j = 0; j < size; j++) {
distances[i * size + j] = distance(currentPosition[i], finalPositions[j]);
}
}
// Getting the threshold
auto threshold = 0.0f;
for (auto i = 0; i < size; i++) {
threshold += distances[i * size + i];
}
// Getting all first node whose cost is under the threshold
for (size_t i = 0; i < size; i++) {
auto dist = distances[i];
auto heuristic = getRemainingScore(usedPosition, size, distances, 1);
if (dist + heuristic <= (threshold + Epsilon)) {
auto v = std::vector<size_t>();
v.push_back(i);
minHeap.emplace(dist, heuristic, v);
}
}
// Doing the algorithm
// This is part is really slow
auto bestTuple = minHeap.top();
do {
// Lower cost path (called current path in the rest of the algorithm)
auto currentTuple = minHeap.top();
minHeap.pop();
usedPosition.clear();
auto positions = std::get<2>(currentTuple);
auto posSize = positions.size();
if (posSize == size) {
// If the current path (in the matrix) contains all elements
auto totalDistance = std::get<0>(currentTuple);
if (totalDistance <= (threshold + Epsilon)) {
threshold = totalDistance;
bestTuple = currentTuple;
}
} else {
// If the current path (in the matrix) doesn't contain all elements
for (auto i = 0; i < posSize; i++) {
usedPosition.insert(positions[i], i);
}
auto currentDist = std::get<0>(currentTuple);
// Getting the next best result for the next line in the current path
for (auto i = 0; i < size; i++) {
if (usedPosition.get(i) == -1) {
auto newDist = currentDist + distances[posSize * size + i];
auto heuristic = getRemainingScore(usedPosition, size, distances, posSize + 1);
if (newDist + heuristic <= (threshold + Epsilon)) {
auto positionCopy = std::vector<size_t>(positions);
positionCopy.push_back(i);
minHeap.emplace(newDist, heuristic, positionCopy);
}
}
}
}
} while (!minHeap.empty() && getTotalHeuristic(minHeap.top()) <= (threshold + Epsilon));
return std::get<2>(bestTuple);
}
int main() {
std::vector<Vector2> currentPosition = { {0, 0},
{1, 0},
{2, 0},
{3, 0},
{4, 0},
{5, 0},
{6, 0},
{7, 0},
{8, 0},
{9, 0}
};
std::vector<Vector2> finalPosition = { {-4, 5},
{-3, 5},
{-2, 5},
{-1, 5},
{0, 5},
{1, 5},
{2, 5},
{3, 5},
{4, 5},
{5, 5}
};
std::vector<size_t> bestFinalPosition = branchAndBoundIterative(currentPosition, finalPosition);
// Output the result
std::cout << "Optimal final positions:" << std::endl;
for (auto i = 0; i < currentPosition.size(); i++) {
auto pos = finalPosition[bestFinalPosition[i]];
std::cout << "(" << pos.x << ", " << pos.y << ") ";
}
return 0;
}
The core algorithm is implemented in the branchAndBoundIterative function. From what I've been able to test, the do while loop seems to be the bottleneck of the algorithm.
The "true" output is the following Optimal final positions: (-4, 5) (-3, 5) (-2, 5) (-1, 5) (0, 5) (1, 5) (2, 5) (3, 5) (4, 5) (5, 5)