fork download
copy 
  1. #include <vector>
  2. #include <functional>
  3. #include <stack>
  4. #include <array>
  5. #include <iostream>
  6. #include <limits>
  7. #include <unordered_set>
  8. #include <cmath>
  9.  
  10. #include <chrono>
  11.  
  12.  
  13. /**
  14.  * @brief Implementation of A* pathfinding algorithm.
  15.  * @see https://w...content-available-to-author-only...s.org/a-search-algorithm/
  16. */
  17. class ASPFWrapper {
  18.     public:
  19.         enum class Heuristic : unsigned char {
  20.             kManhattan,   // Best for 4-directional
  21.             kDiagonal,   // Best for 8-directional
  22.             kEuclidean,   // Works for any directions
  23.         };
  24.  
  25.         enum class MovementType : bool {
  26.             k4Directional,
  27.             k8Directional,
  28.         };
  29.  
  30.         enum class Status : unsigned char {
  31.             kInvalidSrc,
  32.             kInvalidDest,
  33.             kBlockedSrc,
  34.             kBlockedDest,
  35.             kCoincidents,
  36.  
  37.             kSuccess,
  38.             kFailure,
  39.         };
  40.  
  41.         template <typename T = int>
  42.         struct Cell {
  43.             T x, y;
  44.  
  45.             inline bool operator==(Cell const& other) const { return x == other.x && y == other.y; }
  46.             inline Cell operator+(Cell const& other) const { return { x + other.x, y + other.y }; }
  47.             friend inline std::ostream& operator<<(std::ostream& os, Cell const& self) { return os << '(' << self.x << ", " << self.y << ')'; }
  48.  
  49.             template <Heuristic H>
  50.             typename std::enable_if_t<H == Heuristic::kManhattan, double>
  51.             static getH(Cell const& lhs, Cell const& rhs);
  52.  
  53.             template <Heuristic H>
  54.             typename std::enable_if_t<H == Heuristic::kDiagonal, double>
  55.             static getH(Cell const& lhs, Cell const& rhs);
  56.  
  57.             template <Heuristic H>
  58.             typename std::enable_if_t<H == Heuristic::kEuclidean, double>
  59.             static getH(Cell const& lhs, Cell const& rhs);
  60.  
  61.             struct Data {
  62.                 Cell parent;
  63.                 double g, h, f = std::numeric_limits<double>::max();
  64.             };
  65.         };
  66.  
  67.         struct Result {
  68.             const Status status;
  69.             const std::stack<Cell<>> path;
  70.  
  71.             Result(Status status, std::stack<Cell<>> path = {}) : status(status), path(path) {}
  72.         };
  73.  
  74.         template <Heuristic H, MovementType M>
  75.         class ASPF {
  76.             public:
  77.                 inline ASPF(std::vector<std::vector<int>> const& grid);
  78.                 ~ASPF() = default;
  79.  
  80.                 void setBegin(Cell<std::size_t> const& begin);
  81.                 void setEnd(Cell<std::size_t> const& end);
  82.  
  83.                 Result search(Cell<> const& src, Cell<> const& dest);
  84.  
  85.             private:
  86.                 bool isValid(Cell<> const& cell) const;
  87.                 bool isUnblocked(Cell<> const& cell) const;
  88.                 bool isUnblocked(Cell<> const& parent, Cell<> const& successor) const;
  89.  
  90.                 std::stack<Cell<>> getPath(std::vector<std::vector<Cell<>::Data>> const& cellData, Cell<> const& dest) const;
  91.  
  92.                 static inline constexpr auto getDirections() {
  93.                     if constexpr(M == MovementType::k4Directional) {
  94.                         return std::array<Cell<>, 4>{{
  95.                             { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
  96.                         }};
  97.                     } else {
  98.                         return std::array<Cell<>, 8>{{
  99.                             { 1, 0 }, { -1, 0 }, { 0, 1 }, { 0, -1 },
  100.                             { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 },
  101.                         }};
  102.                     }
  103.                 }
  104.  
  105.                 std::vector<std::vector<int>> const& mGrid;
  106.                 Cell<std::size_t> mBegin, mEnd;
  107.  
  108.                 static inline constexpr auto mDirections = getDirections();
  109.         };
  110.  
  111.         template <Heuristic H = Heuristic::kManhattan, MovementType M = MovementType::k4Directional, typename... Args>
  112.         static inline ASPF<H, M> instantiate(Args&&... args) {
  113.             return ASPF<H, M>(std::forward<Args>(args)...);
  114.         }
  115. };
  116.  
  117.  
  118. template <typename T>
  119. template <ASPFWrapper::Heuristic H>
  120. typename std::enable_if_t<H == ASPFWrapper::Heuristic::kManhattan, double>
  121. ASPFWrapper::Cell<T>::getH(Cell const& cell, Cell const& dest) {
  122.     return std::abs(cell.x - dest.x) + std::abs(cell.y - dest.y);
  123. }
  124.  
  125. template <typename T>
  126. template <ASPFWrapper::Heuristic H>
  127. typename std::enable_if_t<H == ASPFWrapper::Heuristic::kDiagonal, double>
  128. ASPFWrapper::Cell<T>::getH(Cell const& cell, Cell const& dest) {
  129.     static constexpr double kNodeLength = 1;
  130.     static constexpr double kNodeDiagonalDistance = std::sqrt(2);
  131.  
  132.     auto dx = std::abs(cell.x - dest.x);
  133.     auto dy = std::abs(cell.y - dest.y);
  134.     return kNodeLength * (dx + dy) + (kNodeDiagonalDistance - kNodeLength * 2) * std::min(dx, dy);
  135. }
  136.  
  137. template <typename T>
  138. template <ASPFWrapper::Heuristic H>
  139. typename std::enable_if_t<H == ASPFWrapper::Heuristic::kEuclidean, double>
  140. ASPFWrapper::Cell<T>::getH(Cell const& cell, Cell const& dest) {
  141.     return std::sqrt(std::pow(cell.x - dest.x, 2) + std::pow(cell.y - dest.y, 2));
  142. }
  143.  
  144. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  145. ASPFWrapper::ASPF<H, M>::ASPF(std::vector<std::vector<int>> const& grid) : mGrid(grid) {
  146.     setBegin({ 0, 0 });
  147.     setEnd({ 0, 0 });
  148. }
  149.  
  150. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  151. bool ASPFWrapper::ASPF<H, M>::isValid(Cell<> const& cell) const {
  152.     return static_cast<int>(mBegin.x) <= cell.x <= static_cast<int>(mEnd.x) && static_cast<int>(mBegin.y) <= cell.y <= static_cast<int>(mEnd.y);
  153. }
  154.  
  155. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  156. bool ASPFWrapper::ASPF<H, M>::isUnblocked(Cell<> const& cell) const {
  157.     return mGrid.at(cell.y).at(cell.x) != 0;
  158. }
  159.  
  160. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  161. bool ASPFWrapper::ASPF<H, M>::isUnblocked(Cell<> const& cell, Cell<> const& successor) const {
  162.     return mGrid.at(successor.y).at(successor.x) != 0;
  163. }
  164.  
  165. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  166. std::stack<ASPFWrapper::Cell<>> ASPFWrapper::ASPF<H, M>::getPath(std::vector<std::vector<Cell<>::Data>> const& cellData, Cell<> const& dest) const {
  167.     std::stack<Cell<>> path;
  168.     auto curr = dest;
  169.  
  170.     while (!(cellData.at(curr.y - mBegin.y).at(curr.x - mBegin.x).parent == curr)) {
  171.         path.push(curr);
  172.         curr = cellData.at(curr.y - mBegin.y).at(curr.x - mBegin.x).parent;
  173.     }
  174.  
  175.     path.push(curr);
  176.     return path;
  177. }
  178.  
  179. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  180. void ASPFWrapper::ASPF<H, M>::setBegin(Cell<std::size_t> const& begin) {
  181.     mBegin = {
  182.         std::max((std::size_t)(0), begin.x),
  183.         std::max((std::size_t)(0), begin.y),
  184.     };
  185. }
  186.  
  187. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  188. void ASPFWrapper::ASPF<H, M>::setEnd(Cell<std::size_t> const& end) {
  189.     mEnd = {
  190.         std::min(mGrid.front().size() - 1, end.x),
  191.         std::min(mGrid.size() - 1, end.y),
  192.     };
  193. }
  194.  
  195. template <ASPFWrapper::Heuristic H, ASPFWrapper::MovementType M>
  196. ASPFWrapper::Result ASPFWrapper::ASPF<H, M>::search(Cell<> const& src, Cell<> const& dest) {
  197.     if (mGrid.empty()) return Result(Status::kFailure);
  198.  
  199.     if (!isValid(src)) return Result(Status::kInvalidSrc);
  200.     if (!isValid(dest)) return Result(Status::kInvalidDest);
  201.     if (!isUnblocked(src)) return Result(Status::kBlockedSrc);
  202.     if (!isUnblocked(dest)) return Result(Status::kBlockedDest);
  203.     if (src == dest) return Result(Status::kCoincidents);
  204.  
  205.     // Note that index-based operations on two following lists require substracting `mBegin` to match `mGrid`
  206.     // Initialize closed list
  207.     std::vector<std::vector<bool>> closedList(mEnd.y - mBegin.y + 1, std::vector<bool>(mEnd.x - mBegin.x + 1, false));   // `false` means not visited (and vice versa)
  208.  
  209.     // Initialize cell data list
  210.     std::vector<std::vector<Cell<>::Data>> cellDataList(mEnd.y - mBegin.y + 1, std::vector<Cell<>::Data>(mEnd.x - mBegin.x + 1, Cell<>::Data{}));
  211.  
  212.     // Initialize starting node parameters
  213.     auto& srcData = cellDataList[src.y - mBegin.y][src.x - mBegin.x];
  214.     srcData.f = 0;
  215.     srcData.parent = src;
  216.  
  217.     // Initialize open list
  218.     std::stack<Cell<>> openList;   // For operations at O(1) time complexity
  219.     openList.push(src);   // Place the starting cell on the open list
  220.  
  221.     double g, h, f;
  222.  
  223.     while (!openList.empty()) {
  224.         g = h = f = 0;
  225.  
  226.         auto parent = openList.top();
  227.         openList.pop();   // Remove cell from open list
  228.         closedList[parent.y - mBegin.y][parent.x - mBegin.x] = true;   // Add cell to closed list
  229.  
  230.         // Generate all successors
  231.         for (const auto& direction : mDirections) {
  232.             auto successor = parent + direction;
  233.             if (!isValid(successor)) continue;
  234.  
  235.             // If successor is destination, search is considered successful
  236.             if (successor == dest) {
  237.                 cellDataList[successor.y - mBegin.y][successor.x - mBegin.x].parent = parent;
  238.                 return Result(Status::kSuccess, getPath(cellDataList, dest));
  239.             }
  240.  
  241.             // Ignore if successor is already on the closed list or is blocked
  242.             if (!closedList.at(successor.y - mBegin.y).at(successor.x - mBegin.x) && isUnblocked(successor)) {
  243.                 g = cellDataList[parent.y - mBegin.y][parent.x - mBegin.x].g + std::sqrt(std::pow(direction.x, 2) + std::pow(direction.y, 2));
  244.                 h = Cell<>::getH<H>(successor, dest);
  245.                 f = g + h;
  246.  
  247.                 auto& successorData = cellDataList[successor.y - mBegin.y][successor.x - mBegin.x];
  248.  
  249.                 // Add successor to the open list if successor is not on the open list or provides a better path
  250.                 if (successorData.f == std::numeric_limits<double>::max() || successorData.f > f) {
  251.                     openList.push(successor);
  252.  
  253.                     // Update successor data
  254.                     successorData.f = f;
  255.                     successorData.g = g;
  256.                     successorData.h = h;
  257.                     successorData.parent = parent;
  258.                 }
  259.             }
  260.         }
  261.     }
  262.  
  263.     // Search is considered unsuccessful if the open list is emptied before destination cell is found
  264.     return Result(Status::kFailure);
  265. }
  266.  
  267.  
  268. /* * * * * * */
  269. int main(void) {
  270.     std::vector<std::vector<int>> vec = {
  271.         {{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  272.         {{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  273.         {{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  274.         {{ 0, 0, 0, 0, 4171, 4171, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  275.         {{ 0, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 0, 4171, 4171, 4171, 0, 0, }},
  276.         {{ 0, 4171, 4171, 4171, 0, 0, 4171, 4171, 4171, 0, 4171, 0, 0, 0, 0, }},
  277.         {{ 0, 4171, 4171, 4171, 0, 0, 4171, 4171, 4171, 0, 4171, 0, 0, 0, 0, }},
  278.         {{ 0, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 0, 0, 0, }},
  279.         {{ 0, 0, 0, 0, 4171, 4171, 0, 4171, 4171, 0, 4171, 4171, 0, 0, 0, }},
  280.         {{ 0, 0, 0, 0, 4171, 4171, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  281.         {{ 0, 0, 0, 0, 4171, 4171, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  282.         {{ 0, 0, 0, 0, 4171, 4171, 0, 4171, 4171, 4171, 4171, 4171, 0, 0, 0, }},
  283.         {{ 0, 4171, 4171, 4171, 4171, 4171, 4171, 4171, 0, 0, 0, 4171, 4171, 4171, 0, }},
  284.         {{ 0, 0, 0, 0, 4171, 4171, 0, 4171, 0, 0, 0, 4171, 4171, 4171, 0, }},
  285.         {{ 0, 0, 0, 0, 4171, 4171, 0, 4171, 0, 0, 0, 4171, 0, 0, 0, }},
  286.         {{ 0, 0, 0, 0, 4171, 4171, 0, 4171, 4171, 4171, 4171, 4171, 0, 0, 0, }},
  287.         {{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, }},
  288.     };
  289.  
  290.     // input is not inverted
  291.     ASPFWrapper::Cell<> src = { 12, 4 };
  292.     ASPFWrapper::Cell<> dest = { 5, 15 };
  293.  
  294.     auto obj = ASPFWrapper::instantiate(vec);
  295.     obj.setBegin({ 1, 1 });
  296.     obj.setEnd({ vec.front().size() - 2, vec.size() - 2 });
  297.  
  298.     auto start = std::chrono::high_resolution_clock::now();
  299.  
  300.     auto res = obj.search(src, dest);
  301.  
  302.     auto end = std::chrono::high_resolution_clock::now();
  303.     auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
  304.  
  305.     std::cout << "Execution time: " << duration.count() << " ms" << std::endl;
  306.     std::cout << "Status: " << static_cast<int>(res.status) << std::endl;
  307.  
  308.     auto path = res.path;
  309.     while (path.size() > 1) {
  310.         std::cout << "(" << path.top().x << ", " << path.top().y << ") -> ";
  311.         path.pop();
  312.     }
  313.     std::cout << path.top() << std::endl;
  314.  
  315.     return 0;
  316. }
Success #stdin #stdout 0s 5300KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Execution time: 9 ms
Status: 5
(12, 4) -> (11, 4) -> (10, 4) -> (10, 5) -> (10, 6) -> (10, 7) -> (9, 7) -> (8, 7) -> (8, 6) -> (8, 5) -> (8, 4) -> (7, 4) -> (6, 4) -> (6, 5) -> (6, 6) -> (6, 7) -> (5, 7) -> (5, 8) -> (5, 9) -> (5, 10) -> (5, 11) -> (5, 12) -> (5, 13) -> (5, 14) -> (5, 15)
https://ideone.com/YDylQr
language:
C++ (gcc 8.3)
created:
13 days ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!