From 5bb82f75793c58bc3491f373a1adecabdef3b645 Mon Sep 17 00:00:00 2001 From: Brady Bodily Date: Mon, 14 Dec 2020 18:24:10 -0700 Subject: [PATCH] Optimal path for square map --- .../.idea/contentModel.xml | 7 +- .../.idea.RobotIntelFinal/.idea/workspace.xml | 95 +++++++++++++++---- ConsoleApp/CoreModule.cs | 1 + ConsoleApp/IPathPlanner.cs | 11 ++- ConsoleApp/Maps/GlobalDirection.cs | 7 +- ConsoleApp/Maps/Heading.cs | 2 - ConsoleApp/Maps/HexMap.cs | 10 +- ConsoleApp/Maps/IMap.cs | 6 +- ConsoleApp/Maps/SquareMap.cs | 47 ++++++--- ConsoleApp/PathPlanner.cs | 56 ++++++++++- ConsoleApp/SimRunner.cs | 46 +++------ 11 files changed, 209 insertions(+), 79 deletions(-) diff --git a/.idea/.idea.RobotIntelFinal/.idea/contentModel.xml b/.idea/.idea.RobotIntelFinal/.idea/contentModel.xml index 5a947e0..95e7f7b 100644 --- a/.idea/.idea.RobotIntelFinal/.idea/contentModel.xml +++ b/.idea/.idea.RobotIntelFinal/.idea/contentModel.xml @@ -9,13 +9,14 @@ + - + @@ -27,12 +28,13 @@ - + + @@ -43,6 +45,7 @@ + diff --git a/.idea/.idea.RobotIntelFinal/.idea/workspace.xml b/.idea/.idea.RobotIntelFinal/.idea/workspace.xml index 054c27e..2817021 100644 --- a/.idea/.idea.RobotIntelFinal/.idea/workspace.xml +++ b/.idea/.idea.RobotIntelFinal/.idea/workspace.xml @@ -18,24 +18,18 @@ + + + - - - + + - - - - - - - - + + + + + + + + + @@ -74,14 +86,15 @@ - + + - + + + + @@ -129,7 +146,8 @@ - + + @@ -153,4 +171,49 @@ + + + + + file://$PROJECT_DIR$/ConsoleApp/SimRunner.cs + 49 + + + + + + + + + file://$PROJECT_DIR$/ConsoleApp/SimRunner.cs + 47 + + + + + + + + + file://$PROJECT_DIR$/ConsoleApp/PathPlanner.cs + 34 + + + + + + + + + + \ No newline at end of file diff --git a/ConsoleApp/CoreModule.cs b/ConsoleApp/CoreModule.cs index de3521f..0b9500a 100644 --- a/ConsoleApp/CoreModule.cs +++ b/ConsoleApp/CoreModule.cs @@ -13,6 +13,7 @@ namespace ConsoleApp container.Register(Reuse.Singleton); container.Register(Reuse.Singleton); container.Register(Reuse.Singleton); + container.Register(Reuse.Singleton); } diff --git a/ConsoleApp/IPathPlanner.cs b/ConsoleApp/IPathPlanner.cs index 4d79ed9..c71d52d 100644 --- a/ConsoleApp/IPathPlanner.cs +++ b/ConsoleApp/IPathPlanner.cs @@ -1,4 +1,11 @@ -$HEADER$namespace $NAMESPACE$ +using System.Collections.Generic; +using ConsoleApp.Maps; + +namespace ConsoleApp { - public interface $INTERFACE$ {$END$} + public interface IPathPlanner + { + Queue GenerateOptimalSquarePath(SquareMap map, IVehicle vehicle); + } + } \ No newline at end of file diff --git a/ConsoleApp/Maps/GlobalDirection.cs b/ConsoleApp/Maps/GlobalDirection.cs index ca7b731..8bff109 100644 --- a/ConsoleApp/Maps/GlobalDirection.cs +++ b/ConsoleApp/Maps/GlobalDirection.cs @@ -1,7 +1,10 @@ namespace ConsoleApp.Maps { - public enum Direction + public enum GlobalDirection { - + North, + South, + East, + West } } \ No newline at end of file diff --git a/ConsoleApp/Maps/Heading.cs b/ConsoleApp/Maps/Heading.cs index df70d1c..0a58e2b 100644 --- a/ConsoleApp/Maps/Heading.cs +++ b/ConsoleApp/Maps/Heading.cs @@ -1,5 +1,3 @@ -using System.Net.NetworkInformation; - namespace ConsoleApp.Maps { public class Oreientation diff --git a/ConsoleApp/Maps/HexMap.cs b/ConsoleApp/Maps/HexMap.cs index a98f900..3ecba18 100644 --- a/ConsoleApp/Maps/HexMap.cs +++ b/ConsoleApp/Maps/HexMap.cs @@ -6,8 +6,8 @@ namespace ConsoleApp.Maps public class HexMap : IHexMap { public Cell[,] Map { get; } - public ICell StartingCell { get; } - public ICell LastCell { get; } + public Cell StartingCell { get; } + public Cell LastCell { get; } /// /// Generate Hex map with cells of 25cm X 25cm @@ -23,9 +23,11 @@ namespace ConsoleApp.Maps var xCellCount = (int)Math.Ceiling((decimal) (x) / 25); var yCellCount = (int)Math.Ceiling((decimal) (y) / 25); + //Initialize Map + Map = new Cell[xCellCount, yCellCount]; + //set last cell; StartingCell = Map[0, 0]; - Map = new Cell[xCellCount, yCellCount]; for (int r = 0; r < yCellCount; r++) { int r_offset = Convert.ToInt32(Math.Floor(Convert.ToDouble(r)/2)); for (int q = r_offset; q < xCellCount - r_offset; q++) { @@ -35,7 +37,7 @@ namespace ConsoleApp.Maps } } - public List PossibleMoves(ICell currentCell) + public List PossibleMoves(ICell currentCell) { throw new NotImplementedException(); } diff --git a/ConsoleApp/Maps/IMap.cs b/ConsoleApp/Maps/IMap.cs index d622bd4..a00f3fb 100644 --- a/ConsoleApp/Maps/IMap.cs +++ b/ConsoleApp/Maps/IMap.cs @@ -6,8 +6,8 @@ namespace ConsoleApp.Maps public interface IMap { public Cell[,] Map { get; } - ICell StartingCell { get; } - ICell LastCell { get; } - public List PossibleMoves(ICell currentCell); + Cell StartingCell { get; } + Cell LastCell { get; } + public List PossibleMoves(ICell currentCell); } } \ No newline at end of file diff --git a/ConsoleApp/Maps/SquareMap.cs b/ConsoleApp/Maps/SquareMap.cs index b408aba..4a6f158 100644 --- a/ConsoleApp/Maps/SquareMap.cs +++ b/ConsoleApp/Maps/SquareMap.cs @@ -6,9 +6,14 @@ namespace ConsoleApp.Maps public class SquareMap : ISquareMap { public Cell[,] Map { get;} - public ICell StartingCell { get; } - public ICell LastCell { get; } + public Cell StartingCell { get; } + public Cell LastCell { get; } + public int Height { get; } + public int Width { get; } + + private int _mapWidth; + private int _mapHeight; public SquareMap(int x, int y) { //convert to cm @@ -18,31 +23,43 @@ namespace ConsoleApp.Maps var xCellCount = (int)Math.Ceiling((decimal) (x) / 25); var yCellCount = (int)Math.Ceiling((decimal) (y) / 25); + //set Width and height fields + _mapWidth = xCellCount-1; + _mapHeight = yCellCount-1; + Height = _mapHeight; + Width = _mapWidth; + + //Initialize Map + Map = new Cell[xCellCount, yCellCount]; + //set last cell; StartingCell = Map[0, 0]; - Map = new Cell[xCellCount, yCellCount]; - for (int i = 0; i < x; i++) + for (int i = 0; i < xCellCount; i++) { - for (int j = 0; j < y; j++) + for (int j = 0; j < yCellCount; j++) { Map[i,j] = new Cell(i, j); } } } - public List PossibleMoves(ICell currentCell) + + public List PossibleMoves(ICell currentCell) { - var forward = (currentCell.X , currentCell.Y + 1); - var backwards= (currentCell.X , currentCell.Y - 1); - var right = (currentCell.X + 1, currentCell.Y); - var left = (currentCell.X - 1, currentCell.Y); - var possibles = new List(); - possibles.Add(Map[forward.Item1, forward.Item2]); - possibles.Add(Map[backwards.Item1, backwards.Item2]); - possibles.Add(Map[right.Item1, right.Item2]); - possibles.Add(Map[left.Item1, left.Item2]); + var possibles = new List(); + if (currentCell.X != 0) + possibles.Add(GlobalDirection.West); + if (currentCell.X != _mapWidth) + possibles.Add(GlobalDirection.East); + if (currentCell.Y != _mapHeight) + possibles.Add(GlobalDirection.North); + if (currentCell.Y != 0) + possibles.Add(GlobalDirection.South); + return possibles; } + + public Cell this[in int x, in int y] => Map[x, y]; } } \ No newline at end of file diff --git a/ConsoleApp/PathPlanner.cs b/ConsoleApp/PathPlanner.cs index 9b781fd..2a75207 100644 --- a/ConsoleApp/PathPlanner.cs +++ b/ConsoleApp/PathPlanner.cs @@ -1,4 +1,56 @@ -$HEADER$namespace $NAMESPACE$ +using System; +using System.Collections.Generic; +using ConsoleApp.Maps; + +namespace ConsoleApp { - public class $CLASS$ {$END$} + public class PathPlanner : IPathPlanner + { + public Queue GenerateOptimalSquarePath(SquareMap map, IVehicle vehicle) + { + var path = new Queue(); + var myCell = map.StartingCell; + var finished = false; + var width_cm = (double)(vehicle.DetectorWidth) * 100; + var swathOffset = (int)Math.Floor((decimal) (width_cm) / 25) + 1; + var currentHeading = GlobalDirection.North; + while (!finished) + { + var availableMoves = map.PossibleMoves(myCell); + if (availableMoves.Contains(GlobalDirection.North) && currentHeading == GlobalDirection.North && + myCell.Y != map.Height) + { + path.Enqueue(map[myCell.X, myCell.Y + 1]); + myCell = map[myCell.X, myCell.Y + 1]; + } + else if (availableMoves.Contains(GlobalDirection.South) && currentHeading == GlobalDirection.South && myCell.Y != 0) + { + path.Enqueue(map[myCell.X, myCell.Y - 1]); + myCell = map[myCell.X, myCell.Y - 1]; + } + else + { + if (myCell.X + swathOffset >= map.Width) + { + finished = true; + } + else + { + for (int i = myCell.X; i < myCell.X + swathOffset; i++) + { + path.Enqueue(map[i, myCell.Y]); + } + myCell = map[myCell.X+swathOffset, myCell.Y]; + if (currentHeading == GlobalDirection.North) + currentHeading = GlobalDirection.South; + else if (currentHeading == GlobalDirection.South) + currentHeading = GlobalDirection.North; + } + } + + } + + return path; + } + } } \ No newline at end of file diff --git a/ConsoleApp/SimRunner.cs b/ConsoleApp/SimRunner.cs index 5dbb361..5c55a52 100644 --- a/ConsoleApp/SimRunner.cs +++ b/ConsoleApp/SimRunner.cs @@ -10,58 +10,42 @@ namespace ConsoleApp { private IMapFactory _mapFactory; private IVehicle _vehicle; - private IMap _squareMap; - private IMap _hexMap; + private int _cellWidth; + private IPathPlanner _pathPlanner; - public SimRunner(IMapFactory mapFactory, IVehicle vehicle) + public SimRunner(IMapFactory mapFactory, IVehicle vehicle, IPathPlanner pathPlanner) { - _squareMap = mapFactory.Maps["SquareMap"]; - _hexMap = mapFactory.Maps["HexMap"]; + _cellWidth = mapFactory.CellWidth; _mapFactory = mapFactory; _vehicle = vehicle; + _pathPlanner = pathPlanner; } public void Run() { - _vehicle.CurrentHexCell = _hexMap.StartingCell; - _vehicle.CurrentSquareCell = _squareMap.StartingCell; - var squareTask = Task.Run(() => SquareSimulation()); - var hexTask = Task.Run(() => HexSimulation()); - while(!squareTask.IsCompleted && !hexTask.IsCompleted){Thread.Sleep(500);} - + + SquareSimulation(); + // var hexTask = Task.Run(() => HexSimulation()); + // while(!squareTask.IsCompleted && !hexTask.IsCompleted){Thread.Sleep(500);} + } private void HexSimulation() { - - var optimalPath = GenerateHexPath(); + var hexMap = (HexMap)_mapFactory.Maps["HexMap"]; + _vehicle.CurrentHexCell = hexMap.StartingCell; } private void SquareSimulation() { - var optimalPath = GenerateSquarePath(); + var squareMap = (SquareMap)_mapFactory.Maps["SquareMap"]; + _vehicle.CurrentSquareCell = squareMap.StartingCell; + var optimalPath = _pathPlanner.GenerateOptimalSquarePath(squareMap, _vehicle); } - private object GenerateSquarePath() - { - throw new NotImplementedException(); - } - - private Queue GenerateHexPath() - { - var path = new Queue(); - var currentCell = _vehicle.CurrentHexCell; - var possibles = _hexMap.PossibleMoves(currentCell); - while (currentCell != _hexMap.LastCell) - { - - } - - return path; - } } } \ No newline at end of file