C++

Forum

 
Trouble with A* pathfinding

Gleade (1)
Hello, I am attempting to create my own version of A*. The trouble I'm having is mainly that my program isn't finding the closed nodes properly. And all in all I'm not sure if I'm even doing it the right way. Is there anyone that can help point me in the right direction?

Here is my code: 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
 
PathFinder::PathFinder(unsigned int width, unsigned int height, std::vector<int> newMap)
{
    this->width = width;
    this->height = height;

    for (unsigned int i = 0; i < width; ++i)
    {
        for (unsigned int j = 0; j < height; ++j)
        {
            // Create our map and assign the nodes their positions
            map.push_back(new Node(i * 32, j * 32));

            // If there is a collision node here, add it to the closed list
            if(newMap[i + j] > 0)
            {

                closedList.push_back(map[i + j]);
            }
        }
    }
}


1
2
3
4
5
6
7
8
9
10
11
12
13
 
bool PathFinder::checkClosed(Node *node)
{
    for (std::vector<Node*>::iterator it = closedList.begin() ; it != closedList.end(); ++it)
    {
        if(*it == node)
        {
            return true;
        }
    }

    return false;

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
 
bool PathFinder::checkOpen(Node *node)
{
    for (std::vector<Node*>::iterator it = openList.begin() ; it != openList.end(); ++it)
    {
        if(*it == node)
        {
            //std::cout << "a" << std::endl;
            return true;
        }
    }

    return false;

}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
 
std::vector<int> PathFinder::findPath(Node &start, Node &finish)
{
    // Initalize variables
    for(std::vector<Node*>::iterator it = map.begin() ; it != map.end(); ++it)
    {
        // Reset our list
        (*it)->Gvalue = 10;
        (*it)->Hvalue = 0;
        (*it)->searchX = 0;
        (*it)->searchY = 0;
         (*it)->shape.setFillColor(sf::Color::Black);
    }
    foundFinish = false;
    std::vector<int> result;
    closedList.clear();
    openList.clear();


    // Set our parent node to the start of the search area
    nodeParent = &start;

    // Add our parent node to the open list
    // to search for surrounding nodes
    openList.push_back(nodeParent);
    // For debugging
    openList[0]->shape.setFillColor(sf::Color::Blue);

    while(!foundFinish)
    {
        // Search for surrounding nodes not in the closed list
        for(unsigned int i=0; i < openList.size(); i++)
        {

            // IF we've found the finish
            if(openList[i]->x == finish.x && openList[i]->y == finish.y)
            {
                foundFinish = true;
            }


            // If we've found the finish
            if(openList[i]->x == finish.x && openList[i]->y == finish.y)
            {
                // We've found the node, so finish here
                foundFinish = true;
                openList[i]->shape.setFillColor(sf::Color::Green);
                // Exit out of the loop
                break;
            }

            for(std::vector<Node*>::iterator it = map.begin() ; it != map.end(); ++it)
            {
                // Find node to the left of us
                if(((*it)->x == openList[i]->x-32 && (*it)->y == openList[i]->y))
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }

                // Find node to the left-diagonal of us
                if(((*it)->x == openList[i]->x-32 && (*it)->y == openList[i]->y-32))
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue + 4;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }

                // Find node to the right of us
                if((*it)->x == openList[i]->x+32 && (*it)->y == openList[i]->y)
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }

                // Find node to the right- diagonal of us
                if((*it)->x == openList[i]->x+32 && (*it)->y == openList[i]->y-32)
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue + 4;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }

                // Find node below us
                if((*it)->x == openList[i]->x && (*it)->y == openList[i]->y + 32)
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }

                // Find node right-below diagonal us
                if((*it)->x == openList[i]->x + 32 && (*it)->y == openList[i]->y + 32)
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue + 4;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }


                // Find node above us
                if((*it)->x == openList[i]->x && (*it)->y == openList[i]->y-32)
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }

                    // Find node above-left diagonal us
                if((*it)->x == openList[i]->x-32 && (*it)->y == openList[i]->y-32)
                {
                    if(!checkClosed(*it) && !checkOpen(*it))
                    {
                        // Add to the GValue
                        (*it)->Gvalue += openList[i]->Gvalue + 4;
                        // Set our Hvalue to the shortest path
                        (*it)->setShortestPath(finish);
                        // Calculate our Fvalue F = G + H
                        (*it)->calculateHValue();
                        openList.push_back(*it);
                    }
                }
            }

            // Remove the current node from the open list to the closed list

            closedList.push_back(openList[i]);
            //openList.erase(openList.begin());
            std::sort(openList.begin(), openList.end(), sortByFValue);
            i = 0;
        }

        // After for loop
    }
}
Topic archived. No new replies allowed.