public static int GetStepsNum(MatrixNode node)
{
int step = 0;
if (node != null)
{
step++;
step += GetStepsNum(node.parent);
}
return(step);
}
internal static MatrixNode GetFirstStep(MatrixNode node)
{
MatrixNode parent = null;
if (node.parent != null && node.parent.fromDistance > 0)
{
parent = GetFirstStep(node.parent);
}
else
{
parent = node;
}
return(parent);
}
public static MatrixNode AStar(char[,] matrix, int fromX, int fromY, int toX, int toY, char[] walls)
{
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// in this version an element in a matrix can move left/up/right/down in one step, two steps for a diagonal move.
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//the keys for greens and reds are x.ToString() + y.ToString() of the matrixNode
Dictionary <string, MatrixNode> greens = new Dictionary <string, MatrixNode>(); //open
Dictionary <string, MatrixNode> reds = new Dictionary <string, MatrixNode>(); //closed
MatrixNode startNode = new MatrixNode {
x = fromX, y = fromY
};
string key = startNode.x.ToString() + startNode.y.ToString();
greens.Add(key, startNode);
KeyValuePair <string, MatrixNode> smallestGreen()
{
KeyValuePair <string, MatrixNode> smallest = greens.ElementAt(0);
foreach (KeyValuePair <string, MatrixNode> item in greens)
{
if (item.Value.sumDistance < smallest.Value.sumDistance)
{
smallest = item;
}
else if (item.Value.sumDistance == smallest.Value.sumDistance && item.Value.toDistance < smallest.Value.toDistance)
{
smallest = item;
}
}
return(smallest);
}
//add these values to current node's x and y values to get the left/up/right/bottom neighbors
List <KeyValuePair <int, int> > fourNeighbors = new List <KeyValuePair <int, int> >()
{
new KeyValuePair <int, int>(-1, 0),
new KeyValuePair <int, int>(0, 1),
new KeyValuePair <int, int>(1, 0),
new KeyValuePair <int, int>(0, -1)
};
int maxX = matrix.GetLength(0);
if (maxX == 0)
{
return(null);
}
int maxY = matrix.GetLength(1);
while (true)
{
if (greens.Count == 0)
{
return(null);
}
KeyValuePair <string, MatrixNode> current = smallestGreen();
if (current.Value.x == toX && current.Value.y == toY)
{
return(current.Value);
}
greens.Remove(current.Key);
reds.Add(current.Key, current.Value);
foreach (KeyValuePair <int, int> plusXY in fourNeighbors)
{
int nbrX = current.Value.x + plusXY.Key;
int nbrY = current.Value.y + plusXY.Value;
string nbrKey = nbrX.ToString() + nbrY.ToString();
if (nbrX < 0 || nbrY < 0 || nbrX >= maxX || nbrY >= maxY ||
walls.Contains(matrix[nbrX, nbrY]) || // 'X' //obstacles marked by 'X'
reds.ContainsKey(nbrKey))
{
continue;
}
if (greens.ContainsKey(nbrKey))
{
MatrixNode curNbr = greens[nbrKey];
int from = Math.Abs(nbrX - fromX) + Math.Abs(nbrY - fromY);
if (from < curNbr.fromDistance)
{
curNbr.fromDistance = from;
curNbr.sumDistance = curNbr.fromDistance + curNbr.toDistance;
curNbr.parent = current.Value;
}
}
else
{
MatrixNode curNbr = new MatrixNode {
x = nbrX, y = nbrY
};
curNbr.fromDistance = Math.Abs(nbrX - fromX) + Math.Abs(nbrY - fromY);
curNbr.toDistance = Math.Abs(nbrX - toX) + Math.Abs(nbrY - toY);
curNbr.sumDistance = curNbr.fromDistance + curNbr.toDistance;
curNbr.parent = current.Value;
greens.Add(nbrKey, curNbr);
}
}
}
}
