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);
}
