public void ConnectSuperNode(Node from, SuperNode superNode, float dist)
{
var directChild = dist <= superNode.GridSize;
if (SuperNodes.ContainsKey(superNode))
{
if (SuperNodes[superNode].Length <= dist)
{
return;
}
}
else
{
if (directChild)
{
superNode.ChildNodes.Add(this);
}
}
SuperNodes[superNode] = new SuperNodeConnection(from, superNode, dist);
if (directChild)
{
foreach (var snode in SuperNodes.Keys)
{
if (snode == superNode || SuperNodes[snode].Length > superNode.GridSize)
{
continue;
}
snode.ConnectTo(superNode, dist, from);
}
}
}
public void RecalculateSuperNodePathAfterAdd(SuperNode superNode)
{
var neighbours = GetNeighbours().Where(n => n.To.SuperNodes.ContainsKey(superNode)).ToList();
var queue = new PriorityQueue <Edge>();
foreach (var neighbour in neighbours)
{
queue.Enqueue(neighbour, neighbour.To.SuperNodes[superNode].Length + neighbour.Length);
}
while (!queue.IsEmpty())
{
var n = queue.Dequeue();
if (!Equals(n.To.SuperNodes[superNode].To))
{
var dist = n.Length + n.To.SuperNodes[superNode].Length;
if (!SuperNodes.ContainsKey(superNode) || SuperNodes[superNode].Length > dist)
{
ConnectSuperNode(n.To, superNode, dist);
foreach (var neighbour in neighbours.Where(ne => Equals(ne.To)))
{
neighbour.To.RecalculateSuperNodePathAfterAdd(superNode);
}
return;
}
}
}
}
public bool RecalculateSuperNodePathAfterDelete(SuperNode superNode, VoxelGraph graph)
{
SuperNodeConnection old = null;
if (SuperNodes.ContainsKey(superNode))
{
old = SuperNodes[superNode];
SuperNodes.Remove(superNode);
}
var neighbours = GetNeighbours().Where(n => n.To.SuperNodes.ContainsKey(superNode)).ToList();
var queue = new PriorityQueue <Edge>();
foreach (var neighbour in neighbours)
{
queue.Enqueue(neighbour, neighbour.To.SuperNodes[superNode].Length + neighbour.Length);
}
while (!queue.IsEmpty())
{
var n = queue.Dequeue();
if (!n.To.SuperNodes.ContainsKey(superNode))
{
continue;
}
if (Equals(n.To.SuperNodes[superNode].To))
{
if (n.To.RecalculateSuperNodePathAfterDelete(superNode, graph))
{
queue.Enqueue(n, n.To.SuperNodes[superNode].Length + n.Length);
}
}
else
{
var dist = n.Length + n.To.SuperNodes[superNode].Length;
if (old != null && old.To.Equals(n.To) && old.Length.Equals(dist))
{
ConnectSuperNode(n.To, superNode, dist);
}
else
{
ConnectSuperNode(n.To, superNode, dist);
graph.MarkDirty(this);
foreach (var neighbour in GetNeighbours().Where(ne => ne.To.SuperNodes.ContainsKey(superNode) && Equals(ne.To.SuperNodes[superNode].To)))
{
neighbour.To.RecalculateSuperNodePathAfterDeleteRec(superNode, graph);
}
}
return(true);
}
}
graph.MarkDirty(this);
superNode.RemoveChildNode(this);
return(false);
}
public void KillSuperNode(SuperNode node, VoxelGraph graph)
{
if (SuperNodes[node].Length <= node.GridSize)
{
graph.MarkDirty(this);
}
SuperNodes.Remove(node);
foreach (var neighbour in GetNeighbours())
{
if (neighbour.To.SuperNodes.ContainsKey(node))
{
neighbour.To.KillSuperNode(node, graph);
}
}
}
public bool ConnectTo(SuperNode node, float dist, Node via)
{
SuperNodeEdge old = null;
if (_neighbours.ContainsKey(node))
{
old = (SuperNodeEdge)_neighbours[node];
}
if (old == null || old.Length >= dist)
{
_neighbours[node] = new SuperNodeEdge(node, dist, via);
node._neighbours[this] = new SuperNodeEdge(this, dist, via);
return(true);
}
return(false);
}
private void RecalculateSuperNodePathAfterDeleteRec(SuperNode superNode, VoxelGraph graph)
{
var neighbours = GetNeighbours().Where(n => n.To.SuperNodes.ContainsKey(superNode));
float length = 0;
Edge closest = null;
foreach (var neighbour in neighbours)
{
var curDist = neighbour.Length + neighbour.To.SuperNodes[superNode].Length;
if (closest == null)
{
closest = neighbour;
length = curDist;
}
else if (length > curDist)
{
closest = neighbour;
length = curDist;
}
}
if (closest == null || closest.To.Equals(SuperNodes[superNode].To))
{
if (SuperNodes[superNode].Length.Equals(length))
{
return;
}
SuperNodes[superNode].Length = length;
}
else
{
SuperNodes.Remove(superNode);
ConnectSuperNode(closest.To, superNode, length);
foreach (var neighbour in GetNeighbours().Where(ne => ne.To.SuperNodes.ContainsKey(superNode) && Equals(ne.To.SuperNodes[superNode].To)))
{
neighbour.To.RecalculateSuperNodePathAfterDeleteRec(superNode, graph);
}
}
if (length >= superNode.GridSize)
{
graph.MarkDirty(this);
}
}
private void AddSupernodeGrid(int gridSize, Vector3I size)
{
for (var dX = Math.Min(gridSize / 2, size[0]); dX <= size[0]; dX += gridSize)
{
for (var dY = Math.Min(gridSize / 2, size[1]); dY <= size[1]; dY += gridSize)
{
for (var dZ = Math.Min(gridSize / 2, size[2]); dZ <= size[2]; dZ += gridSize)
{
var node = _grid.GetNearestItem(new Vector3I(dX, dY, dZ), 5);
if (node == null)
{
continue;
}
var supernode = new SuperNode(node.Position, gridSize);
_gridT1[dX, dY, dZ] = supernode;
Dijkstra.Fill(GetNode(supernode.Position), gridSize, supernode);
}
}
}
}
private void FillSupernodeHoles(int gridSize, Vector3I size, Vector3I startPosition)
{
for (var dX = startPosition.x; dX <= size[0]; dX++)
{
for (var dY = startPosition.y; dY <= size[1]; dY++)
{
for (var dZ = startPosition.z; dZ <= size[2]; dZ++)
{
var node = _grid[dX, dY, dZ];
if (node == null || node.SuperNodes.Count > 0)
{
continue;
}
var supernode = new SuperNode(node.Position, gridSize);
_gridT1[dX, dY, dZ] = supernode;
Dijkstra.Fill(GetNode(supernode.Position), gridSize, supernode);
}
}
}
return;
}
private void ProcessDirtyNodes()
{
foreach (var dirtyNode in _dirtyNodes.ToArray())
{
var node = GetNode(dirtyNode.Position);
if (node == null)
{
continue;
}
if (!dirtyNode.HasDirectSupernode)
{
var supernode = new SuperNode(dirtyNode.Position, _gridSize);
_gridT1[dirtyNode.Position.x, dirtyNode.Position.y, dirtyNode.Position.z] = supernode;
Dijkstra.Fill(node, _gridSize, supernode);
EnsureNeighbourCoverage(_gridSize, new List <SuperNode>()
{
supernode
});
}
}
_dirtyNodes.Clear();
}