Exemplo n.º 1
0
        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);
        }
Exemplo n.º 2
0
        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;
        }