public JumpPointParam(BaseGrid iGrid, GridPos iStartPos, GridPos iEndPos, bool iAllowEndNodeUnWalkable = true, bool iCrossCorner = true, bool iCrossAdjacentPoint = true, HeuristicMode iMode = HeuristicMode.EUCLIDEAN)
{
switch (iMode)
{
case HeuristicMode.MANHATTAN:
m_heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
m_heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
m_allowEndNodeUnWalkable = iAllowEndNodeUnWalkable;
m_crossAdjacentPoint = iCrossAdjacentPoint;
m_crossCorner = iCrossCorner;
openList = new List<Node>();
m_searchGrid = iGrid;
m_startNode = m_searchGrid.GetNodeAt(iStartPos.x, iStartPos.y);
m_endNode = m_searchGrid.GetNodeAt(iEndPos.x, iEndPos.y);
if (m_startNode == null)
m_startNode = new Node(iStartPos.x, iStartPos.y, true);
if (m_endNode == null)
m_endNode = new Node(iEndPos.x, iEndPos.y, true);
m_useRecursive = false;
}
private static void identifySuccessors(JumpPointParam iParam, Node iNode)
{
HeuristicDelegate tHeuristic = iParam.HeuristicFunc;
List <Node> tOpenList = iParam.openList;
if (iParam.EndNode != null)
{
int tEndX = iParam.EndNode.x;
int tEndY = iParam.EndNode.y;
GridPos tNeighbor;
GridPos tJumpPoint;
Node tJumpNode;
List <GridPos> tNeighbors = findNeighbors(iParam, iNode);
for (int i = 0; i < tNeighbors.Count; i++)
{
tNeighbor = tNeighbors[i];
if (iParam.UseRecursive)
{
tJumpPoint = jump(iParam, tNeighbor.x, tNeighbor.y, iNode.x, iNode.y);
}
else
{
tJumpPoint = jumpLoop(iParam, tNeighbor.x, tNeighbor.y, iNode.x, iNode.y);
}
if (tJumpPoint != null)
{
tJumpNode = iParam.SearchGrid.GetNodeAt(tJumpPoint.x, tJumpPoint.y);
if (tJumpNode == null)
{
if (iParam.EndNode.x == tJumpPoint.x && iParam.EndNode.y == tJumpPoint.y)
{
tJumpNode = iParam.SearchGrid.GetNodeAt(tJumpPoint);
}
}
if (tJumpNode.isClosed)
{
continue;
}
// include distance, as parent may not be immediately adjacent:
float tCurNodeToJumpNodeLen = tHeuristic(Math.Abs(tJumpPoint.x - iNode.x), Math.Abs(tJumpPoint.y - iNode.y));
float tStartToJumpNodeLen = iNode.startToCurNodeLen + tCurNodeToJumpNodeLen; // next `startToCurNodeLen` value
if (!tJumpNode.isOpened || tStartToJumpNodeLen < tJumpNode.startToCurNodeLen)
{
tJumpNode.startToCurNodeLen = tStartToJumpNodeLen;
tJumpNode.heuristicCurNodeToEndLen = (tJumpNode.heuristicCurNodeToEndLen == null ? tHeuristic(Math.Abs(tJumpPoint.x - tEndX), Math.Abs(tJumpPoint.y - tEndY)) : tJumpNode.heuristicCurNodeToEndLen);
tJumpNode.heuristicStartToEndLen = tJumpNode.startToCurNodeLen + tJumpNode.heuristicCurNodeToEndLen.Value;
tJumpNode.parent = iNode;
if (!tJumpNode.isOpened)
{
tOpenList.Add(tJumpNode);
tJumpNode.isOpened = true;
}
}
}
}
}
}
public JumpPointParam(BaseGrid iGrid, bool iCrossCorner = true, HeuristicMode iMode = HeuristicMode.EUCLIDEAN)
{
switch (iMode)
{
case HeuristicMode.MANHATTAN:
heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
crossCorner = iCrossCorner;
openList = new List<Node>();
searchGrid = iGrid;
startNode = null;
endNode = null;
}
public JumpPointParam(BaseGrid iGrid, bool iAllowEndNodeUnWalkable = true, bool iCrossCorner = true, bool iCrossAdjacentPoint = true, HeuristicMode iMode = HeuristicMode.EUCLIDEAN)
{
switch (iMode)
{
case HeuristicMode.MANHATTAN:
m_heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
m_heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
m_allowEndNodeUnWalkable = iAllowEndNodeUnWalkable;
m_crossAdjacentPoint = iCrossAdjacentPoint;
m_crossCorner = iCrossCorner;
openList = new List <Node>();
m_searchGrid = iGrid;
m_startNode = null;
m_endNode = null;
m_useRecursive = false;
}
private Vector2Int[] AStar(Vector2Int start, Vector2Int goal, HeuristicDelegate heuristic)
{
List <Vector2Int> openSet = new List <Vector2Int>();
openSet.Add(start);
Dictionary <Vector2Int, Vector2Int> cameFrom = new Dictionary <Vector2Int, Vector2Int>();
Dictionary <Vector2Int, float> gScore = new Dictionary <Vector2Int, float>();
InitiateToInfinity(ref gScore);
gScore[start] = 0;
Dictionary <Vector2Int, float> fScore = new Dictionary <Vector2Int, float>();
InitiateToInfinity(ref fScore);
fScore[start] = heuristic(start, goal);
while (openSet.Count > 0)
{
openSet.Sort((a, b) => (int)fScore[a] - (int)fScore[b]);
Vector2Int current = openSet[0];
if (current == goal)
{
return(ReconstructPath(cameFrom, current));
}
openSet.Remove(current);
foreach (var neighbour in GetNeighboursOf(current))
{
float tentativeGScore = gScore[current] + GetEdgeWeightBetween(current, neighbour);
if (tentativeGScore < gScore[neighbour])
{
if (cameFrom.ContainsKey(neighbour))
{
cameFrom[neighbour] = current;
}
else
{
cameFrom.Add(neighbour, current);
}
gScore[neighbour] = tentativeGScore;
fScore[neighbour] = tentativeGScore + heuristic(neighbour, goal);
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
return(null);
}
private List <MapCity> AStar(MapCity start, MapCity goal, HeuristicDelegate heuristic)
{
//Should be min-heap or priority queue, will use List for readability
List <MapCity> openSet = new List <MapCity>()
{
start
};
Dictionary <MapCity, MapCity> cameFrom = new Dictionary <MapCity, MapCity>();
Dictionary <MapCity, float> gScore = new Dictionary <MapCity, float>();
gScore[start] = 0;
Dictionary <MapCity, float> fScore = new Dictionary <MapCity, float>();
fScore[start] = heuristic(start, goal);
while (openSet.Count > 0)
{
MapCity current = GetLowestFScore(openSet, fScore);
if (current == goal)
{
return(ReconstructPath(cameFrom, current));
}
openSet.Remove(current);
foreach (var neighbour in mapHandler.GetNeighboursOf(current))
{
//Here GetDistanceBetween between is used as the actual connection weigth
float tentativeGScore = gScore[current] + mapHandler.GetDistanceBetween(current, neighbour);
if (!gScore.ContainsKey(neighbour) || tentativeGScore < gScore[neighbour])
{
cameFrom[neighbour] = current;
gScore[neighbour] = tentativeGScore;
fScore[neighbour] = tentativeGScore + heuristic(neighbour, goal);
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
}
}
}
return(null);
}
public TimeSlicedAStarSearch(Node source, Node destination, HeuristicDelegate heuristic)
: base(TimeSlicedSearchTypes.AStar, source)
{
H = heuristic;
_priorityQueue =
new IntervalHeap <ScoredNode>(
new DelegateComparer <ScoredNode>(
(s1, s2) => s1.f.CompareTo(s2.f)));
const float g = 0;
var h = H(source, destination);
_priorityQueue.Add(new ScoredNode(source, g + h, g, null, null));
Destination = destination;
}
public Stack <int[]> run(byte[,] obstacles, int[] start, int[] end, AlgoDelegate alg, HeuristicDelegate h, BackgroundWorker bgw)
{
this.h = h;
this.end = end;
this.obstacles = obstacles;
for (int i = 0; i < obstacles.GetLength(0); i++)
{
obstacles[i, 0] = 1;
obstacles[i, obstacles.GetLength(1) - 1] = 1;
}
for (int i = 0; i < obstacles.GetLength(1); i++)
{
obstacles[0, i] = 1;
obstacles[obstacles.GetLength(0) - 1, i] = 1;
}
open.Add(new Node(start, h(start)));
if (obstacles[open[0].Pos[0], open[0].Pos[1]] == 1 || obstacles[end[0], end[1]] == 1)
{
open.RemoveAt(0);
}
while (open.Count() != 0 && !done)
{
bgw.ReportProgress(0, open[0].Pos);
closed.Add(open[0]);
open.RemoveAt(0);
alg(closed[closed.Count() - 1]);
}
var pathStack = new Stack <int[]>();
if (!done)
{
pathStack.Push(new int[] { -1, -1 });
}
else
{
while (pathNode.Parent != pathNode)
{
pathStack.Push(pathNode.Pos);
pathNode = pathNode.Parent;
}
pathStack.Push(pathNode.Pos);
}
return(pathStack);
}
public void SetHeuristic(HeuristicMode iMode)
{
m_heuristic = null;
switch (iMode)
{
case HeuristicMode.MANHATTAN:
m_heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
m_heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
}
public void SetHeuristic(HeuristicMode iMode)
{
MHeuristic = null;
switch (iMode)
{
case HeuristicMode.Manhattan:
MHeuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.Euclidean:
MHeuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.Chebyshev:
MHeuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
MHeuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
}
// Returns a sequence of block movements
public static List <Tuple2 <Tuple3 <int> > > AStar(Maze m, HeuristicMode hm)
{
HeuristicDelegate heuristic = null;
switch (hm)
{
case HeuristicMode.Misplaced:
heuristic = new HeuristicDelegate(Heuristic.Misplaced);
break;
case HeuristicMode.MisplacedManhattan:
heuristic = new HeuristicDelegate(Heuristic.MisplacedManhattan);
break;
}
// Set up current and target maps
Block[, ,] currentMaze = m.GetBlockMaze();
bool[, ,] currentMap = new bool[currentMaze.GetLength(0), currentMaze.GetLength(1), currentMaze.GetLength(2)];
bool[, ,] targetMap = m.GetBlockMap();
for (int i = 0; i < currentMap.GetLength(0); ++i)
{
for (int j = 0; j < currentMap.GetLength(1); ++j)
{
for (int k = 0; k < currentMap.GetLength(2); ++k)
{
// If it's a block, check if there is no block in target
if (currentMaze [i, j, k] == null)
{
currentMap [i, j, k] = true;
}
}
}
}
// Set up leaves datastruct
SortedDictionary <int, List <TreeNode <bool[, , ]> > > leaves = new SortedDictionary <int, List <TreeNode <bool[, , ]> > >();
leaves.Add(heuristic(currentMap, targetMap), new List <TreeNode <bool[, , ]> > {
new TreeNode <bool[, , ]> (currentMap)
});
// Perform A*
while (leaves.Count > 0)
{
// Pop lowest key off leaves, q
KeyValuePair <int, List <TreeNode <bool[, , ]> > > kvp = leaves.First();
int qKey = kvp.Key;
TreeNode <bool[, , ]> node = kvp.Value[0];
leaves [qKey].RemoveAt(0);
// If that key# is empty, remove
if (leaves [qKey].Count == 0)
{
leaves.Remove(qKey);
}
// Check if goal is reached
if (Util.MazeMapEquals(node.value, targetMap))
{
// Generate path of moves and return
List <Tuple2 <Tuple3 <int> > > moves = new List <Tuple2 <Tuple3 <int> > >();
while (node.parent != null)
{
Tuple3 <int> from = node.changeFrom;
Tuple3 <int> to = node.changeTo;
Tuple2 <Tuple3 <int> > move = new Tuple2 <Tuple3 <int> > (from, to);
moves.Add(move);
node = node.parent;
}
return(moves);
}
// Generate q's children
int childrenCount = 0;
for (int x = 0; x < node.value.GetLength(0); ++x)
{
for (int y = 0; y < node.value.GetLength(1); ++y)
{
for (int z = 0; z < node.value.GetLength(2); ++z)
{
// If it's a block, check for spaces around it
if (!node.value [x, y, z])
{
List <Tuple3 <int> > children = m.GetSpaceNeighbours(x, y, z, node.value);
// Attach parent to children treenode and add to leaves while calculating their heuristic value
for (int i = 0; i < children.Count; ++i)
{
// Get childmap by swapping the block with the space
bool[, ,] childMap = Util.CopyMap(node.value);
childMap [x, y, z] = true;
childMap [children [i].first, children [i].second, children [i].third] = false;
// Attach parent to children and add to leaves with heuristic
Tuple3 <int> changeFrom = new Tuple3 <int>(x, y, z);
Tuple3 <int> changeTo = new Tuple3 <int>(children [i].first, children [i].second, children [i].third);
TreeNode <bool[, , ]> child = new TreeNode <bool[, , ]> (childMap, node, changeFrom, changeTo);
int heuristicValue = heuristic(child.value, targetMap) + child.level;
if (leaves.ContainsKey(heuristicValue))
{
leaves [heuristicValue].Add(child);
}
else
{
leaves.Add(heuristicValue, new List <TreeNode <bool[, , ]> > {
child
});
}
++childrenCount;
}
}
}
}
}
Console.Write(childrenCount);
}
return(null);
}
private void Search(HeuristicDelegate calculate)
{
NodesSearched = 0; // used for performance measuring
// create an indexed priority queue of nodes. The nodes with the
// lowest overall F cost (G+H) are positioned at the front.
var pq = new IndexedPriorityQueueLow(FCosts, Graph.NumNodes);
// put the source node on the queue
pq.Insert(Source);
// while the queue is not empty
while (!pq.Empty())
{
// get lowest cost node from the queue
int nextClosestNode = pq.Pop();
NodesSearched++;
// move this node from the frontier to the spanning tree
ShortestPathTree[nextClosestNode] =
SearchFrontier[nextClosestNode];
// if the target has been found exit
if (nextClosestNode == Target)
{
return;
}
// now to test all the edges attached to this node
foreach (Edge curEdge in Graph.Edges[nextClosestNode])
{
// calculate (H) the heuristic cost from this node to
// the target
float hCost = calculate(Graph, Target, curEdge.To);
// calculate (G) the 'real' cost to this node from the source
float gCost = GCosts[nextClosestNode] + curEdge.Cost;
// if the node has not been added to the frontier, add it and
// update the G and F costs
if (SearchFrontier[curEdge.To] == null)
{
FCosts[curEdge.To] = gCost + hCost;
GCosts[curEdge.To] = gCost;
pq.Insert(curEdge.To);
SearchFrontier[curEdge.To] = curEdge;
}
// if this node is already on the frontier but the cost to
// get here is cheaper than has been found previously, update
// the node costs and frontier accordingly.
else if ((gCost < GCosts[curEdge.To]) &&
(ShortestPathTree[curEdge.To] == null))
{
FCosts[curEdge.To] = gCost + hCost;
GCosts[curEdge.To] = gCost;
pq.ChangePriority(curEdge.To);
SearchFrontier[curEdge.To] = curEdge;
}
}
}
}
// All sorting related data is immutable.
public static Path <T> Solve <T>(T start, T goal, NeighbourDelegate <T> neighboursFunction,
CostDelegate <T> costFunction,
HeuristicDelegate <T> heuristicFunction)
where T : ISatellite
{
var nodeMap = new Dictionary <T, Node <T> >();
var priorityQueue = new PriorityQueue <Node <T> >();
var nStart = new Node <T>(start, 0, heuristicFunction.Invoke(start, goal), null, false);
nodeMap[start] = nStart;
priorityQueue.Enqueue(nStart);
float cost = 0;
while (priorityQueue.Count > 0)
{
Node <T> current = priorityQueue.Dequeue();
if (current.Closed)
{
continue;
}
current.Closed = true;
if (current.Item.Equals(goal))
{
// Return path and cost
var reversePath = new List <T>();
for (Node <T> node = current; node != null; node = node.From)
{
reversePath.Add(node.Item);
cost += node.Cost;
}
reversePath.Reverse();
return(new Path <T>(reversePath, cost));
}
foreach (T item in neighboursFunction.Invoke(current.Item))
{
float new_cost = current.Cost + costFunction.Invoke(current.Item, item);
// If the item has a node, it will either be in the closedSet, or the openSet
if (nodeMap.ContainsKey(item))
{
Node <T> n = nodeMap[item];
if (new_cost <= n.Cost)
{
// Cost via current is better than the old one, discard old node, queue new one.
var new_node = new Node <T>(n.Item, new_cost, n.Heuristic, current, false);
n.Closed = true;
nodeMap[item] = new_node;
priorityQueue.Enqueue(new_node);
}
}
else
{
// It is not in the openSet, create a node and add it
var new_node = new Node <T>(item, new_cost,
heuristicFunction.Invoke(item, goal), current,
false);
priorityQueue.Enqueue(new_node);
nodeMap[item] = new_node;
}
}
}
return(Path.Empty <T>(start));
}
public void SetHeuristic(HeuristicMode iMode)
{
heuristic = null;
switch (iMode)
{
case HeuristicMode.MANHATTAN:
heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
}
public void SetHeuristic(HeuristicMode iMode)
{
m_heuristic = null;
m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
}
public static List <TVertex> TryFindShortestPath <TVertex>(TVertex start, TVertex finish, HeuristicDelegate <TVertex> heuristic)
where TVertex : class, IWeightedGraphVertex <TVertex, float>
{
if (start == null || finish == null)
{
return(null);
}
// Initialize collections.
int unusedIndex = 0;
var reports = new Dictionary <TVertex, Report <TVertex> >();
var toFinalize = new SortedSet <Report <TVertex> >();
var startReport = new Report <TVertex> {
Vertex = start,
Index = unusedIndex++,
Traversed = 0,
Heuristic = heuristic(start),
Predecessor = null
};
reports[start] = startReport;
toFinalize.Add(startReport);
// Build partial paths.
while (toFinalize.Count > 0)
{
// Find free vertex with least cost.
var currentReport = toFinalize.Min;
if (currentReport.Cost == float.PositiveInfinity)
{
break;
}
var currentVertex = currentReport.Vertex;
toFinalize.Remove(currentReport);
// Update costs of adjacent vertices.
currentReport.Finalized = true;
if (currentVertex == finish)
{
break;
}
foreach (var vertexAndWeight in currentVertex.Adjacency)
{
var otherVertex = vertexAndWeight.Key;
float traversed = currentReport.Traversed + vertexAndWeight.Value;
if (!reports.TryGetValue(otherVertex, out var otherReport))
{
otherReport = new Report <TVertex>()
{
Vertex = otherVertex,
Index = unusedIndex++,
Traversed = traversed,
Heuristic = heuristic(otherVertex),
Predecessor = currentVertex
};
reports[otherVertex] = otherReport;
toFinalize.Add(otherReport);
continue;
}
if (otherReport.Finalized)
{
continue;
}
if (traversed < otherReport.Traversed)
{
toFinalize.Remove(otherReport);
otherReport.Traversed = traversed;
otherReport.Predecessor = currentVertex;
toFinalize.Add(otherReport);
}
}
}
// Return resulting path.
if (!reports.ContainsKey(finish))
{
return(null);
}
var path = new List <TVertex>();
var pathVertex = finish;
path.Add(pathVertex);
while (reports[pathVertex].Predecessor != null)
{
pathVertex = reports[pathVertex].Predecessor;
path.Add(pathVertex);
}
return(path);
}
/*
private class NodeComparer : IComparer<Node>
{
public int Compare(Node x, Node y)
{
var result = (x.heuristicStartToEndLen - y.heuristicStartToEndLen);
if (result < 0) return -1;
else
if (result > 0) return 1;
else
{
return 0;
}
}
}
*/
public static List<GridPos> FindPath(AStarParam iParam)
{
var lo = new object();
//var openList = new IntervalHeap<Node>(new NodeComparer());
var openList = new IntervalHeap<Node>();
Node startNode = iParam.StartNode;
Node endNode = iParam.EndNode;
HeuristicDelegate heuristic = iParam.HeuristicFunc;
BaseGrid grid = iParam.SearchGrid;
DiagonalMovement diagonalMovement = iParam.DiagonalMovement;
float weight = iParam.Weight;
startNode.StartToCurNodeLen = 0;
startNode.HeuristicStartToEndLen = 0;
openList.Add(startNode);
startNode.IsOpened = true;
while (openList.Count != 0)
{
Node node = openList.DeleteMin();
node.IsClosed = true;
if (node == endNode)
{
return Node.Backtrace(endNode);
}
List<Node> neighbors = grid.GetNeighbors(node, diagonalMovement);
#if (UNITY)
foreach(var neighbor in neighbors)
#else
Parallel.ForEach(neighbors, neighbor =>
#endif
{
#if (UNITY)
if (neighbor.isClosed) continue;
#else
if (neighbor.IsClosed) return;
#endif
int x = neighbor.X;
int y = neighbor.Y;
float ng = node.StartToCurNodeLen + (float)((x - node.X == 0 || y - node.Y == 0) ? 1 : Math.Sqrt(2));
if (!neighbor.IsOpened || ng < neighbor.StartToCurNodeLen)
{
neighbor.StartToCurNodeLen = ng;
if (neighbor.HeuristicCurNodeToEndLen == null) neighbor.HeuristicCurNodeToEndLen = weight * heuristic(Math.Abs(x - endNode.X), Math.Abs(y - endNode.Y));
neighbor.HeuristicStartToEndLen = neighbor.StartToCurNodeLen + neighbor.HeuristicCurNodeToEndLen.Value;
neighbor.Parent = node;
if (!neighbor.IsOpened)
{
lock (lo)
{
openList.Add(neighbor);
}
neighbor.IsOpened = true;
}
else
{
}
}
}
#if (!UNITY)
);
#endif
}
return new List<GridPos>();
}
public JumpPointParam(JumpPointParam b)
{
m_heuristic = b.m_heuristic;
m_allowEndNodeUnWalkable = b.m_allowEndNodeUnWalkable;
m_crossAdjacentPoint = b.m_crossAdjacentPoint;
m_crossCorner = b.m_crossCorner;
openList = new List<Node>(b.openList);
m_searchGrid = b.m_searchGrid;
m_startNode = b.m_startNode;
m_endNode = b.m_endNode;
m_useRecursive = b.m_useRecursive;
}
public AStar(HeuristicMethod method)
{
_heuristicMethod = method == HeuristicMethod.Manhatten
? (HeuristicDelegate)Heuristic.Manhatten
: (HeuristicDelegate)Heuristic.Euclidean;
}
public JumpPointParam(
IBaseGrid iGrid,
bool iAllowEndNodeUnWalkable = true,
bool iCrossCorner = true,
bool iCrossAdjacentPoint = true,
HeuristicMode iMode = HeuristicMode.EUCLIDEAN)
{
switch (iMode)
{
case HeuristicMode.MANHATTAN:
this.m_heuristic = new HeuristicDelegate(Heuristic.Manhattan);
break;
case HeuristicMode.EUCLIDEAN:
this.m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
case HeuristicMode.CHEBYSHEV:
this.m_heuristic = new HeuristicDelegate(Heuristic.Chebyshev);
break;
default:
this.m_heuristic = new HeuristicDelegate(Heuristic.Euclidean);
break;
}
this.m_allowEndNodeUnWalkable = iAllowEndNodeUnWalkable;
this.m_crossAdjacentPoint = iCrossAdjacentPoint;
this.m_crossCorner = iCrossCorner;
this.openList = new List<Node>();
this.m_searchGrid = iGrid;
this.m_startNode = null;
this.m_endNode = null;
this.m_useRecursive = false;
}
private void Search(HeuristicDelegate calculate)
{
NodesSearched = 0; // used for performance measuring
// create an indexed priority queue of nodes. The nodes with the
// lowest overall F cost (G+H) are positioned at the front.
var pq = new IndexedPriorityQueueLow(FCosts, Graph.NumNodes);
// put the source node on the queue
pq.Insert(Source);
// while the queue is not empty
while (!pq.Empty())
{
// get lowest cost node from the queue
int nextClosestNode = pq.Pop();
NodesSearched++;
// move this node from the frontier to the spanning tree
ShortestPathTree[nextClosestNode] =
SearchFrontier[nextClosestNode];
// if the target has been found exit
if (nextClosestNode == Target)
{
return;
}
// now to test all the edges attached to this node
foreach (Edge curEdge in Graph.Edges[nextClosestNode])
{
// calculate (H) the heuristic cost from this node to
// the target
float hCost = calculate(Graph, Target, curEdge.To);
// calculate (G) the 'real' cost to this node from the source
float gCost = GCosts[nextClosestNode] + curEdge.Cost;
// if the node has not been added to the frontier, add it and
// update the G and F costs
if (SearchFrontier[curEdge.To] == null)
{
FCosts[curEdge.To] = gCost + hCost;
GCosts[curEdge.To] = gCost;
pq.Insert(curEdge.To);
SearchFrontier[curEdge.To] = curEdge;
}
// if this node is already on the frontier but the cost to
// get here is cheaper than has been found previously, update
// the node costs and frontier accordingly.
else if ((gCost < GCosts[curEdge.To]) &&
(ShortestPathTree[curEdge.To] == null))
{
FCosts[curEdge.To] = gCost + hCost;
GCosts[curEdge.To] = gCost;
pq.ChangePriority(curEdge.To);
SearchFrontier[curEdge.To] = curEdge;
}
}
}
}
public bool Search(
IStateTransition transition,
IState initialState,
IState goalState,
HeuristicDelegate heuristic,
out Node result, double initialCost=0)
{
result = null;
var fringe = new BinaryHeap<SearchNode>((x, y) => y.Value > x.Value);
fringe.Insert(new SearchNode()
{
f = heuristic(initialState, goalState),
g = initialCost,
State = initialState
});
var visitedNodes = new Dictionary<IState, SearchNode>();
visitedNodes.Add(initialState, new SearchNode()
{
f = heuristic(initialState,
goalState),
g = initialCost,
State = initialState
});
var history = new Dictionary<IState, Node>();
history.Add(initialState, new Node() { State = initialState });
while (true)
{
if (fringe.Count == 0)
{
return false;
}
var node = fringe.Remove();
if (transition.IsGoal(node.State, goalState))
{
result = history[goalState];
return true;
}
foreach (var x in transition.Expand(node.State))
{
var state = x.Item1;
var _g = x.Item3 + node.g;
var _f = _g + heuristic(state, goalState);
if (transition.IsGoal(state, goalState))
_f = _g;
var action = x.Item2;
SearchNode visitedNode;
Node hnode;
if (visitedNodes.TryGetValue(state, out visitedNode))
{
if (visitedNode.g < _g)
hnode = history[state];
else
continue;
}
else
{
visitedNode = new SearchNode();
visitedNodes.Add(state, visitedNode);
hnode = new Node();
history.Add(state, hnode);
}
visitedNode.f = _f;
visitedNode.g = _g;
visitedNode.State = state;
fringe.Insert(visitedNode);
hnode.Previous = history[node.State];
hnode.Cost = _g;
hnode.PreviousAction = action;
hnode.State = state;
}
}
}
