KDtree : bad performances when the point is far away - c++

I have a basic KDtree implementation so far and the performance is very bad when the points are very far, i.e. if all the points are in [-10;10]^3 and the query is for example at (100, 100, 100), then the bruteforce is 20% faster than the normal version. Why?
This implementation is very basic, I know there is possible improvement everywhere, but just to make work with the minimum vital but still, it shouldn't be that slow... right?
KDTree.h
#pragma once
#include <memory>
#include <vector>
#include <cassert>
#include <vector>
#include <stdexcept>
#include <cmath>
#include <stack>
/**
* #brief K-d tree of 3D float points.
*
* #see Variable names, and algorithm inspired by https://youtu.be/TrqK-atFfWY?t=2567
*/
class KDTree
{
public:
/**** Utility structures ****/
struct Point
{
float x, y, z;
float& operator[](int i)
{
assert(i < 3 && i >= 0);
switch(i) {
case 0: return x;
case 1: return y;
case 2: return z;
}
throw std::runtime_error("Invalid index");
}
const float& operator[](int i) const
{
return const_cast<Point&>(*this)[i];
}
float distance(const Point& other) const
{
return sqrtf(distanceSquared(other));
}
float distanceSquared(const Point& other) const
{
const float dx = (x - other.x);
const float dy = (y - other.y);
const float dz = (z - other.z);
return dx*dx + dy*dy + dz*dz;
}
template<typename T> friend T& operator<<(T& lhs, const Point& rhs)
{
lhs << "(" << rhs.x << ", " << rhs.y << ", " << rhs.z << ")";
return lhs;
}
};
struct AABB
{
Point min, max;
bool inside(const Point& p) const
{
return p.x >= min.x && p.y >= min.y && p.z >= min.z
&& p.x < max.x && p.y < max.y && p.z < max.z;
}
};
public:
/**** Utility (static) functions ****/
/**
* #brief Compute the bounding box of a set of points.
* #param points
* The points to make the boudning box from.
* If the set of points is empty, the returned value is undefined.
* #return
* The bounding box of theses points.
* The bounding box will be always of minimum size, i.e. there will be a vertex on each of the 6 faces of the
* returned AABB.
*/
static AABB computeBoundingBox(const std::vector<Point> &points);
static float median(std::vector<float> vec);
static void store_min(float& current, float newValue)
{
if(current > newValue) {
current = newValue;
}
}
static void store_max(float& current, float newValue)
{
if(current < newValue) {
current = newValue;
}
}
public:
KDTree() = default;
explicit KDTree(const std::vector<Point>& points);
Point computeNearestNeighbor(const Point& pos) const;
private:
/**
* #brief Represent each node of the k-d tree.
*/
struct Node
{
/**
* #brief
* Children. Both of them or neither of them are null.
*/
std::unique_ptr<Node> left, right;
/**
* The distance between the origin and the wall to split (for left node),
* or the distance from the wall and the end to split (for right node).
*/
float splitDistance;
int splitDim;
std::vector<Point> points;
/**
* #return true if this node is a leaf node (it has no children).
*/
bool leaf() const
{
// left or right it doesn't matter
return left == nullptr;
}
};
struct SplitStack
{
int dim;
std::vector<Point> points;
Node* node;
AABB aabb;
};
private:
/**
* #brief Split the tree during the build.
* #param dim The dimension to split.
* #param points The list of remaining candidate points for this area, inside the AABB.
* #param node The node, allocated, to fill.
* #param aabb The bounding box of the node.
*/
void split(std::stack<SplitStack>& stack);
void searchRecursive(const Point& pos, Node* node, float& currentDist, Point& currentNeighbor) const;
private:
AABB m_rootAABB;
std::unique_ptr<Node> m_root;
};
KDTree.cpp
#include "KDTree.h"
#include <climits>
#include <cfloat>
#include <algorithm>
#include <iostream>
#include <cmath>
float KDTree::median(std::vector<float> vec)
{
size_t size = vec.size();
if (size == 0)
{
return 0; // Undefined, really.
}
else
{
std::sort(vec.begin(), vec.end());
if (size % 2 == 0)
{
return (vec[size / 2 - 1] + vec[size / 2]) / 2;
}
else
{
return vec[size / 2];
}
}
}
KDTree::AABB KDTree::computeBoundingBox(const std::vector<Point>& points)
{
AABB res;
if (!points.empty())
{
const auto& firstPoint = points.front();
// Initialize the bounding box to a point
for (int dim = 0; dim < 3; dim++)
{
res.min[dim] = firstPoint[dim];
res.max[dim] = firstPoint[dim];
}
// Grow the bounding box for each point if needed
for (const Point& point: points)
{
for (int dim = 0; dim < 3; dim++)
{
store_min(res.min[dim], point[dim]);
store_max(res.max[dim], point[dim]);
}
}
}
return res;
}
KDTree::KDTree(const std::vector<Point>& points)
{
m_rootAABB = computeBoundingBox(points);
m_root = std::make_unique<Node>();
std::stack<SplitStack> stack;
stack.push(SplitStack{0, points, m_root.get(), m_rootAABB});
split(stack);
}
void KDTree::split(std::stack<SplitStack>& stack)
{
// Split recursively in x, y, z, x, y, z...
// Split at the center
// dim axis -->
// 0 --------- aabb[dim].min --------------------------- aabb[dim].max --------- +inf
// ------------------|--------------------|-------------------|-------------------
// ---------------------- left node ----------- right node -----------------------
// <-----------------> <--------------->
// splitDistance: if left if right
while (!stack.empty())
{
std::vector<Point> points = std::move(stack.top().points);
Node& node = *stack.top().node;
int dim = stack.top().dim;
AABB aabb = stack.top().aabb;
stack.pop();
// Stop condition
if (points.size() > 100)
{
node.splitDim = dim;
// Absolute position in the dimension of the split
node.splitDistance = (aabb.max[dim] + aabb.min[dim]) / 2.0f;
AABB leftAABB = aabb;
leftAABB.max[dim] = node.splitDistance;
AABB rightAABB = aabb;
rightAABB.min[dim] = leftAABB.max[dim];
std::vector<Point> leftPoints, rightPoints;
for (const Point& p: points)
{
if (leftAABB.inside(p))
{
leftPoints.push_back(p);
}
else
{
rightPoints.push_back(p);
}
}
const int nextDim = (dim + 1) % 3;
node.right = std::make_unique<Node>();
stack.push(SplitStack{nextDim, std::move(rightPoints), node.right.get(), rightAABB});
node.left = std::make_unique<Node>();
stack.push(SplitStack{nextDim, std::move(leftPoints), node.left.get(), leftAABB});
}
else
{
// Leaf
node.points = std::move(points);
}
}
}
KDTree::Point KDTree::computeNearestNeighbor(const KDTree::Point& pos) const
{
// Are we left or right?
const Node *node = m_root.get();
AABB aabb = m_rootAABB;
float dist = FLT_MAX;
Point res;
searchRecursive(pos, m_root.get(), dist, res);
return res;
}
void KDTree::searchRecursive(const Point& pos, Node *node, float& currentDist, Point& currentNeighbor) const
{
// Are we on a leaf?
if (node->leaf())
{
// We are on a leaf
// Search brute force into the leaf node
for (const auto& other: node->points)
{
const float d = other.distanceSquared(pos);
if (d < currentDist)
{
currentDist = d;
currentNeighbor = other;
}
}
}
else
{
Node *front, *back;
// Are we on the left side?
if (pos[node->splitDim] < node->splitDistance)
{
// Pos is on the left side
front = node->left.get();
back = node->right.get();
}
else
{
// Pos is on the right side
front = node->right.get();
back = node->left.get();
}
searchRecursive(pos, front, currentDist, currentNeighbor);
// If the current closest point is closer than the closest point of the back face, no need to search in the back
// face because it will be always further.
// If not, we save half of the time for the current node
const float backDist = fabsf(node->splitDistance - pos[node->splitDim]);
// Do not forget all distances all squared
if (backDist * backDist <= currentDist)
{
// If it can be closer, search also in this node
searchRecursive(pos, back, currentDist, currentNeighbor);
}
}
}
main.cpp
#include <iostream>
#include <vector>
#include <random>
#include "KDTree.h"
#include "viewer.h"
#include <chrono>
class Timer {
public:
Timer(const std::string& title) : title_(title), beg_(clock_::now()) {}
~Timer() {
std::cout << title_ << " elapsed: " << elapsed() << "s" << std::endl;
}
void reset() { beg_ = clock_::now(); }
double elapsed() const {
return std::chrono::duration_cast<second_> (clock_::now() - beg_).count();
}
private:
std::string title_;
typedef std::chrono::high_resolution_clock clock_;
typedef std::chrono::duration<double, std::ratio<1> > second_;
std::chrono::time_point<clock_> beg_;
};
std::vector<KDTree::Point> randomPoints(int size, float bounds = 10.0f)
{
std::vector<KDTree::Point> points;
std::uniform_real_distribution<float> dist(-bounds, bounds);
std::mt19937 engine;
for(int i = 0; i < size; i++)
{
KDTree::Point point;
for(int dim = 0; dim < 3; dim++) {
point[dim] = dist(engine);
}
points.push_back(point);
}
return points;
}
int main()
{
auto points = randomPoints(1'000'000);
for(int i = 0; i < points.size(); i++) {
auto& point = points[i];
}
auto aabb = KDTree::computeBoundingBox(points);
KDTree kdtree;
{
Timer timer("KDTree build");
kdtree = KDTree(points);
}
{
const int N = 1'000;
auto testPts = randomPoints(N, 100.0f);
std::vector<KDTree::Point> resKD(N), resBrute(N);
{
Timer timer("KDTree");
for(int i = 0; i < testPts.size(); i++) {
resKD[i] = kdtree.computeNearestNeighbor(testPts[i]);
}
}
{
Timer timer("Bruteforce");
for(int i = 0; i < testPts.size(); i++) {
const auto& test = testPts[i];
// Brute force
KDTree::Point cur;
float curDist = FLT_MAX;
for(const auto& brute : points) {
if(brute.distanceSquared(test) < curDist) {
curDist = brute.distanceSquared(test);
cur = brute;
}
}
resBrute[i] = cur;
}
}
{
float delta = 0.0f;
for(int i = 0; i < N; i++) {
delta += resKD[i].x - resBrute[i].x;
delta += resKD[i].y - resBrute[i].y;
delta += resKD[i].z - resBrute[i].z;
}
std::cout << "delta = " << delta << std::endl;
}
}
return 0;
}
Output:
KDTree build elapsed: 0.190593s
KDTree elapsed: 2.69598s
Bruteforce elapsed: 2.34136s
delta = 0
When I lower to randomPoints(N, 10.0f):
KDTree build elapsed: 0.195519s
KDTree elapsed: 0.000914431s
Bruteforce elapsed: 2.35679s
delta = 0

