public static PathfinderResult Find(char[,] map_, Point start, Point end)
{
var map = map_;
var startChar = map[start.Y, start.X];
map[start.Y, start.X] = '0';
// We can't start the search from the original point, so we need to find which of the below 4 is the best route
var leftStart = new Point(start.X - 1, start.Y);
var rightStart = new Point(start.X + 1, start.Y);
var upStart = new Point(start.X, start.Y - 1);
var downStart = new Point(start.X, start.Y + 1);
var startPoints = new List <Point>()
{
start
//leftStart,
//rightStart,
//upStart,
//downStart,
};
var errors = new List <string>();
for (int i = 0; i < startPoints.Count; i++)
{
var result = ValidatePositions(map, startPoints[i], end);
if (result.Status == PathStatus.Invalid)
{
errors.AddRange(result.Errors);
startPoints.RemoveAt(i);
i--;
}
}
if (startPoints.Count == 0)
{
map[start.Y, start.X] = startChar;
return(new PathfinderResult()
{
Status = PathStatus.Invalid,
Path = new List <Point>(),
Errors = errors,
});
}
var endResults = new List <PathfinderResult>();
foreach (var startPoint in startPoints)
{
//var straightLineStatus = GetStraightLineStatus(map, startPoint, end);
//if (straightLineStatus.Status == PathStatus.Valid)
//{
// return straightLineStatus;
//}
// nodes that have already been analyzed and have a path from the start to them
var closedSet = new List <Point>();
// nodes that have been identified as a neighbor of an analyzed node, but have
// yet to be fully analyzed
var openSet = new List <Point> {
startPoint
};
// a dictionary identifying the optimal origin point to each node. this is used
// to back-track from the end to find the optimal path
var cameFrom = new Dictionary <Point, Point>();
// a dictionary indicating how far each analyzed node is from the start
var currentDistance = new Dictionary <Point, int>();
// a dictionary indicating how far it is expected to reach the end, if the path
// travels through the specified node.
var predictedDistance = new Dictionary <Point, float>();
// initialize the start node as having a distance of 0, and an estmated distance
// of y-distance + x-distance, which is the optimal path in a square grid that
// doesn't allow for diagonal movement
currentDistance.Add(startPoint, 0);
predictedDistance.Add(
startPoint, Math.Abs(startPoint.X - end.X) + Math.Abs(startPoint.Y - end.Y)
);
// if there are any unanalyzed nodes, process them
while (openSet.Count > 0)
{
// get the node with the lowest estimated cost to finish
var current = (
from p in openSet orderby predictedDistance[p] ascending select p
).First();
// if it is the finish, return the path
if (current.X == end.X && current.Y == end.Y)
{
endResults.Add(new PathfinderResult()
{
Status = PathStatus.Valid,
Path = ReconstructPath(cameFrom, end),
});
}
// move current node from open to closed
openSet.Remove(current);
closedSet.Add(current);
// process each valid node around the current node
var neighbours = GetNeighbourNodes(map, current);
foreach (var neighbor in neighbours)
{
var tempCurrentDistance = currentDistance[current] + 1;
// if we already know a faster way to this neighbor, use that route and
// ignore this one
if (closedSet.Contains(neighbor) &&
tempCurrentDistance >= currentDistance[neighbor])
{
continue;
}
// if we don't know a route to this neighbor, or if this is faster,
// store this route
if (!closedSet.Contains(neighbor) ||
tempCurrentDistance < currentDistance[neighbor])
{
if (cameFrom.Keys.Contains(neighbor))
{
cameFrom[neighbor] = current;
}
else
{
cameFrom.Add(neighbor, current);
}
currentDistance[neighbor] = tempCurrentDistance;
predictedDistance[neighbor] =
currentDistance[neighbor]
+ Math.Abs(neighbor.X - end.X)
+ Math.Abs(neighbor.Y - end.Y);
// if this is a new node, add it to processing
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
var endResult = new PathfinderResult()
{
Status = PathStatus.Invalid,
Path = new List <Point>(),
Errors = new List <string>(),
};
// unable to figure out a path, abort.
endResult.Errors.Add(
string.Format(
"unable to find a path between ({0},{1}) and ({2},{3})",
startPoint.X, startPoint.Y,
end.X, end.Y
)
);
endResults.Add(endResult);
}
var validResults = endResults.Where(c => c.Status == PathStatus.Valid);
var invalidResults = endResults.Where(c => c.Status == PathStatus.Invalid);
map[start.Y, start.X] = startChar;
if (validResults.Count() > 0)
{
var newPath = validResults.OrderBy(c => c.Path.Count).FirstOrDefault().Path;
newPath.RemoveAt(0);
return(new PathfinderResult()
{
Status = PathStatus.Valid,
Path = newPath,
});
}
else
{
return(new PathfinderResult()
{
Status = PathStatus.Invalid,
Path = new List <Point>(),
//Errors = new List<string>(invalidResults.SelectMany(c => c.Errors)),
});
}
}
/// <summary>
/// Check if the positions are on the map, and aren't on an 'unwalkable' character
/// </summary>
/// <param name="map">What we need to navigate through</param>
/// <param name="start">Where we start</param>
/// <param name="end">Where we end</param>
private static PathfinderResult ValidatePositions(char[,] map, Point start, Point end)
{
var result = new PathfinderResult()
{
Errors = new List <string>(),
Path = new List <Point>(),
Status = PathStatus.Valid,
};
result.Errors = new List <string>();
if (start.X < 0)
{
result.Errors.Add("Start.X isn't on map: " + start.X);
}
if (start.X >= map.GetLength(1))
{
result.Errors.Add("Start.Y isn't on map: " + start.Y);
}
if (end.X < 0)
{
result.Errors.Add("End.X isn't on map: " + end.X);
}
if (end.X >= map.GetLength(0))
{
result.Errors.Add("End.Y isn't on map: " + end.Y);
}
try
{
var startCharacter = map[start.Y, start.X];
if (!_walkableCharacters.Contains(startCharacter))
{
result.Errors.Add(string.Format("Start position ({0}) is on 'unwalkable' character: {1}", start.ToString(), startCharacter));
}
}
catch (IndexOutOfRangeException e)
{
result.Errors.Add(e.ToString());
}
try
{
var endCharacter = map[end.Y, end.X];
if (!_walkableCharacters.Contains(endCharacter))
{
result.Errors.Add(string.Format("Start position ({0}) is on 'unwalkable' character: {1}", end.ToString(), endCharacter));
}
}
catch (IndexOutOfRangeException e)
{
result.Errors.Add(e.ToString());
}
if (result.Errors.Count > 0)
{
result.Status = PathStatus.Invalid;
}
return(result);
}
