void updateVertex(nodesGraph u)
{
int noNeighbours = 0;
if (u != graph[endCoordX, endCoordY])
{
float TMP = System.Single.PositiveInfinity;
foreach (nodesGraph v in u.nodesList)
{
if (nodeTypes[nodes[v.x, v.y]].isWalkable == 0)
{
noNeighbours++;
continue;
}
else if (v.g <= TMP)
{
u.rhs = v.g + 1;
TMP = v.g;
}
}
if (noNeighbours == u.nodesList.Count)
{
u.rhs = System.Single.PositiveInfinity;
}
}
if (priorityQ.ContainsKey(u))
{
priorityQ.Remove(u);
}
if (u.g != u.rhs)
{
priorityQ.Add(u, calcKey(u));
}
}
Pair <float, float> calcKey(nodesGraph s)
{
Pair <float, float> pair = new Pair <float, float>(
Mathf.Min(s.g, (s.rhs + Mathf.Sqrt((Mathf.Pow(startCoordX - s.x, 2)) + (Mathf.Pow(startCoordY - s.y, 2))))), //first
Mathf.Min(s.g, s.rhs) //second
);
return(pair);
}
void updateEdgeCost(nodesGraph u)
{
if (nodeTypes[nodes[u.x, u.y]].isWalkable > 0)
{
updateVertex(u);
}
foreach (nodesGraph v in u.nodesList)
{
if (nodeTypes[nodes[v.x, v.y]].isWalkable == 0)
{
continue;
}
else
{
updateVertex(v);
}
}
}
void localStart(nodesGraph s)
{
int localStartX = startCoordX;
int localStartY = startCoordY;
float TMPG = s.g;
foreach (nodesGraph v in s.nodesList)
{
if (v.g < TMPG)
{
localStartX = v.x;
localStartY = v.y;
TMPG = v.g;
}
}
startCoordX = localStartX;
startCoordY = localStartY;
}
IEnumerator newRemovedObsticle()
{
float control = 0;
float duration = 2;
Pair <int, int> chosenNodeCoords = pickNode(notWalkableCoords);
Material clr = notWalkableNodes[chosenNodeCoords].GetComponent <Renderer>().material;
Color defaultClr = clr.color;
Color newClr = Color.blue;
while (control < 1)
{
clr.color = Color.Lerp(defaultClr, newClr, control);
control += Time.deltaTime / duration;
yield return(null);
}
Destroy(notWalkableNodes[chosenNodeCoords]);
nodes[chosenNodeCoords.first, chosenNodeCoords.second] = 1;
notWalkableCoords.Remove(chosenNodeCoords);
notWalkableNodes.Remove(chosenNodeCoords);
GameObject newWalk = (GameObject)Instantiate(nodeTypes[nodes[chosenNodeCoords.first, chosenNodeCoords.second]].nodePrefab, new Vector3(chosenNodeCoords.first, 0, chosenNodeCoords.second), Quaternion.identity);
walkableCoords.Add(chosenNodeCoords);
walkableNodes.Add(chosenNodeCoords, newWalk);
graph[chosenNodeCoords.first, chosenNodeCoords.second].g = System.Single.PositiveInfinity;
graph[chosenNodeCoords.first, chosenNodeCoords.second].rhs = System.Single.PositiveInfinity;
newRemovedObsticleData = graph[chosenNodeCoords.first, chosenNodeCoords.second];
updateEdgeCost(newRemovedObsticleData);
updateKeys();
waitForChanges = false;
}
//generating graph nodes with neighbours
void generateGraph()
{
graph = new nodesGraph[nodesAmountX, nodesAmountY];
for (int x = 0; x < nodesAmountX; x++)
{
for (int y = 0; y < nodesAmountY; y++)
{
graph[x, y] = new nodesGraph();
graph[x, y].x = x;
graph[x, y].y = y;
}
}
for (int x = 0; x < nodesAmountX; x++)
{
for (int y = 0; y < nodesAmountY; y++)
{
if (y < nodesAmountY - 1)
{
graph[x, y].nodesList.Add(graph[x, y + 1]);
}
if (x < nodesAmountX - 1)
{
graph[x, y].nodesList.Add(graph[x + 1, y]);
}
if (y > 0)
{
graph[x, y].nodesList.Add(graph[x, y - 1]);
}
if (x > 0)
{
graph[x, y].nodesList.Add(graph[x - 1, y]);
}
}
}
}
void computeShortestPath()
{
nodesGraph u = new nodesGraph();
float key1 = System.Single.PositiveInfinity;
float key2 = System.Single.PositiveInfinity;
do
{
key1 = System.Single.PositiveInfinity;
key2 = System.Single.PositiveInfinity;
foreach (var v in priorityQ)
{
if (v.Value.first < key1)
{
key1 = v.Value.first;
key2 = v.Value.second;
u = v.Key;
}
else if (v.Value.first == key1)
{
if (v.Value.second < key2)
{
key2 = v.Value.second;
u = v.Key;
}
}
}
priorityQ.Remove(u);
if (u.g > u.rhs)
{
u.g = u.rhs;
foreach (nodesGraph s in u.nodesList)
{
if (nodeTypes[nodes[s.x, s.y]].isWalkable == 0)
{
continue;
}
else
{
updateVertex(s);
}
}
}
else
{
u.g = System.Single.PositiveInfinity;
foreach (nodesGraph s in u.nodesList)
{
if (nodeTypes[nodes[s.x, s.y]].isWalkable == 0)
{
continue;
}
else
{
updateVertex(s);
}
}
updateVertex(u);
}
} while (key1 < calcKey(graph[startCoordX, startCoordY]).first ||
key2 < calcKey(graph[startCoordX, startCoordY]).second ||
graph[startCoordX, startCoordY].rhs != graph[startCoordX, startCoordY].g);
}
