private void CreatePath(int startField, int targetField) { while (startField != targetField && targetField > 0) { GridPathNode node = _grid[targetField]; gridPath.AddPosition(node.column, node.row); // get next position targetField = node.parentIndex; } }
private void AddToOpenList(GridPathNode newNode) { int count = _openList.Count; for (int i = 0; i < count; i++) { if (newNode.f < _openList[i].f) { _openList.Insert(i, newNode); return; } } _openList.Add(newNode); }
/// <summary> /// will hardcode possible directions of all nodes into the nodes itself /// </summary> /// <param name="movementPossible"></param> public void CalculatePossibleDirections(MovementPossibleCheck movementPossible = null) { for (int row = 0; row < _rowCount; row++) { //_closedList.Add(new List<GridPathNode>()); for (int column = 0; column < _columnCount; column++) { GridPathNode currentNode = _grid[row * _columnCount + column]; for (int i = 0; i < _checkLength; i++) { int checkRow = currentNode.row + _rowCheck[i]; int checkColumn = currentNode.column + _columnCheck[i]; int checkField = checkRow * _columnCount + checkColumn; // if new position out of bounds, ignore it if (checkRow < 0 || checkColumn < 0 || checkRow >= _rowCount || checkColumn >= _columnCount) { continue; } GridPathNode checkNode = _grid[checkField]; // movement not possible => ignore if (movementPossible == null) { if (checkNode.walkable == false) { continue; } } else { if (movementPossible(currentNode.column, currentNode.row, checkColumn, checkRow) == false) { continue; } } currentNode.AddDirection(checkNode, _costOfMovement[i]); } } } }
// ====================================================================== // public methods // ---------------------------------------------------------------------- public void SetDimensions(int columnCount, int rowCount) { _columnCount = columnCount; _rowCount = rowCount; // prepare closed list _grid = new GridPathNode[rowCount * columnCount]; //_closedList.Clear(); for (int row = 0; row < _rowCount; row++) { //_closedList.Add(new List<GridPathNode>()); for (int column = 0; column < _columnCount; column++) { int index = row * _columnCount + column; _grid[index] = new GridPathNode(column, row, index); } } _fieldCount = _rowCount * _columnCount; gridPath.SetDimensions(_columnCount, _rowCount); }
// calculates path from startPos to endPos public bool FindPath(GridPosition startPos, GridPosition endPos, MovementPossibleCheck movementPossible = null, bool saveNearestPath = true, int maxNodeChecksWithoutProgress = 0) { // assume no path found gridPath.Clear(); if (startPos.row < 0 || startPos.column < 0 || endPos.row < 0 || endPos.column < 0 || startPos.column >= _columnCount || startPos.row >= _rowCount || endPos.column >= _columnCount || endPos.row >= _rowCount) { return false; } // reset open and closed list _openList.Clear(); for (int field = 0; field < _fieldCount; field++) { GridPathNode node = _grid[field]; if (node.closed) node.closed = false; } // set target values int targetNodeIndex = endPos.row * _columnCount + endPos.column; GridPathNode targetNode = _grid[targetNodeIndex]; // check if end point is walkable and then decide if using brute force or sophisticated // brute force works better when no path will be found and maxNodeChecksWithoutProgress is not set bool bruteForce = false; bool movementFromStartToEndIsPossible = false; if (movementPossible != null) movementFromStartToEndIsPossible = movementPossible(startPos.column, startPos.row, endPos.column, endPos.row); else movementFromStartToEndIsPossible = targetNode.walkable; // check if sophisticated or brute force approach if (!movementFromStartToEndIsPossible && maxNodeChecksWithoutProgress <= 0) { // brute force _heuristicValue = 1; bruteForce = true; } else { // sophisticated _heuristicValue = 10; } // set nearest point _nearestDistance = int.MaxValue; _lastTimeNearestPointPushedForward = 0; // add the starting point to the open list int startNodeIndex = startPos.row * _columnCount + startPos.column; GridPathNode startNode = _grid[startNodeIndex]; _openList.Add(startNode); startNode.h = GetHeuristic(startPos.row, startPos.column, endPos.row, endPos.column); startNode.opened = true; _nearestPoint = startNode; // starting point is also nearest point at the moment // while not found and still nodes to check while (targetNode.closed == false && _openList.Count > 0 // and while not the maximum of checks without real progress && (maxNodeChecksWithoutProgress <= 0 || _lastTimeNearestPointPushedForward < maxNodeChecksWithoutProgress)) { // first node in the openList will become the current node GridPathNode currentNode = _openList[0]; // save nearest field if (currentNode.h < _nearestDistance) { _nearestDistance = currentNode.h; _nearestPoint = currentNode; _lastTimeNearestPointPushedForward = 0; } else { _lastTimeNearestPointPushedForward++; } // mark current node as closed currentNode.closed = true; currentNode.opened = false; _openList.RemoveAt(0); for (int i = 0; i < currentNode.directionCount; i++) { GridPathNode.GridMovement gridMovement = currentNode.directions[i]; GridPathNode checkNode = gridMovement.node; // if node is already closed, ignore it if (checkNode.closed) continue; // movement not possible => ignore if (movementPossible == null) { if (checkNode.walkable == false) continue; } else { if (movementPossible(currentNode.column, currentNode.row, checkNode.column, checkNode.row) == false) continue; } // possible movement cost including path to currentNode int g = currentNode.g + gridMovement.movementCost; if (checkNode.opened == false) { checkNode.parentIndex = currentNode.index; checkNode.h = GetHeuristic(checkNode.row, checkNode.column, endPos.row, endPos.column); checkNode.g = g; checkNode.f = checkNode.g + checkNode.h; checkNode.opened = true; if (bruteForce) _openList.Add(checkNode); else AddToOpenList(checkNode); } else { // checkNode is on open list // if path to this checkNode is better from the current Node, update if (checkNode.g > g) { // update parent to the current Node checkNode.parentIndex = currentNode.index; // update path cost, checkNode.h already calculated checkNode.g = g; checkNode.f = checkNode.g + checkNode.h; // do not bother to move node to correct spot in open list, // this way it's faster } } } } // clear opened values for next search int openListCount = _openList.Count; for (int i = 0; i < openListCount; i++) { GridPathNode node = _openList[i]; node.opened = false; } // if target found if (targetNode.closed) { CreatePath(startNodeIndex, targetNodeIndex); return true; } // save nearest path if wished else if (saveNearestPath) { CreatePath(startNodeIndex, _nearestPoint.row * _columnCount + _nearestPoint.column); return false; } // no path and no nearest point else { return false; } }
// calculates path from startPos to endPos public bool FindPath(GridPosition startPos, GridPosition endPos, MovementPossibleCheck movementPossible = null, bool saveNearestPath = true, int maxNodeChecksWithoutProgress = 0) { // assume no path found gridPath.Clear(); if (startPos.row < 0 || startPos.column < 0 || endPos.row < 0 || endPos.column < 0 || startPos.column >= _columnCount || startPos.row >= _rowCount || endPos.column >= _columnCount || endPos.row >= _rowCount) { return(false); } // reset open and closed list _openList.Clear(); for (int field = 0; field < _fieldCount; field++) { GridPathNode node = _grid[field]; if (node.closed) { node.closed = false; } } // set target values int targetNodeIndex = endPos.row * _columnCount + endPos.column; GridPathNode targetNode = _grid[targetNodeIndex]; // check if end point is walkable and then decide if using brute force or sophisticated // brute force works better when no path will be found and maxNodeChecksWithoutProgress is not set bool bruteForce = false; bool movementFromStartToEndIsPossible = false; if (movementPossible != null) { movementFromStartToEndIsPossible = movementPossible(startPos.column, startPos.row, endPos.column, endPos.row); } else { movementFromStartToEndIsPossible = targetNode.walkable; } // check if sophisticated or brute force approach if (!movementFromStartToEndIsPossible && maxNodeChecksWithoutProgress <= 0) { // brute force _heuristicValue = 1; bruteForce = true; } else { // sophisticated _heuristicValue = 10; } // set nearest point _nearestDistance = int.MaxValue; _lastTimeNearestPointPushedForward = 0; // add the starting point to the open list int startNodeIndex = startPos.row * _columnCount + startPos.column; GridPathNode startNode = _grid[startNodeIndex]; _openList.Add(startNode); startNode.h = GetHeuristic(startPos.row, startPos.column, endPos.row, endPos.column); startNode.opened = true; _nearestPoint = startNode; // starting point is also nearest point at the moment // while not found and still nodes to check while (targetNode.closed == false && _openList.Count > 0 // and while not the maximum of checks without real progress && (maxNodeChecksWithoutProgress <= 0 || _lastTimeNearestPointPushedForward < maxNodeChecksWithoutProgress)) { // first node in the openList will become the current node GridPathNode currentNode = _openList[0]; // save nearest field if (currentNode.h < _nearestDistance) { _nearestDistance = currentNode.h; _nearestPoint = currentNode; _lastTimeNearestPointPushedForward = 0; } else { _lastTimeNearestPointPushedForward++; } // mark current node as closed currentNode.closed = true; currentNode.opened = false; _openList.RemoveAt(0); for (int i = 0; i < currentNode.directionCount; i++) { GridPathNode.GridMovement gridMovement = currentNode.directions[i]; GridPathNode checkNode = gridMovement.node; // if node is already closed, ignore it if (checkNode.closed) { continue; } // movement not possible => ignore if (movementPossible == null) { if (checkNode.walkable == false) { continue; } } else { if (movementPossible(currentNode.column, currentNode.row, checkNode.column, checkNode.row) == false) { continue; } } // possible movement cost including path to currentNode int g = currentNode.g + gridMovement.movementCost; if (checkNode.opened == false) { checkNode.parentIndex = currentNode.index; checkNode.h = GetHeuristic(checkNode.row, checkNode.column, endPos.row, endPos.column); checkNode.g = g; checkNode.f = checkNode.g + checkNode.h; checkNode.opened = true; if (bruteForce) { _openList.Add(checkNode); } else { AddToOpenList(checkNode); } } else { // checkNode is on open list // if path to this checkNode is better from the current Node, update if (checkNode.g > g) { // update parent to the current Node checkNode.parentIndex = currentNode.index; // update path cost, checkNode.h already calculated checkNode.g = g; checkNode.f = checkNode.g + checkNode.h; // do not bother to move node to correct spot in open list, // this way it's faster } } } } // clear opened values for next search int openListCount = _openList.Count; for (int i = 0; i < openListCount; i++) { GridPathNode node = _openList[i]; node.opened = false; } // if target found if (targetNode.closed) { CreatePath(startNodeIndex, targetNodeIndex); return(true); } // save nearest path if wished else if (saveNearestPath) { CreatePath(startNodeIndex, _nearestPoint.row * _columnCount + _nearestPoint.column); return(false); } // no path and no nearest point else { return(false); } }
private void CheckColliders(GridPathNode[] grid, int layerMask, int fromColumn, int fromRow, int toColumn, int toRow) { // save raycastsHitTriggers setting bool beforeSetting = Physics2D.queriesHitTriggers; Physics2D.queriesHitTriggers = false; for (int row = fromRow; row <= toRow; row++) { for (int column = fromColumn; column <= toColumn; column++) { // calculate position float posX = _startX + column * _fieldSizeX + _fieldSizeX * 0.5f; float posY = _startY + row * _fieldSizeY + _fieldSizeY * 0.5f; // check for colliders Collider2D coll = Physics2D.OverlapCircle(new Vector2(posX, posY), _fieldSizeX * 0.45f, layerMask); if (coll == null) grid[row * _columnCount + column].SetWalkable(true); else grid[row * _columnCount + column].SetWalkable(false); } } // restore raycastsHitTriggers setting Physics2D.queriesHitTriggers = beforeSetting; }
private void CheckColliders(GridPathNode[] grid, int layerMask) { CheckColliders(grid, layerMask, 0, 0, _columnCount - 1, _rowCount - 1); }