public List <AStarNode> CalculateNaivePath(AStarContext context)
{
context.ATri = navMesh.SearchTriangleForPoint(context.A);
context.BTri = navMesh.SearchTriangleForPoint(context.B);
if (context.ATri == null || context.BTri == null)
{
return(null);
}
if (!pathCache.ContainsKey(context.ATri))
{
pathCache.Add(context.ATri, new Dictionary <NavigationTriangle, List <AStarNode> >());
}
if (!pathCache.ContainsKey(context.BTri))
{
pathCache.Add(context.BTri, new Dictionary <NavigationTriangle, List <AStarNode> >());
}
if (context.ATri == null || context.BTri == null)
{
// TODO: Search nearest on NavMesh just make a ray hit to vs all Constrainted edges
return(null);
}
if (context.ATri == context.BTri)
{
return(new List <AStarNode>()
{
});
}
if (pathCache[context.ATri].ContainsKey(context.BTri))
{
return(new List <AStarNode>(pathCache[context.ATri][context.BTri]));
}
AddTriangleToQueue(context.ATri, null, context);
while (context.NodeQueue.Count > 0)
{
var node = context.NodeQueue.GetNext();
var triangles = this.navMesh.GetTrianglesWithPoint(node.Position);
if (triangles.Contains(context.BTri))
{
var path = new List <AStarNode>(BacktrackFinishPath(node, context));
return(path);
}
foreach (var triangle in triangles)
{
AddTriangleToQueue(triangle, node, context);
}
}
return(null);
}
