public void GetNeighbors(NodeAStar node, ArrayList neighbors)
{
Vector3 neighborPosition = node.position;
int neighborIndex = GetGridIndex(neighborPosition);
int row = GetRowOfIndex(neighborIndex);
int column = GetColumnOfIndex(neighborIndex);
//Bottom
int leftNodeRow = row - 1;
int leftNodeColumn = column;
AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);
//Top
leftNodeRow = row + 1;
leftNodeColumn = column;
AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);
//Right
leftNodeRow = row;
leftNodeColumn = column + 1;
AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);
//Left
leftNodeRow = row;
leftNodeColumn = column - 1;
AssignNeighbor(leftNodeRow, leftNodeColumn, neighbors);
}
void CreateGrid()
{
grid = new NodeAStar[gridSizeX, gridSizeY];
Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2f - Vector3.forward * gridWorldSize.y / 2f;
for (int y = 0; y < gridSizeY; y++)
{
for (int x = 0; x < gridSizeX; x++)
{
// Determine height of terrain mesh at pos
RaycastHit hit;
Vector3 worldPoint = Vector3.zero;
Ray ray = new Ray(new Vector3(worldBottomLeft.x + (x * nodeDiameter + nodeRadius), 150, worldBottomLeft.z + (y * nodeDiameter + nodeRadius)), Vector3.down);
if (Physics.Raycast(ray, out hit, 200))
{
worldPoint = hit.point;
}
else
{
Debug.LogError("NO TERRAIN FOUND IN RAYCAST");
}
bool walkable = !Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask);
grid[x, y] = new NodeAStar(walkable, worldPoint, x, y);
}
}
}
public NodeAStar()
{
hCost = 0.0f;
gCost = 1.0f;
bObstacle = false;
parent = null;
}
public NodeAStar(Vector3 pos)
{
hCost = 0.0f;
gCost = 1.0f;
bObstacle = false;
parent = null;
position = pos;
}
public NodeAStar(GraphPoint <NaviPoint> point, NodeAStar father)
{
this.point = point;
this.father = father;
this.G = 0;
this.H = 0;
this.F = 0;
}
private void FindPath()
{
startPosition = startCube.transform;
endPosition = endCube.transform;
startNode = new NodeAStar(gridManager.GetGridCellCenter(gridManager.GetGridIndex(startPosition.position)));
goalNode = new NodeAStar(gridManager.GetGridCellCenter(gridManager.GetGridIndex(endPosition.position)));
pathArray = AStar.FindPath(startNode, goalNode);
}
private static ArrayList CalculatePath(NodeAStar node)
{
ArrayList list = new ArrayList();
while (node != null)
{
list.Add(node);
node = node.parent;
}
list.Reverse();
return(list);
}
// Check the neighbor. If it's not an obstacle, assign the neighbor.
private void AssignNeighbor(int row, int column, ArrayList neighbors)
{
if (row != -1 && column != -1 && row < numberOfRows && column < numberOfColumns)
{
NodeAStar nodeToAdd = nodes[row, column];
if (!nodeToAdd.bObstacle)
{
neighbors.Add(nodeToAdd);
}
}
}
int GetDistance(NodeAStar nodeA, NodeAStar nodeB)
{
int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
if (dstX > dstY)
{
return(14 * dstY + 10 * (dstX - dstY));
}
else
{
return(14 * dstX + 10 * (dstY - dstX));
}
}
void RetracePath(NodeAStar startNode, NodeAStar endNode)
{
List <NodeAStar> path = new List <NodeAStar>();
NodeAStar currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.parentNode;
}
path.Reverse();
grid.path = path;
}
//IComparable Interface method implementation
public int CompareTo(object obj)
{
NodeAStar node = (NodeAStar)obj;
if (hCost < node.hCost)
{
return(-1);
}
if (hCost > node.hCost)
{
return(1);
}
return(0);
}
private void InitializeNodes()
{
nodes = new NodeAStar[numberOfColumns, numberOfRows];
int index = 0;
for (int i = 0; i < numberOfColumns; i++)
{
for (int j = 0; j < numberOfRows; j++)
{
Vector3 cellPosition = GetGridCellCenter(index);
NodeAStar node = new NodeAStar(cellPosition);
nodes[i, j] = node;
index++;
}
}
}
private void SortInsert(List <NodeAStar> open, NodeAStar childNode)
{
int i = 0;
while (i < open.Count && open[i].F > childNode.F)
{
i++;
}
if (i == open.Count)
{
open.Add(childNode);
}
else
{
open.Insert(i, childNode);
}
}
void FindPath(Vector3 startPos, Vector3 targetPos)
{
NodeAStar startNode = grid.NodeFromWorldPoint(startPos);
NodeAStar targetNode = grid.NodeFromWorldPoint(targetPos);
Heap <NodeAStar> openSet = new Heap <NodeAStar>(grid.MaxSize);
HashSet <NodeAStar> closedSet = new HashSet <NodeAStar>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
NodeAStar currentNode = openSet.PopFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
RetracePath(startNode, targetNode);
return;
}
foreach (NodeAStar neighbourNode in grid.GetNeighbouringNodes(currentNode))
{
if (!neighbourNode.walkable || closedSet.Contains(neighbourNode))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbourNode);
if (newMovementCostToNeighbour < neighbourNode.gCost || !openSet.Contains(neighbourNode))
{
neighbourNode.gCost = newMovementCostToNeighbour;
neighbourNode.hCost = GetDistance(neighbourNode, targetNode);
neighbourNode.parentNode = currentNode;
if (!openSet.Contains(neighbourNode))
{
openSet.Add(neighbourNode);
}
}
}
}
}
public Int32 CompareTo(object obj)
{
if (!(obj is NodeAStar))
{
throw new Exception("Bad NodeAStar comparison.");
}
NodeAStar compareWithMe = (NodeAStar)obj;
Int32 returnValue = 0;
float difference = this.estimatedTotalCost - compareWithMe.estimatedTotalCost;
if (difference > 0)
{
returnValue = 1;
}
else if (difference < 0)
{
returnValue = -1;
}
return returnValue;
}
private void OnDrawGizmos()
{
if (pathArray == null)
{
return;
}
if (pathArray.Count > 0)
{
int index = 1;
foreach (NodeAStar node in pathArray)
{
if (index < pathArray.Count)
{
NodeAStar nextNode = (NodeAStar)pathArray[index];
Debug.DrawLine(node.position, nextNode.position, Color.green);
index++;
}
}
;
}
}
private void OnDrawGizmos()
{
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 100, gridWorldSize.y));
if (onlyDisplayPathGizmos)
{
if (path != null)
{
foreach (NodeAStar node in path)
{
Gizmos.color = Color.black;
Gizmos.DrawSphere(node.worldPosition, nodeRadius - .5f);
}
}
}
else
{
if (grid != null)
{
NodeAStar agentNode = NodeFromWorldPoint(testAgent.position);
foreach (NodeAStar node in grid)
{
Gizmos.color = (node.walkable) ? Color.white : Color.red;
if (path != null)
{
if (path.Contains(node))
{
Gizmos.color = Color.black;
}
}
Gizmos.DrawSphere(node.worldPosition, (nodeRadius - .5f));
}
}
}
}
public List <NodeAStar> GetNeighbouringNodes(NodeAStar node)
{
List <NodeAStar> neighbours = new List <NodeAStar>();
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
if (x == 0 && y == 0)
{
continue;
}
int checkX = node.gridX + x;
int checkY = node.gridY + y;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
{
neighbours.Add(grid[checkX, checkY]);
}
}
}
return(neighbours);
}
public void Push(NodeAStar node)
{
nodes.Add(node);
nodes.Sort();
}
private bool Contains(List <NodeAStar> list, GraphPoint <NaviPoint> graphPoint, out NodeAStar findNode)
{
if ((findNode = list.Find(new Predicate <NodeAStar>(
delegate(NodeAStar node)
{
if (node.point == graphPoint)
{
return(true);
}
else
{
return(false);
}
}))) == null)
{
return(false);
}
else
{
return(true);
}
}
public void Remove(NodeAStar node)
{
nodes.Remove(node);
nodes.Sort();
}
/// <summary>
/// 使用A*算法依据当前信息计算一条最短的路径。
/// 注意，如果目标点在警戒线以内，会返回一条并非如你期望的路径。
/// 所以请自行实现目标点无法到达时的处理逻辑。
/// </summary>
/// <param name="curPos"></param>
/// <param name="aimPos"></param>
/// <returns></returns>
public NaviPoint[] CalPathUseAStar(Vector2 curPos, Vector2 aimPos)
{
#region 制导航图
GraphPoint <NaviPoint>[] map = new GraphPoint <NaviPoint> [Map.Length + 2];
GraphPoint <NaviPoint>[] temp = GraphPoint <NaviPoint> .DepthCopy(Map);
for (int i = 0; i < temp.Length; i++)
{
map[i] = temp[i];
}
#endregion
#region 将当前点和目标点加入到导航图中
int prePointSum = temp.Length;
GraphPoint <NaviPoint> curNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, curPos), new List <GraphPath <NaviPoint> >());
GraphPoint <NaviPoint> aimNaviPoint = new GraphPoint <NaviPoint>(new NaviPoint(null, -1, aimPos), new List <GraphPath <NaviPoint> >());
AddCurPosToNaviMap(map, curNaviPoint, prePointSum, GuardLines, BorderLines);
AddAimPosToNaviMap(map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines);
#endregion
#region 计算最短路径，使用A*算法
List <NodeAStar> open = new List <NodeAStar>();
List <NodeAStar> close = new List <NodeAStar>();
open.Add(new NodeAStar(curNaviPoint, null));
NodeAStar cur = null;
while (open.Count != 0)
{
cur = open[open.Count - 1];
if (cur.point == aimNaviPoint)
{
break;
}
open.RemoveAt(open.Count - 1);
close.Add(cur);
foreach (GraphPath <NaviPoint> path in cur.point.neighbors)
{
if (Contains(close, path.neighbor))
{
continue;
}
else
{
NodeAStar inOpenNode;
if (Contains(open, path.neighbor, out inOpenNode))
{
float G = cur.G + path.weight;
if (inOpenNode.G > G)
{
inOpenNode.G = G;
inOpenNode.F = G + inOpenNode.H;
}
}
else
{
NodeAStar childNode = new NodeAStar(path.neighbor, cur);
childNode.G = cur.G + path.weight;
childNode.H = Vector2.Distance(aimPos, childNode.point.value.Pos);
childNode.F = childNode.G + childNode.H;
SortInsert(open, childNode);
}
}
}
}
//if (cur == null)
// return null;
Stack <NodeAStar> cahe = new Stack <NodeAStar>();
while (cur.father != null)
{
cahe.Push(cur);
cur = cur.father;
}
NaviPoint[] result = new NaviPoint[cahe.Count];
int j = 0;
foreach (NodeAStar node in cahe)
{
result[j] = node.point.value;
j++;
}
return(result);
#endregion
}
private List<Direction> _PathfinderAStar(Coords start, Coords endTopLeft, Coords endBottomRight, BitArray[] _passabilityMap, hFunction h)
{
// NOTE: Should later implemented a collision predictor mechanic to work in tandem
// with the path-finder to provide better agent behavior.
// NOTE: Consider returning the number of tiles scanned in case no path is found.
// This will alert a boxed-in creature of its predicament.
// NOTE: Introduce a flag for a straight-line initial check(for outdoors environmens and
// for when the goal is near).
Int32 rangeX = _passabilityMap.Length;
Int32 rangeY = _passabilityMap[0].Count;
NodeAStar?[,] nodeArray = new NodeAStar?[rangeX, rangeY];
NodeAStar startNode = new NodeAStar();
startNode.costSoFar = 0;
startNode.estimatedTotalCost = h(start);
nodeArray[start.X, start.Y] = startNode;
List<Coords> ListOpen = new List<Coords>();
ListOpen.Add(start);
while (ListOpen.Count > 0)
{
// I have to use this bool the way I've implemented the algo. Consider rewriting.
bool resortList = false;
Coords currentCoords = ListOpen.First();
// Check to see if goal is reached.
//if (currentCoords.Equals(endTopLeft))
if (StaticMathFunctions.CoordinateIsInBox(currentCoords, endTopLeft, endBottomRight))
{
break;
}
NodeAStar currentNode = nodeArray[currentCoords.X, currentCoords.Y].Value;
for (byte i = 0; i <= 3; ++i)
{
Direction currentDir = (Direction)(2 * i + 1);
Coords dirCoords = StaticMathFunctions.DirectionToCoords(currentDir);
Coords potential = currentCoords + dirCoords;
// check if move in dir is allowed
if (potential.X >= 0 && potential.X < rangeX && potential.Y >= 0 && potential.Y < rangeY // bounds check
&& _passabilityMap[potential.X][potential.Y]) // passability check
{
// Using the simplest cost function possible. Can be easily updated
// once tile walkability coefficients are added.
Coords newNodePosition = new Coords(CoordsType.General, currentCoords.X + dirCoords.X, currentCoords.Y + dirCoords.Y);
float accruedCost = currentNode.costSoFar + Constants.MovementCost[(byte)currentDir];
// Straight line correction
if (currentDir == nodeArray[currentCoords.X, currentCoords.Y].Value.connection)
{
accruedCost -= Constants.PathfinderStraightPathCorrection;
}
// Check to see if the node under examination is in the closed list.
//NodeAStar? oldNode = nodeArray[newNodePosition.X, newNodePosition.Y];
if (nodeArray[newNodePosition.X, newNodePosition.Y] != null)
{
// If node is in closed list, see if it needs updating.
if (nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar > accruedCost)
{
float expectedAdditionalCost =
nodeArray[newNodePosition.X, newNodePosition.Y].Value.estimatedTotalCost -
nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar;
NodeAStar nodeToAdd =
new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
ListOpen.Add(newNodePosition);
resortList = true;
}
}
// Node is in open list. Process it.
else
{
float expectedAdditionalCost = h(newNodePosition);
NodeAStar nodeToAdd =
new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
ListOpen.Add(newNodePosition);
resortList = true;
}
}
}
ListOpen.RemoveAt(0);
if (resortList)
{
ListOpen.Sort(
delegate(Coords c1, Coords c2)
{
float difference = nodeArray[c1.X, c1.Y].Value.estimatedTotalCost -
nodeArray[c2.X, c2.Y].Value.estimatedTotalCost;
Int32 returnValue = 0;
if (difference > 0)
{
returnValue = 1;
}
else if (difference < 0)
{
returnValue = -1;
}
return returnValue;
}
);
}
}
List<Direction> ListRoute = new List<Direction>();
// Return empty route if the open list is empty, i.e. there is no path to the target
// Ideally, the game logic should be fixed so that the search isn't even attempted
// if there is no path between the two points.
if (ListOpen.Count == 0)
{
return ListRoute;
}
Coords trackbackCoords = endTopLeft;
while (trackbackCoords != start)
{
Direction newDirection = nodeArray[trackbackCoords.X, trackbackCoords.Y].Value.connection;
ListRoute.Add(newDirection);
trackbackCoords = StaticMathFunctions.CoordsNeighboringInDirection(new Coords(CoordsType.Tile, trackbackCoords),
StaticMathFunctions.OppositeDirection(newDirection));
}
// Might be faster without reversing
//ListRoute.Reverse();
// We skip the reversal, so pick directions from the END of the list.
return ListRoute;
}
public int Act(ref WindjermanGameState gs, NativeList <int> availableActions)
{
//déterminer la distance entre le joueur et le frisbee au moment T
currentDistanceFromFrisbee = CalculerCurrentDistanceFromFrisbee(ref gs);
FindHighGround(ref gs);
//création de la liste des nodes
var listeNodes = new NativeList <NodeAStar>(10, Allocator.Temp);
//pour chaque action disponible
for (int i = 0; i < availableActions.Length; i++)
{
//on crée un node
NodeAStar n = new NodeAStar();
n.distanceFromFrisbee = 0;
n.playerID = this.playerID;
n.gsNode = Rules.Clone(ref gs);
//on execute ladite action pour avoir le gamestate T+1 associé
if (playerID == 0)
{
Rules.Step(ref n.gsNode, availableActions[i], 0);
}
else
{
Rules.Step(ref n.gsNode, 0, availableActions[i]);
}
//déterminer si le joueur a le frisbee à T+1
if (playerID == 0)
{
if (n.gsNode.isFreeze1)
{
n.hasFrisbee = true;
}
else
{
n.hasFrisbee = false;
}
}
else
{
if (n.gsNode.isFreeze2)
{
n.hasFrisbee = true;
}
else
{
n.hasFrisbee = false;
}
}
//déterminer la distance entre le joueur et le frisbee si celui-ci ne l'a pas en main à T+1
if (!n.hasFrisbee)
{
n.CalculerDistanceFromFrisbee();
}
//si le joueur a le frisbee, déterminer sa situation par rapport à l'autre joueur
else
{
n.FindHighGround();
}
//ajouter le node à la liste
listeNodes.Add(n);
}
//une fois les nodes créés, on détermine l'action à réaliser
int action = FindBestAction(ref listeNodes, ref gs, ref availableActions);
//listeNodes.Dispose();
return(action);
}
// Find the path between start node and goal node using A* Algorithm
public static ArrayList FindPath(NodeAStar start, NodeAStar goal)
{
openList = new PriorityQueue();
openList.Push(start);
start.gCost = 0.0f;
start.hCost = EstimateHeuristicCost(start, goal);
closedList = new PriorityQueue();
NodeAStar node = null;
GridManager gridManager = GameObject.FindObjectOfType <GridManager>();
if (gridManager == null)
{
return(null);
}
while (openList.Length != 0)
{
node = openList.GetFirstNode();
if (node.position == goal.position)
{
return(CalculatePath(node));
}
ArrayList neighbors = new ArrayList();
gridManager.GetNeighbors(node, neighbors);
//Update the costs of each neighbor node.
for (int i = 0; i < neighbors.Count; i++)
{
NodeAStar neighborNode = (NodeAStar)neighbors[i];
if (!closedList.Contains(neighborNode))
{
//Cost from current node to this neighbor node
float cost = EstimateHeuristicCost(node, neighborNode);
//Total Cost So Far from start to this neighbor node
float totalCost = node.gCost + cost;
//Estimated cost for neighbor node to the goal
float neighborNodeEstCost = EstimateHeuristicCost(neighborNode, goal);
//Assign neighbor node properties
neighborNode.gCost = totalCost;
neighborNode.parent = node;
neighborNode.hCost = totalCost + neighborNodeEstCost;
//Add the neighbor node to the open list if we haven't already done so.
if (!openList.Contains(neighborNode))
{
openList.Push(neighborNode);
}
}
}
closedList.Push(node);
openList.Remove(node);
}
//We handle the scenario where no goal was found after looping thorugh the open list
if (node.position != goal.position)
{
Debug.LogError("Goal Not Found");
return(null);
}
//Calculate the path based on the final node
return(CalculatePath(node));
}
/// Calculate the estimated Heuristic cost to the goal
private static float EstimateHeuristicCost(NodeAStar currentNode, NodeAStar goalNode)
{
Vector3 cost = currentNode.position - goalNode.position;
return(cost.magnitude);
}
private List<Direction> _PathfinderAStar(Coords start, Coords endTopLeft, Coords endBottomRight, BitArray[] _passabilityMap, hFunction h)
{
// NOTE: Should later implemented a collision predictor mechanic to work in tandem
// with the path-finder to provide better agent behavior.
// NOTE: Consider returning the number of tiles scanned in case no path is found.
// This will alert a boxed-in creature of its predicament.
// NOTE: Introduce a flag for a straight-line initial check(for outdoors environmens and
// for when the goal is near).
Int32 rangeX = _passabilityMap.Length;
Int32 rangeY = _passabilityMap[0].Count;
NodeAStar?[,] nodeArray = new NodeAStar?[rangeX, rangeY];
NodeAStar startNode = new NodeAStar();
startNode.costSoFar = 0;
startNode.estimatedTotalCost = h(start);
nodeArray[start.X, start.Y] = startNode;
List<Coords> ListOpen = new List<Coords>();
ListOpen.Add(start);
while (ListOpen.Count > 0)
{
// I have to use this bool the way I've implemented the algo. Consider rewriting.
bool resortList = false;
Coords currentCoords = ListOpen.First();
// Check to see if goal is reached.
//if (currentCoords.Equals(endTopLeft))
if (StaticMathFunctions.CoordinateIsInBox(currentCoords, endTopLeft, endBottomRight))
{
break;
}
NodeAStar currentNode = nodeArray[currentCoords.X, currentCoords.Y].Value;
for (byte i = 0; i <= 5; ++i)
{
Direction currentDir = (Direction)(i);
//Coords dirCoords = StaticMathFunctions.DirectionToCoords(currentDir);
Coords potential = StaticMathFunctions.CoordsNeighboringInDirection(currentCoords, currentDir);
// check if move in dir is allowed
if (potential.X >= 0 && potential.X < rangeX && potential.Y >= 0 && potential.Y < rangeY // bounds check
&& _passabilityMap[potential.X][potential.Y]) // passability check
{
// Using the simplest cost function possible. Can be easily updated
// once tile walkability coefficients are added.
//Coords newNodePosition = new Coords(CoordsType.General, currentCoords.X + dirCoords.X, currentCoords.Y + dirCoords.Y);
Coords newNodePosition = potential;
float accruedCost = currentNode.costSoFar + 1;
// Straight line correction
if (currentDir == nodeArray[currentCoords.X, currentCoords.Y].Value.connection)
{
accruedCost -= Constants.PathfinderStraightPathCorrection;
}
// Check to see if the node under examination is in the closed list.
//NodeAStar? oldNode = nodeArray[newNodePosition.X, newNodePosition.Y];
if (nodeArray[newNodePosition.X, newNodePosition.Y] != null)
{
// If node is in closed list, see if it needs updating.
if (nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar > accruedCost)
{
float expectedAdditionalCost =
nodeArray[newNodePosition.X, newNodePosition.Y].Value.estimatedTotalCost -
nodeArray[newNodePosition.X, newNodePosition.Y].Value.costSoFar;
NodeAStar nodeToAdd =
new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
ListOpen.Add(newNodePosition);
resortList = true;
}
}
// Node is in open list. Process it.
else
{
float expectedAdditionalCost = h(newNodePosition);
NodeAStar nodeToAdd =
new NodeAStar(currentDir, accruedCost, accruedCost + expectedAdditionalCost);
nodeArray[newNodePosition.X, newNodePosition.Y] = nodeToAdd;
ListOpen.Add(newNodePosition);
resortList = true;
}
}
}
ListOpen.RemoveAt(0);
if (resortList)
{
ListOpen.Sort(
delegate(Coords c1, Coords c2)
{
float difference = nodeArray[c1.X, c1.Y].Value.estimatedTotalCost -
nodeArray[c2.X, c2.Y].Value.estimatedTotalCost;
Int32 returnValue = 0;
if (difference > 0)
{
returnValue = 1;
}
else if (difference < 0)
{
returnValue = -1;
}
return returnValue;
}
);
}
}
List<Direction> ListRoute = new List<Direction>();
// Return empty route if the open list is empty, i.e. there is no path to the target
// Ideally, the game logic should be fixed so that the search isn't even attempted
// if there is no path between the two points.
if (ListOpen.Count == 0)
{
return ListRoute;
}
Coords trackbackCoords = endTopLeft;
while (trackbackCoords != start)
{
Direction newDirection = nodeArray[trackbackCoords.X, trackbackCoords.Y].Value.connection;
ListRoute.Add(newDirection);
trackbackCoords = StaticMathFunctions.CoordsNeighboringInDirection(new Coords(CoordsType.Tile, trackbackCoords),
StaticMathFunctions.OppositeDirection(newDirection));
}
// Might be faster without reversing
//ListRoute.Reverse();
// We skip the reversal, so pick directions from the END of the list.
return ListRoute;
}
public NodeAStar ( GraphPoint<NaviPoint> point, NodeAStar father )
{
this.point = point;
this.father = father;
this.G = 0;
this.H = 0;
this.F = 0;
}
private bool Contains ( List<NodeAStar> list, GraphPoint<NaviPoint> graphPoint, out NodeAStar findNode )
{
if ((findNode = list.Find( new Predicate<NodeAStar>(
delegate( NodeAStar node )
{
if (node.point == graphPoint)
return true;
else
return false;
} ) )) == null)
return false;
else
return true;
}
private void SortInsert ( List<NodeAStar> open, NodeAStar childNode )
{
int i = 0;
while (i < open.Count && open[i].F > childNode.F)
{
i++;
}
if (i == open.Count)
open.Add( childNode );
else
open.Insert( i, childNode );
}
/// <summary>
/// 使用A*算法依据当前信息计算一条最短的路径。
/// 注意，如果目标点在警戒线以内，会返回一条并非如你期望的路径。
/// 所以请自行实现目标点无法到达时的处理逻辑。
/// </summary>
/// <param name="curPos"></param>
/// <param name="aimPos"></param>
/// <returns></returns>
public NaviPoint[] CalPathUseAStar ( Vector2 curPos, Vector2 aimPos )
{
#region 复制导航图
GraphPoint<NaviPoint>[] map = new GraphPoint<NaviPoint>[Map.Length + 2];
GraphPoint<NaviPoint>[] temp = GraphPoint<NaviPoint>.DepthCopy( Map );
for (int i = 0; i < temp.Length; i++)
{
map[i] = temp[i];
}
#endregion
#region 将当前点和目标点加入到导航图中
int prePointSum = temp.Length;
GraphPoint<NaviPoint> curNaviPoint = new GraphPoint<NaviPoint>( new NaviPoint( null, -1, curPos ), new List<GraphPath<NaviPoint>>() );
GraphPoint<NaviPoint> aimNaviPoint = new GraphPoint<NaviPoint>( new NaviPoint( null, -1, aimPos ), new List<GraphPath<NaviPoint>>() );
AddCurPosToNaviMap( map, curNaviPoint, prePointSum, GuardLines, BorderLines );
AddAimPosToNaviMap( map, aimNaviPoint, curNaviPoint, prePointSum, GuardLines, BorderLines );
#endregion
#region 计算最短路径，使用A*算法
List<NodeAStar> open = new List<NodeAStar>();
List<NodeAStar> close = new List<NodeAStar>();
open.Add( new NodeAStar( curNaviPoint, null ) );
NodeAStar cur = null;
while (open.Count != 0)
{
cur = open[open.Count - 1];
if (cur.point == aimNaviPoint)
break;
open.RemoveAt( open.Count - 1 );
close.Add( cur );
foreach (GraphPath<NaviPoint> path in cur.point.neighbors)
{
if (Contains( close, path.neighbor ))
{
continue;
}
else
{
NodeAStar inOpenNode;
if (Contains( open, path.neighbor, out inOpenNode ))
{
float G = cur.G + path.weight;
if (inOpenNode.G > G)
{
inOpenNode.G = G;
inOpenNode.F = G + inOpenNode.H;
}
}
else
{
NodeAStar childNode = new NodeAStar( path.neighbor, cur );
childNode.G = cur.G + path.weight;
childNode.H = Vector2.Distance( aimPos, childNode.point.value.Pos );
childNode.F = childNode.G + childNode.H;
SortInsert( open, childNode );
}
}
}
}
//if (cur == null)
// return null;
Stack<NodeAStar> cahe = new Stack<NodeAStar>();
while (cur.father != null)
{
cahe.Push( cur );
cur = cur.father;
}
NaviPoint[] result = new NaviPoint[cahe.Count];
int j = 0;
foreach (NodeAStar node in cahe)
{
result[j] = node.point.value;
j++;
}
return result;
#endregion
}
