Пример #1
0
    // Set Start and goal states
    public void SetStartAndGoalStates(MapSearchNode Start, MapSearchNode Goal)
    {
        m_Start = AllocateNode();
        m_Goal  = AllocateNode();

        System.Diagnostics.Debug.Assert((m_Start != null && m_Goal != null));

        m_Start.m_UserState = Start;
        m_Goal.m_UserState  = Goal;

        m_State = SearchState.Searching;

        // Initialise the AStar specific parts of the Start Node
        // The user only needs fill out the state information
        m_Start.g      = 0;
        m_Start.h      = m_Start.m_UserState.GoalDistanceEstimate(m_Goal.m_UserState);
        m_Start.f      = m_Start.g + m_Start.h;
        m_Start.parent = null;

        // Push the start node on the Open list
        m_OpenList.Add(m_Start);

        // Initialise counter for search steps
        m_Steps = 0;

#if PATHFIND_DEBUG
        System.Console.WriteLine("Starting pathfind. Start: " + m_Start.m_UserState.position + ", Goal: " + m_Goal.m_UserState.position);
#endif
    }
Пример #2
0
    // Here's the heuristic function that estimates the distance from a Node
    // to the Goal.
    public float GoalDistanceEstimate(MapSearchNode nodeGoal)
    {
        double X = (double)position.x - (double)nodeGoal.position.x;
        double Y = (double)position.y - (double)nodeGoal.position.y;

        return((float)System.Math.Sqrt((X * X) + (Y * Y)));
    }
Пример #3
0
 void AddNeighbourNode(int xOffset, int yOffset, NodePosition parentPos, AStarPathfinder aStarSearch)
 {
     if (ValidNeigbour(xOffset, yOffset) &&
         !(parentPos.x == position.x + xOffset && parentPos.y == position.y + yOffset))
     {
         NodePosition  neighbourPos = new NodePosition(position.x + xOffset, position.y + yOffset);
         MapSearchNode newNode      = pathfinder.AllocateMapSearchNode(neighbourPos);
         aStarSearch.AddSuccessor(newNode);
     }
 }
Пример #4
0
    // This generates the successors to the given Node. It uses a helper function called
    // AddSuccessor to give the successors to the AStar class. The A* specific initialisation
    // is done for each node internally, so here you just set the state information that
    // is specific to the application
    public bool GetSuccessors(AStarPathfinder aStarSearch, MapSearchNode parentNode)
    {
        NodePosition parentPos = new NodePosition(0, 0);

        if (parentNode != null)
        {
            parentPos = parentNode.position;
        }

        // push each possible move except allowing the search to go backwards
        AddNeighbourNode(-1, 0, parentPos, aStarSearch);
        AddNeighbourNode(0, -1, parentPos, aStarSearch);
        AddNeighbourNode(1, 0, parentPos, aStarSearch);
        AddNeighbourNode(0, 1, parentPos, aStarSearch);

        return(true);
    }
Пример #5
0
    // User calls this to add a successor to a list of successors
    // when expanding the search frontier
    public bool AddSuccessor(MapSearchNode state)
    {
        Node node = AllocateNode();

        if (node != null)
        {
            node.m_UserState = state;
            m_Successors.Add(node);

            if (m_Successors.Count > successorListHighWaterMark)
            {
                successorListHighWaterMark = m_Successors.Count;
            }
            return(true);
        }

        return(false);
    }
Пример #6
0
    private void RenderTiles(AStarPathfinder pathfinder)
    {
        MapSearchNode node = pathfinder.GetSolutionStart();

        for ( ;;)
        {
            node = pathfinder.GetSolutionNext();

            if (node == null)
            {
                break;
            }

            GameObject obj = objs[node.position.x, node.position.y];
            if (obj != null)
            {
                obj.GetComponent <MeshRenderer>().material.color = Color.green;
            }
        }
        ;
    }
