diff --git a/cpp/Findpathsimproved.cpp b/cpp/Findpathsimproved.cpp new file mode 100644 index 0000000..e651e89 --- /dev/null +++ b/cpp/Findpathsimproved.cpp @@ -0,0 +1,231 @@ +#include +#include +#include +#include +#include +#include +#include "stlastar.h" // Include the A* search library + +// Define debugging constants +#define DEBUG_LISTS 0 +#define DEBUG_LIST_LENGTHS_ONLY 0 + +// Define the dimensions of the map +const int MAP_WIDTH = 20; +const int MAP_HEIGHT = 20; + +// Create a 2D array to represent the world map +std::vector world_map(MAP_WIDTH * MAP_HEIGHT, 0); + +// Function to get the map value at a given (x, y) coordinate +int GetMap(int x, int y) { + if (x < 0 || x >= MAP_WIDTH || y < 0 || y >= MAP_HEIGHT) { + return 9; // Out of bounds, return an obstacle value + } + return world_map[(y * MAP_WIDTH) + x]; +} + +// Define the map search node class +class MapSearchNode { +public: + int x; // x-coordinate of the node + int y; // y-coordinate of the node + + MapSearchNode() : x(0), y(0) {} + MapSearchNode(int px, int py) : x(px), y(py) {} + + // Heuristic function to estimate the distance to the goal + float GoalDistanceEstimate(MapSearchNode &nodeGoal); + + // Check if this node is the goal + bool IsGoal(MapSearchNode &nodeGoal); + + // Generate successors for this node + bool GetSuccessors(AStarSearch *astarsearch, MapSearchNode *parent_node); + + // Get the cost to move to a successor node + float GetCost(MapSearchNode &successor); + + // Check if two nodes are the same state + bool IsSameState(MapSearchNode &rhs); + + // Hash function for the node + size_t Hash(); + + // Print node information + void PrintNodeInfo(); +}; + +// Check if two nodes are in the same state +bool MapSearchNode::IsSameState(MapSearchNode &rhs) { + return (x == rhs.x) && (y == rhs.y); +} + +// Hash function for the node +size_t MapSearchNode::Hash() { + size_t h1 = std::hash{}(x); + size_t h2 = std::hash{}(y); + return h1 ^ (h2 << 1); +} + +// Print information about the node +void MapSearchNode::PrintNodeInfo() { + const int strSize = 100; + char str[strSize]; + std::snprintf(str, strSize, "Node position : (%d,%d)\n", x, y); + std::cout << str; +} + +// Heuristic function to estimate the distance to the goal +float MapSearchNode::GoalDistanceEstimate(MapSearchNode &nodeGoal) { + return std::abs(x - nodeGoal.x) + std::abs(y - nodeGoal.y); +} + +// Check if this node is the goal +bool MapSearchNode::IsGoal(MapSearchNode &nodeGoal) { + return (x == nodeGoal.x) && (y == nodeGoal.y); +} + +// Generate successors for this node +bool MapSearchNode::GetSuccessors(AStarSearch *astarsearch, MapSearchNode *parent_node) { + int parent_x = -1; + int parent_y = -1; + + if (parent_node) { + parent_x = parent_node->x; + parent_y = parent_node->y; + } + + MapSearchNode NewNode; + + // Generate successors by moving in four directions + if ((GetMap(x - 1, y) < 9) && !((parent_x == x - 1) && (parent_y == y))) { + NewNode = MapSearchNode(x - 1, y); + astarsearch->AddSuccessor(NewNode); + } + + if ((GetMap(x, y - 1) < 9) && !((parent_x == x) && (parent_y == y - 1))) { + NewNode = MapSearchNode(x, y - 1); + astarsearch->AddSuccessor(NewNode); + } + + if ((GetMap(x + 1, y) < 9) && !((parent_x == x + 1) && (parent_y == y))) { + NewNode = MapSearchNode(x + 1, y); + astarsearch->AddSuccessor(NewNode); + } + + if ((GetMap(x, y + 1) < 9) && !((parent_x == x) && (parent_y == y + 1))) { + NewNode = MapSearchNode(x, y + 1); + astarsearch->AddSuccessor(NewNode); + } + + return true; +} + +// Get the cost to move to a successor node +float MapSearchNode::GetCost(MapSearchNode &successor) { + return static_cast(GetMap(x, y)); +} + +// Main function +int main(int argc, char *argv[]) { + std::cout << "STL A* Search implementation\n(C)2001 Justin Heyes-Jones\n"; + + // Create an instance of the A* search class + AStarSearch astarsearch; + + unsigned int SearchCount = 0; + const unsigned int NumSearches = 1; + + while (SearchCount < NumSearches) { + // Create a start state with random coordinates + MapSearchNode nodeStart; + nodeStart.x = std::rand() % MAP_WIDTH; + nodeStart.y = std::rand() % MAP_HEIGHT; + + // Define the goal state with random coordinates + MapSearchNode nodeEnd; + nodeEnd.x = std::rand() % MAP_WIDTH; + nodeEnd.y = std::rand() % MAP_HEIGHT; + + // Set the start and goal states + astarsearch.SetStartAndGoalStates(nodeStart, nodeEnd); + + unsigned int SearchState; + unsigned int SearchSteps = 0; + + do { + // Perform a single step of the A* search + SearchState = astarsearch.SearchStep(); + SearchSteps++; + +#if DEBUG_LISTS + std::cout << "Steps:" << SearchSteps << "\n"; + int len = 0; + std::cout << "Open:\n"; + MapSearchNode *p = astarsearch.GetOpenListStart(); + while (p) { + len++; +#if !DEBUG_LIST_LENGTHS_ONLY + ((MapSearchNode *)p)->PrintNodeInfo(); +#endif + p = astarsearch.GetOpenListNext(); + } + + std::cout << "Open list has " << len << " nodes\n"; + + len = 0; + std::cout << "Closed:\n"; + p = astarsearch.GetClosedListStart(); + while (p) { + len++; +#if !DEBUG_LIST_LENGTHS_ONLY + p->PrintNodeInfo(); +#endif + p = astarsearch.GetClosedListNext(); + } + std::cout << "Closed list has " << len << " nodes\n"; +#endif + + } while (SearchState == AStarSearch::SEARCH_STATE_SEARCHING); + + if (SearchState == AStarSearch::SEARCH_STATE_SUCCEEDED) { + std::cout << "Search found the goal state\n"; + + MapSearchNode *node = astarsearch.GetSolutionStart(); + +#if DISPLAY_SOLUTION + std::cout << "Displaying solution\n"; +#endif + int steps = 0; + + node->PrintNodeInfo(); + for (;;) { + node = astarsearch.GetSolutionNext(); + + if (!node) { + break; + } + + node->PrintNodeInfo(); + steps++; + } + + std::cout << "Solution steps: " << steps << std::endl; + + // Free the solution nodes when done + astarsearch.FreeSolutionNodes(); + } else if (SearchState == AStarSearch::SEARCH_STATE_FAILED) { + std::cout << "Search terminated. Did not find the goal state\n"; + } + + // Display the number of loops the search went through + std::cout << "SearchSteps: " << SearchSteps << "\n"; + + SearchCount++; + + astarsearch.EnsureMemoryFreed(); + } + + return 0; +}