As written on wikipedia:
Additionally, even in low-dimensional space, if the average pairwise distance between the k nearest neighbors of the query point is significantly less than the average distance between the query point and each of the k nearest neighbors, the performance of nearest neighbor search degrades towards linear, since the distances from the query point to each nearest neighbor are of similar magnitude. (In the worst case, consider a cloud of points distributed on the surface of a sphere centered at the origin. Every point is equidistant from the origin, so a search for the nearest neighbor from the origin would have to iterate through all points on the surface of the sphere to identify the nearest neighbor – which in this case is not even unique.)
So this is normal to see performance decrease in a KD Tree when the points are far.

Related

Im trying to make an astar algorithm for a school thing

Here is my code i've been working on. I've scraped off all the errors from the code. But i cant get the code to run correctly and the application either explodes or doesn't atleast visualise the astar.
#include <SFML/Graphics.hpp>
#include <queue>
#include <unordered_map>
#include <cmath>
#include <vector>
const int WIDTH = 10;
const int HEIGHT = 10;
struct Node
{
int x;
int y;
mutable float gScore;
mutable float fScore;
mutable const Node* parent;
Node(int _x, int _y) : x(_x), y(_y), gScore(0), fScore(0), parent(nullptr) {}
bool operator==(const Node& other) const
{
return x == other.x && y == other.y;
}
};
// Hash function for nodes, needed for storing them in an unordered_map
struct NodeHash
{
std::size_t operator()(const Node& node) const
{
return std::hash<int>()(node.x) ^ std::hash<int>()(node.y);
}
};
// Overload the ">" operator for nodes, needed for using them in a priority queue
struct NodeGreater
{
bool operator()(const Node& left, const Node& right) const
{
return left.fScore > right.fScore;
}
};
// Represents a 2D grid
class Grid
{
public:
Grid()
{
// Initialize the grid with all nodes as walkable
for (int x = 0; x < WIDTH; x++)
{
for (int y = 0; y < HEIGHT; y++)
{
nodes[x][y] = true;
}
}
}
// Set the walkability of a node
void setNode(int x, int y, bool walkable)
{
nodes[x][y] = walkable;
}
// Check if a node is walkable
bool isNodeWalkable(int x, int y)
{
return nodes[x][y];
}
private:
bool nodes[WIDTH][HEIGHT];
};
// Calculate the Euclidean distance between two nodes
float euclideanDistance(Node a, Node b)
{
int xDist = b.x - a.x;
int yDist = b.y - a.y;
return std::sqrt(xDist * xDist + yDist * yDist);
}
// Estimate the cost of reaching the end node from a given node
float heuristicCostEstimate(Node start, Node end)
{
return euclideanDistance(start, end);
}
// Get the neighbors of a given node
std::vector<Node> getNeighbors(Grid grid, Node node)
{
std::vector<Node> neighbors;
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
// Skip the current node
if (x == 0 && y == 0)
{
continue;
}
int neighborX = node.x + x;
int neighborY = node.y + y;
// Make sure the neighbor is within the bounds of the grid
if (neighborX >= 0 && neighborX < WIDTH && neighborY >= 0 && neighborY < HEIGHT)
{
// Make sure the neighbor is walkable
if (grid.isNodeWalkable(neighborX, neighborY))
{
Node neighbor = { neighborX, neighborY };
neighbors.push_back(neighbor);
}
}
}
}
return neighbors;
}
// Reconstruct the path from the end node to the start node
std::vector<Node> reconstructPath(Node end)
{
std::vector<Node> path;
Node current = end;
while (current.parent != nullptr)
{
path.push_back(current);
current = *current.parent;
}
path.push_back(current);
std::reverse(path.begin(), path.end());
return path;
}
// Run the A* algorithm to find the shortest path from the start to the end node
std::vector<Node> aStar(Grid grid, Node start, Node end)
{
// Initialize the open and closed sets
std::priority_queue<Node, std::vector<Node>, NodeGreater> openSet;
std::unordered_map<Node, bool, NodeHash> closedSet;
// Add the start node to the open set
openSet.push(start);
// Set the starting node's gScore to 0 and fScore to the estimated cost to the end node
start.gScore = 0;
start.fScore = heuristicCostEstimate(start, end);
// Keep searching until the open set is empty
while (!openSet.empty())
{
// Get the node in the open set with the lowest fScore
Node current = openSet.top();
openSet.pop();
// If the current node is the end node, we have found the shortest path
if (current == end)
{
return reconstructPath(current);
}
// Add the current node to the closed set
closedSet[current] = true;
// Get the neighbors of the current node
std::vector<Node> neighbors = getNeighbors(grid, current);
// Iterate through the neighbors
for (const Node& neighbor : neighbors)
{
// Skip the neighbor if it is already in the closed set
if (closedSet.count(neighbor) > 0)
{
continue;
}
// Calculate the tentative gScore for the neighbor
float tentativeGScore = current.gScore + euclideanDistance(current, neighbor);
bool neighborIsBetter = false;
bool neighborInOpenSet = false;
// Check if the neighbor is already in the open set
std::priority_queue<Node, std::vector<Node>, NodeGreater> tempOpenSet = openSet;
while (!tempOpenSet.empty())
{
Node tempNeighbor = tempOpenSet.top();
tempOpenSet.pop();
if (tempNeighbor == neighbor)
{
neighborInOpenSet = true;
break;
}
}
// Update the neighbor's scores and parent if the tentative gScore is lower than
if (tentativeGScore < neighbor.gScore)
{
neighborIsBetter = true;
neighbor.gScore = tentativeGScore;
neighbor.fScore = tentativeGScore + heuristicCostEstimate(neighbor, end);
}
if (!neighborInOpenSet || neighborIsBetter)
{
// Update the neighbor's parent to the current node
neighbor.parent = &current;
// Add the neighbor to the open set if it is not already there
if (!neighborInOpenSet)
{
openSet.push(neighbor);
}
}
}
}
// If we reach here, it means that we have not found a path to the end node
return std::vector<Node>();
}
int main()
{
// Create the window and grid
sf::RenderWindow window(sf::VideoMode(500, 500), "A*");
Grid grid;
// Set some nodes as unwalkable
grid.setNode(3, 3, false);
grid.setNode(3, 4, false);
grid.setNode(3, 5, false);
grid.setNode(4, 3, false);
// Set the start and end nodes
Node start = { 1, 1 };
Node end = { 8, 8 };
// Run the A* algorithm and get the path
std::vector<Node> path = aStar(grid, start, end);
// Main loop
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed)
window.close();
}
window.clear();
// Draw the grid
for (int x = 0; x < WIDTH; x++)
{
for (int y = 0; y < HEIGHT; y++)
{
sf::RectangleShape rect(sf::Vector2f(50, 50));
rect.setPosition(x * 50, y * 50);
if (grid.isNodeWalkable(x, y))
{
rect.setFillColor(sf::Color::White);
}
else
{
rect.setFillColor(sf::Color::Black);
}
window.draw(rect);
}
}
// Draw the path
for (Node node : path)
{
sf::RectangleShape rect(sf::Vector2f(50, 50));
rect.setPosition(node.x * 50, node.y * 50);
rect.setFillColor(sf::Color::Green);
window.draw(rect);
}
// Draw the start and end nodes
sf::RectangleShape startRect(sf::Vector2f(50, 50));
startRect.setPosition(start.x * 50, start.y * 50);
startRect.setFillColor(sf::Color::Red);
window.draw(startRect);
sf::RectangleShape endRect(sf::Vector2f(50, 50));
endRect.setPosition(end.x * 50, end.y * 50);
endRect.setFillColor(sf::Color::Blue);
window.draw(endRect);
window.display();
}
return 0;
}
any idea why its doing this?
I've tried to debug the code and i cant seem to find the problem. Either the code doesn't run or it just shows the same thing

