├── map
├── map.pgm
└── map.yaml
├── img
└── path_exam.png
├── package.xml
├── launch
├── run.launch
└── Map.rviz
├── README.md
├── include
└── path_generator
│ ├── path_generator.h
│ └── a_star.hpp
├── CMakeLists.txt
└── src
├── path_generator.cpp
└── a_star.cpp
/map/map.pgm:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zang09/AStar-ROS/HEAD/map/map.pgm
--------------------------------------------------------------------------------
/img/path_exam.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/zang09/AStar-ROS/HEAD/img/path_exam.png
--------------------------------------------------------------------------------
/map/map.yaml:
--------------------------------------------------------------------------------
1 | image: map.pgm
2 | resolution: 0.100000
3 | origin: [-9.500000, -10.000000, 0.0]
4 | negate: 0
5 | occupied_thresh: 0.65
6 | free_thresh: 0.196
7 |
8 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | path_generator
4 | 0.0.0
5 | The path_generator package
6 |
7 | haebum
8 |
9 | TODO
10 |
11 | catkin
12 | roscpp
13 | nav_msgs
14 | geometry_msgs
15 |
16 |
17 |
18 |
19 |
--------------------------------------------------------------------------------
/launch/run.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # A* Path Planner
2 |
3 | ### Astar demo in ROS Rviz
4 | 
5 |
6 |
7 | ### Prerequisition
8 | Install package
9 | ```
10 | $ sudo apt install ros-${ROS_DISTRO}-map-server
11 | ```
12 | ### How to use
13 | 1. Clone Package
14 | ```
15 | $ mkdir -p ~/catkin_ws/src
16 | $ cd ~/catkin_ws/src
17 | $ git clone https://github.com/zang09/AStar-ROS.git
18 | ```
19 | 2. Build Package
20 | ```
21 | $ cd ..
22 | $ catkin_make
23 | $ source ~/catkin_ws/devel/setup.bash
24 | ```
25 | 3. Launch packages
26 | ```
27 | $ roslaunch path_generator run.launch
28 | ```
29 | 4. Send goal pose by rviz tool `2D Nav Goal`
30 |
--------------------------------------------------------------------------------
/include/path_generator/path_generator.h:
--------------------------------------------------------------------------------
1 | #ifndef PATH_GENERATOR_H
2 | #define PATH_GENERATOR_H
3 |
4 | #include "../include/path_generator/a_star.hpp"
5 |
6 | #include
7 |
8 | #include
9 | #include
10 |
11 |
12 | class PathGenerator
13 | {
14 | public:
15 | PathGenerator();
16 | ~PathGenerator();
17 |
18 | private:
19 | void subscribeAndPublish();
20 | void gridMapHandler(const nav_msgs::OccupancyGrid::ConstPtr &map_msg);
21 | void navGoalHandler(const geometry_msgs::PoseStamped::ConstPtr &goal_msg);
22 |
23 | private:
24 | // ROS
25 | ros::NodeHandle nh_;
26 | ros::Subscriber sub_grid_map_;
27 | ros::Subscriber sub_nav_goal_;
28 | ros::Publisher pub_robot_path_;
29 |
30 | AStar::Generator map_generator_;
31 | nav_msgs::MapMetaData map_info_;
32 | bool map_exsit_;
33 | };
34 |
35 | #endif //PATH_GENERATOR_H
36 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.0.2)
2 | project(path_generator)
3 |
4 | ## Compile as C++11, supported in ROS Kinetic and newer
5 | # add_compile_options(-std=c++11)
6 |
7 | find_package(catkin REQUIRED COMPONENTS
8 | roscpp
9 | nav_msgs
10 | geometry_msgs
11 | )
12 |
13 | ## System dependencies are found with CMake's conventions
14 | # find_package(Boost REQUIRED COMPONENTS system)
15 |
16 | ################################################
17 | ## Declare ROS messages, services and actions ##
18 | ################################################
19 |
20 | ###################################
21 | ## catkin specific configuration ##
22 | ###################################
23 | catkin_package(
24 | # INCLUDE_DIRS include
25 | # LIBRARIES path_generator
26 | # CATKIN_DEPENDS roscpp
27 | # DEPENDS system_lib
28 | )
29 |
30 | ###########
31 | ## Build ##
32 | ###########
33 |
34 | include_directories(include ${catkin_INCLUDE_DIRS})
35 |
36 | add_executable(${PROJECT_NAME} src/path_generator.cpp src/a_star.cpp)
37 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
38 |
--------------------------------------------------------------------------------
/include/path_generator/a_star.hpp:
--------------------------------------------------------------------------------
1 | /*
2 | Copyright (c) 2015, Damian Barczynski
3 | Following tool is licensed under the terms and conditions of the ISC license.
4 | For more information visit https://opensource.org/licenses/ISC.
5 | */
6 | #ifndef ASTAR_HPP
7 | #define ASTAR_HPP
8 |
9 | #include
10 | #include
11 | #include
12 | #include
13 |
14 | namespace AStar
15 | {
16 | struct Vec2i
17 | {
18 | int x, y;
19 |
20 | bool operator == (const Vec2i& coordinates_);
21 | bool operator != (const Vec2i& coordinates_);
22 | };
23 |
24 | using uint = unsigned int;
25 | using HeuristicFunction = std::function;
26 | using CoordinateList = std::vector;
27 |
28 | struct Node
29 | {
30 | uint G, H;
31 | Vec2i coordinates;
32 | Node *parent;
33 |
34 | Node(Vec2i coord_, Node *parent_ = nullptr);
35 | uint getScore();
36 | };
37 |
38 | using NodeSet = std::vector;
39 |
40 | class Generator
41 | {
42 | bool detectCollision(Vec2i coordinates_);
43 | Node* findNodeOnList(NodeSet& nodes_, Vec2i coordinates_);
44 | void releaseNodes(NodeSet& nodes_);
45 |
46 | public:
47 | Generator();
48 | void setWorldSize(Vec2i worldSize_);
49 | void setDiagonalMovement(bool enable_);
50 | void setHeuristic(HeuristicFunction heuristic_);
51 | CoordinateList findPath(Vec2i source_, Vec2i target_);
52 | void addCollision(Vec2i coordinates_);
53 | void addCollision(Vec2i coordinates_, int size);
54 | void removeCollision(Vec2i coordinates_);
55 | void clearCollisions();
56 |
57 | private:
58 | HeuristicFunction heuristic;
59 | CoordinateList direction, walls;
60 | Vec2i worldSize;
61 | uint directions;
62 | };
63 |
64 | class Heuristic
65 | {
66 | static Vec2i getDelta(Vec2i source_, Vec2i target_);
67 |
68 | public:
69 | static uint manhattan(Vec2i source_, Vec2i target_);
70 | static uint euclidean(Vec2i source_, Vec2i target_);
71 | static uint octagonal(Vec2i source_, Vec2i target_);
72 | };
73 | }
74 |
75 | #endif // ASTAR_HPP
76 |
--------------------------------------------------------------------------------
/src/path_generator.cpp:
--------------------------------------------------------------------------------
1 | #include "../include/path_generator/path_generator.h"
2 |
3 | #include
4 |
5 | PathGenerator::PathGenerator()
6 | {
7 | subscribeAndPublish();
8 | }
9 |
10 | PathGenerator::~PathGenerator()
11 | {
12 |
13 | }
14 |
15 | void PathGenerator::subscribeAndPublish()
16 | {
17 | sub_grid_map_ = nh_.subscribe("map", 1, &PathGenerator::gridMapHandler, this);
18 | sub_nav_goal_ = nh_.subscribe("move_base_simple/goal", 1, &PathGenerator::navGoalHandler, this);
19 | pub_robot_path_ = nh_.advertise("robot_path", 1, true);
20 | }
21 |
22 | void PathGenerator::gridMapHandler(const nav_msgs::OccupancyGrid::ConstPtr &map_msg)
23 | {
24 | ROS_INFO("Generating map..");
25 | map_exsit_ = false;
26 |
27 | map_info_ = map_msg->info;
28 |
29 | // Generate Map, Options
30 | map_generator_.setWorldSize({(int)map_info_.width, (int)map_info_.height}); //{x, y}
31 | map_generator_.setHeuristic(AStar::Heuristic::manhattan);
32 | map_generator_.setDiagonalMovement(true);
33 |
34 | // Add Wall
35 | int x, y;
36 | for(int i=0; idata[i] != 0)
42 | {
43 | map_generator_.addCollision({x, y}, 3);
44 | }
45 | }
46 |
47 | ROS_INFO("Success build map!");
48 | map_exsit_ = true;
49 | }
50 |
51 | void PathGenerator::navGoalHandler(const geometry_msgs::PoseStamped::ConstPtr &goal_msg)
52 | {
53 | if(!map_exsit_) return;
54 |
55 | ROS_INFO("\033[1;32mGoal received!\033[0m");
56 |
57 | // Round goal coordinate
58 | float goal_x = round(goal_msg->pose.position.x*10)/10;
59 | float goal_y = round(goal_msg->pose.position.y*10)/10;
60 |
61 | // Remmaping coordinate
62 | AStar::Vec2i target;
63 | target.x = (goal_x - map_info_.origin.position.x) / map_info_.resolution;
64 | target.y = (goal_y - map_info_.origin.position.y) / map_info_.resolution;
65 |
66 | AStar::Vec2i source;
67 | source.x = (0 - map_info_.origin.position.x) / map_info_.resolution;
68 | source.y = (0 - map_info_.origin.position.y) / map_info_.resolution;
69 |
70 | // Find Path
71 | auto path = map_generator_.findPath(source, target);
72 |
73 | nav_msgs::Path path_msg;
74 | if(path.empty())
75 | {
76 | ROS_INFO("\033[1;31mFail generate path!\033[0m");
77 | return;
78 | }
79 |
80 | for(auto coordinate=path.end()-1; coordinate>=path.begin(); --coordinate)
81 | {
82 | geometry_msgs::PoseStamped point_pose;
83 |
84 | // Remmaping coordinate
85 | point_pose.pose.position.x = (coordinate->x + map_info_.origin.position.x / map_info_.resolution) * map_info_.resolution;
86 | point_pose.pose.position.y = (coordinate->y + map_info_.origin.position.y / map_info_.resolution) * map_info_.resolution;
87 | path_msg.poses.push_back(point_pose);
88 | }
89 |
90 | path_msg.header.frame_id = "map";
91 | pub_robot_path_.publish(path_msg);
92 |
93 | ROS_INFO("\033[1;36mSuccess generate path!\033[0m");
94 | }
95 |
96 | int main(int argc, char **argv)
97 | {
98 | ros::init(argc, argv, "path_generator");
99 |
100 | ROS_INFO("\033[1;32m----> Path Generator Node is Started.\033[0m");
101 |
102 | PathGenerator PG;
103 |
104 | ros::spin();
105 | return 0;
106 | }
107 |
--------------------------------------------------------------------------------
/launch/Map.rviz:
--------------------------------------------------------------------------------
1 | Panels:
2 | - Class: rviz/Displays
3 | Help Height: 0
4 | Name: Displays
5 | Property Tree Widget:
6 | Expanded:
7 | - /Global Options1
8 | - /Status1
9 | Splitter Ratio: 0.5
10 | Tree Height: 901
11 | - Class: rviz/Selection
12 | Name: Selection
13 | - Class: rviz/Tool Properties
14 | Expanded:
15 | - /2D Pose Estimate1
16 | - /2D Nav Goal1
17 | - /Publish Point1
18 | Name: Tool Properties
19 | Splitter Ratio: 0.5886790156364441
20 | - Class: rviz/Views
21 | Expanded:
22 | - /Current View1
23 | Name: Views
24 | Splitter Ratio: 0.5
25 | - Class: rviz/Time
26 | Experimental: false
27 | Name: Time
28 | SyncMode: 0
29 | SyncSource: ""
30 | Preferences:
31 | PromptSaveOnExit: true
32 | Toolbars:
33 | toolButtonStyle: 2
34 | Visualization Manager:
35 | Class: ""
36 | Displays:
37 | - Alpha: 0.699999988079071
38 | Class: rviz/Map
39 | Color Scheme: map
40 | Draw Behind: false
41 | Enabled: true
42 | Name: Map
43 | Topic: /map
44 | Unreliable: false
45 | Use Timestamp: false
46 | Value: true
47 | - Alpha: 0.5
48 | Cell Size: 0.10000000149011612
49 | Class: rviz/Grid
50 | Color: 160; 160; 164
51 | Enabled: true
52 | Line Style:
53 | Line Width: 0.029999999329447746
54 | Value: Lines
55 | Name: Grid
56 | Normal Cell Count: 0
57 | Offset:
58 | X: 0
59 | Y: 0
60 | Z: 0
61 | Plane: XY
62 | Plane Cell Count: 190
63 | Reference Frame:
64 | Value: true
65 | - Class: rviz/Axes
66 | Enabled: true
67 | Length: 1
68 | Name: Origin
69 | Radius: 0.10000000149011612
70 | Reference Frame:
71 | Value: true
72 | - Alpha: 1
73 | Axes Length: 1
74 | Axes Radius: 0.10000000149011612
75 | Class: rviz/Pose
76 | Color: 255; 25; 0
77 | Enabled: true
78 | Head Length: 0.30000001192092896
79 | Head Radius: 0.10000000149011612
80 | Name: Goal Pose
81 | Shaft Length: 1
82 | Shaft Radius: 0.05000000074505806
83 | Shape: Arrow
84 | Topic: /move_base_simple/goal
85 | Unreliable: false
86 | Value: true
87 | - Alpha: 1
88 | Buffer Length: 1
89 | Class: rviz/Path
90 | Color: 117; 80; 123
91 | Enabled: true
92 | Head Diameter: 0.30000001192092896
93 | Head Length: 0.20000000298023224
94 | Length: 0.30000001192092896
95 | Line Style: Billboards
96 | Line Width: 0.05000000074505806
97 | Name: Path
98 | Offset:
99 | X: 0
100 | Y: 0
101 | Z: 0
102 | Pose Color: 255; 85; 255
103 | Pose Style: None
104 | Radius: 0.029999999329447746
105 | Shaft Diameter: 0.10000000149011612
106 | Shaft Length: 0.10000000149011612
107 | Topic: /robot_path
108 | Unreliable: false
109 | Value: true
110 | Enabled: true
111 | Global Options:
112 | Background Color: 48; 48; 48
113 | Default Light: true
114 | Fixed Frame: map
115 | Frame Rate: 30
116 | Name: root
117 | Tools:
118 | - Class: rviz/Interact
119 | Hide Inactive Objects: true
120 | - Class: rviz/MoveCamera
121 | - Class: rviz/Select
122 | - Class: rviz/FocusCamera
123 | - Class: rviz/Measure
124 | - Class: rviz/SetInitialPose
125 | Theta std deviation: 0.2617993950843811
126 | Topic: /initialpose
127 | X std deviation: 0.5
128 | Y std deviation: 0.5
129 | - Class: rviz/SetGoal
130 | Topic: /move_base_simple/goal
131 | - Class: rviz/PublishPoint
132 | Single click: true
133 | Topic: /clicked_point
134 | Value: true
135 | Views:
136 | Current:
137 | Angle: 4.710001468658447
138 | Class: rviz/TopDownOrtho
139 | Enable Stereo Rendering:
140 | Stereo Eye Separation: 0.05999999865889549
141 | Stereo Focal Distance: 1
142 | Swap Stereo Eyes: false
143 | Value: false
144 | Invert Z Axis: false
145 | Name: Current View
146 | Near Clip Distance: 0.009999999776482582
147 | Scale: 39.35321044921875
148 | Target Frame:
149 | Value: TopDownOrtho (rviz)
150 | X: 0.3317674994468689
151 | Y: -1.4358853101730347
152 | Saved: ~
153 | Window Geometry:
154 | Displays:
155 | collapsed: false
156 | Height: 1052
157 | Hide Left Dock: false
158 | Hide Right Dock: true
159 | QMainWindow State: 000000ff00000000fd000000040000000000000156000003c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000003c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065000000000000000780000004f300fffffffb0000000800540069006d0065010000000000000450000000000000000000000624000003c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
160 | Selection:
161 | collapsed: false
162 | Time:
163 | collapsed: false
164 | Tool Properties:
165 | collapsed: false
166 | Views:
167 | collapsed: true
168 | Width: 1920
169 | X: 0
170 | Y: 0
171 |
--------------------------------------------------------------------------------
/src/a_star.cpp:
--------------------------------------------------------------------------------
1 | #include "../include/path_generator/a_star.hpp"
2 |
3 | #include
4 |
5 | using namespace std::placeholders;
6 |
7 | bool AStar::Vec2i::operator == (const Vec2i& coordinates_)
8 | {
9 | return (x == coordinates_.x && y == coordinates_.y);
10 | }
11 |
12 | bool AStar::Vec2i::operator != (const Vec2i& coordinates_)
13 | {
14 | return !(x == coordinates_.x && y == coordinates_.y);
15 | }
16 |
17 | AStar::Vec2i operator + (const AStar::Vec2i& left_, const AStar::Vec2i& right_)
18 | {
19 | return { left_.x + right_.x, left_.y + right_.y };
20 | }
21 |
22 | AStar::Node::Node(Vec2i coordinates_, Node *parent_)
23 | {
24 | parent = parent_;
25 | coordinates = coordinates_;
26 | G = H = 0;
27 | }
28 |
29 | AStar::uint AStar::Node::getScore()
30 | {
31 | return G + H;
32 | }
33 |
34 | AStar::Generator::Generator()
35 | {
36 | setDiagonalMovement(false);
37 | setHeuristic(&Heuristic::manhattan);
38 | direction = {
39 | { 0, 1 }, { 1, 0 }, { 0, -1 }, { -1, 0 },
40 | { -1, -1 }, { 1, 1 }, { -1, 1 }, { 1, -1 }
41 | };
42 | }
43 |
44 | void AStar::Generator::setWorldSize(Vec2i worldSize_)
45 | {
46 | worldSize = worldSize_;
47 | }
48 |
49 | void AStar::Generator::setDiagonalMovement(bool enable_)
50 | {
51 | directions = (enable_ ? 8 : 4);
52 | }
53 |
54 | void AStar::Generator::setHeuristic(HeuristicFunction heuristic_)
55 | {
56 | heuristic = std::bind(heuristic_, _1, _2);
57 | }
58 |
59 | void AStar::Generator::addCollision(Vec2i coordinates_)
60 | {
61 | if (coordinates_.x < 0 || coordinates_.x >= worldSize.x ||
62 | coordinates_.y < 0 || coordinates_.y >= worldSize.y)
63 | return;
64 |
65 | if(!detectCollision(coordinates_))
66 | walls.push_back(coordinates_);
67 | }
68 |
69 | void AStar::Generator::addCollision(Vec2i coordinates_, int size)
70 | {
71 | if(size == 0)
72 | {
73 | addCollision(coordinates_);
74 | return;
75 | }
76 |
77 | for(int i=0; i<4; i++)
78 | {
79 | Vec2i new_coord = coordinates_ + direction[i];
80 | addCollision(new_coord, size-1);
81 | }
82 | }
83 |
84 | void AStar::Generator::removeCollision(Vec2i coordinates_)
85 | {
86 | auto it = std::find(walls.begin(), walls.end(), coordinates_);
87 | if (it != walls.end()) {
88 | walls.erase(it);
89 | }
90 | }
91 |
92 | void AStar::Generator::clearCollisions()
93 | {
94 | walls.clear();
95 | }
96 |
97 | AStar::CoordinateList AStar::Generator::findPath(Vec2i source_, Vec2i target_)
98 | {
99 | Node *current = nullptr;
100 | NodeSet openSet, closedSet;
101 | openSet.reserve(100);
102 | closedSet.reserve(100);
103 | openSet.push_back(new Node(source_));
104 |
105 | struct timespec start, end;
106 | clock_gettime(CLOCK_MONOTONIC, &start);
107 |
108 | while (!openSet.empty()) {
109 | auto current_it = openSet.begin();
110 | current = *current_it;
111 |
112 | for (auto it = openSet.begin(); it != openSet.end(); it++) {
113 | auto node = *it;
114 | if (node->getScore() <= current->getScore()) {
115 | current = node;
116 | current_it = it;
117 | }
118 | }
119 |
120 | clock_gettime(CLOCK_MONOTONIC, &end);
121 | double time_result = end.tv_sec - start.tv_sec + (end.tv_nsec - start.tv_nsec)*1e-9;
122 |
123 | if (time_result > 1.0) { //time failure limit
124 | printf("time: %lf\n", time_result);
125 | break;
126 | }
127 |
128 | if (current->coordinates == target_) {
129 | break;
130 | }
131 |
132 | closedSet.push_back(current);
133 | openSet.erase(current_it);
134 |
135 | for (uint i = 0; i < directions; ++i) {
136 | Vec2i newCoordinates(current->coordinates + direction[i]);
137 | if (detectCollision(newCoordinates) ||
138 | findNodeOnList(closedSet, newCoordinates)) {
139 | continue;
140 | }
141 |
142 | uint totalCost = current->G + ((i < 4) ? 10 : 14);
143 |
144 | Node *successor = findNodeOnList(openSet, newCoordinates);
145 | if (successor == nullptr) {
146 | successor = new Node(newCoordinates, current);
147 | successor->G = totalCost;
148 | successor->H = heuristic(successor->coordinates, target_);
149 | openSet.push_back(successor);
150 | }
151 | else if (totalCost < successor->G) {
152 | successor->parent = current;
153 | successor->G = totalCost;
154 | }
155 | }
156 | }
157 |
158 | CoordinateList path;
159 |
160 | if (current->coordinates == target_)
161 | {
162 | while (current != nullptr)
163 | {
164 | path.push_back(current->coordinates);
165 | current = current->parent;
166 | }
167 | }
168 |
169 | releaseNodes(openSet);
170 | releaseNodes(closedSet);
171 |
172 | return path;
173 | }
174 |
175 | AStar::Node* AStar::Generator::findNodeOnList(NodeSet& nodes_, Vec2i coordinates_)
176 | {
177 | for (auto node : nodes_) {
178 | if (node->coordinates == coordinates_) {
179 | return node;
180 | }
181 | }
182 | return nullptr;
183 | }
184 |
185 | void AStar::Generator::releaseNodes(NodeSet& nodes_)
186 | {
187 | for (auto it = nodes_.begin(); it != nodes_.end();) {
188 | delete *it;
189 | it = nodes_.erase(it);
190 | }
191 | }
192 |
193 | bool AStar::Generator::detectCollision(Vec2i coordinates_)
194 | {
195 | if (coordinates_.x < 0 || coordinates_.x >= worldSize.x ||
196 | coordinates_.y < 0 || coordinates_.y >= worldSize.y ||
197 | std::find(walls.begin(), walls.end(), coordinates_) != walls.end())
198 | {
199 | return true;
200 | }
201 |
202 | return false;
203 | }
204 |
205 | AStar::Vec2i AStar::Heuristic::getDelta(Vec2i source_, Vec2i target_)
206 | {
207 | return{ abs(source_.x - target_.x), abs(source_.y - target_.y) };
208 | }
209 |
210 | AStar::uint AStar::Heuristic::manhattan(Vec2i source_, Vec2i target_)
211 | {
212 | auto delta = std::move(getDelta(source_, target_));
213 | return static_cast(10 * (delta.x + delta.y));
214 | }
215 |
216 | AStar::uint AStar::Heuristic::euclidean(Vec2i source_, Vec2i target_)
217 | {
218 | auto delta = std::move(getDelta(source_, target_));
219 | return static_cast(10 * sqrt(pow(delta.x, 2) + pow(delta.y, 2)));
220 | }
221 |
222 | AStar::uint AStar::Heuristic::octagonal(Vec2i source_, Vec2i target_)
223 | {
224 | auto delta = std::move(getDelta(source_, target_));
225 | return 10 * (delta.x + delta.y) + (-6) * std::min(delta.x, delta.y);
226 | }
227 |
--------------------------------------------------------------------------------