/**
* reset
*/
public void Reset()
{
PathFinding.AStarAgent agent = _agent as PathFinding.AStarAgent;
if (agent != null && agent._asm != null)
{
_goal = null;
}
_goal = null; //.Reset();
_map = null; //.Reset();
_storage = null; //.Reset();
#if UNITY_EDITOR && !MULTI_THREAD
if (PathFinding.PathFindingManager.DEBUG)
{
if (_agent != null && ((PathFinding.AStarAgent)_agent)._asm == this)
{
// UnityEngine.Debug.LogError("reset path error");
}
}
#endif
_inQueue = false;
_revsCurrentRun = 0;
_curPosIdx = -1;
_agent = null;
_maxRevolutions = C_MaxRevolutions;
ResetMachineData();
#if ASTARDEBUG
_totalRevs = 0;
#endif
}
public AStarNode GetAStarNode(int x, int y, int pathId, IAStarMap map, bool isIgnored = false)
{
GridMap gMap = map as GridMap;
int idx = gMap.GetIdx(x, y);
GridNode nNode;
if (_gridNodes.TryGetValue(idx, out nNode) || gMap.IsWalkable(x, y) || isIgnored)
{
if (nNode == null)
{
nNode = _gridNodesPool.New();
_gridNodes[idx] = nNode;
}
if (nNode.pathId != pathId)
{
nNode.Reset();
nNode.NodeID = idx;
nNode.gridPos.x = x;
nNode.gridPos.y = y;
nNode.pathId = pathId;
}
return(nNode);
}
return(null);
}
public override AStarNode GetAStarNode(int id, int pathId, IAStarMap map, bool isIgnored = false)
{
GridMap gMap = map as GridMap;
IInt2 pos = gMap.GetGridPos(id);
return(GetAStarNode(pos.x, pos.y, pathId, gMap, isIgnored));
}
//public virtual AStarNode FindInOpenSet(AStarNode node)
//{
//}
//public virtual AStarNode FindInClosedSet(short node);
// public virtual void RemoveFromClosedSet(short nodeId, IAStarMap map);
//public virtual void RemoveFromOpenSet(AStarNode node, IAStarMap map)
//{
// _heap.Remove(node.HeapIndex);
// // map.re
// node.Flag = AStarNode.ENodeState.closed;
//}
public virtual AStarNode RemoveCheapestOpenNode(IAStarMap map)
{
if (_heap.numberOfItems > 0)
{
return(_heap.Remove(0));
}
return(null);
}
public void Ini(IAgent agent, IAStarGoal goal, AStarStorageBase storage, IAStarMap aStarMap)
{
Reset();
_agent = agent;
_goal = goal;
_storage = storage;
_map = aStarMap;
}
/// <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);
}
/// <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>
/// Constructs an A* algorithm object.
/// </summary>
/// <param name="map"></param>
/// <param name="comparison"></param>
public AStar(IAStarMap <T> map, Func <T, T, bool> comparison)
{
mMap = map;
mEquals = comparison;
}
/// <summary>
/// Constructs an A* algorithm object.
/// </summary>
/// <param name="map"></param>
/// <param name="comparer"></param>
public AStar(IAStarMap <T> map, IEqualityComparer <T> comparer)
: this(map, comparer.Equals)
{
}
/// <summary>
/// Constructs an A* algorithm object.
/// </summary>
/// <param name="map"></param>
public AStar(IAStarMap <T> map)
: this(map, (x, y) => x.Equals(y))
{
}
public virtual void AddToClosedSet(AStarNode node, IAStarMap map)
{
node.Flag = AStarNode.ENodeState.closed;
// node.Next = null;
// node.Previous = null;
}
public virtual void AddToOpenSet(AStarNode node, IAStarMap map)
{
_heap.Add(node);
node.Flag = AStarNode.ENodeState.open;
}
public virtual AStarNode GetAStarNeighbour(AStarNode pAStarNode, int iNeighbor, int searchId, IAStarMap map, IInt2 curPos)
{
return(null);
}
public virtual AStarNode GetAStarNode(int id, int searchId, IAStarMap map, bool isTarget = false)
{
return(null);
}
public override AStarNode GetAStarNeighbour(AStarNode node, int index, int pathId, IAStarMap map, IInt2 curPos)
{
GridNode gNode = (GridNode)node;
if (gNode != null && index >= 0 && index < GridMap.neighbourOffset.Length)
{
int x = gNode.gridPos.x + GridMap.neighbourOffset[index].x;
int z = gNode.gridPos.y + GridMap.neighbourOffset[index].y;
bool bSkip = false;
#if !DYNAMIC_FORCE
IInt2 pos = IInt2.zero;
pos.x = x;
pos.y = z;
IInt2 diff = curPos - pos;
int posIdx = map.GetIdx(pos.x, pos.y);
int curposIdx = map.GetIdx(curPos.x, curPos.y);
EDynamicBlockStatus status = map.DynamicBlockedStatus(posIdx, -1, -1, curposIdx);
bSkip = (status == EDynamicBlockStatus.blocked && diff.sqrMagnitudeInt > GridMap.c_dynamicRangeSqr) ||
status == EDynamicBlockStatus.ignore;
#endif
return(GetAStarNode(x, z, pathId, map, bSkip));
}
return(null);
}
public AStar(IAStarMap map)
{
Map = map;
}
