public void MoveTo(int x, int y, bool ignoreblocking = false) { SetupPathFinder(); if (Map.Map[x, y].Blocked && !ignoreblocking) { _currentpath = null; return; } TargetX = x; TargetY = y; var position = CordUtil.WorldToTile(transform.position); var currentx = position.First; var currenty = position.Second; Pathfinder.ResultFound = r => { _currentpath = r; if (_currentpath != null) { _nextworldpos = CordUtil.TileToWorld(_currentpath.X, _currentpath.Y); } }; _currentpath = null; Pathfinder.FindPath(currentx, currenty, x, y, this); }
private void btnGenerateMap_Click(object sender, EventArgs e) { txtMap.Text = ""; // Start with a clear map (don't add any obstacles) InitializeMap(7, 5, Point.Empty, Point.Empty, true); PathFinding pathFinder = new PathFinding(searchParameters); PathFindingResult pathResult = pathFinder.FindPath(); //txtMap.Text += ShowRoute("The algorithm should find a direct path without obstacles:", path); //txtMap.Text += Environment.NewLine; //// Now add an obstacle //InitializeMap(7, 5, Point.Empty, Point.Empty); //AddWallWithGap(); //pathFinder = new PathFinding(searchParameters); //path = pathFinder.FindPath(); //txtMap.Text += ShowRoute("The algorithm should find a route around the obstacle:", path); //txtMap.Text += Environment.NewLine; //// Create a barrier between the start and end points //InitializeMap(7, 5, Point.Empty, Point.Empty); //AddWallWithoutGap(); //pathFinder = new PathFinding(searchParameters); //path = pathFinder.FindPath(); //txtMap.Text += ShowRoute("The algorithm should not be able to find a route around the barrier:", path); //txtMap.Text += Environment.NewLine; //// Create a maze with custom start and end points //InitializeMap(7, 5, new Point(0, 4), new Point(6, 4)); //AddWallWithMaze(); //pathFinder = new PathFinding(searchParameters); //path = pathFinder.FindPath(); //txtMap.Text += ShowRoute("The algorithm should be able to find a long route around the barrier:", path); //txtMap.Text += Environment.NewLine; //// Create a maze with custom start and end points //InitializeMap(7, 5, new Point(0, 4), new Point(4, 2)); //AddWallWithSpinningMaze(); //pathFinder = new PathFinding(searchParameters); //path = pathFinder.FindPath(); //txtMap.Text += ShowRoute("The algorithm should be able to find a long route around the barrier:", path); //txtMap.Text += Environment.NewLine; // Create a larger maze with custom start and end points InitializeMap(70, 40, new Point(0, 0), new Point(69, 39), false); this.map = GenerateMap.GenerateRandomMap(this.map, 70, 40, 40); pathFinder = new PathFinding(searchParameters); pathResult = pathFinder.FindPath(); txtMap.Text += ShowRoute("The algorithm should be able to find a long route around the random blocks:", pathResult.Path); txtMap.Text += Environment.NewLine; //Console.WriteLine("Press any key to exit..."); //Console.ReadKey(); }
void FixedUpdate() { SetupPathFinder(); if (_currentpath == null) { return; } if (Vector3.Distance(_nextworldpos, transform.position) <= _distanceBeforeNext) { if (_currentpath.Next == null) { _currentpath = null; return; } if (Map.Map[_currentpath.Next.X, _currentpath.Next.Y].Blocked) { MoveTo(TargetX, TargetY); return; } _currentpath = _currentpath.Next; SwapPosition(_currentpath.X, _currentpath.Y); if (_currentpath == null) { return; } _nextworldpos = CordUtil.TileToWorld(_currentpath.X, _currentpath.Y); } transform.position = Vector3.MoveTowards(transform.position, _nextworldpos, Speed * Time.fixedDeltaTime); }
private void btnGenerateMap_Click(object sender, EventArgs e) { txtMap.Text = ""; int width = int.Parse(txtWidth.Text); int height = int.Parse(txtHeight.Text); int range = int.Parse(txtRange.Text); int startingWidth = 3; int startingHeight = 3; // Create a larger maze with custom start and end points InitializeMap(width, height, new Point(0, 0), new Point(width - 1, height - 1), false); AddRandomItems(width, height, 40); AddStartingLocation(startingWidth, startingHeight); //pathFinder = new PathFinding(searchParameters); //path = pathFinder.FindPath(); PathFinding pathFinder = new PathFinding(searchParameters); PathFindingResult pathResult = pathFinder.FindPath(); pathResult.Path.AddRange(PossibleTiles.FindTiles(new Point(0, 0), range, this.map)); txtMap.Text += ShowRoute("The algorithm should be able to find a long route around the random blocks:", pathResult.Path); txtMap.Text += Environment.NewLine; }
public PathFindingResult <PickingTravelStep> FindPath(PickingOrder order) { var result = new PathFindingResult <PickingTravelStep> { Success = true, }; var currentPosition = new PickingTravelStep(_warehouseLayout.GetPickingStartPosition(), new Dictionary <long, int>(order.RequiredArticles)); var possiblePickingSlots = order.RequiredArticles .SelectMany(x => _warehouseLayout.GetPickingSlotsWithSku(x.Key)) .OrderBy(x => x.AlleyNumber).ThenBy(x => x.PositionNumber) .ToList(); var precalculatedRoutes = GetRoutesBetweenSlots(possiblePickingSlots); var remainingInOrder = new Dictionary <long, int>(order.RequiredArticles); foreach (var possiblePickingSlot in possiblePickingSlots) { var previousPosition = currentPosition; if (!remainingInOrder.TryGetValue(possiblePickingSlot.Sku, out int positionUnits) || remainingInOrder[possiblePickingSlot.Sku] < 1) { continue; } if (positionUnits < 1) { continue; } var unitsToTake = Math.Min(positionUnits, remainingInOrder[possiblePickingSlot.Sku]); _warehouseLayout.GetPickingSlots().FirstOrDefault(x => x == possiblePickingSlot).Units -= unitsToTake; remainingInOrder[possiblePickingSlot.Sku] -= unitsToTake; currentPosition = new PickingTravelStep(possiblePickingSlot, unitsToTake, new Dictionary <long, int>(remainingInOrder)); currentPosition.Parent = previousPosition; if (!remainingInOrder.Any(x => x.Value > 0)) { break; } } var endPosition = _warehouseLayout.GetPickingEndPosition(); var finalStep = new PickingTravelStep(endPosition, currentPosition.PendingSkus); finalStep.CostFromStart = currentPosition.CostFromStart + FindTravelCostBetween(precalculatedRoutes, finalStep.Position, currentPosition.Position); finalStep.Parent = currentPosition; currentPosition = finalStep; var steps = new List <ITravelStep>(); while (currentPosition != null) { var nextPosition = currentPosition.Parent; currentPosition.Parent = null; steps.Add(currentPosition); if (nextPosition != null && currentPosition != (PickingTravelStep)nextPosition) { var route = FindTravelRouteBetween(precalculatedRoutes, currentPosition.Position, nextPosition.Position).Route; foreach (var coord in route) { result.PathCoordinates.Add(coord); } } currentPosition = nextPosition as PickingTravelStep; } result.Steps = steps.ToArray(); return(result); }
public PathFindingResult <PickingTravelStep> FindPath(PickingOrder order) { var result = new PathFindingResult <PickingTravelStep> { Success = true, }; var openList = new Heap <PickingTravelStep>(); var currentPosition = new PickingTravelStep(_warehouseLayout.GetPickingStartPosition(), new Dictionary <long, int>(order.RequiredArticles)); var possiblePickingSlots = order.RequiredArticles .SelectMany(x => _warehouseLayout.GetPickingSlotsWithSku(x.Key)) .ToList(); var routesBetweenSlots = GetRoutesBetweenSlots(possiblePickingSlots); var endPosition = _warehouseLayout.GetPickingEndPosition(); while (currentPosition.PendingSkus.Any(x => x.Value > 0)) { if (currentPosition.Sku > 0 && currentPosition.PickingSlot.Units > 0 && currentPosition.PendingSkus.ContainsKey(currentPosition.Sku)) { var requiredUnits = currentPosition.PendingSkus[currentPosition.Sku]; var unitsToTake = Math.Min(requiredUnits, currentPosition.PickingSlot.AvailableUnits); currentPosition.PendingSkus[currentPosition.Sku] -= unitsToTake; } if (currentPosition.PendingSkus.All(x => x.Value == 0)) { result.Success = true; break; } var possibleNextSlots = possiblePickingSlots.Where(x => !currentPosition.VisitedSlots.Contains(x) && currentPosition.PendingSkus.TryGetValue(x.Sku, out var value) && value > 0) .ToList(); if (!possibleNextSlots.Any()) { result.Success = false; break; } foreach (var nextSlot in possibleNextSlots) { var remainingRequiredUnits = currentPosition.PendingSkus[nextSlot.Sku]; var unitsToTake = Math.Min(remainingRequiredUnits, nextSlot.Units - nextSlot.ReservedUnits); var remainingSlots = possibleNextSlots.Where(x => x != nextSlot); if (unitsToTake == remainingRequiredUnits) { remainingSlots = remainingSlots.Where(x => x.Sku != nextSlot.Sku); } var remainingSlotsList = remainingSlots.ToList(); var next = new PickingTravelStep(nextSlot, unitsToTake, new Dictionary <long, int>(currentPosition.PendingSkus)) { Parent = currentPosition, }; next.CostFromStart = currentPosition.CostFromStart + FindTravelCostBetween(routesBetweenSlots, next.Position, currentPosition.Position); next.VisitedSlots = new List <PickingSlot>(currentPosition.VisitedSlots) { nextSlot }; var tentativeCost = FindTravelCostBetween(routesBetweenSlots, nextSlot.Position, endPosition); tentativeCost += currentPosition.CostFromStart; tentativeCost += remainingSlotsList.Sum(x => FindTravelCostBetween(routesBetweenSlots, nextSlot.Position, x.Position)); openList.Add(new HeapNode <PickingTravelStep>(next, tentativeCost)); } currentPosition = openList.TakeHeapHeadPosition(); } var finalStep = new PickingTravelStep(endPosition, currentPosition.PendingSkus); finalStep.CostFromStart = currentPosition.CostFromStart + FindTravelCostBetween(routesBetweenSlots, finalStep.Position, currentPosition.Position); finalStep.Parent = currentPosition; currentPosition = finalStep; // recreate found path and reserve items on stock var pickedArticles = order.RequiredArticles.Select(x => x.Key).ToDictionary(x => x, x => 0); var steps = new List <ITravelStep>(); while (currentPosition != null) { if (pickedArticles.ContainsKey(currentPosition.Sku)) { pickedArticles[currentPosition.Sku] += currentPosition.UnitsToTake; } if (currentPosition.PickingSlot != null) { _warehouseLayout.ReserveArticles(currentPosition.PickingSlot.Address, currentPosition.Sku, currentPosition.UnitsToTake); } var nextPosition = currentPosition.Parent; currentPosition.Parent = null; steps.Add(currentPosition); if (nextPosition != null && currentPosition != (PickingTravelStep)nextPosition) { var route = FindTravelRouteBetween(routesBetweenSlots, currentPosition.Position, nextPosition.Position).Route; foreach (var coord in route) { result.PathCoordinates.Add(coord); } } currentPosition = nextPosition as PickingTravelStep; } result.Steps = steps.ToArray(); return(result); }
public PathFindingResult <TravelStep> FindPath(TravelStep startTravelStep, TravelStep endTravelStep, bool traverse) { var result = new PathFindingResult <TravelStep>(); if (!_warehouseLayout.IsWalkable(startTravelStep.Position)) { return(new PathFindingResult <TravelStep>() { Success = false }); } if (!_warehouseLayout.IsWalkable(endTravelStep.Position)) { return(new PathFindingResult <TravelStep>() { Success = false }); } var openList = new Heap <TravelStep>(); var closedList = new byte[_warehouseLayout.Width + 1, _warehouseLayout.Height + 1]; var addedToOpenList = new int[_warehouseLayout.Width + 1, _warehouseLayout.Height + 1]; var currentPosition = startTravelStep; openList.Add(new HeapNode <TravelStep>(currentPosition, currentPosition.TraverseCost * currentPosition.EuclidianDistanceTo(endTravelStep))); var stepCount = 0; while (openList.HasMore()) { stepCount++; currentPosition = openList.TakeHeapHeadPosition(); if (currentPosition == endTravelStep) // sukces { break; } closedList[currentPosition.X, currentPosition.Y] = 1; var movementOptions = GetAvailableTravelSteps(currentPosition, traverse); // dodaj wyszukane do sterty foreach (var position in movementOptions) { result.CheckedCoordinates.Add(position.Position); if (closedList[position.X, position.Y] == 1) { continue; } //var tentativeGScore = currentPosition.CostFromStart + currentPosition.EuclidianDistanceTo(position); var gScore = currentPosition.CostFromStart + position.TraverseCost; var cost = gScore + currentPosition.CostFromStart * position.ManhattanDistanceTo(endTravelStep); position.Parent = currentPosition; position.CostFromStart = gScore; if (addedToOpenList[position.X, position.Y] == 0) { openList.Add(new HeapNode <TravelStep>(position, cost)); addedToOpenList[position.X, position.Y] = (int)gScore; } else if (addedToOpenList[position.X, position.Y] > gScore) { openList.Reinsert(new HeapNode <TravelStep>(position, cost)); } } } result.Success = currentPosition == endTravelStep; //Debug.WriteLine("Wykonano krokow: " + stepCount + ". Wynik końcowy: " + (currentPosition == endTravelStep ? "POWODZENIE" : "NIEPOWODZENIE")); // powrót po śladach var steps = new List <ITravelStep>(); while (currentPosition != startTravelStep) { var nextPosition = currentPosition.Parent; currentPosition.Parent = null; steps.Add(currentPosition); result.PathCoordinates.Add(currentPosition.Position); currentPosition = nextPosition as TravelStep; } steps.Reverse(); result.Steps = steps.ToArray(); return(result); }
public PathFindingResult FindPath(Position startPosition, Position endPosition, bool traverse) { var result = new PathFindingResult { PathCoordinates = new byte[_grid.SizeX + 1, _grid.SizeY + 1], CheckedCoordinates = new byte[_grid.SizeX + 1, _grid.SizeY + 1], }; if (!_grid.IsPositionAvailable(startPosition)) { throw new Exception("Start position unavailable"); } if (!_grid.IsPositionAvailable(endPosition)) { throw new Exception("End position unavailable"); } var openList = new Heap(); // lista zamkniętych lokalizacji w postaci tablicy bitów diametralnie skraca czas wykonania // algorytmu względem List<Position> (3 min -> 0,3 sec !!) var closedList = new byte[_grid.SizeX + 1, _grid.SizeY + 1]; var addedToOpenList = new int[_grid.SizeX + 1, _grid.SizeY + 1]; var currentPosition = startPosition; openList.Add(new HeapNode(currentPosition, currentPosition.TraverseCost * currentPosition.EuclidianDistanceTo(endPosition))); var stepCount = 0; while (openList.HasMore()) { stepCount++; currentPosition = openList.TakeHeapHeadPosition(); if (currentPosition == endPosition) // sukces! { break; } closedList[currentPosition.X, currentPosition.Y] = 1; // wyszukaj możliwe i niewykorzystane możliwości ruchów var movementOptions = _grid.GetAvailableNeighbours(currentPosition, traverse); // dodaj wyszukane do stosu foreach (var position in movementOptions) { result.CheckedCoordinates[position.X, position.Y] = 1; if (closedList[position.X, position.Y] == 1) { continue; } var tentativeGScore = currentPosition.CostFromStart + currentPosition.EuclidianDistanceTo(position); var gScore = currentPosition.CostFromStart + position.TraverseCost; var cost = gScore + position.TraverseCost * position.EuclidianDistanceTo(endPosition); position.Parent = currentPosition; position.CostFromStart = (int)gScore; if (addedToOpenList[position.X, position.Y] == 0) { openList.Add(new HeapNode(position, cost)); addedToOpenList[position.X, position.Y] = (int)cost; } else if (addedToOpenList[position.X, position.Y] != 0 && addedToOpenList[position.X, position.Y] > tentativeGScore) { openList.Reinsert(new HeapNode(position, cost)); } } } result.Success = currentPosition == endPosition; result.FinalCost = currentPosition.CostFromStart; Debug.WriteLine("Wykonano krokow: " + stepCount + ". Wynik końcowy: " + (currentPosition == endPosition ? "POWODZENIE" : "NIEPOWODZENIE")); // powrót po śladach while (currentPosition != startPosition) { var nextPosition = currentPosition.Parent; currentPosition.Parent = null; result.PathCoordinates[currentPosition.X, currentPosition.Y] = 1; currentPosition = nextPosition; } return(result); }