Fix collisionMap (all SimulationMaps)

Assets/Scripts/ExperimentSimulations/ConscientiousReactiveExperiment.cs

@@ -107,8 +107,8 @@ private void Start()
 
             var constraintName = "Global";
             var robotConstraints = constraintsDict[constraintName];
-            var mapConfig = new BuildingMapConfig(123, widthInTiles: mapSize, heightInTiles: mapSize);
-            var mapConfig2 = new BuildingMapConfig(124, widthInTiles: mapSize, heightInTiles: mapSize);
+            var mapConfig = new BuildingMapConfig(123, widthInTiles: mapSize, heightInTiles: mapSize, brokenCollisionMap: false);
+            var mapConfig2 = new BuildingMapConfig(124, widthInTiles: mapSize, heightInTiles: mapSize, brokenCollisionMap: false);
             var algoName = "conscientious_reactive";
             const int robotCount = 1;
             var spawningPosList = new List<Vector2Int>();

Assets/Scripts/ExplorationAlgorithm/Greed/Greed.cs

@@ -132,7 +132,7 @@ public void UpdateLogic()
                 var combinedMessage = receivedHeartbeat.Dequeue();
                 foreach (var message in receivedHeartbeat)
                 {
-                    combinedMessage = combinedMessage.Combine(message);
+                    combinedMessage = combinedMessage.Combine(message, _logicTicks);
                 }
             }
 
@@ -275,10 +275,10 @@ public HeartbeatMessage(SlamMap map)
             _map = map;
         }
 
-        public HeartbeatMessage Combine(HeartbeatMessage heartbeatMessage)
+        public HeartbeatMessage Combine(HeartbeatMessage heartbeatMessage, int tick)
         {
             List<SlamMap> maps = new() { heartbeatMessage._map, _map };
-            SlamMap.Synchronize(maps); //layers of pass by reference, map in controller is updated with the info from message
+            SlamMap.Synchronize(maps, tick); //layers of pass by reference, map in controller is updated with the info from message
             return this;
         }
 

Assets/Scripts/ExplorationAlgorithm/HenrikAlgo/HenrikExplorationAlgorithm.cs

@@ -26,13 +26,16 @@ public class HenrikExplorationAlgorithm : IExplorationAlgorithm
         private Vector2Int? _targetTile;
         private uint _ticksSinceHeartbeat;
 
+        private int _logicTicks;
+
         public void SetController(Robot2DController controller)
         {
             _robotController = controller;
         }
 
         public void UpdateLogic()
         {
+            _logicTicks++;
             ShareSlamMap();
             if (_robotController.GetStatus() == RobotStatus.Idle)
             {
@@ -65,7 +68,7 @@ private void ShareSlamMap()
                 var combinedMessage = receivedHeartbeats[0];
                 foreach (var message in receivedHeartbeats[1..])
                 {
-                    combinedMessage = combinedMessage.Combine(message);
+                    combinedMessage = combinedMessage.Combine(message, _logicTicks);
                 }
             }
             _ticksSinceHeartbeat++;
@@ -91,10 +94,10 @@ public HeartbeatMessage(SlamMap map)
             _map = map;
         }
 
-        public HeartbeatMessage Combine(HeartbeatMessage heartbeatMessage)
+        public HeartbeatMessage Combine(HeartbeatMessage heartbeatMessage, int tick)
         {
             List<SlamMap> maps = new() { heartbeatMessage._map, _map };
-            SlamMap.Synchronize(maps); //layers of pass by reference, map in controller is updated with the info from message
+            SlamMap.Synchronize(maps, tick); //layers of pass by reference, map in controller is updated with the info from message
             return this;
         }
 

Assets/Scripts/ExplorationAlgorithm/Minotaur/Messages/HeartbeatMessage.cs

@@ -48,12 +48,12 @@ public HeartbeatMessage(int ID, SlamMap map, List<Doorway> doorways, Vector2Int
                 this.previousIntersections = previousIntersections;
             }
 
