diff --git a/Assets/Scripts/Statistics/ExplorationTracker.cs b/Assets/Scripts/Statistics/ExplorationTracker.cs index d1159839..294c4488 100644 --- a/Assets/Scripts/Statistics/ExplorationTracker.cs +++ b/Assets/Scripts/Statistics/ExplorationTracker.cs @@ -53,6 +53,9 @@ public class ExplorationTracker : Tracker _newlyExploredTriangles = new(); + private readonly int _traces; + private readonly float _traceIntervalDegrees; + public ExplorationTracker(SimulationMap collisionMap, ExplorationVisualizer explorationVisualizer, RobotConstraints constraints) : base(collisionMap, explorationVisualizer, constraints, tile => new ExplorationCell(isExplorable: !Tile.IsWall(tile.Type))) { @@ -61,6 +64,10 @@ public ExplorationTracker(SimulationMap collisionMap, ExplorationVisualize _currentVisualizationMode = new AllRobotsExplorationVisualization(_map); _totalExplorableTriangles = collisionMap.Count(x => !Tile.IsWall(x.Item2.Type)); + + const float tracesPerMeter = 2f; + _traces = _constraints.SlamRayTraceCount ?? (int)(Mathf.PI * 2f * _constraints.SlamRayTraceRange * tracesPerMeter); + _traceIntervalDegrees = 360f / _traces; } protected override void CreateSnapShot() @@ -128,19 +135,6 @@ protected override void OnAfterFirstTick(MonaRobot[] robots) base.OnAfterFirstTick(robots); } - protected override void AfterRayTracingARobot(MonaRobot robot) - { - // Register newly explored cells of this robot for visualization - _currentVisualizationMode.RegisterNewlyExploredCells(robot, _newlyExploredTriangles); - _newlyExploredTriangles.Clear(); - } - - protected override void OnNewlyExploredTriangles(int index, ExplorationCell cell) - { - _newlyExploredTriangles.Add((index, cell)); - ExploredTriangles++; - } - public override void SetVisualizedRobot(MonaRobot? robot) { _selectedRobot = robot; @@ -195,5 +189,77 @@ public void ShowSelectedRobotSlamMap() SetVisualizationMode(new SelectedRobotSlamMapVisualization(_selectedRobot.Controller)); } + + protected override void OnBeforeLogicUpdate(MonaRobot[] robots) + { + base.OnBeforeLogicUpdate(robots); + + // The user can specify the tick interval at which the slam map is updated. + var shouldUpdateSlamMap = _constraints.AutomaticallyUpdateSlam && + _currentTick % _constraints.SlamUpdateIntervalInTicks == 0; + + PerformRayTracing(robots, shouldUpdateSlamMap); + } + + // Updates both exploration tracker and robot slam maps + private void PerformRayTracing(MonaRobot[] robots, bool shouldUpdateSlamMap) + { + var visibilityRange = _constraints.SlamRayTraceRange; + + foreach (var robot in robots) + { + SlamMap? slamMap = null; + + if (shouldUpdateSlamMap) + { + slamMap = robot.Controller.SlamMap; + slamMap.ResetRobotVisibility(); + } + + var position = (Vector2)robot.transform.position; + + // Use amount of traces specified by user, or calculate circumference and use trace at interval of 4 + for (var i = 0; i < _traces; i++) + { + var angle = i * _traceIntervalDegrees; + // Avoid ray casts that can be parallel to the lines of a triangle + if (angle % 45 == 0) + { + angle += 0.5f; + } + + _rayTracingMap.Raytrace(position, angle, visibilityRange, (index, cell) => + { + if (cell.IsExplorable) + { + if (!cell.IsExplored) + { + cell.LastExplorationTimeInTicks = _currentTick; + cell.ExplorationTimeInTicks += 1; + + _newlyExploredTriangles.Add((index, cell)); + ExploredTriangles++; + } + + cell.RegisterExploration(_currentTick); + } + + if (slamMap != null) + { + 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.SetCurrentlyVisibleByTriangle(triangleIndex: index, localCoordinate, isOpen: cell.IsExplorable); + } + + return cell.IsExplorable; + }); + } + + // Register newly explored cells of this robot for visualization + _currentVisualizationMode.RegisterNewlyExploredCells(robot, _newlyExploredTriangles); + _newlyExploredTriangles.Clear(); + } + } } } \ No newline at end of file diff --git a/Assets/Scripts/Trackers/PatrollingTracker.cs b/Assets/Scripts/Trackers/PatrollingTracker.cs index 7cf4af33..bfbcfc71 100644 --- a/Assets/Scripts/Trackers/PatrollingTracker.cs +++ b/Assets/Scripts/Trackers/PatrollingTracker.cs @@ -80,7 +80,7 @@ public void OnReachedVertex(int vertexId, int atTick) } } - protected override void OnLogicUpdate(IReadOnlyList robots) + protected override void OnLogicUpdate(MonaRobot[] robots) { var worstGraphIdleness = 0; var graphIdlenessSum = 0; diff --git a/Assets/Scripts/Trackers/Tracker.cs b/Assets/Scripts/Trackers/Tracker.cs index d7cf61d6..614e8a03 100644 --- a/Assets/Scripts/Trackers/Tracker.cs +++ b/Assets/Scripts/Trackers/Tracker.cs @@ -1,5 +1,4 @@ using System; -using System.Collections.Generic; using Maes.Map; using Maes.Map.MapGen; @@ -8,8 +7,6 @@ using Maes.Statistics; using Maes.Visualizers; -using UnityEngine; - namespace Maes.Trackers { public abstract class Tracker : ITracker @@ -22,7 +19,7 @@ public abstract class Tracker : ITracker protected readonly TVisualizer _visualizer; protected readonly SimulationMap _map; - private readonly RayTracingMap _rayTracingMap; + protected readonly RayTracingMap _rayTracingMap; private readonly int _explorationMapWidth; private readonly int _explorationMapHeight; @@ -39,9 +36,6 @@ public abstract class Tracker : ITracker // Set by derived class constructor. protected TVisualizationMode _currentVisualizationMode = null!; - private readonly int _traces; - private readonly float _traceIntervalDegrees; - private readonly CoverageCalculator.MiniTileConsumer _preCoverageTileConsumerDelegate; protected Tracker(SimulationMap collisionMap, TVisualizer visualizer, RobotConstraints constraints, Func mapper) @@ -50,10 +44,6 @@ protected Tracker(SimulationMap collisionMap, TVisualizer visualizer, Robo _constraints = constraints; _map = collisionMap.FMap(mapper); - const float tracesPerMeter = 2f; - _traces = _constraints.SlamRayTraceCount ?? (int)(Mathf.PI * 2f * _constraints.SlamRayTraceRange * tracesPerMeter); - _traceIntervalDegrees = 360f / _traces; - _visualizer.SetSimulationMap(_map, collisionMap.ScaledOffset); _rayTracingMap = new RayTracingMap(_map); @@ -63,10 +53,7 @@ protected Tracker(SimulationMap collisionMap, TVisualizer visualizer, Robo public void LogicUpdate(MonaRobot[] robots) { - // The user can specify the tick interval at which the slam map is updated. - var shouldUpdateSlamMap = _constraints.AutomaticallyUpdateSlam && - _currentTick % _constraints.SlamUpdateIntervalInTicks == 0; - PerformRayTracing(robots, shouldUpdateSlamMap); + OnBeforeLogicUpdate(robots); // In the first tick, the robot does not have a position in the slam map. if (!_isFirstTick) @@ -103,7 +90,8 @@ public void LogicUpdate(MonaRobot[] robots) _currentTick++; } - protected virtual void OnLogicUpdate(IReadOnlyList robots) { } + protected virtual void OnBeforeLogicUpdate(MonaRobot[] robots) { } + protected virtual void OnLogicUpdate(MonaRobot[] robots) { } public abstract void SetVisualizedRobot(MonaRobot? robot); @@ -117,67 +105,6 @@ protected virtual void OnAfterFirstTick(MonaRobot[] robots) protected abstract void CreateSnapShot(); - // Updates both exploration tracker and robot slam maps - private void PerformRayTracing(MonaRobot[] robots, bool shouldUpdateSlamMap) - { - var visibilityRange = _constraints.SlamRayTraceRange; - - foreach (var robot in robots) - { - SlamMap? slamMap = null; - - if (shouldUpdateSlamMap) - { - slamMap = robot.Controller.SlamMap; - slamMap.ResetRobotVisibility(); - } - - var position = (Vector2)robot.transform.position; - - // Use amount of traces specified by user, or calculate circumference and use trace at interval of 4 - for (var i = 0; i < _traces; i++) - { - var angle = i * _traceIntervalDegrees; - // Avoid ray casts that can be parallel to the lines of a triangle - if (angle % 45 == 0) - { - angle += 0.5f; - } - - _rayTracingMap.Raytrace(position, angle, visibilityRange, (index, cell) => - { - if (cell.IsExplorable) - { - if (!cell.IsExplored) - { - cell.LastExplorationTimeInTicks = _currentTick; - cell.ExplorationTimeInTicks += 1; - OnNewlyExploredTriangles(index, cell); - } - - cell.RegisterExploration(_currentTick); - } - - if (slamMap != null) - { - 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.SetCurrentlyVisibleByTriangle(triangleIndex: index, localCoordinate, isOpen: cell.IsExplorable); - } - - return cell.IsExplorable; - }); - } - - AfterRayTracingARobot(robot); - } - } - - protected virtual void OnNewlyExploredTriangles(int index, TCell cell) { } - - protected virtual void AfterRayTracingARobot(MonaRobot robot) { } - protected virtual void SetVisualizationMode(TVisualizationMode newMode) { _currentVisualizationMode = newMode;