public Node SetNode(GridPos iPos, bool? iWalkable = null)
{
if (iWalkable.HasValue)
{
if (iWalkable.Value == true)
{
if (m_nodes.ContainsKey(iPos))
return m_nodes[iPos];
Node newNode = new Node(iPos.x, iPos.y, iWalkable);
m_nodes.Add(iPos, newNode);
return newNode;
}
else
{
removeNode(iPos);
}
}
else
{
Node newNode = new Node(iPos.x, iPos.y, true);
m_nodes.Add(iPos, newNode);
return newNode;
}
return null;
}
public override BaseGrid Clone()
{
int tWidth = width;
int tHeight = height;
Node[][] tNodes = this.m_nodes;
StaticGrid tNewGrid = new StaticGrid(tWidth, tHeight, null);
Node[][] tNewNodes = new Node[tWidth][];
for (int widthTrav = 0; widthTrav < tWidth; widthTrav++)
{
tNewNodes[widthTrav] = new Node[tHeight];
for (int heightTrav = 0; heightTrav < tHeight; heightTrav++)
{
tNewNodes[widthTrav][heightTrav] = new Node(widthTrav, heightTrav, tNodes[widthTrav][heightTrav].walkable);
}
}
tNewGrid.m_nodes = tNewNodes;
return tNewGrid;
}
private Node[][] buildNodes(int iWidth, int iHeight, bool[][] iMatrix)
{
Node[][] tNodes = new Node[iWidth][];
for (int widthTrav = 0; widthTrav < iWidth; widthTrav++)
{
tNodes[widthTrav] = new Node[iHeight];
for (int heightTrav = 0; heightTrav < iHeight; heightTrav++)
{
tNodes[widthTrav][heightTrav] = new Node(widthTrav, heightTrav, null);
}
}
if (iMatrix == null)
{
return tNodes;
}
if (iMatrix.Length != iWidth || iMatrix[0].Length != iHeight)
{
throw new System.ApplicationException("Matrix size does not fit");
}
for (int widthTrav = 0; widthTrav < iWidth; widthTrav++)
{
for (int heightTrav = 0; heightTrav < iHeight; heightTrav++)
{
if (iMatrix[widthTrav][heightTrav])
{
tNodes[widthTrav][heightTrav].walkable = true;
}
else
{
tNodes[widthTrav][heightTrav].walkable = false;
}
}
}
return tNodes;
}
public JumpPointParam(BaseGrid iGrid, GridPos iStartPos, GridPos iEndPos, bool iAllowEndNodeUnWalkable = true, bool iCrossCorner = true, bool iCrossAdjacentPoint = true, HeuristicMode iMode = HeuristicMode.EUCLIDEAN)
{
switch (iMode)
{
case HeuristicMode.MANHATTAN:
m_heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
m_heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
m_allowEndNodeUnWalkable = iAllowEndNodeUnWalkable;
m_crossAdjacentPoint = iCrossAdjacentPoint;
m_crossCorner = iCrossCorner;
openList = new List<Node>();
m_searchGrid = iGrid;
m_startNode = m_searchGrid.GetNodeAt(iStartPos.x, iStartPos.y);
m_endNode = m_searchGrid.GetNodeAt(iEndPos.x, iEndPos.y);
if (m_startNode == null)
m_startNode = new Node(iStartPos.x, iStartPos.y, true);
if (m_endNode == null)
m_endNode = new Node(iEndPos.x, iEndPos.y, true);
m_useRecursive = false;
}
private static void identifySuccessors(JumpPointParam iParam, Node iNode)
{
HeuristicDelegate tHeuristic = iParam.HeuristicFunc;
List<Node> tOpenList = iParam.openList;
int tEndX = iParam.EndNode.x;
int tEndY = iParam.EndNode.y;
GridPos tNeighbor;
GridPos? tJumpPoint;
Node tJumpNode;
List<GridPos> tNeighbors = findNeighbors(iParam, iNode);
for (int i = 0; i < tNeighbors.Count; i++)
{
tNeighbor = tNeighbors[i];
if(iParam.UseRecursive)
tJumpPoint = jump(iParam, tNeighbor.x, tNeighbor.y, iNode.x, iNode.y);
else
tJumpPoint = jumpLoop(iParam, tNeighbor.x, tNeighbor.y, iNode.x, iNode.y);
if (tJumpPoint != null)
{
tJumpNode = iParam.SearchGrid.GetNodeAt(tJumpPoint.Value.x, tJumpPoint.Value.y);
if (tJumpNode == null)
{
if (iParam.EndNode.x == tJumpPoint.Value.x && iParam.EndNode.y == tJumpPoint.Value.y)
tJumpNode = iParam.SearchGrid.GetNodeAt(tJumpPoint.Value);
}
if (tJumpNode.isClosed)
{
continue;
}
// include distance, as parent may not be immediately adjacent:
float tCurNodeToJumpNodeLen = tHeuristic(Math.Abs(tJumpPoint.Value.x - iNode.x), Math.Abs(tJumpPoint.Value.y - iNode.y));
float tStartToJumpNodeLen = iNode.startToCurNodeLen + tCurNodeToJumpNodeLen; // next `startToCurNodeLen` value
if (!tJumpNode.isOpened || tStartToJumpNodeLen < tJumpNode.startToCurNodeLen)
{
tJumpNode.startToCurNodeLen = tStartToJumpNodeLen;
tJumpNode.heuristicCurNodeToEndLen = (tJumpNode.heuristicCurNodeToEndLen == null ? tHeuristic(Math.Abs(tJumpPoint.Value.x - tEndX), Math.Abs(tJumpPoint.Value.y - tEndY)) : tJumpNode.heuristicCurNodeToEndLen);
tJumpNode.heuristicStartToEndLen = tJumpNode.startToCurNodeLen + tJumpNode.heuristicCurNodeToEndLen.Value;
tJumpNode.parent = iNode;
if (!tJumpNode.isOpened)
{
tOpenList.Add(tJumpNode);
tJumpNode.isOpened = true;
}
}
}
}
}
private static List<GridPos> findNeighbors(JumpPointParam iParam, Node iNode)
{
Node tParent = (Node)iNode.parent;
int tX = iNode.x;
int tY = iNode.y;
int tPx, tPy, tDx, tDy;
List<GridPos> tNeighbors = new List<GridPos>();
List<Node> tNeighborNodes;
Node tNeighborNode;
// directed pruning: can ignore most neighbors, unless forced.
if (tParent != null)
{
tPx = tParent.x;
tPy = tParent.y;
// get the normalized direction of travel
tDx = (tX - tPx) / Math.Max(Math.Abs(tX - tPx), 1);
tDy = (tY - tPy) / Math.Max(Math.Abs(tY - tPy), 1);
if (iParam.CrossCorner)
{
// search diagonally
if (tDx != 0 && tDy != 0)
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy))
{
tNeighbors.Add(new GridPos(tX, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
{
tNeighbors.Add(new GridPos(tX + tDx, tY));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + tDy))
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) || iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
{
tNeighbors.Add(new GridPos(tX + tDx, tY + tDy));
}
else if (iParam.CrossAdjacentPoint)
{
tNeighbors.Add(new GridPos(tX + tDx, tY + tDy));
}
}
if (iParam.SearchGrid.IsWalkableAt(tX - tDx, tY + tDy))
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX - tDx, tY))
{
tNeighbors.Add(new GridPos(tX - tDx, tY + tDy));
}
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - tDy))
{
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY) && !iParam.SearchGrid.IsWalkableAt(tX, tY - tDy))
{
tNeighbors.Add(new GridPos(tX + tDx, tY - tDy));
}
}
}
// search horizontally/vertically
else
{
if (tDx == 0)
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy))
{
tNeighbors.Add(new GridPos(tX, tY + tDy));
if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX + 1, tY))
{
tNeighbors.Add(new GridPos(tX + 1, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX - 1, tY))
{
tNeighbors.Add(new GridPos(tX - 1, tY + tDy));
}
}
else if (iParam.CrossAdjacentPoint)
{
if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX + 1, tY))
{
tNeighbors.Add(new GridPos(tX + 1, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX - 1, tY))
{
tNeighbors.Add(new GridPos(tX - 1, tY + tDy));
}
}
}
else
{
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
{
tNeighbors.Add(new GridPos(tX + tDx, tY));
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY + 1))
{
tNeighbors.Add(new GridPos(tX + tDx, tY + 1));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY - 1))
{
tNeighbors.Add(new GridPos(tX + tDx, tY - 1));
}
}
else if (iParam.CrossAdjacentPoint)
{
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY + 1))
{
tNeighbors.Add(new GridPos(tX + tDx, tY + 1));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY - 1))
{
tNeighbors.Add(new GridPos(tX + tDx, tY - 1));
}
}
}
}
}
else // if(!iParam.CrossCorner)
{
// search diagonally
if (tDx != 0 && tDy != 0)
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy))
{
tNeighbors.Add(new GridPos(tX, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
{
tNeighbors.Add(new GridPos(tX + tDx, tY));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + tDy))
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
tNeighbors.Add(new GridPos(tX + tDx, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX - tDx, tY + tDy))
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX - tDx, tY))
tNeighbors.Add(new GridPos(tX - tDx, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - tDy))
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY - tDy) && iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
tNeighbors.Add(new GridPos(tX + tDx, tY - tDy));
}
}
// search horizontally/vertically
else
{
if (tDx == 0)
{
if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy))
{
tNeighbors.Add(new GridPos(tX, tY + tDy));
if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX + 1, tY))
{
tNeighbors.Add(new GridPos(tX + 1, tY + tDy));
}
if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX - 1, tY))
{
tNeighbors.Add(new GridPos(tX - 1, tY + tDy));
}
}
if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY))
tNeighbors.Add(new GridPos(tX + 1, tY));
if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY))
tNeighbors.Add(new GridPos(tX - 1, tY));
}
else
{
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY))
{
tNeighbors.Add(new GridPos(tX + tDx, tY));
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + 1) && iParam.SearchGrid.IsWalkableAt(tX, tY + 1))
{
tNeighbors.Add(new GridPos(tX + tDx, tY + 1));
}
if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - 1) && iParam.SearchGrid.IsWalkableAt(tX, tY - 1))
{
tNeighbors.Add(new GridPos(tX + tDx, tY - 1));
}
}
if (iParam.SearchGrid.IsWalkableAt(tX, tY + 1))
tNeighbors.Add(new GridPos(tX, tY + 1));
if (iParam.SearchGrid.IsWalkableAt(tX, tY - 1))
tNeighbors.Add(new GridPos(tX, tY - 1));
}
}
}
}
// return all neighbors
else
{
tNeighborNodes = iParam.SearchGrid.GetNeighbors(iNode, iParam.CrossCorner, iParam.CrossAdjacentPoint);
for (int i = 0; i < tNeighborNodes.Count; i++)
{
tNeighborNode = tNeighborNodes[i];
tNeighbors.Add(new GridPos(tNeighborNode.x, tNeighborNode.y));
}
}
return tNeighbors;
}
public void Reset(GridPos iStartPos, GridPos iEndPos, BaseGrid iSearchGrid = null)
{
openList.Clear();
m_startNode = null;
m_endNode = null;
if (iSearchGrid != null)
m_searchGrid = iSearchGrid;
m_searchGrid.Reset();
m_startNode = m_searchGrid.GetNodeAt(iStartPos.x, iStartPos.y);
m_endNode = m_searchGrid.GetNodeAt(iEndPos.x, iEndPos.y);
if (m_startNode == null)
m_startNode = new Node(iStartPos.x, iStartPos.y, true);
if (m_endNode == null)
m_endNode = new Node(iEndPos.x, iEndPos.y, true);
}
public List<Node> GetNeighbors(Node iNode, bool iCrossCorners, bool iCrossAdjacentPoint)
{
int tX = iNode.x;
int tY = iNode.y;
List<Node> neighbors = new List<Node>();
bool tS0 = false, tD0 = false,
tS1 = false, tD1 = false,
tS2 = false, tD2 = false,
tS3 = false, tD3 = false;
GridPos pos = new GridPos();
if (this.IsWalkableAt(pos.Set(tX, tY - 1)))
{
neighbors.Add(GetNodeAt(pos));
tS0 = true;
}
if (this.IsWalkableAt(pos.Set(tX + 1, tY)))
{
neighbors.Add(GetNodeAt(pos));
tS1 = true;
}
if (this.IsWalkableAt(pos.Set(tX, tY + 1)))
{
neighbors.Add(GetNodeAt(pos));
tS2 = true;
}
if (this.IsWalkableAt(pos.Set(tX - 1, tY)))
{
neighbors.Add(GetNodeAt(pos));
tS3 = true;
}
if (iCrossCorners && iCrossAdjacentPoint)
{
tD0 = true;
tD1 = true;
tD2 = true;
tD3 = true;
}
else if (iCrossCorners)
{
tD0 = tS3 || tS0;
tD1 = tS0 || tS1;
tD2 = tS1 || tS2;
tD3 = tS2 || tS3;
}
else
{
tD0 = tS3 && tS0;
tD1 = tS0 && tS1;
tD2 = tS1 && tS2;
tD3 = tS2 && tS3;
}
if (tD0 && this.IsWalkableAt(pos.Set(tX - 1, tY - 1)))
{
neighbors.Add(GetNodeAt(pos));
}
if (tD1 && this.IsWalkableAt(pos.Set(tX + 1, tY - 1)))
{
neighbors.Add(GetNodeAt(pos));
}
if (tD2 && this.IsWalkableAt(pos.Set(tX + 1, tY + 1)))
{
neighbors.Add(GetNodeAt(pos));
}
if (tD3 && this.IsWalkableAt(pos.Set(tX - 1, tY + 1)))
{
neighbors.Add(GetNodeAt(pos));
}
return neighbors;
}
public bool Equals(Node p)
{
// If parameter is null return false:
if ((object)p == null)
{
return false;
}
// Return true if the fields match:
return (x == p.x) && (y == p.y);
}
public static List<GridPos> Backtrace(Node iNode)
{
List<GridPos> path = new List<GridPos>();
path.Add(new GridPos(iNode.x, iNode.y));
while (iNode.parent != null)
{
iNode = (Node)iNode.parent;
path.Add(new GridPos(iNode.x, iNode.y));
}
path.Reverse();
return path;
}