Implementing a* pathfinding in c++

Trying to implement A* pathfinding for a small game.
Running into some strange bugs.
Code:
class PathNode
{
private:
const PathNode* parent;
GameObject position;
int g;
int h;
int f;
public:
PathNode(GameObject _position, int _g, int _h, const PathNode* _parent = nullptr) {
parent = _parent;
position = _position;
g = _g; // distance between the start node and the current node
h = _h; // distance between the current node and the end node
f = g + h; // Total cost of the node (g + h)
}
PathNode(const PathNode& other) {
position = other.position;
g = other.g; // distance between the start node and the current node
h = other.h; // distance between the current node and the end node
f = other.f; // Total cost of the node (g + h)
parent = other.parent;
}
bool operator==(const PathNode& other) const {
return (this->position == other.position);
}
bool operator!=(const PathNode& other) const {
return !operator==(other);
}
// GETTERS:
int getg() const;
int geth() const;
int getf() const;
const PathNode* getParent() const;
const GameObject getPosition() const;
// SETTERS:
void setg(int newG);
void seth(int newH);
void setf(int newF);
void setParent(const PathNode* newParent);
void setPosition(GameObject newPos);
};
And The PathFinding code:
char Ghost::aStar(char board[BoardYSize][BoardXSize], GameObject end) {
int startEndDistance = pow((getX() - end.getX()), 2) + pow((getY() - end.getY()), 2);
PathNode startNode(getPosition(), 0, startEndDistance); // in the ctor of PathNode, the default values of Parent == nullptr.
PathNode endNode(end, startEndDistance, 0); // in the ctor of PathNode, the default values of Parent == nullptr.
// Initialize both Open and Closed Lists:
vector<PathNode> openList;
vector<PathNode> closedList;
openList.reserve(500);
closedList.reserve(500);
// Add the StartNode:
openList.push_back(startNode);
// Loop until you find the end:
while (openList.size() > 0) {
// Get the current Node.
PathNode currentNode(openList[0]); // Default copy ctor.
int index = 0;
for (int i = 0; i < openList.size(); i++) {
if (openList[i].getf() < currentNode.getf()) {
index = i;
currentNode = openList[i];
}
}
// Pop Current off openList, and add to closedList:
openList.erase(openList.begin() + index);
closedList.push_back(currentNode);
// If found the end, return:
if (currentNode == endNode) {
// return direction of the first move of the path:
const PathNode* nextNode = &currentNode;
const PathNode* firstMoveNode = nullptr;
while (*nextNode != startNode) {
firstMoveNode = nextNode;
nextNode = nextNode->getParent();
}
GameObject newPos = firstMoveNode->getPosition();
GameObject currentPos = startNode.getPosition();
if (newPos.getX() - currentPos.getX() == 0 && newPos.getY() - currentPos.getY() == 1)
return 'w';
else if (newPos.getX() - currentPos.getX() == 0 && newPos.getY() - currentPos.getY() == -1)
return 'x';
else if (newPos.getX() - currentPos.getX() == 1 && newPos.getY() - currentPos.getY() == 0)
return 'd';
else if (newPos.getX() - currentPos.getX() == -1 && newPos.getY() - currentPos.getY() == 0)
return 'a';
else
return 's';
}
// Generate Children:
vector <PathNode> children;
// temp is used to generate the children of the currentNode.
GameObject temp[4];
temp[0] = GameObject(0, 1); // Position above currentNode
temp[1] = GameObject(1, 0); // Position right of currentNode
temp[2] = GameObject(0, -1); // Position left of currentNode
temp[3] = GameObject(-1, 0); // Position below currentNode
for (int i = 0 ; i < 4; i++) {
// Get Node Position:
GameObject nodePosition(currentNode.getPosition() + temp[i]);
// Make Sure within Range of board & Not a Wall (walkable terrain):
if (!checkLegalMove(nodePosition, board))
continue;
PathNode newNode(nodePosition, 0, 0, &currentNode);
children.push_back(newNode);
}
// Loop through Children:
for (auto child : children) {
bool addChild = true;
// Child is on the closedList:
for (auto closedChild : closedList) {
if (child == closedChild) {
addChild = false;
continue;
}
}
if (!addChild) continue;
// Create the G H and F values:
child.setg(currentNode.getg() + 1);
child.seth(pow((child.getPosition().getX() - endNode.getPosition().getX()), 2) + pow((child.getPosition().getY() - endNode.getPosition().getY()), 2));
child.setf(child.getg() + child.geth());
// Child is already in the openList:
for (auto openNode : openList) {
if (child == openNode && child.getg() > openNode.getg()) {
addChild = false;
continue;
}
}
if (!addChild) continue;
openList.push_back(child);
}
}
return 's';
}
My goal is to use A* to find a route and simply return the direction of the first position to travel to.
The Problem I run into:
The problem is that when the condition of currentNode == endNode is met (reached the goal).
Every PathNode's parent is simply a pointing to itself (according to the debugger).
I don't understand why it's happening, I assume The current pointer gets destroyed at some point in time but i can't figure out why it's happening.
You're storing a pointer to a local variable, so every node will have that same local variable as the parent (and any access to it would be Undefined Behavior once it has been destroyed). This happens in the PathNode newNode(nodePosition, 0, 0, &currentNode); declaration.
You might want to change currentNode to be a pointer, rather than an object, and have it point to the parent object in the closedList vector. This would allow you to update what node it points at without having to make copies. You've reserved enough space for 500 entries so you won't have a problem with dangling pointers until you add the 501st element to the vector.

