| | | 1 | | using System.Collections.Generic; |
| | | 2 | | using System.Linq; |
| | | 3 | | using UnityEngine; |
| | | 4 | | |
| | | 5 | | // The cost-based assignment assigns hierarchical objects to each other based on the pairwise costs |
| | | 6 | | // between them. |
| | | 7 | | public class CostBasedAssignment : AssignmentBase { |
| | | 8 | | // Delegate for determining the cost of an assignment between two hierarchical objects. |
| | | 9 | | public delegate float CostDelegate(IHierarchical first, IHierarchical second); |
| | | 10 | | |
| | | 11 | | // Delegate for performing the assignment. |
| | | 12 | | public delegate Plugin.StatusCode AssignDelegate(int numFirst, int numSecond, float[] costs, |
| | | 13 | | int[] assignedFirsts, int[] assignedSeconds, |
| | | 14 | | out int numAssignments); |
| | | 15 | | |
| | | 16 | | // Maximum cost to prevent overflow. |
| | | 17 | | private const float _maxCost = 1e12f; |
| | | 18 | | |
| | | 19 | | private readonly CostDelegate _costFunction; |
| | | 20 | | private readonly AssignDelegate _assignFunction; |
| | | 21 | | |
| | 4 | 22 | | public CostBasedAssignment(CostDelegate costFunction, AssignDelegate assignFunction) { |
| | 2 | 23 | | _costFunction = costFunction; |
| | 2 | 24 | | _assignFunction = assignFunction; |
| | 2 | 25 | | } |
| | | 26 | | |
| | | 27 | | // Run the assignment algorithm and assign the hierarchical objects. |
| | | 28 | | public override List<AssignmentItem> Assign(IReadOnlyList<IHierarchical> first, |
| | 10 | 29 | | IReadOnlyList<IHierarchical> second) { |
| | 10 | 30 | | int numFirst = first.Count; |
| | 10 | 31 | | int numSecond = second.Count; |
| | 14 | 32 | | if (numFirst == 0 || numSecond == 0) { |
| | 4 | 33 | | return new List<AssignmentItem>(); |
| | | 34 | | } |
| | | 35 | | |
| | | 36 | | // Find all pairwise assignment costs. |
| | 6 | 37 | | var assignmentCosts = new float[numFirst * numSecond]; |
| | 159 | 38 | | for (int firstIndex = 0; firstIndex < numFirst; ++firstIndex) { |
| | 287 | 39 | | for (int secondIndex = 0; secondIndex < numSecond; ++secondIndex) { |
| | 63 | 40 | | float cost = _costFunction(first[firstIndex], second[secondIndex]); |
| | 63 | 41 | | float clampedCost = Mathf.Clamp(cost, -_maxCost, _maxCost); |
| | 63 | 42 | | if (cost != clampedCost) { |
| | 0 | 43 | | Debug.LogWarning($"Assignment cost was clamped from {cost} to {clampedCost}."); |
| | 0 | 44 | | } |
| | 63 | 45 | | assignmentCosts[firstIndex * numSecond + secondIndex] = clampedCost; |
| | 63 | 46 | | } |
| | 49 | 47 | | } |
| | | 48 | | |
| | | 49 | | // Solve the assignment problem. |
| | 6 | 50 | | var assignedFirstIndices = new int[numFirst]; |
| | 6 | 51 | | var assignedSecondIndices = new int[numFirst]; |
| | 6 | 52 | | Plugin.StatusCode status = |
| | | 53 | | _assignFunction(numFirst, numSecond, assignmentCosts, assignedFirstIndices, |
| | | 54 | | assignedSecondIndices, out int numAssignments); |
| | 6 | 55 | | if (status != Plugin.StatusCode.StatusOk) { |
| | 0 | 56 | | Debug.LogError( |
| | | 57 | | $"Failed to run the assignment with status code {status}. " + |
| | | 58 | | $"Number of first: {numFirst}, number of second: {numSecond}, " + |
| | | 59 | | $"minimum cost: {assignmentCosts.Min()}, maximum cost: {assignmentCosts.Max()}."); |
| | 0 | 60 | | return new List<AssignmentItem>(); |
| | | 61 | | } |
| | | 62 | | |
| | 6 | 63 | | var assignments = new List<AssignmentItem>(); |
| | 159 | 64 | | for (int i = 0; i < numAssignments; ++i) { |
| | 49 | 65 | | int firstIndex = assignedFirstIndices[i]; |
| | 49 | 66 | | int secondIndex = assignedSecondIndices[i]; |
| | 49 | 67 | | assignments.Add( |
| | | 68 | | new AssignmentItem { First = first[firstIndex], Second = second[secondIndex] }); |
| | 49 | 69 | | } |
| | 6 | 70 | | return assignments; |
| | 10 | 71 | | } |
| | | 72 | | } |