private void addInitialNode(int x, int y, MinHeap <GraphNode, float> queue, bool openB)
{
var flags = openB ? 2u : 1u;
var vertexID = graph.GetVertexID(x, y);
if (vertexID.ID != -1)
{
var node = new GraphNode()
{
VertexID = vertexID.ID,
ChunkID = vertexID.ChunkID,
_flags = flags
};
queue.Add(node, 0);
}
// Its not a graph node hence we add the direct h reachable
else
{
var node = new GraphNode()
{
VertexID = -1,
ChunkID = -1,
_flags = flags
};
var reachable = graph.GetDirectHReachable(x, y);
for (var i = 0; i < reachable.Count; i++)
{
var neighbor = new GraphNode()
{
VertexID = reachable[i].ID,
ChunkID = reachable[i].ChunkID,
_flags = flags
};
var cost = SubGoalGrid.Diagonal(x, y, reachable[i].GridPosition / graph.sizeY, reachable[i].GridPosition % graph.sizeY);
if (openB)
{
neighbor.ParentB = node;
neighbor.CostB = cost;
}
else
{
neighbor.ParentA = node;
neighbor.CostA = cost;
}
queue.Add(neighbor, cost);
}
}
}
// ConstructEdges constructs edges for subgoalds within the specified area
public void ConstructEdges(List <Vertex> includingNeighborVertices, Dictionary <int, int> includingNeighborGridPositionToVertex)
{
var chunkX = chunkNumber / chunkSizeY;
var chunkY = chunkNumber % chunkSizeY;
var startX = chunkX * chunkSize;
var startY = chunkY * chunkSize;
var endX = chunkX * chunkSize + chunkSize;
var endY = chunkY * chunkSize + chunkSize;
var edges = new List <Edge>();
for (var i = 0; i < includingNeighborVertices.Count; i++)
{
var x = includingNeighborVertices[i].GridPosition / sizeY;
var y = includingNeighborVertices[i].GridPosition % sizeY;
var gridPosition = x * sizeY + y;
if (x < startX || x >= endX || y < startY || y >= endY)
{
continue;
}
var otherSubGoals = GetDirectHReachable(x, y, includingNeighborGridPositionToVertex);
// Get the real vertex id
if (gridPositionToVertex.TryGetValue(gridPosition, out int vID))
{
vertices[vID] = new Vertex
{
EdgeOffset = edges.Count,
GridPosition = gridPosition
};
}
else
{
throw new Exception("This should never happen");
}
for (int j = 0, count = otherSubGoals.Count; j < count; j++)
{
var otherSubGoal = otherSubGoals[j];
if (otherSubGoal == i)
{
continue;
}
var ox = includingNeighborVertices[otherSubGoal].GridPosition / sizeY;
var oy = includingNeighborVertices[otherSubGoal].GridPosition % sizeY;
var oGridPosition = includingNeighborVertices[otherSubGoal].GridPosition;
var isOutsideOfChunk = (ox < startX || ox >= endX || oy < startY || oy >= endY);
var edgeID = (ulong)otherSubGoal << 32 | (ulong)(uint)i;
var toVertexID = -1;
if (isOutsideOfChunk)
{
toVertexID = -1;
}
else if (gridPositionToVertex.TryGetValue(oGridPosition, out int oID))
{
toVertexID = oID;
}
else
{
throw new Exception("This should never happen");
}
float cost = SubGoalGrid.Diagonal(x, y, ox, oy);
if (grid.GetWeight(x, y) == CellType.Road && grid.GetWeight(ox, oy) == CellType.Road)
{
cost *= 0.5f;
}
edges.Add(new Edge()
{
ToVertex = toVertexID,
ToVertexGridPosition = oGridPosition,
Cost = cost
});
}
}
this.edges = edges.ToArray();
}
// ConstructEdges constructs edges for subgoalds within the specified area
public void ConstructEdges(List <Vertex> includingNeighborVertices, Dictionary <int, int> includingNeighborGridPositionToVertex)
{
var chunkX = chunkNumber / chunkSizeY;
var chunkY = chunkNumber % chunkSizeY;
var startX = chunkX * overlayGraph.chunkSize;
var startY = chunkY * overlayGraph.chunkSize;
var endX = chunkX * overlayGraph.chunkSize + overlayGraph.chunkSize;
var endY = chunkY * overlayGraph.chunkSize + overlayGraph.chunkSize;
edges = new List <Edge>();
vertexEdgeMapping = new List <int>();
var createdEdges = new Dictionary <ulong, int>();
for (var i = 0; i < includingNeighborVertices.Count; i++)
{
var x = includingNeighborVertices[i].GridPosition / sizeY;
var y = includingNeighborVertices[i].GridPosition % sizeY;
var gridPosition = x * sizeY + y;
if (x < startX || x >= endX || y < startY || y >= endY)
{
continue;
}
var otherSubGoals = GetDirectHReachable(x, y, includingNeighborGridPositionToVertex);
// Get the real vertex id
if (gridPositionToVertex.TryGetValue(gridPosition, out int vID))
{
vertices[vID] = new Vertex
{
EdgeOffset = vertexEdgeMapping.Count,
GridPosition = gridPosition,
CellNumber = vertices[vID].CellNumber
};
}
else
{
throw new Exception("This should never happen");
}
for (int j = 0, count = otherSubGoals.Count; j < count; j++)
{
var otherSubGoal = otherSubGoals[j];
if (otherSubGoal == i)
{
continue;
}
var ox = includingNeighborVertices[otherSubGoal].GridPosition / sizeY;
var oy = includingNeighborVertices[otherSubGoal].GridPosition % sizeY;
var oGridPosition = includingNeighborVertices[otherSubGoal].GridPosition;
var isOutsideOfChunk = (ox < startX || ox >= endX || oy < startY || oy >= endY);
var edgeID = (ulong)otherSubGoal << 32 | (ulong)(uint)i;
if (createdEdges.TryGetValue(edgeID, out int edgeOffset) == false)
{
var toVertexID = -1;
if (isOutsideOfChunk)
{
toVertexID = -1;
}
else if (gridPositionToVertex.TryGetValue(oGridPosition, out int oID))
{
toVertexID = oID;
}
else
{
throw new Exception("This should never happen");
}
edges.Add(new Edge()
{
FromVertex = vID,
ToVertex = toVertexID,
ToVertexGridPosition = oGridPosition,
Cost = SubGoalGrid.Diagonal(x, y, ox, oy)
});
var newEdgeID = (ulong)i << 32 | (ulong)(uint)otherSubGoal;
createdEdges[newEdgeID] = edges.Count - 1;
vertexEdgeMapping.Add(edges.Count - 1);
}
else
{
vertexEdgeMapping.Add(edgeOffset);
}
}
}
}