Polygonal A* Pathfinding in C++17
There's loads of resources out there on the A* search algorithm and how it can be applied to game development, but there aren't many comprehensive tutorials with working examples pertaining to polygonal maps rather than tile maps, with what few exist seemingly lost to time in my experience.
This article is based on David Gouveia's "Pathfinding on a 2D Polygonal Map" blog post from 2011. My aim is to essentially take the concepts described in his post and walk you through a complete implementation of them. Without his article I don't think I could've implemented this myself. I also want to go over some pitfalls and "gotcha's" I ran into while incorporating his examples into my project.
To keep it brief (if you really want to learn more about pathfinding and its history, read the post linked above), here's the gist of what we need to do:
- Take in a polygon represented as a list of vertices, as well as a starting point and ending point (or "goal")
- Find all of the concave vertices in the polygon and use them to create a node hierarchy for the A* algorithm
- Link concave vertices that are within line-of-sight of each other with each other
- Link your starting node to the rest of your nodes
- Link the rest of your nodes to your ending node
- Traverse the node hierarchy with an A* implementation
If we've done everything right, we should end up with something like this.
Finding concave vertices
We need to know how to do this since our node hierarchy will mostly be comprised of concave vertices. Convex vertices aren't important, although it's worth noting that's not necessarily the case if your polygon has holes. For each vertex we'll compute the vector cross product of its previous point against its own point, as well as its own point against its next point.
Based on the cross product we can deduce the following:
- If the result is positive, this is a counter-clockwise turn/convex angle
- If the result is negative, this is a clockwise turn/concave angle
- If the result is zero, these are collinear points.
std::vector<int> FindConcavePoints(const Polygon& polygon)
{
std::vector<int> concave_points;
size_t n = polygon.size();
for (int i = 0; i < n; ++i)
{
XY a = polygon[(i - 1 + n) % n];
XY b = polygon[(i)];
XY c = polygon[(i + 1) % n];
float cross = (b.x - a.x) * (c.y - b.y) - (b.y - a.y) * (c.x - b.x);
if (cross >= 0)
continue;
concave_points.push_back(i);
}
return concave_points;
}
Determining visibility
Our concave vertices need to be linked together to have a functional node hierarchy for the A* algorithm. We'll do this based on their visibility to each other; in other words, based on whether or not they're within each others' line-of-sight.
First we need to be able to deduce whether or not a point exists within our polygon. We'll use a simple ray-casting algorithm:
bool PointInPolygon(Polygon polygon, XY point)
{
bool inside = false;
// Bail if this isn't actually a polygon
if (polygon.size() < 3)
return false;
for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++)
{
float xi = polygon[i].x, yi = polygon[i].y;
float xj = polygon[j].x, yj = polygon[j].y;
// Perform a ray-cast to determine if the point is on the correct side of the edge
if (((yi > point.y) != (yj > point.y)) && (point.x < (xj - xi) * (point.y - yi) / (yj - yi) + xi))
inside ^= 1;
}
return inside;
}
We'll also need to perform an intersection test between the two nodes we're testing:
bool LineSegmentsCross(XY a, XY b, XY c, XY d)
{
float denominator = ((b.x - a.x) * (d.y - c.y)) - ((b.y - a.y) * (d.x - c.x));
if (CompareFloat(denominator, 0.0f))
return false;
float numerator1 = ((a.y - c.y) * (d.x - c.x)) - ((a.x - c.x) * (d.y - c.y));
float numerator2 = ((a.y - c.y) * (b.x - a.x)) - ((a.x - c.x) * (b.y - a.y));
if (CompareFloat(numerator1, 0.0f) || CompareFloat(numerator2, 0.0f))
return false;
float r = numerator1 / denominator;
float s = numerator2 / denominator;
return (r > 0 && r < 1) && (s > 0 && s < 1);
}
Now we can implement our InLineOfSight() function...
bool InLineOfSight(XY start, XY end, const Polygon& polygon)
{
// Start/end locations are the same -- consider these within LoS
if (CompareFloat(Distance(start, end), 0.0f, 0.000001f))
return true;
// Start/end locations are outside the polygon -- don't consider these within LoS
if (!PointInPolygon(polygon, start) || !PointInPolygon(polygon, end))
return false;
// Start/end segments intersect polygon edges -- should this be the case, don't consider these within LoS
for (int i = 0; i < polygon.size(); i++)
{
XY line_start = polygon[i];
XY line_end = polygon[(i + 1) % polygon.size()];
if (LineSegmentsCross(start, end, line_start, line_end))
return false;
}
// Center between start/end locations is outside the polygon and not part of a line segment -- should this be the case,
// don't consider these within LoS. This addresses the same issue as the X/Y alignment hack above.
XY middle = {
(start.x + end.x) * 0.5f,
(start.y + end.y) * 0.5f
};
bool in_middle = false;
for (int i = 0; i < polygon.size(); i++)
{
XY line_start = polygon[i];
XY line_end = polygon[(i + 1) % polygon.size()];
if (PointInLine(middle.x, middle.y, line_start.x, line_start.y, line_end.x, line_end.y))
in_middle = true;
}
if (!PointInPolygon(polygon, middle) && !in_middle)
return false;
// If no intersections and both points are inside, they are within LoS
return true;
}
Building the node hierarchy
We now have everything we need to build a node hierarchy. First let's define what a "node" actually is:
struct Node
{
XY position{};
float f = 0.0f, g = 0.0f, h = 0.0f;
bool open = false, closed = false;
Node* parent = nullptr;
std::vector<std::pair<Node*, float>> children;
Node() {}
Node(XY position): position(position) {}
void AddChild(Node* child, float distance)
{
children.emplace_back(child, distance);
}
float DistanceTo(Node* other)
{
return Distance(position, other->position);
}
};
This is a pretty standard parent/child relationship, but here's the bits that aren't:
WTF does "g" mean?
"g" is the "cost" of the path from the start node to this one. It represents the path cost incurred to reach the current node from the starting node.
WTF does "h" mean?
"h" is a the result of our heuristic function, which in this case, will be the distance between this node to the end. This is why the A* algorithm is considered an "informed search"; it has some prior knowledge of the world (expressed via the heuristic function) and it uses it to guide the search (expressed via searching nodes by their heuristic value).
WTF does "f" mean?
"f" is simply the sum of "g" and "h", i.e. our total estimated cost of the cheapest solution through the current node. The A* algorithm selects the node with the lowest "f"-cost to explore the next, aiming to find the optimal path.
What about "open" and "closed?"
The A* algorithm is responsible for two lists: a list of "open" nodes and a list of "closed" nodes. The former contains nodes that have been discovered but not fully explored. The latter contains nodes that have already been explored and expanded. Once a node has been fully expanded (i.e., all its neighbors have been examined) it is moved from the open list to the closed list.
The DistanceTo() function
This just calls Distance() which calculates the Euclidean distance between the two points. It's implemented as follows:
float Distance(XY p1, XY p2)
{
return sqrtf((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));
}
With all that said and done, let's set up our FindPath() function:
bool FindPath(XY start, XY end, const Polygon& polygon, Path& path_out, PathMetadata* metadata)
{
// Step 1 -- Build a node hierarchy.
// We'll start by finding all of the concave points in the polygon; these will be our primary pathfinding nodes.
std::vector<int> concaves = FindConcavePoints(polygon);
std::vector<Node> nodes;
std::vector<std::pair<size_t, size_t>> links;
Node start_node(start);
Node end_node(end);
...
This is pretty self-explanatory for the most part. The optional "metadata" parameter is what we'll use to output debugging information.
Preprocessing
Before we start linking nodes together, I'm going to go through the input polygon and calculate the normals of each vertex.
Why?
Two reasons:
- It's completely possible for our visibility detection to fail in the case of collinear lines, floating point rounding errors, etc..
- In the context of a video game, you wouldn't expect an NPC to walk perfectly along the wall, rather beside the wall.
By calculating the polygon's vertex normals, we can "push in" the concave vertices we want to use as graph nodes. This way our character will be where we expect them to be, and our line-of-sight test will never fail.
^ TL;DR
std::unordered_map<int, XY> normals;
std::unordered_map<int, XY> v_normals;
for (size_t i = 0; i < polygon.size(); i++)
{
size_t j = (i + 1) % polygon.size();
XY normal = {-(polygon[j].y - polygon[i].y), polygon[j].x - polygon[i].x};
float normal_len = sqrtf(normal.x * normal.x + normal.y * normal.y);
normal.x /= normal_len;
normal.y /= normal_len;
normals.emplace(i, normal);
XY out1 = {polygon[i].x + (polygon[j].x - polygon[i].x) * 0.5f, polygon[i].y + (polygon[j].y - polygon[i].y) * 0.5f};
XY out2 = {out1.x + normal.x, out1.y + normal.y};
if (metadata)
metadata->normal_lines.emplace_back(out1, out2);
}
for (const auto& [i, normal]: normals)
{
size_t j = (normals.size() + i - 1) % normals.size();
XY v_normal = {normals[i].x + normals[j].x, normals[i].y + normals[j].y};
float v_normal_len = sqrtf(v_normal.x * v_normal.x + v_normal.y * v_normal.y);
v_normal.x /= v_normal_len;
v_normal.y /= v_normal_len;
v_normals.emplace(i, v_normal);
if (metadata)
metadata->normal_lines.emplace_back(polygon[i], XY{polygon[i].x + v_normal.x, polygon[i].y + v_normal.y});
}
...
Linking polygon nodes
Now we can go through all of the concave points and create A* nodes from them. Additionally, link together ones which are in line-of-sight of each other:
for (size_t i = 0; i < concaves.size(); i++)
{
XY position = {polygon[concaves[i]].x + v_normals[concaves[i]].x, polygon[concaves[i]].y + v_normals[concaves[i]].y};
// Should our vertex with the normal applied lands outside of the polygon, push it back in...
while (!PointInPolygon(polygon, position))
{
position.x -= v_normals[concaves[i]].x * FUDGE;
position.y -= v_normals[concaves[i]].y * FUDGE;
}
if (metadata)
metadata->concave_points.push_back(position);
nodes.emplace_back(position);
for (size_t j = i + 1; j < concaves.size(); j++)
{
XY other_position = {
polygon[concaves[j]].x + v_normals[concaves[j]].x, polygon[concaves[j]].y + v_normals[concaves[j]].y
};
while (!PointInPolygon(polygon, other_position))
{
other_position.x -= v_normals[concaves[j]].x * FUDGE;
other_position.y -= v_normals[concaves[j]].y * FUDGE;
}
if (InLineOfSight(position, other_position, polygon))
{
if (metadata)
metadata->linked_concave_points.emplace_back(position, other_position);
links.emplace_back(i, j);
}
}
}
for (const auto& [i, j]: links)
{
nodes[i].AddChild(&nodes[j], nodes[i].DistanceTo(&nodes[j]));
nodes[j].AddChild(&nodes[i], nodes[i].DistanceTo(&nodes[j]));
}
...
For us to be able to properly search the whole graph from our starting position, we need to link our start and end nodes together as well:
for (Node& node: nodes)
{
if (InLineOfSight(end_node.position, node.position, polygon))
{
node.AddChild(&end_node, node.DistanceTo(&end_node));
if (metadata)
{
metadata->end_lines.push_back(end_node.position);
metadata->end_lines.push_back(node.position);
}
}
if (InLineOfSight(start_node.position, node.position, polygon))
{
start_node.AddChild(&node, node.DistanceTo(&start_node));
if (metadata)
{
metadata->start_lines.push_back(start_node.position);
metadata->start_lines.push_back(node.position);
}
}
}
...
Implementing A*
Finally, the fun part! I won't take any credit for the following code, I had gotten to writing my own implementation using Sahnvour's library as a reference and realized "wait this literally just does everything I need already, what am I doing?"
In any case, here it is:
// Portions of this code are licensed under the BSD 2-clause license; see LICENSE.txt for details.
float f = 0.0f, g = 0.0f, h = 0.0f;
std::vector<Node*> open, closed;
std::make_heap(open.begin(), open.end(), CompareF);
open.push_back(&start_node);
std::push_heap(open.begin(), open.end(), CompareF);
start_node.open = true;
Node* current_node = nullptr;
while (!open.empty())
{
std::sort(open.begin(), open.end(), CompareF);
current_node = open.front(); // pop n node from open for which f is minimal
std::pop_heap(open.begin(), open.end(), CompareF);
open.pop_back();
current_node->open = false;
current_node->closed = true;
closed.push_back(current_node);
if (current_node == &end_node)
{
Node* parent = current_node->parent;
path_out.push_back(current_node->position);
while (parent != nullptr)
{
path_out.push_back(parent->position);
parent = parent->parent;
}
return true;
}
for (const auto& [child, distance]: current_node->children) // for each successor n' of n
{
g = current_node->g + distance; // stance from start + distance between the two nodes
if ((child->open || child->closed) && child->g <= g) // n' is already opened or closed with lower cost g(n')
continue; // consider next successor
h = Distance(child->position, end_node.position);
f = g + h;
child->f = f;
child->g = g;
child->h = h;
child->parent = current_node;
if (child->closed)
child->closed = false;
if (!child->open)
{
open.push_back(child);
std::push_heap(open.begin(), open.end(), CompareF);
child->open = true;
}
}
}
return false;
}
The code maintains a min-heap using the following comparator function:
float CompareF(const Node* a, const Node* b)
{
return a->f < b->f;
}
In other words, every time we insert or remove anything from the "open" list, the list is sorted based on the minimum total cost of traversal ("f")
Putting it all together
Here's a working example and demo (thanks to WebAssembly and Emscripten) you can play around with:
#include <math.h>
#include <stdint.h>
#include <string.h>
#include <iostream>
#include <algorithm>
#include <unordered_map>
#include <vector>
#include <utility>
#include <SDL2/SDL.h>
namespace {
struct XY
{
float x, y;
};
struct PathMetadata
{
std::vector<XY> concave_points;
std::vector<std::pair<XY, XY>> linked_concave_points;
std::vector<std::pair<XY, XY>> normal_lines;
std::vector<XY> start_lines;
std::vector<XY> end_lines;
};
using Polygon = std::vector<XY>;
using Path = std::vector<XY>;
const float FUDGE = 0.05f;
const float SCALE = 18.0f;
SDL_Window* window;
SDL_Renderer* renderer;
Polygon polygon;
Path path;
PathMetadata path_metadata;
XY path_start;
XY path_end;
XY mouse;
bool show_debug = true;
float Distance(XY p1, XY p2)
{
return sqrtf((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y));
}
bool CompareFloat(float a, float b, float epsilon = 0.000001f)
{
return a > b - epsilon && a < b + epsilon;
}
std::vector<int> FindConcavePoints(const Polygon& polygon)
{
std::vector<int> concave_points;
size_t n = polygon.size();
for (int i = 0; i < n; ++i)
{
XY a = polygon[(i - 1 + n) % n];
XY b = polygon[(i)];
XY c = polygon[(i + 1) % n];
float cross = (b.x - a.x) * (c.y - b.y) - (b.y - a.y) * (c.x - b.x);
if (cross >= 0)
continue;
concave_points.push_back(i);
}
return concave_points;
}
bool LineSegmentsCross(XY a, XY b, XY c, XY d)
{
float denominator = ((b.x - a.x) * (d.y - c.y)) - ((b.y - a.y) * (d.x - c.x));
if (CompareFloat(denominator, 0.0f))
return false;
float numerator1 = ((a.y - c.y) * (d.x - c.x)) - ((a.x - c.x) * (d.y - c.y));
float numerator2 = ((a.y - c.y) * (b.x - a.x)) - ((a.x - c.x) * (b.y - a.y));
if (CompareFloat(numerator1, 0.0f) || CompareFloat(numerator2, 0.0f))
return false;
float r = numerator1 / denominator;
float s = numerator2 / denominator;
return (r > 0 && r < 1) && (s > 0 && s < 1);
}
bool PointInPolygon(Polygon polygon, XY point)
{
const float epsilon = 0.5f;
bool inside = false;
if (polygon.size() < 3)
return false;
for (int i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++)
{
float xi = polygon[i].x, yi = polygon[i].y;
float xj = polygon[j].x, yj = polygon[j].y;
// Perform a ray-cast to determine if the point is on the correct side of the edge
if (((yi > point.y) != (yj > point.y)) && (point.x < (xj - xi) * (point.y - yi) / (yj - yi) + xi))
inside ^= 1;
}
return inside;
}
bool PointInLine(float x1, float y1, float x2, float y2, float x0, float y0)
{
return CompareFloat((y2 - y1) * x0 + (x1 - x2) * y0 + (x2 * y1 - x1 * y2), 0.0f);
}
bool InLineOfSight(XY start, XY end, const Polygon& polygon)
{
// Start/end locations are the same -- consider these within LoS
if (CompareFloat(Distance(start, end), 0.0f, 0.000001f))
return true;
// Start/end locations are outside the polygon -- don't consider these within LoS
if (!PointInPolygon(polygon, start) || !PointInPolygon(polygon, end))
return false;
// Start/end segments intersect polygon edges -- should this be the case, don't consider these within LoS
for (int i = 0; i < polygon.size(); i++)
{
XY line_start = polygon[i];
XY line_end = polygon[(i + 1) % polygon.size()];
if (LineSegmentsCross(start, end, line_start, line_end))
return false;
}
// Center between start/end locations is outside the polygon and not part of a line segment -- should this be the case,
// don't consider these within LoS. This addresses the same issue as the X/Y alignment hack above.
XY middle = {
(start.x + end.x) * 0.5f,
(start.y + end.y) * 0.5f
};
bool in_middle = false;
for (int i = 0; i < polygon.size(); i++)
{
XY line_start = polygon[i];
XY line_end = polygon[(i + 1) % polygon.size()];
if (PointInLine(middle.x, middle.y, line_start.x, line_start.y, line_end.x, line_end.y))
in_middle = true;
}
if (!PointInPolygon(polygon, middle) && !in_middle)
return false;
// If no intersections and both points are inside, they are within LoS
return true;
}
struct Node
{
XY position{};
float f = 0.0f, g = 0.0f, h = 0.0f; // A* stuff
bool open = false, closed = false; // A* stuff
Node* parent = nullptr;
std::vector<std::pair<Node*, float>> children;
Node() {}
Node(XY position): position(position) {}
void AddChild(Node* child, float distance)
{
children.emplace_back(child, distance);
}
float DistanceTo(Node* other)
{
return Distance(position, other->position);
}
};
// Comparator for STL heap and vector types
float CompareF(const Node* a, const Node* b)
{
return a->f < b->f;
}
bool FindPath(XY start, XY end, const Polygon& polygon, Path& path_out, PathMetadata* metadata)
{
// Step 1 -- Build a node hierarchy.
// We'll start by finding all of the concave points in the polygon; these will be our primary pathfinding nodes.
std::vector<int> concaves = FindConcavePoints(polygon);
std::vector<Node> nodes;
std::vector<std::pair<size_t, size_t>> links;
Node start_node(start);
Node end_node(end);
// Next calculate the normals of the polygon's vertices, allowing us to offset them to reduce the likelihood of encountering
// weird edge cases in our line-of-sight detection. Doing this also lets us ensure that our generated path will be slightly
// distant from the edges of the polygon, making it more suitable for something like an NPC in a game, which would otherwise
// collide with the edges whilst traversing the path.
std::unordered_map<int, XY> normals;
std::unordered_map<int, XY> v_normals;
for (size_t i = 0; i < polygon.size(); i++)
{
size_t j = (i + 1) % polygon.size();
XY normal = {-(polygon[j].y - polygon[i].y), polygon[j].x - polygon[i].x};
float normal_len = sqrtf(normal.x * normal.x + normal.y * normal.y);
normal.x /= normal_len;
normal.y /= normal_len;
normals.emplace(i, normal);
XY out1 = {polygon[i].x + (polygon[j].x - polygon[i].x) * 0.5f, polygon[i].y + (polygon[j].y - polygon[i].y) * 0.5f};
XY out2 = {out1.x + normal.x, out1.y + normal.y};
if (metadata)
metadata->normal_lines.emplace_back(out1, out2);
}
for (const auto& [i, normal]: normals)
{
size_t j = (normals.size() + i - 1) % normals.size();
XY v_normal = {normals[i].x + normals[j].x, normals[i].y + normals[j].y};
float v_normal_len = sqrtf(v_normal.x * v_normal.x + v_normal.y * v_normal.y);
v_normal.x /= v_normal_len;
v_normal.y /= v_normal_len;
v_normals.emplace(i, v_normal);
if (metadata)
metadata->normal_lines.emplace_back(polygon[i], XY{polygon[i].x + v_normal.x, polygon[i].y + v_normal.y});
}
// Now go through all of the concave points and create A* nodes from them. Additionally, link together ones which are in
// line-of-sight of each other.
for (size_t i = 0; i < concaves.size(); i++)
{
XY position = {polygon[concaves[i]].x + v_normals[concaves[i]].x, polygon[concaves[i]].y + v_normals[concaves[i]].y};
// Should our vertex with the normal applied lands outside of the polygon, push it back in...
while (!PointInPolygon(polygon, position))
{
position.x -= v_normals[concaves[i]].x * FUDGE;
position.y -= v_normals[concaves[i]].y * FUDGE;
}
if (metadata)
metadata->concave_points.push_back(position);
nodes.emplace_back(position);
for (size_t j = i + 1; j < concaves.size(); j++)
{
XY other_position = {
polygon[concaves[j]].x + v_normals[concaves[j]].x, polygon[concaves[j]].y + v_normals[concaves[j]].y
};
while (!PointInPolygon(polygon, other_position))
{
other_position.x -= v_normals[concaves[j]].x * FUDGE;
other_position.y -= v_normals[concaves[j]].y * FUDGE;
}
if (InLineOfSight(position, other_position, polygon))
{
if (metadata)
metadata->linked_concave_points.emplace_back(position, other_position);
links.emplace_back(i, j);
}
}
}
for (const auto& [i, j]: links)
{
nodes[i].AddChild(&nodes[j], nodes[i].DistanceTo(&nodes[j]));
nodes[j].AddChild(&nodes[i], nodes[i].DistanceTo(&nodes[j]));
}
// Now figure out which nodes are in line-of-sight of our start and end positions.
for (Node& node: nodes)
{
if (InLineOfSight(end_node.position, node.position, polygon))
{
node.AddChild(&end_node, node.DistanceTo(&end_node));
if (metadata)
{
metadata->end_lines.push_back(end_node.position);
metadata->end_lines.push_back(node.position);
}
}
if (InLineOfSight(start_node.position, node.position, polygon))
{
start_node.AddChild(&node, node.DistanceTo(&start_node));
if (metadata)
{
metadata->start_lines.push_back(start_node.position);
metadata->start_lines.push_back(node.position);
}
}
}
// Step 2 -- We have our node hierarchy, now perform A* pathfinding.
// This implementation is based on Antoine Vugliano's Pathfinder project.
// Portions of this code are licensed under the BSD 2-clause license; see LICENSE.txt for details.
float f = 0.0f, g = 0.0f, h = 0.0f;
std::vector<Node*> open, closed;
std::make_heap(open.begin(), open.end(), CompareF);
open.push_back(&start_node);
std::push_heap(open.begin(), open.end(), CompareF);
start_node.open = true;
Node* current_node = nullptr;
while (!open.empty())
{
std::sort(open.begin(), open.end(), CompareF);
current_node = open.front();
std::pop_heap(open.begin(), open.end(), CompareF);
open.pop_back();
current_node->open = false;
current_node->closed = true;
closed.push_back(current_node);
if (current_node == &end_node)
{
Node* parent = current_node->parent;
path_out.push_back(current_node->position);
while (parent != nullptr)
{
path_out.push_back(parent->position);
parent = parent->parent;
}
return true;
}
for (const auto& [child, distance]: current_node->children)
{
g = current_node->g + distance;
if ((child->open || child->closed) && child->g <= g)
continue;
h = Distance(child->position, end_node.position);
f = g + h;
child->f = f;
child->g = g;
child->h = h;
child->parent = current_node;
if (child->closed)
child->closed = false;
if (!child->open)
{
open.push_back(child);
std::push_heap(open.begin(), open.end(), CompareF);
child->open = true;
}
}
}
return false;
}
void Init()
{
path_start = {15, 6};
path_end = {18, 22};
polygon = {{3, 15}, {3, 3}, {24, 3}, {24, 8}, {6, 8}, {6, 10}, {24, 10}, {24, 24}, {15, 24}, {15, 17}, {21, 17}, {21, 15}};
}
void PerformPathfinding()
{
path_metadata.concave_points.clear();
path_metadata.linked_concave_points.clear();
path_metadata.normal_lines.clear();
path_metadata.start_lines.clear();
path_metadata.end_lines.clear();
path.clear();
if (!FindPath(path_start, path_end, polygon, path, &path_metadata))
std::cerr << "Failed to find path\n";
}
void DrawPoint(float x, float y, uint32_t color)
{
SDL_FRect point_rect = { x * SCALE - SCALE * 0.25f, y * SCALE - SCALE * 0.25f, SCALE * 0.5f, SCALE * 0.5f };
SDL_SetRenderDrawColor(renderer, (color >> 24) & 0xFF, (color >> 16) & 0xFF, (color >> 8) & 0xFF, (color) & 0xFF);
SDL_RenderFillRectF(renderer, &point_rect);
}
void DrawLine(float x1, float y1, float x2, float y2, uint32_t color)
{
SDL_SetRenderDrawColor(renderer, (color >> 24) & 0xFF, (color >> 16) & 0xFF, (color >> 8) & 0xFF, (color) & 0xFF);
SDL_RenderDrawLineF(renderer, x1 * SCALE, y1 * SCALE, x2 * SCALE, y2 * SCALE);
}
void DrawPath(const Path& path, uint32_t color)
{
if (path.empty())
return;
SDL_SetRenderDrawColor(renderer, (color >> 24) & 0xFF, (color >> 16) & 0xFF, (color >> 8) & 0xFF, (color) & 0xFF);
for (int i = 0; i < path.size() - 1; i++)
{
XY start = path[i];
XY end = path[i + 1];
SDL_RenderDrawLineF(renderer, start.x * SCALE, start.y * SCALE, end.x * SCALE, end.y * SCALE);
}
}
void DrawPolygon(const Polygon& polygon, uint32_t color)
{
SDL_SetRenderDrawColor(renderer, (color >> 24) & 0xFF, (color >> 16) & 0xFF, (color >> 8) & 0xFF, (color) & 0xFF);
for (int i = 0; i < path.size(); i++)
{
XY start = path[i];
XY end = path[(i + 1) % path.size()];
SDL_RenderDrawLineF(renderer, start.x * SCALE, start.y * SCALE, end.x * SCALE, end.y * SCALE);
}
}
void DrawString(const char* txt, int x, int y)
{
static uint64_t font[] = {0x7cfc3cfcfefe3cc6,0x7e7ec6c0c6c67cfc,0x7cfc7cfcc6c6c6c6,0xccfe187c0000600c,0x0000c6c666c6c0c0,
0x66c61818ccc0eee6,0xc6c6c6c6c630c6c6,0xc6c6cc0618c60000,0x30180000c6c6c0c6,0xc0c0c0c61818d8c0,0xfef6c6c6c6c6c030,
0xc6c6c66ccc0c1806,0x00001830fc00fefc,0xc0c6f8f8c0fe1818,0xf0c0d6dec6c6c6c6,0x7030c6c6c6387818,0x180c00000c600000,
0xc6c6c0c6c0c0cec6,0x1818d8c0c6cec6fc,0xc6fc1c30c6c6d66c,0x3030181800000c60,0x0000c6c6c0c6c0c0,0xc2c61818ccc0c6c6,
0xc6c0c6c60630c6c6,0xfec6306018300000,0x1830fc00c6c666c6,0xc0c066c61818c6c0,0xc6c6c6c0ccc6c630,0xc66ceec630c00000,
0x180830180000c6fc,0x3cfcfec03cc67e70,0xc6fec6c67cc07ac6,0x7c307c38c6c630fe,0x18301818600c0000,0x0c0000007c187c7c,
0xccfe7cfe7c7c7c30,0x467c6c000000d8c0,0xe0e060c030c03060,0x3c3c0c000000c638,0xc6c6ccc0c606c6c6,0xc678e6d66c00c0c0,
0xd8c0c060c0606060,0x30604242183000cc,0xc6180606ccc0c006,0xc6c6c6cc4cd0fe00,0xc0c04840c060c060,0x6060303099991830,
0x00ccd6180c1cfe7c,0xfc0c7c7e6c001870,0x6c6c00009080c060,0xc060c0303030a1a5,0x30fcfc30d6181806,0x0c06c618c6063000,
0x301c6cd800000000,0xc060c06060603018,0xa1a5303000ccc618,0x30060c06c630c606,0xd8006416fe00c040,0x0000c060c0606060,
0x3018999d603000cc,0xc61860c60cc6c630,0xc6c6cc00ced66c00,0xc0c00000c060c060,0x6060300c42466000,0x00007c3cfe7c0c7c,
0x7c307c7c7600c47c,0x6c0000000000e0e0,0x60c030c0300c3c3c};
static const char* font_mapping = "abcdefghijklmnopqrstuvwxyz!?.,><= /+-*0123456789&^%$#~:;\"'[](){}|\\`@";
static SDL_Surface* font_surface = NULL;
static SDL_Texture* font_texture = NULL;
if (!font_surface)
{
font_surface = SDL_CreateRGBSurfaceWithFormat(0, 272, 16, 32, SDL_PIXELFORMAT_ABGR8888);
for (int i = 0; i < 272 * 16; i++)
((uint32_t*)font_surface->pixels)[i] = ((font[i >> 6] >> (63 - (i & 63))) & 1) * 0xFFFFFFFF;
font_texture = SDL_CreateTextureFromSurface(renderer, font_surface);
}
for (int i = 0; i < strlen(txt); i++)
{
const char* pos = strchr(font_mapping, tolower(txt[i]));
size_t ci = pos ? pos - font_mapping : 0;
SDL_Rect src_rect = {(ci % 34) * 8, (ci / 34) * 8, 8, 8};
SDL_Rect dst_rect = {x + i * 8, y, 8, 8};
SDL_RenderCopy(renderer, font_texture, &src_rect, &dst_rect);
}
}
void Render()
{
// Draw map
SDL_SetRenderDrawBlendMode(renderer, SDL_BLENDMODE_BLEND);
SDL_SetRenderDrawColor(renderer, 0xFF, 0xFF, 0xFF, 0xFF);
for (int i = 0; i < polygon.size(); i++)
{
XY start = polygon[i];
XY end = polygon[(i + 1) % polygon.size()];
SDL_RenderDrawLineF(renderer, start.x * SCALE, start.y * SCALE, end.x * SCALE, end.y * SCALE);
}
// Draw start/end points
DrawPoint(path_start.x, path_start.y, 0xFF0000FF);
DrawPoint(path_end.x, path_end.y, 0x0000FFFF);
// Draw path metadata
if (show_debug)
{
// Draw data gathered from the pathfinding process
for (XY xy : path_metadata.concave_points)
DrawPoint(xy.x, xy.y, 0x00FFFFFF);
for (const auto& [i, j]: path_metadata.linked_concave_points)
DrawLine(i.x+1/SCALE, i.y+1/SCALE, j.x+1/SCALE, j.y+1/SCALE, 0xFF6600FF); // Offset a tiny bit for visibility
for (const auto& [i, j]: path_metadata.normal_lines)
DrawLine(i.x, i.y, j.x, j.y, 0xFF00FFFF);
DrawPath(path_metadata.start_lines, 0xFF00007F);
DrawPath(path_metadata.end_lines, 0x0000FF7F);
// Draw preview
DrawLine(polygon.back().x, polygon.back().y, mouse.x / SCALE, mouse.y / SCALE, 0xFFFFFF55);
}
// Draw generated path
DrawPath(path, 0x00AA00FF);
// """GUI"""
DrawString("Polygon-based A* implementation", 0, 0);
DrawString("-------------------------------", 0, 9);
DrawString("MOUSE1 = Draw Polygons, MOUSE2 = Clear Screen, S = move start, E = move end", 0, 18);
DrawString("X = Find Path, LSHIFT = Toggle path metadata & polygon edit preview", 0, 27);
}
} // namespace
/*
* SDL boilerplate
*/
int main(int, char*[])
{
SDL_Init(SDL_INIT_EVERYTHING);
window = SDL_CreateWindow("Pathfinding", SDL_WINDOWPOS_UNDEFINED, SDL_WINDOWPOS_UNDEFINED, 640, 480, SDL_WINDOW_SHOWN);
renderer = SDL_CreateRenderer(window, -1, SDL_RENDERER_PRESENTVSYNC);
Init();
for (;;)
{
for (SDL_Event e; SDL_PollEvent(&e); )
{
switch (e.type)
{
case SDL_QUIT:
goto cleanup;
case SDL_MOUSEMOTION:
mouse.x = (float)e.motion.x;
mouse.y = (float)e.motion.y;
break;
case SDL_KEYDOWN:
switch (e.key.keysym.scancode)
{
case SDL_SCANCODE_X:
PerformPathfinding();
break;
case SDL_SCANCODE_S:
path_start.x = mouse.x / SCALE;
path_start.y = mouse.y / SCALE;
break;
case SDL_SCANCODE_E:
path_end.x = mouse.x / SCALE;
path_end.y = mouse.y / SCALE;
break;
case SDL_SCANCODE_LSHIFT:
show_debug ^= 1;
break;
}
break;
case SDL_KEYUP:
break;
case SDL_MOUSEBUTTONDOWN:
switch (e.button.button)
{
case 1:
polygon.push_back({mouse.x / SCALE, mouse.y / SCALE});
break;
case 2:
break;
case 3:
polygon.clear();
break;
}
break;
}
}
SDL_SetRenderDrawColor(renderer, 0x00, 0x00, 0x55, 0x00);
SDL_RenderClear(renderer);
Render();
SDL_RenderPresent(renderer);
}
cleanup:
SDL_DestroyRenderer(renderer);
SDL_DestroyWindow(window);
SDL_Quit();
return 0;
}
You can also download the source here. To compile it, simply run g++ single.cpp -std=c++17 -lSDL2 -lm; ./a.out.
Last updated July 29th, 2024 at 00:39 PM CST