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();
}