public void drawLinePathfindHallways()
{
DungeonPathfinder3D aStar = new DungeonPathfinder3D(size);
foreach (var edge in selectedEdges)
{
var startRoom = (edge.U as Vertex <Room>).Item;
var endRoom = (edge.V as Vertex <Room>).Item;
var startPosf = startRoom.bounds.center;
var endPosf = endRoom.bounds.center;
var startPos = new Vector3Int((int)startPosf.x, (int)startPosf.y, (int)startPosf.z);
var endPos = new Vector3Int((int)endPosf.x, (int)endPosf.y, (int)endPosf.z);
var path = aStar.FindPath(startPos, endPos, (DungeonPathfinder3D.Node a, DungeonPathfinder3D.Node b) => {
var pathCost = new DungeonPathfinder3D.PathCost();
var delta = b.Position - a.Position;
if (delta.y == 0)
{
//flat hallway
pathCost.cost = Vector3Int.Distance(b.Position, endPos); //heuristic
if (grid[b.Position] == CellType.Stairs)
{
return(pathCost);
}
else if (grid[b.Position] == CellType.Room)
{
pathCost.cost += 5;
}
else if (grid[b.Position] == CellType.None)
{
pathCost.cost += 1;
}
pathCost.traversable = true;
}
else
{
//staircase
if ((grid[a.Position] != CellType.None && grid[a.Position] != CellType.Hallway) ||
(grid[b.Position] != CellType.None && grid[b.Position] != CellType.Hallway))
{
return(pathCost);
}
pathCost.cost = 100 + Vector3Int.Distance(b.Position, endPos); //base cost + heuristic
int xDir = Mathf.Clamp(delta.x, -1, 1);
int zDir = Mathf.Clamp(delta.z, -1, 1);
Vector3Int verticalOffset = new Vector3Int(0, delta.y, 0);
Vector3Int horizontalOffset = new Vector3Int(xDir, 0, zDir);
if (!grid.InBounds(a.Position + verticalOffset) ||
!grid.InBounds(a.Position + horizontalOffset) ||
!grid.InBounds(a.Position + verticalOffset + horizontalOffset))
{
return(pathCost);
}
if (grid[a.Position + horizontalOffset] != CellType.None ||
grid[a.Position + horizontalOffset * 2] != CellType.None ||
grid[a.Position + verticalOffset + horizontalOffset] != CellType.None ||
grid[a.Position + verticalOffset + horizontalOffset * 2] != CellType.None)
{
return(pathCost);
}
pathCost.traversable = true;
pathCost.isStairs = true;
}
return(pathCost);
});
if (path != null)
{
for (int i = 0; i < path.Count; i++)
{
var current = path[i];
if (grid[current] == CellType.None)
{
grid[current] = CellType.Hallway;
}
if (i > 0)
{
var prev = path[i - 1];
var delta = current - prev;
if (delta.y != 0) //y轴不同，涉及上楼或下楼的
{
int xDir = Mathf.Clamp(delta.x, -1, 1);
int zDir = Mathf.Clamp(delta.z, -1, 1);
Vector3Int verticalOffset = new Vector3Int(0, delta.y, 0);
Vector3Int horizontalOffset = new Vector3Int(xDir, 0, zDir);
grid[prev + horizontalOffset] = CellType.Stairs;
grid[prev + horizontalOffset * 2] = CellType.Stairs;
grid[prev + verticalOffset + horizontalOffset] = CellType.Stairs;
grid[prev + verticalOffset + horizontalOffset * 2] = CellType.Stairs;
Debug.LogWarning("PlaceStairs");
/*
* PlaceStairs(prev + horizontalOffset);
* PlaceStairs(prev + horizontalOffset * 2);
* PlaceStairs(prev + verticalOffset + horizontalOffset);
* PlaceStairs(prev + verticalOffset + horizontalOffset * 2);
*/
}
Debug.DrawLine(prev + new Vector3(0.5f, 0.5f, 0.5f), current + new Vector3(0.5f, 0.5f, 0.5f), Color.red, 100, false);
}
}
foreach (var pos in path)
{
if (grid[pos] == CellType.Hallway)
{
// PlaceHallway(pos);
//Debug.LogWarning("PlaceHallway");
}
}
}
}
}
public List <Vector3Int> FindPath(Vector3Int start, Vector3Int end, Func <Node, Node, PathCost> costFunction)
{
ResetNodes();
queue.Clear();
closed.Clear();
queue = new SimplePriorityQueue <Node, float>();
closed = new HashSet <Node>();
grid[start].Cost = 0;
queue.Enqueue(grid[start], 0);
while (queue.Count > 0)
{
Node node = queue.Dequeue();
closed.Add(node);
if (node.Position == end)
{
return(ReconstructPath(node));
}
foreach (var offset in neighbors)
{
if (!grid.InBounds(node.Position + offset))
{
continue;
}
var neighbor = grid[node.Position + offset];
if (closed.Contains(neighbor))
{
continue;
}
if (node.PreviousSet.Contains(neighbor.Position))
{
continue;
}
var pathCost = costFunction(node, neighbor);
if (!pathCost.traversable)
{
continue;
}
if (pathCost.isStairs)
{
int xDir = Mathf.Clamp(offset.x, -1, 1);
int zDir = Mathf.Clamp(offset.z, -1, 1);
Vector3Int verticalOffset = new Vector3Int(0, offset.y, 0);
Vector3Int horizontalOffset = new Vector3Int(xDir, 0, zDir);
if (node.PreviousSet.Contains(node.Position + horizontalOffset) ||
node.PreviousSet.Contains(node.Position + horizontalOffset * 2) ||
node.PreviousSet.Contains(node.Position + verticalOffset + horizontalOffset) ||
node.PreviousSet.Contains(node.Position + verticalOffset + horizontalOffset * 2))
{
continue;
}
}
float newCost = node.Cost + pathCost.cost;
if (newCost < neighbor.Cost)
{
neighbor.Previous = node;
neighbor.Cost = newCost;
if (queue.TryGetPriority(node, out float existingPriority))
{
queue.UpdatePriority(node, newCost);
}
else
{
queue.Enqueue(neighbor, neighbor.Cost);
}
neighbor.PreviousSet.Clear();
neighbor.PreviousSet.UnionWith(node.PreviousSet);
neighbor.PreviousSet.Add(node.Position);
if (pathCost.isStairs)
{
int xDir = Mathf.Clamp(offset.x, -1, 1);
int zDir = Mathf.Clamp(offset.z, -1, 1);
Vector3Int verticalOffset = new Vector3Int(0, offset.y, 0);
Vector3Int horizontalOffset = new Vector3Int(xDir, 0, zDir);
neighbor.PreviousSet.Add(node.Position + horizontalOffset);
neighbor.PreviousSet.Add(node.Position + horizontalOffset * 2);
neighbor.PreviousSet.Add(node.Position + verticalOffset + horizontalOffset);
neighbor.PreviousSet.Add(node.Position + verticalOffset + horizontalOffset * 2);
}
}
}
}
return(null);
}
