private void IdentitySuccessors(JPSNode currNode, JPSNode goalNode, HashSet <JPSNode> openSet, HashSet <JPSNode> explored, PriorityQueue <JPSNode, float> priorityQueue)
{
JPSNode[] neibours = GetNeibours_JPS(currNode);
for (int i = 0; i < neibours.Length; i++)
{
if (neibours[i] == null)
{
continue;
}
if (explored.Contains(neibours[i]))
{
Debug.Log("Skip neibour explored.");
continue;
}
JPSNode jumpNode = Jump(currNode, neibours[i], goalNode);
if (jumpNode == null)
{
continue;
}
float dist = Heuristic(currNode.x, currNode.z, jumpNode.x, jumpNode.z);
float newCost = currNode.realCost + dist;
if (openSet.Contains(jumpNode) == false || newCost < jumpNode.realCost)
{
jumpNode.realCost = newCost;
jumpNode.previous = currNode;
float estimateCost = Heuristic(jumpNode.x, jumpNode.z, goalNode.x, goalNode.z);
priorityQueue.Insert(jumpNode, jumpNode.realCost + estimateCost);
openSet.Add(jumpNode);
}
}
}
private void AddToOpen(JPSNode newNode)
{
for (int i = 0; i < open.Count; i++)
{
if (open[i].pos == newNode.pos)
{
return;
}
}
for (int i = 0; i < closed.Count; i++)
{
if (closed[i].pos == newNode.pos)
{
return;
}
}
/*
* if (newNode.pos == new Vector3(2, 0, 1) || newNode.pos == new Vector3(2, 0, 3))
* {
* Debug.Log(newNode.pos + "F: " + newNode.priority + " H:" + newNode.H);
* }
*/
if (DebugMode)
{
floor.HighlightUV((int)newNode.pos.x, (int)newNode.pos.z, Color.blue);
}
open.Add(newNode);
}
private void CreateScene(Texture2D texture)
{
GameObject p = new GameObject("Pathfinding Scene");
isObstacle = new bool[texture.width, texture.height];
size = texture.width;
nodeMap = new JPSNode[size, size];
Color c;
for (int i = 0; i < texture.width; i++)
{
for (int j = 0; j < texture.height; j++)
{
float value = texture.GetPixel(i, j).r;
// 阻挡
if (value == 0)
{
isObstacle[i, j] = true;
c = Color.black;
}
else
{
isObstacle[i, j] = false;
c = Color.white;
}
nodeMap[i, j] = new JPSNode(i, j, Single.PositiveInfinity, isObstacle[i, j]);
GameObject go = Instantiate(blockPrefab, p.transform);
go.transform.position = new Vector3(i, 0, j);
SetBlockColor(go, c);
nodeMap[i, j].block = go;
}
}
}
public IEnumerator FindPathJPS(JPSNode startNode, JPSNode goalNode)
{
if (!InRange(startNode) || !InRange(goalNode))
{
throw new IndexOutOfRangeException();
}
if (startNode.isObstacle || goalNode.isObstacle)
{
Debug.LogError("起点/终点处于障碍物！");
yield break;
}
switch (heuristic)
{
case EHeuristic.EulerDistance:
Heuristic = EulerDistance;
break;
case EHeuristic.ManhattonDistance:
Heuristic = ManhattonDistance;
break;
case EHeuristic.DiagonalDistance:
Heuristic = DiagonalDistance;
break;
default:
Heuristic = EulerDistance;
break;
}
Stopwatch watch = Stopwatch.StartNew();
// --JPS Path Finding
PriorityQueue <JPSNode, float> priorityQueue = new PriorityQueue <JPSNode, float>(0f);
HashSet <JPSNode> openSet = new HashSet <JPSNode>();
HashSet <JPSNode> explored = new HashSet <JPSNode>();
openSet.Add(startNode);
startNode.realCost = 0f;
startNode.previous = null;
priorityQueue.Insert(startNode, 0f);
while (priorityQueue.Count() != 0)
{
JPSNode currNode = priorityQueue.Pop();
if (currNode.x == goalNode.x && currNode.z == goalNode.z)
{
watch.Stop();
//Debug.Log($"JPS寻路完成，耗时{watch.ElapsedMilliseconds}ms");
_pathForDisplay = ConstructPath(currNode);
yield break;
}
explored.Add(currNode);
openSet.Remove(currNode);
IdentitySuccessors(currNode, goalNode, openSet, explored, priorityQueue);
if (showProcess)
{
yield return(waitTime);
}
}
Debug.LogError("JPS 没找到路径");
}
public JPSNode(int x, int z, float realCost, bool obstacle)
{
this.x = x;
this.z = z;
this.realCost = realCost;
previous = null;
isObstacle = obstacle;
}
private List <JPSNode> ConstructPath(JPSNode n)
{
List <JPSNode> nodeList = new List <JPSNode>();
nodeList.Add(n);
while (n.previous != null)
{
n = n.previous;
nodeList.Add(n);
}
return(nodeList);
}
private JPSNode Jump(JPSNode parentNode, JPSNode neibour, JPSNode goal)
{
if (neibour == null || !IsWalkable(neibour))
{
return(null);
}
if (neibour == goal)
{
return(neibour);
}
int dx = neibour.x - parentNode.x;
int dz = neibour.z - parentNode.z;
// 检查对角线
if (dx != 0 && dz != 0)
{
// 检查对角线时也要对水平或垂直的进行检查，如果后续检查存在跳点，则该点是跳点
if (Jump(neibour, GetNode(neibour.x + dx, neibour.z), goal) != null ||
Jump(neibour, GetNode(neibour.x, neibour.z + dz), goal) != null)
{
return(neibour);
}
if ((!IsWalkable(GetNode(neibour.x - dx, neibour.z)) && IsWalkable(GetNode(neibour.x - dx, neibour.z + dz))) ||
(!IsWalkable(GetNode(neibour.x, neibour.z - dz)) && IsWalkable(GetNode(neibour.x + dx, neibour.z - dz))))
{
return(neibour);
}
}
else
{
// 水平
if (dx != 0)
{
if ((IsWalkable(GetNode(neibour.x + dx, neibour.z + 1)) && !IsWalkable(GetNode(neibour.x, neibour.z + 1))) ||
(IsWalkable(GetNode(neibour.x + dx, neibour.z - 1)) && !IsWalkable(GetNode(neibour.x, neibour.z - 1))))
{
return(neibour);
}
}
else
{
if ((IsWalkable(GetNode(neibour.x - 1, neibour.z + dz)) && !IsWalkable(GetNode(neibour.x - 1, neibour.z))) ||
(IsWalkable(GetNode(neibour.x + 1, neibour.z + dz)) && !IsWalkable(GetNode(neibour.x + 1, neibour.z))))
{
return(neibour);
}
}
}
//SetBlockColor(neibour.block, Color.cyan, drawPath);
return(Jump(neibour, GetNode(neibour.x + dx, neibour.z + dz), goal));
}
private bool IsWalkable(JPSNode node)
{
if (node == null)
{
return(false);
}
if (InRange(node) == false)
{
return(false);
}
if (node.isObstacle)
{
return(false);
}
return(true);
}
public JPSNode(Vector3 newPos, JPSNode par, Vector3 dest)
{
pos = newPos;
parent = par;
int dx = (int)Mathf.Abs(dest.x - pos.x);
int dz = (int)Mathf.Abs(dest.z - pos.z);
H = (dx + dz) + ((Mathf.Sqrt(2) - 2) * Mathf.Min(dx, dz));
// Chebyshev Mathf.Max(Mathf.Abs(dest.x - pos.x), Mathf.Abs(dest.z - pos.z));
directions = new List <Vector3>();
if (parent != null)
{
parentPos = parent.pos;
parentF = parent.priority;
G += parent.G
+ (Mathf.Sqrt(Mathf.Pow(parent.pos.x - pos.x, 2) + Mathf.Pow(parent.pos.z - pos.z, 2)));
}
priority = G + H;
}
private JPSNode[] GetAllNeibours(JPSNode n)
{
int x = n.x;
int z = n.z;
JPSNode[] neibours = new JPSNode[8];
if (InRange(x - 1, z))
{
neibours[0] = nodeMap[x - 1, z];
}
if (InRange(x, z + 1))
{
neibours[1] = nodeMap[x, z + 1];
}
if (InRange(x + 1, z))
{
neibours[2] = nodeMap[x + 1, z];
}
if (InRange(x, z - 1))
{
neibours[3] = nodeMap[x, z - 1];
}
if (InRange(x - 1, z - 1))
{
neibours[4] = nodeMap[x - 1, z - 1];
}
if (InRange(x - 1, z + 1))
{
neibours[5] = nodeMap[x - 1, z + 1];
}
if (InRange(x + 1, z + 1))
{
neibours[6] = nodeMap[x + 1, z + 1];
}
if (InRange(x + 1, z - 1))
{
neibours[7] = nodeMap[x + 1, z - 1];
}
return(neibours);
}
public void Search()
{
searched = true;
//Reset Debug floor
if (DebugMode)
{
foreach (JPSNode node in closed)
{
floor.HighlightUV((int)node.pos.x, (int)node.pos.z, Color.white);
}
foreach (JPSNode node in path)
{
floor.HighlightUV((int)node.pos.x, (int)node.pos.z, Color.white);
}
}
//Prime search values
searching = true;
destinationFound = false;
placeInPath = 0;
open.Clear();
closed.Clear();
path.Clear();
currentNode = new JPSNode(startingPosition, null, destination);
//First node must have all directions
currentNode.AddDirection(new Vector3(1, 0, 1));
currentNode.AddDirection(new Vector3(1, 0, -1));
currentNode.AddDirection(new Vector3(-1, 0, 1));
currentNode.AddDirection(new Vector3(-1, 0, -1));
currentNode.AddDirection(new Vector3(1, 0, 0));
currentNode.AddDirection(new Vector3(-1, 0, 0));
currentNode.AddDirection(new Vector3(0, 0, 1));
currentNode.AddDirection(new Vector3(0, 0, -1));
AddToOpen(currentNode);
//begin main search loop
while (!destinationFound && open.Count > 0)
{
currentNode = open[0];
for (int i = 0; i < open.Count; i++)
{
if (currentNode.priority > open[i].priority)
{
currentNode = open[i];
}
}
open.Remove(currentNode);
closed.Add(currentNode);
for (int i = 0; i < currentNode.directions.Count; i++)
{
if (currentNode.directions[i].magnitude == 1)
{
FindNodes(currentNode.pos + currentNode.directions[i], currentNode, currentNode.directions[i], true);
}
else
{
FindNodes(currentNode.pos + currentNode.directions[i], currentNode, currentNode.directions[i], false);
}
}
}
//Prepare path for unit
currentNode = null;
for (int i = 0; i < closed.Count; i++)
{
if (closed[i].pos == destination)
{
currentNode = closed[i];
}
}
if (DebugMode)
{
Debug.Log("Path Cost: " + currentNode.priority);
}
if (currentNode != null)
{
while (currentNode.parent != null)
{
if (DebugMode)
{
floor.HighlightUV((int)currentNode.pos.x, (int)currentNode.pos.z, Color.green);
}
path.Add(currentNode);
currentNode = currentNode.parent;
}
path.Reverse();
if (destinationFound)
{
navigating = true;
}
}
searching = false;
}
private bool FindNodes(Vector3 position, JPSNode parent, Vector3 direction, bool core)
{
Vector3 currentPosition = position;
while (IsInBounds(currentPosition) && !ObjectAt(currentPosition))
{
if (currentPosition == destination)
{
JPSNode destNode = new JPSNode(currentPosition, parent, destination);
closed.Add(destNode);
destinationFound = true;
return(true);
}
if (direction.magnitude > 1) // do diagonal stuff
{
bool addDiag = false;
JPSNode diagNode = new JPSNode(currentPosition, parent, destination);
if (!ObjectAt(currentPosition + new Vector3(-direction.x, 0, 0)) ||
!ObjectAt(currentPosition + new Vector3(0, 0, -direction.z)))
{
if (!ObjectAt(currentPosition + new Vector3(-direction.x, 0, 0)) && ObjectAt(currentPosition + new Vector3(0, 0, -direction.z)))
{
diagNode.AddDirection(new Vector3(direction.x, 0, -direction.z));
addDiag = true;
}
else if (!ObjectAt(currentPosition + new Vector3(0, 0, -direction.z)) && ObjectAt(currentPosition + new Vector3(-direction.x, 0, 0)))
{
diagNode.AddDirection(new Vector3(-direction.x, 0, direction.z));
addDiag = true;
}
if (FindNodes(currentPosition + new Vector3(direction.x, 0, 0), diagNode, new Vector3(direction.x, 0, 0), false))
{
diagNode.AddDirection(new Vector3(direction.x, 0, 0));
addDiag = true;
}
if (FindNodes(currentPosition + new Vector3(0, 0, direction.z), diagNode, new Vector3(0, 0, direction.z), false))
{
diagNode.AddDirection(new Vector3(0, 0, direction.z));
addDiag = true;
}
if (addDiag)
{
AddToOpen(diagNode);
}
}
else
{
return(false);
}
}
else // do horizontal/vertical stuff
{
bool addNode = false;
JPSNode newNode = new JPSNode(currentPosition, parent, destination);
newNode.AddDirection(direction);
if (currentPosition == destination)
{
closed.Add(newNode);
destinationFound = true;
return(true);
}
bool horizVert = (Mathf.Abs(direction.x) == 1) ? true : false;
if (!ObjectAt(currentPosition + direction) &&
ObjectAt(currentPosition + ((horizVert) ? new Vector3(0, 0, 1) : new Vector3(1, 0, 0))) &&
!ObjectAt(currentPosition + ((horizVert) ? new Vector3(direction.x, 0, 1) : new Vector3(1, 0, direction.z))))
{
newNode.AddDirection((horizVert) ? new Vector3(direction.x, 0, 1) : new Vector3(1, 0, direction.z));
addNode = true;
}
if (!ObjectAt(currentPosition + direction) &&
ObjectAt(currentPosition + ((horizVert) ? new Vector3(0, 0, -1) : new Vector3(-1, 0, 0))) &&
!ObjectAt(currentPosition + ((horizVert) ? new Vector3(direction.x, 0, -1) : new Vector3(-1, 0, direction.z))))
{
newNode.AddDirection((horizVert) ? new Vector3(direction.x, 0, -1) : new Vector3(-1, 0, direction.z));
addNode = true;
}
if (addNode)
{
if (core)
{
AddToOpen(newNode);
}
return(true);
}
}
currentPosition += direction;
}
return(false);
}
private JPSNode[] GetNeibours_JPS(JPSNode node)
{
JPSNode[] nodes = new JPSNode[8];
if (node.previous == null)
{
return(GetAllNeibours(node));
}
JPSNode parentNode = node.previous;
int x = node.x, z = node.z;
int px = parentNode.x;
int pz = parentNode.z;
int dx = (x - px) / Mathf.Max(Mathf.Abs(x - px), 1);
int dz = (z - pz) / Mathf.Max(Mathf.Abs(z - pz), 1);
if (dx != 0 && dz != 0)
{
if (IsWalkable(GetNode(x, z + dz)))
{
nodes[0] = GetNode(x, z + dz);
}
if (IsWalkable(GetNode(x + dx, z)))
{
nodes[1] = GetNode(x + dx, z);
}
if (IsWalkable(GetNode(x + dx, z + dz)))
{
nodes[2] = GetNode(x + dx, z + dz);
}
if (!IsWalkable(GetNode(x - dx, z)))
{
nodes[3] = GetNode(x - dx, z + dz);
}
if (!IsWalkable(GetNode(x, z - dz)))
{
nodes[4] = GetNode(x + dx, z - dz);
}
}
else
{
if (dx == 0)
{
//nodes[0] = GetNode(x, z + dz);
//nodes[1] = GetNode(x + 1, z + dz);
//nodes[2] = GetNode(x - 1, z + dz);
if (IsWalkable(GetNode(x, z + dz)))
{
nodes[0] = GetNode(x, z + dz);
}
if (IsWalkable(GetNode(x + 1, z + dz)))
{
nodes[1] = GetNode(x + 1, z + dz);
}
if (IsWalkable(GetNode(x - 1, z + dz)))
{
nodes[2] = GetNode(x - 1, z + dz);
}
}
else
{
//nodes[0] = GetNode(x + dx, z);
//nodes[1] = GetNode(x + dx, z + 1);
//nodes[2] = GetNode(x + dx, z - 1);
if (IsWalkable(GetNode(x + dx, z)))
{
nodes[0] = GetNode(x + dx, z);
}
if (IsWalkable(GetNode(x + dx, z + 1)))
{
nodes[1] = GetNode(x + dx, z + 1);
}
if (IsWalkable(GetNode(x + dx, z - 1)))
{
nodes[2] = GetNode(x + dx, z - 1);
}
}
}
return(nodes);
}
public IEnumerator FindPathAstar(int xstart, int zstart, int xend, int zend)
{
// 一些设置
if (!InRange(xstart, zstart) || !InRange(xend, zend))
{
throw new ArgumentOutOfRangeException();
}
if (isObstacle[xend, zend] || isObstacle[xstart, zstart])
{
yield break;
}
switch (heuristic)
{
case EHeuristic.EulerDistance:
Heuristic = EulerDistance;
break;
case EHeuristic.ManhattonDistance:
Heuristic = ManhattonDistance;
break;
case EHeuristic.DiagonalDistance:
Heuristic = DiagonalDistance;
break;
default:
Heuristic = EulerDistance;
break;
}
SetBlockColor(nodeMap[xstart, zstart].block, Color.yellow, drawPath);
SetBlockColor(nodeMap[xend, zend].block, Color.blue, drawPath);
// 寻路流程
Stopwatch watch = Stopwatch.StartNew();
HashSet <JPSNode> reachedList = new HashSet <JPSNode>();
HashSet <JPSNode> exploredList = new HashSet <JPSNode>();
PriorityQueue <JPSNode, float> pq = new PriorityQueue <JPSNode, float>(0f);
JPSNode[] neibours = new JPSNode[8];
nodeMap[xstart, zstart].realCost = 0f;
pq.Insert(nodeMap[xstart, zstart], 0f);
reachedList.Add(nodeMap[xstart, zstart]);
while (pq.Count() != 0)
{
JPSNode currNode = pq.Pop();
if (currNode.x == xend && currNode.z == zend)
{
watch.Stop();
Debug.Log($"寻路完成 耗时{watch.ElapsedMilliseconds}ms");
_pathForDisplay = ConstructPath(currNode);
yield break;
//return ConstructPath(currNode);
}
exploredList.Add(currNode);
reachedList.Remove(currNode);
SetBlockColor(currNode.block, Color.gray, drawPath);
neibours = GetAllNeibours(currNode);
for (int i = 0; i < 8; i++)
{
if (neibours[i] != null && neibours[i].isObstacle == false && !exploredList.Contains(neibours[i]))
{
float step = i > 4 ? 1.41421356f : 1f;
float estimateCost = Heuristic(neibours[i].x, neibours[i].z, xend, zend);
float newCost = currNode.realCost + step;
if (reachedList.Contains(neibours[i]) == false || newCost < neibours[i].realCost)
{
float beforeCost = neibours[i].realCost;
neibours[i].previous = currNode;
neibours[i].realCost = newCost;
if (reachedList.Contains(neibours[i]) == false)
{
Debug.Log($"Astart Add {neibours[i].x},{neibours[i].z}: {beforeCost + estimateCost} -> {estimateCost + neibours[i].realCost}");
}
else
{
Debug.Log($"Astar Update {neibours[i].x},{neibours[i].z}: {beforeCost + estimateCost} -> {estimateCost + neibours[i].realCost}");
}
pq.Insert(neibours[i], neibours[i].realCost + estimateCost);
reachedList.Add(neibours[i]);
SetBlockColor(neibours[i].block, Color.yellow, drawPath);
}
}
}
if (showProcess)
{
yield return(null);
}
}
//EditorUtility.ClearProgressBar();
}
private bool InRange(JPSNode node)
{
return(node.x >= 0 && node.z >= 0 && node.x < size && node.z < size);
}
private float GetDistance(JPSNode a, JPSNode b)
{
return(EulerDistance(a.x, a.z, b.x, b.z));
}
