// 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; } }
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; } }