private IList <GridPos> GetJumpPointsFromJumpPointFinder(Vector2Int startPoint, Vector2Int targetPoint) { if (!_gridInfoProvider.IsWalkable(targetPoint)) { return(new GridPos[0]); } GridPos startPosition = ZeroBasedPosition(startPoint); GridPos targetPosition = ZeroBasedPosition(targetPoint); // todo: when iAllowEndNodeUnWalkable is set to false, it seems that search is performed anyway — it just finishes with failure. Fix! JumpPointParam jumpPointsParams = new JumpPointParam(_navigationGrid, startPosition, targetPosition, iAllowEndNodeUnWalkable: false, iMode: HeuristicMode.MANHATTAN); // todo: note that resetting the grid causes all the nodes to be created again, // which probably causes bad performance (5-30 ms for 30x50 grid). // Possible solutions: use DynamicGridWPool instead of StaticGrid; refactor the library; use other library. jumpPointsParams.Reset(startPosition, targetPosition); IList <GridPos> resultPathList = JumpPointFinder.FindPath(jumpPointsParams); if (!resultPathList.Any()) { return(new GridPos[0]); } return(resultPathList); }
public static List <GridPos> JPSPlus(JumpPointParam JumpPointParameters, GridPos cell1, GridPos cell2) { if (JumpPointParameters != null) { JumpPointParameters.Reset(cell1, cell2); return(JumpPointFinder.GetFullPath(JumpPointFinder.FindPath(JumpPointParameters))); } return(new List <GridPos>()); }
public List <GridPos> GetNodes() { myGrid.Reset(myCurrent, myDestination); List <GridPos> resultPathList = JumpPointFinder.FindPath(myGrid); Convert convert = new Convert(); List <GridPos> realList = convert.Points(resultPathList); return(realList); }
private void findPath() { m_jumpParam.CurIterationType = m_tRecursive.isOn ? IterationType.RECURSIVE : IterationType.LOOP; m_jumpParam.DiagonalMovement = (DiagonalMovement)m_ddDiagonalMode.value; foreach (Testbed i in m_testbedList) { m_jumpParam.Reset(i.start, i.end); i.result = JumpPointFinder.FindPath(m_jumpParam); getOpenClose(i); drawResult(i); } }
public List <GridPos> JPSPlus(JumpPointParam JumpPointParameters, GridPos cell1, GridPos cell2) { List <GridPos> lpath = new List <GridPos>(); if (JumpPointParameters != null) { JumpPointParameters.Reset(cell1, cell2); List <GridPos> resultPathList = JumpPointFinder.FindPath(JumpPointParameters); lpath = JumpPointFinder.GetFullPath(resultPathList); } return(lpath); }
/// <summary> /// Keep in mind this method returns the start end and turn points of the path, and not all movement steps. /// </summary> private List <GridPos> FindPath(int fromX, int fromZ, int toX, int toZ) { JumpPointParam jpParam = GetJumpPointParam(); GridPos from = new GridPos(fromX, fromZ); GridPos to = new GridPos(toX, toZ); jpParam.Reset(from, to); List <GridPos> routeFound = JumpPointFinder.FindPath(jpParam); return(routeFound); }
/// <summary> /// Finds a path between 2 position on a map /// </summary> /// <param name="startPosition">Starting position</param> /// <param name="endPosition">Destination</param> /// <param name="map">Game map currently being used</param> /// <param name="endNodeUnWalkable"></param> /// <returns>A list of Vector2 waypoints that the object must traverse to get to its destination</returns> internal Stack <Vector2> FindPath(Vector2 startPosition, Vector2 endPosition, ref Map.Map map, EndNodeUnWalkableTreatment endNodeUnWalkable = EndNodeUnWalkableTreatment.DISALLOW) { if (!Map.Map.IsOnTop(endPosition) || !map.GetCollisionMap().GetWalkabilityGrid().IsWalkableAt(VectorToGridPos(endPosition))) { return(new Stack <Vector2>()); } var startGridPos = VectorToGridPos(startPosition); var endGridPos = VectorToGridPos(endPosition); if (ClearDirectPath(startGridPos.x, startGridPos.y, endGridPos.x, endGridPos.y, ref map)) { var pathVector = new Stack <Vector2>(1); pathVector.Push(endPosition); return(pathVector); } else { if (mJpParam != null) { mJpParam.Reset(VectorToGridPos(startPosition), VectorToGridPos(endPosition)); } else { mJpParam = new JumpPointParam(map.GetCollisionMap().GetWalkabilityGrid(), startGridPos, iDiagonalMovement: DiagonalMovement.OnlyWhenNoObstacles, iEndPos: endGridPos, iAllowEndNodeUnWalkable: endNodeUnWalkable, iMode: HeuristicMode.MANHATTAN); } var pathGrid = JumpPointFinder.FindPath(mJpParam); var pathVector = new Stack <Vector2>(pathGrid.Count); pathVector.Push(endPosition); for (var i = pathGrid.Count - 1; i > 0; i--) { var gridPos = GridPosToVector2(pathGrid[i]); pathVector.Push(gridPos); } return(pathVector); } }
public List <Vector2> FindPath(Vector2 start, Vector2 end) { GridPos s = new GridPos((int)(start.x * 2), (int)(start.y * 2)); GridPos e = new GridPos((int)(end.x * 2), (int)(end.y * 2)); if (s.x < 0 || s.x >= mSearchGrid.width || s.y < 0 || s.y >= mSearchGrid.height || e.x < 0 || e.x >= mSearchGrid.width || e.y < 0 || e.y >= mSearchGrid.height) { return(EmptyPath); } mParam.Reset(s, e, mSearchGrid); var path = JumpPointFinder.FindPath(mParam); return(path.Select(item => new Vector2(item.x / 2.0f, item.y / 2.0f)).Skip(1).ToList()); }
public void GetPath(Vector3 targetPos) { if (nextPath > Time.time) { return; } nextPath = Time.time + 0.2f; targetNode = 0; GridPos startPos = new GridPos(Mathf.RoundToInt(transform.position.x), Mathf.RoundToInt(transform.position.z)); GridPos endPos = new GridPos(Mathf.RoundToInt(targetPos.x), Mathf.RoundToInt(targetPos.z)); jpParam.Reset(startPos, endPos); JumpPointFinder.FindPath(jpParam, out targetPath); }
private void monsterUpdate(object sender, MonsterUpdateEventArgs e) { PlayerSeen = canSeePlayer(e.engine.map.mGrid, e.playerPos); if (moves.Count == 0) { if (PlayerSeen) { JumpPointParam jParam = new JumpPointParam(e.engine.searchgrid, false, false); jParam.Reset(new GridPos(pos.X, pos.Y), new GridPos(e.playerPos.X, e.playerPos.Y)); List <GridPos> resultPathList = JumpPointFinder.FindPath(jParam); resultPathList = JumpPointFinder.GetFullPath(resultPathList); if (moves.Count != 0) { moves.Clear(); } for (int i = 0; i < resultPathList.Count; i++) { moves.Enqueue(new Vector2(resultPathList[i].x, resultPathList[i].y)); } } } if (moves.Count != 0) { var dxdy = e.engine.map.getDxDy(pos, moves.Dequeue()); if (!e.engine.checkEntity(pos, e.playerPos, dxdy.X, dxdy.Y)) { move(dxdy.X, dxdy.Y); } } if (PlayerSeen == false) { Vector2 a = rndMoves[rnd.Next(rndMoves.Count)]; if (e.engine.map.canMove(pos, a.X, a.Y)) { move(a.X, a.Y); } } if (isPlayerNear(e.playerPos)) { attack(e.engine.player); } }
public List <Node> FindPath(GameObject source, GameObject target) { source.GetComponent <Movement>().currentPath = null; clear_graph(); List <Node> tOpenList = iParam.openList; //открытый список Node tNode = null; Node tStartNode = graph[ source.GetComponent <Movement>().TileX, source.GetComponent <Movement>().TileY ]; Node tEndNode = graph[ target.GetComponent <Movement>().TileX, target.GetComponent <Movement>().TileY ]; iParam.Reset(tStartNode, tEndNode); tOpenList.Add(tStartNode); tStartNode.isOpened = true; while (tOpenList.Count > 0) { tOpenList.Sort(); tNode = (Node)tOpenList[0]; tOpenList.RemoveAt(0); tNode.isClosed = true; if (tNode.xz == tEndNode.xz && tNode.yz == tEndNode.yz) { return(Node.Backtrace(tNode)); // перестраиваем путь } identifySuccessors(iParam, tNode); } return(new List <Node>()); }
public List <Vector3> GetPath(Vector3 startLocation) { startPos = new GridPos((int)Mathf.Floor(startLocation.x), (int)Mathf.Floor(startLocation.y), (int)Mathf.Floor(startLocation.z)); if (jpParam != null) { jpParam.Reset(startPos, endPos); } else { jpParam = new JumpPointParam(searchGrid, startPos, endPos, EndNodeUnWalkableTreatment.ALLOW, DiagonalMovement.IfAtLeastOneWalkable, HeuristicMode.EUCLIDEAN); } resultPathList = JumpPointFinder.FindPath(jpParam); List <Vector3> posList = new List <Vector3>(); foreach (GridPos pos in resultPathList) { posList.Add(new Vector3(pos.x, pos.y, pos.z)); } return(posList); }
public List <Tile> FindPath(int sX, int sY, int eX, int eY) { GridPos start = new GridPos(sX, sY); GridPos end = new GridPos(eX, eY); if (!_searchGrid.IsWalkableAt(end)) { return(new List <Tile>()); } jpParam.Reset(start, end, _searchGrid); List <GridPos> result = JumpPointFinder.FindPath(jpParam); List <Tile> result2 = new List <Tile>(); foreach (GridPos gp in result) { result2.Add(_mapLayout[gp.x, gp.y]); } return(result2); }
public static Path generatePath(Dimension _Dimension, Vector2 _StartPosition, Vector2 _EndPosition) { try { int var_SizeX = 20; //(int)Math.Abs(_StartPosition.X - _EndPosition.X)/16 + 2;//20; int var_SizeY = 20; //(int)Math.Abs(_StartPosition.Y - _EndPosition.Y)/16 + 2;//20; int var_StartX = (int)((_StartPosition.X % (Region.regionSizeX * Chunk.chunkSizeX * Block.BlockSize)) % (Chunk.chunkSizeX * Block.BlockSize) / Block.BlockSize); int var_StartY = (int)((_StartPosition.Y % (Region.regionSizeY * Chunk.chunkSizeY * Block.BlockSize)) % (Chunk.chunkSizeY * Block.BlockSize) / Block.BlockSize); int var_TargetX = (int)((_EndPosition.X % (Region.regionSizeX * Chunk.chunkSizeX * Block.BlockSize)) % (Chunk.chunkSizeX * Block.BlockSize) / Block.BlockSize); int var_TargetY = (int)((_EndPosition.Y % (Region.regionSizeY * Chunk.chunkSizeY * Block.BlockSize)) % (Chunk.chunkSizeY * Block.BlockSize) / Block.BlockSize); var_TargetX = var_TargetX - var_StartX + var_SizeX / 2; var_TargetY = var_TargetY - var_StartY + var_SizeY / 2; BaseGrid searchGrid = new DynamicGridWPool(SingletonHolder <NodePool> .Instance); JumpPointParam jumpParam = new JumpPointParam(searchGrid, true, true, false, HeuristicMode.EUCLIDEAN); GridPos startPos = new GridPos(var_SizeX / 2, var_SizeX / 2); GridPos endPos = new GridPos(var_TargetX, var_TargetY); for (int x = 0; x < var_SizeX; x++) { for (int y = 0; y < var_SizeY; y++) { int var_X = (int)_StartPosition.X + (-var_SizeX / 2 + x) * Block.BlockSize; int var_Y = (int)_StartPosition.Y + (-var_SizeX / 2 + y) * Block.BlockSize; Block var_Block = _Dimension.getBlockAtCoordinate(new Vector3(var_X, var_Y, 0)); bool var_IsWalkAble = false; if (var_Block != null) { if (var_Block.IsWalkAble) { var_IsWalkAble = true; } else if (var_Block.Objects.Count > 0) { if (x == var_SizeX / 2 && y == var_SizeY / 2) { var_IsWalkAble = true; } if (x == var_TargetX && y == var_TargetY) { var_IsWalkAble = true; } } } searchGrid.SetWalkableAt(new GridPos(x, y), var_IsWalkAble); } } jumpParam.CrossCorner = true; jumpParam.CrossAdjacentPoint = false; jumpParam.UseRecursive = false; // KP ;D jumpParam.Reset(startPos, endPos); List <GridPos> resultList = JumpPointFinder.FindPath(jumpParam); LinkedList <PathNode> var_PathNodes = new LinkedList <PathNode>(); foreach (GridPos var_GridPos in resultList) { PathNode var_PathNode = new PathNode(); var_PathNode.X = var_GridPos.x; var_PathNode.Y = var_GridPos.y; int var_X = (int)_StartPosition.X + (-var_SizeX / 2 + var_PathNode.X) * Block.BlockSize; int var_Y = (int)_StartPosition.Y + (-var_SizeX / 2 + var_PathNode.Y) * Block.BlockSize; Block var_Block = _Dimension.getBlockAtCoordinate(new Vector3(var_X, var_Y, 0)); var_PathNode.block = var_Block; var_PathNodes.AddLast(var_PathNode); } /*for (int y = 0; y < var_SizeY; y++) * { * for (int x = 0; x < var_SizeY; x++) * { * Console.ForegroundColor = ConsoleColor.White; * if (x == 10 && y == 10) * { * Console.ForegroundColor = ConsoleColor.Green; * } * foreach (PathNode var_PathNode in var_PathNodes) * { * if (var_PathNode.X == x && var_PathNode.Y == y) * { * Console.ForegroundColor = ConsoleColor.Red; * } * } * if (!searchGrid.IsWalkableAt(x,y)) * { * Console.Write("x"); * } * else * { * Console.Write("-"); * } * } * Console.WriteLine(); * }*/ Path var_Result = new Path(var_PathNodes); return(var_Result); } catch (Exception ex) { Logger.Logger.LogErr(ex.ToString()); } return(null); }
public virtual void ReplanGlobal(Platform platform) { // create searchGrid data structure for the EpPathFinding BaseGrid searchGrid = new StaticGrid(platform.Map.Rows, platform.Map.Columns); List <Tuple <double, GridPos> > searchPoses = new List <Tuple <double, GridPos> > (); // possible search poses for (int i = 0; i < platform.Map.Rows; i++) { for (int j = 0; j < platform.Map.Columns; j++) { if (platform.Map.MapMatrix[i, j] < platform.OccupiedThreshold) { searchGrid.SetWalkableAt(i, j, true); } if ((platform.Map.MapMatrix[i, j] >= platform.FreeThreshold) && (platform.Map.MapMatrix[i, j] <= platform.OccupiedThreshold)) { RegionLimits limits = platform.Map.CalculateLimits(i, j, 1); List <Pose> posesl = limits.GetPosesWithinLimits(); foreach (Pose p in posesl) { if (platform.Map.MapMatrix[p.X, p.Y] < platform.FreeThreshold) { double d = Math.Sqrt(Math.Pow(p.X - platform.Pose.X, 2) + Math.Pow(p.Y - platform.Pose.Y, 2)); searchPoses.Add(new Tuple <double, GridPos>(d, new GridPos(i, j))); break; } } } } } // set unaccessable for those places where are platforms and their enviroment within 1 step radius foreach (Platform plt in platform.ObservedPlatforms) { if (plt.Equals(platform)) { continue; } RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1); List <Pose> posesl = limits.GetPosesWithinLimits(); foreach (Pose p in posesl) { searchGrid.SetWalkableAt(p.X, p.Y, false); } } // bound the search to avoid large computation // select the first closest 50 candidates based on L2 distance int maxNumOfSearchPoses = 50; searchPoses.Sort((t1, t2) => t1.Item1.CompareTo(t2.Item1)); if (searchPoses.Count > maxNumOfSearchPoses) { searchPoses.RemoveRange(maxNumOfSearchPoses, searchPoses.Count - maxNumOfSearchPoses); } // init search GridPos startPos = new GridPos(platform.Pose.X, platform.Pose.Y); GridPos endPos = new GridPos(20, 10); JumpPointParam jpParam = new JumpPointParam(searchGrid, startPos, endPos, false, true, true); // find the best path double bestPathScore = Double.PositiveInfinity; List <GridPos> bestPath = null; foreach (Tuple <double, GridPos> p in searchPoses) { //jpParam.Reset(startPos, p); jpParam.Reset(new GridPos(platform.Pose.X, platform.Pose.Y), p.Item2); List <GridPos> resultPathList = JumpPointFinder.FindPath(jpParam); if (resultPathList.Count > 2) { double score = 0; for (int i = 1; i < resultPathList.Count; i++) { score += Math.Sqrt(Math.Pow(resultPathList[i].x - resultPathList[i - 1].x, 2) + Math.Pow(resultPathList[i].y - resultPathList[i - 1].y, 2)); } if (score < bestPathScore) { bestPathScore = score; bestPath = resultPathList; bestFronterier = new Pose(resultPathList.Last().x, resultPathList.Last().y); } } } // convert the best path to command sequence if ((bestPath != null) && (bestPath.Count > 2)) { List <Pose> bestPathConv = new List <Pose>(); bestPathConv.Add(platform.Pose); for (int i = 1; i < bestPath.Count; i++) { Pose prevPose = bestPathConv.Last(); Pose goalPose = new Pose(bestPath[i].x, bestPath[i].y); int dxl = Math.Sign(goalPose.X - prevPose.X); int dyl = Math.Sign(goalPose.Y - prevPose.Y); while (!prevPose.Equals(goalPose)) // it's a bit dangerous here { Pose newPose = new Pose(prevPose.X + dxl, prevPose.Y + dyl); prevPose = newPose; bestPathConv.Add(newPose); } } for (int i = bestPathConv.Count - 2; i > 0; i--) { int dx = bestPathConv[i + 1].X - bestPathConv[i].X; int dy = bestPathConv[i + 1].Y - bestPathConv[i].Y; double dalpha = Math.Atan2(dy, dx); Pose newPose = new Pose(bestPathConv[i].X, bestPathConv[i].Y, dalpha); commandSequence.Push(newPose); } } }
private void btnSearch_Click(object sender, EventArgs e) { for (int resultTrav = 0; resultTrav < m_resultLine.Count; resultTrav++) { m_resultLine[resultTrav].Dispose(); } m_resultLine.Clear(); for (int resultTrav = 0; resultTrav < m_resultBox.Count; resultTrav++) { m_resultBox[resultTrav].Dispose(); } m_resultBox.Clear(); GridPos startPos = new GridPos(); GridPos endPos = new GridPos(); for (int widthTrav = 0; widthTrav < width; widthTrav++) { for (int heightTrav = 0; heightTrav < height; heightTrav++) { if (m_rectangles[widthTrav][heightTrav].boxType != BoxType.Wall) { searchGrid.SetWalkableAt(new GridPos(widthTrav, heightTrav), true); } else { searchGrid.SetWalkableAt(new GridPos(widthTrav, heightTrav), false); } if (m_rectangles[widthTrav][heightTrav].boxType == BoxType.Start) { startPos.x = widthTrav; startPos.y = heightTrav; } if (m_rectangles[widthTrav][heightTrav].boxType == BoxType.End) { endPos.x = widthTrav; endPos.y = heightTrav; } } } jumpParam.DiagonalMovement = (DiagonalMovement)cbbJumpType.SelectedIndex; jumpParam.UseRecursive = cbUseRecursive.Checked; jumpParam.Reset(startPos, endPos); List <GridPos> resultList = JumpPointFinder.FindPath(jumpParam); for (int resultTrav = 0; resultTrav < resultList.Count - 1; resultTrav++) { m_resultLine.Add(new GridLine(m_rectangles[resultList[resultTrav].x][resultList[resultTrav].y], m_rectangles[resultList[resultTrav + 1].x][resultList[resultTrav + 1].y])); } for (int widthTrav = 0; widthTrav < jumpParam.SearchGrid.width; widthTrav++) { for (int heightTrav = 0; heightTrav < jumpParam.SearchGrid.height; heightTrav++) { if (jumpParam.SearchGrid.GetNodeAt(widthTrav, heightTrav) == null) { continue; } if (jumpParam.SearchGrid.GetNodeAt(widthTrav, heightTrav).isOpened) { ResultBox resultBox = new ResultBox(widthTrav * 20, heightTrav * 20 + 50, ResultBoxType.Opened); m_resultBox.Add(resultBox); } if (jumpParam.SearchGrid.GetNodeAt(widthTrav, heightTrav).isClosed) { ResultBox resultBox = new ResultBox(widthTrav * 20, heightTrav * 20 + 50, ResultBoxType.Closed); m_resultBox.Add(resultBox); } } } this.Invalidate(); }
private void btnSearch_Click(object sender, EventArgs e) { for (var resultTrav = 0; resultTrav < resultLine.Count; resultTrav++) { resultLine[resultTrav].Dispose(); } resultLine.Clear(); for (var resultTrav = 0; resultTrav < resultBox.Count; resultTrav++) { resultBox[resultTrav].Dispose(); } resultBox.Clear(); var startPos = new GridPos(); var endPos = new GridPos(); for (var x = 0; x < width; x++) { for (var y = 0; y < height; y++) { _ = rectangles[x][y].BoxType != BoxType.Wall ? searchGrid.SetWalkableAt(x, y, true) : searchGrid.SetWalkableAt(x, y, false); if (rectangles[x][y].BoxType == BoxType.Start) { startPos.X = x; startPos.Y = y; } if (rectangles[x][y].BoxType == BoxType.End) { endPos.X = x; endPos.Y = y; } } } jumpParam.DiagonalMovement = (DiagonalMovement)cbbJumpType.SelectedIndex; jumpParam.CurIterationType = cbUseRecursive.Checked ? IterationType.Recursive : IterationType.Loop; jumpParam.Reset(startPos, endPos); var resultList = JumpPointFinder.FindPath(jumpParam); for (var resultTrav = 0; resultTrav < resultList.Count - 1; resultTrav++) { resultLine.Add(new GridLine(rectangles[resultList[resultTrav].X][resultList[resultTrav].Y], rectangles[resultList[resultTrav + 1].X][resultList[resultTrav + 1].Y])); } for (var x = 0; x < jumpParam.SearchGrid.Width; x++) { for (var y = 0; y < jumpParam.SearchGrid.Height; y++) { if (jumpParam.SearchGrid.GetNodeAt(x, y) == null) { continue; } if (jumpParam.SearchGrid.GetNodeAt(x, y).IsOpened) { var resultBox = new ResultBox(x * 20, (y * 20) + 50, ResultBoxType.Opened); this.resultBox.Add(resultBox); } if (jumpParam.SearchGrid.GetNodeAt(x, y).IsClosed) { var resultBox = new ResultBox(x * 20, (y * 20) + 50, ResultBoxType.Closed); this.resultBox.Add(resultBox); } } } Invalidate(); }