private static Path CalculateLowlevelPath(VoxelGraph graph, Vector3I from, List <Vector3I> to)
{
var start = graph.GetNode(from) ?? graph.GetClosestNode(from, 5);
if (start == null)
{
return(null);
}
var targets = to.Select(graph.GetNode).Where(t => t != null).ToList();
if (targets.Count == 0)
{
return(null);
}
var path = new Path(start, targets, graph.GetPathRegistry());
path.Thread = new Thread(() =>
{
path = AStar.GetPath(start, targets, path);
path.Finished = true;
path.State = PathState.Ready;
});
path.Thread.Start();
return(path);
}
public Path FollowPath(Path path, VoxelGraph graph)
{
if (path.IsT0 || path.Nodes == null)
{
return(path);
}
var lastSuper = (SuperNode)path.Nodes.Last();
var pos = 0;
var curNode = path.Start;
var visited = new List <Node>();
float length = 0;
while (!curNode.SuperNodes.ContainsKey(lastSuper))
{
if (visited.Contains(curNode))
{
throw new Exception("Loop detected");
}
visited.Add(curNode);
for (var i = 1; i >= 0; i--)
{
if (curNode.SuperNodes.ContainsKey((SuperNode)path.Nodes[pos + i]))
{
var nextStep = curNode.SuperNodes[(SuperNode)path.Nodes[pos + i]];
curNode = nextStep.To;
length += nextStep.Length - (nextStep.To != null ? (nextStep.To.SuperNodes[(SuperNode)path.Nodes[pos + i]] != null ? nextStep.To.SuperNodes[(SuperNode)path.Nodes[pos + i]].Length : 0) : 0);
pos += i;
break;
}
}
}
var finalPath = Path.Calculate(graph, curNode.Position, path.Targets.Select(t => t.Position).ToList());
finalPath.Thread.Join();
visited.AddRange(finalPath.Nodes);
var walkedPath = new Path(path.Start, path.Targets, graph.GetPathRegistry())
{
Nodes = visited,
Length = length + finalPath.Length
};
finalPath.Dispose();
return(walkedPath);
}
public HighLevelPath(Node start, List <Node> target, VoxelGraph graph) : base(start, target, graph.GetPathRegistry())
{
_graph = graph;
}