public void FindNextTarget(Vector2 position, Graph.Graph graph) { //Find closest Node to self and goal GraphNode closestToGoal = graph.FindClosestNode(position); GraphNode closestToMe = graph.FindClosestNode(this.Position); float currDistance = float.MaxValue; GraphNode currNode = closestToGoal; /* * GraphNode prevNode = closestToMe; * while (currNode != closestToMe && closestToMe.Neighbors[currNode] != currNode) * { * prevNode = currNode; * currNode = closestToMe.Neighbors[currNode]; * } */ //Find closest Node to player from neighbors foreach (GraphNode node in closestToMe.Neighbors.Values) { if (!RecentTargets.Contains(node.Position)) { float thisDistance = Vector2.Distance(node.Position, closestToGoal.Position); if (thisDistance < currDistance) { currNode = node; currDistance = thisDistance; } } } //Keep track of recent nodes so we don't loop while (RecentTargets.Count > 3) { RecentTargets.RemoveAt(0); } //path.Reverse(); //path.Insert(0, closestToMe); //showPath(); //Set target and add to recenttargets so we don't move backwards CurrentTarget = currNode.Position; RecentTargets.Add(CurrentTarget); }