int CalcH(AStarNode end, AStarNode node)
{
return(STEP * (Math.Abs(end.X - node.X) + Math.Abs(end.Y - node.Y)));
}
public override int GetNeighbourNodeCount(AStarNode node)
{
List <GraphEdge> edgeList = GetEdgeList(node.id);
return(edgeList != null ? edgeList.Count : 0);
}
public override AStarNode GetNeighbourNode(AStarNode node, int index)
{
List <GraphEdge> edgeList = GetEdgeList(node.id);
return(GetNode(edgeList[index].to) as AStarNode);
}
private static IEnumerator AStartPathFinding(PathfindNode startPoint, PathfindNode endPoint, PathComplete Callback, Dictionary <Point, PathfindNode> walkableArea, WalkType walkType, int loopMaxCount = int.MaxValue)
{
if (startPoint.Point == endPoint.Point)
{
Callback.Invoke(null);
}
int startGroup = startPoint.GetParameter(walkType).Group;
if (PathFindingManager.PrecalculatedPaths != null)
{
// foreach(PointCouple couple in PathFindingManager.PrecalculatedPaths.Keys)
// {
// //Debug.Log(couple);
// if(couple.StartPoint == startPoint.Point && couple.EndPoint == endPoint.Point)
// {
// // Debug.Log("FIND COUPLE");
// }
// }
if (PathFindingManager.PrecalculatedPaths.ContainsKey(new PointCouple(startPoint.Point, endPoint.Point)))
{
//instance.Processing = false;
Callback.Invoke(PathFindingManager.PrecalculatedPaths[new PointCouple(startPoint.Point, endPoint.Point)].Clone());
yield break;
}
}
Dictionary <Point, PathfindNode> reachablePoints = walkableArea; // s_instance.CompleteArea;
if (reachablePoints.ContainsKey(endPoint.Point) == false)
{
//Debug.Log("Node not reachable: "+endPoint.Point);
//instance.Processing = false;
Callback.Invoke(null);
yield break;
}
// if (BelongSameGroup (startPoint,endPoint,walkType) == false) {
//
// int endGroup = endPoint.GetParameter (walkType).Group;
// List<int> walkedGroup = new List<int>();
// List<PathfindNode> intermediates = new List<PathfindNode>();
// intermediates.Insert(0,startPoint);
// bool succes = GetIntermediatesPoint (startGroup, endGroup, walkType,ref walkedGroup,ref intermediates,endPoint);
// if(succes == false)
// {
// //instance.Processing = false;
// Callback.Invoke(null);
// yield break;
// }
// intermediates.Add(endPoint);
// Enemy PathOwner = Callback.Target as Enemy;
// PathOwner.ClearStepedPath();
// for(int i =0;i<intermediates.Count-1;i++)
// {
// yield return instance.StartCoroutine (AStartPathFinding (intermediates [i], intermediates [i+1], PathOwner.BuildPathFromStepCallback, walkableArea, walkType, loopMaxCount));
// }
//
// //instance.Processing = false;
// Callback.Invoke(PathOwner.GetStepedPath());
// yield break;
// }
List <AStarNode> openSet = new List <AStarNode>();
Dictionary <Point, AStarNode> openSetDico = new Dictionary <Point, AStarNode>();
openSet.Add(new AStarNode(startPoint));
openSetDico.Add(startPoint.Point, openSet[openSet.Count - 1]);
Dictionary <Point, AStarNode> closedSet = new Dictionary <Point, AStarNode>();
float fscore; // = (endPoint-startPoint).MagnitudeZero();
fscore = Point.MagnitudeZero(endPoint.Point - startPoint.Point); //+(1 -Vector2.Dot(toNeighbor,toEnd))*20;
int loopLimit = 0;
while (openSet.Count > 0 && loopMaxCount > 0)
{
//Debug.Log(openSet.Count);
loopMaxCount--;
loopLimit++;
openSet.Sort();
AStarNode current = openSet[0];
if (current.Position == endPoint)
{
Path path = new Path(endPoint);
while (current.Parent != null)
{
path.steps.Add(current.Position);
current = current.Parent;
}
path.steps.Reverse();
if (PathFindingManager.PrecalculatedPaths == null)
{
PathFindingManager.PrecalculatedPaths = new Dictionary <PointCouple, Path>();
}
PointCouple couple = new PointCouple(startPoint.Point, endPoint.Point);
if (PathFindingManager.PrecalculatedPaths.ContainsKey(couple))
{
Debug.Log("Try add existing couple: " + couple);
}
else
{
PathFindingManager.PrecalculatedPaths.Add(couple, path);
}
//instance.Processing = false;
Callback.Invoke(path.Clone());
yield break;
}
openSetDico.Remove(openSet[0].Position.Point);
openSet.RemoveAt(0);
closedSet.Add(current.Position.Point, current);
if (loopLimit > 10)
{
yield return(new WaitForEndOfFrame());
loopLimit = 0;
}
for (int i = 0; i < Neighbourhood.Length; i++)
{
PathFindNeighbourg neighbourPath = Neighbourhood [i];
PathfindNode neighbour = new PathfindNode(neighbourPath.offset);
PathfindNode neigbourRealPosition = neighbour + current.Position;
if (reachablePoints.ContainsKey(neigbourRealPosition.Point) == false)
{
continue;
}
AStarNode AstarNeighbor = new AStarNode(neigbourRealPosition);
AstarNeighbor.Position = neigbourRealPosition;
//current.Position;
AstarNeighbor.Parent = current;
if (closedSet.ContainsKey(AstarNeighbor.Position.Point))
{
continue;
}
bool containsStartGroup = instance.CompleteNodeArea.GetNodeAt(neigbourRealPosition.Point).GetParameter(walkType).ContainsGroupOf(endPoint, walkType);
if (containsStartGroup == false)
{
continue;
}
float tentativeGscore = current.gscore + neighbourPath.cost;
AStarNode InSetNeighbor = null;
//GetNodeInList(AstarNeighbor,openSet);
if (openSetDico.ContainsKey(AstarNeighbor.Position.Point))
{
InSetNeighbor = openSetDico [AstarNeighbor.Position.Point];
}
if (InSetNeighbor == null || tentativeGscore < InSetNeighbor.gscore)
{
bool needAddToSet = false;
if (InSetNeighbor == null)
{
InSetNeighbor = new AStarNode(neigbourRealPosition);
needAddToSet = true;
}
InSetNeighbor.Parent = current;
InSetNeighbor.gscore = tentativeGscore;
InSetNeighbor.fscore = InSetNeighbor.gscore + heuristique(AstarNeighbor, endPoint.Point, current.Position.Point);
if (needAddToSet)
{
AstarNeighbor.fscore = InSetNeighbor.fscore;
AstarNeighbor.gscore = InSetNeighbor.gscore;
openSet.Add(AstarNeighbor);
openSetDico.Add(AstarNeighbor.Position.Point, openSet [openSet.Count - 1]);
}
}
}
}
//instance.Processing = false;
Callback.Invoke(null);
//return null;
}
public static Path AStartPathFinding(PathfindNode startPoint, PathfindNode endPoint, Dictionary <Point, PathfindNode> reachablePoints, int loopMaxCount = int.MaxValue)
{
if (reachablePoints.ContainsKey(endPoint.Point) == false)
{
return(null);
}
if (PathFindingManager.PrecalculatedPaths != null && PathFindingManager.PrecalculatedPaths.ContainsKey(new PointCouple(startPoint.Point, endPoint.Point)))
{
return(PathFindingManager.PrecalculatedPaths[new PointCouple(startPoint.Point, endPoint.Point)].Clone());
}
List <AStarNode> openSet = new List <AStarNode>();
Dictionary <Point, AStarNode> openSetDico = new Dictionary <Point, AStarNode>();
openSet.Add(new AStarNode(startPoint));
openSetDico.Add(startPoint.Point, openSet[openSet.Count - 1]);
Dictionary <Point, AStarNode> closedSet = new Dictionary <Point, AStarNode>();
float fscore; // = (endPoint-startPoint).MagnitudeZero();
fscore = Point.MagnitudeZero(endPoint.Point - startPoint.Point); //+(1 -Vector2.Dot(toNeighbor,toEnd))*20;
int loopLimit = loopMaxCount;
while (openSet.Count > 0 && loopLimit > 0)
{
//Debug.Log(openSet.Count);
loopLimit--;
openSet.Sort();
AStarNode current = openSet[0];
if (current.Position == endPoint)
{
Path path = new Path(endPoint);
while (current.Parent != null)
{
path.steps.Add(instance.GetNodeAt(current.Position.Point));
current = current.Parent;
}
path.steps.Reverse();
if (PathFindingManager.PrecalculatedPaths == null)
{
PathFindingManager.PrecalculatedPaths = new Dictionary <PointCouple, Path>();
}
PathFindingManager.PrecalculatedPaths.Add(new PointCouple(startPoint.Point, endPoint.Point), path);
return(path.Clone());
}
openSetDico.Remove(openSet[0].Position.Point);
openSet.RemoveAt(0);
if (closedSet.ContainsKey(current.Position.Point))
{
int fdfg = 4;
}
closedSet.Add(current.Position.Point, current);
for (int i = 0, NeighbourhoodLength = Neighbourhood.Length; i < NeighbourhoodLength; i++)
{
PathFindNeighbourg neighbourPath = Neighbourhood [i];
PathfindNode neighbour = new PathfindNode(neighbourPath.offset);
PathfindNode neigbourRealPosition = neighbour + current.Position;
if (reachablePoints.ContainsKey(neigbourRealPosition.Point) == false)
{
continue;
}
AStarNode AstarNeighbor = new AStarNode(neigbourRealPosition);
AstarNeighbor.Position = neigbourRealPosition;
//current.Position;
AstarNeighbor.Parent = current;
if (closedSet.ContainsKey(AstarNeighbor.Position.Point))
{
continue;
}
float tentativeGscore = current.gscore + neighbourPath.cost;
AStarNode InSetNeighbor = null;
//GetNodeInList(AstarNeighbor,openSet);
if (openSetDico.ContainsKey(AstarNeighbor.Position.Point))
{
InSetNeighbor = openSetDico [AstarNeighbor.Position.Point];
}
if (InSetNeighbor == null || tentativeGscore < InSetNeighbor.gscore)
{
bool needAddToSet = false;
if (InSetNeighbor == null)
{
InSetNeighbor = new AStarNode(neigbourRealPosition);
needAddToSet = true;
}
InSetNeighbor.Parent = current;
InSetNeighbor.gscore = tentativeGscore;
InSetNeighbor.fscore = InSetNeighbor.gscore + heuristique(AstarNeighbor, endPoint.Point, current.Position.Point);
if (needAddToSet)
{
AstarNeighbor.fscore = InSetNeighbor.fscore;
AstarNeighbor.gscore = InSetNeighbor.gscore;
openSet.Add(AstarNeighbor);
openSetDico.Add(AstarNeighbor.Position.Point, openSet [openSet.Count - 1]);
}
}
}
}
return(null);
}
public override AStarNode GetNeighbourNode(AStarNode node, int index)
{
int id = (node as NavMeshNode).connections[index];
return(GetNode(id) as AStarNode);
}
public override int GetNeighbourNodeCount(AStarNode node)
{
return((node as NavMeshNode).connections.Length);
}
public override int GetNeighbourNodeCount(AStarNode node)
{
return(NEIGHBOUR_COUNT);
}
public bool IsNodeWalkable(AStarNode currNode)
{
return(currNode.Flag == AStarNode.ENodeState.walkable);
}
public FP CalculateHDist(AStarNode node1, AStarNode node2)
{
return(CalculateHValue((GridNode)node1, (GridNode)node2, _heuristic));
}