Hungarian Algorithm Optimizations

src/MODULE.bazel

@@ -203,6 +203,14 @@ http_archive(
     url = "https://github.com/saebyn/munkres-cpp/archive/61086fcf5b3a8ad4bda578cdc2d1dca57b548786.tar.gz",
 )
 
+http_archive(
+    name = "lapjv_cpp",
+    build_file = "@//extlibs:lapjv_cpp.BUILD",
+    sha256 = "f4e180eee3eb683e38e4a394060080eabfa49749de7ac5617b55dffb9fee0667",
+    strip_prefix = "LAPJV-algorithm-c-40b5883b7492107e87ba8fb6abeb2aa05591dd34",
+    url = "https://github.com/yongyanghz/LAPJV-algorithm-c/archive/40b5883b7492107e87ba8fb6abeb2aa05591dd34.tar.gz",
+)
+
 http_archive(
     name = "LTC4151",
     build_file = "@//extlibs:LTC4151.BUILD",

src/extlibs/lapjv_cpp.BUILD

@@ -0,0 +1,24 @@
+# Description:
+#   This library provides us with an implementation of the LAPJV algorithm
+#   https://en.wikipedia.org/wiki/Hungarian_algorithm
+#   https://github.com/yongyanghz/LAPJV-algorithm-c/
+
+cc_library(
+    name = "lapjv_cpp",
+    hdrs = glob(
+        [
+            "src/**/*.h",
+        ],
+    ),
+    srcs = glob(
+        [
+            "src/**/*.cpp",
+        ],
+    ),
+    include_prefix = "lapjv/",
+    includes = [
+        "./src",
+    ],
+    strip_include_prefix = "src/",
+    visibility = ["//visibility:public"],
+)

src/software/ai/hl/stp/play/BUILD

@@ -98,7 +98,7 @@ cc_library(
         "//software/ai/passing:pass_with_rating",
         "//software/util/sml_fsm",
         "@boost//:coroutine2",
-        "@munkres_cpp",
+        "@lapjv_cpp",
         "@tracy",
     ],
 )

src/software/ai/hl/stp/play/play.cpp

@@ -1,6 +1,6 @@
 #include "software/ai/hl/stp/play/play.h"
 
-#include <munkres/munkres.h>
+#include <lapjv/lap.h>
 
 #include <Tracy.hpp>
 
@@ -296,7 +296,17 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector,
 
     // The rows of the matrix are the "workers" (the robots) and the columns are the
     // "jobs" (the Tactics).
-    Matrix<double> matrix(num_rows, num_cols);
+    // LAPJV requires a square matrix.
+    size_t dim = std::max(num_rows, num_cols);
+
+    // We use a flat vector to store the cost matrix data, and a vector of pointers
+    // to pass to lapjv as 'double **'.
+    std::vector<double> cost_matrix_data(dim * dim, 0.0);
+    std::vector<double*> assigncost(dim);
+    for(size_t i = 0; i < dim; ++i)
+    {
+        assigncost[i] = &cost_matrix_data[i * dim];
+    }
 
     // Initialize the matrix with the cost of assigning each Robot to each Tactic
     for (size_t row = 0; row < num_rows; row++)
@@ -321,85 +331,80 @@ Play::assignTactics(const WorldPtr &world_ptr, TacticVector tactic_vector,
                 robot_capabilities.begin(), robot_capabilities.end(),
                 std::inserter(missing_capabilities, missing_capabilities.begin()));
 
+            double cost;
             if (missing_capabilities.size() > 0)
             {
                 // We arbitrarily increase the cost, so that robots with missing
                 // capabilities are not assigned
-                matrix(row, col) = robot_cost_for_tactic * 10.0 + 10.0;
+                cost = robot_cost_for_tactic * 10.0 + 10.0;
             }
             else
             {
                 // capability requirements are satisfied, use real cost
-                matrix(row, col) = robot_cost_for_tactic;
+                cost = robot_cost_for_tactic;
             }
+            assigncost[row][col] = cost;
         }
     }
 
-    // Apply the Munkres/Hungarian algorithm to the matrix.
-    Munkres<double> m;
-    m.solve(matrix);
+    // Apply the LAPJV algorithm to the matrix.
+    std::vector<int> rowsol(dim);
+    std::vector<int> colsol(dim);
+    std::vector<double> u(dim);
+    std::vector<double> v(dim);
 
