// Find start and end Node
private void SetStartAndEndNode(Vector3 start, Vector3 end)
{
startNode = FindClosestNode(start);
// endNode = FindClosestNode(end);
// print ("setstartandendnode " + end);
endNode = FindClosestNode(end);
}
// Find start and end Node
private void SetStartAndEndNode(int p, Vector3 start, Vector3 end, int previous)
{
startNode = FindClosestNode(p, start, previous);
// endNode = FindClosestNode(end);
// print ("setstartandendnode " + end);
endNode = FindClosestNode(p, end, -1);
}
private WaypointListNode FindClosestNode(Vector3 pos)
{
Stopwatch a = new Stopwatch();
a.Start();
int ID = -1;
float lowestDist = Mathf.Infinity;
foreach (WaypointNode m in Map)
{
float d = Vector3.Distance(m.position, pos);
if (d < lowestDist)
{
ID = m.ID;
lowestDist = d;
}
}
if (ID > -1)
{
WaypointListNode wp = new WaypointListNode(Map[ID].position, Map[ID].ID, null, Map[ID].neighbors);
return(wp);
}
else
{
return(null);
}
}
private float GetMovementCost(WaypointListNode p1, WaypointNode p2)
{
float weight = 1;
if (isNeighbor(p1, p2))
{
weight = p1.I;
}
return(weight * Vector3.Distance(p1.position, p2.position));
}
public WaypointListNode(Vector3 p, int id, WaypointListNode wpParent = null, List<WaypointNode> n = null, float f = 0, float g = 0, float h = 0)
{
position = p;
ID = id;
parent = wpParent;
neighbors = n;
F = f;
G = g;
H = h;
}
private float GetI(WaypointListNode n1, WaypointNode n2)
{
if (n1.ID + 1 == n2.ID && n1.I != 0)
{
UnityEngine.Debug.Log("WOoo special " + n1.ID + "\n");
return(n1.I);
//return 1;
}
return(1);
}
private bool isNeighbor(WaypointListNode p1, WaypointNode p2)
{
foreach (WaypointNode n in p1.neighbors)
{
if (n.ID == p2.ID)
{
return(true);
}
}
return(false);
}
void Start ()
{
GameObject[] M = GameObject.FindGameObjectsWithTag("Waypoint");
Map = new WaypointNode[M.Length];
for (int i = 0; i < M.Length; i++)
{
Map[i] = M[i].GetComponent<WaypointNode>();
Map[i].ID = i;
}
openList = new WaypointListNode[Map.Length];
closedList = new WaypointListNode[Map.Length];
}
public void IniateWaypoints()
{
GameObject[] M = GameObject.FindGameObjectsWithTag("Waypoint");
Map = new WaypointNode[M.Length];
for (int i = 0; i < M.Length; i++)
{
Map[i] = M[i].GetComponent <WaypointNode>();
Map[i].ID = i;
}
openList = new WaypointListNode[Map.Length];
closedList = new WaypointListNode[Map.Length];
}
void Start()
{
/*
* GameObject[] M = GameObject.FindGameObjectsWithTag("Waypoint");
* Map = new WaypointNode[M.Length];
* for (int i = 0; i < M.Length; i++)
* {
* Map[i] = M[i].GetComponent<WaypointNode>();
* Map[i].ID = i;
* }
*/
openList = new WaypointListNode[Map.Length];
closedList = new WaypointListNode[Map.Length];
}
private void NeighbourCheck(int p)
{
foreach (WaypointNode wp in currentNode.neighbors)
{
if (wp != null)
{
if (!OnClosedList(p, wp.ID))
{
//If it is not on the open list then add it to
if (!OnOpenList(p, wp.ID))
{
WaypointListNode addNode = new WaypointListNode(wp.position, wp.ID, wp.Iii, currentNode, wp.neighbors);
addNode.H = GetHeuristics(endNode.position, wp.position); // vertex to goal
addNode.G = GetMovementCost(currentNode, wp) + currentNode.G; //start to vertex
addNode.F = addNode.H + addNode.G;
//Insert on open list
// UnityEngine.Debug.Log("CurrentNode " + currentNode.ID + " For " + addNode.ID + " H " + addNode.H + " G " + addNode.G + " F " + addNode.F + "\n");
paths[p].openList[addNode.ID] = addNode;
//Insert on sorted list
BHInsertNode(p, new NodeSearch(addNode.ID, addNode.F));
//sortedOpenList.Add(new NodeSearch(addNode.ID, addNode.F));
}
else
{
///If it is on openlist then check if the new paths movement cost is lower
WaypointListNode n = GetNodeFromOpenList(p, wp.ID);
if (currentNode.G + GetMovementCost(currentNode, wp) < paths[p].openList[wp.ID].G)
{
n.parent = currentNode;
n.G = currentNode.G + GetMovementCost(currentNode, wp);
n.F = n.G + (n.H);
BHSortNode(p, n.ID, n.F);
//ChangeFValue(n.ID, n.F);
}
}
}
}
}
}
private void NeighbourCheck()
{
foreach (WaypointNode wp in currentNode.neighbors)
{
if (wp != null)
{
if (!OnClosedList(wp.ID))
{
//If it is not on the open list then add it to
if (!OnOpenList(wp.ID))
{
WaypointListNode addNode = new WaypointListNode(wp.position, wp.ID, currentNode, wp.neighbors);
addNode.H = GetHeuristics(endNode.position, wp.position);
addNode.G = GetMovementCost(currentNode.position, wp.position) + currentNode.G;
addNode.F = addNode.H + addNode.G;
//Insert on open list
openList[addNode.ID] = addNode;
//Insert on sorted list
BHInsertNode(new NodeSearch(addNode.ID, addNode.F));
//sortedOpenList.Add(new NodeSearch(addNode.ID, addNode.F));
}
else
{
///If it is on openlist then check if the new paths movement cost is lower
WaypointListNode n = GetNodeFromOpenList(wp.ID);
if (currentNode.G + GetMovementCost(currentNode.position, wp.position) < openList[wp.ID].G)
{
n.parent = currentNode;
n.G = currentNode.G + GetMovementCost(currentNode.position, wp.position);
n.F = n.G + n.H;
BHSortNode(n.ID, n.F);
//ChangeFValue(n.ID, n.F);
}
}
}
}
}
}
private WaypointListNode FindClosestNode(int p, Vector3 pos, int previous)
{
bool check_previous = !(previous < 0);
Stopwatch a = new Stopwatch();
a.Start();
int ID = -1;
float lowestDist = Mathf.Infinity;
foreach (WaypointNode m in paths[p].Map)
{
if (check_previous && previous <= m.ID && Random.RandomRange(0, 1f) > 0.25f)
{
continue;
}
float d = Vector3.Distance(m.position, pos);
if (d < lowestDist)
{
ID = m.ID;
lowestDist = d;
}
}
if (ID > -1)
{
//UnityEngine.Debug.Log(paths[p].ToString());
//UnityEngine.Debug.Log(paths[p].Map.Length);
WaypointListNode wp = new WaypointListNode(paths[p].Map[ID].position, paths[p].Map[ID].ID, paths[p].Map[ID].Iii, null, paths[p].Map[ID].neighbors);
return(wp);
//print("Closest waypoint is " + ID);
}
else
{
return(null);
}
}
// Find start and end Node
private void SetStartAndEndNode(Vector3 start, Vector3 end)
{
startNode = FindClosestNode(start);
endNode = FindClosestNode(end);
}
public List <Vector3> FindPath(Vector3 startPos, Vector3 endPos)
{
//The list we returns when path is found
List <Vector3> returnPath = new List <Vector3>();
//Find start and end nodes, if we cant return null and stop!
SetStartAndEndNode(startPos, endPos);
CheckEndPosition(endPos);
if (startNode != null && endNode != null)
{
//Clear lists if they are filled
Array.Clear(openList, 0, openList.Length);
Array.Clear(closedList, 0, openList.Length);
if (sortedOpenList.Count > 0)
{
sortedOpenList.Clear();
}
//Insert start node
openList[startNode.ID] = startNode;
//sortedOpenList.Add(new NodeSearch(startNode.ID, startNode.F));
BHInsertNode(new NodeSearch(startNode.ID, startNode.F));
bool endLoop = false;
while (!endLoop)
{
//If we have no nodes on the open list AND we are not at the end, then we got stucked! return empty list then.
if (sortedOpenList.Count == 0)
{
print("Empty Openlist, closedList");
return(new List <Vector3>());
}
//Get lowest node and insert it into the closed list
int id = BHGetLowest();
//sortedOpenList.Sort(sort);
//int id = sortedOpenList[0].ID;
currentNode = openList[id];
closedList[currentNode.ID] = currentNode;
openList[id] = null;
//sortedOpenList.RemoveAt(0);
if (currentNode.ID == endNode.ID)
{
endLoop = true;
continue;
}
//Now look at neighbours, check for unwalkable tiles, bounderies, open and closed listed nodes.
NeighbourCheck();
}
while (true)
{
returnPath.Add(currentNode.position);
if (currentNode.parent != null)
{
currentNode = currentNode.parent;
}
else
{
break;
}
}
returnPath.Reverse();
//Now make sure we do not go backwards or go to long
if (freeSpot && SearchMethod == WaypointMethod.ExactPosition)
{
returnPath.Add(endPos);
if (returnPath.Count > 2)
{
if (Vector3.Distance(returnPath[returnPath.Count - 1], returnPath[returnPath.Count - 3]) < Vector3.Distance(returnPath[returnPath.Count - 3], returnPath[returnPath.Count - 2]) && freeSpot)
{
returnPath.RemoveAt(returnPath.Count - 2);
}
}
}
//Check if start pos i closer to second waypoint
if (returnPath.Count > 1)
{
if (Vector3.Distance(returnPath[1], startPos) < Vector3.Distance(returnPath[0], returnPath[1]))
{
returnPath.RemoveAt(0);
}
}
return(returnPath);
}
else
{
return(null);
}
}
public List<Vector3> FindPath(Vector3 startPos, Vector3 endPos)
{
//The list we returns when path is found
List<Vector3> returnPath = new List<Vector3>();
//Find start and end nodes, if we cant return null and stop!
SetStartAndEndNode(startPos, endPos);
CheckEndPosition(endPos);
if (startNode != null && endNode != null)
{
//Clear lists if they are filled
Array.Clear(openList, 0, openList.Length);
Array.Clear(closedList, 0, openList.Length);
if (sortedOpenList.Count > 0) { sortedOpenList.Clear(); }
//Insert start node
openList[startNode.ID] = startNode;
//sortedOpenList.Add(new NodeSearch(startNode.ID, startNode.F));
BHInsertNode(new NodeSearch(startNode.ID, startNode.F));
bool endLoop = false;
while (!endLoop)
{
//If we have no nodes on the open list AND we are not at the end, then we got stucked! return empty list then.
if (sortedOpenList.Count == 0)
{
print("Empty Openlist, closedList");
return new List<Vector3>();
}
//Get lowest node and insert it into the closed list
int id = BHGetLowest();
//sortedOpenList.Sort(sort);
//int id = sortedOpenList[0].ID;
currentNode = openList[id];
closedList[currentNode.ID] = currentNode;
openList[id] = null;
//sortedOpenList.RemoveAt(0);
if (currentNode.ID == endNode.ID)
{
endLoop = true;
continue;
}
//Now look at neighbours, check for unwalkable tiles, bounderies, open and closed listed nodes.
NeighbourCheck();
}
while (true)
{
returnPath.Add(currentNode.position);
if (currentNode.parent != null)
{
currentNode = currentNode.parent;
}
else
{
break;
}
}
returnPath.Reverse();
//Now make sure we do not go backwards or go to long
if (freeSpot && SearchMethod == WaypointMethod.ExactPosition)
{
returnPath.Add(endPos);
if (returnPath.Count > 2)
{
if (Vector3.Distance(returnPath[returnPath.Count - 1], returnPath[returnPath.Count - 3]) < Vector3.Distance(returnPath[returnPath.Count - 3], returnPath[returnPath.Count - 2]) && freeSpot)
{
returnPath.RemoveAt(returnPath.Count - 2);
}
}
}
//Check if start pos i closer to second waypoint
if (returnPath.Count > 1)
{
if (Vector3.Distance(returnPath[1], startPos) < Vector3.Distance(returnPath[0], returnPath[1]))
{
returnPath.RemoveAt(0);
}
}
return returnPath;
}
else
{
return null;
}
}
// Find start and end Node
private void SetStartAndEndNode(Vector3 start, Vector3 end)
{
startNode = FindClosestNode(start);
endNode = FindClosestNode(end);
}
private void NeighbourCheck()
{
foreach (WaypointNode wp in currentNode.neighbors)
{
if (wp != null)
{
if (!OnClosedList(wp.ID))
{
//If it is not on the open list then add it to
if (!OnOpenList(wp.ID))
{
WaypointListNode addNode = new WaypointListNode(wp.position, wp.ID, currentNode, wp.neighbors);
addNode.H = GetHeuristics(endNode.position, wp.position);
addNode.G = GetMovementCost(currentNode.position, wp.position) + currentNode.G;
addNode.F = addNode.H + addNode.G;
//Insert on open list
openList[addNode.ID] = addNode;
//Insert on sorted list
BHInsertNode(new NodeSearch(addNode.ID, addNode.F));
//sortedOpenList.Add(new NodeSearch(addNode.ID, addNode.F));
}
else
{
///If it is on openlist then check if the new paths movement cost is lower
WaypointListNode n = GetNodeFromOpenList(wp.ID);
if (currentNode.G + GetMovementCost(currentNode.position, wp.position) < openList[wp.ID].G)
{
n.parent = currentNode;
n.G = currentNode.G + GetMovementCost(currentNode.position, wp.position);
n.F = n.G + n.H;
BHSortNode(n.ID, n.F);
//ChangeFValue(n.ID, n.F);
}
}
}
}
}
}
private WaypointListNode FindClosestNode(Vector3 pos)
{
Stopwatch a = new Stopwatch();
a.Start();
int ID = -1;
float lowestDist = Mathf.Infinity;
foreach (WaypointNode m in Map)
{
float d = Vector3.Distance(m.position, pos);
if (d < lowestDist)
{
ID = m.ID;
lowestDist = d;
}
}
if (ID > -1)
{
WaypointListNode wp = new WaypointListNode(Map[ID].position, Map[ID].ID, null, Map[ID].neighbors);
return wp;
}
else
{
return null;
}
}
