From cc3df00fd460323fe55305369506e08c370afce3 Mon Sep 17 00:00:00 2001 From: Mads Mogensen Date: Mon, 25 Nov 2024 14:52:33 +0100 Subject: [PATCH 1/2] Fix collisionMap (all SimulationMaps) Before it did some strange things with SquareGrid that was really broken as the map did not align to CoarseMap tiles which made many places impossible for the robot to navigate to. Now it has been fixed by bypassing the squaregrid stuff and just using the Tile[,] map to create the collision map. Fixes a bug in cave map where the PatrollingMap threw an exception due to Astar being unable to path to the waypoint. Also when the robot collides with something it switches from using the precomupted astar paths to the robotcontroller path and move to functionality to hopefully navigate around the obstacle. We now calculate paths both ways from a waypoint to its neighbor and back. This sometime creates two different paths (of the same length) which helps with robots colliding with each other. Co-Authored-By: Casper-NS <37834568+Casper-NS@users.noreply.github.com> --- .../Scripts/Map/MapGen/BuildingGenerator.cs | 4 +- Assets/Scripts/Map/MapGen/MeshGenerator.cs | 31 ++++++++-------- Assets/Scripts/Map/MapGen/Tile.cs | 2 + Assets/Scripts/Map/PathFinding/AStar.cs | 10 +---- Assets/Scripts/Map/PatrollingMap.cs | 19 ---------- .../PatrollingAlgorithm.cs | 37 +++++++++++++++++-- Assets/Scripts/Robot/IRobotController.cs | 2 +- Assets/Scripts/Robot/Robot2DController.cs | 7 ++-- .../Statistics/PatrollingVisualizer.cs | 4 +- 9 files changed, 61 insertions(+), 55 deletions(-) diff --git a/Assets/Scripts/Map/MapGen/BuildingGenerator.cs b/Assets/Scripts/Map/MapGen/BuildingGenerator.cs index 54bf08a1..1d8773ef 100644 --- a/Assets/Scripts/Map/MapGen/BuildingGenerator.cs +++ b/Assets/Scripts/Map/MapGen/BuildingGenerator.cs @@ -292,9 +292,9 @@ private List 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++) diff --git a/Assets/Scripts/Map/MapGen/MeshGenerator.cs b/Assets/Scripts/Map/MapGen/MeshGenerator.cs index 6d1682fe..cc8d04d6 100644 --- a/Assets/Scripts/Map/MapGen/MeshGenerator.cs +++ b/Assets/Scripts/Map/MapGen/MeshGenerator.cs @@ -155,7 +155,7 @@ internal SimulationMap GenerateMesh(Tile[,] map, float wallHeight, Generate2DColliders(); - return GenerateCollisionMap(_squareGrid2D, + return GenerateCollisionMap(_squareGrid2D, map, new Vector2(_squareGrid2D.XOffset, _squareGrid2D.YOffset), disableCornerRounding, rooms); } @@ -183,11 +183,11 @@ private void CreateRoofMesh() WallRoof.mesh = wallRoofMesh; } - private static SimulationMap GenerateCollisionMap(SquareGrid squareGrid, Vector3 offset, + private static SimulationMap GenerateCollisionMap(SquareGrid squareGrid, Tile[,] tileMap, Vector3 offset, bool removeRoundedCorners, List rooms) { - var width = squareGrid.Squares.GetLength(0); - var height = squareGrid.Squares.GetLength(1); + var width = tileMap.GetLength(0); + var height = tileMap.GetLength(1); // Create a bool type SimulationMap with default value of false in all cells var collisionMap = new SimulationMap(() => new Tile(TileType.Room), width, height, offset, rooms); @@ -195,11 +195,12 @@ private static SimulationMap GenerateCollisionMap(SquareGrid squareGrid, V { for (var y = 0; y < height; y++) { - var square = squareGrid.Squares[x, y]; - var collisionTile = collisionMap.GetTileByLocalCoordinate(x, y); - // Create triangles from all the points in the squares - // assigned to variables "vertices" and "triangles" - AdaptCollisionMapTile(collisionTile, square, removeRoundedCorners); + var tile = collisionMap.GetTileByLocalCoordinate(x, y); + + for (var i = 0; i < 8; i++) + { + tile.SetCellValue(i, tileMap[x, y]); + } } } @@ -785,7 +786,7 @@ public bool Contains(int vertexIndex) } } - private class SquareGrid + private sealed class SquareGrid { public readonly Square[,] Squares; public readonly float XOffset, YOffset; @@ -802,8 +803,8 @@ public SquareGrid(Tile[,] map) var controlNodes = new ControlNode[nodeCountX, nodeCountY]; // In Marching squares, squares are offset by 0.5 - XOffset = -mapWidth / 2 + 0.5f; - YOffset = -mapHeight / 2 + 0.5f; + XOffset = -mapWidth / 2f + 0.5f; + YOffset = -mapHeight / 2f + 0.5f; for (var x = 0; x < nodeCountX; x++) { @@ -829,7 +830,7 @@ public SquareGrid(Tile[,] map) } } - internal class Square + private sealed class Square { // This class is used in the marching squares algorithm. // Control nodes can be either on or off @@ -897,7 +898,7 @@ public Square(ControlNode topLeft, ControlNode topRight, ControlNode bottomRight } } - internal class Node + private class Node { public Vector3 Position; public int VertexIndex = -1; @@ -916,7 +917,7 @@ public Node(Node node, Vector3 position) } } - internal class ControlNode : Node + private sealed class ControlNode : Node { public readonly bool IsWall; public readonly Node Above, Right; diff --git a/Assets/Scripts/Map/MapGen/Tile.cs b/Assets/Scripts/Map/MapGen/Tile.cs index b528fe99..e3d3253a 100644 --- a/Assets/Scripts/Map/MapGen/Tile.cs +++ b/Assets/Scripts/Map/MapGen/Tile.cs @@ -21,6 +21,7 @@ using System; using System.Collections.Generic; +using System.Diagnostics; namespace Maes.Map.MapGen { @@ -34,6 +35,7 @@ public enum TileType Brick } + [DebuggerDisplay("{Type}")] public readonly struct Tile { public readonly TileType Type; diff --git a/Assets/Scripts/Map/PathFinding/AStar.cs b/Assets/Scripts/Map/PathFinding/AStar.cs index 9818add3..5330fdd3 100644 --- a/Assets/Scripts/Map/PathFinding/AStar.cs +++ b/Assets/Scripts/Map/PathFinding/AStar.cs @@ -106,8 +106,7 @@ public Vector2Int[] Path() if (currentCoordinate == targetCoordinate) { - var path = currentTile.Path(); - return path; + return currentTile.Path(); } foreach (var dir in CardinalDirection.GetCardinalAndOrdinalDirections()) @@ -208,13 +207,6 @@ private static bool IsSolid(Vector2Int coord, IPathFindingMap map, bool optimist ? map.IsOptimisticSolid(coord) : map.IsSolid(coord); } - private bool IsAnyNeighborOpen(Vector2Int targetCoordinate, IPathFindingMap pathFindingMap, bool optimistic) - { - return !IsSolid(targetCoordinate + Vector2Int.up + Vector2Int.left, pathFindingMap, optimistic) || !IsSolid(targetCoordinate + Vector2Int.up, pathFindingMap, optimistic) || - !IsSolid(targetCoordinate + Vector2Int.left, pathFindingMap, optimistic) || !IsSolid(targetCoordinate + Vector2Int.up + Vector2Int.right, pathFindingMap, optimistic) || - !IsSolid(targetCoordinate + Vector2Int.right, pathFindingMap, optimistic) || !IsSolid(targetCoordinate + Vector2Int.down + Vector2Int.left, pathFindingMap, optimistic) || - !IsSolid(targetCoordinate + Vector2Int.down, pathFindingMap, optimistic) || !IsSolid(targetCoordinate + Vector2Int.down + Vector2Int.right, pathFindingMap, optimistic); - } private static float OctileHeuristic(Vector2Int from, Vector2Int to) { diff --git a/Assets/Scripts/Map/PatrollingMap.cs b/Assets/Scripts/Map/PatrollingMap.cs index b393a422..2673b1dd 100644 --- a/Assets/Scripts/Map/PatrollingMap.cs +++ b/Assets/Scripts/Map/PatrollingMap.cs @@ -60,33 +60,14 @@ public PatrollingMap Clone() { foreach (var neighbor in vertex.Neighbors) { - if (paths.ContainsKey((vertex.Id, neighbor.Id))) - { - continue; - } - var path = aStar.GetOptimisticPath(vertex.Position, neighbor.Position, coarseMap) ?? throw new InvalidOperationException("No path from vertex to neighbor"); var pathSteps = AStar.PathToStepsCheap(path).ToArray(); paths.Add((vertex.Id, neighbor.Id), pathSteps); - paths.Add((neighbor.Id, vertex.Id), ReversePathSteps(pathSteps)); } } return paths; } - - private static PathStep[] ReversePathSteps(PathStep[] pathSteps) - { - var reversedPathSteps = new PathStep[pathSteps.Length]; - for (var i = 0; i < pathSteps.Length; i++) - { - var originalPathStep = pathSteps[i]; - var reversedPathStep = new PathStep(originalPathStep.End, originalPathStep.Start, null!); // HACK: set to null to avoid allocations - reversedPathSteps[pathSteps.Length - i - 1] = reversedPathStep; - } - - return reversedPathSteps; - } } } \ No newline at end of file diff --git a/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs b/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs index 4cbcd213..d398059b 100644 --- a/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs +++ b/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs @@ -33,6 +33,8 @@ public abstract class PatrollingAlgorithm : IPatrollingAlgorithm // Set by SetController private Robot2DController _controller = null!; + private bool _hasCollided; + private bool _firstCollision; protected event OnReachVertex? OnReachVertexHandler; @@ -78,17 +80,40 @@ public virtual void UpdateLogic() return; } - if (_currentPath.Count != 0) + if (_controller.IsCurrentlyColliding) + { + _firstCollision = !_hasCollided; + _hasCollided = true; + } + + if (_hasCollided) { - if (_controller.IsCurrentlyColliding) + // Do default AStar + _currentPath.Clear(); + var currentPosition = _controller.SlamMap.CoarseMap.GetCurrentPosition(); + if (currentPosition != TargetVertex.Position) { - _controller.PathAndMoveTo(TargetVertex.Position); + if (_firstCollision) + { + _controller.StopCurrentTask(); + } + _controller.PathAndMoveTo(TargetVertex.Position, dependOnBrokenBehaviour: false); } else { - PathAndMoveToTarget(); + _hasCollided = false; + SetNextVertex(); } + + _firstCollision = false; + + return; + } + + if (_currentPath.Count != 0) + { + PathAndMoveToTarget(); return; } @@ -112,6 +137,10 @@ public virtual string GetDebugInfo() .AppendLine(AlgorithmName) .Append("Target vertex position: ") .AppendLine(TargetVertex.Position.ToString()) + .Append("Has Collided: ") + .Append(_hasCollided) + .Append(" First Collision: ") + .AppendLine(_firstCollision.ToString()) .ToString(); } diff --git a/Assets/Scripts/Robot/IRobotController.cs b/Assets/Scripts/Robot/IRobotController.cs index fdbb4a3a..7b4b8756 100644 --- a/Assets/Scripts/Robot/IRobotController.cs +++ b/Assets/Scripts/Robot/IRobotController.cs @@ -79,7 +79,7 @@ public interface IRobotController /// Must be called continuously untill the final target is reached /// /// COARSEGRAINED tile as final target - void PathAndMoveTo(Vector2Int tile); + void PathAndMoveTo(Vector2Int tile, bool dependOnBrokenBehaviour = true); /// /// Estimates the time of arrival for the robot to reach the specified destination. diff --git a/Assets/Scripts/Robot/Robot2DController.cs b/Assets/Scripts/Robot/Robot2DController.cs index 862b5b61..7e7d109b 100644 --- a/Assets/Scripts/Robot/Robot2DController.cs +++ b/Assets/Scripts/Robot/Robot2DController.cs @@ -330,6 +330,7 @@ protected void AssertRobotIsInIdleState(string attemptedActionName) public void StopCurrentTask() { _currentTask = null; + _currentPath.Clear(); } public void Broadcast(object data) @@ -416,7 +417,7 @@ public void Move(float distanceInMeters, bool reverse = false) /// If there is already a path, does not recompute /// /// COARSEGRAINED tile as final target - public void PathAndMoveTo(Vector2Int tile) + public void PathAndMoveTo(Vector2Int tile, bool dependOnBrokenBehaviour = true) { if (GetStatus() != RobotStatus.Idle) { @@ -450,7 +451,7 @@ public void PathAndMoveTo(Vector2Int tile) _currentTarget = _currentPath.Dequeue(); } - var relativePosition = SlamMap.CoarseMap.GetTileCenterRelativePosition(_currentTarget); + var relativePosition = SlamMap.CoarseMap.GetTileCenterRelativePosition(_currentTarget, dependOnBrokenBehaviour: dependOnBrokenBehaviour); if (relativePosition.Distance < 0.5f) { if (_currentPath.Count == 0) @@ -459,7 +460,7 @@ public void PathAndMoveTo(Vector2Int tile) } _currentTarget = _currentPath.Dequeue(); - relativePosition = SlamMap.CoarseMap.GetTileCenterRelativePosition(_currentTarget); + relativePosition = SlamMap.CoarseMap.GetTileCenterRelativePosition(_currentTarget, dependOnBrokenBehaviour: dependOnBrokenBehaviour); } #region DrawPath #if DEBUG diff --git a/Assets/Scripts/Statistics/PatrollingVisualizer.cs b/Assets/Scripts/Statistics/PatrollingVisualizer.cs index cafb4423..94c16e08 100644 --- a/Assets/Scripts/Statistics/PatrollingVisualizer.cs +++ b/Assets/Scripts/Statistics/PatrollingVisualizer.cs @@ -29,9 +29,9 @@ public void SetLineOfSightVertices(SimulationMap simulationMap, Patrolling LineOfSightVertices.CreateLineOfSightVertices(); } - public override void SetSimulationMap(SimulationMap simulationMap, Vector3 offset) + public override void SetSimulationMap(SimulationMap newMap, Vector3 offset) { - base.SetSimulationMap(simulationMap, Vector3.zero); + base.SetSimulationMap(newMap, Vector3.zero); foreach (var visualizer in _visualizerObjects) { From 3086311f4380462ea8c2425f732a922d105f57e7 Mon Sep 17 00:00:00 2001 From: Mads Mogensen Date: Fri, 29 Nov 2024 12:38:17 +0100 Subject: [PATCH 2/2] Add Custom AStar implementation The current implementation is broken and paths through walls when they are 1 tile thick, which happens in our non-broken collisionmap. Also fix issues with the PatrollingAlgorithm going off in a strange direction. --- .../ConscientiousReactiveExperiment.cs | 4 +- .../ExplorationAlgorithm/Greed/Greed.cs | 6 +- .../HenrikAlgo/HenrikExplorationAlgorithm.cs | 9 +- .../Minotaur/Messages/HeartbeatMessage.cs | 4 +- .../Minotaur/MinotaurAlgorithm.cs | 6 +- .../TnfExplorationAlgorithm.cs | 4 +- .../ffTnfExplorationAlgorithm.cs | 4 +- Assets/Scripts/Map/CoarseGrainedMap.cs | 61 +++++++------ Assets/Scripts/Map/MapGen/BitMapGenerator.cs | 4 +- .../Scripts/Map/MapGen/BuildingGenerator.cs | 2 +- .../Scripts/Map/MapGen/BuildingMapConfig.cs | 6 +- Assets/Scripts/Map/MapGen/CaveGenerator.cs | 2 +- Assets/Scripts/Map/MapGen/CaveMapConfig.cs | 6 +- Assets/Scripts/Map/MapGen/MapSpawner.cs | 4 +- Assets/Scripts/Map/MapGen/MeshGenerator.cs | 31 ++++--- .../Map/PathFinding/IPathFindingMap.cs | 8 +- .../Map/PathFinding/{AStar.cs => MyAStar.cs} | 87 ++++++++++++++++++- .../{AStar.cs.meta => MyAStar.cs.meta} | 0 Assets/Scripts/Map/PatrollingMap.cs | 7 +- Assets/Scripts/Map/SimulationMap.cs | 5 +- Assets/Scripts/Map/SlamMap.cs | 75 +++++++++------- Assets/Scripts/Map/VisibleTilesCoarseMap.cs | 23 +++-- .../PatrollingAlgorithm.cs | 14 +-- Assets/Scripts/Robot/CommunicationManager.cs | 6 +- Assets/Scripts/Robot/ISlamAlgorithm.cs | 2 +- Assets/Scripts/Robot/Robot2DController.cs | 11 ++- .../Scripts/Statistics/ExplorationTracker.cs | 2 +- Assets/packages.config | 1 + 28 files changed, 276 insertions(+), 118 deletions(-) rename Assets/Scripts/Map/PathFinding/{AStar.cs => MyAStar.cs} (81%) rename Assets/Scripts/Map/PathFinding/{AStar.cs.meta => MyAStar.cs.meta} (100%) diff --git a/Assets/Scripts/ExperimentSimulations/ConscientiousReactiveExperiment.cs b/Assets/Scripts/ExperimentSimulations/ConscientiousReactiveExperiment.cs index c92fc1a7..17e5ec55 100644 --- a/Assets/Scripts/ExperimentSimulations/ConscientiousReactiveExperiment.cs +++ b/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(); diff --git a/Assets/Scripts/ExplorationAlgorithm/Greed/Greed.cs b/Assets/Scripts/ExplorationAlgorithm/Greed/Greed.cs index b9763452..0ca094b0 100644 --- a/Assets/Scripts/ExplorationAlgorithm/Greed/Greed.cs +++ b/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 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; } diff --git a/Assets/Scripts/ExplorationAlgorithm/HenrikAlgo/HenrikExplorationAlgorithm.cs b/Assets/Scripts/ExplorationAlgorithm/HenrikAlgo/HenrikExplorationAlgorithm.cs index 82f210f5..57e7ccdf 100644 --- a/Assets/Scripts/ExplorationAlgorithm/HenrikAlgo/HenrikExplorationAlgorithm.cs +++ b/Assets/Scripts/ExplorationAlgorithm/HenrikAlgo/HenrikExplorationAlgorithm.cs @@ -26,6 +26,8 @@ public class HenrikExplorationAlgorithm : IExplorationAlgorithm private Vector2Int? _targetTile; private uint _ticksSinceHeartbeat; + private int _logicTicks; + public void SetController(Robot2DController controller) { _robotController = controller; @@ -33,6 +35,7 @@ public void SetController(Robot2DController 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 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; } diff --git a/Assets/Scripts/ExplorationAlgorithm/Minotaur/Messages/HeartbeatMessage.cs b/Assets/Scripts/ExplorationAlgorithm/Minotaur/Messages/HeartbeatMessage.cs index 409f825a..fca6c919 100644 --- a/Assets/Scripts/ExplorationAlgorithm/Minotaur/Messages/HeartbeatMessage.cs +++ b/Assets/Scripts/ExplorationAlgorithm/Minotaur/Messages/HeartbeatMessage.cs @@ -48,12 +48,12 @@ public HeartbeatMessage(int ID, SlamMap map, List 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 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++) diff --git a/Assets/Scripts/ExplorationAlgorithm/Minotaur/MinotaurAlgorithm.cs b/Assets/Scripts/ExplorationAlgorithm/Minotaur/MinotaurAlgorithm.cs index b1f641cf..d7c7d420 100644 --- a/Assets/Scripts/ExplorationAlgorithm/Minotaur/MinotaurAlgorithm.cs +++ b/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 GetWalls(IEnumerable tiles) var previousDirection = (startPoint.corrected - sortedTiles[1].corrected).GetAngleRelativeToX(); var result = new List(); - 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) diff --git a/Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/TnfExplorationAlgorithm.cs b/Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/TnfExplorationAlgorithm.cs index f486142d..6b90aa0e 100644 --- a/Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/TnfExplorationAlgorithm.cs +++ b/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) { diff --git a/Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/ffTnfExplorationAlgorithm.cs b/Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/ffTnfExplorationAlgorithm.cs index 75a479c9..b435eb73 100644 --- a/Assets/Scripts/ExplorationAlgorithm/TheNextFrontier/ffTnfExplorationAlgorithm.cs +++ b/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) { diff --git a/Assets/Scripts/Map/CoarseGrainedMap.cs b/Assets/Scripts/Map/CoarseGrainedMap.cs index 5b5353a1..2bcb790f 100644 --- a/Assets/Scripts/Map/CoarseGrainedMap.cs +++ b/Assets/Scripts/Map/CoarseGrainedMap.cs @@ -41,9 +41,8 @@ public class CoarseGrainedMap : IPathFindingMap private bool[,] _tilesCoveredStatus; private SlamMap.SlamTileStatus[,] _optimisticTileStatuses; private HashSet _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(); /// /// A lower-resolution map (half the resolution of a ). @@ -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(); - 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 GetUnexploredTiles() { var res = new List(); - 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) /// the target that the path should end at. /// if true, returns path getting the closest to the target, if no full path can be found. /// if true, treats unseen tiles as open in the path finding algorithm. Treats unseen tiles as solid otherwise. - 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); } /// @@ -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; } + /// whether or not a tile at a given position is solid. public bool IsSolid(Vector2Int coordinate) { @@ -451,12 +458,12 @@ public Vector2Int GetCurrentTile() public static void Synchronize(List 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 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 maps, SlamMap.SlamTileStat /// public static void Combine(CoarseGrainedMap map, List 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 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 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; } /// false, only if the tile at the coordinate is known to be solid. diff --git a/Assets/Scripts/Map/MapGen/BitMapGenerator.cs b/Assets/Scripts/Map/MapGen/BitMapGenerator.cs index c330571f..8466ee53 100644 --- a/Assets/Scripts/Map/MapGen/BitMapGenerator.cs +++ b/Assets/Scripts/Map/MapGen/BitMapGenerator.cs @@ -37,7 +37,7 @@ public class BitMapGenerator : MapGenerator /// /// Method for creating a map from a 2D array of Tiles. /// - public SimulationMap CreateMapFromBitMap(Tile[,] bitmap, int seed, float wallHeight = 2.0f, int borderSize = 1) + public SimulationMap 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 CreateMapFromBitMap(Tile[,] bitmap, int seed, float w // Create mesh var meshGen = GetComponent(); 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); diff --git a/Assets/Scripts/Map/MapGen/BuildingGenerator.cs b/Assets/Scripts/Map/MapGen/BuildingGenerator.cs index 1d8773ef..6928da3a 100644 --- a/Assets/Scripts/Map/MapGen/BuildingGenerator.cs +++ b/Assets/Scripts/Map/MapGen/BuildingGenerator.cs @@ -76,7 +76,7 @@ public SimulationMap GenerateBuildingMap(BuildingMapConfig config, float w rooms.ForEach(r => r.OffsetCoordsBy(_config.BorderSize, _config.BorderSize)); var meshGen = GetComponent(); 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); diff --git a/Assets/Scripts/Map/MapGen/BuildingMapConfig.cs b/Assets/Scripts/Map/MapGen/BuildingMapConfig.cs index 1c72e734..77b07624 100644 --- a/Assets/Scripts/Map/MapGen/BuildingMapConfig.cs +++ b/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) diff --git a/Assets/Scripts/Map/MapGen/CaveGenerator.cs b/Assets/Scripts/Map/MapGen/CaveGenerator.cs index 45431756..d24c4f85 100644 --- a/Assets/Scripts/Map/MapGen/CaveGenerator.cs +++ b/Assets/Scripts/Map/MapGen/CaveGenerator.cs @@ -97,7 +97,7 @@ private SimulationMap CreateCaveMapWithMesh(CaveMapConfig caveConfig, Rand var meshGen = GetComponent(); var collisionMap = meshGen.GenerateMesh((Tile[,])borderedMap.Clone(), wallHeight, - true, survivingRooms); + true, survivingRooms, _caveConfig.BrokenCollisionMap); // Rotate to fit 2D view _plane.rotation = Quaternion.AngleAxis(-90, Vector3.right); diff --git a/Assets/Scripts/Map/MapGen/CaveMapConfig.cs b/Assets/Scripts/Map/MapGen/CaveMapConfig.cs index a17b6160..91ed2e35 100644 --- a/Assets/Scripts/Map/MapGen/CaveMapConfig.cs +++ b/Assets/Scripts/Map/MapGen/CaveMapConfig.cs @@ -54,6 +54,8 @@ public struct CaveMapConfig public int NeighbourWallsNeededToStayWall { get; } + public bool BrokenCollisionMap { get; } + internal CaveMapConfig(MaesYamlConfigLoader.MaesConfigType config, int seed) : this( randomSeed: seed, @@ -77,7 +79,8 @@ public CaveMapConfig( int wallThresholdSize = 1, int roomThresholdSize = 20, int borderSize = 2, - int neighbourWallsNeededToStayWall = 3) + int neighbourWallsNeededToStayWall = 3, + bool brokenCollisionMap = true) { // Only fill percent between and including 0 to 100 are allowed if (0 > randomFillPercent || randomFillPercent >= 100) @@ -105,6 +108,7 @@ public CaveMapConfig( BorderSize = borderSize; NeighbourWallsNeededToStayWall = neighbourWallsNeededToStayWall; + BrokenCollisionMap = brokenCollisionMap; } } } \ No newline at end of file diff --git a/Assets/Scripts/Map/MapGen/MapSpawner.cs b/Assets/Scripts/Map/MapGen/MapSpawner.cs index 3b30fdac..cc6dd66b 100644 --- a/Assets/Scripts/Map/MapGen/MapSpawner.cs +++ b/Assets/Scripts/Map/MapGen/MapSpawner.cs @@ -38,10 +38,10 @@ public SimulationMap GenerateMap(BuildingMapConfig buildingConfig, float w return buildingGenerator.GenerateBuildingMap(buildingConfig, wallHeight); } - public SimulationMap GenerateMap(Tile[,] bitmap, int seed, float wallHeight = 2.0f, int borderSize = 1) + public SimulationMap GenerateMap(Tile[,] bitmap, int seed, float wallHeight = 2.0f, int borderSize = 1, bool brokenCollisionMap = true) { var bitMapGenerator = gameObject.AddComponent(); - return bitMapGenerator.CreateMapFromBitMap(bitmap, seed, wallHeight, borderSize); + return bitMapGenerator.CreateMapFromBitMap(bitmap, seed, wallHeight, borderSize, brokenCollisionMap: brokenCollisionMap); } } } \ No newline at end of file diff --git a/Assets/Scripts/Map/MapGen/MeshGenerator.cs b/Assets/Scripts/Map/MapGen/MeshGenerator.cs index cc8d04d6..d3ad7c53 100644 --- a/Assets/Scripts/Map/MapGen/MeshGenerator.cs +++ b/Assets/Scripts/Map/MapGen/MeshGenerator.cs @@ -100,7 +100,7 @@ internal void ClearMesh() } internal SimulationMap GenerateMesh(Tile[,] map, float wallHeight, - bool disableCornerRounding, List rooms) + bool disableCornerRounding, List rooms, bool brokenCollisionMap) { InnerWallsRenderer2D.materials = Materials.ToArray(); InnerWallsRenderer3D.materials = Materials.ToArray(); @@ -156,7 +156,7 @@ internal SimulationMap GenerateMesh(Tile[,] map, float wallHeight, Generate2DColliders(); return GenerateCollisionMap(_squareGrid2D, map, - new Vector2(_squareGrid2D.XOffset, _squareGrid2D.YOffset), disableCornerRounding, rooms); + new Vector2(_squareGrid2D.XOffset, _squareGrid2D.YOffset), disableCornerRounding, rooms, brokenCollisionMap: brokenCollisionMap); } private void CreateRoofMesh() @@ -184,22 +184,33 @@ private void CreateRoofMesh() } private static SimulationMap GenerateCollisionMap(SquareGrid squareGrid, Tile[,] tileMap, Vector3 offset, - bool removeRoundedCorners, List rooms) + bool removeRoundedCorners, List rooms, bool brokenCollisionMap) { - var width = tileMap.GetLength(0); - var height = tileMap.GetLength(1); + var width = brokenCollisionMap ? squareGrid.Squares.GetLength(0) : tileMap.GetLength(0); + var height = brokenCollisionMap ? squareGrid.Squares.GetLength(1) : tileMap.GetLength(1); // Create a bool type SimulationMap with default value of false in all cells - var collisionMap = new SimulationMap(() => new Tile(TileType.Room), width, height, offset, rooms); + var collisionMap = new SimulationMap(() => new Tile(TileType.Room), width, height, offset, rooms, brokenCollisionMap: brokenCollisionMap); for (var x = 0; x < width; x++) { for (var y = 0; y < height; y++) { - var tile = collisionMap.GetTileByLocalCoordinate(x, y); - - for (var i = 0; i < 8; i++) + if (brokenCollisionMap) { - tile.SetCellValue(i, tileMap[x, y]); + var square = squareGrid.Squares[x, y]; + var collisionTile = collisionMap.GetTileByLocalCoordinate(x, y); + // Create triangles from all the points in the squares + // assigned to variables "vertices" and "triangles" + AdaptCollisionMapTile(collisionTile, square, removeRoundedCorners); + } + else + { + var tile = collisionMap.GetTileByLocalCoordinate(x, y); + + for (var i = 0; i < 8; i++) + { + tile.SetCellValue(i, tileMap[x, y]); + } } } } diff --git a/Assets/Scripts/Map/PathFinding/IPathFindingMap.cs b/Assets/Scripts/Map/PathFinding/IPathFindingMap.cs index cfbcbe7e..2dbf1ed1 100644 --- a/Assets/Scripts/Map/PathFinding/IPathFindingMap.cs +++ b/Assets/Scripts/Map/PathFinding/IPathFindingMap.cs @@ -29,6 +29,12 @@ namespace Maes.Map.PathFinding { public interface IPathFindingMap { + public bool BrokenCollisionMap { get; } + + public int LastUpdateTick { get; } + + public int Width { get; } + public int Height { get; } public bool IsSolid(Vector2Int coordinate); @@ -44,7 +50,7 @@ public interface IPathFindingMap public Vector2Int? GetNearestTileFloodFill(Vector2Int targetCoordinate, SlamTileStatus lookupStatus, HashSet? excludedTiles = null); - public Vector2Int GetCurrentPosition(); + public Vector2Int GetCurrentPosition(bool dependOnBrokenBehavior = false); /// /// This is for debugging purposes only to be able to easily convert coordinates to world units for drawing. diff --git a/Assets/Scripts/Map/PathFinding/AStar.cs b/Assets/Scripts/Map/PathFinding/MyAStar.cs similarity index 81% rename from Assets/Scripts/Map/PathFinding/AStar.cs rename to Assets/Scripts/Map/PathFinding/MyAStar.cs index 5330fdd3..ab192018 100644 --- a/Assets/Scripts/Map/PathFinding/AStar.cs +++ b/Assets/Scripts/Map/PathFinding/MyAStar.cs @@ -27,13 +27,19 @@ using Maes.Utilities; using Maes.Utilities.Priority_Queue; +using Roy_T.AStar.Paths; +using Roy_T.AStar.Primitives; + using UnityEngine; using static Maes.Map.SlamMap; +using Grid = Roy_T.AStar.Grids.Grid; +using Size = Roy_T.AStar.Primitives.Size; + namespace Maes.Map.PathFinding { - public class AStar : IPathFinder + public class MyAStar : IPathFinder { private class AStarTile { @@ -72,6 +78,79 @@ public Vector2Int[] Path() } } + private Grid? _cachedGrid = null; + private Grid? _cachedOptimisticGrid = null; + private int _lastUpdateTick = 0; + private readonly PathFinder _pathFinder = new(); + + public Vector2Int[]? GetNonBrokenPath(Vector2Int startCoordinate, Vector2Int targetCoordinate, + IPathFindingMap pathFindingMap, bool beOptimistic = false, bool acceptPartialPaths = false) + { + if (_lastUpdateTick != pathFindingMap.LastUpdateTick) + { + _cachedGrid = null; + _cachedOptimisticGrid = null; + } + + if (beOptimistic ? _cachedOptimisticGrid == null : _cachedGrid == null) + { + _lastUpdateTick = pathFindingMap.LastUpdateTick; + var width = pathFindingMap.Width; + var height = pathFindingMap.Height; + + var gridSize = new GridSize(width, height); + var cellSize = new Size(Distance.FromMeters(1), Distance.FromMeters(1)); + var traversalVelocity = Velocity.FromMetersPerSecond(1); + + var grid = Grid.CreateGridWithLateralAndDiagonalConnections(gridSize, cellSize, traversalVelocity); + for (var x = 0; x < width; x++) + { + for (var y = 0; y < height; y++) + { + if (beOptimistic ? pathFindingMap.IsOptimisticSolid(new Vector2Int(x, y)) : pathFindingMap.IsSolid(new Vector2Int(x, y))) + { + var gridPosition = new GridPosition(x, y); + grid.DisconnectNode(gridPosition); + grid.RemoveDiagonalConnectionsIntersectingWithNode(gridPosition); + } + } + } + + if (beOptimistic) + { + _cachedOptimisticGrid = grid; + } + else + { + _cachedGrid = grid; + } + } + + var paths = _pathFinder.FindPath(new GridPosition(startCoordinate.x, startCoordinate.y), + new GridPosition(targetCoordinate.x, targetCoordinate.y), beOptimistic ? _cachedOptimisticGrid : _cachedGrid); + + var tilePath = new Vector2Int[paths.Edges.Count + 1]; + + for (var i = 0; i < paths.Edges.Count; i++) + { + if (i == 0) + { + var pos = paths.Edges[0].Start.Position; + tilePath[0] = new Vector2Int((int)pos.X, (int)pos.Y); + } + + var position = paths.Edges[i].End.Position; + tilePath[i + 1] = new Vector2Int((int)position.X, (int)position.Y); + } + + if (paths.Edges.Count == 0 || (paths.Type == PathType.ClosestApproach && !acceptPartialPaths)) + { + return null; + } + + return tilePath; + } + public Vector2Int[]? GetOptimisticPath(Vector2Int startCoordinate, Vector2Int targetCoordinate, IPathFindingMap pathFindingMap, bool acceptPartialPaths = false) { return GetPath(startCoordinate, targetCoordinate, pathFindingMap, beOptimistic: true, acceptPartialPaths: acceptPartialPaths); @@ -79,6 +158,12 @@ public Vector2Int[] Path() public Vector2Int[]? GetPath(Vector2Int startCoordinate, Vector2Int targetCoordinate, IPathFindingMap pathFindingMap, bool beOptimistic = false, bool acceptPartialPaths = false) { + if (!pathFindingMap.BrokenCollisionMap) + { + return GetNonBrokenPath(startCoordinate, targetCoordinate, pathFindingMap, beOptimistic: beOptimistic, + acceptPartialPaths: acceptPartialPaths); + } + var candidates = new SimplePriorityQueue(); var bestCandidateOnTile = new Dictionary(); var startTileHeuristic = OctileHeuristic(startCoordinate, targetCoordinate); diff --git a/Assets/Scripts/Map/PathFinding/AStar.cs.meta b/Assets/Scripts/Map/PathFinding/MyAStar.cs.meta similarity index 100% rename from Assets/Scripts/Map/PathFinding/AStar.cs.meta rename to Assets/Scripts/Map/PathFinding/MyAStar.cs.meta diff --git a/Assets/Scripts/Map/PatrollingMap.cs b/Assets/Scripts/Map/PatrollingMap.cs index 2673b1dd..3ef2e521 100644 --- a/Assets/Scripts/Map/PatrollingMap.cs +++ b/Assets/Scripts/Map/PatrollingMap.cs @@ -54,14 +54,15 @@ public PatrollingMap Clone() // HACK: Creating a slam map with robot constraints seems a bit hacky tbh :( var slamMap = new SlamMap(simulationMap, new RobotConstraints(mapKnown: true), 0); var coarseMap = slamMap.CoarseMap; - var aStar = new AStar(); + var aStar = new MyAStar(); var paths = new Dictionary<(int, int), PathStep[]>(); foreach (var vertex in vertices) { foreach (var neighbor in vertex.Neighbors) { - var path = aStar.GetOptimisticPath(vertex.Position, neighbor.Position, coarseMap) ?? throw new InvalidOperationException("No path from vertex to neighbor"); - var pathSteps = AStar.PathToStepsCheap(path).ToArray(); + //var path = aStar.GetOptimisticPath(vertex.Position, neighbor.Position, coarseMap) ?? throw new InvalidOperationException("No path from vertex to neighbor"); + var path = aStar.GetNonBrokenPath(vertex.Position, neighbor.Position, coarseMap) ?? throw new InvalidOperationException("No path from vertex to neighbor"); + var pathSteps = MyAStar.PathToStepsCheap(path).ToArray(); paths.Add((vertex.Id, neighbor.Id), pathSteps); } diff --git a/Assets/Scripts/Map/SimulationMap.cs b/Assets/Scripts/Map/SimulationMap.cs index a24f58e8..d07f73eb 100644 --- a/Assets/Scripts/Map/SimulationMap.cs +++ b/Assets/Scripts/Map/SimulationMap.cs @@ -45,16 +45,19 @@ public class SimulationMap : IEnumerable<(int, TCell)> // They are used for robot spawning public readonly List Rooms = new(); + public readonly bool BrokenCollisionMap; + // The tiles of the map (each tile containing 8 triangle cells) private readonly SimulationMapTile[,] _tiles; public SimulationMap(Functional.Factory cellFactory, int widthInTiles, int heightInTiles, - Vector2 scaledOffset, List rooms) + Vector2 scaledOffset, List rooms, bool brokenCollisionMap) { Rooms = rooms; ScaledOffset = scaledOffset; WidthInTiles = widthInTiles; HeightInTiles = heightInTiles; + BrokenCollisionMap = brokenCollisionMap; _tiles = new SimulationMapTile[widthInTiles, heightInTiles]; for (var x = 0; x < widthInTiles; x++) { diff --git a/Assets/Scripts/Map/SlamMap.cs b/Assets/Scripts/Map/SlamMap.cs index 87e396ea..69b3f09f 100644 --- a/Assets/Scripts/Map/SlamMap.cs +++ b/Assets/Scripts/Map/SlamMap.cs @@ -38,7 +38,14 @@ namespace Maes.Map { public class SlamMap : ISlamAlgorithm, IPathFindingMap { - private readonly int _widthInTiles, _heightInTiles; + public bool BrokenCollisionMap => _collisionMap.BrokenCollisionMap; + public int LastUpdateTick { get; private set; } + public int Width { get; } + + public int Height { get; } + + // Size of a tile in world space + private readonly float _tileSize; private SlamTileStatus[,] _tiles; private readonly VisibleTile[,] _currentlyVisibleTiles; @@ -70,15 +77,15 @@ public SlamMap(SimulationMap collisionMap, RobotConstraints robotConstrain { _collisionMap = collisionMap; _robotConstraints = robotConstraints; - _widthInTiles = collisionMap.WidthInTiles * 2; - _heightInTiles = collisionMap.HeightInTiles * 2; + Width = collisionMap.WidthInTiles * 2; + Height = collisionMap.HeightInTiles * 2; _offset = collisionMap.ScaledOffset; _random = new Random(randomSeed); - _pathFinder = new AStar(); + _pathFinder = new MyAStar(); _tiles = robotConstraints.MapKnown ? SetTilesAsKnownMap(collisionMap) : EmptyMap(); - _currentlyVisibleTiles = new VisibleTile[_widthInTiles, _heightInTiles]; + _currentlyVisibleTiles = new VisibleTile[Width, Height]; CoarseMap = new CoarseGrainedMap(this, collisionMap.WidthInTiles, collisionMap.HeightInTiles, _offset, robotConstraints.MapKnown); _visibleTilesCoarseMap = new VisibleTilesCoarseMap(this, collisionMap.WidthInTiles, collisionMap.HeightInTiles, _offset); @@ -86,10 +93,10 @@ public SlamMap(SimulationMap collisionMap, RobotConstraints robotConstrain private SlamTileStatus[,] SetTilesAsKnownMap(SimulationMap collisionMap) { - var tiles = new SlamTileStatus[_widthInTiles, _heightInTiles]; - for (var x = 0; x < _widthInTiles; x++) + var tiles = new SlamTileStatus[Width, Height]; + for (var x = 0; x < Width; x++) { - for (var y = 0; y < _heightInTiles; y++) + for (var y = 0; y < Height; y++) { var tile = collisionMap.GetTileByLocalCoordinate(x / 2, y / 2); var triangles = tile.GetTriangles(); @@ -107,10 +114,10 @@ public SlamMap(SimulationMap collisionMap, RobotConstraints robotConstrain private SlamTileStatus[,] EmptyMap() { - var tiles = new SlamTileStatus[_widthInTiles, _heightInTiles]; - for (var x = 0; x < _widthInTiles; x++) + var tiles = new SlamTileStatus[Width, Height]; + for (var x = 0; x < Width; x++) { - for (var y = 0; y < _heightInTiles; y++) + for (var y = 0; y < Height; y++) { tiles[x, y] = SlamTileStatus.Unseen; } @@ -137,18 +144,26 @@ public Vector2Int LocalCoordinateToPathFindingCoordinate(Vector2Int localCoordin return localCoordinate / 2; } - public void SetExploredByCoordinate(Vector2Int localCoordinate, bool isOpen) + public void SetExploredByCoordinate(Vector2Int localCoordinate, bool isOpen, int tick) { var x = localCoordinate.x; var y = localCoordinate.y; - if (_tiles[x, y] != SlamTileStatus.Solid) + + var status = _tiles[x, y]; + + if (status != SlamTileStatus.Solid) { - _tiles[x, y] = isOpen ? SlamTileStatus.Open : SlamTileStatus.Solid; + var newStatus = isOpen ? SlamTileStatus.Open : SlamTileStatus.Solid; + if (status != newStatus) + { + _tiles[x, y] = newStatus; + LastUpdateTick = tick; + } } } - public Vector2Int GetCurrentPosition() + public Vector2Int GetCurrentPosition(bool dependOnBrokenPosition = true) { var currentPosition = GetApproxPosition(); // Since the resolution of the slam map is double, we round to nearest half @@ -184,9 +199,9 @@ public SlamTileStatus GetVisibleTileStatus(int x, int y) public List GetVisibleTiles() { var visibleTilesList = new List(); - for (var x = 0; x < _widthInTiles; x++) + for (var x = 0; x < Width; x++) { - for (var y = 0; y < _heightInTiles; y++) + for (var y = 0; y < Height; y++) { var tile = _currentlyVisibleTiles[x, y]; if (tile.Generation == _visibleTilesGeneration && tile.TileStatus != SlamTileStatus.Unseen) @@ -265,15 +280,15 @@ public void UpdateApproxPosition(Vector2 worldPosition) } // Synchronizes the given slam maps to create a new one - public static void Synchronize(List maps) + public static void Synchronize(List maps, int tick) { - var globalMap = new SlamTileStatus[maps[0]._widthInTiles, maps[0]._heightInTiles]; + var globalMap = new SlamTileStatus[maps[0].Width, maps[0].Height]; foreach (var map in maps) { - for (var x = 0; x < map._widthInTiles; x++) + for (var x = 0; x < map.Width; x++) { - for (var y = 0; y < map._heightInTiles; y++) + for (var y = 0; y < map.Height; y++) { if (map._tiles[x, y] != SlamTileStatus.Unseen && globalMap[x, y] != SlamTileStatus.Solid) { @@ -286,6 +301,7 @@ public static void Synchronize(List maps) foreach (var map in maps) { map._tiles = (SlamTileStatus[,])globalMap.Clone(); + map.LastUpdateTick = tick; } // Synchronize coarse maps @@ -297,15 +313,15 @@ public static void Synchronize(List maps) /// Synchronizes one map with a list of other s. /// s /// - public static void Combine(SlamMap target, List others) + public static void Combine(SlamMap target, List others, int tick) { - var globalMap = new SlamTileStatus[target._widthInTiles, target._heightInTiles]; + var globalMap = new SlamTileStatus[target.Width, target.Height]; foreach (var other in others) { - for (var x = 0; x < target._widthInTiles; x++) + for (var x = 0; x < target.Width; x++) { - for (var y = 0; y < target._heightInTiles; y++) + for (var y = 0; y < target.Height; y++) { if (other._tiles[x, y] != SlamTileStatus.Unseen) { @@ -316,6 +332,7 @@ public static void Combine(SlamMap target, List others) } target._tiles = (SlamTileStatus[,])globalMap.Clone(); + target.LastUpdateTick = tick; CoarseGrainedMap.Combine(target.CoarseMap, others.Select(o => o.GetCoarseMap()).ToList(), globalMap); } @@ -328,9 +345,9 @@ public Dictionary GetExploredTiles() { var res = new Dictionary(); - for (var x = 0; x < _widthInTiles; x++) + for (var x = 0; x < Width; x++) { - for (var y = 0; y < _heightInTiles; y++) + for (var y = 0; y < Height; y++) { if (_tiles[x, y] != SlamTileStatus.Unseen) { @@ -365,8 +382,8 @@ public void SetApproxRobotAngle(float robotAngle) public bool IsWithinBounds(Vector2Int slamCoordinate) { - return slamCoordinate.x > 0 && slamCoordinate.x < _widthInTiles && - slamCoordinate.y > 0 && slamCoordinate.y < _heightInTiles; + return slamCoordinate.x > 0 && slamCoordinate.x < Width && + slamCoordinate.y > 0 && slamCoordinate.y < Height; } public bool IsCoordWithinBounds(Vector2Int coordinate) diff --git a/Assets/Scripts/Map/VisibleTilesCoarseMap.cs b/Assets/Scripts/Map/VisibleTilesCoarseMap.cs index 2470c7ee..91d9fbf1 100644 --- a/Assets/Scripts/Map/VisibleTilesCoarseMap.cs +++ b/Assets/Scripts/Map/VisibleTilesCoarseMap.cs @@ -30,19 +30,17 @@ namespace Maes.Map public class VisibleTilesCoarseMap : IPathFindingMap { private readonly SlamMap _slamMap; - private readonly int _width; - private readonly int _height; private readonly Vector2 _offset; - private readonly AStar _aStar; + private readonly MyAStar _aStar; public VisibleTilesCoarseMap(SlamMap slamMap, int width, int height, Vector2 offset) { _slamMap = slamMap; - _width = width; - _height = height; + Width = width; + Height = height; _offset = offset; - _aStar = new AStar(); + _aStar = new MyAStar(); } public Vector2Int[]? GetPath(Vector2Int coarseTileFrom, Vector2Int coarseTileTo, bool acceptPartialPaths = false) @@ -61,6 +59,12 @@ public VisibleTilesCoarseMap(SlamMap slamMap, int width, int height, Vector2 off return path; } + public bool BrokenCollisionMap => _slamMap.BrokenCollisionMap; + public int LastUpdateTick => _slamMap.LastUpdateTick; + public int Width { get; } + + public int Height { get; } + public bool IsSolid(Vector2Int coordinate) { var slamTile = ToSlamMapCoordinate(coordinate); @@ -134,7 +138,7 @@ public static Vector2Int ToSlamMapCoordinate(Vector2Int localCoordinate) 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 the status of the given tile (Solid, Open or Unseen) @@ -199,9 +203,10 @@ private static SlamMap.SlamTileStatus AggregateStatusPessimistic(SlamMap.SlamTil return SlamMap.SlamTileStatus.Open; } - public Vector2Int GetCurrentPosition() + public Vector2Int GetCurrentPosition(bool dependOnBrokenBehaviour = true) { - return Vector2Int.FloorToInt(_slamMap.ApproximatePosition - _offset); + var approximatePosition = _slamMap.ApproximatePosition - _offset; + return dependOnBrokenBehaviour ? Vector2Int.FloorToInt(approximatePosition) : Vector2Int.RoundToInt(approximatePosition); } public Vector3 TileToWorld(Vector2 tile) diff --git a/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs b/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs index d398059b..e7b27d84 100644 --- a/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs +++ b/Assets/Scripts/PatrollingAlgorithms/PatrollingAlgorithm.cs @@ -36,6 +36,8 @@ public abstract class PatrollingAlgorithm : IPatrollingAlgorithm private bool _hasCollided; private bool _firstCollision; + private readonly StringBuilder _stringBuilder = new(); + protected event OnReachVertex? OnReachVertexHandler; public void SetController(Robot2DController controller) @@ -66,11 +68,11 @@ public virtual void UpdateLogic() if (_goingToInitialVertex) { _targetVertex ??= GetClosestVertex(); - var currentPosition = _controller.SlamMap.CoarseMap.GetCurrentPosition(); + var currentPosition = _controller.SlamMap.CoarseMap.GetCurrentPosition(dependOnBrokenBehavior: false); if (currentPosition != TargetVertex.Position) { // Do normal astar pathing - _controller.PathAndMoveTo(TargetVertex.Position); + _controller.PathAndMoveTo(TargetVertex.Position, dependOnBrokenBehaviour: false); } else { @@ -90,7 +92,7 @@ public virtual void UpdateLogic() { // Do default AStar _currentPath.Clear(); - var currentPosition = _controller.SlamMap.CoarseMap.GetCurrentPosition(); + var currentPosition = _controller.SlamMap.CoarseMap.GetCurrentPosition(dependOnBrokenBehavior: false); if (currentPosition != TargetVertex.Position) { if (_firstCollision) @@ -126,14 +128,16 @@ private void SetNextVertex() OnReachTargetVertex(currentVertex); _targetVertex = NextVertex(); _currentPath = new Queue(_paths[(currentVertex.Id, _targetVertex.Id)]); + _currentTarget = null; } protected abstract Vertex NextVertex(); public virtual string GetDebugInfo() { + _stringBuilder.Clear(); return - new StringBuilder() + _stringBuilder .AppendLine(AlgorithmName) .Append("Target vertex position: ") .AppendLine(TargetVertex.Position.ToString()) @@ -192,7 +196,7 @@ private void PathAndMoveToTarget() private Vertex GetClosestVertex() { - var position = _controller.GetSlamMap().GetCoarseMap().GetCurrentPosition(); + var position = _controller.GetSlamMap().GetCoarseMap().GetCurrentPosition(dependOnBrokenBehavior: false); var closestVertex = _vertices[0]; var closestDistance = Vector2Int.Distance(position, closestVertex.Position); foreach (var vertex in _vertices.AsSpan(1)) diff --git a/Assets/Scripts/Robot/CommunicationManager.cs b/Assets/Scripts/Robot/CommunicationManager.cs index 23a0caa6..7b9381cd 100644 --- a/Assets/Scripts/Robot/CommunicationManager.cs +++ b/Assets/Scripts/Robot/CommunicationManager.cs @@ -247,7 +247,7 @@ public void LogicUpdate() && _robotConstraints.DistributeSlam // Are we distributing slam? && _localTickCounter % _robotConstraints.SlamSynchronizeIntervalInTicks == 0) { - SynchronizeSlamMaps(); + SynchronizeSlamMaps(_localTickCounter); } if (GlobalSettings.ShouldWriteCsvResults && _localTickCounter % GlobalSettings.TicksPerStatsSnapShot == 0) @@ -263,7 +263,7 @@ public void LogicUpdate() _communicationGroups = null; } - private void SynchronizeSlamMaps() + private void SynchronizeSlamMaps(int tick) { _communicationGroups = GetCommunicationGroups(); @@ -274,7 +274,7 @@ private void SynchronizeSlamMaps() .Select(r => r.Controller.SlamMap) .ToList(); - SlamMap.Synchronize(slamMaps); + SlamMap.Synchronize(slamMaps, tick); } } diff --git a/Assets/Scripts/Robot/ISlamAlgorithm.cs b/Assets/Scripts/Robot/ISlamAlgorithm.cs index 167598f8..db35000e 100644 --- a/Assets/Scripts/Robot/ISlamAlgorithm.cs +++ b/Assets/Scripts/Robot/ISlamAlgorithm.cs @@ -48,7 +48,7 @@ public interface ISlamAlgorithm public SlamMap.SlamTileStatus GetVisibleTileStatus(int x, int y); /// The current position of the robot as a slam tile coordinate (rounded down) - public Vector2Int GetCurrentPosition(); + public Vector2Int GetCurrentPosition(bool dependOnBrokenBehaviour = true); /// /// Returns the perceived status of the given tile as a . diff --git a/Assets/Scripts/Robot/Robot2DController.cs b/Assets/Scripts/Robot/Robot2DController.cs index 7e7d109b..7ea9cab4 100644 --- a/Assets/Scripts/Robot/Robot2DController.cs +++ b/Assets/Scripts/Robot/Robot2DController.cs @@ -419,6 +419,8 @@ public void Move(float distanceInMeters, bool reverse = false) /// COARSEGRAINED tile as final target public void PathAndMoveTo(Vector2Int tile, bool dependOnBrokenBehaviour = true) { + var closeness = dependOnBrokenBehaviour ? 0.5f : 0.25f; + if (GetStatus() != RobotStatus.Idle) { return; @@ -431,13 +433,14 @@ public void PathAndMoveTo(Vector2Int tile, bool dependOnBrokenBehaviour = true) if (_currentPath.Count == 0) { - var robotCurrentPosition = Vector2Int.FloorToInt(SlamMap.CoarseMap.GetApproximatePosition()); + var approximatePosition = SlamMap.CoarseMap.GetApproximatePosition(); + var robotCurrentPosition = dependOnBrokenBehaviour ? Vector2Int.FloorToInt(approximatePosition) : Vector2Int.RoundToInt(approximatePosition); if (robotCurrentPosition == tile) { return; } - var pathList = SlamMap.CoarseMap.GetPath(tile, beOptimistic: true); + var pathList = SlamMap.CoarseMap.GetPath(tile, beOptimistic: true, dependOnBrokenBehavior: dependOnBrokenBehaviour); if (pathList == null) { return; @@ -452,7 +455,7 @@ public void PathAndMoveTo(Vector2Int tile, bool dependOnBrokenBehaviour = true) } var relativePosition = SlamMap.CoarseMap.GetTileCenterRelativePosition(_currentTarget, dependOnBrokenBehaviour: dependOnBrokenBehaviour); - if (relativePosition.Distance < 0.5f) + if (relativePosition.Distance < closeness) { if (_currentPath.Count == 0) { @@ -481,7 +484,7 @@ public void PathAndMoveTo(Vector2Int tile, bool dependOnBrokenBehaviour = true) { Rotate(relativePosition.RelativeAngle); } - else if (relativePosition.Distance > 0.5f) + else if (relativePosition.Distance > closeness) { Move(relativePosition.Distance); } diff --git a/Assets/Scripts/Statistics/ExplorationTracker.cs b/Assets/Scripts/Statistics/ExplorationTracker.cs index 2eb50a1b..66eab572 100644 --- a/Assets/Scripts/Statistics/ExplorationTracker.cs +++ b/Assets/Scripts/Statistics/ExplorationTracker.cs @@ -253,7 +253,7 @@ private void PerformRayTracing(List robots, bool shouldUpdateSlamMap) { var localCoordinate = slamMap.TriangleIndexToCoordinate(index); // Update robot slam map if present (slam map only non-null if 'shouldUpdateSlamMap' is true) - slamMap.SetExploredByCoordinate(localCoordinate, isOpen: cell.IsExplorable); + slamMap.SetExploredByCoordinate(localCoordinate, isOpen: cell.IsExplorable, tick: _currentTick); slamMap.SetCurrentlyVisibleByTriangle(triangleIndex: index, localCoordinate, isOpen: cell.IsExplorable); } diff --git a/Assets/packages.config b/Assets/packages.config index bfc36772..ca98633b 100644 --- a/Assets/packages.config +++ b/Assets/packages.config @@ -1,4 +1,5 @@  + \ No newline at end of file