/// <summary>
/// 选择F值最小的，作为ActiveNode
/// </summary>
/// <returns>Continue or not,true continue, false path found!</returns>
bool SelectMinF()
{
IAStarNode new_active_node = open_list[0];
if (open_list.Count > 1)
{
for (int i = 1; i < open_list.Count; i++)
{
if (new_active_node.F > open_list[i].F)
{
new_active_node = open_list[i];
}
}
}
//删除将要返回的node。
open_list.Remove(new_active_node);
close_list.Add(new_active_node);
active_node = new_active_node;
if (active_node == destination)
{
return(false);
}
return(true);
}
public void AddNeighbour(IAStarNode node)
{
if (!_neighbours.Contains(node))
{
_neighbours.Add(node);
}
}
// Simple Manhatten distance heuristic since we want to tend toward 'squared' paths.
private static float CalculateH(IAStarNode source, IAStarNode target)
{
Vector2 sourcePos = source.coordinates;
Vector2 targetPos = target.coordinates;
return(Mathf.Abs(targetPos.x - sourcePos.x) + Mathf.Abs(targetPos.y - sourcePos.y));
}
/// <summary>
/// Create a maze solver.
/// </summary>
/// <param name="_startingNode">Starting node.</param>
/// <param name="_IsSleepBetweenLoopsEnabled">Func to check if solver should sleep (100 ms)
/// between loops. Return true to sleep between loops.</param>
public AStarSolver(IAStarNode _startingNode, Func <bool> _IsSleepBetweenLoopsEnabled)
{
startingNode = _startingNode;
IsSleepBetweenLoopsEnabled = _IsSleepBetweenLoopsEnabled;
SleepTimeInMs = DEFAULT_SLEEP_TIME_MS;
}
/// <summary>
/// Find a solution for a maze. (Template Method Design Pattern).
/// </summary>
/// <returns>An ordered list (first to last) of nodes to be visited to reach a solution;
/// if interrupted, returns an empty list.</returns>
public virtual IAStarNode[] FindSolution()
{
Stopwatch watch;
ElapsedTimeInMs = 0;
openList = new List <IAStarNode>();
closedList = new List <IAStarNode>();
InitializeOpenList();
IAStarNode nextNode;
do
{
watch = Stopwatch.StartNew();
nextNode = FindLightestNode();
closedList.Add(nextNode);
CurrNode = nextNode;
ReportProgress();
if (IsSleepBetweenLoopsEnabled())
{
Thread.Sleep(SleepTimeInMs);
}
if (HasFoundASolution())
{
break;
}
RefreshOpenList();
IAStarNode[] neighbors = FindNeighbors();
openList.AddRange(neighbors);
watch.Stop();
ElapsedTimeInMs += watch.ElapsedMilliseconds;
if (IsCancelationPending())
{
break;
}
} while (openList.Count > 0);
IAStarNode[] solution = null;
if (HasFoundASolution() || (IsCancelationPending() != true) && (openList.Count > 0))
{
solution = BuildSolutionArray();
}
else
{
solution = new IAStarNode[0];
}
return(solution);
}
public float CalculateMoveCost(IAStarNode node)
{
//接続タイプによってはNGを出す
//MapPathNode node2 = node as MapPathNode;
return(1);
}
private AStarWorkingNode CreateWorkingNode(IAStarNode self, IAStarNode goal, AStarWorkingNode parent, float gScore)
{
return(new AStarWorkingNode(self)
{
GScore = gScore,
HScore = Heuristic(self, goal),
Parent = parent,
});
}
/// <summary>
/// 尝试更新最短距离
/// </summary>
/// <param name="g"></param>
/// <param name="h"></param>
/// <param name="trySetNode"></param>
/// <returns></returns>
public float TryUpdateFCost(float g, float h, IAStarNode trySetNode)
{
if (g + h < f)
{
this._g = g;
this._h = h;
this._parentNode = trySetNode;
}
return(f);
}
/// <summary>
/// 尝试更新最短距离
/// </summary>
/// <param name="g"></param>
/// <param name="h"></param>
/// <param name="trySetNode"></param>
/// <returns></returns>
public float TryUpdateFCost(float g, float h, IAStarNode trySetNode)
{
if (g + h < F)
{
this.g = g;
this.h = h;
this.parentNode = trySetNode;
}
return(F);
}
void WarmUp(IAStarNode start_node, IAStarNode end_node)
{
start = start_node;
destination = end_node;
open_list = new List <IAStarNode>();
close_list = new List <IAStarNode>();
path = new List <IAStarNode>();
//第一个节点。加入Open表。
AddToOpenList(start, null);//父节点为Null，代表的就是起始节点！
}
private BaseAction[] CreateActions(IAStarNode node)
{
var actions = new List <BaseAction>();
var actionNode = node as ActionNode; // TODO TODO this doesn't seems right. Binding to solver!
if (actionNode is ClimbLadder)
{
// use the interaction creator!
}
return(null);
}
public float CostTo(IAStarNode neighbour)
{
IConfigurableAstarNode node = (IConfigurableAstarNode)neighbour;
if (node != null)
{
return(node.Cost);
}
else
{
throw new System.Exception("Can't cast IAstarNode");
}
}
void Update()
{
// if left button pressed
if (Input.GetMouseButtonDown(0))
{
// Ray emerging from the main camera
Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit hit;
if (Physics.Raycast(ray, out hit))
{
int numHex = 0;
// Taking the numbers of "Hex12" => 12
if (int.TryParse(hit.collider.gameObject.name.Substring(3, hit.collider.gameObject.name.Length - 3), out numHex))
{
if ((start == null) && (((AStarNode)allNodes[numHex]).Cost != int.MaxValue))
{
deleteOldPathColors();
start = allNodes[numHex];
// Giving start a green color
rend = ((AStarNode)allNodes[numHex]).Hex.GetComponent <Renderer>();
rend.material.color = Color.green;
}
else if ((goal == null) && (((AStarNode)allNodes[numHex]).Cost != int.MaxValue))
{
goal = allNodes[numHex];
// Giving goal a green color
rend = ((AStarNode)allNodes[numHex]).Hex.GetComponent <Renderer>();
rend.material.color = Color.green;
path = (List <IAStarNode>)AStar.GetPath(start, goal);
// Giving the path a red color
foreach (IAStarNode node in path)
{
if ((path.IndexOf(node) != 0) && (path.IndexOf(node) != path.Count - 1))
{
rend = ((AStarNode)node).Hex.GetComponent <Renderer>();
rend.material.color = Color.red;
}
}
// Reinit Start and Goal for the next Use
start = null;
goal = null;
}
}
}
}
}
public AStarSearch(IAStarNode start, IAStarNode end, calcSpecialNodeCostFunc calcspecialcostfunc, getAdjacnentNodesFunc getadjacentnodesfunc, isOpenRouteFunc isopenroutefunc,calcScoreHeuristicFunc calcscoreheuristaic)
{
//Debug.Log("Init AStar!!");
SpecialNodeCostFunc = calcspecialcostfunc != null ? calcspecialcostfunc : this.defaultSpecialNodeCost;
//Debug.Log(string.Format("isNull: {0}:{1}",SpecialNodeCostFunc !=null,calcspecialcostfunc !=null));
GetAdjacentNodesFunc = getadjacentnodesfunc != null? getadjacentnodesfunc : this.getAdjacentNodes;
IsOpenRouteFunc = isopenroutefunc != null? isopenroutefunc : this.isOpenRoute;
CalcScoreHeuristic = calcscoreheuristaic != null ? calcscoreheuristaic : this.defaultCalcScoreHeuristic;
EndNode = end;
start.Parent = Dummy;
start.Score = calcScore(start);
StartNode = start;
init();
search();
}
// Using Manhattan distance
public float EstimatedCostTo(IAStarNode goal)
{
AStarNode aStarNodeGoal = (AStarNode)goal;
float dx = aStarNodeGoal.Position.x - this.Position.x;
float dy = aStarNodeGoal.Position.y - this.Position.y;
if (Math.Sign(dx) == Math.Sign(dy))
{
return(Math.Abs(dx + dy));
}
else
{
return(Math.Max(Math.Abs(dx), Math.Abs(dy)));
}
}
public float EstimatedCostTo(IAStarNode target)
{
IConfigurableAstarNode node = (IConfigurableAstarNode)target;
if (node != null)
{
Vector2 origin = new Vector2(transform.position.x, transform.position.z);
Vector2 destination = new Vector2(node.Transform.position.x, node.Transform.position.z);;
return(Utility.PythagoreanDistance(origin, destination));
}
else
{
throw new System.Exception("Can't cast IAstarNode");
}
}
/// <summary>
/// Find the node with best heuristic and retrieve it from the list.
/// (will remove its reference from the list).
/// </summary>
/// <returns>The node with best heuristics in the list.</returns>
IAStarNode FindLightestNode()
{
IAStarNode bestNode = openList[0];
for (int i = 1; i < openList.Count; i++)
{
IAStarNode currNode = openList[i];
if (currNode.TotalCost < bestNode.TotalCost)
{
bestNode = currNode;
}
}
openList.Remove(bestNode);
return(bestNode);
}
/// <summary>
/// Retrieve all nodes of a solution starting from the last node. Reverse the solution so earlier
/// nodes are in the starting positions. (Hook Operation)
/// </summary>
/// <returns>The nodes that compose the solution.</returns>
protected virtual IAStarNode[] BuildSolutionArray()
{
List <IAStarNode> solution = new List <IAStarNode>();
IAStarNode nextNode = CurrNode;
// Create a method for this.
while (nextNode != null && nextNode.PrevNode != null)
{
solution.Add(nextNode);
nextNode = nextNode.PrevNode;
}
solution.Reverse();
return(solution.ToArray());
}
public List <IAStarNode> NeighborNodes(IAStarNode current)
{
List <IAStarNode> neighbors = new List <IAStarNode>();
TileCell northCell = GetCellCoordinateInDirection(current.X, current.Y, CardinalDirection.North);
TileCell eastCell = GetCellCoordinateInDirection(current.X, current.Y, CardinalDirection.East);
TileCell southCell = GetCellCoordinateInDirection(current.X, current.Y, CardinalDirection.South);
TileCell westCell = GetCellCoordinateInDirection(current.X, current.Y, CardinalDirection.West);
if (northCell != null && northCell.IsVoid)
{
northCell = null;
}
if (eastCell != null && eastCell.IsVoid)
{
eastCell = null;
}
if (southCell != null && southCell.IsVoid)
{
southCell = null;
}
if (westCell != null && westCell.IsVoid)
{
westCell = null;
}
if (northCell != null)
{
neighbors.Add(northCell);
}
if (eastCell != null)
{
neighbors.Add(eastCell);
}
if (southCell != null)
{
neighbors.Add(southCell);
}
if (westCell != null)
{
neighbors.Add(westCell);
}
return(neighbors);
}
private IAStarNode ReturnINodeInOpenSetWithLowestFScore(HashSet <IAStarNode> openSet, Dictionary <IAStarNode, Double> fScore)
{
IAStarNode lowest = null;
Double smallestFScore = Double.MaxValue;
foreach (IAStarNode node in openSet)
{
Double fScoreForNode = fScore[node];
if (fScoreForNode < smallestFScore)
{
lowest = node;
smallestFScore = fScoreForNode;
}
}
return(lowest);
}
private List<IAStarNode> getAdjacentNodes(IAStarNode node)
{
List<IAStarNode> ret = new List<IAStarNode>();
Vector2 pos = node.Position;
for (int i = 1; i < MyCharacterController.directionDelta.GetLength(0); i++) {
int dx = MyCharacterController.directionDelta[i, 0];
int dy = MyCharacterController.directionDelta[i, 1];
if (dx == 0 && dy == 0) continue;
Vector2 targetpos = pos + new Vector2(dx, dy);
Collider2D road = Physics2D.OverlapPoint(targetpos, TagList.getLayerMask(TagList.Road));//.gameObject.GetComponent<TileEntity>();
Collider2D wall = Physics2D.OverlapPoint(targetpos, TagList.getLayerMask(TagList.Wall));
TileEntity target = null;
if (wall != null)
{
target = wall.GetComponent<TileEntity>();
}
else if (road != null) {
target = road.GetComponent<TileEntity>();
}
//if(c2d!=null)Debug.Log("C2D: "+c2d+", "+c2d.transform.position);
if (target != null) ret.Add(target);
}
return ret;
}
private void search()
{
IAStarNode current= null;
while (openlist.Count != 0) {
openlist.Sort();
current = openlist[0];
if (current == EndNode)
{
IAStarNode[] list = getNodeTree(current).ToArray();
//Debug.LogFormat("C:{0},F:{1},L:{2}",list.Length,list[0].Position,list[list.Length-1].Position);
Route = list;
//Debug.logger.Log("Route",showNodes(list));
//Debug.logger.Log("Close", showNodes(closedlist.ToArray()));
return;
}
else {
closedlist.Add(current);
openlist.Remove(current);
// 隣接ノードの収集
List<IAStarNode> adjacents = GetAdjacentNodesFunc(current); //getAdjacentNodes(current);
foreach (IAStarNode adjacent in adjacents) {
bool isopen = !openlist.Contains(adjacent) && !closedlist.Contains(adjacent) && IsOpenRouteFunc(adjacent);
//Debug.LogFormat("Adj:{0},{2} Open: {1}",adjacent.Position,isopen,adjacent.Score);
if (isopen) {
adjacent.Parent = current;
adjacent.Score = calcScore(adjacent);
//Debug.LogFormat("N:{0}, Score:{1}",adjacent.Position,adjacent.Score);
if(adjacent.Score < IgnoreCost) openlist.Add(adjacent);
}
}
}
}
if (current != null)
{
//GameController.PlayingLogger.addLog(current.Position.ToString());
Route = getNodeTree(current).ToArray();
}
else {
Route = new IAStarNode[] { };
}
}
/// <summary>
/// 设置默认父节点
/// </summary>
/// <param name="node"></param>
public void SetDefaultParent(IAStarNode node)
{
this.parentNode = node;
}
public Planner(IAStarNode _startingNode, GameManager game) : base(_startingNode, () => false)
{
this.game = game;
ActionNode.SetGameManager(game);
unresolvedConditions = new Queue <EffectType>();
}
public int CompareTo(IAStarNode other)
{
if (this.Score > other.Score)
{
return 1;
}
else if (this.Score < other.Score)
{
return -1;
}
return 0;
}
public void routeRenew(IAStarNode newEndNode)
{
EndNode = newEndNode;
init();
search();
}
public string showNodes(IAStarNode[] nodes)
{
StringBuilder stb = new StringBuilder();
foreach (var n in nodes) {
stb.AppendLine(string.Format("Pos:{0}, Score:{1}", n.Position, n.Score));
}
return stb.ToString();
}
public AStarSearch(IAStarNode start, IAStarNode end,calcSpecialNodeCostFunc calcspecialcostfunc)
: this(start,end,calcspecialcostfunc,null)
{
//GetAdjacentNodesFunc = getAdjacentNodes;
}
public AStarSearch(IAStarNode start, IAStarNode end, calcSpecialNodeCostFunc calcspecialcostfunc, getAdjacnentNodesFunc getadjacentnodesfunc)
: this(start,end,calcspecialcostfunc,getadjacentnodesfunc,null,null)
{
//IsOpenRouteFunc = isOpenRoute;
}
public AStarSearch(IAStarNode start, IAStarNode end, getAdjacnentNodesFunc getadjacentnodesfunc,isOpenRouteFunc isopenroutefunc)
: this(start, end, node => { return 0; }, getadjacentnodesfunc,isopenroutefunc,null)
{
}
public AStarSearch(IAStarNode start, IAStarNode end,isOpenRouteFunc isopenroutefunc)
: this(start, end, node => { return 0; }, null, isopenroutefunc,null)
{
//GetAdjacentNodesFunc = getAdjacentNodes;
}
public AStarSearch(IAStarNode start, IAStarNode end,getAdjacnentNodesFunc getadjacentnodesfunc)
: this(start, end, node => { return 0; },getadjacentnodesfunc)
{
}
public AStarResult Solve(IAStarNode start, IAStarNode end)
{
Reset();
// Step 1 - Add The Starting Node To The Open List
_open.Enqueue(CreateWorkingNode(start, end, null, 0));
AStarWorkingNode endNode = null;
while (_open.Count > 0)
{
// Step 2:
// Take The Best Node From The Open List
AStarWorkingNode currentNode = _open.Dequeue();
// Step 3:
// Add All Connections To The Open List
foreach (IAStarNodeConnection connection in currentNode.Self.Connections)
{
var candidateNode = CreateWorkingNode(connection.Node, end, currentNode, currentNode.GScore + connection.Cost);
// but only if have havn't been previously closed
if (!_closed.Contains(candidateNode) && !_open.Contains(candidateNode))
{
_open.Enqueue(candidateNode);
}
// If it has been previously closed; check to see if this is better
else if (_closed.Contains(candidateNode))
{
AStarWorkingNode closedNode = _closed.FirstOrDefault(x => x.Self == candidateNode.Self);
if (candidateNode.FScore < closedNode.FScore)
{
closedNode.Parent = candidateNode.Parent;
closedNode.GScore = candidateNode.GScore;
}
}
}
// then close the current node
_closed.Add(currentNode);
// Step 3b
// If We Are At The End, Exit Search Earily
if (currentNode.Self == end)
{
endNode = currentNode;
break;
}
}
// Step 4
// Ready The Result Object Check For Path Success
// If We Didnt Succeed; Find The Best Candidate For The End Point
AStarResult result = new AStarResult();
result.IsSuccess = endNode != null;
if (!result.IsSuccess)
{
endNode = _closed
.OrderBy(x => x.HScore)
.FirstOrDefault();
}
// Step 5:
// Compile The Path
LinkedList <IAStarNode> resultPath = new LinkedList <IAStarNode>();
while (endNode.Parent != null)
{
resultPath.AddFirst(endNode.Self);
endNode = endNode.Parent;
}
resultPath.AddFirst(endNode.Self);
result.Result = resultPath;
return(result);
}
/// <summary>
/// Returns the best path from start to the goal, where the first entry in the list is the start, and the last is the goal.
/// Returns null if the goal is not reachable.
/// </summary>
public List <IAStarNode> ComputePath(IAStarMap map, IAStarNode start, IAStarNode goal)
{
List <IAStarNode> pathFromStartToGoal = ComputePaths(map, start, goal, 1)[0];
return(pathFromStartToGoal);
}
/// <summary>
/// 如果不可达，返回null
/// 如果可达，返回路径。
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <returns></returns>
public List <IAStarNode> GetPathTo(IAStarNode start, IAStarNode end)
{
//Debug.Log(" Start " + start + "end" + end);
WarmUp(start, end);
int dead_lock = 0;
while (open_list.Count > 0)
{
if (SelectMinF() == true)
{
IAStarNode[] neighours = active_node.Neibhours;
for (int i = 0; i < neighours.Length; i++)
{
if (neighours[i] != null && neighours[i].Walkable && close_list.Contains(neighours[i]) == false)
{
int new_G_weight = active_node.G + neighours[i].GWeightTo(active_node);
//如果邻居不在Open表中，那么直接将邻居加入Open表，并将active_node设置为其父节点。
if (open_list.Contains(neighours[i]) == false)
{
neighours[i].G = new_G_weight;
AddToOpenList(neighours[i], active_node);
}
else//
{
if (neighours[i].G < new_G_weight)
{
neighours[i].G = new_G_weight;
neighours[i].ParentNode = active_node;
}
}
}
}
}
else
{
int trace_path_lock = 0;
//找到了
while (active_node != start)
{
path.Add(active_node);
//Debug.Log(string.Format("Path add {0}-{1}", active_node.X, active_node.Y));
active_node = active_node.ParentNode;
trace_path_lock++;
if (trace_path_lock > 2000)
{
break;
}
}
path.Add(start);
//Debug.Log(string.Format("Path add {0}-{1}", start.X, start.Y));
path.Reverse();
return(path);
}
//外层死循环安全锁
dead_lock++;
if (dead_lock > 100000)
{
break;
}
}
return(null);//没找到
}
public bool reachableTo(IAStarNode dst,float movecost)
{
if (dst == Route[0]) return true;
if (movecost <= 0) return false;
if (Route.Length == 0) return false;
float total = getTotalCostFor(dst);
//Debug.Log(string.Format("TC: {0}, MC:{1}",total,movecost));
return total <= movecost;
}
/// <summary>
/// 加入Open表
/// 计算F值，一次就够了。
/// 添加ParentNode，一步完成，防止错误。
/// </summary>
/// <param name="node"></param>
/// <param name="parent_node"></param>
void AddToOpenList(IAStarNode node, IAStarNode parent_node)
{
open_list.Add(node);
node.CalculeteH(destination);
node.ParentNode = parent_node;
}
public static IAStarNode[] CalculatePath(IAStarNode source, IAStarNode target, out int directionChangeCount)
{
// Use the max value for an invalid path.
directionChangeCount = int.MaxValue;
if (source == null || target == null)
{
Debug.LogWarning("AStar.CalculatePath called with a null value. Returned path will be empty.");
return(new IAStarNode[0]);
}
_openList.Clear();
_closedList.Clear();
// Add the first step with default values
_openList.Add(new StepInfo(source, null));
do
{
// Always check the step in the open list with the lowest f score first. Save the index so we can use RemoveAt rather than traverse the list again.
int currentIndex;
StepInfo currentStep = GetLowestFScore(_openList, out currentIndex);
// We're done as soon as the target node has the lowest f score in the open list
if (currentStep.node == target)
{
// Subtract 1 from the turn count since the first move is always considered a change in direction.
directionChangeCount = currentStep.turns - 1;
return(ExtractPath(currentStep));
}
// Move the node from the open to the closed list
_openList.RemoveAt(currentIndex);
_closedList.Add(currentStep);
IAStarNode[] neighbours = currentStep.node.GetNeighbours();
foreach (IAStarNode n in neighbours)
{
StepInfo neighbour = new StepInfo(n, currentStep);
// Make sure to compare both the node and the direction in the closed list. We need to be able to add the same node to the open list multiple times
// from different directions in order to get the lowest number of changes in direction.
if (n.pathfindingWeight < 0 || _closedList.Find(x => x.node == neighbour.node && x.direction == neighbour.direction) != null)
{
continue;
}
neighbour.g = currentStep.g + n.pathfindingWeight;
neighbour.turns = currentStep.turns;
if (neighbour.direction != currentStep.direction)
{
// Add a severe penalty for changing directions to encourage the fewest possible turns
neighbour.g += 100;
neighbour.turns++;
}
neighbour.f = neighbour.g + CalculateH(n, target);
// Check if the node already exists in the open list. Again, make sure to check both node and direction
StepInfo existingOpenStep = _openList.Find(x => x.node == neighbour.node && x.direction == neighbour.direction);
if (existingOpenStep == null || existingOpenStep.direction != neighbour.direction)
{
// Add the new step to the open list if we didn't find it.
_openList.Add(neighbour);
}
else if (neighbour.g < existingOpenStep.g)
{
// Copy values into the existing step so we don't have to traverse the List again to remove it.
existingOpenStep.g = neighbour.g;
existingOpenStep.f = neighbour.f;
existingOpenStep.turns = neighbour.turns;
existingOpenStep.parent = neighbour.parent;
}
}
} while(_openList.Count > 0);
// Hit the end of the open list without finding a valid path
return(new IAStarNode[0]);
}
private void init()
{
openlist = new List<IAStarNode>();
closedlist = new List<IAStarNode>();
openlist.Add(StartNode);
Route = new IAStarNode[] { };
}
public AStarSearch(IAStarNode start, IAStarNode end)
: this(start,end,node=> { return 0; })
{
}
public StepInfo(IAStarNode node, StepInfo parent)
{
this.node = node;
this.parent = parent;
}
bool isOpenRoute(IAStarNode node)
{
var tile = node as TileEntity;
if (tile != null) {
return tile.IsTrafficAllowed(this.CharacterController);
}
bool ret = node.IsOpenRoute;
return ret;
}
/// <summary> /// Returns the estimated, heuristic movement cost needed to get /// from this node to the specified other one. /// </summary> /// <param name="target"> /// Target node. /// </param> /// <returns> /// Estimated movement cost. /// </returns> public abstract int EstimateHeuristicMovementCost(IAStarNode target);
float defaultCalcScoreHeuristic(IAStarNode node,float totalcost)
{
return 0;// Vector2.Distance(node.Position,EndNode.Position);
}
public static float getTotalCostFor(IAStarNode node)
{
float costsum = 0;
IAStarNode current = node;
while (current != Dummy) {
costsum += current.MoveCost;
current = current.Parent;
}
return costsum;
}
/// <summary>
/// 重置节点数据
/// </summary>
public void Reset()
{
g = h = float.MaxValue / 2;
parentNode = null;
}
/// <summary>
/// Returns the Manhattan distance from this tile to the target.
/// </summary>
/// <param name="target">Target to compute the distance to.</param>
/// <returns>Manhattan distance from this tile to the target.</returns>
public int EstimateHeuristicMovementCost(IAStarNode target)
{
var targetTile = (MapTile)target;
return Math.Abs(targetTile.Pos.X - this.Pos.X) + Math.Abs(targetTile.Pos.Y - this.Pos.Y);
}
bool isOpenRoute(IAStarNode node)
{
return node.IsOpenRoute;
}
public int CompareTo(IAStarNode other)
{
//Debug.Log("Compare");
return (int)(this.Score - other.Score);
}
float calcScore(IAStarNode node)
{
//Debug.LogFormat("isNull:{0}",SpecialNodeCostFunc == null);
float costsum = node.MoveCost + SpecialNodeCostFunc(node);
IAStarNode parent = node.Parent;
while (parent != Dummy) {
costsum += parent.Score;
parent = parent.Parent;
}
return costsum + CalcScoreHeuristic(node,costsum);
}
private static List<IAStarNode> getNodeTree(IAStarNode terminal)
{
List<IAStarNode> ret = new List<IAStarNode>();
ret.Add(terminal);
IAStarNode parent = terminal.Parent;
while (parent != Dummy) {
ret.Insert(0, parent);
parent = parent.Parent;
}
return ret;
}
float defaultSpecialNodeCost(IAStarNode node)
{
return 0;
}
private List <IAStarNode> ConstructPathFromStartToGoal(Dictionary <IAStarNode, IAStarNode> cameFromSet, IAStarNode current)
{
List <IAStarNode> path = new List <IAStarNode>();
path.Add(current);
while (cameFromSet.ContainsKey(current))
{
current = cameFromSet[current];
path.Insert(0, current);
}
return(path);
}
protected virtual float calcSpecialCost(IAStarNode node)
{
//Collider2D c2d = Physics2D.OverlapPoint(node.Position, charamask);
var list = new List<EnemyController>(GameController.DungeonInformation.MoveResevationList.ToArray());
var list2 = GameController.DungeonInformation.Enemys.FindAll(enemy => !GameController.DungeonInformation.MoveResevationList.Contains((EnemyController)enemy));
var find1 = list.Find(enemy => enemy.NextPosition.Equals(node.Position));//list2.Find(enemy => enemy.CurrentPosition.Equals(node.Position));
if (find1 != null && find1 != CharacterController)
{
return AStarSearch.IgnoreCost;
}
float ret = 100f + (CurrentGoalPosition - node.Position).magnitude;
if (node.Parent != AStarSearch.Dummy && GameController.DungeonInformation.hasWallBetween(node.Position, node.Parent.Position)) {
//Debug.Log("HasWall!");
return AStarSearch.IgnoreCost;
}
if (CharacterController.canMoveNextPosition(node.Position)) ret -= 30;
if (MyCharacterController.isSameLine(node.Position, CurrentGoalPosition)) ret -= 20;
if (node.Position.x == CurrentGoalPosition.x) ret -= 20;
if (node.Position.y == CurrentGoalPosition.y) ret -= 20;
return ret;
}
/// <summary>
/// Returns the N best paths from start to goal. Will only return paths that exist. If no paths exist, this list will be empty.
/// </summary>
public List <List <IAStarNode> > ComputePaths(IAStarMap map, IAStarNode start, IAStarNode goal, Int32 nPaths)
{
List <List <IAStarNode> > allPaths = new List <List <IAStarNode> >();
// http://en.wikipedia.org/wiki/A*_search_algorithm
List <IAStarNode> pathFromStartToGoal = null;
HashSet <IAStarNode> closedSet = new HashSet <IAStarNode>(m_nodeComparer);
HashSet <IAStarNode> openSet = new HashSet <IAStarNode>(m_nodeComparer);
openSet.Add(start);
Dictionary <IAStarNode, IAStarNode> cameFromSet = new Dictionary <IAStarNode, IAStarNode>(m_nodeComparer);
Dictionary <IAStarNode, Double> gScore = new Dictionary <IAStarNode, Double>(m_nodeComparer);
Dictionary <IAStarNode, Double> fScore = new Dictionary <IAStarNode, Double>(m_nodeComparer);
gScore.Add(start, 0);
Double fScoreStart = gScore[start] + map.HeuristicCost(start, goal);
fScore.Add(start, fScoreStart);
while (openSet.Count > 0 && nPaths > 0) // While there are still nodes to explore
{
IAStarNode current = ReturnINodeInOpenSetWithLowestFScore(openSet, fScore);
if (current == goal)
{
// We've found a path. Add it to the list.
pathFromStartToGoal = ConstructPathFromStartToGoal(cameFromSet, current);
allPaths.Add(pathFromStartToGoal);
nPaths--; // We have succeeded with one path.
openSet.Remove(current); // We don't want to add "current" to the openset; we want to backup and keep going.
}
else
{
openSet.Remove(current);
closedSet.Add(current);
List <IAStarNode> neighbors = map.NeighborNodes(current);
foreach (IAStarNode neighbor in neighbors)
{
if (!closedSet.Contains(neighbor))
{
Double tentativeGScore = gScore[current] + map.TrueCost(current, neighbor);
if (!openSet.Contains(neighbor) || tentativeGScore < gScore[neighbor])
{
cameFromSet[neighbor] = current;
gScore[neighbor] = tentativeGScore;
Double fScoreNeighbor = gScore[neighbor] + map.HeuristicCost(neighbor, goal);
fScore[neighbor] = fScoreNeighbor;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
}
}
}
}
}
// We used to return null entries for each Nth path that does not exist. However, I think this is not useful in real cases.
//while(nPaths > 0)
//{
// // We promised there'd be one entry per desired path.
// allPaths.Add(null);
// nPaths--;
//}
return(allPaths);
}
float calcScoreHeuristic(IAStarNode node,float totalcost)
{
float ret = 0;
return ret;
}