-    // The Munkres matrix gets solved such that there will be exactly one 0 in every
-    // row and exactly one 0 in every column. All other values will be -1. The 0's
-    // indicate the "workers" and "jobs" (robots and tactics for us) that are most
-    // optimally paired together
-    //
-    // Example matrices:
-    //        -1, 0,-1,         and            0,-1,
-    //         0,-1,-1,                       -1, 0,
-    //        -1,-1, 0,
+    lap(dim, assigncost.data(), rowsol.data(), colsol.data(), u.data(), v.data());
+
+    // rowsol[i] is the column assigned to row i.
     for (size_t row = 0; row < num_rows; row++)
     {
-        for (size_t col = 0; col < num_tactics; col++)
+        int col = rowsol[row];
+        // Check if the assigned column is a valid tactic index
+        // If col >= num_tactics, it was assigned to a dummy tactic
+        if (col >= 0 && col < static_cast<int>(num_tactics))
         {
-            auto val = matrix(row, col);
-            if (val == 0)
-            {
-                RobotId robot_id = robots_to_assign.at(row).id();
-                current_tactic_robot_id_assignment.emplace(tactic_vector.at(col),
-                                                           robot_id);
-                tactic_vector.at(col)->setLastExecutionRobot(robot_id);
-
-                auto primitives = primitive_sets.at(col);
-                CHECK(primitives.contains(robot_id))
-                    << "Couldn't find a primitive for robot id " << robot_id;
-
-                // Create the list of obstacles
-                auto motion_constraints = buildMotionConstraintSet(
-                    world_ptr->gameState(), *tactic_vector.at(col));
-
-                // Only generate primitive proto message for the final primitive to robot
-                // assignment
-                auto [traj_path, primitive_proto] =
-                    primitives[robot_id]->generatePrimitiveProtoMessage(
-                        *world_ptr, motion_constraints, robot_trajectories,
-                        obstacle_factory);
-
-                if (traj_path.has_value())
-                {
-                    robot_trajectories.insert_or_assign(robot_id, traj_path.value());
-                }
-                else
-                {
-                    robot_trajectories.erase(robot_id);
-                }
+            RobotId robot_id = robots_to_assign.at(row).id();
+            current_tactic_robot_id_assignment.emplace(tactic_vector.at(col),
+                                                       robot_id);
+            tactic_vector.at(col)->setLastExecutionRobot(robot_id);
 
-                primitives_to_run->mutable_robot_primitives()->insert(
-                    {robot_id, *primitive_proto});
-                remaining_robots.erase(
-                    std::remove_if(remaining_robots.begin(), remaining_robots.end(),
-                                   [robots_to_assign, row](const Robot &robot) {
-                                       return robot.id() == robots_to_assign.at(row).id();
-                                   }),
-                    remaining_robots.end());
-
-                primitives[robot_id]->getVisualizationProtos(obstacle_list,
-                                                             path_visualization);
-                break;
+            auto primitives = primitive_sets.at(col);
+            CHECK(primitives.contains(robot_id))
+                << "Couldn't find a primitive for robot id " << robot_id;
+
+            // Create the list of obstacles
+            auto motion_constraints = buildMotionConstraintSet(
+                world_ptr->gameState(), *tactic_vector.at(col));
+
+            // Only generate primitive proto message for the final primitive to robot
+            // assignment
+            auto [traj_path, primitive_proto] =
+                primitives[robot_id]->generatePrimitiveProtoMessage(
+                    *world_ptr, motion_constraints, robot_trajectories,
+                    obstacle_factory);
+
+            if (traj_path.has_value())
+            {
+                robot_trajectories.insert_or_assign(robot_id, traj_path.value());
             }
+            else
+            {
+                robot_trajectories.erase(robot_id);
+            }
+
+            primitives_to_run->mutable_robot_primitives()->insert(
+                {robot_id, *primitive_proto});
+            remaining_robots.erase(
+                std::remove_if(remaining_robots.begin(), remaining_robots.end(),
+                               [robots_to_assign, row](const Robot &robot) {
+                                   return robot.id() == robots_to_assign.at(row).id();
+                               }),
+                remaining_robots.end());
+
+            primitives[robot_id]->getVisualizationProtos(obstacle_list,
+                                                         path_visualization);
         }
     }
-
     return std::tuple<std::vector<Robot>, std::unique_ptr<TbotsProto::PrimitiveSet>,
                       std::map<std::shared_ptr<const Tactic>, RobotId>>{
         remaining_robots, std::move(primitives_to_run),