/// <summary>Creates a new PositionedNode.</summary>
public static TNode Create(int id, NavArea <TNode, TLink> parentNavArea, NavArea <TNode, TLink> otherParentNavArea, SimpleLine portalForFirstParent)
{
Point portalCenter = _LineCenter(portalForFirstParent);
var node = new TNode();
node.mID = id;
node.mParentNavArea1 = parentNavArea;
node.mParentNavArea2 = otherParentNavArea;
node.mPortalSide1 = portalForFirstParent;
node.mPortalSide1ParentAreaId = parentNavArea.ID;
node.mPortalSide2 = NavMesh <TNode, TLink> ._GetInvertedLine(portalForFirstParent);
node.mPortalSide2ParentAreaId = otherParentNavArea.ID;
node.Position.X = (float)portalCenter.X;
node.Position.Y = (float)portalCenter.Y;
//node.mActive = true;
return(node);
}
/// <summary>Goes through all Nodes and ties to find Node which rect the point is inside of.</summary>
/// <returns>Node of which rect the point is inside of, or null if point is not inside any node's rect.</returns>
public NavArea <TNode, TLink> FindNavRectFromPoint(ref Point point)
{
NavArea <TNode, TLink> containingNavArea = null;
double bestDistance = double.PositiveInfinity;
double d;
float r;
// Find the closest poly for the starting and ending point
foreach (var navArea in _NavAreas)
{
r = navArea.Polygon.BoundingRadius;
// Start
d = RUtils.Distance2D(ref navArea.Polygon.Position, ref point);
if (d <= bestDistance && d <= r && navArea.Polygon.IsPointInside((float)point.X, (float)point.Y)) // @
{
containingNavArea = navArea;
bestDistance = d;
}
}
// No matching polygons locations for the point (so no path can be found)
// = point not on nav mesh
return(containingNavArea);
}
public SimpleLine GetPortalSideFor(NavArea <TNode, TLink> navArea)
{
return(GetPortalSideFor(navArea.ID));
}
/// <summary>
/// - Create "fake" Portal Node in the "middle" of NavArea.
/// - Tell all Portal Nodes in containing NavArea to link (one way) to the new fake Node.
/// = other Portal Nodes will create links to this fake Node.
/// CleanupFakeEndNodeLinks() has to be called after fake Node is not needed anymore to remove those "fake" links.
/// </summary>
/// <param name="position"></param>
/// <param name="availablePortals"></param>
/// <param name="containingNavAreaID"></param>
/// <returns></returns>
public static TNode CreateFakeEndNode(ref Point position, List <TNode> availablePortals, NavArea <TNode, TLink> containingNavArea)
{
var fakePortalNode = _CreateFakeNode(ref position, availablePortals, -2);
float distanceBetweeNodes;
foreach (var otherPortalNode in availablePortals)
{
distanceBetweeNodes = (float)RUtils.Distance2D(ref fakePortalNode.Position, ref otherPortalNode.Position);
otherPortalNode.LinkToOneWay(
fakePortalNode,
distanceBetweeNodes, // cost
// Add real portal line here.
containingNavArea
);
}
return(fakePortalNode);
}
/// <summary></summary>
/// <param name="position">World position of this fake Node.</param>
/// <param name="availablePortals">Portals leading out of NavArea this fake Node is positioned in.</param>
/// <param name="containingNavArea">NavArea this fake Node is positioned in.</param>
public static TNode CreateFakeNodeDebug(ref Point position, List <TNode> availablePortals, bool isEndNode, NavArea <TNode, TLink> containingNavArea)
{
TNode node;
if (isEndNode)
{
node = CreateFakeEndNode(ref position, availablePortals, containingNavArea);
}
else
{
node = CreateFakeStartNode(ref position, availablePortals);
}
node.mParentNavArea1 = containingNavArea;
node.mParentNavArea2 = containingNavArea;
return(node);
}
/// <summary>Find a path from the start point to the end point using this nav mesh.</summary>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <param name="drawPolyPath">Whether or not to visualize the path through the polygons - e.g. the path that astar found.</param>
/// <param name="drawFinalPath">Whether or not to visualize the path through the path that was returned.</param>
/// <returns>An array of points and nodes if a path is found, or nulls if no path</returns>
public List <Point> FindPath(Point startPoint, Point endPoint, out List <TNode> portalNodesPath
/*, bool drawPolyPath = false, bool drawFinalPath = false*/)
{
#region --- Find the closest poly for the starting and ending point
NavArea <TNode, TLink> startArea = FindNavRectFromPoint(ref startPoint);
NavArea <TNode, TLink> endArea = FindNavRectFromPoint(ref endPoint);
// No matching polygons locations for the start or end, so no path found
// = start or end point not on nav mesh
if (startArea == null || endArea == null)
{
portalNodesPath = null;
return(null);
}
// If the start and end polygons are the same, return a direct path
if (startArea == endArea)
{
List <Point> pointsPath = new List <Point> {
startPoint, endPoint
};
//if (drawFinalPath) this.debugDrawPath(phaserPath, 0xffd900, 10);
portalNodesPath = null; // not traversing any Portal Nodes
return(pointsPath);
}
#endregion --- Find the closest poly for the starting and ending point END
#region --- A* search
// --- Search!
portalNodesPath = new List <TNode>();
GetPath(
#if DEBUG
PortalNodeBase <TLink, TNode> .CreateFakeNodeDebug(ref startPoint, startArea.Portals, false, startArea),
PortalNodeBase <TLink, TNode> .CreateFakeNodeDebug(ref endPoint, endArea.Portals, true, endArea),
#else
PortalNodeBase <TLink, TNode> .CreateFakeStartNode(ref startPoint, startArea.Portals),
PortalNodeBase <TLink, TNode> .CreateFakeEndNode(ref endPoint, endArea.Portals, endArea),
#endif
portalNodesPath
);
if (portalNodesPath.Count == 0)
{
// While the start and end polygons may be valid, no path between them
portalNodesPath = null;
return(null);
}
#endregion --- A* search END
#region --- Funnel algorithm
// We have a path, so now time for the funnel algorithm
#if o
D.WriteLine("======== Path search ========");
D.WriteLine(" --- Path ---");
foreach (var pathNode in portalNodesPath)
{
D.WriteLine(" " + pathNode.ID);
}
D.WriteLine(" --- Channel ---");
#endif
Channel channel = new Channel();
channel.Add(startPoint);
SimpleLine portal;
int countLimit = portalNodesPath.Count - 1;
TNode pathPortalNode;
TNode nextPathPortalNode;
for (int i = 1; i < countLimit; i++) // skipping first Node - I don't need it's Links - I don't need their Portals
{
pathPortalNode = portalNodesPath[i];
nextPathPortalNode = portalNodesPath[i + 1];
#if o
D.WriteLine($" pathPortalNode: {pathPortalNode.ID} and pathPortalNode: {nextPathPortalNode.ID}");
#endif
// Find the portal
portal = null;
foreach (var link in pathPortalNode.Links)
{
//if ( link.NodeLinkingTo.ReferenceEquals(nextPathPortalNode) )
if (link.NodeLinkingTo.ID == nextPathPortalNode.ID)
{
#if o
D.WriteLine($" link to {link.NodeLinkingTo.ID} - link found and portal added");
#endif
portal = link.Portal;
// Push the portal vertices into the channel
channel.Add(portal.Start, portal.End);
break;
}
#if o
else
{
D.WriteLine($" link to {link.NodeLinkingTo.ID} - link NOT found !");
}
#endif
}
}
channel.Add(endPoint);
// Pull a string along the channel to run the funnel
channel.StringPull();
PortalNodeBase <TLink, TNode> .CleanupFakeEndNodeLinks(endArea.Portals);
// Clone path, excluding duplicates - needed ? @
Point? lastPoint = null;
List <Point> finalPointsPath = new List <Point>();
foreach (var point in channel.Path)
{
//var newPoint = p.clone();
var newPoint = point;
//if (!lastPoint || !newPoint.equals(lastPoint))
if (lastPoint.HasValue == false || newPoint != lastPoint)
{
finalPointsPath.Add(newPoint);
}
lastPoint = newPoint;
}
return(finalPointsPath);
#endregion --- Funnel algorithm END
}
