public PathingNode(Vector2Int nodeCoordinate, PathingNode parentNode, int gCost, int hCost)
{
this.parentNode = parentNode;
coordinate = nodeCoordinate;
this.hCost = hCost;
this.gCost = gCost;
}
public void updatePath()
{
PathingNode start;
PathingNode end;
if (Network.isServer)
{
start = GameState.getInstance().pathMan.player1SpawnShadow;
end = GameState.getInstance().pathMan.player1EndShadow;
}
else
{
start = GameState.getInstance().pathMan.player2SpawnShadow;
end = GameState.getInstance().pathMan.player2EndShadow;
}
path = new List <PathingNode>();
PathingNode next = start.nextNode;
path.Add(next);
int count = 0;
while (next != end && count < 1000)
{
next = next.nextNode;
path.Add(next);
count++;
}
renderer.SetVertexCount(path.Count);
for (int i = 0; i < path.Count; i++)
{
renderer.SetPosition(i, new Vector3(path[i].x, 0, path[i].z));
}
}
public Vector3 getNextPos(Vector3 pos)
{
// Debug.Log("pos = " + pos.x + "," + pos.z);
origin.x = pos.x;
origin.z = pos.z;
temp.x = nextNode.x;
temp.z = nextNode.z;
dir = temp - origin;
if (Physics.Raycast(origin, dir, dir.magnitude, towerMask))
{
nextNode = grid[(int)(pos.x + 0.5f)][(int)(pos.z + 0.5f)].nextNode;
}
else
{
for (tempNode = nextNode.nextNode; !nextNode.Equals(tempNode); tempNode = tempNode.nextNode)
{
dir = tempNode.getPos() - origin;
if (Physics.Raycast(origin, dir, dir.magnitude, towerMask))
{
break;
}
nextNode = tempNode;
}
}
return(new Vector3(nextNode.x, 0, nextNode.z));
}
public PathingNode(int x_, int z_)
{
x = x_;
z = z_;
pathable = true;
bestNode = this;
nextNode = this;
}
public void initBest(Dictionary <int, Dictionary <int, PathingNode> > grid, PathingNode endNode, int xmin, int xmax, int zmin, int zmax)
{
for (int x = xmin; x <= xmax; x++)
{
for (int z = zmin; z <= zmax; z++)
{
grid[x][z].bestNode = endNode;
}
}
}
//Создает игровой объект в определенных координатах, добавляет его в pathingPositions
private GameObject CreateAt(int x, int y, GameObject toInstantiate)
{
GameObject instance = Instantiate(toInstantiate, new Vector3(x, y, 0f), Quaternion.identity);
instance.transform.SetParent(dungeonHolder.transform);
pathingPositions[x, y] = new PathingNode();
if (toInstantiate.CompareTag("Wall"))
{
pathingPositions[x, y].walkable = false;
}
pathingPositions[x, y].obj = instance;
return(instance);
}
private PathingNode SelectLowestCostNode(List <PathingNode> openNodes)
{
PathingNode lowestCostNode = openNodes[0];
for (int i = 1; i < openNodes.Count; i++)
{
if (openNodes[i].fCost < lowestCostNode.fCost || openNodes[i].fCost == lowestCostNode.fCost && openNodes[i].hCost < lowestCostNode.hCost)
{
lowestCostNode = openNodes[i];
}
}
return(lowestCostNode);
}
private Path ConstructPath(PathingNode node)
{
//UnityEngine.Debug.Log(node);
Path path = new Path();
while (node.parentNode != null)
{
path.Add(node.coordinate);
node = node.parentNode;
}
path.Add(node.coordinate);
return(path);
}
private static int maxChecksPerFrame = 1000; //In case of error states
#endregion Fields
#region Methods
public static GridPoint[] Path(GridPoint start, GridPoint goal)
{
List<PathingNode> closedSet = new List<PathingNode> ();
List<PathingNode> openSet = new List<PathingNode> ();
PathingNode currNode = new PathingNode (start);
openSet.Add (currNode);
int count = 0;
while (openSet.Count!=0 && count<maxChecksPerFrame) {
count++;
currNode=openSet[0];
foreach(PathingNode node in openSet){
if(node.GetFScore(goal)<currNode.GetFScore(goal)){
currNode = node;
}
}
if(currNode.loc.transform.position==goal.transform.position){
return GetPathFromNode(currNode);
}
else{
openSet.Remove(currNode);
closedSet.Add(currNode);
List<GridPoint> moves = GetPossibleNextPointsFromPosition(currNode.loc);
foreach(GridPoint move in moves){
PathingNode potentialNode = new PathingNode(move,currNode);
if(closedSet.Contains(potentialNode)){
}
else if(openSet.Contains(potentialNode)){
int index = openSet.IndexOf(potentialNode);
if(openSet[index].g_score > potentialNode.g_score){
openSet[index]=potentialNode;
}
}
else{
openSet.Add(potentialNode);
}
}
}
}
return null;
}
private Dictionary <int, Dictionary <int, PathingNode> > makeNodes(int xmin, int xmax, int zmin, int zmax)
{
Dictionary <int, Dictionary <int, PathingNode> > ret = new Dictionary <int, Dictionary <int, PathingNode> >();
for (int x = xmin; x <= xmax; x++)
{
ret[x] = new Dictionary <int, PathingNode>();
for (int z = zmin; z <= zmax; z++)
{
ret[x][z] = new PathingNode(x, z);
}
}
return(ret);
}
public void turnOff(float x, float z)
{
Debug.Log("Turning off nodes near (" + x + "," + z + ")");
int xmin = Mathf.FloorToInt(x);
int xmax = Mathf.CeilToInt(x);
int zmin = Mathf.FloorToInt(z);
int zmax = Mathf.CeilToInt(z);
bool isPlayer1 = z > 0;
Dictionary <int, Dictionary <int, PathingNode> > dict = isPlayer1 ? player1Zone : player2Zone;
Dictionary <int, Dictionary <int, PathingNode> > dictShadow = isPlayer1 ? player1ZoneShadow : player2ZoneShadow;
PathingNode end = isPlayer1 ? player1End : player2End;
PathingNode endShadow = isPlayer1 ? player1EndShadow : player2EndShadow;
for (int i = xmin; i <= xmax; i++)
{
for (int j = zmin; j <= zmax; j++)
{
Dictionary <int, PathingNode> inDict;
if (!dict.TryGetValue(i, out inDict))
{
continue;
}
if (!inDict.ContainsKey(j))
{
continue;
}
dict[i][j].pathable = false;
dictShadow[i][j].pathable = false;
}
}
if (recalculateNow)
{
recalculate(dict, end);
recalculate(dictShadow, endShadow);
}
}
//Private method to trace back a node to get a path
private Vector3[] GetPathFromNode(PathingNode goal)
{
List <Vector3> protoPath = new List <Vector3>();
if (goal.GetParent() == null)
{
protoPath.Add(new Vector3(goal.loc.x, goal.loc.y, goal.loc.z));
goal = goal.GetParent();
}
else
{
while (goal.GetParent() != null)
{
protoPath.Add(new Vector3(goal.loc.x, goal.loc.y, goal.loc.z));
goal = goal.GetParent();
}
}
protoPath.Reverse();
return(protoPath.ToArray());
}
public bool pathExists(Dictionary <int, Dictionary <int, PathingNode> > grid, PathingNode start, PathingNode end)
{
int count = 0;
int numNodes = 1000;
PathingNode next = start;
while (count <= numNodes)
{
if (next == end)
{
return(true);
}
if (!next.pathable)
{
return(false);
}
next = next.nextNode;
count++;
}
return(false);
}
private static GridPoint[] GetPathFromNode(PathingNode goal)
{
List<GridPoint> protoPath = new List<GridPoint>();
if (goal.GetParent () == null) {
//Debug.Log ("Hit the if; "+goal.loc.transform.position);
protoPath.Add (goal.loc);
goal = goal.GetParent();
}
else{
//Debug.Log ("Hit the else");
while (goal.GetParent()!=null) {
//Debug.Log ("Hit the while: "+goal.loc.transform.position);
protoPath.Add (goal.loc);
goal = goal.GetParent();
}
}
protoPath.Reverse ();
return protoPath.ToArray ();
}
void Awake()
{
path = new Stack <PathingNode>();
//Create zones
//Player 1
player1Zone = makeNodes(-25, 25, 2, 22);
player1End = player1Zone[-25][12];
player1Spawn = player1Zone[25][12];
player1ZoneShadow = makeNodes(-25, 25, 2, 22);
player1EndShadow = player1ZoneShadow[-25][12];
player1SpawnShadow = player1ZoneShadow[25][12];
//Player 2
player2Zone = makeNodes(-25, 25, -22, -2);
player2End = player2Zone[25][-12];
player2Spawn = player2Zone[-25][-12];
player2ZoneShadow = makeNodes(-25, 25, -22, -2);
player2EndShadow = player2ZoneShadow[25][-12];
player2SpawnShadow = player2ZoneShadow[-25][-12];
//Calculate
recalculate(player1Zone, player1End);
recalculate(player1ZoneShadow, player1End);
recalculate(player2Zone, player2End);
recalculate(player2ZoneShadow, player2End);
//Initiate bestNodes
initBest(player1Zone, player1End, -25, 25, 2, 22);
initBest(player1ZoneShadow, player1EndShadow, -25, 25, 2, 22);
initBest(player2Zone, player2End, -25, 25, -22, -2);
initBest(player2ZoneShadow, player2EndShadow, -25, 25, -22, -2);
//For Show
player1ZoneSize = player1Zone.Count;
player2ZoneSize = player2Zone.Count;
}
//The method used by the pathfinding thread, calls back with the finished path when ready if no other threads are started first
private void PathFindingThread(uint activeThreadID, LocationData locationData, Vector2Int targetNode, Action <Path> CALLBACK)
{
NavGrid navgrid = locationData.navgrid;
bool foundPath = false;
// A* PATHFINDING IMPLEMENTATION
List <PathingNode> openNodes = new List <PathingNode>();
HashSet <PathingNode> closedNodes = new HashSet <PathingNode>();
PathingNode selectedNode = null;
openNodes.Add(new PathingNode(locationData.coordinates, null, 0, CalculateHCost(locationData.coordinates, targetNode)));
while (openNodes.Count > 0)
{
selectedNode = SelectLowestCostNode(openNodes);
openNodes.Remove(selectedNode);
closedNodes.Add(selectedNode);
if (selectedNode.coordinate == targetNode)
{
foundPath = true;
break;
}
//Check each surrounding node
for (int i = 0; i < 4; i++)
{
//Setup direction
NavGrid.Direction direction = NavGrid.Direction.NORTH;
Vector2Int offset = new Vector2Int();
switch (i)
{
case 0:
direction = NavGrid.Direction.NORTH;
offset = new Vector2Int(0, 1);
break;
case 1:
direction = NavGrid.Direction.EAST;
offset = new Vector2Int(1, 0);
break;
case 2:
direction = NavGrid.Direction.SOUTH;
offset = new Vector2Int(0, -1);
break;
case 3:
direction = NavGrid.Direction.WEST;
offset = new Vector2Int(-1, 0);
break;
}
//Is there is no wall
if (!navgrid.HasWall(direction, selectedNode.coordinate))
{
Vector2Int newCoordinate = selectedNode.coordinate + offset;
PathingNode newPathNode = new PathingNode(newCoordinate, selectedNode);
//Make sure it isn't in the closed list and Check for pathability
if (!closedNodes.Contains(newPathNode) && navgrid.IsPathable(newCoordinate))
{
//Compute score
newPathNode.gCost = selectedNode.gCost;
newPathNode.hCost = CalculateHCost(newCoordinate, targetNode);
//Add to open list if its not already in it
if (!openNodes.Contains(newPathNode))
{
openNodes.Add(newPathNode);
}
//If it is, modify it if the new version has a lower gcost
else
{
int index = openNodes.IndexOf(newPathNode);
if (newPathNode.gCost < openNodes[index].gCost)
{
openNodes.Remove(newPathNode);
openNodes.Add(newPathNode);
}
}
}
}
}
}
if (!foundPath)
{
return;
}
//Construct and send the path
Path path = ConstructPath(selectedNode);
if (this.activeThreadID == activeThreadID)
{
CALLBACK(path);
}
}
public PathingNode(Vector2Int coordinate, PathingNode parentNode)
{
this.parentNode = parentNode;
this.coordinate = coordinate;
}
public PathingNode(Vector3i _loc, Vector3i goal, PathingNode parent) : this(_loc, goal)
{
_parent = parent;
g_score = parent.g_score + 1;
}
public override bool Equals(object obj)
{
PathingNode objNode = (PathingNode)obj;
return(obj != null && objNode != null && loc.Equals(objNode.loc));
}
public IEnumerator GenerateDirectPath()
{
Vector2 current = Exit;
int[,] grid = new int[MazeWidth, MazeLength];
PriorityQueue <Vector2> queue = new PriorityQueue <Vector2>();
queue.Add(current, 0f);
grid[(int)current.x, (int)current.y] = 1;
float LoopTime = Time.realtimeSinceStartup;
bool solved = false;
while (queue.Count > 0)
{
current = queue.Get(0);
queue.RemoveAt(0);
if (current == Entrance)
{
solved = true;
break;
}
for (int n = 0; n < 4; n++)
{
// doesn't exit this direction, discard.
if (!HasExit(current, (CardinalDirection)n))
{
continue;
}
PathingNode d = new PathingNode(current, (CardinalDirection)n);
Vector2 next = d.LeadsTo();
// the other node has already been searched, and is a lower value. Discard.
if (grid[(int)next.x, (int)next.y] != 0 && grid[(int)next.x, (int)next.y] < grid[(int)current.x, (int)current.y] + 1)
{
continue;
}
// set arrival cost in grid
grid[(int)next.x, (int)next.y] = grid[(int)current.x, (int)current.y] + 1;
// add this node to the queue
float heuristic = Mathf.Abs(next.x - Entrance.x) + Mathf.Abs(next.y - Entrance.y);
queue.Add(next, grid[(int)next.x, (int)next.y] + heuristic);
}
// grid[(int)current.x, (int)current.y];
if (Time.realtimeSinceStartup - LoopTime > 0.1)
{
Debug.Log("searching...");
yield return(null);
LoopTime = Time.realtimeSinceStartup;
}
}
if (!solved)
{
yield break;
}
// backtrace...
current = Entrance;
List <PathingNode> path = new List <PathingNode>();
while (current != Exit)
{
for (int n = 0; n < 4; n++)
{
CardinalDirection c = (CardinalDirection)n;
if (!HasExit(current, c))
{
continue;
}
PathingNode d = new PathingNode(current, c);
Vector2 next = d.LeadsTo();
if (grid[(int)next.x, (int)next.y] >= grid[(int)current.x, (int)current.y])
{
continue;
}
path.Insert(0, d); // path from exit to entrance...
current = next;
break;
// check if any of the four directions has a node with lower value than the current.
}
if (Time.realtimeSinceStartup - LoopTime > 0.1)
{
yield return(null);
Debug.Log("backtrace...");
LoopTime = Time.realtimeSinceStartup;
}
}
DirectPath = path;
yield break;
}
public IEnumerator GenerateRightHandPath()
{
List <PathingNode> path = new List <PathingNode>();
Vector2 current = Entrance;
CardinalDirection direction = CardinalDirection.Right;
float LoopTime = Time.realtimeSinceStartup;
while (current != Exit)
{
// check each exit in turn...
for (int n = 0; n < 4; n++)
{
CardinalDirection newDirection = (CardinalDirection)(((int)direction + 4 + 3 + n) % 4);
if (HasExit(current, newDirection))
{
direction = newDirection;
PathingNode directionNode = new PathingNode(current, direction);
path.Add(directionNode);
current = directionNode.LeadsTo();
break;
}
}
if (Time.realtimeSinceStartup - LoopTime > 0.1)
{
yield return(null);
LoopTime = Time.realtimeSinceStartup;
}
if (current.x < 0 || current.y < 0)
{
break;
}
bool PathLoops = false;
for (int n = 0; n < path.Count - 1; n++)
{
if (path[n] == path[path.Count - 1])
{
PathLoops = true;
break;
}
if (Time.realtimeSinceStartup - LoopTime > 0.1)
{
// Debug.Log("yield: " + path.Count + ": " + current);
yield return(null);
LoopTime = Time.realtimeSinceStartup;
}
}
if (PathLoops)
{
break;
}
}
RightHandPath = path;
yield break;
}
public void recalculate(Dictionary <int, Dictionary <int, PathingNode> > grid, PathingNode endNode)
{
int zmin, zmax;
if (grid == player1Zone || grid == player1ZoneShadow)
{
zmin = 2;
zmax = 22;
}
else
{
zmin = -22;
zmax = -2;
}
Dictionary <int, Dictionary <int, PathingNode> > visitedNodes = new Dictionary <int, Dictionary <int, PathingNode> >();
for (int i = -25; i <= 25; i++)
{
visitedNodes[i] = new Dictionary <int, PathingNode>();
}
Queue <PathingNode> enqueuedNodes = new Queue <PathingNode>();
enqueuedNodes.Enqueue(endNode);
visitedNodes[endNode.x][endNode.z] = endNode;
while (enqueuedNodes.Count > 0)
{
PathingNode evalNode = enqueuedNodes.Dequeue();
int x = evalNode.x;
int z = evalNode.z;
if (!evalNode.pathable)
{
continue;
}
enqueueNode(enqueuedNodes, visitedNodes, grid, evalNode, x + 1, z, 4);
enqueueNode(enqueuedNodes, visitedNodes, grid, evalNode, x - 1, z, 2);
enqueueNode(enqueuedNodes, visitedNodes, grid, evalNode, x, z + 1, 3);
enqueueNode(enqueuedNodes, visitedNodes, grid, evalNode, x, z - 1, 1);
}
/*for (int i = -25; i <= 25; i++)
* {
* for (int j = zmin; j <= zmax; j++)
* {
* if (!visitedNodes[i].ContainsKey(j)) continue;
*
* PathingNode temp = visitedNodes[i][j].bestNode;
* PathingNode last = visitedNodes[i][j];
* Vector3 origin = new Vector3(last.x, 0.1f, last.z);
* Vector3 dest = new Vector3(temp.x, 0.1f, temp.z);
* Vector3 dir = dest - origin;
* if (Physics.Raycast(origin, dir, dir.magnitude, towerMask)) //back up
* {
* last = visitedNodes[i][j].nextNode;
* for (temp = last.nextNode; (!temp.Equals(endNode)); temp = temp.nextNode)
* {
* dest.x = temp.x;
* dest.z = temp.z;
* dir = dest - origin;
* if (Physics.Raycast(origin, dir, dir.magnitude, towerMask)) break;
* last = temp;
* }
* visitedNodes[i][j].bestNode = last;
*
*
* }
* else //go forward
* {
* last = temp;
* for (temp = temp.nextNode; (!temp.Equals(endNode)); temp = temp.nextNode)
* {
* dest.x = temp.x;
* dest.z = temp.z;
* dir = dest - origin;
* if (Physics.Raycast(origin, dir, dir.magnitude, towerMask)) break;
* last = temp;
* }
* visitedNodes[i][j].bestNode = last;
* }
* }
* }*/
// if(grid == player1Zone)
// {
// Debug.Log("-----------------------------------");
// string val = "";
// for(int i = 10; i >= -10; i--)
// {
//
//
// for(int j = xmin; j <= xmax; j++)
// {
// if(grid[j][i] == player1End)
// val += "E";
// else
// val += grid[j][i].dir;
// }
// val += "\n";
//
// }
// Debug.Log (val);
// }
}
public static Path GetPath(Vector3 from, Vector3 to, int maxAllowedCycles = 1000)
{
Vector2Int start = new Vector2Int(Mathf.FloorToInt(from.x), Mathf.FloorToInt(from.y));
Vector2Int target = new Vector2Int(Mathf.FloorToInt(to.x), Mathf.FloorToInt(to.y));
PathingNode targetNode = new PathingNode(target, 0, int.MaxValue);
PathingNode lastNode = null;
//TODO if performance sucks, implement chunk portals and check paths in chunks first.
List <PathingNode> open = new List <PathingNode>();
List <PathingNode> closed = new List <PathingNode>();
open.Add(new PathingNode(start, 0, 0));
int cycles = 0;
while (open.Count > 0)
{
cycles++;
if (cycles > maxAllowedCycles)
{
return(null);
}
var n = open[0];
if (n.Equals(targetNode))
{
int numClosed = closed.Count;
if (numClosed <= 0)
{
return(null);
}
targetNode.parent = lastNode;
break;
}
var t = n.p.MoveUp();
var l = n.p.MoveLeft();
var r = n.p.MoveRight();
var b = n.p.MoveDown();
var tl = n.p.MoveUpLeft();
var tr = n.p.MoveUpRight();
var bl = n.p.MoveDownLeft();
var br = n.p.MoveDownRight();
bool added = false;
int gCost = n.gCost + 1;
n.open = false;
closed.Add(n);
open.RemoveAt(0);
bool lAllowed = !grid[t.x, t.y];
bool rAllowed = !grid[r.x, r.y];
bool bAllowed = !grid[b.x, b.y];
bool tAllowed = !grid[tl.x, tl.y];
if (tAllowed)
{
var n0 = new PathingNode(t, cameFromDown, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
existing.parent = n;
}
}
if (bAllowed)
{
var n0 = new PathingNode(b, cameFromUp, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
existing.parent = n;
}
}
if (rAllowed)
{
var n0 = new PathingNode(r, cameFromLeft, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
existing.parent = n;
}
}
if (lAllowed)
{
var n0 = new PathingNode(l, cameFromRight, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
}
}
if (tAllowed && lAllowed)
{
if (!grid[tl.x, tl.y])
{
var n0 = new PathingNode(tl, cameFromDownRight, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
}
}
}
if (tAllowed && rAllowed)
{
if (!grid[tr.x, tr.y])
{
var n0 = new PathingNode(tr, cameFromDownLeft, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
}
}
}
if (bAllowed && lAllowed)
{
if (!grid[bl.x, bl.y])
{
var n0 = new PathingNode(bl, cameFromUpRight, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
}
}
}
if (bAllowed && rAllowed)
{
if (!grid[br.x, br.y])
{
var n0 = new PathingNode(br, cameFromUpLeft, gCost, n);
var existing = open.Find((searchNode) => searchNode.Equals(n0));
if (existing == null)
{
n0.CalcHeuristicCost(target);
open.Add(n0);
added = true;
}
else if (existing.open)
{
existing.gCost = gCost;
existing.cameFrom = n0.cameFrom;
}
}
}
if (added)
{
open.Sort((node0, node1) =>
{
return(node0.fCost.CompareTo(node1.fCost));
});
}
lastNode = n;
}
List <Vector3> points = new List <Vector3>();
PathingNode nBacktrack = targetNode;
while (nBacktrack != null)
{
points.Add(new Vector3(nBacktrack.p.x + 0.5f, nBacktrack.p.y + 0.5f, 0f));
nBacktrack = nBacktrack.parent;
}
points.Reverse();
points.RemoveAt(0);
Path p = new Path();
p.positions = points.ToArray();
return(p);
}
private void enqueueNode(Queue <PathingNode> queue, Dictionary <int, Dictionary <int, PathingNode> > visitedNodes, Dictionary <int, Dictionary <int, PathingNode> > grid, PathingNode evalNode, int x, int z, int dir)
{
PathingNode node;
if (!grid.ContainsKey(x))
{
return;
}
if (!grid[x].TryGetValue(z, out node))
{
return;
}
if (visitedNodes[x].ContainsKey(z))
{
return;
}
visitedNodes[x][z] = node;
queue.Enqueue(node);
node.nextNode = evalNode;
node.dir = dir;
}
//Quick implementation of A* pathing
private Vector3[] AStar(Vector3 start, Vector3 goal)
{
List <PathingNode> closedSet = new List <PathingNode> ();
List <PathingNode> openSet = new List <PathingNode> ();
Vector3i goalI = new Vector3i(goal.x, goal.y, goal.z);
Vector3i startI = new Vector3i(start.x, start.y - midPointHeight, start.z);
PathingNode currNode = new PathingNode(startI, goalI);
openSet.Add(currNode);
int count = 0;
//Brief check to stop if we're already at the goal; or if our current position is not viable
if (startI.Equals(goalI) || !IsViablePosition(startI))
{
return(null);
}
//Actual A* pathing
while (openSet.Count != 0 && count < maxChecksPerFrame)
{
count++;
currNode = openSet[0];
//get best next node
foreach (PathingNode node in openSet)
{
if (node.GetFScore() < currNode.GetFScore())
{
currNode = node;
}
}
//Did we make it
if (currNode.loc.DistanceSquared(goalI) < 3 || count == maxChecksPerFrame)
{
return(GetPathFromNode(currNode));
}
else
{
openSet.Remove(currNode);
closedSet.Add(currNode);
//Get next allowable moves
List <Vector3i> moves = GetPossibleNextMovesFromPosition(currNode.loc);
foreach (Vector3i move in moves)
{
PathingNode potentialNode = new PathingNode(move, goalI, currNode);
if (closedSet.Contains(potentialNode))
{
}
else if (openSet.Contains(potentialNode)) //See if we need to update the node
{
int index = openSet.IndexOf(potentialNode);
if (openSet[index].g_score > potentialNode.g_score)
{
//Update the node if this version is better
openSet[index] = potentialNode;
}
}
else
{
//Add the node to the openSet if its new
openSet.Add(potentialNode);
}
}
}
}
return(null);
}
void DrawOpenNodes(TestState state)
{
if (state.OpenNodes == null)
{
return;
}
int nodeCount = state.OpenNodes.Count;
if (nodeCount == 0)
{
return;
}
Vector3[] verts = new Vector3[nodeCount * 4];
int[] indecs = new int[nodeCount * 6];
Color[] colors = new Color[nodeCount * 4];
int v = 0;
int i = 0;
for (int k = 0; k < nodeCount; ++k)
{
PathingNode node = state.OpenNodes[k];
verts[v] = new Vector3(node.Location.x, 0, node.Location.y);
verts[v + 1] = new Vector3(node.Location.x, 0, node.Location.y + 1);
verts[v + 2] = new Vector3(node.Location.x + 1, 0, node.Location.y + 1);
verts[v + 3] = new Vector3(node.Location.x + 1, 0, node.Location.y);
Color color = state.VisitedNodes.Contains(node) ? state.VisitedNodeColor : state.OpenNodeColor;
colors[v] = colors[v + 1] = colors[v + 2] = colors[v + 3] = color;
indecs[i++] = v;
indecs[i++] = v + 1;
indecs[i++] = v + 2;
indecs[i++] = v;
indecs[i++] = v + 2;
indecs[i++] = v + 3;
v += 4;
}
string childName = state.Name + "_OpenNodes";
Transform child = transform.Find(childName);
if (child == null)
{
child = new GameObject(childName).transform;
child.SetParent(transform);
child.localPosition = new Vector3(0, 0.1f, 0);
}
Mesh mesh = new Mesh();
mesh.indexFormat = UnityEngine.Rendering.IndexFormat.UInt32;
mesh.vertices = verts;
mesh.triangles = indecs;
mesh.colors = colors;
MeshFilter mf = child.gameObject.GetComponent <MeshFilter>();
if (mf == null)
{
mf = child.gameObject.AddComponent <MeshFilter>();
}
mf.sharedMesh = mesh;
MeshRenderer mr = child.gameObject.GetComponent <MeshRenderer>();
if (mr == null)
{
mr = child.gameObject.AddComponent <MeshRenderer>();
Material mat = new Material(Shader.Find("Unlit/NewUnlitColor"));
//mat.SetColor("_Color", state.OpenNodeColor);
mr.material = mat;
}
}
public PathingNode(GridPoint _loc, PathingNode parent)
: this(_loc)
{
_parent = parent;
g_score = parent.g_score + 1;
}