public void DrawPolygon(Polygon polygon)
{
List<MeshNode> cornerNodes = polygon.GetRectCorners();
List<Vector2> cornerVectors = new List<Vector2>();
for (int i = 0; i < 4; i++)
{
cornerVectors.Add(ToScreenCoords(cornerNodes[i].mVector));
}
for (int i = 0; i <= 1; i++)
{
CodeTest.DrawLine(cornerVectors[i], cornerVectors[i + 2], 1, Color.Purple);
}
foreach (Edge e in polygon.GetEdges())
{
if (e.GetContrahent() == null)
DrawSingleEdge(e);
else
DrawEdge(e);
}
foreach (MeshNode node in polygon.GetNodes())
{
DrawNode(node);
}
}
/// <summary>
/// Adds a polygon to the mesh.
/// </summary>
public void AddPolygon(Polygon toAdd)
{
Debug.Assert(toAdd.GetEdges() != null);
int[] indexes = GetCoveredSectorIndexes(toAdd);
for (int y = indexes[1]; y <= indexes[3]; y++)
for (int x = indexes[0]; x <= indexes[2]; x++ )
{
Debug.Assert(!mSectors[x, y].Contains(toAdd));
mSectors[x,y].Add(toAdd);
}
}
/// <summary>
/// Checks if given polygons form a concave union.
/// The polygons must be neighboured and rectangular.
/// </summary>
private bool AreConcaveRectSpeedUp(Polygon p1, Polygon p2)
{
// we already assume they are neighboured
// ReSharper disable CompareOfFloatsByEqualityOperator
// Since we assume they are neighboured, they will have to share the same
// MeshNodes at their common border. Therefore, the value, coming from the same
// instance field, must be exactly equal.
if ((p1.Xmin == p2.Xmin && p1.Xmax == p2.Xmax)
|| (p1.Ymin == p2.Ymin && p1.Ymax == p2.Ymax))
// ReSharper restore CompareOfFloatsByEqualityOperator
return true;
return false;
}
/// <summary>
/// Unites two given polygons into one and returns it. The two polygons
/// are deleted. Optionally applies a cleanup to the new polygon.
/// </summary>
/// <param name="unionEdges">the edges of the new united polygon</param>
/// <param name="polygon1">the first of the polygons that are to be united</param>
/// <param name="polygon2">the second of the polygons that are to be united</param>
/// <param name="allowCleanUp">if the edges of the united polygon should
/// be cleaned up</param>
/// <returns>the new united polygon</returns>
private Polygon ApplyPolygonUnion(List<Edge> unionEdges, Polygon polygon1, Polygon polygon2, bool allowCleanUp)
{
Debug.Assert(! polygon1.CanBeCleanedUp(), "Cannot unite with a dirty polygon!");
Debug.Assert(!polygon2.CanBeCleanedUp(), "Cannot unite with a dirty polygon!");
Polygon unionPolygon = new Polygon(unionEdges);
unionPolygon.ImposeOwnership();
polygon1.Delete(true);
RemovePolygon(polygon1);
polygon2.Delete(true);
RemovePolygon(polygon2);
AddPolygon(unionPolygon);
if (allowCleanUp) unionPolygon.CleanUp(true);
return unionPolygon;
}
/// <summary>
/// Returns a new mesh instance. This instance has its polygons auto-generated according to
/// the given set "blocked" of impassable (int,int)-points. The largest possibly passable area will be
/// a rectangle from (0,0) to (width,height).
/// </summary>
/// <param name="blocked">contains all integer points that are impassable</param>
/// <param name="remainingPolygons">The polygons that still exist within area</param>
/// <param name="coveredByPolygonBefore">the nodes that are covered by remainingPolygons</param>
/// <param name="scale">the mesh scale</param>
/// <param name="area">the area that should be filled with new polygons</param>
/// <param name="createdNodes">the still existing nodes that must be integrated</param>
/// <param name="unite">if everything in the area should be united at last</param>
/// <returns>a new instance of Navigation Mesh that contains all freshly created polygons (but not remainingPolygons)</returns>
public static NavigationMesh CreateInstanceFromBlockmap(PointSet<int> blocked, List<Polygon> remainingPolygons, PointSet<int> coveredByPolygonBefore, int scale, Rectangle area, List<MeshNode> createdNodes, bool unite = true)
{
GlobalValues globalValues = GlobalValues.GetInstance();
PointSet<int> coveredByPremarkedPolygon = new PointSet<int>();
if (globalValues.mShowMeshCreation)
{
CodeTest.mRects2ToDraw.Add(area);
//CodeTest.mSetsToDraw.Add(coveredByPolygonBefore); // slows drawing down for big meshes
//CodeTest.mSetsToDraw.Add(coveredByPremarkedPolygon);
//CodeTest.mSetsToDraw.Add(blocked);
}
Debug.Assert(area.Width > 0 && area.Height > 0 && area.X >= 0 && area.Y >= 0);
int areaWidth = (area.Width / scale);
int areaHeight = (area.Height / scale);
int areaOffsetX = (area.X / scale);
int areaOffsetY = (area.Y / scale);
PointSet<int> nodeToCreate = new PointSet<int>();
PointSet<int> nodeCreated = new PointSet<int>();
foreach (MeshNode node in createdNodes)
{
int x = ((int)(node.mVector.X)) / scale;
int y = ((int)(node.mVector.Y)) / scale;
nodeToCreate.Add(x, y);
nodeCreated.Add(x, y);
}
#if DEBUG
if (areaWidth < 20 && areaHeight < 20)
{
Debug.WriteLine("blocked (part):");
NavigationMesh.PrintPointSet(blocked, areaOffsetX, areaOffsetY, areaWidth + 1, areaHeight + 1);
}
#endif
List<Rectangle> premarkedPolygons = new List<Rectangle>();
// a point(a,b) in coveredByPolygon indicates that there is already
// a registered polygon which covers at least the square (a,b)-(a+1,b+1)
for (int y = areaOffsetY; y < areaOffsetY + areaHeight; y++)
{
for (int x = areaOffsetX; x < areaOffsetX + areaWidth; x++)
{
// we wanna create a new rectangular polygon(x,y,*,*)
// as big as possible.
if (!coveredByPolygonBefore.Contains(x, y) &&
!coveredByPremarkedPolygon.Contains(x, y) &&
!blocked.Contains(x, y) &&
!blocked.Contains(x + 1, y) &&
!blocked.Contains(x, y + 1) &&
!blocked.Contains(x + 1, y + 1))
{
int polyWidth = 1;
int polyHeight = 1;
while (x + polyWidth <= areaOffsetX + areaWidth
&& HorizontalFree(blocked, x, y, polyWidth, polyHeight)
&& HorizontalFree(coveredByPolygonBefore, x, y, polyWidth - 1, polyHeight - 1)
&& HorizontalFree(coveredByPremarkedPolygon, x, y, polyWidth - 1, polyHeight - 1)
&& y + polyHeight <= areaOffsetY + areaHeight
&& VerticalFree(blocked, x, y, polyWidth, polyHeight)
&& VerticalFree(coveredByPolygonBefore, x, y, polyWidth - 1, polyHeight - 1)
&& VerticalFree(coveredByPremarkedPolygon, x, y, polyWidth - 1, polyHeight - 1))
{
if (globalValues.mShowMeshCreation)
{
Rectangle rectToDraw = new Rectangle(scale * x, scale * y, scale * polyWidth, scale * polyHeight);
CodeTest.mRectsToDraw.Add(rectToDraw);
CodeTest.DrawNow();
CodeTest.mRectsToDraw.Remove(rectToDraw);
}
polyWidth++;
polyHeight++;
}
polyWidth--;
polyHeight--;
Debug.Assert(polyWidth > 0 && polyHeight > 0 && polyWidth == polyHeight);
// We won't create the polygon now because there is a little
// catch (see below), but we are keeping it in mind.
// => fill premarkedPolygons now...
if (globalValues.mShowMeshCreation)
{
Rectangle premarkedFullScaled = new Rectangle(scale * x, scale * y, scale * polyWidth, scale * polyHeight);
CodeTest.mRectsToDraw.Add(premarkedFullScaled);
}
Rectangle premarked = new Rectangle(x, y, polyWidth, polyHeight);
premarkedPolygons.Add(premarked);
// ...and mark nodes as used:
nodeToCreate.Add(x, y);
nodeToCreate.Add(x + polyWidth, y);
nodeToCreate.Add(x + polyWidth, y + polyHeight);
nodeToCreate.Add(x, y + polyHeight);
// register in coveredByPolygon:
for (int i = x; i < x + polyWidth; i++)
{
for (int j = y; j < y + polyHeight; j++)
{
coveredByPremarkedPolygon.Add(i, j);
}
}
}
}
}
Debug.WriteLine("premarked polygons:" + premarkedPolygons.Count);
if (globalValues.mShowMeshCreation)
{
for (int i = 0; i < premarkedPolygons.Count; i++)
CodeTest.mRectsToDraw.RemoveAt(CodeTest.mRectsToDraw.Count - 1);
}
// Now that we have full knowledge about ALL nodes to create, we can
// create the appropriate polygons. The catch is that sometimes it doesn't
// suffice to create the polygon between its corners, but there are polygons
// with more vertices in between.
int mapWidth = GameLogic.GetInstance().GetMap().GetWidth();
int mapHeight = GameLogic.GetInstance().GetMap().GetHeight();
NavigationMesh result = new NavigationMesh(mapWidth, mapHeight);
if (globalValues.mShowMeshCreation) CodeTest.mMeshesToDraw.Add(result);
// Create each premarked polygon:
foreach (Rectangle rectangle in premarkedPolygons)
{
List<Point> vertexList = GetVertexList(rectangle, nodeToCreate);
Debug.Assert(vertexList.Count >= 4);
List<MeshNode> nodes = new List<MeshNode>();
// check which nodes are already created and which need to be created:
foreach (Point point in vertexList)
{
MeshNode node = RetrieveOrCreateNode(point, scale, nodeCreated, createdNodes, remainingPolygons, coveredByPolygonBefore);
nodes.Add(node);
}
// now we can chain up our nodes:
Polygon polygon = new Polygon(nodes);
polygon.ImposeOwnership();
// add polygon to our mesh:
// NOTE: WE MAY NOT ALLOW POLYGON CLEAN UP DURING MESH CONSTRUCTION!!!!
// Damn error, I've found you at last...
result.AddPolygon(polygon);
if (globalValues.mShowMeshCreation) CodeTest.DrawNow();
}
// Now unite and clean everything: (At least I think we are allowed now...)
if (unite) result.WholeUnionRun(true, null);
if (globalValues.mShowMeshCreation)
{
CodeTest.mMeshesToDraw.Remove(result);
CodeTest.mRects2ToDraw.Remove(area);
//CodeTest.mSetsToDraw.Remove(coveredByPolygonBefore); // again, taken out for being slow
//CodeTest.mSetsToDraw.Remove(coveredByPremarkedPolygon);
//CodeTest.mSetsToDraw.Remove(blocked);
}
return result;
}
/// <summary>
/// For code testing purpose:
/// Returns a new special-shaped mesh instance.
/// </summary>
public static NavigationMesh[] GetTestInstanceForCleanUp()
{
NavigationMesh[] result = new NavigationMesh[3];
for (int x = 0; x < 3; x++)
{
MeshNode a = new MeshNode(0, 0);
MeshNode b = new MeshNode(0, 10);
MeshNode c = new MeshNode(5, 10);
MeshNode d = new MeshNode(5, 0);
MeshNode e = new MeshNode(0, 15);
MeshNode f = new MeshNode(10, 15);
MeshNode g = new MeshNode(10, 10);
MeshNode h = new MeshNode(15, 15);
MeshNode i = new MeshNode(15, 0);
MeshNode j = new MeshNode(10, 0);
Edge ab = new Edge(a, b);
ab.ImposeOwnership();
Edge bc = new Edge(b, c);
bc.ImposeOwnership();
Edge cd = new Edge(c, d);
cd.ImposeOwnership();
Edge da = new Edge(d, a);
da.ImposeOwnership();
Edge be = new Edge(b, e); be.ImposeOwnership();
Edge ef = new Edge(e, f); ef.ImposeOwnership();
Edge fg = new Edge(f, g); fg.ImposeOwnership();
Edge gc = new Edge(g, c); gc.ImposeOwnership();
Edge cb = new Edge(c, b); cb.ImposeOwnership();
Edge fh = new Edge(f, h); fh.ImposeOwnership();
Edge hi = new Edge(h, i); hi.ImposeOwnership();
Edge ij = new Edge(i, j); ij.ImposeOwnership();
Edge jg = new Edge(j, g); jg.ImposeOwnership();
Edge gf = new Edge(g, f); gf.ImposeOwnership();
Polygon pA = new Polygon(new List<Edge> { ab, bc, cd, da });
pA.ImposeOwnership();
Polygon pB = new Polygon(new List<Edge> { be, ef, fg, gc, cb });
pB.ImposeOwnership();
Polygon pC = new Polygon(new List<Edge> { fh, hi, ij, jg, gf });
pC.ImposeOwnership();
result[x] = new NavigationMesh(15,15);
result[x].AddPolygon(pA);
result[x].AddPolygon(pB);
result[x].AddPolygon(pC);
}
return result;
}
/// <summary>
/// Takes the result from PathFinder.CalculatePath() and returns a cleaned
/// up path from it.
/// </summary>
public static Path GetCleanedUpVectorPath(AStarNode target, bool startPassable, Polygon endPolygon)
{
// First get the list of meshNodes:
List<MeshNode> meshNodes = new List<MeshNode>();
while (target != null)
{
meshNodes.Add(target.mNode);
target = target.mPrecedent;
}
// If we don't need to reach the start point, we just remove the most recent point:
if (!startPassable) meshNodes.RemoveAt(meshNodes.Count - 1);
meshNodes.Reverse();
// We're ready to kick ass!
if (meshNodes.Count > 2)
{ // We can only clean up with 3 or more nodes
int uniteFromIndex = 0;
while (uniteFromIndex < meshNodes.Count - 2)
{
Debug.Assert(uniteFromIndex < meshNodes.Count - 2);
int i = uniteFromIndex + 2;
Debug.Assert(i < meshNodes.Count);
Stack<Edge> firstCandidateEdges = new Stack<Edge>(meshNodes[uniteFromIndex].GetPolyOwnersEdges());
Debug.Assert(firstCandidateEdges.Count > 0,
"GetCleanedUpVectorPath(): Could not find first candidate edges!");
Debug.Assert(uniteFromIndex < i - 1);
Debug.Assert(uniteFromIndex < meshNodes.Count);
Debug.Assert(i < meshNodes.Count);
while (i < meshNodes.Count && IsCleanUpPossible(GetCleanUpEndPolygons(i, meshNodes, endPolygon),
firstCandidateEdges,
meshNodes[uniteFromIndex].mVector,
meshNodes[i].mVector))
{
i++;
}
// Whatever case has broken the loop, we try to
// clean up from uniteFromIndex to i-1 if feasible:
i--;
if (uniteFromIndex < i - 1)
{
int removedNodeCount = i - uniteFromIndex - 1;
Debug.Assert(removedNodeCount > 0);
CleanUp(uniteFromIndex, i, meshNodes);
}
uniteFromIndex++;
}
// uniteFromIndex is too high to clean up anymore
}
// We're done:
Stack<Vector2> stack = new Stack<Vector2>();
// fill path backwards (it's a stack after all):
for (int j = meshNodes.Count - 1; j >= 0; j--)
{
stack.Push(meshNodes[j].mVector);
}
return new Path(stack);
}
/// <summary>
/// Checks the end condition of our AStar algorithm.
/// </summary>
private bool EndConditionMet(AStarNode current, bool endPassable, Polygon endPolygon, List<MeshNode> endNodes)
{
if (endPassable)
{ // use endPolygon to determine termination
// we're done if the current node is part of the endPolygon
return (current.mNode.mPolyOwners.Contains(endPolygon));
}
/*else*/
{ // use endNodes to determine termination
// we're done if the current node is part of endNodes:
return (endNodes.Contains(current.mNode));
}
}
/// <summary>
/// Returns a new polygon instance that is the union of this
/// and the given polygon. Union is at the first common edge found.
/// Returns null if no common edge found.
/// </summary>
/// <param name="polygon2">the polygon with which to unite</param>
/// <param name="commonEdge">returns this polygon's edge that reversely equals
/// commonEdge2 from polygon2</param>
/// <param name="commonEdge2">returns polygon2's edge that reversely equals
/// commonEdge from this polygon</param>
/// <returns>united polygon if possible, else null</returns>
public List<Edge> GetUnion(Polygon polygon2, out Edge commonEdge, out Edge commonEdge2)
{
List<Edge> commonEdges = GetFirstIncidentEdge(polygon2);
if (commonEdges == null)
{
commonEdge = null;
commonEdge2 = null;
return null;
}
commonEdge = commonEdges[0];
int commonEdgeIndex = mEdges.IndexOf(commonEdge);
commonEdge2 = commonEdges[1];
List<Edge> edges2 = polygon2.GetEdges();
int edge2Index = edges2.IndexOf(commonEdge2);
List<Edge> newEdges = new List<Edge>();
// copy from mEdges up to the index of edge:
int length = mEdges.Count;
for (int i = 0; i < commonEdgeIndex; i++)
{
Debug.Assert(mEdges[i].mEndNode == mEdges[i+1].mStartNode);
newEdges.Add(mEdges[i]);
}
// copy along edges2 from and to edge2Index
int length2 = edges2.Count;
int j = (edge2Index + 1) % length2;
if (commonEdgeIndex > 0) Debug.Assert(mEdges[commonEdgeIndex-1].mEndNode == edges2[j].mStartNode);
while (edges2[j] != commonEdge2)
{
Debug.Assert(edges2[j].mEndNode == edges2[(j+1)%length2].mStartNode);
newEdges.Add(edges2[j]);
j = (j + 1) % length2;
}
Debug.Assert(j == edge2Index);
j--;
if (j < 0) j = j + length2;
if (commonEdgeIndex+1<length) Debug.Assert(edges2[j].mEndNode == mEdges[commonEdgeIndex + 1].mStartNode);
// now we're again in mEdges of the first polygon, copy the rest:
for (int h = commonEdgeIndex + 1; h < length; h++)
{
Debug.Assert(mEdges[h].mEndNode == mEdges[(h+1)%length].mStartNode);
newEdges.Add(mEdges[h]);
}
Debug.Assert(AllEdgesIncident(newEdges));
// newEdges finished
return newEdges;
}
public int CountIncidentEdges(Polygon polygon2)
{
int counter = 0;
List<Edge> edges2 = polygon2.GetEdges();
foreach (Edge edge in mEdges)
{
foreach (Edge edge2 in edges2)
{
if (edge.RecursiveReverse(edge2))
{
counter++;
}
}
}
return counter;
}
private static void PolygonTests()
{
double pi = Math.PI;
List<MeshNode> nodes = new List<MeshNode> { new MeshNode(0, 0), new MeshNode(10, 0), new MeshNode(10, 10), new MeshNode(0, 10) };
Polygon polygon = new Polygon(nodes);
polygon.ImposeOwnership();
List<Edge> edges = polygon.GetEdges();
Debug.Assert(HelperMethods.Circa(
HelperMethods.GetAngleDifference(edges[0].ToVector(), edges[1].ToVector()), pi / 2));
Debug.Assert(Polygon.IsConcave(polygon.GetEdges()), "CodeTest.PolygonTests(): Polygon not concave!");
Debug.Assert(polygon.PointInPolygon(new Vector2(10, 5), false), "CodeTest.PolygonTests(): 10,5 not in Polygon!");
Random r = new Random();
Vector2 point1 = new Vector2((float)(10 * r.NextDouble()), (float)(10 * r.NextDouble()));
Debug.Assert(polygon.PointInPolygon(point1, false), "CodeTest.PolygonTests(): Random point not in Polygon!");
Vector2 p1 = new Vector2(0, 0);
Vector2 p2 = new Vector2(0, 10);
Vector2 p3 = new Vector2(10, 0);
Vector2 p4 = new Vector2(10, 10);
Debug.Assert(polygon.PointInPolygon(p1, false), "PointInPolygon Test failed!");
Debug.Assert(polygon.PointInPolygon(p2, false), "PointInPolygon Test failed!");
Debug.Assert(polygon.PointInPolygon(p3, false), "PointInPolygon Test failed!");
Debug.Assert(polygon.PointInPolygon(p4, false), "PointInPolygon Test failed!");
List<MeshNode> nodes2 = new List<MeshNode> { nodes[1], new MeshNode(20, 0), new MeshNode(20, 10), nodes[2] };
Polygon polygon2 = new Polygon(nodes2);
polygon2.ImposeOwnership();
List<Polygon> polygonNeighbours = polygon.GetAdjacentPolygons();
List<Polygon> polygon2Neighbours = polygon2.GetAdjacentPolygons();
Debug.Assert(polygonNeighbours.Count == 1 && polygonNeighbours.Contains(polygon2));
Debug.Assert(polygon2Neighbours.Count == 1 && polygon2Neighbours.Contains(polygon));
Edge commonEdge, commonEdge2;
List<Edge> unitedEdges = polygon.GetUnion(polygon2, out commonEdge, out commonEdge2);
Polygon unitedPolygon = new Polygon(unitedEdges);
unitedPolygon.ImposeOwnership();
polygon.Delete(true);
polygon2.Delete(true);
unitedPolygon.CleanUp(true);
Debug.Assert(commonEdge.mStartNode == null);
Debug.Assert(commonEdge.mEndNode == null);
Debug.Assert(commonEdge2.mStartNode == null);
Debug.Assert(commonEdge2.mEndNode == null);
Debug.Assert(unitedPolygon.EdgesHaveOneOwner());
Debug.Assert(unitedPolygon.NodesEdgesOwnerTest());
Debug.WriteLine("Use breakpoint to check the last union test.");
Debug.WriteLine("Polygon tests done.");
unitedPolygon.Delete(true);
}
/// <summary>
/// Tests for Navigation Mesh and Pathfinder,
/// including performance test.
/// </summary>
private static void PathfinderTest()
{
Debug.WriteLine("Create testMesh...");
NavigationMesh[] testMesh = NavigationMeshFactory.GetTestInstanceForCleanUp();
Debug.WriteLine("testMesh created. Try 1. pathfinding...");
PathFinder.GetInstance().SetMesh(testMesh);
Stopwatch watch = Stopwatch.StartNew();
Path path = PathFinder.GetInstance().CalculatePath(new Vector2(1, 1), new Vector2(4, 14), typeof(Minion));
Debug.Assert(path != null);
watch.Stop();
Debug.WriteLine("1. Pathfinding done in " + watch.ElapsedMilliseconds +
" ms. Use breakpoint to check the returned path.");
GameLogic.GetInstance().GetGameStateManager().UnloadGame();
// ********************* second pathfinder test **************************
Debug.WriteLine("Try 2. pathfinding on another testMesh...");
watch = Stopwatch.StartNew();
int width = 5000;
int height = 5000;
Map map = Map.GetRandomInstance(20, width, height);
Debug.Assert(map != null);
GameLogic.GetInstance().SetMap(map);
Debug.Assert(GameLogic.GetInstance().GetMap() != null);
PathFinder.GetInstance().LoadMeshFromObstructions(width, height, 5);
watch.Stop();
Debug.WriteLine("Mesh creation done in " + watch.ElapsedMilliseconds + " ms.");
watch = Stopwatch.StartNew();
// ReSharper disable RedundantAssignment
// I want to read this out via debugger
path = PathFinder.GetInstance().CalculatePath(new Vector2(0, 0), new Vector2(width - 1, height - 1), typeof(Minion));
// ReSharper restore RedundantAssignment
watch.Stop();
Debug.WriteLine("2. Pathfinding done in " + watch.ElapsedMilliseconds +
" ms. Check path via breakpoint.");
// ******************************* Extreme case: Check that a direct connection is NOT feasible **************************
List<Polygon> endPolygons = new List<Polygon>();
List<MeshNode> meshNodes = new List<MeshNode>();
for (int y = 0; y < 4; y++)
{
for (int x = 0; x < 4; x++)
{
meshNodes.Add(new MeshNode(x, y));
}
}
List<Polygon> polygons = new List<Polygon>();
for (int y = 0; y < 3; y++)
{
for (int x = 0; x < 3; x++)
{
if (!(x == 1 && y == 1))
{
Polygon polygon = new Polygon(new List<MeshNode> { meshNodes[y * 4 + x], meshNodes[y * 4 + (x + 1)], meshNodes[(y + 1) * 4 + (x + 1)], meshNodes[(y + 1) * 4 + x] });
polygon.ImposeOwnership();
polygons.Add(polygon);
}
}
}
int startNodeIndex = 9;
int endNodeIndex = 6;
endPolygons.Add(polygons[2]);
Stack<Edge> firstCandidateEdges = new Stack<Edge>(meshNodes[startNodeIndex].GetPolyOwnersEdges());
Debug.Assert(!PathCleaner.IsCleanUpPossible(endPolygons, firstCandidateEdges, meshNodes[startNodeIndex].mVector, meshNodes[endNodeIndex].mVector));
GameLogic.GetInstance().GetGameStateManager().UnloadGame();
// ******************* Direct connection test ******************
int length = 3;
int count = 3;
testMesh = NavigationMeshFactory.GetTestInstance(length, length, count, count, false);
PathFinder.GetInstance().SetMesh(testMesh);
Debug.Assert(PathFinder.GetInstance().DirectConnection(new Vector2(2, 0), new Vector2(2, 3), typeof(Minion)), "Direct connection failed for (2,0)-(2,3)");
Debug.Assert(PathFinder.GetInstance().DirectConnection(new Vector2(8, 1), new Vector2(3, 1), typeof(Minion)), "Direct connection failed for (8,1)-(3,1)");
Debug.Assert(PathFinder.GetInstance().DirectConnection(new Vector2(8, 1), new Vector2(3, 3), typeof(Minion)), "Direct connection failed for (8,1)-(3,3)");
Debug.Assert(PathFinder.GetInstance().DirectConnection(new Vector2(4, 9), new Vector2(5, 4), typeof(Minion)), "Direct connection failed for (4,9)-(5,4)");
Random r = new Random();
Vector2 point1 = new Vector2(r.Next(length * count + 1), r.Next(length * count + 1));
Vector2 point2 = new Vector2(r.Next(length * count + 1), r.Next(length * count + 1));
Debug.Assert(PathFinder.GetInstance().DirectConnection(point1, point2, typeof(Minion)), "Direct connection test failed for (" + point1.X + "," + point1.Y + ")-(" + point2.X + "," + point2.Y + ")");
GameLogic.GetInstance().GetGameStateManager().UnloadGame();
}
private static void HelperMethodsTest()
{
// Angle:
Debug.Assert(HelperMethods.Circa(
HelperMethods.GetAngleDifference(new Vector2(1, 0), new Vector2(0, -1)), -Math.PI / 2));
// PointOnLine:
Debug.Assert(HelperMethods.PointOnLine(new Vector2(1, 1), new Vector2(0, 0), new Vector2(2, 2)));
Debug.Assert(HelperMethods.PointOnLine(new Vector2(1, 1), new Vector2(2, 2), new Vector2(0, 0)));
Debug.Assert(HelperMethods.PointOnLine(new Vector2(1, 1), new Vector2(2, 0), new Vector2(0, 2)));
Debug.Assert(!HelperMethods.PointOnLine(new Vector2(2, 0), new Vector2(1, 1), new Vector2(0, 2)));
Debug.Assert(HelperMethods.PointOnLine(new Vector2(1, 1), new Vector2(1, 1), new Vector2(0, 0)));
Debug.Assert(HelperMethods.PointOnLine(new Vector2(0, 2), new Vector2(0, 0), new Vector2(0, 2)));
Debug.Assert(HelperMethods.PointOnLine(new Vector2(0, 0), new Vector2(0, 2), new Vector2(0, 0)));
// PointInPolygon:
// These points are all inside:
Polygon polygon = new Polygon(new List<MeshNode> { new MeshNode(0, 0), new MeshNode(2, 0), new MeshNode(2, 2), new MeshNode(1, 2), new MeshNode(0, 2), new MeshNode(0, 1) });
polygon.ImposeOwnership();
foreach (MeshNode node in polygon.GetNodes())
{
Debug.Assert(polygon.PointInPolygon(node.mVector, false));
}
Debug.Assert(polygon.PointInPolygon(new Vector2(1, 0), false));
Debug.Assert(polygon.PointInPolygon(new Vector2(1, 1), false));
Debug.Assert(polygon.PointInPolygon(new Vector2(2, 1), false));
// Now for some points outside:
Debug.Assert(!polygon.PointInPolygon(new Vector2(0, -1), false));
Debug.Assert(!polygon.PointInPolygon(new Vector2(1, -1), false));
Debug.Assert(!polygon.PointInPolygon(new Vector2(2, -1), false));
Debug.Assert(!polygon.PointInPolygon(new Vector2(0, 3), false));
Debug.Assert(!polygon.PointInPolygon(new Vector2(1, 3), false));
Debug.Assert(!polygon.PointInPolygon(new Vector2(2, 3), false));
polygon.Delete(true);
}
// ReSharper restore UnusedMember.Local
// ReSharper disable UnusedMember.Local
// Sometimes I want to call this test for debugging, so it's okay that it's currently
// not used.
private static void CreateInstanceFromBlockmapTest()
{
PointSet<int> blocked = new PointSet<int>();
blocked.Add(12,12);
PointSet<int> coveredByPolygon = new PointSet<int>();
coveredByPolygon.Add(14,10);
coveredByPolygon.Add(14,11);
coveredByPolygon.Add(14,12);
coveredByPolygon.Add(14,13);
coveredByPolygon.Add(14,14);
Polygon remainingPolygon = new Polygon(new List<MeshNode>{new MeshNode(14,10), new MeshNode(15,10), new MeshNode(15,14), new MeshNode(14,14)});
Rectangle area = new Rectangle(10,10,4,4);
List<MeshNode> createdNodes = remainingPolygon.GetNodes();
NavigationMesh mesh = NavigationMeshFactory.CreateInstanceFromBlockmap(blocked,
new List<Polygon>{remainingPolygon},
coveredByPolygon,
1,
area,
createdNodes,
false);
mesh.CheckIntegrity();
Debug.WriteLine("CreateInstanceFromBlockmapTest ended.");
GameLogic.GetInstance().GetGameStateManager().UnloadGame();
}
/// <summary>Returns the index bounds for those sectors that are covered
/// by given rectangular polygon.</summary>
/// <returns>int[]{xmin, ymin, xmax, ymax}</returns>
private int[] GetCoveredSectorIndexes(Polygon polyRect)
{
int[] result = new int[4];
result[0] = ((int)polyRect.Xmin) / mSectorWidth;
result[1] = ((int)polyRect.Ymin) / mSectorHeight;
result[2] = ((int)polyRect.Xmax) / mSectorWidth;
result[3] = ((int)polyRect.Ymax) / mSectorHeight;
return result;
}
/// <summary>
/// If existant, returns the first common edge to the given polygon, else null.
/// Result is a List of two edges: first edge belongs to this polygon,
/// second edge belongs to given polygon2. Both are equivalent, e.g. are the
/// "one" common edge.
/// </summary>
/// <param name="polygon2"></param>
/// <returns></returns>
private List<Edge> GetFirstIncidentEdge(Polygon polygon2)
{
Debug.Assert(mEdges != null);
Debug.Assert(polygon2.GetEdges() != null);
Debug.Assert(IsConcave(mEdges));
Debug.Assert(IsConcave(polygon2.mEdges));
bool edgesFound = false;
List<Edge> commonEdges = null;
List<Edge> edges2 = polygon2.GetEdges();
// find the first common edge of both:
foreach (Edge edge in mEdges)
{
Debug.Assert(edge.mPolyOwners.Count == 1);
foreach (Edge edge2 in edges2)
{
if (edge.RecursiveReverse(edge2))
{
if (!edgesFound)
{
commonEdges = new List<Edge> { edge, edge2 };
edgesFound = true;
// return commonEdges;
}
else
{
Debug.Assert(IsConcave(mEdges));
Debug.Assert(IsConcave(polygon2.mEdges));
throw new Exception("Polygon.GetFirstIncidentEdges() found too many results!");
}
}
}
}
return commonEdges;
}
private void RemovePolygon(Polygon toRemove)
{
int[] indexes = GetCoveredSectorIndexes(toRemove);
for (int y = indexes[1]; y <= indexes[3]; y++)
for (int x = indexes[0]; x <= indexes[2]; x++)
{
bool success = mSectors[x, y].Remove(toRemove);
Debug.Assert(success);
Debug.Assert(!mSectors[x, y].Contains(toRemove));
}
}
public static Polygon CreateRectInstance(Rectangle shape, List<MeshNode> nodesToUse, List<Polygon> polygons)
{
MeshNode leftUpper = null;
MeshNode rightUpper = null;
MeshNode rightLower = null;
MeshNode leftLower = null;
foreach(MeshNode node in nodesToUse)
{
// ReSharper disable CompareOfFloatsByEqualityOperator
// I expect all node coordinates to be rounding-error-free
if (node.mVector.X == shape.X && node.mVector.Y == shape.Y)
leftUpper = node;
if (node.mVector.X == shape.X+shape.Width && node.mVector.Y == shape.Y)
rightUpper = node;
if (node.mVector.X == shape.X+shape.Width && node.mVector.Y == shape.Y+shape.Height)
rightLower = node;
if (node.mVector.X == shape.X && node.mVector.Y == shape.Y+shape.Height)
leftLower = node;
// ReSharper restore CompareOfFloatsByEqualityOperator
}
if (leftUpper == null)
{
leftUpper = new MeshNode(shape.X, shape.Y);
nodesToUse.Add(leftUpper);
foreach (Polygon polygon in polygons) polygon.TrySplit(leftUpper);
}
if (rightUpper == null)
{
rightUpper = new MeshNode(shape.X+shape.Width, shape.Y);
nodesToUse.Add(rightUpper);
foreach (Polygon polygon in polygons) polygon.TrySplit(rightUpper);
}
if (rightLower == null)
{
rightLower = new MeshNode(shape.X+shape.Width, shape.Y+shape.Height);
nodesToUse.Add(rightLower);
foreach (Polygon polygon in polygons) polygon.TrySplit(rightLower);
}
if (leftLower == null)
{
leftLower = new MeshNode(shape.X, shape.Y+shape.Height);
nodesToUse.Add(leftLower);
foreach (Polygon polygon in polygons) polygon.TrySplit(leftLower);
}
Polygon result = new Polygon(new List<MeshNode> {leftUpper, rightUpper, rightLower, leftLower});
foreach (MeshNode node in nodesToUse) result.TrySplit(node);
return result;
}
/// <summary>
/// This does the actual work for CalculatePath(). CalculatePath() did just
/// determine all the parameters for it.
/// </summary>
private Path CalculatePath2(Vector2 end, MeshNode startMeshNode, List<MeshNode> firstNeighbours, Polygon endPolygon, List<MeshNode> endNodes, bool startPassable, bool endPassable)
{
Debug.Assert(mMesh != null, "There is no navigation mesh! Load a map and then call PathFinder.GetInstance().LoadMeshFromObstructions().");
// uses A* algorithm
// !! remember to distinguish between latest costs (costs up to that point)
// !! and heuristic costs (estimated path costs over that point to target)
// I use AStarNode instead of MeshNode / Vector2 because I have to save
// additional information, like predecessor and latest costs.
// WorkingQueue should be in ascending order
// of heuristic value. I use my own comparer to allow multiple
// equal keys (since we may have nodes with the same heuristic value).
MinHeap<AStarNode> workingQueue = new MinHeap<AStarNode>();
// workingQueue doesn't support a quick check if some coords are
// already contained, so we handle that with a corresp. structure:
PointSet<float> workingCoords = new PointSet<float>();
PointSet<float> exploredCoords = new PointSet<float>();
// TODO: Evaluate First Loop iteration separately
double startCostsToEnd = HelperMethods.EuklidDistance(startMeshNode.mVector, end);
AStarNode current = new AStarNode(0, startCostsToEnd, startMeshNode, null);
if (EndConditionMet(current, endPassable, endPolygon, endNodes))
return HandleEnding(current, end, startPassable, endPassable, endPolygon);
exploredCoords.Add(startMeshNode.mVector.X, startMeshNode.mVector.Y);
HandleNeighbours(current, firstNeighbours, workingQueue, workingCoords, exploredCoords, end);
// TODO: First iteration evaluation finished
while (workingQueue.Count > 0)
{
//Debug.WriteLine("No. of nodes in the workingQueue: "+workingQueue.Count);
//OutputHeuristics(workingQueue);
current = GetNextNodeFromWorkingQueue(workingQueue, workingCoords);
//Debug.WriteLine("Now examining "+current.mNode.mVector.X+","+current.mNode.mVector.Y);
if (EndConditionMet(current, endPassable, endPolygon, endNodes))
return HandleEnding(current, end, startPassable, endPassable, endPolygon);
// The shortest path to current has been found,
// so we won't have to touch it ever again!
exploredCoords.Add(current.mNode.mVector.X, current.mNode.mVector.Y);
//Debug.WriteLine("Explored " + current.mNode.mVector.X + "," + current.mNode.mVector.Y);
// Add all unexplored unblocked neighbours to workingQueue:
List<MeshNode> neighbours = GetNeighbours2(current.mNode);
HandleNeighbours(current, neighbours, workingQueue, workingCoords, exploredCoords, end);
}
// Working queue got empty, but we still didn't reach our target
// -> No path found
Debug.WriteLine("WARNING - Pathfinder: No path found!");
return null;
}
/// <summary>
/// For code testing purposes:
/// Creates a raster of polygons where each one is rectangle-shaped and adjacent to each other.
/// Note that you have to pass x as countX for x columns of squares,
/// the same applies in y direction. The returned mesh then accepts points in the range of
/// (0,0)-(cellWidth*countX, cellHeight*countY).
/// </summary>
/// <param name="cellWidth">the width of each polygon</param>
/// <param name="cellHeight">the height of each polygon</param>
/// <param name="countX">the number of polygons to create in horizontal direction</param>
/// <param name="countY">the number of polygons to create in vertical direction</param>
/// <param name="doUnite">if the polygons should be united where possible (reduces raster complexity)</param>
/// <returns>the new mesh instance</returns>
public static NavigationMesh[] GetTestInstance(int cellWidth, int cellHeight, int countX, int countY, bool doUnite)
{
Debug.Assert(countX >= 1 && countY >= 1 && cellWidth > 0 && cellHeight > 0);
NavigationMesh[] testMesh = new NavigationMesh[3];
for (int i = 0; i < 3; i++)
{
testMesh[i] = new NavigationMesh(countX * cellWidth, countY * cellHeight);
MeshNode[,] nodes = new MeshNode[countX + 1, countY + 1];
for (int x = 0; x <= countX; x++)
{
for (int y = 0; y <= countY; y++)
{
nodes[x, y] = new MeshNode(x * cellWidth, y * cellHeight);
}
}
for (int x = 0; x < countX; x++)
{
for (int y = 0; y < countY; y++)
{
List<Edge> edges = new List<Edge>
{
new Edge(nodes[x, y], nodes[x + 1, y]),
new Edge(nodes[x + 1, y], nodes[x + 1, y + 1]),
new Edge(nodes[x + 1, y + 1], nodes[x, y + 1]),
new Edge(nodes[x, y + 1], nodes[x, y])
};
foreach (Edge edge in edges)
{
edge.ImposeOwnership();
}
Polygon polygon = new Polygon(edges);
polygon.ImposeOwnership();
testMesh[i].AddPolygon(polygon);
}
}
if (doUnite)
{
testMesh[i].WholeUnionRun(true, null);
}
}
return testMesh;
}
/// <summary>
/// Prepares the result for CalculatePath2().
/// </summary>
private Path HandleEnding(AStarNode current, Vector2 end, bool startPassable, bool endPassable, Polygon endPolygon)
{
// we're done!
AStarNode last;
if (endPassable && !current.mNode.mVector.Equals(end))
// We really want to get to the end point!
last = new AStarNode(0, 0, new MeshNode(end), current);
else
// We don't need to reach the end point
last = current;
// path cleanup needed?
if (GlobalValues.GetInstance().mCleanUpPath)
return PathCleaner.GetCleanedUpVectorPath(last, startPassable, endPolygon);
/*else*/
return GetPath(last, startPassable);
}
private static List<Polygon> GetCleanUpEndPolygons(int endIndex, List<MeshNode> meshNodes, Polygon endPolygon)
{
List<Polygon> result;
if (endIndex == meshNodes.Count - 1)
{
result = new List<Polygon> { endPolygon };
}
else
{
result = meshNodes[endIndex].mPolyOwners;
}
return result;
}
