private void Awake()
{
openSet = new PathfindingHeap <PathfinderNode>(MAXPATHHEAPSIZE);
activeNodes = new Dictionary <Point, PathfinderNode>(1000);
closedSet = new HashSet <PathfinderNode>();
instance = this;
}
public abstract void ProcessNode(
PathfinderNode currentNode,
PathfinderNode startNode,
PathfinderNode targetNode,
PathfindingHeap <PathfinderNode> openSet,
HashSet <PathfinderNode> closedSet,
Dictionary <Point, PathfinderNode> activeNodes,
Grid grid,
int maxPathLength
);
private void Awake()
{
instance = this;
pathfinding = GetComponent <PathfindingHeap>();
}
void FindPath (Vector3 startPos, Vector3 targetPos)
{
UnityEngine.Debug.Log("starting stopwatch in FindPath");
Stopwatch sw = new Stopwatch();
sw.Start();
List<Vector3> waypoints = new List<Vector3>();
bool pathSuccess = false;
PathfindingNode startNode = grid.GetNodeFromWorldPoint(startPos);
UnityEngine.Debug.Log("startNode: " + startNode);
PathfindingNode targetNode = grid.GetNodeFromWorldPoint(targetPos);
UnityEngine.Debug.Log("end node: " + targetNode);
//UnityEngine.Debug.Log("if statement starting");
UD.Log("target.walkable = " + targetNode.walkable + " ::: start.walkable = " + startNode.walkable);
if (startNode.walkable && targetNode.walkable)
{
//UD.Log("both start and target are walkable");
PathfindingHeap<PathfindingNode> openSet = new PathfindingHeap<PathfindingNode>(grid.MaxSize);
HashSet<PathfindingNode> closedSet = new HashSet<PathfindingNode>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
PathfindingNode currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
print ("Path Found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach(PathfindingNode neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
//UnityEngine.Debug.Log(currentNode);
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
//yield return null;
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
UnityEngine.Debug.Log("path: " + waypoints);
}
//UnityEngine.Debug.Log("calling to request manager");
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
}
// Vytvoří objekt pathfinding, který obdrží pole s uzly a jejich počet
public Pathfinding(T[] nodes, int count)
{
_nodes = nodes;
_nodeCount = count;
_openSet = new PathfindingHeap <T>(_nodeCount);
}
public override void ProcessNode(
PathfinderNode currentNode,
PathfinderNode startNode,
PathfinderNode targetNode,
PathfindingHeap <PathfinderNode> openSet,
HashSet <PathfinderNode> closedSet,
Dictionary <Point, PathfinderNode> activeNodes,
Grid grid,
int maxPathLength
)
{
Vector3 currentLocation = grid.NodeToWorldCoord(currentNode.GetGridCoord());
List <PathfinderNode> neighbors = PathfinderHelper.GetNeighbors(
currentNode,
activeNodes,
currentNodeCreator
);
foreach (PathfinderNode neighbour in neighbors)
{
Vector3 neighbourLocation = grid.NodeToWorldCoord(neighbour.GetGridCoord());
if (!neighbour.IsWalkable() ||
closedSet.Contains(neighbour))
{
continue;
}
CostResult newStrategyCost =
currentCostStrategy.GetAdditionalCostAt(
currentLocation,
neighbourLocation
);
//neighbour.UpdateAccumulatedStrategyCost(newStrategyCost);
int newPhysicalGCost =
currentNode.GetPhysicalGCost()
+ PathfinderHelper.GetDistance(currentNode, neighbour);
int newStrategyGCost =
currentNode.GetStrategyGCost()
+ neighbour.GetExtractor().Extract(newStrategyCost);
int newMovementCostToNeighbour =
newPhysicalGCost + newStrategyGCost;
// bool smaller = newStrategyGCost < neighbour.GetStrategyGCost();
//if (smaller)
// {
//DrawGizmo.AddGizmo(Color.green, newStrategyGCost + " " + neighbour.GetStrategyGCost(), neighbour.GetLocation());
//}
//Debug.Log(neighbour.GetGCost());
if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour))
{
//Debug.Log(neighbour.GetGCost());
//DrawGizmo.AddGizmo(Color.green, "" + currentNode.GetExtractor().Extract(newStrategyCost), neighbour.GetLocation());
neighbour.SetStrategyCost(
newStrategyCost
);
neighbour.SetStrategyGCost(
newStrategyGCost
);
neighbour.SetPhysicalGCost(newPhysicalGCost);
neighbour.SetHCost(GetDistance(neighbour, targetNode));
neighbour.SetParent(currentNode);
if (!openSet.Contains(neighbour) &&
neighbour.WithInRangeOfStart(maxPathLength)
)
{
openSet.Add(neighbour);
PathfinderVisualizer.Visit(neighbour);
/*DrawGizmo.AddGizmo(Color.grey, "", grid.NodeToWorldCoord(
* neighbour.GetGridCoord())
* );*/
}
else
{
openSet.UpdateItem(neighbour);
}
}
else
{
}
}
}
