override public void doStep()
{
if (openSet.Count == 0 && closedSet.Count == 0)
{
initAlgorithm();
}
else if (Finished || openSet.Count == 0)
{
Finished = true;
return;
}
Node activeNode = openSet [0];
foreach (Node node in openSet)
{
if (nodeData [node].FCost <= nodeData [activeNode].FCost)
{
activeNode = node;
}
}
openSet.Remove(activeNode);
closedSet.Add(activeNode);
activeNode.Type = NodeType.CLOSED;
if (activeNode == grid.TargetNode)
{
Finished = true;
paintPath();
return;
}
foreach (Node neighbour in grid.getNeighbours(activeNode))
{
if (closedSet.Contains(neighbour))
{
continue;
}
AStarData neighbourData;
if (!nodeData.TryGetValue(neighbour, out neighbourData))
{
neighbourData = new AStarData();
nodeData.Add(neighbour, neighbourData);
}
int newCostToNeighbour = nodeData [activeNode].GCost + getNodeDistance(activeNode, neighbour);
if (newCostToNeighbour < neighbourData.GCost || !openSet.Contains(neighbour))
{
neighbourData.GCost = newCostToNeighbour;
neighbourData.HCost = getNodeDistance(neighbour, grid.TargetNode);
neighbourData.Parent = activeNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
neighbour.Type = NodeType.OPEN;
}
}
}
}
public Map2dAStar(Func <bool> canJump, MazeLevel maze, VisionMapping vision, Transform transform, GameObject prefab_debug_astar)
{
this.maze = maze;
this.prefab_debug_astar = prefab_debug_astar;
SetAstarSourceData(vision, () => maze.Map, canJump, ResetCalcSpace,
(Coord c, out Coord n) => {
AStarData a = calcSpace.At(c);
Coord f = a.from; //e = a._edge;
n = f != Coord.NegativeOne ? f : c;
return(a._edge);
},
(c, f, e) => {
AStarData a = calcSpace.At(c);
a._edge = e; a.from = f;
},
c => calcSpace.At(c).f, (c, f) => { calcSpace.At(c).f = f; },
c => calcSpace.At(c).g, (c, f) => { calcSpace.At(c).g = f; });
ResetCalcSpace(Map.GetSize());
Vector3 p = transform.position;
Coord here = maze.GetCoord(p);
Start(here, here);
if (prefab_debug_astar != null)
{
maze.seen.onChange += RefreshVision;
vision.onChange += RefreshVision;
}
}
public AStarData(AStarData data, bool ventured)
{
this.previous_tile = data.previous_tile;
this.distance = data.distance;
this.waste = data.waste;
this.ventured = ventured;
}
public AStarGrid(int x, int y, int g, int h)
{
X = x;
Y = y;
data = new AStarData(g, h);
}
private List <NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end)
{
AStarData startData = new AStarData
{
cameFrom = null,
fScore = 0,
gScore = 0,
hScore = 0
};
Dictionary <NavMesh2DNode, NodeDataPair> openList = new Dictionary <NavMesh2DNode, NodeDataPair>();
Dictionary <NavMesh2DNode, NodeDataPair> closedList = new Dictionary <NavMesh2DNode, NodeDataPair>();
openList.Add(start, new NodeDataPair
{
n = start,
d = startData
});
while (openList.Count != 0)
{
NodeDataPair current = null;
{
float lowestFscore = float.MaxValue;
foreach (NodeDataPair n in openList.Values)
{
if (n.d.fScore < lowestFscore)
{
lowestFscore = n.d.fScore;
current = n;
}
}
}
openList.Remove(current.n);
closedList.Add(current.n, current);
if (closedList.ContainsKey(end))
{
//found a path
List <NavMesh2DNode> pathFound = new List <NavMesh2DNode>();
ReconstructPath(current, ref pathFound);
pathFound.Reverse();
//Path path = new Path(startNode, endNode);
//FindGoingTo(endNode, null);
//ReconstructPath(startNode, ref path);
//path.Smooth();
return(pathFound);
}
foreach (NavMesh2DConnection connection in current.n.connections)
{
NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex);
if (closedList.ContainsKey(connectedMesh2DNode))
{
continue;
}
NodeDataPair pairAlreadyInOpenlist = null;
if (openList.ContainsKey(connectedMesh2DNode))
{
pairAlreadyInOpenlist = openList[connectedMesh2DNode];
}
if (pairAlreadyInOpenlist == null)
{ //if the neighbor doesn't exist
AStarData nextNodeData = new AStarData();
nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find
//paths that are less dangerous, etc. danger being the heucistic.
nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore;
nextNodeData.cameFrom = current;
openList.Add(connectedMesh2DNode, new NodeDataPair
{
n = connectedMesh2DNode,
d = nextNodeData
});
}
else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore)
{ //if the neighbor exists and has bigger g score
pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position); //connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position);
pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore;
pairAlreadyInOpenlist.d.cameFrom = current;
}
}
}
return(null);
}
private List<NavMesh2DNode> RunPathWorker(NavMesh2DNode start, NavMesh2DNode end)
{
AStarData startData = new AStarData
{
cameFrom = null,
fScore = 0,
gScore = 0,
hScore = 0
};
Dictionary<NavMesh2DNode, NodeDataPair> openList = new Dictionary<NavMesh2DNode, NodeDataPair>();
Dictionary<NavMesh2DNode, NodeDataPair> closedList = new Dictionary<NavMesh2DNode, NodeDataPair>();
openList.Add(start, new NodeDataPair
{
n = start,
d = startData
});
while (openList.Count != 0)
{
NodeDataPair current = null;
{
float lowestFscore = float.MaxValue;
foreach (NodeDataPair n in openList.Values)
{
if (n.d.fScore < lowestFscore)
{
lowestFscore = n.d.fScore;
current = n;
}
}
}
openList.Remove(current.n);
closedList.Add(current.n, current);
if (closedList.ContainsKey(end))
{
//found a path
List<NavMesh2DNode> pathFound = new List<NavMesh2DNode>();
ReconstructPath(current, ref pathFound);
pathFound.Reverse();
//Path path = new Path(startNode, endNode);
//FindGoingTo(endNode, null);
//ReconstructPath(startNode, ref path);
//path.Smooth();
return pathFound;
}
foreach (NavMesh2DConnection connection in current.n.connections)
{
NavMesh2DNode connectedMesh2DNode = GetNode(connection.connectedNodeIndex);
if (closedList.ContainsKey(connectedMesh2DNode))
continue;
NodeDataPair pairAlreadyInOpenlist = null;
if (openList.ContainsKey(connectedMesh2DNode))
pairAlreadyInOpenlist = openList[connectedMesh2DNode];
if (pairAlreadyInOpenlist == null)
{ //if the neighbor doesn't exist
AStarData nextNodeData = new AStarData();
nextNodeData.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
nextNodeData.hScore = CalculateHeuristic(connectedMesh2DNode.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position); //heucistics will be used to find
//paths that are less dangerous, etc. danger being the heucistic.
nextNodeData.fScore = nextNodeData.gScore + nextNodeData.hScore;
nextNodeData.cameFrom = current;
openList.Add(connectedMesh2DNode, new NodeDataPair
{
n = connectedMesh2DNode,
d = nextNodeData
});
}
else if (current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position) < pairAlreadyInOpenlist.d.gScore)
{ //if the neighbor exists and has bigger g score
pairAlreadyInOpenlist.d.gScore = current.d.gScore + Vector3.Distance(current.n.position, connectedMesh2DNode.position);
pairAlreadyInOpenlist.d.hScore = CalculateHeuristic(pairAlreadyInOpenlist.n.position, end.position);//connection.Key == endNode ? Vector3.Distance(current.position,endNode.position) * 10 : Vector3.Distance(connection.Key.position,endNode.position);//Vector3.Distance(connection.Key.position,endNode.position);
pairAlreadyInOpenlist.d.fScore = pairAlreadyInOpenlist.d.gScore + pairAlreadyInOpenlist.d.hScore;
pairAlreadyInOpenlist.d.cameFrom = current;
}
}
}
return null;
}
/// <summary>
/// Sets up the searchSpace for using A*.
/// </summary>
/// <returns> the modified searchspace, the startnode, and the end node.</returns>
/// <param name="_searchSpace">The searchspace of the function.</param>
/// <param name="_start"> The start node if known, otherwise this is generated.</param>
/// <param name="_goal">The goal node if known, otherwise this is generated.</param>
private AStarData AStarSetup (List<GameObject> _searchSpace, GameObject _start, GameObject _goal)
{
//List<List<GameObject>> aStarData = new List<List<GameObject>> ();
GameObject start, goal;
//if start is not provided, generate one
if (_start)
{
start = _start;
}
else
{
start = GenerateRandomNode (_searchSpace);
}
//if goal is not provided, generate one
if (_goal)
{
goal = _goal;
}
else
{
goal = GenerateRandomNode (_searchSpace);
}
//set start's cost so far to 0
start.GetComponent<TruncOct> ().nodeData.tentativeDist = 0;
//setHeuristics
_searchSpace = SetHeuristics (_searchSpace, start, goal);
//wrap start and goal nodes into mono-member lists for packing into aStarData
List<GameObject> startHolder = new List<GameObject> (1) {start},
goalHolder = new List<GameObject> (1) {goal};
AStarData dataHolder = new AStarData ();
dataHolder.searchSpace = _searchSpace;
dataHolder.startObject = start;
dataHolder.goalObject = goal;
return dataHolder;
}
public static List <Position> DrawLine(Position start, Position destination, DrawContoller draw_option)
{
Stopwatch timer = new Stopwatch();
timer.Reset();
timer.Start();
if (start == destination)
{
return(new List <Position>()
{
start
});
}
Dictionary <Position, AStarData> data_map = new Dictionary <Position, AStarData>();
List <Position> quick_search = new List <Position>();
List <Position> medium_search = new List <Position>();
List <Position> slow_search = new List <Position>();
// first position is tile, second position is x = distance travelled and y = waste
quick_search.Add(start);
data_map.Add(start, new AStarData(new Position(-1, -1), 0, 0));
List <Position> result = new List <Position>();
Position current_tile = start;
bool path_found = false;
while (quick_search.Count + medium_search.Count + slow_search.Count > 0 && !path_found && !draw_option.QuickStop())
{
data_map[current_tile] = new AStarData(data_map[current_tile], true);
Position goal_direction = GetDirection(current_tile, destination);
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
Position adjacent_tile = GetAdjacent(current_tile, i, j);
if (map.ContainsKey(adjacent_tile))
{
if (adjacent_tile == destination)
{
if (draw_option.GetDistance(current_tile, adjacent_tile) > 0)
{
float distance = data_map[current_tile].distance + draw_option.GetDistance(current_tile, adjacent_tile);
float waste = data_map[current_tile].waste;
data_map.Add(adjacent_tile, new AStarData(current_tile, distance, waste));
path_found = true;
}
}
else
{
float distance = draw_option.GetDistance(current_tile, adjacent_tile);
if (distance > 0)
{
distance += data_map[current_tile].distance;
float waste = data_map[current_tile].waste + Mathf.Abs(goal_direction.x - i) + Mathf.Abs(goal_direction.y - j);
if (!data_map.ContainsKey(adjacent_tile))
{
data_map.Add(adjacent_tile, new AStarData(current_tile, distance, waste));
}
else
{
if (data_map[adjacent_tile].distance > distance)
{
data_map[adjacent_tile] = new AStarData(current_tile, distance, waste, data_map[adjacent_tile].ventured);
}
}
if (!data_map[adjacent_tile].ventured)
{
if (waste == data_map[current_tile].waste)
{
if (!quick_search.Contains(adjacent_tile))
{
quick_search.Add(adjacent_tile);
}
if (medium_search.Contains(adjacent_tile))
{
medium_search.Remove(adjacent_tile);
}
else if (slow_search.Contains(adjacent_tile))
{
slow_search.Remove(adjacent_tile);
}
}
else if (waste - data_map[current_tile].waste < 2)
{
if (!medium_search.Contains(adjacent_tile))
{
medium_search.Add(adjacent_tile);
}
if (slow_search.Contains(adjacent_tile))
{
slow_search.Remove(adjacent_tile);
}
}
else
{
if (!slow_search.Contains(adjacent_tile))
{
slow_search.Add(adjacent_tile);
}
}
}
}
}
}
}
}
if (quick_search.Count > 0)
{
//Sort Quick Search
quick_search.Sort((tile1, tile2) => data_map[tile1].waste.CompareTo(data_map[tile2].waste));
current_tile = quick_search[0];
quick_search.Remove(current_tile);
}
else if (medium_search.Count > 0)
{
//Sort medium_search
medium_search.Sort((tile1, tile2) => data_map[tile1].waste.CompareTo(data_map[tile2].waste));
current_tile = medium_search[0];
medium_search.Remove(current_tile);
}
else if (slow_search.Count > 0)
{
slow_search.Sort((tile1, tile2) => data_map[tile1].waste.CompareTo(data_map[tile2].waste));
current_tile = slow_search[0];
slow_search.Remove(current_tile);
}
}
if (!data_map.ContainsKey(destination))
{
throw new NoPathException(start + " to " + destination + " has no path");
}
AStarData item = data_map[destination];
result.Add(destination);
while (data_map.ContainsKey(item.previous_tile))
{
result.Add(item.previous_tile);
item = data_map[item.previous_tile];
}
result.Reverse();
return(result);
}
