|
|
/*
|
|
|
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, <EFBFBD>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;
|
|
|
} |
