Added PathFinding #2 Video
Javidx9
6 years ago
committed by
GitHub
parent
72b31e83cf
commit
2ebb88a942
@ -0,0 +1,426 @@ |
|||||||
|
/*
|
||||||
|
OneLoneCoder.com - Path Finding #2 - Wave Propagation & Potential Fields |
||||||
|
"...never get lost again, so long as you know where you are" - @Javidx9 |
||||||
|
|
||||||
|
|
||||||
|
Background |
||||||
|
~~~~~~~~~~ |
||||||
|
A nice follow up alternative to the A* Algorithm. Wave propagation is less |
||||||
|
applicable to multiple objects with multiple destinations, but fantatsic |
||||||
|
for multiple objects all reaching the same destination. |
||||||
|
|
||||||
|
WARNING! This code is NOT OPTIMAL!! It is however very robust. There |
||||||
|
are many ways to optimise this further. |
||||||
|
|
||||||
|
License (OLC-3) |
||||||
|
~~~~~~~~~~~~~~~ |
||||||
|
|
||||||
|
Copyright 2018 OneLoneCoder.com |
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without |
||||||
|
modification, are permitted provided that the following conditions |
||||||
|
are met: |
||||||
|
|
||||||
|
1. Redistributions or derivations of source code must retain the above |
||||||
|
copyright notice, this list of conditions and the following disclaimer. |
||||||
|
|
||||||
|
2. Redistributions or derivative works in binary form must reproduce |
||||||
|
the above copyright notice. This list of conditions and the following |
||||||
|
disclaimer must be reproduced in the documentation and/or other |
||||||
|
materials provided with the distribution. |
||||||
|
|
||||||
|
3. Neither the name of the copyright holder nor the names of its |
||||||
|
contributors may be used to endorse or promote products derived |
||||||
|
from this software without specific prior written permission. |
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||||
|
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||||
|
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR |
||||||
|
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT |
||||||
|
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, |
||||||
|
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT |
||||||
|
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, |
||||||
|
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY |
||||||
|
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
||||||
|
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
||||||
|
|
||||||
|
Links |
||||||
|
~~~~~ |
||||||
|
YouTube: https://www.youtube.com/javidx9
|
||||||
|
Discord: https://discord.gg/WhwHUMV
|
||||||
|
Twitter: https://www.twitter.com/javidx9
|
||||||
|
Twitch: https://www.twitch.tv/javidx9
|
||||||
|
GitHub: https://www.github.com/onelonecoder
|
||||||
|
Patreon: https://www.patreon/javidx9
|
||||||
|
Homepage: https://www.onelonecoder.com
|
||||||
|
|
||||||
|
Relevant Videos |
||||||
|
~~~~~~~~~~~~~~~ |
||||||
|
Part #1 https://youtu.be/icZj67PTFhc
|
||||||
|
Part #2 https://youtu.be/0ihciMKlcP8
|
||||||
|
|
||||||
|
Author |
||||||
|
~~~~~~ |
||||||
|
David Barr, aka javidx9, ©OneLoneCoder 2018 |
||||||
|
*/ |
||||||
|
#define OLC_PGE_APPLICATION |
||||||
|
#include "olcPixelGameEngine.h" |
||||||
|
|
||||||
|
#include <vector> |
||||||
|
#include <list> |
||||||
|
#include <algorithm> |
||||||
|
#include <utility> |
||||||
|
|
||||||
|
|
||||||
|
// Override base class with your custom functionality
|
||||||
|
class PathFinding_FlowFields : public olc::PixelGameEngine |
||||||
|
{ |
||||||
|
public: |
||||||
|
PathFinding_FlowFields() |
||||||
|
{ |
||||||
|
sAppName = "PathFinding - Flow Fields"; |
||||||
|
} |
||||||
|
|
||||||
|
private: |
||||||
|
int nMapWidth; |
||||||
|
int nMapHeight; |
||||||
|
int nCellSize; |
||||||
|
int nBorderWidth; |
||||||
|
|
||||||
|
bool *bObstacleMap; |
||||||
|
|
||||||
|
int *nFlowFieldZ; |
||||||
|
float *fFlowFieldY; |
||||||
|
float *fFlowFieldX; |
||||||
|
|
||||||
|
int nStartX; |
||||||
|
int nStartY; |
||||||
|
int nEndX; |
||||||
|
int nEndY; |
||||||
|
|
||||||
|
int nWave = 1; |
||||||
|
|
||||||
|
public: |
||||||
|
bool OnUserCreate() override |
||||||
|
{ |
||||||
|
nBorderWidth = 4; |
||||||
|
nCellSize = 32; |
||||||
|
nMapWidth = ScreenWidth() / nCellSize; |
||||||
|
nMapHeight = ScreenHeight() / nCellSize; |
||||||
|
bObstacleMap = new bool[nMapWidth * nMapHeight]{ false }; |
||||||
|
nFlowFieldZ = new int[nMapWidth * nMapHeight]{ 0 }; |
||||||
|
fFlowFieldX = new float[nMapWidth * nMapHeight]{ 0 }; |
||||||
|
fFlowFieldY = new float[nMapWidth * nMapHeight]{ 0 }; |
||||||
|
|
||||||
|
nStartX = 3; |
||||||
|
nStartY = 7; |
||||||
|
nEndX = 12; |
||||||
|
nEndY = 7; |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
bool OnUserUpdate(float fElapsedTime) override |
||||||
|
{ |
||||||
|
// Little convenience lambda 2D -> 1D
|
||||||
|
auto p = [&](int x, int y) { return y * nMapWidth + x; }; |
||||||
|
|
||||||
|
// User Input
|
||||||
|
int nSelectedCellX = GetMouseX() / nCellSize; |
||||||
|
int nSelectedCellY = GetMouseY() / nCellSize; |
||||||
|
|
||||||
|
if (GetMouse(0).bReleased) |
||||||
|
{ |
||||||
|
// Toggle Obstacle if mouse left clicked
|
||||||
|
bObstacleMap[p(nSelectedCellX, nSelectedCellY)] = |
||||||
|
!bObstacleMap[p(nSelectedCellX, nSelectedCellY)]; |
||||||
|
}
|
||||||
|
|
||||||
|
if (GetMouse(1).bReleased) |
||||||
|
{ |
||||||
|
nStartX = nSelectedCellX; |
||||||
|
nStartY = nSelectedCellY; |
||||||
|
} |
||||||
|
|
||||||
|
if (GetKey(olc::Key::Q).bReleased) |
||||||
|
{ |
||||||
|
nWave++; |
||||||
|
} |
||||||
|
|
||||||
|
if (GetKey(olc::Key::A).bReleased) |
||||||
|
{ |
||||||
|
nWave--; |
||||||
|
if (nWave == 0) |
||||||
|
nWave = 1; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// 1) Prepare flow field, add a boundary, and add obstacles
|
||||||
|
// by setting the flow Field Height (Z) to -1
|
||||||
|
for (int x = 0; x < nMapWidth; x++) |
||||||
|
{ |
||||||
|
for (int y = 0; y < nMapHeight; y++) |
||||||
|
{ |
||||||
|
// Set border or obstacles
|
||||||
|
if (x == 0 || y == 0 || x == (nMapWidth - 1) || y == (nMapHeight - 1) || bObstacleMap[p(x, y)]) |
||||||
|
{ |
||||||
|
nFlowFieldZ[p(x, y)] = -1; |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
nFlowFieldZ[p(x, y)] = 0; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
// 2) Propagate a wave (i.e. flood-fill) from target location. Here I use
|
||||||
|
// a tuple, of {x, y, distance} - though you could use a struct or
|
||||||
|
// similar.
|
||||||
|
std::list<std::tuple<int, int, int>> nodes; |
||||||
|
|
||||||
|
// Add the first discovered node - the target location, with a distance of 1
|
||||||
|
nodes.push_back({ nEndX, nEndY, 1 }); |
||||||
|
|
||||||
|
while (!nodes.empty()) |
||||||
|
{ |
||||||
|
// Each iteration through the discovered nodes may create newly discovered
|
||||||
|
// nodes, so I maintain a second list. It's important not to contaminate
|
||||||
|
// the list being iterated through.
|
||||||
|
std::list<std::tuple<int, int, int>> new_nodes; |
||||||
|
|
||||||
|
// Now iterate through each discovered node. If it has neighbouring nodes
|
||||||
|
// that are empty space and undiscovered, add those locations to the
|
||||||
|
// new nodes list
|
||||||
|
for (auto &n : nodes) |
||||||
|
{ |
||||||
|
int x = std::get<0>(n); // Map X-Coordinate
|
||||||
|
int y = std::get<1>(n); // Map Y-Coordinate
|
||||||
|
int d = std::get<2>(n); // Distance From Target Location
|
||||||
|
|
||||||
|
// Set distance count for this node. NOte that when we add nodes we add 1
|
||||||
|
// to this distance. This emulates propagating a wave across the map, where
|
||||||
|
// the front of that wave increments each iteration. In this way, we can
|
||||||
|
// propagate distance information 'away from target location'
|
||||||
|
nFlowFieldZ[p(x, y)] = d; |
||||||
|
|
||||||
|
// Add neigbour nodes if unmarked, i.e their "height" is 0. Any discovered
|
||||||
|
// node or obstacle will be non-zero
|
||||||
|
|
||||||
|
// Check East
|
||||||
|
if ((x + 1) < nMapWidth && nFlowFieldZ[p(x + 1, y)] == 0) |
||||||
|
new_nodes.push_back({ x + 1, y, d + 1 }); |
||||||
|
|
||||||
|
// Check West
|
||||||
|
if ((x - 1) >= 0 && nFlowFieldZ[p(x - 1, y)] == 0) |
||||||
|
new_nodes.push_back({ x - 1, y, d + 1 }); |
||||||
|
|
||||||
|
// Check South
|
||||||
|
if ((y + 1) < nMapHeight && nFlowFieldZ[p(x, y + 1)] == 0) |
||||||
|
new_nodes.push_back({ x, y + 1, d + 1 }); |
||||||
|
|
||||||
|
// Check North
|
||||||
|
if ((y - 1) >= 0 && nFlowFieldZ[p(x, y - 1)] == 0) |
||||||
|
new_nodes.push_back({ x, y - 1, d + 1 }); |
||||||
|
} |
||||||
|
|
||||||
|
// We will now have potentially multiple nodes for a single location. This means our
|
||||||
|
// algorithm will never complete! So we must remove duplicates form our new node list.
|
||||||
|
// Im doing this with some clever code - but it is not performant(!) - it is merely
|
||||||
|
// convenient. I'd suggest doing away with overhead structures like linked lists and sorts
|
||||||
|
// if you are aiming for fastest path finding.
|
||||||
|
|
||||||
|
// Sort the nodes - This will stack up nodes that are similar: A, B, B, B, B, C, D, D, E, F, F
|
||||||
|
new_nodes.sort([&](const std::tuple<int, int, int> &n1, const std::tuple<int, int, int> &n2) |
||||||
|
{ |
||||||
|
// In this instance I dont care how the values are sorted, so long as nodes that
|
||||||
|
// represent the same location are adjacent in the list. I can use the p() lambda
|
||||||
|
// to generate a unique 1D value for a 2D coordinate, so I'll sort by that.
|
||||||
|
return p(std::get<0>(n1), std::get<1>(n1)) < p(std::get<0>(n2), std::get<1>(n2)); |
||||||
|
}); |
||||||
|
|
||||||
|
// Use "unique" function to remove adjacent duplicates : A, B, -, -, -, C, D, -, E, F -
|
||||||
|
// and also erase them : A, B, C, D, E, F
|
||||||
|
new_nodes.unique([&](const std::tuple<int, int, int> &n1, const std::tuple<int, int, int> &n2) |
||||||
|
{ |
||||||
|
return p(std::get<0>(n1), std::get<1>(n1)) == p(std::get<0>(n2), std::get<1>(n2)); |
||||||
|
}); |
||||||
|
|
||||||
|
// We've now processed all the discoverd nodes, so clear the list, and add the newly
|
||||||
|
// discovered nodes for processing on the next iteration
|
||||||
|
nodes.clear(); |
||||||
|
nodes.insert(nodes.begin(), new_nodes.begin(), new_nodes.end()); |
||||||
|
|
||||||
|
// When there are no more newly discovered nodes, we have "flood filled" the entire
|
||||||
|
// map. The propagation phase of the algorithm is complete
|
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
// 3) Create Path. Starting a start location, create a path of nodes until you reach target
|
||||||
|
// location. At each node find the neighbour with the lowest "distance" score.
|
||||||
|
std::list<std::pair<int, int>> path; |
||||||
|
path.push_back({ nStartX, nStartY }); |
||||||
|
int nLocX = nStartX; |
||||||
|
int nLocY = nStartY; |
||||||
|
bool bNoPath = false; |
||||||
|
|
||||||
|
while (!(nLocX == nEndX && nLocY == nEndY) && !bNoPath) |
||||||
|
{ |
||||||
|
std::list<std::tuple<int, int, int>> listNeighbours; |
||||||
|
|
||||||
|
// 4-Way Connectivity
|
||||||
|
if ((nLocY - 1) >= 0 && nFlowFieldZ[p(nLocX, nLocY - 1)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX, nLocY - 1, nFlowFieldZ[p(nLocX, nLocY - 1)] }); |
||||||
|
|
||||||
|
if ((nLocX + 1) < nMapWidth && nFlowFieldZ[p(nLocX + 1, nLocY)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX + 1, nLocY, nFlowFieldZ[p(nLocX + 1, nLocY)] }); |
||||||
|
|
||||||
|
if ((nLocY + 1) < nMapHeight && nFlowFieldZ[p(nLocX, nLocY + 1)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX, nLocY + 1, nFlowFieldZ[p(nLocX, nLocY + 1)] }); |
||||||
|
|
||||||
|
if ((nLocX - 1) >= 0 && nFlowFieldZ[p(nLocX - 1, nLocY)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX - 1, nLocY, nFlowFieldZ[p(nLocX - 1, nLocY)] }); |
||||||
|
|
||||||
|
// 8-Way Connectivity
|
||||||
|
if ((nLocY - 1) >= 0 && (nLocX - 1) >= 0 && nFlowFieldZ[p(nLocX - 1, nLocY - 1)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX - 1, nLocY - 1, nFlowFieldZ[p(nLocX - 1, nLocY - 1)] }); |
||||||
|
|
||||||
|
if ((nLocY - 1) >= 0 && (nLocX + 1) < nMapWidth && nFlowFieldZ[p(nLocX + 1, nLocY - 1)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX + 1, nLocY - 1, nFlowFieldZ[p(nLocX + 1, nLocY - 1)] }); |
||||||
|
|
||||||
|
if ((nLocY + 1) < nMapHeight && (nLocX - 1) >= 0 && nFlowFieldZ[p(nLocX - 1, nLocY + 1)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX - 1, nLocY + 1, nFlowFieldZ[p(nLocX - 1, nLocY + 1)] }); |
||||||
|
|
||||||
|
if ((nLocY + 1) < nMapHeight && (nLocX + 1) < nMapWidth && nFlowFieldZ[p(nLocX + 1, nLocY + 1)] > 0) |
||||||
|
listNeighbours.push_back({ nLocX + 1, nLocY + 1, nFlowFieldZ[p(nLocX + 1, nLocY + 1)] }); |
||||||
|
|
||||||
|
// Sprt neigbours based on height, so lowest neighbour is at front
|
||||||
|
// of list
|
||||||
|
listNeighbours.sort([&](const std::tuple<int, int, int> &n1, const std::tuple<int, int, int> &n2) |
||||||
|
{ |
||||||
|
return std::get<2>(n1) < std::get<2>(n2); // Compare distances
|
||||||
|
}); |
||||||
|
|
||||||
|
if (listNeighbours.empty()) // Neighbour is invalid or no possible path
|
||||||
|
bNoPath = true; |
||||||
|
else |
||||||
|
{ |
||||||
|
nLocX = std::get<0>(listNeighbours.front()); |
||||||
|
nLocY = std::get<1>(listNeighbours.front()); |
||||||
|
path.push_back({ nLocX, nLocY }); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
// 4) Create Flow "Field"
|
||||||
|
for (int x = 1; x < nMapWidth - 1; x++) |
||||||
|
{ |
||||||
|
for (int y = 1; y < nMapHeight - 1; y++) |
||||||
|
{ |
||||||
|
float vx = 0.0f; |
||||||
|
float vy = 0.0f; |
||||||
|
|
||||||
|
vy -= (float)((nFlowFieldZ[p(x, y + 1)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x, y + 1)]) - nFlowFieldZ[p(x, y)]); |
||||||
|
vx -= (float)((nFlowFieldZ[p(x + 1, y)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x + 1, y)]) - nFlowFieldZ[p(x, y)]); |
||||||
|
vy += (float)((nFlowFieldZ[p(x, y - 1)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x, y - 1)]) - nFlowFieldZ[p(x, y)]); |
||||||
|
vx += (float)((nFlowFieldZ[p(x - 1, y)] <= 0 ? nFlowFieldZ[p(x, y)] : nFlowFieldZ[p(x - 1, y)]) - nFlowFieldZ[p(x, y)]); |
||||||
|
|
||||||
|
float r = 1.0f / sqrtf(vx*vx + vy * vy); |
||||||
|
fFlowFieldX[p(x, y)] = vx * r; |
||||||
|
fFlowFieldY[p(x, y)] = vy * r; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Draw Map
|
||||||
|
Clear(olc::BLACK); |
||||||
|
|
||||||
|
for (int x = 0; x < nMapWidth; x++) |
||||||
|
{ |
||||||
|
for (int y = 0; y < nMapHeight; y++) |
||||||
|
{ |
||||||
|
olc::Pixel colour = olc::BLUE; |
||||||
|
|
||||||
|
if (bObstacleMap[p(x, y)]) |
||||||
|
colour = olc::GREY; |
||||||
|
|
||||||
|
if (nWave == nFlowFieldZ[p(x, y)]) |
||||||
|
colour = olc::DARK_CYAN; |
||||||
|
|
||||||
|
if (x == nStartX && y == nStartY) |
||||||
|
colour = olc::GREEN; |
||||||
|
|
||||||
|
if (x == nEndX && y == nEndY) |
||||||
|
colour = olc::RED; |
||||||
|
|
||||||
|
// Draw Base
|
||||||
|
FillRect(x * nCellSize, y * nCellSize, nCellSize - nBorderWidth, nCellSize - nBorderWidth, colour); |
||||||
|
|
||||||
|
// Draw "potential" or "distance" or "height" :D
|
||||||
|
//DrawString(x * nCellSize, y * nCellSize, std::to_string(nFlowFieldZ[p(x, y)]), olc::WHITE);
|
||||||
|
|
||||||
|
if (nFlowFieldZ[p(x, y)] > 0) |
||||||
|
{ |
||||||
|
float ax[4], ay[4]; |
||||||
|
float fAngle = atan2f(fFlowFieldY[p(x, y)], fFlowFieldX[p(x, y)]); |
||||||
|
float fRadius = (float)(nCellSize - nBorderWidth) / 2.0f; |
||||||
|
int fOffsetX = x * nCellSize + ((nCellSize - nBorderWidth) / 2); |
||||||
|
int fOffsetY = y * nCellSize + ((nCellSize - nBorderWidth) / 2); |
||||||
|
ax[0] = cosf(fAngle) * fRadius + fOffsetX; |
||||||
|
ay[0] = sinf(fAngle) * fRadius + fOffsetY; |
||||||
|
ax[1] = cosf(fAngle) * -fRadius + fOffsetX; |
||||||
|
ay[1] = sinf(fAngle) * -fRadius + fOffsetY; |
||||||
|
ax[2] = cosf(fAngle + 0.1f) * fRadius * 0.7f + fOffsetX; |
||||||
|
ay[2] = sinf(fAngle + 0.1f) * fRadius * 0.7f + fOffsetY; |
||||||
|
ax[3] = cosf(fAngle - 0.1f) * fRadius * 0.7f + fOffsetX; |
||||||
|
ay[3] = sinf(fAngle - 0.1f) * fRadius * 0.7f + fOffsetY; |
||||||
|
|
||||||
|
DrawLine(ax[0], ay[0], ax[1], ay[1], olc::CYAN); |
||||||
|
DrawLine(ax[0], ay[0], ax[2], ay[2], olc::CYAN); |
||||||
|
DrawLine(ax[0], ay[0], ax[3], ay[3], olc::CYAN); |
||||||
|
|
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
bool bFirstPoint = true; |
||||||
|
int ox, oy; |
||||||
|
for (auto &a : path) |
||||||
|
{ |
||||||
|
if (bFirstPoint) |
||||||
|
{ |
||||||
|
ox = a.first; |
||||||
|
oy = a.second; |
||||||
|
bFirstPoint = false; |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
DrawLine( |
||||||
|
ox * nCellSize + ((nCellSize - nBorderWidth) / 2), |
||||||
|
oy * nCellSize + ((nCellSize - nBorderWidth) / 2), |
||||||
|
a.first * nCellSize + ((nCellSize - nBorderWidth) / 2), |
||||||
|
a.second * nCellSize + ((nCellSize - nBorderWidth) / 2), olc::YELLOW); |
||||||
|
|
||||||
|
ox = a.first; |
||||||
|
oy = a.second; |
||||||
|
|
||||||
|
FillCircle(ox * nCellSize + ((nCellSize - nBorderWidth) / 2), oy * nCellSize + ((nCellSize - nBorderWidth) / 2), 10, olc::YELLOW); |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
return true; |
||||||
|
} |
||||||
|
}; |
||||||
|
|
||||||
|
|
||||||
|
int main() |
||||||
|
{ |
||||||
|
PathFinding_FlowFields demo; |
||||||
|
if (demo.Construct(512, 480, 2, 2)) |
||||||
|
demo.Start(); |
||||||
|
return 0; |
||||||
|
} |
||||||
Loading…
Reference in new issue