132 lines
3.8 KiB
C#
132 lines
3.8 KiB
C#
using System.Collections.Generic;
|
|
using Godot;
|
|
|
|
public class Pathfinding
|
|
{
|
|
private static AStar3D aStar = new AStar3D();
|
|
private static Dictionary<Vector3I, long> coordToId = new Dictionary<Vector3I, long>();
|
|
private static Dictionary<long, Vector3I> idToCoord = new Dictionary<long, Vector3I>();
|
|
private static long nextId = 1;
|
|
private static Dictionary<int, (long fromId, long toId)> verticalConnections = new Dictionary<int, (long fromId, long toId)>();
|
|
|
|
private static long GetOrCreateId(Vector3I coord)
|
|
{
|
|
if (coordToId.TryGetValue(coord, out long id))
|
|
return id;
|
|
|
|
id = nextId++;
|
|
coordToId[coord] = id;
|
|
idToCoord[id] = coord;
|
|
|
|
return id;
|
|
}
|
|
|
|
public static void BuildAStarGraph()
|
|
{
|
|
aStar.Clear();
|
|
coordToId.Clear();
|
|
idToCoord.Clear();
|
|
verticalConnections.Clear();
|
|
nextId = 1;
|
|
|
|
for (int y = 0; y < GameData.ruinSize; y++)
|
|
{
|
|
for (int x = 0; x < GameData.layerSize; x++)
|
|
{
|
|
for (int z = 0; z < GameData.layerSize; z++)
|
|
{
|
|
Vector3I coord = new Vector3I(x, y, z);
|
|
Tile tile = GameData.map[y].tiles[x, z];
|
|
|
|
if (tile == null || tile.collapsedMesh == null) continue;
|
|
|
|
long id = GetOrCreateId(coord);
|
|
|
|
aStar.AddPoint(id, tile.Position);
|
|
}
|
|
}
|
|
}
|
|
|
|
foreach (KeyValuePair<Vector3I, long> kvp in coordToId)
|
|
{
|
|
Vector3I from = kvp.Key;
|
|
long fromId = kvp.Value;
|
|
|
|
foreach (Vector3I offset in WFC.offsets3D)
|
|
{
|
|
Vector3I to = new Vector3I(
|
|
from.X + offset.X,
|
|
from.Y + offset.Y,
|
|
from.Z + offset.Z
|
|
);
|
|
|
|
if (!coordToId.ContainsKey(to)) continue;
|
|
|
|
if (!WFC.CanWalk3D(from, to)) continue;
|
|
|
|
long toId = coordToId[to];
|
|
|
|
if (from.Y != to.Y && GameData.map[from.Y].tiles[from.X, from.Z].collapsedMesh == "gate")
|
|
{
|
|
verticalConnections[from.Y] = (fromId, toId);
|
|
if (GameData.map[from.Y].isGateOpen)
|
|
{
|
|
if (!aStar.ArePointsConnected(fromId, toId))
|
|
{
|
|
aStar.ConnectPoints(fromId, toId, true);
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
|
|
if (!aStar.ArePointsConnected(fromId, toId))
|
|
{
|
|
aStar.ConnectPoints(fromId, toId);
|
|
}
|
|
}
|
|
}
|
|
|
|
for (int y = 0; y < GameData.ruinSize; y++)
|
|
{
|
|
UpdateGatePoint(y, false);
|
|
}
|
|
}
|
|
|
|
public static void UpdateGatePoint(int layer, bool isOpen)
|
|
{
|
|
if (!verticalConnections.ContainsKey(layer)) return;
|
|
|
|
(long fromId, long toId) = verticalConnections[layer];
|
|
|
|
if (isOpen)
|
|
{
|
|
if (!aStar.ArePointsConnected(fromId, toId))
|
|
{
|
|
aStar.ConnectPoints(fromId, toId, true);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (aStar.ArePointsConnected(fromId, toId))
|
|
{
|
|
aStar.DisconnectPoints(fromId, toId);
|
|
}
|
|
}
|
|
}
|
|
|
|
public static List<Vector3> GetPath(Vector3I start, Vector3I end)
|
|
{
|
|
if (!coordToId.ContainsKey(start) || !coordToId.ContainsKey(end)) return new List<Vector3>();
|
|
|
|
long startId = coordToId[start];
|
|
long endId = coordToId[end];
|
|
|
|
return new List<Vector3>(aStar.GetPointPath(startId, endId));
|
|
}
|
|
|
|
public static Vector3I GetClosestStartPoint(Vector3 robotPosition)
|
|
{
|
|
return idToCoord[aStar.GetClosestPoint(robotPosition)];
|
|
}
|
|
}
|