public void Init(MatrixMap matrixMap, Enemy enemy, Action onEnemyPassed, EPathFormula formula, float moveSpeed, float x, float y, int targetX, int targetY) { _matrixMap = matrixMap; _enemy = enemy; _onEnemyPassed = onEnemyPassed; _formula = formula; _moveSpeed = moveSpeed; _startGridPosition = new GridPosition(x, y); _endGridPosition = new GridPosition(targetX, targetY); InitializePosition(); UpdatePath(); }
protected virtual double NeighborDistance(PathNode inStart, PathNode inEnd, EPathFormula formula) { var diffX = Math.Abs(inStart.X - inEnd.X); var diffY = Math.Abs(inStart.Y - inEnd.Y); switch (diffX + diffY) { case 1: return(1); case 2: return(Math.Sqrt(2)); case 0: return(0); default: throw new ApplicationException(); } }
protected override double Heuristic(PathNode inStart, PathNode inEnd, EPathFormula formula) { var dx = Math.Abs(inStart.X - inEnd.X); var dy = Math.Abs(inStart.Y - inEnd.Y); switch (formula) { case EPathFormula.Eucledian: return(Math.Sqrt(dx * dx + dy * dy)); case EPathFormula.EucledianSquared: return(dx * dx + dy * dy); case EPathFormula.Diagonal: return(Math.Min(dx, dy)); case EPathFormula.Manhatten: return(dx * dy + dx + dy); } return(Math.Abs(inStart.X - inEnd.X) + Math.Abs(inStart.Y - inEnd.Y)); }
protected override double NeighborDistance(PathNode inStart, PathNode inEnd, EPathFormula formula) { return(Heuristic(inStart, inEnd, formula)); }
/// <summary> /// Returns null, if no path is found. Start- and End-node are included in returned path. The user context /// is passed to IsWalkable(). /// </summary> public LinkedList <TPathNode> Search(Vector2 inStartNode, Vector2 inEndNode, TUserContext inUserContext, EPathFormula formula, bool flying) { var startNode = _searchSpace[(int)inStartNode.x, (int)inStartNode.y]; var endNode = _searchSpace[(int)inEndNode.x, (int)inEndNode.y]; if (startNode == endNode) { return(new LinkedList <TPathNode>(new[] { startNode.UserContext })); } var neighborNodes = new PathNode[4]; _closedSet.Clear(); _openSet.Clear(); _runtimeGrid.Clear(); _orderedOpenSet.Clear(); for (var x = 0; x < Width; x++) { for (var y = 0; y < Height; y++) { _cameFrom[x, y] = null; } } startNode.G = 0; startNode.H = Heuristic(startNode, endNode, formula); startNode.F = startNode.H; _openSet.Add(startNode); _orderedOpenSet.Push(startNode); _runtimeGrid.Add(startNode); while (!_openSet.IsEmpty) { var x = _orderedOpenSet.Pop(); if (x == endNode) { var result = ReconstructPath(_cameFrom, _cameFrom[endNode.X, endNode.Y]); result.AddLast(endNode.UserContext); return(result); } _openSet.Remove(x); _closedSet.Add(x); StoreNeighborNodes(x, neighborNodes); foreach (var y in neighborNodes) { bool tentativeIsBetter; if (y == null) { continue; } if (!flying) { if (!y.UserContext.IsWalkable(inUserContext)) { continue; } } if (_closedSet.Contains(y)) { continue; } var tentativeGScore = _runtimeGrid[x].G + NeighborDistance(x, y, formula); var wasAdded = false; if (!_openSet.Contains(y)) { _openSet.Add(y); tentativeIsBetter = true; wasAdded = true; } else if (tentativeGScore < _runtimeGrid[y].G) { tentativeIsBetter = true; } else { tentativeIsBetter = false; } if (tentativeIsBetter) { _cameFrom[y.X, y.Y] = x; if (!_runtimeGrid.Contains(y)) { _runtimeGrid.Add(y); } _runtimeGrid[y].G = tentativeGScore; _runtimeGrid[y].H = Heuristic(y, endNode, formula); _runtimeGrid[y].F = _runtimeGrid[y].G + _runtimeGrid[y].H; if (wasAdded) { _orderedOpenSet.Push(y); } else { _orderedOpenSet.Update(y); } } } } return(null); }
protected virtual double Heuristic(PathNode inStart, PathNode inEnd, EPathFormula formula) { return(Math.Sqrt((inStart.X - inEnd.X) * (inStart.X - inEnd.X) + (inStart.Y - inEnd.Y) * (inStart.Y - inEnd.Y))); }