protected void Initialize(int sx, int sy)
{
mx = sx;
my = sy;
map = new PathNode2D[mx, my];
for (int x = 0; x < mx; x++)
{
for (int y = 0; y < my; y++)
{
map[x, y] = new PathNode2D(x, y);
}
}
}
protected override bool VerifyPassConditions(PathNode2D node, PathNode2D parent, int d, Dictionary <string, int> passConditions)
{
bool isPassable = true;
if (d % 2 == 0 & passConditions["diagonalBlocking"] == 1)
{
if (IsInMapBounds(parent.X + dirRectGrid2D[RectGrid2D_NextDir(d), 0], parent.Y + dirRectGrid2D[RectGrid2D_NextDir(d), 1]))
{
isPassable = isPassable && map[parent.X + dirRectGrid2D[RectGrid2D_NextDir(d), 0], parent.Y + dirRectGrid2D[RectGrid2D_NextDir(d), 1]].GetBool("isPassable");
}
if (IsInMapBounds(parent.X + dirRectGrid2D[RectGrid2D_PrevDir(d), 0], parent.Y + dirRectGrid2D[RectGrid2D_PrevDir(d), 1]))
{
isPassable = isPassable && map[parent.X + dirRectGrid2D[RectGrid2D_PrevDir(d), 0], parent.Y + dirRectGrid2D[RectGrid2D_PrevDir(d), 1]].GetBool("isPassable");
}
}
return(isPassable && base.VerifyPassConditions(node, parent, d, passConditions));
}
public PathNode2D(PathNode2D source)
{
X = source.X;
Y = source.Y;
foreach (KeyValuePair <string, int> p in source.properties_i)
{
properties_i.Add(p.Key, p.Value);
}
foreach (KeyValuePair <string, string> p in source.properties_s)
{
properties_s.Add(p.Key, p.Value);
}
foreach (KeyValuePair <string, float> p in source.properties_f)
{
properties_f.Add(p.Key, p.Value);
}
foreach (KeyValuePair <string, bool> p in source.properties_b)
{
properties_b.Add(p.Key, p.Value);
}
}
protected override void AssignNodeProperties(PathNode2D node, PathNode2D parent, int d, Dictionary <string, int> passConditions, Dictionary <string, int> pathWeights)
{
base.AssignNodeProperties(node, parent, d, passConditions, pathWeights);
}
private static int ComparePathWeights(PathNode2D a, PathNode2D b)
{
return(a.GetInt("pathWeight").CompareTo(b.GetInt("pathWeight")));
}
public GridPath2D(List <PathNode2D> path)
{
Path = path;
PathStart = Path.Count > 0 ? Path[Path.Count - 1] : null;
PathEnd = Path.Count > 0 ? Path[0] : null;
}
public GridPath2D FindPath(
int startX, int startY,
int endX, int endY,
bool allowStraight = true,
bool allowDiagonal = true,
Options options = null,
Dictionary <string, int> sortingOptions = null,
Dictionary <string, int> passConditions = null,
Dictionary <string, int> pathWeights = null
)
{
// Pre-check
if (map == null)
{
throw new Exception("Map array is not initialized!");
}
if (!IsInMapBounds(startX, startY))
{
throw new Exception("Starting coordinates are out of map bounds!");
}
if (!IsInMapBounds(endX, endY))
{
throw new Exception("Target coordinates are out of map bounds!");
}
// Clear map/path flags
for (int x = 0; x < mx; x++)
{
for (int y = 0; y < my; y++)
{
map[x, y].SetProperty("isProcessed", false);
map[x, y].SetProperty("isPath", false);
map[x, y].SetProperty("isBetterCheckEnabled", Math.Sqrt(Math.Pow(x - endX, 2) + Math.Pow(y - endY, 2)) < options.isBetterCheckRadius);
}
}
// Init.
if (options == null)
{
options = new Options();
}
List <PathNode2D> toBeProcessed = new List <PathNode2D>();
List <PathNode2D> processed = new List <PathNode2D>();
List <PathNode2D> endPoints = new List <PathNode2D>();
map[startX, startY].SetProperty("isProcessed", true);
// Starting node
PathNode2D n = new PathNode2D(map[startX, startY]);
n.SetProperty("parent", -1);
n.SetProperty("stack", 0);
processed.Add(n);
toBeProcessed.Add(n);
/// Find path
/// ---------
while (toBeProcessed.Count > 0 && endPoints.Count < options.maxResults)
{
// Sort open stack
if (options.enableOpenStackSorting)
{
SortOpenStack(ref toBeProcessed, sortingOptions);
}
// Process open stack
for (int i = toBeProcessed.Count - 1; i >= 0; i--)
{
PathNode2D node = toBeProcessed[i];
for (int j = 7; j >= 0; j--)
{
// Get direction
int d = dirRectGrid2DEnumHelper[node.Dir][j];
if (!allowStraight && d % 2 == 1)
{
continue;
}
if (!allowDiagonal && d % 2 == 0)
{
continue;
}
// Get & validate coords
int newX = node.X + dirRectGrid2D[d, 0];
int newY = node.Y + dirRectGrid2D[d, 1];
if (!IsInMapBounds(newX, newY))
{
continue;
}
// Check conditions
PathNode2D mapNode = map[newX, newY];
if (mapNode.GetBool("isProcessed") && (!mapNode.GetBool("isBetterCheckEnabled") || !IsBetterThan(node, mapNode)))
{
continue;
}
if (!VerifyPassConditions(mapNode, node, d, passConditions))
{
continue;
}
// Process node
PathNode2D nextNode = new PathNode2D(map[newX, newY]);
nextNode.SetProperty("isProcessed", true);
nextNode.SetProperty("parent", node.GetInt("stack"));
nextNode.SetProperty("stack", processed.Count);
SetDistance(nextNode, node);
AssignNodeProperties(nextNode, node, d, passConditions, pathWeights); // use this to propagate additional data like accumulated custom weights (number of direction changes, hazards encountered, etc.) from source to target node
processed.Add(nextNode);
map[newX, newY] = nextNode;
// Target found
if (nextNode.X == endX && nextNode.Y == endY)
{
endPoints.Add(nextNode);
}
else
{
toBeProcessed.Add(nextNode);
}
}
toBeProcessed.RemoveAt(i);
}
}
/// ---------
List <PathNode2D> path = new List <PathNode2D>();
/// Construct path
/// ---------
if (endPoints.Count > 0)
{
SortOpenStack(ref endPoints, sortingOptions);
int pathFound = endPoints.Count - 1;
map[endPoints[pathFound].X, endPoints[pathFound].Y].SetProperty("isPath", true);
path.Add(endPoints[pathFound]);
int parent = endPoints[pathFound].GetInt("parent");
do
{
int px = processed[parent].X;
int py = processed[parent].Y;
map[px, py].SetProperty("isPath", true);
if (px != startX || py != startY)
{
path.Add(processed[parent]);
}
parent = processed[parent].GetInt("parent");
} while (parent != -1);
}
/// ---------
// Result
GridPath2D result = new GridPath2D(path);
return(result);
}
protected virtual bool IsBetterThan(PathNode2D a, PathNode2D b)
{
return((a.Dist + 1) < b.Dist);
}
protected virtual void AssignNodeProperties(PathNode2D node, PathNode2D parent, int d, Dictionary <string, int> passConditions, Dictionary <string, int> pathWeights)
{
node.Dir = d;
}
protected virtual void SetDistance(PathNode2D node, PathNode2D parent)
{
node.Dist = parent.Dist + 1;
}
protected virtual bool VerifyPassConditions(PathNode2D node, PathNode2D parent, int d, Dictionary <string, int> passConditions)
{
return(node.GetBool("isPassable"));
}