private List <MapNode> findPath(MapNode start, MapNode end, bool eightDirection = false, bool useBinaryHeap = true)
{
if (useBinaryHeap)
{
_open = new BinaryHeapOpenList <MapNode>();
}
else
{
_open = new ListOpenList <MapNode>();
}
_closed = new List <MapNode>();
List <MapNode> result = new List <MapNode>();
start.parentNode = null;
//设置或者不设置为0，问题不大。唯一的可能问题是寻路多次之后有可能溢出
start.g = 0;
start.h = 0;
start.f = 0;
//-----------------------
if (end == null)
{
return(result);
}
if (!end.isPassAble())
{
return(result);
}
end.parentNode = null;
_open.push(start);
while ((_open.getCount() > 0) && (_closed.IndexOf(end) == -1))
{
//if (_open is ListOpenList<MapNode>)
//{
// (_open as ListOpenList<MapNode>).printSelf();
//}
_current = _open.getSmallest();
checkAround(_current, end, eightDirection);
_closed.Add(_current);
}
MapNode c = end;
while (c != null)
{
result.Insert(0, c);
c = c.parentNode;
}
if (result.Count == 1)
{
result.Clear();
result.Add(start);
}
return(result);
}
public AStar(IEnumerable<IntVector3> initialLocations, IAStarTarget target)
{
this.MaxNodeCount = 200000;
this.CancellationToken = CancellationToken.None;
m_target = target;
m_nodeMap = new Dictionary<IntVector3, AStarNode>();
m_openList = new BinaryHeap<AStarNode>();
foreach (var p in initialLocations)
{
ushort g = 0;
ushort h = m_target.GetHeuristic(p);
var node = new AStarNode(p, null);
node.G = g;
node.H = h;
m_openList.Add(node);
m_nodeMap.Add(p, node);
}
}
public AStar(IEnumerable <IntVector3> initialLocations, IAStarTarget target)
{
this.MaxNodeCount = 200000;
this.CancellationToken = CancellationToken.None;
m_target = target;
m_nodeMap = new Dictionary <IntVector3, AStarNode>();
m_openList = new BinaryHeap <AStarNode>();
foreach (var p in initialLocations)
{
ushort g = 0;
ushort h = m_target.GetHeuristic(p);
var node = new AStarNode(p, null);
node.G = g;
node.H = h;
m_openList.Add(node);
m_nodeMap.Add(p, node);
}
}
public AStarImpl(IAStarEnvironment environment, IntPoint3 src, DirectionSet srcPositioning,
IAStarTarget target, int maxNodeCount = 200000, CancellationToken? cancellationToken = null)
{
m_environment = environment;
m_src = src;
m_srcPositioning = srcPositioning;
m_maxNodeCount = maxNodeCount;
m_cancellationToken = cancellationToken.HasValue ? cancellationToken.Value : CancellationToken.None;
m_target = target;
m_nodeMap = new Dictionary<IntPoint3, AStarNode>();
m_openList = new BinaryHeap<AStarNode>();
AddInitialNodes();
}
public Path FindPath(Cell startCell, Cell endCell, bool allowDiagonals, int movementPoints = (short)-1)
{
var success = false;
var matrix = new PathNode[Map.MapSize + 1];
IOpenList openList = m_useLogNodeSearch ? (IOpenList) new LogOpenList(new ComparePfNodeMatrix(matrix)) : new LinearOpenList(new ComparePfNodeMatrix(matrix));
var closedList = new List <PathNode>();
var location = startCell;
var counter = 0;
if (movementPoints == 0)
{
return(Path.GetEmptyPath(m_map, startCell));
}
matrix[location.Id].Cell = location;
matrix[location.Id].Parent = null;
matrix[location.Id].Cost = 0;
matrix[location.Id].Heuristic = 0;
matrix[location.Id].Status = NodeState.Open;
var distToEnd = startCell.ManhattanDistanceTo(endCell);
var endCellAux = startCell;
openList.Push(location);
while (openList.Count > 0)
{
location = openList.Pop();
matrix[location.Id].Status = NodeState.Closed;
if (location == endCell)
{
success = true;
break;
}
if (counter > SearchLimit)
{
return(Path.GetEmptyPath(m_map, startCell));
}
for (int i = 0; i < 8; i++)
{
var isDiagonal = DiagonalsDirections.Contains(i);
if (isDiagonal && !allowDiagonals)
{
continue;
}
var newLocation = location.GetNearestCellInDirection(Directions[i]);
if (newLocation == null)
{
continue;
}
if (newLocation.Id < 0 || newLocation.Id >= Map.MapSize)
{
continue;
}
if (matrix[newLocation.Id].Status == NodeState.Closed)
{
continue;
}
if (!m_context.IsCellWalkable(newLocation))
{
continue;
}
double baseCost;
if (newLocation == endCell)
{
baseCost = 1;
}
else
{
baseCost = GetCellCost(newLocation, m_throughEntities);
}
double cost = matrix[location.Id].Cost + baseCost * (isDiagonal ? DiagonalCost : HorizontalCost);
// adjust the cost if the current cell is aligned with the start cell or the end cell
if (m_throughEntities)
{
bool alignedWithEnd = newLocation.X + newLocation.Y == endCell.X + endCell.Y ||
newLocation.X - newLocation.Y == endCell.X - endCell.Y;
bool alignedWithStart = newLocation.X + newLocation.Y == startCell.X + startCell.Y ||
newLocation.X - newLocation.Y == startCell.X - startCell.Y;
if (newLocation.X + newLocation.Y != endCell.X + endCell.Y && newLocation.X - newLocation.Y != endCell.X - endCell.Y ||
newLocation.X + newLocation.Y != startCell.X + startCell.Y && newLocation.X - newLocation.Y != startCell.X - startCell.Y)
{
cost += newLocation.ManhattanDistanceTo(endCell);
cost += newLocation.ManhattanDistanceTo(startCell);
}
// tests diagonales now
if (newLocation.X == endCell.X || newLocation.Y == endCell.Y)
{
cost -= 3;
}
if (alignedWithEnd || !isDiagonal)
{
cost -= 2;
}
if (newLocation.X == startCell.X || newLocation.Y == startCell.Y)
{
cost -= 3;
}
if (alignedWithStart)
{
cost -= 2;
}
var currentDistToEnd = newLocation.ManhattanDistanceTo(endCell);
if (currentDistToEnd < distToEnd)
{
// if aligned with end
if (newLocation.X == endCell.X || newLocation.Y == endCell.Y ||
alignedWithEnd)
{
distToEnd = currentDistToEnd;
endCellAux = newLocation;
}
}
}
if (matrix[newLocation.Id].Status == NodeState.Open)
{
if (matrix[newLocation.Id].Cost <= cost)
{
continue;
}
matrix[newLocation.Id].Parent = location;
matrix[newLocation.Id].Cost = cost;
}
else
{
matrix[newLocation.Id].Cell = newLocation;
matrix[newLocation.Id].Parent = location;
matrix[newLocation.Id].Cost = cost;
matrix[newLocation.Id].Heuristic = GetHeuristic(newLocation, endCell);
openList.Push(newLocation);
}
matrix[newLocation.Id].Status = NodeState.Open;
}
counter++;
}
if (success)
{
var node = matrix[endCell.Id];
// use auxiliary end if not found
if (node.Status != NodeState.Closed)
{
node = matrix[endCellAux.Id];
}
while (node.Parent != null)
{
closedList.Add(node);
node = matrix[node.Parent.Id];
}
closedList.Add(node);
}
closedList.Reverse();
if (allowDiagonals)
{
return(CreateAndOptimisePath(closedList));
}
else
{
if (movementPoints > 0 && closedList.Count > movementPoints + 1)
{
return(new Path(m_map, closedList.Take(movementPoints + 1).Select(entry => entry.Cell)));
}
return(new Path(m_map, closedList.Select(entry => entry.Cell)));
}
}
/// <summary>
/// 对比测试用的非启发算法，不要使用
/// </summary>
/// <param name="startX"></param>
/// <param name="startY"></param>
/// <param name="endX"></param>
/// <param name="endY"></param>
/// <param name="eightDirection"></param>
/// <param name="useBinaryHeap"></param>
/// <returns></returns>
public List <MapNode> findPathByXYNoInspire(int startX, int startY, int endX, int endY, bool eightDirection = false)
{
//Debug.Log(startX + " " + startY + " " + endX + " " + endY);
List <MapNode> result = new List <MapNode>();
if (startX == endX && startY == endY)
{
result.Add(_mapdata[startY, startX]);
return(result);
}
MapNode start = _mapdata[startY, startX];
MapNode end = _mapdata[endY, endX];
_open = new ListOpenList <MapNode>();
_open.push(start);
start.parentNode = null;
start.g = start.h = start.f = 0;
end.parentNode = null;
_closed = new List <MapNode>();
while (_open.getCount() > 0 && (_closed.IndexOf(end) == -1))
{
//不去是用启发算法去找一个可能最优的,而是随便找一个来计算
MapNode mn = (_open as ListOpenList <MapNode>).pop();
//检查周围
if (!eightDirection)
{
for (int i = 0; i < 4; i++)
{
int _x = mn.x + _xd[i];
int _y = mn.y + _yd[i];
if (_x < 0 || _y < 0 || (_x >= _width) || (_y >= _height))
{
continue;
}
MapNode testN = _mapdata[_y, _x];
if (_closed.IndexOf(testN) != -1)
{
continue;
}
if (!testN.isPassAble())
{
_closed.Add(testN);
testN.parentNode = mn;
continue;
}
if (_open.indexOf(testN) != -1)
{
if (testN.g > mn.g + testN.weight)
{
testN.g = mn.g + testN.weight;
testN.parentNode = mn;
}
continue;
}
else
{
_open.push(testN);
testN.parentNode = mn;
testN.g = mn.g + testN.weight;
}
}
}
else
{
for (int i = 0; i < 8; i++)
{
int _x = mn.x + _xde[i];
int _y = mn.y + _yde[i];
if (_x < 0 || _y < 0 || (_x >= _width) || (_y >= _height))
{
continue;
}
MapNode testN = _mapdata[_y, _x];
if (_closed.IndexOf(testN) != -1)
{
continue;
}
if (!testN.isPassAble())
{
_closed.Add(testN);
testN.parentNode = mn;
continue;
}
if (_open.indexOf(testN) != -1)
{
if (testN.g > mn.g + testN.weight)
{
testN.g = mn.g + testN.weight;
testN.parentNode = mn;
}
continue;
}
else
{
_open.push(testN);
testN.parentNode = mn;
testN.g = mn.g + testN.weight;
}
}
}
//加入closed
_closed.Add(mn);
}
MapNode c = end;
while (c != null)
{
result.Insert(0, c);
c = c.parentNode;
}
if (result.Count == 1)
{
result.Clear();
result.Add(start);
}
if (result.Count < 2)
{
//找不到路
}
return(result);
}
