Using the following A star visualization as a way to compare path accuracy, I found a large variation between my implementation and this one.
https://qiao.github.io/PathFinding.js/visual/
Path I'm comparing to:
(source: imgsafe.org)
My test paths:
(source: imgsafe.org)
There are times when it seems like the algorithm is checking too few nodes (i.e Test#6). Is this to be expected, or is it not correct?
Important variables in algorithm:
TileMap* m_tileMap;
vector<Tile*> m_openList;
vector<Tile*> m_path;
// Direct mapping of 2D tile map.
// Stores the list type for the same-indexed tile
vector<vector<int>> m_listMap;
Comparator for sorting open list:
struct CompareNodes
{
// sorts lowest F cost to end of vector
bool operator() (Tile* lhs, Tile* rhs)
{
return lhs->getFCost() > rhs->getFCost();
}
};
High level implementation:
vector<Tile*> PathGenerator::generatePath(Tile* startNode, Tile* endNode)
{
setUpListMap();
startNode->setGCost(0);
startNode->setHCost(calculateHCost(startNode, endNode)); // Manhattan (no diagonal). { abs(y2 - y1) + abs(x2 - x1) }
startNode->calculateFCost(); // calculates G+H internally
m_openList.push_back(startNode);
Vector2D startNodePos = startNode->getMapPos();
m_listMap[startNodePos.x][startNodePos.y] = LIST_TYPES::OPEN;
Tile* currentNode;
while (m_openList.empty() == false)
{
currNode = m_openList[m_openList.size() - 1];
m_openList.pop_back();
Vector2D currNodePos = currNode->getMapPos();
m_listMap[currNodePos.x][currNodePos.y] = LIST_TYPES::CLOSED;
if (currNode != endNode)
{
vector<Tile*> neighbours = findNeighbours(currNode);
removeUnnecessaryNodes(&neighbours); // remove walls and closed nodes
computeCosts(&neighbours, currNode, endNode);
addUniqueNodesToOpenList(&neighbours); // ignores duplicates and then sorts open list
}
else
{
m_path = getPath(currNode);
resetLists(); // erases all vectors
}
}
return m_path;
}
void PathGenerator::computeCosts(vector<Tile*>* nodes, Tile* current, Tile* end)
{
int newGCost = current->getGCost() + 1;
for (int i = 0; i < nodes->size(); i++)
{
Tile* node = nodes->at(i);
unsigned int nodeGCost = node->getGCost(); // G cost defaults to max int limit
if (newG < nodeGCost)
{
// set up node costs like above
node->setParentNode(current);
}
}
}
I've added the most important code. If the high level functions don't help to find the source of the issue, let me know and I'll add the implementation for them also.
Help appreciated.
The sorting part seems correct, but since it's a vector this should be very easy for you to verify.
Instead, try using a for-loop as a test-case to make sure you're really get the lowest f-cost node:
Tile* currnode = m_openlist[0];
for (int i = 0; i < m_openList.size() i++)
{
if (m_openList[i]->getFCost() < currnode->getFCost())
currnode = m_openList[i];
}
See if that fixes it. If it does, there's an issue in your sort, but i'm not sure what the issue would be.
Also, in your computeCosts function, you do:
for (int i = 0; i < nodes->size(); i++)
{
Tile* node = nodes->at(i);
//.. other code
}
Since you're using an std::vector, why not make use of its functionality, and use iterators or a range based loop:
// Iterators
for (auto it = nodes->begin(); it != nodes->end(); it++)
{
Tile* node = *it;
//.. other code
}
// Range based loop
for (auto node : *nodes)
{
//.. other code
}
Related
I have tasks and I want to calculate the most profitable order to arrange them.
Instead of checking every permutation and doing n*n! calculations, I want to build a tree of permutations, that is, the number of children at each level decreases by 1, and at each node the sub-permutation that has already been calculated will be saved and not recalculated.
For example, if I have 4 tasks, the tree will look like this:
My attached code is missing. I don't know how to build the tree and the give nodes the indexes as in the figure. I know how to deal with a binary tree, but not with a tree where the number of children is different at each lavel.
(The value of each task depends on its location.
I know how to do that, so I didn't include it in the question).
int n = 4;
struct node
{
int task_index = -1;
double value;
struct node **next;
};
void build_tree(node *current_node, int current_level = 0)
{
if (current_level < 1 || current_level >= n)
return;
// current_node->task_index = ? ;
current_node->next = new node *[n - current_level];
for (int i = 0; i < n - current_level; i++)
{
build_tree(current_node->next[i], current_level + 1);
}
}
void print_tree(node *current_node, int current_level = 0)
{
// print indexes
}
void delete_tree(node *current_node, int current_level = 0)
{
// delete nodes
}
int main()
{
struct node *root = new node;
build_tree(root);
print_tree(root);
delete_tree(root);
delete root;
return 0;
}
void build_tree(node *current_node, int current_level = 0)
{
if (current_level < 1 || current_level >= n)
return;
// current_node->task_index = ? ;
current_node->next = new node *[n - current_level];
for (int i = 0; i < n - current_level; i++)
{
build_tree(current_node->next[i], current_level + 1);
}
}
When called with the default parameter of current_level = 0, as you illustrate in your code below, this function exits on the first line without doing anything. You need to decide whether you are indexing starting from 0 or from 1.
Other than that, the general outline of the algorithm looks okay, although I did not explicitly check for correctness.
Now, more broadly: is this an exercise to see if you can write a tree structure, or are you trying to get the job done? In the latter case you probably want to use a prebuilt data structure like that in the boost graph library.
If it's an exercise in building a tree structure, is it specifically an exercise to see if you can write code dealing with raw pointers-to-pointers? If not, you should work with the correct C++ containers for the job. For instance you probably want to store the list of child nodes in a std::vector rather than have a pointer-to-pointer with the only way to tell how many child nodes exist being the depth of the node in the tree. (There may be some use case for such an extremely specialized structure if you are hyper-optimizing something for a very specific reason, but it doesn't look like that's what's going on here.)
From your explanation what you are trying to build is a data structure that reuses sub-trees for common permutations:
012 -> X
210 -> X
such that X is only instantiated once. This, of course, is recursive, seeing as
01 -> Y
10 -> Y
Y2 -> X
If you look at it closely, there are 2^n such subtrees, because any prefix can have any one of the n input tasks used or not. This means you can represent the subtree as an index into an array of size 2^n, with a total footprint O(n*2^n), which improves on the vastly larger >n! tree:
struct Edge {
std::size_t task;
std::size_t sub;
};
struct Node {
std::vector<Edge> successor; // size in [0,n]
};
std::vector<Node> permutations; // size exactly 2^n
This will have this structure:
permutations: 0 1 2 3 4 ...
|-^
|---^
|-------^
|---^
|-^
Where the node at, e.g., location 3 has both task 0 and 1 already used and "points" to all (n-2) subtrees.
Of course, building this is not entirely trivial, but it compressed the search space and allows you re-use results for specific sub-trees.
You can build the table like this:
permutations.resize(1<<n);
for (std::size_t i = 0; i < size(permutations); ++i) {
permutations[i].successor.reserve(n); // maybe better heuristic?
for (std::size_t j = 0; j < n; ++j) {
if (((1<<j) & i) == 0) {
permutations[i].successor.push_back({j,(1<<j)|i});
}
}
}
Here is a live demo for n=4.
The recursive way to generate permutations is if you have n items then all of the permutations of the items are each of the n items concatenated with the permutations of the n-1 remaining items. In code this is easier to do if you pass around the collection of items.
Below I do it with an std::vector<int>. Once using a vector it makes more sense to just follow the "rule of zero" pattern and let the nodes have vectors of children and then not need to dynamically allocate anything manually:
#include <vector>
#include <algorithm>
#include <iostream>
struct node
{
int task_index = -1;
double value;
std::vector<node> next;
};
std::vector<int> remove_item(int item, const std::vector<int>& items) {
std::vector<int> output(items.size() - 1);
std::copy_if(items.begin(), items.end(), output.begin(),
[item](auto v) {return v != item; }
);
return output;
}
void build_tree(node& current_node, const std::vector<int>& tasks)
{
auto n = static_cast<int>(tasks.size());
for (auto curr_task : tasks) {
node child{ curr_task, 0.0, {} };
if (n > 1) {
build_tree(child, remove_item(curr_task, tasks));
}
current_node.next.emplace_back(std::move(child));
}
}
void print_tree(const node& current_node)
{
std::cout << "( " << current_node.task_index << " ";
for (const auto& child : current_node.next) {
print_tree(child);
}
std::cout << " )";
}
int main()
{
node root{ -1, 0.0, {} };
build_tree(root, { 1, 2, 3 });
print_tree(root);
return 0;
}
I'm working on a C++ quadtree implementation for collision detection. I tried to adapt this Java implementation to C++ by using pointers; namely, storing the child nodes of each node as Node pointers (code at the end). However, since my understanding of pointers is still rather lacking, I am struggling to understand why my Quadtree class produces the following two issues:
When splitting a Node in 4, the debugger tells me that all my childNodes entries are identical to the first one, i.e., same address and bounds.
Even if 1. is ignored, I get an Access violation reading location 0xFFFFFFFFFFFFFFFF, which I found out is a consequence of the childNode pointees being deleted after the first split, resulting in undefined behaviour.
My question is: what improvements should I make to my Quadtree.hpp so that each Node can contain 4 distinct child node pointers and have those references last until the quadtree is cleared?
What I have tried so far:
Modifying getChildNode according to this guide and using temporary variables in split() to avoid all 4 entries of childNodes to point to the same Node:
void split() {
for (int i = 0; i < 4; i++) {
Node temp = getChildNode(level, bounds, i + 1);
childNodes[i] = &(temp);
}
}
but this does not solve the problem.
This one is particularly confusing. My initial idea was to just store childNodes as Nodes themselves, but turns out that cannot be done while we're defining the Node class itself. Hence, it looks like the only way to store Nodes is by first creating them and then storing pointers to them as I tried to do in split(), yet it seems that those will not "last" until we've inserted all the objects since the pointees get deleted (run out of scope) and we get the aforementioned undefined behaviour. I also thought of using smart pointers, but that seems to only overcomplicate things.
The code:
Quadtree.hpp
#pragma once
#include <vector>
#include <algorithm>
#include "Box.hpp"
namespace quadtree {
class Node {
public:
Node(int p_level, quadtree::Box<float> p_bounds)
:level(p_level), bounds(p_bounds)
{
parentWorld = NULL;
}
// NOTE: mandatory upon Quadtree initialization
void setParentWorld(World* p_world_ptr) {
parentWorld = p_world_ptr;
}
/*
Clears the quadtree
*/
void clear() {
objects.clear();
for (int i = 0; i < 4; i++) {
if (childNodes[i] != nullptr) {
(*(childNodes[i])).clear();
childNodes[i] = nullptr;
}
}
}
/*
Splits the node into 4 subnodes
*/
void split() {
for (int i = 0; i < 4; i++) {
childNodes[i] = &getChildNode(level, bounds, i + 1);;
}
}
/*
Determine which node the object belongs to. -1 means
object cannot completely fit within a child node and is part
of the parent node
*/
int getIndex(Entity* p_ptr_entity) {
quadtree::Box<float> nodeBounds;
quadtree::Box<float> entityHitbox;
for (int i = 0; i < 4; i++) {
nodeBounds = childNodes[i]->bounds;
ComponentHandle<Hitbox> hitbox;
parentWorld->unpack(*p_ptr_entity, hitbox);
entityHitbox = hitbox->box;
if (nodeBounds.contains(entityHitbox)) {
return i;
}
}
return -1; // if no childNode completely contains Entity Hitbox
}
/*
Insert the object into the quadtree. If the node
exceeds the capacity, it will split and add all
objects to their corresponding nodes.
*/
void insertObject(Entity* p_ptr_entity) {
if (childNodes[0] != nullptr) {
int index = getIndex(p_ptr_entity);
if (index != -1) {
(*childNodes[index]).insertObject(p_ptr_entity); // insert in child node
return;
}
}
objects.push_back(p_ptr_entity); // add to parent node
if (objects.size() > MAX_OBJECTS && level < MAX_DEPTH) {
if (childNodes[0] == nullptr) {
split();
}
int i = 0;
while (i < objects.size()) {
int index = getIndex(objects[i]);
if (index != -1)
{
Entity* temp_entity = objects[i];
{
// remove i-th element of the vector
using std::swap;
swap(objects[i], objects.back());
objects.pop_back();
}
(*childNodes[index]).insertObject(temp_entity);
}
else
{
i++;
}
}
}
}
/*
Return all objects that could collide with the given object
*/
std::vector<Entity*> retrieve(Entity* p_ptr_entity, std::vector<Entity*> returnObjects) {
int index = getIndex(p_ptr_entity);
if (index != -1 && childNodes[0] == nullptr) {
(*childNodes[index]).retrieve(p_ptr_entity, returnObjects);
}
returnObjects.insert(returnObjects.end(), objects.begin(), objects.end());
return returnObjects;
}
World* getParentWorld() {
return parentWorld;
}
private:
int MAX_OBJECTS = 10;
int MAX_DEPTH = 5;
World* parentWorld; // used to unpack entities
int level; // depth of the node
quadtree::Box<float> bounds; // boundary of nodes in the game's map
std::vector<Entity*> objects; // list of objects contained in the node: pointers to Entitites in the game
Node* childNodes[4];
quadtree::Box<float> getQuadrantBounds(quadtree::Box<float> p_parentBounds, int p_quadrant_id) {
quadtree::Box<float> quadrantBounds;
quadrantBounds.width = p_parentBounds.width / 2;
quadrantBounds.height = p_parentBounds.height / 2;
switch (p_quadrant_id) {
case 1: // NE
quadrantBounds.top = p_parentBounds.top;
quadrantBounds.left = p_parentBounds.width / 2;
break;
case 2: // NW
quadrantBounds.top = p_parentBounds.top;
quadrantBounds.left = p_parentBounds.left;
break;
case 3: // SW
quadrantBounds.top = p_parentBounds.height / 2;
quadrantBounds.left = p_parentBounds.left;
break;
case 4: // SE
quadrantBounds.top = p_parentBounds.height / 2;
quadrantBounds.left = p_parentBounds.width / 2;
break;
}
return quadrantBounds;
}
Node& getChildNode(int parentLevel, Box<float> parentBounds, int quadrant) {
static Node temp = Node(parentLevel + 1, getQuadrantBounds(parentBounds, quadrant));
return temp;
}
};
}
Where Box is just a helper class that contains some helper methods for rectangular shapes and collision detection. Any help would be greatly appreciated!
i would like some help for my AStar algorithm search, which takes from my point of view far to long. Even though my map is with 500 * 400 coordinates(objectively is my tile graph a bit smaller since I don't took the walls into the TileGraph.) large, I would like to expect the result after a few seconds. The world looks like this, despite the task not being mine
I want to search from marked coordinates "Start"(120|180) to "Ziel"(320|220), which currently takes 48 minutes. And sorry for all, who don't speak german, but the text at the picture isn't important.
At first I want to show you, what I've programmed for A*. In General adapted myself to the pseudocode at https://en.wikipedia.org/wiki/A*_search_algorithm .
bool AStarPath::Processing(Node* Start, Node* End)
m_Start = Start;
m_End = End;
for (Node* n : m_SearchRoom->GetAllNodes())
{
DistanceToStart[n] = std::numeric_limits<float>::infinity();
CameFrom[n] = nullptr;
}
DistanceToStart[m_Start] = 0;
NotEvaluatedNodes.AddElement(0, m_Start);
while (NotEvaluatedNodes.IsEmpty() == false)
{
Node* currentNode = NotEvaluatedNodes.GetElement();
NotEvaluatedNodes.DeleteElement();
if (currentNode == m_End)
{
ReconstructPath();
return true;
}
EvaluatedNodes.insert(currentNode);
ExamineNeighbours(currentNode);
}
return false;
//End Processing
void AStarPath::ExamineNeighbours(Node* current)
for (Node* neighbour : m_SearchRoom->GetNeighbours(current))
{
if (std::find(EvaluatedNodes.begin(), EvaluatedNodes.end(), neighbour) != EvaluatedNodes.end())
{
continue;
}
bool InOpenSet = NotEvaluatedNodes.ContainsElement(neighbour);
float tentative_g_score = DistanceToStart[current] + DistanceBetween(current, neighbour);
if (InOpenSet == true && tentative_g_score >= DistanceToStart[neighbour])
{
continue;
}
CameFrom[neighbour] = current;
DistanceToStart[neighbour] = tentative_g_score;
float Valuation = tentative_g_score + DistanceBetween(neighbour, m_End);
if (InOpenSet == false)
{
NotEvaluatedNodes.AddElement(Valuation, neighbour);
}
else
{
NotEvaluatedNodes.UpdatePriority(neighbour, Valuation);
}
}
//END ExamineNeighbours
double AStarPath::DistanceBetween(Node* a, Node* b)
return sqrt(pow(m_SearchRoom->GetNodeX(a) - m_SearchRoom->GetNodeX(b), 2)
+ pow(m_SearchRoom->GetNodeY(a) - m_SearchRoom->GetNodeY(b), 2));
//END DistanceBetween
I'm sorry for the bad formatting, but I don't really know how to work with the code blocks here.
class AStarPath
private:
std::unordered_set<Node*> EvaluatedNodes;
Binary_Heap NotEvaluatedNodes;
std::unordered_map<Node*, float> DistanceToStart;
std::unordered_map<Node*, Node*> CameFrom;
std::vector<Node*> m_path;
TileGraph* m_SearchRoom;
//END Class AStarPath
Anyway, i have thought myself over my problem already and changed some things.
Firstly, I implemented a binary heap instead of the std::priority_queue. I used a page at policyalmanac for it, but I'm not permitted to add another link, so I can't really give you the address. It improved the performance, but it still takes quite long as I told at the beginning.
Secondly, I used unordered containers (if there are two options), so that the containers don't have to be sorted after the changes. For my EvaluatedNodes I took the std::unordered_set, since from my knowledge it's fastest for std::find, which I use for containment checks.
The usage of std::unordered_map is caused by the need of having seperate keys and values.
Thirdly, I thought about splitting my map into nodes, which represent multiple coordinates(instead of now where one node represents one coordinate) , but I'm not really sure how to choose them. I thought about setting points at position, that the algorithm decises based on the length and width of the map and add neighbouring coordinates, if there aren't a specific distance or more away from the base node/coordinate and I can reach them only from previous added coordinates. To Check whether there is a ability to walk, I would have used the regular A*, with only the coordinates(converted to A* nodes), which are in these big nodes. Despite this I'm unsure which coordinates I should take for the start and end of this pathfinding. This would probably reduce the number of nodes/coordinates, which are checked, if I only use the coordinates/nodes, which were part of the big nodes.(So that only nodes are used, which where part of the bigger nodes at an upper level)
I'm sorry for my english, but hope that all will be understandable. I'm looking forward to your answers and learning new techniques and ways to handle problems and as well learn about all the hundreds of stupids mistakes I produced.
If any important aspect is unclear or if I should add more code/information, feel free to ask.
EDIT: Binary_Heap
class Binary_Heap
private:
std::vector<int> Index;
std::vector<int> m_Valuation;
std::vector<Node*> elements;
int NodesChecked;
int m_NumberOfHeapItems;
void TryToMoveElementUp(int i_pos);
void TryToMoveElementDown(int i_pos);
public:
Binary_Heap(int i_numberOfElements);
void AddElement(int Valuation, Node* element);
void DeleteElement();
Node* GetElement();
bool IsEmpty();
bool ContainsElement(Node* i_node);
void UpdatePriority(Node* i_node, float newValuation);
Binary_Heap::Binary_Heap(int i_numberOfElements)
Index.resize(i_numberOfElements);
elements.resize(i_numberOfElements);
m_Valuation.resize(i_numberOfElements);
NodesChecked = 0;
m_NumberOfHeapItems = 0;
void Binary_Heap::AddElement(int valuation, Node* element)
++NodesChecked;
++m_NumberOfHeapItems;
Index[m_NumberOfHeapItems] = NodesChecked;
m_Valuation[NodesChecked] = valuation;
elements[NodesChecked] = element;
TryToMoveElementUp(m_NumberOfHeapItems);
void Binary_Heap::DeleteElement()
elements[Index[1]] = nullptr;
m_Valuation[Index[1]] = 0;
Index[1] = Index[m_NumberOfHeapItems];
--m_NumberOfHeapItems;
TryToMoveElementDown(1);
bool Binary_Heap::IsEmpty()
return m_NumberOfHeapItems == 0;
Node* Binary_Heap::GetElement()
return elements[Index[1]];
bool Binary_Heap::ContainsElement(Node* i_element)
return std::find(elements.begin(), elements.end(), i_element) != elements.end();
void Binary_Heap::UpdatePriority(Node* i_node, float newValuation)
if (ContainsElement(i_node) == false)
{
AddElement(newValuation, i_node);
}
else
{
int treePosition;
for (int i = 1; i < Index.size(); i++)
{
if (elements[Index[i]] == i_node)
{
treePosition = i;
break;
}
}
//Won't influence each other, since only one of them will change the position
TryToMoveElementUp(treePosition);
TryToMoveElementDown(treePosition);
}
void Binary_Heap::TryToMoveElementDown(int i_pos)
int nextPosition = i_pos;
while (true)
{
int currentPosition = nextPosition;
if (2 * currentPosition + 1 <= m_NumberOfHeapItems)
{
if (m_Valuation[Index[currentPosition]] >= m_Valuation[Index[2 * currentPosition]])
{
nextPosition = 2 * currentPosition;
}
if (m_Valuation[Index[currentPosition]] >= m_Valuation[Index[2 * currentPosition + 1]])
{
nextPosition = 2 * currentPosition + 1;
}
}
else
{
if (2 * currentPosition <= m_NumberOfHeapItems)
{
if (m_Valuation[Index[currentPosition]] >= m_Valuation[Index[2 * currentPosition]])
{
nextPosition = 2 * currentPosition;
}
}
}
if (currentPosition != nextPosition)
{
int tmp = Index[currentPosition];
Index[currentPosition] = Index[nextPosition];
Index[nextPosition] = tmp;
}
else
{
break;
}
}
void Binary_Heap::TryToMoveElementUp(int i_pos)
int treePosition = i_pos;
while (treePosition != 1)
{
if (m_Valuation[Index[treePosition]] <= m_Valuation[Index[treePosition / 2]])
{
int tmp = Index[treePosition / 2];
Index[treePosition / 2] = Index[treePosition];
Index[treePosition] = tmp;
treePosition = treePosition / 2;
}
else
{
break;
}
}
This line introduces major inefficiency, as it needs to iterate over all the nodes in the queue, in each iteration.
bool InOpenSet = NotEvaluatedNodes.ContainsElement(neighbour);
Try using a more efficient data structure, e.g. the unordered_set you use for EvaluatedNodes. Whenever you push or pop a node from the heap, modify the set accordingly to always contain only the nodes in the heap.
My feeble attempt at an A* Algorithm is generating unpredictable errors.
My FindAdjacent() function is clearly a mess, and it actually doesn't work when I step through it. This is my first time trying a path finding algorithm, so this is all new to me.
When the application actually manages to find the goal nodes and path (or so I think), it can never set the path (called from within main by pressing enter). I do not know why it is unable to do this from looking at the SetPath() function.
Any help would be hugely appreciated, here's my code:
NODE CLASS
enum
{
NODE_TYPE_NONE = 0,
NODE_TYPE_NORMAL,
NODE_TYPE_SOLID,
NODE_TYPE_PATH,
NODE_TYPE_GOAL
};
class Node
{
public:
Node () : mTypeID(0), mNodeCost(0), mX(0), mY(0), mParent(0){};
public:
int mTypeID;
int mNodeCost;
int mX;
int mY;
Node* mParent;
};
PATH FINDING
/**
* finds the path between star and goal
*/
void AStarImpl::FindPath()
{
cout << "Finding Path." << endl;
GetGoals();
while (!mGoalFound)
GetF();
}
/**
* modifies linked list to find adjacent, walkable nodes
*/
void AStarImpl::FindAdjacent(Node* pNode)
{
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
if (i != 0 && j != 0)
if (Map::GetInstance()->mMap[pNode->mX+i][pNode->mY+j].mTypeID != NODE_TYPE_SOLID)
{
for (vector<Node*>::iterator iter = mClosedList.begin(); iter != mClosedList.end(); iter++)
{
if ((*iter)->mX != Map::GetInstance()->mMap[pNode->mX + i][pNode->mY + j].mX && (*iter)->mY != Map::GetInstance()->mMap[pNode->mX + i][pNode->mY + j].mY)
{
Map::GetInstance()->mMap[pNode->mX+i][pNode->mY+j].mParent = pNode;
mOpenList.push_back(&Map::GetInstance()->mMap[pNode->mX+i][pNode->mY+j]);
}
}
}
}
mClosedList.push_back(pNode);
}
/**
* colour the found path
*/
void AStarImpl::SetPath()
{
vector<Node*>::iterator tParent;
mGoalNode->mTypeID = NODE_TYPE_PATH;
Node *tNode = mGoalNode;
while (tNode->mParent)
{
tNode->mTypeID = NODE_TYPE_PATH;
tNode = tNode->mParent;
}
}
/**
* returns a random node
*/
Node* AStarImpl::GetRandomNode()
{
int tX = IO::GetInstance()->GetRand(0, MAP_WIDTH - 1);
int tY = IO::GetInstance()->GetRand(0, MAP_HEIGHT - 1);
Node* tNode = &Map::GetInstance()->mMap[tX][tY];
return tNode;
}
/**
* gets the starting and goal nodes, then checks te starting nodes adjacent nodes
*/
void AStarImpl::GetGoals()
{
// get the two nodes
mStartNode = GetRandomNode();
mGoalNode = GetRandomNode();
mStartNode->mTypeID = NODE_TYPE_GOAL;
mGoalNode->mTypeID = NODE_TYPE_GOAL;
// insert start node into the open list
mOpenList.push_back(mStartNode);
// find the starting nodes adjacent ndoes
FindAdjacent(*mOpenList.begin());
// remove starting node from open list
mOpenList.erase(mOpenList.begin());
}
/**
* finds the best f
*/
void AStarImpl::GetF()
{
int tF = 0;
int tBestF = 1000;
vector<Node*>::const_iterator tIter;
vector<Node*>::const_iterator tBestNode;
for (tIter = mOpenList.begin(); tIter != mOpenList.end(); ++tIter)
{
tF = GetH(*tIter);
tF += (*tIter)->mNodeCost;
if (tF < tBestF)
{
tBestF = tF;
tBestNode = tIter;
}
}
if ((*tBestNode) != mGoalNode)
{
Node tNode = **tBestNode;
mOpenList.erase(tBestNode);
FindAdjacent(&tNode);
}
else
{
mClosedList.push_back(mGoalNode);
mGoalFound = true;
}
}
/**
* returns the heuristic from the given node to goal
*/
int AStarImpl::GetH(Node *pNode)
{
int H = (int) fabs((float)pNode->mX - mGoalNode->mX);
H += (int) fabs((float)pNode->mY - mGoalNode->mY);
H *= 10;
return H;
}
A few suggestions:
ADJACENCY TEST
The test in FindAdjacent will only find diagonal neighbours at the moment
if (i != 0 && j != 0)
If you also want to find left/right/up/down neighbours you would want to use
if (i != 0 || j != 0)
ADJACENCY LOOP
I think your code looks suspicious in FindAdjacent at the line
for (vector<Node*>::iterator iter = mClosedList.begin(); iter != mClosedList.end(); iter++)
I don't really understand the intention here. I would have expected mClosedList to start empty, so this loop will never execute, and so nothing will ever get added to mOpenList.
My expectation at this part of the algorithm would be for you to test for each neighbour whether it should be added to the open list.
OPENLIST CHECK
If you look at the A* algorithm on wikipedia you will see that you are also missing the section starting
if neighbor not in openset or tentative_g_score < g_score[neighbor]
in which you should also check in FindAdjacent whether your new node is already in the OpenSet before adding it, and if it is then only add it if the score is better.
Below is an implementation of Dijkstra's algorithm I wrote from the pseudocode in the Wikipedia article. For a graph with about 40 000 nodes and 80 000 edges, it takes 3 or 4 minutes to run. Is that anything like the right order of magnitude? If not, what's wrong with my implementation?
struct DijkstraVertex {
int index;
vector<int> adj;
vector<double> weights;
double dist;
int prev;
bool opt;
DijkstraVertex(int vertexIndex, vector<int> adjacentVertices, vector<double> edgeWeights) {
index = vertexIndex;
adj = adjacentVertices;
weights = edgeWeights;
dist = numeric_limits<double>::infinity();
prev = -1; // "undefined" node
opt = false; // unoptimized node
}
};
void dijsktra(vector<DijkstraVertex*> graph, int source, vector<double> &dist, vector<int> &prev) {
vector<DijkstraVertex*> Q(G); // set of unoptimized nodes
G[source]->dist = 0;
while (!Q.empty()) {
sort(Q.begin(), Q.end(), dijkstraDistComp); // sort nodes in Q by dist from source
DijkstraVertex* u = Q.front(); // u = node in Q with lowest dist
u->opt = true;
Q.erase(Q.begin());
if (u->dist == numeric_limits<double>::infinity()) {
break; // all remaining vertices are inaccessible from the source
}
for (int i = 0; i < (signed)u->adj.size(); i++) { // for each neighbour of u not in Q
DijkstraVertex* v = G[u->adj[i]];
if (!v->opt) {
double alt = u->dist + u->weights[i];
if (alt < v->dist) {
v->dist = alt;
v->prev = u->index;
}
}
}
}
for (int i = 0; i < (signed)G.size(); i++) {
assert(G[i] != NULL);
dist.push_back(G[i]->dist); // transfer data to dist for output
prev.push_back(G[i]->prev); // transfer data to prev for output
}
}
There are several things you can improve on this:
implementing the priority queue with sort and erase adds a factor of |E| to the runtime - use the heap functions of the STL to get a log(N) insertion and removal into the queue.
do not put all the nodes in the queue at once but only those where you have discovered a path (which may or may not be the optimal, as you can find an indirect path through nodes in the queue).
creating objects for every node creates unneccessary memory fragmentation. If you care about squeezing out the last 5-10%, you could think about a solution to represent the incidence matrix and other information directly as arrays.
Use priority_queue.
My Dijkstra implementation:
struct edge
{
int v,w;
edge(int _w,int _v):w(_w),v(_v){}
};
vector<vector<edge> > g;
enum color {white,gray,black};
vector<int> dijkstra(int s)
{
int n=g.size();
vector<int> d(n,-1);
vector<color> c(n,white);
d[s]=0;
c[s]=gray;
priority_queue<pair<int,int>,vector<pair<int,int> >,greater<pair<int,int> > > q; // declare priority_queue
q.push(make_pair(d[s],s)); //push starting vertex
while(!q.empty())
{
int u=q.top().second;q.pop(); //pop vertex from queue
if(c[u]==black)continue;
c[u]=black;
for(int i=0;i<g[u].size();i++)
{
int v=g[u][i].v,w=g[u][i].w;
if(c[v]==white) //new vertex found
{
d[v]=d[u]+w;
c[v]=gray;
q.push(make_pair(d[v],v)); //add vertex to queue
}
else if(c[v]==gray && d[v]>d[u]+w) //shorter path to gray vertex found
{
d[v]=d[u]+w;
q.push(make_pair(d[v],v)); //push this vertex to queue
}
}
}
return d;
}