Пример #7
0
    // constructor just initialises private data
    public AStarPathfinder()
    {
        // Allocate all lists
        m_OpenList   = new List <Node>(kPreallocatedOpenListSlots);
        m_ClosedList = new List <Node>(kPreallocatedClosedListSlots);
        m_Successors = new List <Node>(kPreallocatedSuccessorSlots);

        m_FixedSizeAllocator = new List <Node>(kPreallocatedNodes);
        for (int i = 0; i < kPreallocatedNodes; ++i)
        {
            Node n = new Node();
            m_FixedSizeAllocator.Add(n);
        }

        mapSearchNodePool = new List <MapSearchNode>(kPreallocatedMapSearchNodes);
        for (int i = 0; i < kPreallocatedMapSearchNodes; ++i)
        {
            MapSearchNode msn = new MapSearchNode(this);
            mapSearchNodePool.Add(msn);
        }
    }
	// constructor just initialises private data
	public AStarPathfinder()
	{
		// Allocate all lists		
		m_OpenList = new List<Node>(kPreallocatedOpenListSlots);
		m_ClosedList = new List<Node>(kPreallocatedClosedListSlots);
		m_Successors = new List<Node>(kPreallocatedSuccessorSlots);

		m_FixedSizeAllocator = new List<Node>(kPreallocatedNodes);
		for (int i = 0; i < kPreallocatedNodes; ++i)
		{
			Node n = new Node();
			m_FixedSizeAllocator.Add(n);
		}

		mapSearchNodePool = new List<MapSearchNode>(kPreallocatedMapSearchNodes);
		for (int i = 0; i < kPreallocatedMapSearchNodes; ++i)
		{
			MapSearchNode msn = new MapSearchNode(this);
			mapSearchNodePool.Add(msn);
		}
	}
	// User calls this to add a successor to a list of successors
	// when expanding the search frontier
	public bool AddSuccessor(MapSearchNode state)
	{
		Node node = AllocateNode();

		if (node != null)
		{
			node.m_UserState = state;
			m_Successors.Add(node);

			if (m_Successors.Count > successorListHighWaterMark)
			{
				successorListHighWaterMark = m_Successors.Count;
			}
			return true;
		}

		return false;
	}
	// Set Start and goal states
	public void SetStartAndGoalStates(MapSearchNode Start, MapSearchNode Goal)
	{
		m_Start = AllocateNode();
		m_Goal = AllocateNode();

		System.Diagnostics.Debug.Assert((m_Start != null && m_Goal != null));

		m_Start.m_UserState = Start;
		m_Goal.m_UserState = Goal;

		m_State = SearchState.Searching;

		// Initialise the AStar specific parts of the Start Node
		// The user only needs fill out the state information
		m_Start.g = 0; 
		m_Start.h = m_Start.m_UserState.GoalDistanceEstimate( m_Goal.m_UserState );
		m_Start.f = m_Start.g + m_Start.h;
		m_Start.parent = null;

		// Push the start node on the Open list
		m_OpenList.Add(m_Start);

		// Initialise counter for search steps
		m_Steps = 0;

#if PATHFIND_DEBUG
		System.Console.WriteLine("Starting pathfind. Start: " + m_Start.m_UserState.position + ", Goal: " + m_Goal.m_UserState.position);
#endif
	}
Пример #11
0
    static bool Pathfind(NodePosition startPos, NodePosition goalPos, AStarPathfinder pathfinder)
    {
        // Reset the allocated MapSearchNode pointer
        pathfinder.InitiatePathfind();

        // Create a start state
        MapSearchNode nodeStart = pathfinder.AllocateMapSearchNode(startPos);

        // Define the goal state
        MapSearchNode nodeEnd = pathfinder.AllocateMapSearchNode(goalPos);

        // Set Start and goal states
        pathfinder.SetStartAndGoalStates(nodeStart, nodeEnd);

        // Set state to Searching and perform the search
        AStarPathfinder.SearchState searchState = AStarPathfinder.SearchState.Searching;
        uint searchSteps = 0;

        do
        {
            searchState = pathfinder.SearchStep();
            searchSteps++;
        }while (searchState == AStarPathfinder.SearchState.Searching);

        // Search complete
        bool pathfindSucceeded = (searchState == AStarPathfinder.SearchState.Succeeded);

        if (pathfindSucceeded)
        {
            // Success
            Path newPath          = new Path();
            int  numSolutionNodes = 0;                  // Don't count the starting cell in the path length

            // Get the start node
            MapSearchNode node = pathfinder.GetSolutionStart();
            newPath.Add(node.position);
            ++numSolutionNodes;

            // Get all remaining solution nodes
            for ( ;;)
            {
                node = pathfinder.GetSolutionNext();

                if (node == null)
                {
                    break;
                }

                ++numSolutionNodes;
                newPath.Add(node.position);
            }
            ;

            // Once you're done with the solution we can free the nodes up
            pathfinder.FreeSolutionNodes();

            System.Console.WriteLine("Solution path length: " + numSolutionNodes);
            System.Console.WriteLine("Solution: " + newPath.ToString());
        }
        else if (searchState == AStarPathfinder.SearchState.Failed)
        {
            // FAILED, no path to goal
            System.Console.WriteLine("Pathfind FAILED!");
        }

        return(pathfindSucceeded);
    }
