private void _CalculateNeighbors()
{
#if o
D.WriteLine(" * NavMesh._CalculateNeighbors()");
#endif
// Fill out the neighbor information for each NavArea
// Find and create Portals (Portal Nodes) for each NavArea
#region
NavArea <TNode, TLink> navArea;
NavArea <TNode, TLink> otherNavArea;
AxisAlignedRectangle navAreaPolygon;
AxisAlignedRectangle otherNavAreaPolygon;
TNode portalNode;
SimpleLine portal;
int portalId = 1;
for (int i = 0; i < _NavAreas.Count; i++)
{
navArea = _NavAreas[i];
navAreaPolygon = navArea.Polygon;
#if o
D.WriteLine(" navArea: " + navAreaPolygon.Name);
//(navArea as PositionedNode).CheckedAsMain = true;
#endif
for (int j = i + 1; j < _NavAreas.Count; j++)
{
otherNavArea = _NavAreas[j];
otherNavAreaPolygon = otherNavArea.Polygon;
#if o
D.WriteLine(" otherNavPoly: " + otherNavAreaPolygon.Name);
#endif
#region - Check polygons distance
// Check if the other navpoly is within range to touch
// Distance between centers
var distanceBetweenCenters = RUtils.Distance2D(ref navAreaPolygon.Position, ref otherNavAreaPolygon.Position);
// If Distance between centers is bigger than combined radii, they are not in range
// If Distance between centers is smaller or equal, they are in range (not necessarily touching)
if (distanceBetweenCenters >= navAreaPolygon.BoundingRadius + otherNavAreaPolygon.BoundingRadius)
{
// Not in range => proceed to another navpoly
#if o
D.WriteLine($" Not in range (distanceBetweenCenters: {distanceBetweenCenters} totalRadii: {navAreaPolygon.BoundingRadius + otherNavAreaPolygon.BoundingRadius})");
#endif
continue;
}
#if o
D.WriteLine($" In range (distanceBetweenCenters: {distanceBetweenCenters} totalRadii: {navAreaPolygon.BoundingRadius + otherNavAreaPolygon.BoundingRadius})"); \
#endif
#endregion - Check polygons distance END
// The are in range, so check each edge pairing
// to find shared edge and shared part of edges = portal
#region - Using common sense
// Here I know they do overlap
// Calculate the portal between the two polygons
// - THIS NEEDS TO BE IN CLOCKWISE ORDER, RELATIVE TO EACH POLYGON
// _GetSegmentOverlap()
// returns horizontal: always left-to-right
// returns vertical: always bottom-to-top
portal = null;
// other is above
if (navAreaPolygon.Top == otherNavAreaPolygon.Bottom)
{
#if o
D.WriteLine($" Other above -- Touching this Top ({navAreaPolygon.Top}) - other Bottom ({otherNavAreaPolygon.Bottom})");
#endif
portal = _GetSegmentOverlap(navArea.EdgeTop, otherNavArea.EdgeBottom, true, false);
#if o
// Debug visuals
if (portal != null)
{
//_DrawDebugVisualForPortal(portal.Start.X + 3f, portal.Start.Y - 3f, portal.End.X - 3f, portal.End.Y - 3f);
Debug.ShowLine(portal, Color.Yellow);
}
#endif
}
// other is below
else if (navAreaPolygon.Bottom == otherNavAreaPolygon.Top)
{
#if o
D.WriteLine($" Other below -- Touching this Bottom ({navAreaPolygon.Bottom}) - other Top ({otherNavAreaPolygon.Top})");
#endif
portal = _GetSegmentOverlap(navArea.EdgeBottom, otherNavArea.EdgeTop, true, true);
#if o
// Debug visuals
if (portal != null)
{
//_DrawDebugVisualForPortal(portal.Start.X - 3f, portal.Start.Y + 3f, portal.End.X + 3f, portal.End.Y + 3f);
Debug.ShowLine(portal, Color.Yellow);
}
#endif
}
// other is to left
else if (navAreaPolygon.Left == otherNavAreaPolygon.Right)
{
#if o
D.WriteLine($" Other to left -- Touching this Left ({navAreaPolygon.Left}) - other Right ({otherNavAreaPolygon.Right})");
#endif
portal = _GetSegmentOverlap(navArea.EdgeLeft, otherNavArea.EdgeRight, false, false);
#if o
// Debug visuals
if (portal != null)
{
//_DrawDebugVisualForPortal(portal.Start.X + 3f, portal.Start.Y + 3f, portal.End.X + 3f, portal.End.Y - 3f);
Debug.ShowLine(portal, Color.Yellow);
}
#endif
}
// other is to right
else if (navAreaPolygon.Right == otherNavAreaPolygon.Left)
{
#if o
D.WriteLine($" Other to right -- Touching this Right ({navAreaPolygon.Right}) - other Left ({otherNavAreaPolygon.Left})");
#endif
portal = _GetSegmentOverlap(navArea.EdgeRight, otherNavArea.EdgeLeft, false, true);
#if o
// Debug visuals
if (portal != null)
{
//_DrawDebugVisualForPortal(portal.Start.X - 3f, portal.Start.Y - 3f, portal.End.X - 3f, portal.End.Y + 3f);
Debug.ShowLine(portal, Color.Yellow);
}
#endif
}
// not touching
else
{
// Not actually touching => proceed to another navpoly
#if o
D.WriteLine($" Not Touching");
#endif
continue;
}
#endregion - Using common sense END
if (portal != null) // this check IS needed
{
// Found portal between 2 NavAreas
// - have to add portal Node
portalNode = PortalNodeBase <TLink, TNode> .Create(portalId, navArea, otherNavArea, portal);
portalId++;
//
// - have to link this portal node to all other Portal Nodes in both this and other NavArea
// BUT at this point I dont know all Portal Nodes in this or the other NavArea
// => store both portal lines (sides) in this Portal Node
// => store both NavAreas in this Portal Node
// => do the linking at the end of NavMesh creation
//
// - assign Portal Node to this NavArea
navArea.Portals.Add(portalNode);
//
// - assign Portal Node to other NavArea
otherNavArea.Portals.Add(portalNode);
//
//navArea.LinkTo(otherNavArea, portal);
_PortalNodes.Add(portalNode);
#if o
#region -- Debug / visuals
Debug.ShowText(ref portalNode.Position, portalNode.ID.ToString());
/*Debug.ShowLine(portal, Color.Yellow);
* var circle = ShapeManager.AddCircle();
* circle.Radius = 6f;
* circle.Color = Color.Yellow;
* circle.X = (float)portal.Start.X;
* circle.Y = (float)portal.Start.Y;
* Debug.ShowLine(navAreaPolygon.Position, otherNavAreaPolygon.Position, Debug.Gray32);*/
#endregion -- Debug / visuals END
#endif
}
#if o
else
{
D.WriteLine($" Not Touching");
}
#endif
} // for other j
} // for one i
#endregion
// Link Portal Nodes together inside each NavArea
#region
navArea = null;
portalNode = null;
#if o
TNode otherPortalNode = null;
//D.WriteLine("========== Portals' Linking ==========");
#endif
for (int i = 0; i < _NavAreas.Count; i++)
{
navArea = _NavAreas[i];
#if o
//D.WriteLine("NavArea Portals: " + navArea.Portals.Count + " " + navArea.Polygon.Name);
#endif
if (navArea.Portals.Count > 1)
{
for (int iPortalNode = 0; iPortalNode < navArea.Portals.Count; iPortalNode++)
{
portalNode = navArea.Portals[iPortalNode]; // as TNode;
#if o
//D.WriteLine(" PortalNode " + portalNode.ID);
#endif
for (int iOtherPortalNode = iPortalNode + 1; iOtherPortalNode < navArea.Portals.Count; iOtherPortalNode++)
{
#if o
otherPortalNode = navArea.Portals[iOtherPortalNode]; // as TNode;
//D.WriteLine(" Links to " + otherPortalNode.ID);
portalNode.LinkTo(otherPortalNode, navArea);
#else
portalNode.LinkTo(navArea.Portals[iOtherPortalNode], navArea);
#endif
}
}
}
}
#if o
//D.WriteLine("====================================\n");
#endif
#endregion
#region -- Debug / visuals for links
#if o
//D.WriteLine("========== Portals' Links ==========");
Xna.Vector3 linkLineShift = new Xna.Vector3(2f, 2f, 0f);
foreach (var navAreaa in NavAreas)
{
//D.WriteLine("NavArea " + navAreaa.Polygon.Name);
foreach (var portalNodee in navAreaa.Portals)
{
//D.WriteLine(" PortalNode " + portalNodee.ID);
foreach (var link in portalNodee.Links)
{
Debug.ShowLine(portalNodee.Position + linkLineShift, link.NodeLinkingTo.Position + linkLineShift, Debug.Gray32);
//D.WriteLine(" Links to " + link.NodeLinkingTo.ID);
}
}
}
//D.WriteLine("====================================");
/*// Debug
* var sb = new StringBuilder("--------------\n");
* PositionedNode cNode;
* foreach (var tNode in _NavPolygons)
* {
* cNode = tNode as PositionedNode;
* sb.Append(tNode.Polygon.Name).Append(" ").Append(cNode.CheckedAsMain).Append(" ").Append(cNode.CheckedAsOther)
* .Append(" ").Append(tNode.Links.Count)
* .AppendLine();
* }
* sb.AppendLine("--------------");
* D.WriteLine(sb.ToString());*/
#endif
#endregion -- Debug visuals END
}
/// <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
}