public List <AStarNode> CalculatePath(DeterministicVector2 a, DeterministicVector2 b)
{
AStarContext context = new AStarContext()
{
A = a,
B = b
};
var result = constrainedEdgePolygons.CalculateAnyIntersection(new NavigationEdge(context.A, context.B));
if (!result.SegmentsIntersect)
{
return(new List <AStarNode>()
{
new AStarNode(a, b),
new AStarNode(b, b),
});
}
List <AStarNode> path = CalculateNaivePath(context);
if (path == null)
{
return(null);
}
path.Add(new AStarNode(b, b));
path.Insert(0, new AStarNode(a, b));
var resultPath = new List <AStarNode>(path);
OptimizePath(resultPath);
return(resultPath);
}
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);
}
private List <AStarNode> BacktrackFinishPath(AStarNode end, AStarContext context)
{
List <AStarNode> result = new List <AStarNode>()
{
end
};
while (result[0].Parent != null)
{
result.Insert(0, result[0].Parent);
}
AddFunnelInformation(result);
pathCache[context.ATri].Add(context.BTri, new List <AStarNode>(result));
var reversed = new List <AStarNode>(result);
reversed.Reverse();
pathCache[context.BTri].Add(context.ATri, reversed);
return(result);
}
private void AddPointToQueue(DeterministicVector2 trianglePoint, AStarNode parent, AStarContext context)
{
if (context.AllNodes.ContainsKey(trianglePoint))
{
bool readdNeeded = context.NodeQueue.Remove(context.AllNodes[trianglePoint]);
context.AllNodes[trianglePoint].Parent = parent;
if (readdNeeded)
{
context.NodeQueue.Add(context.AllNodes[trianglePoint]);
}
return;
}
var newNode = new AStarNode(trianglePoint, context.B, parent);
context.AllNodes.Add(trianglePoint, newNode);
context.NodeQueue.Add(newNode);
}
private void AddTriangleToQueue(NavigationTriangle triangle, AStarNode parent, AStarContext context)
{
AddPointToQueue(triangle.U, parent, context);
AddPointToQueue(triangle.V, parent, context);
AddPointToQueue(triangle.W, parent, context);
AddPointToQueue(triangle.S0.Midpoint(), parent, context);
AddPointToQueue(triangle.S1.Midpoint(), parent, context);
AddPointToQueue(triangle.S2.Midpoint(), parent, context);
}