Пример #12
0
    // Main


    public static int main(string[] args)
    {

      Console.WriteLine("STL A* Search implementation\n(C)2001 Justin Heyes-Jones");

      // Our sample problem defines the world as a 2d array representing a terrain
      // Each element contains an integer from 0 to 5 which indicates the cost 
      // of travel across the terrain. Zero means the least possible difficulty 
      // in travelling (think ice rink if you can skate) whilst 5 represents the 
      // most difficult. 9 indicates that we cannot pass.

      // Create an instance of the search class...

      AStarSearch astarsearch = new AStarSearch();
      bool[] path = new bool[MAP_HEIGHT * MAP_WIDTH];

      // Create a start state
      MapSearchNode nodeStart = new MapSearchNode();
      nodeStart.x = 0;
      nodeStart.y = 0;

      // Define the goal state
      MapSearchNode nodeEnd = new MapSearchNode();
      nodeEnd.x = 19;
      nodeEnd.y = 19;

      // Set Start and goal states

      astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd);

      SearchState searchState;
      uint searchSteps = 0;

      do
      {
        searchState = astarsearch.SearchStep();
        searchSteps++;
      }
      while (searchState == SearchState.Searching);

      if (searchState == SearchState.Succeeded)
      {
        Console.Write("Search found goal state");
        MapSearchNode node = astarsearch.GetSolutionStart() as MapSearchNode;

        Console.WriteLine( "Displaying solution");
        int steps = 0;
        node.PrintNodeInfo();
        for (; ; )
        {
          node = astarsearch.GetSolutionNext() as MapSearchNode;
          if (node == null)
          {
            break;
          }

          path[node.y * MAP_HEIGHT + node.x] = true;
          //node.PrintNodeInfo();
          steps++;
        };
        for (int y = 0; y < MAP_HEIGHT; y++)
        {
          for (int x = 0; x < MAP_WIDTH; x++)
          {
            Console.Write(path[y * MAP_HEIGHT + x] ? "x" : "o");
          }
          Console.WriteLine();
        }
        Console.WriteLine("Solution steps {0}", steps);
        // Once you're done with the solution you can free the nodes up
        astarsearch.FreeSolutionNodes();
      }
      else if (searchState == SearchState.Failed)
      {
        Console.WriteLine("Search terminated. Did not find goal state");

      }

      // Display the number of loops the search went through
      Console.WriteLine("searchSteps : " + searchSteps);

      return 0;
    }
Пример #13
0
 public bool IsSameState(MapSearchNode rhs)
 {
     return(position.x == rhs.position.x &&
            position.y == rhs.position.y);
 }
Пример #14
0
 // given this node, what does it cost to move to successor. In the case
 // of our map the answer is the map terrain value at this node since that is
 // conceptually where we're moving
 public float GetCost(MapSearchNode successor)
 {
     // Implementation specific
     return(m_map.GetMap(successor.position.x, successor.position.y));
 }
Пример #15
0
    IEnumerator KK()
    {
        pathfinder = new AStarPathfinder();
        // Pathfind(new NodePosition(0, 0), new NodePosition(2, 4), pathfinder);
        // Reset the allocated MapSearchNode pointer
        pathfinder.InitiatePathfind();

        // Create a start state
        MapSearchNode nodeStart = pathfinder.AllocateMapSearchNode(new NodePosition(0, 0));

        // Define the goal state
        MapSearchNode nodeEnd = pathfinder.AllocateMapSearchNode(new NodePosition(GOAL_X, GOAL_Y));

        // Set Start and goal states
        pathfinder.SetStartAndGoalStates(nodeStart, nodeEnd);

        // Set state to Searching and perform the search
        AStarPathfinder.SearchState searchState = AStarPathfinder.SearchState.Searching;
        uint searchSteps = 0;

        yield return(null);

        do
        {
            // if (Input.GetKey(KeyCode.J))
            {
                Debug.Log("xx-- getkeydown");
                searchState = pathfinder.SearchStep();
                searchSteps++;

                RenderTiles(pathfinder);
            }

            if (searchState == AStarPathfinder.SearchState.Searching)
            {
                yield return(null);
            }
        }while (searchState == AStarPathfinder.SearchState.Searching);

        // Search complete
        bool pathfindSucceeded = (searchState == AStarPathfinder.SearchState.Succeeded);

        if (pathfindSucceeded)
        {
            // Success
            Path newPath          = new Path();
            int  numSolutionNodes = 0;                  // Don't count the starting cell in the path length

            // Get the start node
            MapSearchNode node = pathfinder.GetSolutionStart();
            newPath.Add(node.position);
            ++numSolutionNodes;

            // Get all remaining solution nodes
            for ( ;;)
            {
                node = pathfinder.GetSolutionNext();

                if (node == null)
                {
                    break;
                }

                ++numSolutionNodes;
                newPath.Add(node.position);

                //
                GameObject obj = objs[node.position.x, node.position.y];
                if (obj != null)
                {
                    obj.GetComponent <MeshRenderer>().material.color = Color.green;
                }
            }
            ;

            // Once you're done with the solution we can free the nodes up
            pathfinder.FreeSolutionNodes();

            Debug.Log("xx-- Solution path length: " + numSolutionNodes);
            Debug.Log("xx-- Solution: " + newPath.ToString());
        }
        else if (searchState == AStarPathfinder.SearchState.Failed)
        {
            // FAILED, no path to goal
            Debug.Log("xx-- Pathfind FAILED!");
        }
    }
