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