Compare value of std::shared_ptr in STL Heap and std::find. (Trying to implement A*)

TL;DR
I have a vector of std::shared_ptr's that I have to run std::push_heap, std::pop_heap, and std::find on. How do I compare the the things the pointers are pointing to rather than the pointers themselves?
Hi, I am trying to implement A* in my game and am having some trouble figuring out how to store all the nodes. Since I can't store a member of the same class in a class I have to use pointers. I can't use raw pointers because I realized that I would end up pointing to locations in a std::vector rather than the values stored in them. I now have decided that I will use std::shared_ptr's. However, I come across the issue in the TL;DR above. The Node object has an overload of the > operator, how can I end up using that. Here is my code; if anyone sees something that can be improved, please do say so.
Node.h
#ifndef NODE_H
#define NODE_H
#include <memory>
#include <SFML/Graphics.hpp>
class Node
{
public:
//Constructors
Node(std::shared_ptr<Node>, const sf::Vector2i&);
Node(const sf::Vector2i&);
//Getters
std::shared_ptr<Node> getParentNodePtr() const;
float getDistanceValue() const;
float getHeuristicValue() const;
float getTotalValue() const;
bool isStartNode() const;
sf::Vector2i getPosition() const;
//Setters
void setDistanceValue(float);
void setHeuristicValue(float);
void setTotalValue(float);
void setIsStartNode(bool);
//Comparison overloads
bool operator() (const Node&, const Node&) const;
bool operator== (const Node&) const;
bool operator< (const Node&) const;
private:
std::shared_ptr<Node> parentNodePtr_;
//Values
sf::Vector2i position_;
float distanceValue_ = 0.0f;
float heuristicValue_ = 0.0f;
float totalValue_ = 0.0f;
bool startNode_ = false;
};
#endif
Node.cpp
#include "Node.h"
Node::Node(std::shared_ptr<Node> parentNodePtr, const sf::Vector2i& position)
:parentNodePtr_(parentNodePtr)
{
position_ = parentNodePtr_->getPosition() + position * 32;
}
Node::Node(const sf::Vector2i& position)
:position_(position)
{}
//Getters
std::shared_ptr<Node> Node::getParentNodePtr() const { return parentNodePtr_; }
float Node::getDistanceValue() const { return distanceValue_; }
float Node::getHeuristicValue() const { return heuristicValue_; }
float Node::getTotalValue() const { return totalValue_; }
bool Node::isStartNode() const { return startNode_; }
sf::Vector2i Node::getPosition() const { return position_; }
//Setters
void Node::setDistanceValue(float distanceValue) { distanceValue_ = distanceValue; }
void Node::setHeuristicValue(float heuristicValue) { heuristicValue_ = heuristicValue; }
void Node::setTotalValue(float totalValue) { totalValue_ = totalValue; }
void Node::setIsStartNode(bool startNode) { startNode_ = startNode; }
//Comparison functor
bool Node::operator() (const Node& lhv, const Node& rhv) const {return lhv.totalValue_ < rhv.totalValue_; /*Backwards because I want a min-heap/min-priority-queue*/ }
bool Node::operator== (const Node& rhv) const {return position_ == rhv.position_; }
bool Node::operator< (const Node& rhv) const { return totalValue_ > rhv.totalValue_; }
Relevant Zombie.cpp
NOTE: sPNodes_ is a std::stack of std::shared_ptr's
Formatting wrong, use pastebin: http://pastebin.com/Z9NxTELV
void Zombie::findPath(std::vector< std::vector<Tile> >* pVTiles)
{
if(targetPosition_ != sf::Vector2i(0.0f, 0.0f))
{
//Initiates lists
std::vector<std::shared_ptr<Node>> openList;
std::vector<std::shared_ptr<Node>> closedList;
std::shared_ptr<Node> currentNode;
bool pathFound = false;
//Initiates the great journey
openList.push_back(std::make_shared<Node>(sf::Vector2i(positionGlobal_.x - fmod(positionGlobal_.x, 32.0f) + 16.0f, positionGlobal_.y - fmod(positionGlobal_.y, 32.0f) + 16.0f)));
openList.back()->setIsStartNode(true);
std::push_heap(openList.begin(), openList.end()); //NEED COMPARISON FOR THE POINTERS HERE
while(!pathFound)
{
//Gets the a pointer to the top item in the openList, then moves it to the closed list
std::pop_heap(openList.begin(), openList.end()); //NEED COMPARISON FOR THE POINTERS HERE
currentNode = openList.back();
closedList.push_back(currentNode);
openList.pop_back();
//Debug info
std::cout << std::endl << "TargetPosition: " << targetPosition_.x / 32 << ", " << targetPosition_.y / 32 << std::endl;
std::cout << "Position: " << currentNode->getPosition().x / 32 << ", " << currentNode->getPosition().y / 32 << std::endl;
std::cout << "Distance from start node: " << currentNode->getDistanceValue() / 32.0f << std::endl;
std::cout << "Heuristic Value: " << currentNode->getHeuristicValue() / 32.0f<< std::endl;
std::cout << "Total: " << currentNode->getTotalValue() / 32.0f << std::endl;
//For the eight neighboring tiles/nodes
for (int i = 0; i < 8; ++i)
{
int xPos;
int yPos;
//xPos
if(i == 0 || i == 4)
xPos = 0;
else if(i > 0 && i < 4)
xPos = 1;
else
xPos = -1;
//yPos
if(i == 2 || i == 6)
yPos = 0;
else if(i < 2 || i > 6)
yPos = 1;
else
yPos = -1;
sf::Vector2i nodePosition = currentNode->getPosition() + sf::Vector2i(xPos * 32, yPos * 32);
//Stop working if the node/tile is a wall or contains a tree
if(pVTiles->at(nodePosition.x / 32).at(nodePosition.y / 32).getType() == "unwalkable" || pVTiles->at(nodePosition.y / 32).at(nodePosition.x / 32).hasItem())
continue;
//Creates a node for the tile
auto node = std::make_shared<Node>(currentNode, sf::Vector2i(xPos, yPos));
//Checks to see if it is the target adds node to stack and breaks if so
if(node->getPosition() == targetPosition_)
{
std::cout << "found target!" << std::endl;
pathFound = true;
sPNodes_.push(node);
break;
}
//If it's not the target
if(!pathFound)
{
float parentDistanceValue = node->getParentNodePtr()->getDistanceValue();
//Distance is 1.4f x 32 if diagonal, 1 x 32 otherwise
if(xPos == yPos)
node->setDistanceValue(parentDistanceValue + 44.8f);
else
node->setDistanceValue(parentDistanceValue + 32.0f);
//Gets the distance to the target(Heuristic) and then gets the total(Distance + Heuristic)
node->setHeuristicValue(sqrt(pow(node->getPosition().x - targetPosition_.x, 2) + pow(node->getPosition().y - targetPosition_.y, 2)));
node->setTotalValue(node->getHeuristicValue() + node->getDistanceValue());
//Makes sure the node is not already in the open or closed list (NEED TO COMPARE VALUE OF WHAT THE POINTERS POINT TO HERE)
if(std::find(openList.begin(), openList.end(), node) == openList.end() && std::find(closedList.begin(), closedList.end(), node) == closedList.end())
{
openList.push_back(node);
std::push_heap(openList.begin(), openList.end()); //NEED COMPARISON HERE
}
}
}
}
//Keeps stacking parent nodes until the start is reached
while(!sPNodes_.top()->isStartNode())
{
std::cout << "stacking backward..." << std::endl;
std::cout << "Position: " << sPNodes_.top()->getPosition().x / 32 << ", " << sPNodes_.top()->getPosition().y / 32 << std::endl;
auto parent = sPNodes_.top()->getParentNodePtr();
sPNodes_.push(parent);
}
}
}
Thanks!
The second form of std::push_heap takes custom comparator as the third parameter, so you can give it the < overload:
struct SPtrNodeLess {
bool operator()(const std::shared_ptr<Node> &first, const std::shared_ptr<Node> &second) const {
return *first < *second;
}
}
std::push_heap(h.begin(), h.end(), SPtrNodeLess())
The std::find_if takes the unary predicate in similar fashion:
struct NodeSPtrEqual {
const Node &n;
NodeSPtrEqual(const Node &n) : n(n) {}
bool operator()(const std::shared_ptr<Node> &n2) {
return n == *n2;
}
}
std::find_if(h.begin(), h.end(), NodeSPtrEqual(node));
Or alternatively you can overload the operator== and use classic std::find:
bool operator==(const Node &n, const std::shared_ptr<Node> &n2) {
return n == *n2;
}
bool operator==(const std::shared_ptr<Node> &n, const Node &n2) {
return *n == n2;
}
std::find(h.begin(), h.end(), node);

