public void Delete() { RevokeOwnership(); mStartNode = null; mEndNode = null; mPolyOwners = null; }
/// <summary> /// DO NOT FORGET TO IMPOSE OWNERSHIP AFTER CREATION!!! /// </summary> /// <param name="start"></param> /// <param name="end"></param> public Edge(MeshNode start, MeshNode end) { if (start == end || start.mVector.Equals(end.mVector)) Debug.WriteLine("Something is wrong with that edge!"); mStartNode = start; mEndNode = end; mPolyOwners = new List<Polygon>(); }
/// <summary> /// Given a start and an end point, returns a path of points /// to follow (going through the path stack from beginning to end). These points /// form the shortest path for the unit to the target, respecting the /// registered obstructions of the pathfinder. /// If no path could be found, null is returned. /// </summary> /// <param name="start">start point</param> /// <param name="end">the goal / end point</param> /// <param name="unitType">the unit type for which the path should be calculated (one of </param> /// <returns>a path, e.g. a stack of ordered points to follow, or null if no path found</returns> public Path CalculatePath(Vector2 start, Vector2 end, Type unitType) { Debug.Assert(mMesh != null, "There is no navigation mesh! Load a map and then call PathFinder.GetInstance().LoadMeshFromObstructions()."); BindPointIntoMeshBounds(ref start); BindPointIntoMeshBounds(ref end); int meshIndex = DecideMeshIndex(unitType); // Check if we're done from the start: if (DirectConnection(start, end, meshIndex)) { // we're done just from the start! // no path cleanup needed here Stack<Vector2> stack = new Stack<Vector2>(); stack.Push(end); stack.Push(start); return new Path(stack); } // Determine start mesh node and first neighbours: MeshNode startMeshNode; List<MeshNode> firstNeighbours; Polygon startPolygon = mMesh[meshIndex].FindPolygon(start); bool startPassable; if (startPolygon == null) { // start point is impassable -> Create a virtual mesh node there startPassable = false; startMeshNode = new MeshNode(start); // Get the nearest free points as first neighbours: firstNeighbours = GetNearestFreeNodes(start, unitType); } else { // start point is passable, maybe even an existing mesh node? startPassable = true; MeshNode alreadyExistingStartNode = startPolygon.FindNode(start); if (alreadyExistingStartNode == null) { // the point is not already a meshNode -> create a virtual one startMeshNode = new MeshNode(start); startMeshNode.mPolyOwners.Add(startPolygon); } else { startMeshNode = alreadyExistingStartNode; } firstNeighbours = GetNeighbours2(startMeshNode); } // Determine list of end nodes: List<MeshNode> endNodes = new List<MeshNode>(); Polygon endPolygon = mMesh[meshIndex].FindPolygon(end); bool endPassable; if (endPolygon == null) { // end point lies in impassable area -> take nearest meshnodes // as end nodes endPassable = false; endNodes = GetNearestFreeNodes(end, unitType); } else { // end point passable -> all nodes of that polygon are end nodes endPassable = true; endNodes.AddRange(endPolygon.GetNodes()); } // Ready to do the path calculation: return CalculatePath2(end, startMeshNode, firstNeighbours, endPolygon, endNodes, startPassable, endPassable); }
/// <summary> /// Determines the neighbours for our AStar algorithm. /// </summary> private List<MeshNode> GetNeighbours2(MeshNode node) { GlobalValues globalValues = GlobalValues.GetInstance(); List<MeshNode> neighbours = new List<MeshNode>(); int interval = globalValues.mPathfinderIntermediateGranularity * globalValues.MeshScale; foreach (Polygon polygon in node.mPolyOwners) { foreach (Edge edge in polygon.GetEdges()) { Edge contrahent = edge.GetContrahent(); if (!neighbours.Contains(edge.mStartNode)) neighbours.Add(edge.mStartNode); if (contrahent != null && !( node.mPolyOwners.Contains(edge.mPolyOwners[0]) && node.mPolyOwners.Contains(contrahent.mPolyOwners[0]) ) ) { List<Polygon> owners = new List<Polygon>(); owners.AddRange(edge.mPolyOwners); owners.AddRange(contrahent.mPolyOwners); double edgeLength = edge.GetLength(); for (int i = interval; i < edgeLength; i += interval) { Vector2 pointOnEdge = edge.GetSectionPoint(i); MeshNode nodeOnEdge = new MeshNode(pointOnEdge); nodeOnEdge.mPolyOwners = owners; neighbours.Add(nodeOnEdge); } } // commented out because this should be collected by the // next edge startnode above /*if (!neighbours.Contains(edge.mEndNode)) neighbours.Add(edge.mEndNode);*/ } } return neighbours; }
/// <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> /// Helper method for this.CreateInstanceFromBlockmap(): /// Creates a MeshNode with the given point coordinates if it doesn't already exist and returns it. /// If a node exists, its coordinates are registered in createdNodes and an instance is found in nodeCreated. /// So, if it exists, this method returns its instance from nodeCreated, /// while if not, the new instance is added to createdNodes and returned, /// and its coordinates are put into nodeCreated. /// </summary> /// <param name="point">the coordinates of the MeshNode which will be checked</param> /// <param name="scale">The meshscale that is applied to the returned MeshNode</param> /// <param name="nodeCreated">the list of already created node instances</param> /// <param name="createdNodes">the list of coordinates of already existing nodes</param> /// <param name="remainingPolygons">we need the surrounding existant polygons, because if a claimed node does /// not exist and is created at the edge of an existing polygon, this edge has to be split up and the node /// must be inserted into the polygon's outline.</param> /// <param name="coveredByPolygonBefore">is used to detect if the requested node lies /// on an already existing polygon</param> /// <returns></returns> private static MeshNode RetrieveOrCreateNode(Point point, int scale, PointSet<int> nodeCreated, List<MeshNode> createdNodes, List<Polygon> remainingPolygons, PointSet<int> coveredByPolygonBefore) { MeshNode result; if (!nodeCreated.Contains(point.X, point.Y)) { // A new node will be created result = new MeshNode(scale * point.X, scale * point.Y); createdNodes.Add(result); nodeCreated.Add(point.X, point.Y); if ((coveredByPolygonBefore.Contains(point.X, point.Y) || coveredByPolygonBefore.Contains(point.X - 1, point.Y - 1)) && (coveredByPolygonBefore.Contains(point.X - 1, point.Y) || coveredByPolygonBefore.Contains(point.X, point.Y - 1)) ) { // the new node must be made part of a remaining polygon! bool successSplit; foreach (Polygon polygon in remainingPolygons) { successSplit = polygon.TrySplit(result); if (successSplit) break; } } } else { // An existing node will be used result = FindNodeInList(createdNodes, scale * point.X, scale * point.Y); } 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> /// 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; }
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> /// Tries to insert the given node into the polygon's outline and returns success. /// </summary> public bool TrySplit(MeshNode node) { foreach (Edge edge in mEdges) { if ((edge.mStartNode != node) && (edge.mEndNode != node) && HelperMethods.PointOnLine(node.mVector, edge.mStartNode.mVector, edge.mEndNode.mVector, 0)) { int edgeIndex = mEdges.IndexOf(edge); List<Edge> newSplitEdges = edge.GetSplit(node); edge.Delete(); mEdges[edgeIndex] = newSplitEdges[1]; mEdges.Insert(edgeIndex, newSplitEdges[0]); ImposeOwnership(); Debug.Assert(AllEdgesIncident(mEdges)); Debug.Assert(GetNodes().Contains(node)); return true; } } return false; }
public List<Edge> GetSplit(MeshNode node) { Debug.Assert(mStartNode != node && mEndNode != node); Debug.Assert(HelperMethods.PointOnLine(node.mVector, mStartNode.mVector, mEndNode.mVector, 0)); Edge e1 = new Edge(mStartNode, node); Edge e2 = new Edge(node, mEndNode); e1.ImposeOwnership(); e2.ImposeOwnership(); return new List<Edge>{e1, e2}; }
private void DrawNode(MeshNode node) { Vector2 pos = ToScreenCoords(node.mVector); CodeTest.DrawLine(pos, new Vector2(pos.X + 1, pos.Y + 1), 2, mNodeColor); }