public void Add(aNode node) { node.HeapIndex = currentItemcount; nodes[currentItemcount] = node; SortUp(node); currentItemcount++; }
//Manage Disconnecting void Disconnect() { Debug.Log("Disconnected."); AudioManager.Instance.PlayClip(disconnectSound, AudioManager.Instance.GetChannel("SFX")); node.outputs.Remove(outNode); aNode s = outNode.GetComponent <aNode>(); s.sendingSignal = false; if (!s.GetComponent <aDAW>()) { //Make sure s is not powered anymore s.inputs.Remove(node.gameObject); if (s.inputs.Count == 0) { s.isPowered = false; } } else { while (s.GetComponent <aDAW>().signalObjs.Contains(node.signalObject)) { s.connectionRenderers.RemoveAt(s.GetComponent <aDAW>().signalObjs.IndexOf(node.signalObject)); s.GetComponent <aDAW>().signalObjs.Remove(node.signalObject); } //Make sure s is not powered anymore s.inputs.Remove(node.gameObject); if (s.inputs.Count == 0) { s.isPowered = false; } } }
private void SortDown(aNode node) { while (true) { int childIndexLeft = node.HeapIndex * 2 + 1; int childIndexRight = node.HeapIndex * 2 + 2; int swapIndex = 0; if (childIndexLeft < currentItemcount) { swapIndex = childIndexLeft; if (childIndexRight < currentItemcount && nodes[childIndexLeft].CompareTo(nodes[childIndexRight]) < 0) // if he has a child on the right and the child on the right has a higher priority { swapIndex = childIndexRight; } if (node.CompareTo(nodes[swapIndex]) < 0) { Swap(node, nodes[swapIndex]); } else // right place = exit { break; } } else // no more childs = exit { break; } } }
// Constructor public AStar(World w, aNode start, aNode goal) { openList = new OpenList<aNode>(); closedList = new ArrayList(); startNode = start; goalNode = goal; thisWorld = w; }
private void Swap(aNode a, aNode b) { nodes[a.HeapIndex] = b; nodes[b.HeapIndex] = a; int indexSwap = a.HeapIndex; a.HeapIndex = b.HeapIndex; b.HeapIndex = indexSwap; }
public aNode RemoveFirst() { aNode firstNode = nodes[0]; currentItemcount--; nodes[0] = nodes[currentItemcount]; nodes[0].HeapIndex = 0; SortDown(nodes[0]); return(firstNode); }
static void Main(string[] args) { int V = 1, EndV = 7; int numberOFNodes = 8; // this is + 1 as being 1 based for (int i = 0; i < numberOFNodes; i++) { graph[i] = new aNode(); } makeGraph(); DFS(V, EndV); Console.ReadLine(); }
private int GetDistance(aNode a, aNode b) { int distanceX = Mathf.Abs(a.gridX - b.gridX); int distanceY = Mathf.Abs(a.gridY - b.gridY); if (distanceX > distanceY) { return(14 * distanceY + 10 * (distanceX - distanceY)); } else { return(14 * distanceX + 10 * (distanceY - distanceX)); } }
private Vector3[] RetracePath(aNode startNode, aNode targetNode) { List <aNode> path = new List <aNode>(); aNode currentNode = targetNode; while (currentNode != startNode) { path.Add(currentNode); currentNode = currentNode.parent; } Vector3[] waypoints = SimplifyPath(path); Array.Reverse(waypoints); return(waypoints); }
static void Main(string[] args) { aNode[] graph = new aNode[100]; bool[] Discovered = new bool[100]; bool[] CompletelyExplored = new bool[100]; int numberOFNodes = 7; // this is + 1 as being 1 based int S = 1, D = 6; //int S = 1, D = 5; for (int i = 0; i < numberOFNodes; i++) { graph[i] = new aNode(); } makeUpGraph(ref graph); findShortestRoute(S, D, graph, numberOFNodes); Console.ReadLine(); }
private void SortUp(aNode node) { int parentIndex = (node.HeapIndex - 1) / 2; while (true) { aNode parentNode = nodes[parentIndex]; if (node.CompareTo(parentNode) > 0) { Swap(node, parentNode); } else { break; } } }
public List <aNode> GetNeighbours(aNode node) { List <aNode> neighbours = new List <aNode>(); for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { int checkX = node.gridX + x; int checkY = node.gridY + y; if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) { neighbours.Add(grid[checkX, checkY]); } } } neighbours.Remove(node); return(neighbours); }
private void CreateGrid() { CalculateGridSize(); grid = new aNode[gridSizeX, gridSizeY]; Vector3 worldBottomLeft = transform.position - (Vector3.right * gridWorldSize.x / 2) - (Vector3.forward * gridWorldSize.y / 2); for (int x = 0; x < gridSizeX; x++) { for (int y = 0; y < gridSizeY; y++) { Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius); bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask)); //int movementPenalty = walkable ? (0) : obstacleAvoidance; //Walkable Pen int movementPenalty = 0; Ray ray = new Ray(worldPoint + Vector3.up * 50f, Vector3.down); RaycastHit hit; if (Physics.Raycast(ray, out hit, 100f, walkableMask)) { movementPenalty = 0; // it has hit a walkbable. } if (!walkable) { movementPenalty = obstacleAvoidance; } grid[x, y] = new aNode(walkable, worldPoint, x, y, movementPenalty); } } BlurPenaltyMap(3); }
public static void Main(string[] args) { World world = new World (); bool dataValid; int[] matchesArray = new int[2]; int userBeginX, userBeginY, userGoalX, userGoalY; char newIOPos, newWorld; bool firstRun = true; do { world.generateUnpathableBlocks (); do { Console.Clear(); if (firstRun) { Console.Write(world.worldPrint () + "\n"); Console.WriteLine("Welcome! This is a demo of the A* pathfinding algorithm."); Console.WriteLine("(Grid Positions can be entered as (1,1) or 1,1 or 1 1.)\n"); } else Console.Write(world.worldPrint() + "\n\n\n"); MatchCollection matches = null; do { // for data validation Console.Write ("Enter (x,y) for the start position: "); dataValid = false; String temp = Console.ReadLine (); matches = Regex.Matches (temp, @"\d+"); if (matches.Count == 2) { dataValid = true; int val = 0; for (int i = 0; i < 2 && dataValid; i++) { matchesArray[i] = ((Int32.TryParse(matches[i].Value, out val)) && val <= 15 && val > 0) ? val : -1; dataValid = matchesArray[i] == -1 ? false: true; } } dataValid = world.blockIsPathable(matchesArray[0] - 1, matchesArray[1] - 1); if (!dataValid) Console.WriteLine("The coordinates you entered were not accepted."); } while (!dataValid); userBeginX = matchesArray[0]; userBeginY = matchesArray[1]; matchesArray [0] = matchesArray [1] = 0; // goal entry do { // for data validation Console.Write ("Enter (x,y) for the goal position: "); dataValid = false; String temp = Console.ReadLine (); matches = Regex.Matches (temp, @"\d+"); if (matches.Count == 2) { dataValid = true; int val = 0; for (int i = 0; i < 2 && dataValid; i++) { matchesArray[i] = ((Int32.TryParse(matches[i].Value, out val)) && val <= 15 && val > 0) ? val : -1; dataValid = matchesArray[i] == -1 ? false: true; } } dataValid = world.blockIsPathable(matchesArray[0] - 1, matchesArray[1] - 1) && dataValid; if (!dataValid) Console.WriteLine("The coordinates you entered were not accepted."); } while (!dataValid); userGoalX = matchesArray [0]; userGoalY = matchesArray [1]; // initialize the start node and goal node aNode beginNode = new aNode (userBeginX-1, userBeginY-1); aNode goalNode = new aNode (userGoalX-1, userGoalY-1); // find the solution AStar aStarInstance = new AStar (world, beginNode, goalNode); bool solutionExists = aStarInstance.AStarSearch (); if (solutionExists && aStarInstance.generatedPath.Count > 1) { // print solution Console.Clear(); Console.Write(world.worldPrint(aStarInstance.generatedPath, beginNode, goalNode)); Console.WriteLine("\nThe path was found!"); Console.WriteLine("Here is the path from start to finish:"); foreach (aNode n in aStarInstance.generatedPath) Console.Write("({0},{1}) ", n.x+1, n.y+1); Console.Write("\n"); } else if (solutionExists && aStarInstance.generatedPath.Count == 1) { Console.Clear(); Console.Write(world.worldPrint(aStarInstance.generatedPath)); Console.WriteLine("\nLooks like you didn't go anywhere..."); Console.WriteLine("Here is where you started and ended:"); foreach (aNode n in aStarInstance.generatedPath) Console.Write("({0},{1}) ", n.x+1, n.y+1); Console.Write("\n"); } else { Console.Clear(); Console.Write("\n\n" + world.worldPrint(beginNode, goalNode)); Console.WriteLine("\nNo path found.\n"); Console.Write("\n"); } // same board, new positions? Console.Write("\n Would you like to try new start and goal positions? : [YyNn] "); do { try { newIOPos = Console.ReadLine()[0]; } catch { dataValid = false; newIOPos = 'f'; continue; } if (newIOPos != 'Y' && newIOPos != 'y' && newIOPos != 'N' && newIOPos != 'n') { dataValid = false; Console.Write("Key invalid. (Try Y or N)"); } else { dataValid = true; if(newIOPos == 'y' || newIOPos == 'Y') { Console.Clear(); Console.Write("\n\n\n"); } } } while (!dataValid); } while (newIOPos == 'y' || newIOPos == 'Y'); // new board and positions? Console.Write(" Would you like to try with a new board? : [YyNn] "); do { try { newWorld = Console.ReadLine()[0]; } catch { dataValid = false; newWorld = 'f'; continue; } if (newWorld != 'Y' && newWorld != 'y' && newWorld != 'N' && newWorld != 'n') { dataValid = false; Console.Write("Key invalid. (Try Y or N)"); } else { dataValid = true; } } while (!dataValid); } while (newWorld == 'y' || newWorld == 'Y'); Console.WriteLine ("Thanks for the visit. \nPress any key..."); Console.Read (); }
public void FindPath(aPathRequest request, Action <aPathResult> callback) { Vector3[] wayPoints = new Vector3[0]; bool pathSuccess = false; aNode startNode = grid.NodeFromWorldPoint(request.pathStart); aNode targetNode = grid.NodeFromWorldPoint(request.pathEnd); if (startNode.walkable && targetNode.walkable) { aHeap openSet = new aHeap(grid.MaxSize); List <aNode> closedSet = new List <aNode>(); openSet.Add(startNode); while (openSet.Count > 0) { aNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (aNode neighbour in grid.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCost = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenality; if (newMovementCost < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCost; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } if (pathSuccess) { wayPoints = RetracePath(startNode, targetNode); } pathSuccess = wayPoints.Length > 0; callback(new aPathResult(wayPoints, pathSuccess, request.callback)); }
public bool Contains(aNode node) { return(Equals(nodes[node.HeapIndex], node)); }
public void UpdateItem(aNode node) { SortUp(node); }
// includes path cost generation for adjacent and diagonal neighbors public int getG(aNode a, aNode b) { int dx = Math.Abs (a.x - b.x); int dy = Math.Abs (a.y - b.y); int G; if (dy == 0 && dx == 1 || dy == 1 && dx == 0) G = 10; // adjacent else if (dy == 1 && dx == 1) G = 14; // diagonal else G = 0; return G; }
// calculate Manhattan distance method public int getH(aNode node) { int dx = Math.Abs(node.x - goalNode.x); int dy = Math.Abs(node.y - goalNode.y); return D * (dx + dy); }
public aNode[] getNeighbors(aNode n) { bool[] coords = new bool[8]; int x = n.x; int y = n.y; // find pathable neighbors pathable int nIndex = 0; for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) // checks -1 and +1 above/bellow the node if ((i * 3) + j != 4) // don't check the center of the 3x3 area coords[nIndex++] = thisWorld.blockIsPathable((j + x) - 1, (i + y) -1); // set the number of neighbors to initialize array int countNeighbors = 0; for (int i = 0; i < coords.Length; i++) if (coords [i]) countNeighbors++; aNode[] neighbs = new aNode[countNeighbors]; // to put the neighbors in the array sequentially nIndex = 0; if (coords[0]) // up-left neighbs[nIndex++] = new aNode(x - 1, y - 1); if (coords[1]) // up neighbs[nIndex++] = new aNode(x, y - 1); if (coords[2]) // up-right neighbs[nIndex++] = new aNode(x + 1, y - 1); if (coords[3]) // left neighbs[nIndex++] = new aNode(x - 1, y); if (coords[4]) // right neighbs[nIndex++] = new aNode(x + 1, y); if (coords[5]) // down-left neighbs[nIndex++] = new aNode(x - 1, y + 1); if (coords[6]) // down neighbs[nIndex++] = new aNode(x, y + 1); if (coords[7]) // down-right neighbs[nIndex++] = new aNode(x + 1, y + 1); return neighbs; }
public String worldPrint(ArrayList path, aNode start, aNode goal) { String output = ""; char[,] worldCoordPath = worldCoordinates.Clone() as char[,]; foreach (aNode n in path) worldCoordPath[n.x ,n.y] = '#'; worldCoordPath[start.x, start.y] = 'S'; worldCoordPath[goal.x, goal.y] = 'G'; for (int i = 0; i < WorldTilesN; i++) { output += i+1; if (i < 9) output += " "; } output += "\n"; for (int i = 0; i < WorldTilesN; i++) { for (int j = 0; j < WorldTilesN; j++) output += worldCoordPath[j,i] + " "; output += (i + 1) + "\n"; } return output; }
// Update is called once per frame public void Update() { //Bug fixing things if (sendingSignal == true && signalObject == null) { GameObject[] sigPos = GameObject.FindGameObjectsWithTag("SignalPosition"); for (int index = 0; index < sigPos.Length; index++) { if (sigPos[index].GetComponent <SignalFlowObject>().objectSignalNumber == this.nodeSignalNumber) { signalObject = sigPos[index].GetComponent <SignalFlowObject>().signalFlowObjectType; } } } //Win checking stuff if (isPowered == false) { nodeSignalNumber = 0; } //Making sure signals don't show when there is no power if (nodeType != Type.ET_MICROPHONE && isPowered == false) { signalObject = null; } //Only execute if this is above 0 if (outputs.Count > 0) { ShowConnections(); } if (connectionRenderers != null && signalObject != null && sendingSignal) { for (int index = 0; index < connectionRenderers.Count; index++) { counter += Time.deltaTime; if (counter > 0.5f && connectionRenderers[index] != null) { counter = 0; GameObject tri1 = Instantiate(signalObject, this.gameObject.transform.position, Quaternion.identity); if (tri1 != null && tri1.GetComponent <LineShape>()) { tri1.GetComponent <LineShape>().positionA = connectionRenderers[index].GetComponent <LineRenderer>().GetPosition(0); tri1.GetComponent <LineShape>().positionB = connectionRenderers[index].GetComponent <LineRenderer>().GetPosition(1); } } } } if (inputs.Count > 0) { foreach (GameObject n in inputs) { aNode an = n.GetComponent <aNode>(); if (an.sendingSignal) { sendingSignal = true; } } } }