Пример #16
0
      // This generates the successors to the given Node. It uses a helper function called
      // AddSuccessor to give the successors to the AStar class. The A* specific initialisation
      // is done for each node internally, so here you just set the state information that
      // is specific to the application
      public bool GetSuccessors(AStarSearch astarsearch, PuzzleState parentState)
      {
        MapSearchNode parent_node = parentState as MapSearchNode;

        int parent_x = -1;
        int parent_y = -1;

        if (parent_node != null)
        {
          parent_x = (int)parent_node.x;
          parent_y = (int)parent_node.y;
        }


        MapSearchNode NewNode;

        // push each possible move except allowing the search to go backwards

        if ((GetMap(x - 1, y) < 9)
          && !((parent_x == x - 1) && (parent_y == y))
          )
        {
          NewNode = new MapSearchNode(x - 1, y);
          astarsearch.AddSuccessor(NewNode);
        }

        if ((GetMap(x, y - 1) < 9)
          && !((parent_x == x) && (parent_y == y - 1))
          )
        {
          NewNode = new MapSearchNode(x, y - 1);
          astarsearch.AddSuccessor(NewNode);
        }

        if ((GetMap(x + 1, y) < 9)
          && !((parent_x == x + 1) && (parent_y == y))
          )
        {
          NewNode = new MapSearchNode(x + 1, y);
          astarsearch.AddSuccessor(NewNode);
        }


        if ((GetMap(x, y + 1) < 9)
          && !((parent_x == x) && (parent_y == y + 1))
          )
        {
          NewNode = new MapSearchNode(x, y + 1);
          astarsearch.AddSuccessor(NewNode);
        }

        return true;
      }
Пример #17
0
 public bool IsGoal(MapSearchNode nodeGoal)
 {
     return(position.x == nodeGoal.position.x && position.y == nodeGoal.position.y);
 }
Пример #18
0
        private void FindPath()
        {
            AStarPathfinder pathfinder = new AStarPathfinder();
            NodePosition    startPos = m_startGrid.m_position;
            NodePosition    endPos = m_endGrid.m_position;
            int             width, height;
            var             mapData = GetMapData(out width, out height);
            var             map     = new Map(mapData, width, height);

            pathfinder.InitiatePathfind(map);
            MapSearchNode nodeStart = pathfinder.AllocateMapSearchNode(startPos);
            MapSearchNode nodeEnd   = pathfinder.AllocateMapSearchNode(endPos);

            pathfinder.SetStartAndGoalStates(nodeStart, nodeEnd);

            AStarPathfinder.SearchState searchState = AStarPathfinder.SearchState.Searching;
            uint searchSteps = 0;

            do
            {
                searchState = pathfinder.SearchStep();
                searchSteps++;
            }while (searchState == AStarPathfinder.SearchState.Searching);

            // Search complete
            bool pathfindSucceeded = (searchState == AStarPathfinder.SearchState.Succeeded);

            if (pathfindSucceeded)
            {
                // Success
                Path newPath          = new Path();
                int  numSolutionNodes = 0;  // Don't count the starting cell in the path length

                // Get the start node
                MapSearchNode node = pathfinder.GetSolutionStart();
                newPath.Add(node.position);
                ++numSolutionNodes;

                // Get all remaining solution nodes
                for (; ;)
                {
                    node = pathfinder.GetSolutionNext();

                    if (node == null)
                    {
                        break;
                    }

                    ++numSolutionNodes;
                    newPath.Add(node.position);
                }
                ;

                // Once you're done with the solution we can free the nodes up
                pathfinder.FreeSolutionNodes();
                m_path = newPath;
                _sceneView.Repaint();
                Debug.Log("Solution path length: " + numSolutionNodes);
                Debug.Log("Solution: " + newPath.ToString());
            }
            else if (searchState == AStarPathfinder.SearchState.Failed)
            {
                // FAILED, no path to goal
                Debug.Log("Pathfind FAILED!");
            }
        }