c++ - sorting std::vector in pathfinding

I'm trying to implement a rough pathfinding example into a game i'm working on. I'm at a point where I need to sort a std::vector<Tile*> and i've tried to do so with the following but I get a bunch of errors I can't figure out. I've also tried to change the references in the sortF struct to pointers, but I get another error - Comparison between pointer and integer ('Tile *' and 'int').
The error in question is: No matching function for call to object of type 'Stage::sortF'
Wondering what exactly i'm doing wrong here.
(and if anyone had any comments on the pathfinding that would be good too)
in Stage.h public
struct sortF
{
bool operator()(const Tile& a, const Tile& b) const
{
return a.f > b.f;
}
};
in Stage.cpp
bool Stage::tilePath(Tile* start, Tile* end)
{
std::vector<Tile*> path;
std::vector<Tile*> open;
std::vector<Tile*> closed;
start->previousTile = start;
start->g = 0;
start->h = 0;
start->f = 0;
int i, j;
float g, h, f;
int sx, sy, ex, ey;
int cost;
Tile* current = start;
Tile* neighbor = NULL;
Tile* previous = NULL;
std::cout << neighbor << std::endl;
while(current != end) {
sx = fmaxf(0, current->x - 1);
sy = fmaxf(0, current->y - 1);
ex = fminf(17 - 1, current->x + 1);
ey = fminf(6 - 1, current->y + 1);
for(i = sx; i <= ex; i++) {
for(j = sy; j <= ey; j++) {
neighbor = tiles[((j - 1) * 17) + i - 1];
if(neighbor == current || !neighbor->walkable) continue;
previous = current;
if(false /* raytrace */) {
} else {
cost = (current->x != neighbor->x || current->y != neighbor->y) ? 1.4 : 1;
g = current->g + cost;
h = euclidian(neighbor, end);
f = g + h;
}
if(std::find(open.begin(), open.end(), neighbor) != open.end() ||
std::find(closed.begin(), closed.end(), neighbor) != closed.end()) {
if(neighbor->f > f) {
neighbor->f = f;
neighbor->g = g;
neighbor->h = h;
neighbor->previousTile = current;
}
} else {
neighbor->f = f;
neighbor->g = g;
neighbor->h = h;
neighbor->previousTile = current;
open.push_back(current);
}
}
}
closed.push_back(current);
if(open.size() == 0) {
return false;
}
std::sort(open.begin(), open.end(), sortF());
current = open[0];
std::remove(open.begin(), open.end(), 0);
}
return true;
}
Note: You didn't include your error messages, so the following answer is more or less based on view compiling:
sortF(), not sortF
error: expected primary-expression before ‘)’ token
std::sort(open.begin(), open.end(), sortF);
^
You need an instance of sortF, not the type struct sortF. Either use sortF() to create a temporary object, or use a function instead of a functor:
bool sortF(const Tile& a, const Tile& b)
{
return a.f > b.f;
}
Tile* vs const Tile&
You use std::sort on a std::vector<Tile*>, but your comparing function uses const Tile& as parameter. Either use std::vector<Tile> or correct the type in your function:
bool sortF(const Tile* a, const Tile* b)
{
return a->f > b->f;
}
Since the elements of the std::vector of type Tile*, the function that compares two items of the std::vector must take two Tile*s.
struct sortF
{
bool operator()(Tile* ap, Tile* bp) const
{
return a->f > b->f;
}
};
You definitely must change references to pointers in the comparer function. You didn't tell where exactly you got the error about comparison between pointer and integer, but I believe it happened on this line:
std::remove(open.begin(), open.end(), 0);
std::remove() takes a value type of the container, not an index. What you wanted to do is probably
open.pop_front()
That said, this operation as well as sorting take quite a bit of time, and as Javi V commented, using heap structure is preferable here.