-            public HeartbeatMessage Combine(HeartbeatMessage heartbeatMessage, MinotaurAlgorithm minotaur)
+            public HeartbeatMessage Combine(HeartbeatMessage heartbeatMessage, MinotaurAlgorithm minotaur, int tick)
             {
                 minotaur._otherRobotPositions[heartbeatMessage.ID] = (heartbeatMessage.location, minotaur._waypoint.HasValue ? minotaur._waypoint.Value.Destination : null);
                 minotaur._previousIntersections.UnionWith(heartbeatMessage.previousIntersections);
                 List<SlamMap> maps = new() { heartbeatMessage.map, map };
-                SlamMap.Synchronize(maps); //layers of pass by reference, map in controller is updated with the info from message
+                SlamMap.Synchronize(maps, tick); //layers of pass by reference, map in controller is updated with the info from message
 
                 var amount = heartbeatMessage.doorways.Count;
                 for (var i = 0; i < amount; i++)

Assets/Scripts/ExplorationAlgorithm/Minotaur/MinotaurAlgorithm.cs

@@ -195,9 +195,9 @@ public void UpdateLogic()
             if (receivedHeartbeat.Length > 1)
             {
                 var combinedMessage = receivedHeartbeat[0];
-                foreach (var message in receivedHeartbeat[1..])
+                foreach (var message in receivedHeartbeat.AsSpan(1))
                 {
-                    combinedMessage = combinedMessage.Combine(message, this);
+                    combinedMessage = combinedMessage.Combine(message, this, _logicTicks);
                 }
             }
 
@@ -641,7 +641,7 @@ private List<Line2D> GetWalls(IEnumerable<Vector2Int> tiles)
             var previousDirection = (startPoint.corrected - sortedTiles[1].corrected).GetAngleRelativeToX();
 
             var result = new List<Line2D>();
-            for (var i = 0; i < sortedTiles.Count() - 1; i++)
+            for (var i = 0; i < sortedTiles.Length - 1; i++)
             {
                 var direction = (sortedTiles[i].corrected - sortedTiles[i + 1].corrected).GetAngleRelativeToX();
                 if (previousDirection != direction || map.GetTileStatus(sortedTiles[i].original + CardinalDirection.AngleToDirection(direction).OppositeDirection().Vector) != SlamTileStatus.Solid)

Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/TnfExplorationAlgorithm.cs

@@ -86,6 +86,7 @@ private enum TnfStatus
         private const float AngleDelta = .5f;
         private const float MinimumMoveDistance = .3f;
 
+        private int _logicTicks;
         private int _logicTicksSinceLastCommunication;
         private bool _bonked;
         private readonly System.Random _random;
@@ -189,6 +190,7 @@ private float UtilityFunction(Frontier frontier, float normalizerConstant)
 
         public void UpdateLogic()
         {
+            _logicTicks++;
             _robotPos = _map.GetApproximatePosition();
             _robotPosInt = Vector2Int.RoundToInt(_robotPos);
 
@@ -340,7 +342,7 @@ private void AwaitCommunication()
             // Largest Robot ID synchronizes to save on Simulator CPU time
             if (!received.Cast<(SlamMap, int, Vector2)>().Any(p => p.Item2 > _robotId))
             {
-                SlamMap.Synchronize(newMaps);
+                SlamMap.Synchronize(newMaps, _logicTicks);
             }
             if (_robotTnfStatus == TnfStatus.OutOfFrontiers)
             {

Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/ffTnfExplorationAlgorithm.cs

@@ -85,6 +85,7 @@ private enum TnfStatus
         private const float AngleDelta = .5f;
         private const float MinimumMoveDistance = .3f;
 
+        private int _logicTicks;
         private int _logicTicksSinceLastCommunication;
         private bool _bonked;
         private readonly System.Random _random;
@@ -207,6 +208,7 @@ private float UtilityFunction(Frontier frontier, float normalizerConstant)
 
         public void UpdateLogic()
         {
+            _logicTicks++;
             _robotPos = _map.GetApproximatePosition();
             _robotPosInt = Vector2Int.RoundToInt(_robotPos);
 
@@ -358,7 +360,7 @@ private void AwaitCommunication()
             // Largest Robot ID synchronizes to save on Simulator CPU time
             if (!received.Cast<(ISlamAlgorithm, int, Vector2)>().Any(p => p.Item2 > _robotId))
             {
-                SlamMap.Synchronize(newMaps);
+                SlamMap.Synchronize(newMaps, _logicTicks);
             }
             if (_robotTnfStatus == TnfStatus.OutOfFrontiers)
             {

Assets/Scripts/Map/CoarseGrainedMap.cs

@@ -41,9 +41,8 @@ public class CoarseGrainedMap : IPathFindingMap
         private bool[,] _tilesCoveredStatus;
         private SlamMap.SlamTileStatus[,] _optimisticTileStatuses;
         private HashSet<Vector2Int> _excludedTiles = new(); // This is pretty bad
-        private readonly int _width, _height;
         private readonly Vector2 _offset;
-        private readonly AStar _aStar = new();
+        private readonly MyAStar _aStar = new();
 
         /// <summary>
         /// A lower-resolution map (half the resolution of a <see cref="SlamMap"/>).
@@ -56,8 +55,8 @@ public class CoarseGrainedMap : IPathFindingMap
         public CoarseGrainedMap(SlamMap slamMap, int width, int height, Vector2 offset, bool mapKnown = false)
         {
             _slamMap = slamMap;
-            _width = width;
-            _height = height;
+            Width = width;
+            Height = height;
             _offset = offset;
             _tilesCoveredStatus = new bool[width, height];
             _optimisticTileStatuses = SetTileStatuses(slamMap, width, height, mapKnown);
@@ -89,9 +88,10 @@ public Vector2 GetApproximatePosition()
             return _slamMap.ApproximatePosition - _offset;
         }
 
-        public Vector2Int GetCurrentPosition()
+        public Vector2Int GetCurrentPosition(bool dependOnBrokenBehavior = true)
         {
-            return Vector2Int.FloorToInt(GetApproximatePosition());
+            var approximatePosition = GetApproximatePosition();
+            return dependOnBrokenBehavior ? Vector2Int.FloorToInt(approximatePosition) : Vector2Int.RoundToInt(approximatePosition);
         }
 
         public float GetApproximateGlobalDegrees()
@@ -116,9 +116,9 @@ public RelativePosition GetTileCenterRelativePosition(Vector2Int tileCoord, bool
         {
             var res = new Dictionary<Vector2Int, SlamMap.SlamTileStatus>();
 
-            for (var x = 0; x < _width; x++)
+            for (var x = 0; x < Width; x++)
             {
-                for (var y = 0; y < _height; y++)
+                for (var y = 0; y < Height; y++)
                 {
                     var pos = new Vector2Int(x, y);
                     if (GetTileStatus(pos) != SlamMap.SlamTileStatus.Unseen)
@@ -137,9 +137,9 @@ public List<Vector2Int> GetUnexploredTiles()
         {
             var res = new List<Vector2Int>();
 
-            for (var x = 0; x < _width; x++)
+            for (var x = 0; x < Width; x++)
             {
-                for (var y = 0; y < _height; y++)
+                for (var y = 0; y < Height; y++)
                 {
                     var pos = new Vector2Int(x, y);
                     if (GetTileStatus(pos) == SlamMap.SlamTileStatus.Unseen)
@@ -181,13 +181,13 @@ private void AssertWithinBounds(Vector2Int coordinate)
         {
             if (!IsWithinBounds(coordinate))
             {
-                throw new ArgumentException($"Given coordinate is out of bounds {coordinate} ({_width}, {_height})");
+                throw new ArgumentException($"Given coordinate is out of bounds {coordinate} ({Width}, {Height})");
             }
         }
 
         public bool IsCoordWithinBounds(Vector2Int coordinate)
         {
-            return (coordinate.x >= 0 && coordinate.x < _width && coordinate.y >= 0 && coordinate.y < _height) && !CheckIfAnyIsStatus(coordinate, SlamMap.SlamTileStatus.Solid);
+            return (coordinate.x >= 0 && coordinate.x < Width && coordinate.y >= 0 && coordinate.y < Height) && !CheckIfAnyIsStatus(coordinate, SlamMap.SlamTileStatus.Solid);
         }
 
         // Returns the status of the given tile (Solid, Open or Unseen)
@@ -341,10 +341,11 @@ public Vector2Int GetGlobalNeighbour(CardinalDirection direction)
         /// <param name="target">the target that the path should end at.</param>
         /// <param name="beOptimistic">if <b>true</b>, returns path getting the closest to the target, if no full path can be found.</param>
         /// <param name="beOptimistic">if <b>true</b>, treats unseen tiles as open in the path finding algorithm. Treats unseen tiles as solid otherwise.</param>
-        public Vector2Int[]? GetPath(Vector2Int target, bool beOptimistic = false, bool acceptPartialPaths = false)
+        public Vector2Int[]? GetPath(Vector2Int target, bool beOptimistic = false, bool acceptPartialPaths = false, bool dependOnBrokenBehavior = true)
         {
             var approxPosition = GetApproximatePosition();
-            return _aStar.GetPath(Vector2Int.FloorToInt(approxPosition), target, this, beOptimistic: beOptimistic, acceptPartialPaths: acceptPartialPaths);
+            var position = dependOnBrokenBehavior ? Vector2Int.FloorToInt(approxPosition) : Vector2Int.RoundToInt(approxPosition);
+            return _aStar.GetPath(position, target, this, beOptimistic: beOptimistic, acceptPartialPaths: acceptPartialPaths);
         }
 
         /// <summary>
@@ -407,6 +408,12 @@ public Vector2Int GetGlobalNeighbour(CardinalDirection direction)
                 : _aStar.PathToSteps(path);
         }
 
+        public bool BrokenCollisionMap => _slamMap.BrokenCollisionMap;
+        public int LastUpdateTick => _slamMap.LastUpdateTick;
+        public int Width { get; }
+
+        public int Height { get; }
+
         /// <returns>whether or not a tile at a given position is solid.</returns>
         public bool IsSolid(Vector2Int coordinate)
         {
@@ -451,12 +458,12 @@ public Vector2Int GetCurrentTile()
         public static void Synchronize(List<CoarseGrainedMap> maps, SlamMap.SlamTileStatus[,] newSlamStatuses)
         {
             // Synchronize exploration bool statuses
-            var globalExplorationStatuses = new bool[maps[0]._width, maps[0]._height];
+            var globalExplorationStatuses = new bool[maps[0].Width, maps[0].Height];
             foreach (var map in maps)
             {
-                for (var x = 0; x < map._width; x++)
+                for (var x = 0; x < map.Width; x++)
                 {
-                    for (var y = 0; y < map._height; y++)
+                    for (var y = 0; y < map.Height; y++)
                     {
                         globalExplorationStatuses[x, y] |= map._tilesCoveredStatus[x, y];
                     }
@@ -468,10 +475,10 @@ public static void Synchronize(List<CoarseGrainedMap> maps, SlamMap.SlamTileStat
             }
 
             // Synchronize tile statuses
-            var globalMap = new SlamMap.SlamTileStatus[maps[0]._width, maps[0]._height];
-            for (var x = 0; x < maps[0]._width; x++)
+            var globalMap = new SlamMap.SlamTileStatus[maps[0].Width, maps[0].Height];
+            for (var x = 0; x < maps[0].Width; x++)
             {
-                for (var y = 0; y < maps[0]._height; y++)
+                for (var y = 0; y < maps[0].Height; y++)
                 {
                     var slamX = x * 2;
                     var slamY = y * 2;
@@ -496,12 +503,12 @@ public static void Synchronize(List<CoarseGrainedMap> maps, SlamMap.SlamTileStat
         /// <param name="newSlamStatuses"></param>
         public static void Combine(CoarseGrainedMap map, List<CoarseGrainedMap> others, SlamMap.SlamTileStatus[,] newSlamStatuses)
         {
-            var globalExplorationStatuses = new bool[map._width, map._height];
+            var globalExplorationStatuses = new bool[map.Width, map.Height];
             foreach (var other in others)
             {
-                for (var x = 0; x < other._width; x++)
+                for (var x = 0; x < other.Width; x++)
                 {
-                    for (var y = 0; y < other._height; y++)
+                    for (var y = 0; y < other.Height; y++)
                     {
                         globalExplorationStatuses[x, y] |= other._tilesCoveredStatus[x, y];
                     }
@@ -510,10 +517,10 @@ public static void Combine(CoarseGrainedMap map, List<CoarseGrainedMap> others,
             map._tilesCoveredStatus = (bool[,])globalExplorationStatuses.Clone();
 
             // Synchronize tile statuses
-            var globalMap = new SlamMap.SlamTileStatus[others[0]._width, others[0]._height];
-            for (var x = 0; x < others[0]._width; x++)
+            var globalMap = new SlamMap.SlamTileStatus[others[0].Width, others[0].Height];
+            for (var x = 0; x < others[0].Width; x++)
             {
-                for (var y = 0; y < others[0]._height; y++)
+                for (var y = 0; y < others[0].Height; y++)
                 {
                     var slamX = x * 2;
                     var slamY = y * 2;
@@ -530,7 +537,7 @@ public static void Combine(CoarseGrainedMap map, List<CoarseGrainedMap> others,
 
         public bool IsWithinBounds(Vector2Int coordinate)
         {
-            return coordinate.x >= 0 && coordinate.x < _width && coordinate.y >= 0 && coordinate.y < _height;
+            return coordinate.x >= 0 && coordinate.x < Width && coordinate.y >= 0 && coordinate.y < Height;
         }
 
         /// <returns><b>false</b>, only if the tile at the coordinate is known to be solid.</returns>

Assets/Scripts/Map/MapGen/BitMapGenerator.cs

@@ -37,7 +37,7 @@ public class BitMapGenerator : MapGenerator
         /// <summary>
         /// Method for creating a map from a 2D array of Tiles.
         /// </summary>
-        public SimulationMap<Tile> CreateMapFromBitMap(Tile[,] bitmap, int seed, float wallHeight = 2.0f, int borderSize = 1)
+        public SimulationMap<Tile> CreateMapFromBitMap(Tile[,] bitmap, int seed, float wallHeight = 2.0f, int borderSize = 1, bool brokenCollisionMap = true)
         {
             _bitmap = bitmap;
             _wallHeight = wallHeight;
@@ -64,7 +64,7 @@ public SimulationMap<Tile> CreateMapFromBitMap(Tile[,] bitmap, int seed, float w
             // Create mesh
             var meshGen = GetComponent<MeshGenerator>();
             var collisionMap = meshGen.GenerateMesh((Tile[,])cleanedMap.Clone(), _wallHeight,
-                true, survivingRooms);
+                true, survivingRooms, brokenCollisionMap: brokenCollisionMap);
 
             // Rotate to fit 2D view
             _plane.rotation = Quaternion.AngleAxis(-90, Vector3.right);

Assets/Scripts/Map/MapGen/BuildingGenerator.cs

@@ -76,7 +76,7 @@ public SimulationMap<Tile> GenerateBuildingMap(BuildingMapConfig config, float w
             rooms.ForEach(r => r.OffsetCoordsBy(_config.BorderSize, _config.BorderSize));
             var meshGen = GetComponent<MeshGenerator>();
             var collisionMap = meshGen.GenerateMesh((Tile[,])borderedMap.Clone(), _wallHeight, true,
-                rooms);
+                rooms, _config.BrokenCollisionMap);
 
             // Rotate to fit 2D view
             _plane.rotation = Quaternion.AngleAxis(-90, Vector3.right);
@@ -292,9 +292,9 @@ private List<Room> GetSortedRooms(Tile[,] map)
 
         private Tile[,] CloseOffHallwayEnds(Tile[,] oldMap)
         {
-            var map = oldMap.Clone() as Tile[,];
+            var map = (Tile[,])oldMap.Clone();
 
-            var mapWidth = map!.GetLength(0);
+            var mapWidth = map.GetLength(0);
             var mapHeight = map.GetLength(1);
             var tileType = _type ?? throw new InvalidOperationException("_type is null");
             for (var x = 0; x < mapWidth; x++)

Assets/Scripts/Map/MapGen/BuildingMapConfig.cs

@@ -49,6 +49,8 @@ namespace Maes.Map.MapGen
         public int BorderSize { get; }
         public int WallThickness { get; }
 
+        public bool BrokenCollisionMap { get; }
+
         internal BuildingMapConfig(MaesYamlConfigLoader.MaesConfigType config, int seed) : this(
             randomSeed: seed, wallThickness: config.Map!.BuildingConfig!.WallThickness, widthInTiles: config.Map.WidthInTiles,
             heightInTiles: config.Map.HeightInTiles,
@@ -71,7 +73,8 @@ public BuildingMapConfig(
             uint doorWidth = 2,
             int doorPadding = 2,
             uint roomSplitChancePercent = 85,
-            int borderSize = 1)
+            int borderSize = 1,
+            bool brokenCollisionMap = true)
         {
             if ((2 * doorPadding + doorWidth) > minRoomSideLength)
             {
@@ -98,6 +101,7 @@ public BuildingMapConfig(
             DoorPadding = doorPadding;
             RoomSplitChancePercent = roomSplitChancePercent;
             BorderSize = borderSize;
+            BrokenCollisionMap = brokenCollisionMap;
         }
 
         public bool Equals(BuildingMapConfig other)