public static int GetDistance(BlankGrid nodeA, BlankGrid nodeB) // type of distance?
{
int dstMin = Math.Min(Math.Abs(nodeA.X - nodeB.X), Math.Abs(nodeA.Y - nodeB.Y));
int dstMax = Math.Max(Math.Abs(nodeA.X - nodeB.X), Math.Abs(nodeA.Y - nodeB.Y));
return((14 * dstMin) + (10 * (dstMax - dstMin)));
}
static List <BlankGrid> RetracePath(BlankGrid startNode, BlankGrid endNode)
{
List <BlankGrid> path = new List <BlankGrid>();
BlankGrid currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.PathParent;
}
path.Reverse();
return(path);
}
public static List <BlankGrid> GetMoveNeighbors(CellState stateInfo, BlankGrid position, Tuple <int, int> dimensions)
{
List <Tuple <int, int> > neighbors = new List <Tuple <int, int> >();
int mNeighborhood = (int)(stateInfo.mobileNeighborhood);
if (mNeighborhood > 0)
{
neighbors.Add(new Tuple <int, int>(position.X - 1, position.Y));
neighbors.Add(new Tuple <int, int>(position.X + 1, position.Y));
neighbors.Add(new Tuple <int, int>(position.X, position.Y - 1));
neighbors.Add(new Tuple <int, int>(position.X, position.Y + 1));
if (mNeighborhood > 1)
{
neighbors.Add(new Tuple <int, int>(position.X - 1, position.Y - 1));
neighbors.Add(new Tuple <int, int>(position.X + 1, position.Y + 1));
neighbors.Add(new Tuple <int, int>(position.X + 1, position.Y - 1));
neighbors.Add(new Tuple <int, int>(position.X - 1, position.Y + 1));
}
}
switch (stateInfo.gridType)
{
case GridType.Box:
neighbors = neighbors.Where(x => (x.Item1 >= 0) && (x.Item1 < dimensions.Item1) && (x.Item2 >= 0) && (x.Item2 < dimensions.Item2)).ToList();
break;
case GridType.CylinderH:
neighbors = neighbors.Where(x => (x.Item1 >= 0) && (x.Item1 < dimensions.Item1)).ToList();
break;
case GridType.CylinderW:
neighbors = neighbors.Where(x => (x.Item2 >= 0) && (x.Item2 < dimensions.Item2)).ToList();
break;
case GridType.Torus:
break;
}
for (int i = 0; i < neighbors.Count; i++)
{
int x = neighbors[i].Item1;
int y = neighbors[i].Item2;
if (x < 0)
{
x += dimensions.Item1;
}
else if (x >= dimensions.Item1)
{
x -= dimensions.Item1;
}
if (y < 0)
{
y += dimensions.Item2;
}
else if (y >= dimensions.Item2)
{
y -= dimensions.Item2;
}
neighbors[i] = new Tuple <int, int>(x, y);
}
List <BlankGrid> neigh = neighbors.Select(x => position.Parent.grid[(x.Item1 * dimensions.Item1) + x.Item2]).Where(xx => xx != null).ToList();
return(neigh);
}
public static List <BlankGrid> FindPath(AgentController agent, BlankGrid destination, bool destinationEmpty) // how to handle destination containing something vs. being empty, or rather what to do when going to an object.
{
var startNode = agent.Cell;
// if contains agent, then make "target node" check for something around it.
HashSet <BlankGrid> closedSet = new HashSet <BlankGrid>();
//Heap<PathfindingNode> openSet = new Heap<PathfindingNode>(agent.Parent.gridHeight * agent.Parent.gridWidth);
List <BlankGrid> openSet = new List <BlankGrid>();
openSet.Add(startNode);
List <BlankGrid> targetNeighbors = GetMoveNeighbors(agent.Parent.GetStateInfo(agent.currentState), destination, new Tuple <int, int>(agent.Parent.gridWidth, agent.Parent.gridHeight));
while (openSet.Count > 0)
{
//PathfindingNode currentNode = openSet.RemoveFirst();// for heap implementation
BlankGrid currentNode = openSet[0];
for (int i = 0; i < openSet.Count; i++)
{
if (openSet[i].FCost < currentNode.FCost || openSet[i].FCost == currentNode.FCost && openSet[i].HCost < currentNode.HCost)
{
currentNode = openSet[i];
}
}
openSet.Remove(currentNode);
// end list version
closedSet.Add(currentNode);
if (destinationEmpty)
{
if (currentNode == destination)
{
return(RetracePath(startNode, currentNode));
}
}
else
{
foreach (var targetNeighbor in targetNeighbors)
{
if (currentNode == targetNeighbor)
{
return(RetracePath(startNode, currentNode));
}
}
}
List <BlankGrid> neighbors = GetMoveNeighbors(agent.Parent.GetStateInfo(agent.currentState), currentNode, new Tuple <int, int>(agent.Parent.gridWidth, agent.Parent.gridHeight));
foreach (var neighbor in neighbors)
{
if (neighbor.ContainsAgent || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbor = currentNode.GCost + GetDistance(currentNode, neighbor);
if (newMovementCostToNeighbor < neighbor.GCost || !openSet.Contains(neighbor))
{
neighbor.GCost = newMovementCostToNeighbor;
neighbor.HCost = GetDistance(neighbor, destination);
neighbor.PathParent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
//else
//{
// openSet[openSet.IndexOf(neighbor)]
//}
}
}
}
return(null);
}