use of undeclared identifier c++ xcode

I work in cocos2d-x help the problem. error in this line
string route = pathFind(xA, yA, xB, yB);
code HelloWorldScene.cpp
#include "HelloWorldScene.h"
#include "SimpleAudioEngine.h"
#include <iostream>
#include <iomanip>
#include <math.h>
#include <queue>
#include <string>
using namespace std;
using namespace cocos2d;
using namespace CocosDenshion;
const int n = 20; // horizontal size of the map
const int m = 20; // vertical size size of the map
static int MAPP[n][m];
static int putX[100];
static int putY[100];
static int closed_nodes_map[n][m]; // map of closed (tried-out) nodes
static int open_nodes_map[n][m]; // map of open (not-yet-tried) nodes
static int dir_map[n][m]; // map of directions
const int dir = 8; // number of possible directions to go at any position
static int dx[dir] = { 1, 1, 0, -1, -1, -1, 0, 1 };
static int dy[dir] = { 0, 1, 1, 1, 0, -1, -1, -1 };
class node
{
// current position
int xPos;
int yPos;
// total distance already travelled to reach the node
int level;
// priority=level+remaining distance estimate
int priority; // smaller: higher priority
public:
node(int xp, int yp, int d, int p)
{
xPos = xp; yPos = yp; level = d; priority = p;
}
int getxPos() const { return xPos; }
int getyPos() const { return yPos; }
int getLevel() const { return level; }
int getPriority() const { return priority; }
void updatePriority(const int & xDest, const int & yDest)
{
priority = level + estimate(xDest, yDest) * 10; //A*
}
// give better priority to going strait instead of diagonally
void nextLevel(const int & i) // i: direction
{
level += (dir == 8 ? (i % 2 == 0 ? 10 : 14) : 10);
}
// Estimation function for the remaining distance to the goal.
const int & estimate(const int & xDest, const int & yDest) const
{
static int xd, yd, d;
xd = xDest - xPos;
yd = yDest - yPos;
// Euclidian Distance
d = static_cast<int>(sqrt(xd*xd + yd*yd));
// Manhattan distance
//d=abs(xd)+abs(yd);
// Chebyshev distance
//d=max(abs(xd), abs(yd));
return(d);
}
};
CCScene* HelloWorld::scene()
{
// 'scene' is an autorelease object
CCScene *scene = CCScene::create();
// 'layer' is an autorelease object
HelloWorld *layer = HelloWorld::create();
// add layer as a child to scene
scene->addChild(layer);
// return the scene
return scene;
}
// on "init" you need to initialize your instance
bool HelloWorld::init()
{
//////////////////////////////
// 1. super init first
if ( !CCLayer::init() )
{
return false;
}
for (int y = 0; y < m; y++)
{
for (int x = 0; x < n; x++) MAPP[x][y] = 0;
}
int xA=0, yA=0, xB=7, yB=5;
// get the route
string route = pathFind(xA, yA, xB, yB);
// follow the route on the map and display it
if (route.length() > 0)
{
int j; char c;
int x = xA;
int y = yA;
MAPP[x][y] = 2;
for (int i = 0; i < route.length(); i++)
{
c = route.at(i);
j = atoi(&c);
x = x + dx[j];
y = y + dy[j];
MAPP[x][y] = 3;
}
MAPP[x][y] = 4;
}
int putS=0;
for (int y = 0; y < m; y++)
{
for (int x = 0; x < n; x++)
{
if (MAPP[x][y] == 3)
{
putY[putS] = y;
putX[putS] = x;
putS++;
}
}
}
return true;
}
bool operator < (const node & a, const node & b)
{
return a.getPriority() > b.getPriority();
}
// A-star algorithm.
// The route returned is a string of direction digits.
string pathFind(const int & xStart, const int & yStart,
const int & xFinish, const int & yFinish)
{
static priority_queue<node> pq[2]; // list of open (not-yet-tried) nodes
static int pqi; // pq index
static node* n0;
static node* m0;
static int i, j, x, y, xdx, ydy;
static char c;
pqi = 0;
// reset the node maps
for (y = 0; y < m; y++)
{
for (x = 0; x < n; x++)
{
closed_nodes_map[x][y] = 0;
open_nodes_map[x][y] = 0;
}
}
// create the start node and push into list of open nodes
n0 = new node(xStart, yStart, 0, 0);
n0->updatePriority(xFinish, yFinish);
pq[pqi].push(*n0);
open_nodes_map[x][y] = n0->getPriority(); // mark it on the open nodes map
// A* search
while (!pq[pqi].empty())
{
// get the current node w/ the highest priority
// from the list of open nodes
n0 = new node(pq[pqi].top().getxPos(), pq[pqi].top().getyPos(),
pq[pqi].top().getLevel(), pq[pqi].top().getPriority());
x = n0->getxPos(); y = n0->getyPos();
pq[pqi].pop(); // remove the node from the open list
open_nodes_map[x][y] = 0;
// mark it on the closed nodes map
closed_nodes_map[x][y] = 1;
// quit searching when the goal state is reached
//if((*n0).estimate(xFinish, yFinish) == 0)
if (x == xFinish && y == yFinish)
{
// generate the path from finish to start
// by following the directions
string path = "";
while (!(x == xStart && y == yStart))
{
j = dir_map[x][y];
c = '0' + (j + dir / 2)%dir;
path = c + path;
x += dx[j];
y += dy[j];
}
// garbage collection
delete n0;
// empty the leftover nodes
while (!pq[pqi].empty()) pq[pqi].pop();
return path;
}
// generate moves (child nodes) in all possible directions
for (i = 0; i < dir; i++)
{
xdx = x + dx[i]; ydy = y + dy[i];
if (!(xdx<0 || xdx>n - 1 || ydy<0 || ydy>m - 1 || MAPP[xdx][ydy] == 1
|| closed_nodes_map[xdx][ydy] == 1))
{
// generate a child node
m0 = new node(xdx, ydy, n0->getLevel(),
n0->getPriority());
m0->nextLevel(i);
m0->updatePriority(xFinish, yFinish);
// if it is not in the open list then add into that
if (open_nodes_map[xdx][ydy] == 0)
{
open_nodes_map[xdx][ydy] = m0->getPriority();
pq[pqi].push(*m0);
// mark its parent node direction
dir_map[xdx][ydy] = (i + dir / 2)%dir;
}
else if (open_nodes_map[xdx][ydy] > m0->getPriority())
{
// update the priority info
open_nodes_map[xdx][ydy] = m0->getPriority();
// update the parent direction info
dir_map[xdx][ydy] = (i + dir / 2)%dir;
// replace the node
// by emptying one pq to the other one
// except the node to be replaced will be ignored
// and the new node will be pushed in instead
while (!(pq[pqi].top().getxPos() == xdx &&
pq[pqi].top().getyPos() == ydy))
{
pq[1 - pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pq[pqi].pop(); // remove the wanted node
// empty the larger size pq to the smaller one
if (pq[pqi].size() > pq[1 - pqi].size()) pqi = 1 - pqi;
while (!pq[pqi].empty())
{
pq[1 - pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pqi = 1 - pqi;
pq[pqi].push(*m0); // add the better node instead
}
else delete m0; // garbage collection
}
}
delete n0; // garbage collection
}
return ""; // no route found
}
void HelloWorld::menuCloseCallback(CCObject* pSender)
{
CCDirector::sharedDirector()->end();
#if (CC_TARGET_PLATFORM == CC_PLATFORM_IOS)
exit(0);
#endif
}
code HelloWorldScene.h
#ifndef __HELLOWORLD_SCENE_H__
#define __HELLOWORLD_SCENE_H__
#include "cocos2d.h"
class HelloWorld : public cocos2d::CCLayer
{
public:
// Method 'init' in cocos2d-x returns bool, instead of 'id' in cocos2d-iphone (an object pointer)
virtual bool init();
// there's no 'id' in cpp, so we recommend to return the class instance pointer
static cocos2d::CCScene* scene();
// a selector callback
void menuCloseCallback(CCObject* pSender);
// preprocessor macro for "static create()" constructor ( node() deprecated )
CREATE_FUNC(HelloWorld);
};
#endif // __HELLOWORLD_SCENE_H__
pathFind is a function, you should prototype it. Add this somewhere the top of HelloWorldScene.cpp
string pathFind(const int & xStart, const int & yStart,
const int & xFinish, const int & yFinish);
Or if you are intending to call this function from another module then put the prototype in an appropriate header file.