|
1
|
|
|
<?php |
|
2
|
|
|
/* |
|
3
|
|
|
MIT License |
|
4
|
|
|
Copyright (c) 2014 Peter Petermann |
|
5
|
|
|
|
|
6
|
|
|
Permission is hereby granted, free of charge, to any person |
|
7
|
|
|
obtaining a copy of this software and associated documentation |
|
8
|
|
|
files (the "Software"), to deal in the Software without |
|
9
|
|
|
restriction, including without limitation the rights to use, |
|
10
|
|
|
copy, modify, merge, publish, distribute, sublicense, and/or sell |
|
11
|
|
|
copies of the Software, and to permit persons to whom the |
|
12
|
|
|
Software is furnished to do so, subject to the following |
|
13
|
|
|
conditions: |
|
14
|
|
|
|
|
15
|
|
|
The above copyright notice and this permission notice shall be |
|
16
|
|
|
included in all copies or substantial portions of the Software. |
|
17
|
|
|
|
|
18
|
|
|
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
|
19
|
|
|
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES |
|
20
|
|
|
OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
|
21
|
|
|
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT |
|
22
|
|
|
HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, |
|
23
|
|
|
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING |
|
24
|
|
|
FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR |
|
25
|
|
|
OTHER DEALINGS IN THE SOFTWARE. |
|
26
|
|
|
*/ |
|
27
|
|
|
namespace PathFinder; |
|
28
|
|
|
|
|
29
|
|
|
use Psr\Log\LoggerAwareInterface; |
|
30
|
|
|
use Psr\Log\LoggerAwareTrait; |
|
31
|
|
|
|
|
32
|
|
|
/** |
|
33
|
|
|
* Class AStar |
|
34
|
|
|
* |
|
35
|
|
|
* @package PathFinder |
|
36
|
|
|
*/ |
|
37
|
|
|
class AStar implements LoggerAwareInterface |
|
38
|
|
|
{ |
|
39
|
|
|
/** |
|
40
|
|
|
* we use the trait here, which will provide a setLogger method, and a logger member |
|
41
|
|
|
*/ |
|
42
|
|
|
use LoggerAwareTrait; |
|
43
|
|
|
|
|
44
|
|
|
/** |
|
45
|
|
|
* @var Node[] |
|
46
|
|
|
*/ |
|
47
|
|
|
protected $open = []; |
|
48
|
|
|
|
|
49
|
|
|
/** |
|
50
|
|
|
* @var string[] |
|
51
|
|
|
*/ |
|
52
|
|
|
protected $closed = []; |
|
53
|
|
|
|
|
54
|
|
|
/** |
|
55
|
|
|
* find a path from Node $start to Node $end |
|
56
|
|
|
* @param Node $start |
|
57
|
|
|
* @param Node $end |
|
58
|
|
|
* @return array|Node[] |
|
59
|
|
|
*/ |
|
60
|
2 |
|
public function findPath(Node $start, Node $end) |
|
61
|
|
|
{ |
|
62
|
|
|
// no path if there is no need for moving |
|
63
|
2 |
|
if ($start->equals($end)) { |
|
64
|
|
|
$this->getLogger()->debug("$start equals $end, route is empty"); |
|
65
|
|
|
return []; |
|
66
|
|
|
} |
|
67
|
|
|
|
|
68
|
2 |
|
$this->addToOpen($start); |
|
69
|
|
|
|
|
70
|
2 |
|
$foundTarget = false; |
|
71
|
|
|
|
|
72
|
2 |
|
while (!$foundTarget && count($this->open) > 0) { |
|
73
|
2 |
|
$this->getLogger()->debug("sorting open nodes by cost. Node count:" . count($this->open)); |
|
74
|
2 |
|
uasort( |
|
75
|
2 |
|
$this->open, |
|
76
|
2 |
|
function (Node $a, Node $b) use ($end) { |
|
77
|
2 |
|
return $a->getFCost($end) - $b->getFCost($end); |
|
78
|
2 |
|
} |
|
79
|
|
|
); |
|
80
|
|
|
|
|
81
|
|
|
/** @var Node $current */ |
|
82
|
2 |
|
$current = $this->open[array_keys($this->open)[0]]; |
|
83
|
2 |
|
unset($this->open[array_keys($this->open)[0]]); |
|
84
|
|
|
|
|
85
|
2 |
|
$this->getLogger()->debug("current node selected: " . $current); |
|
86
|
|
|
|
|
87
|
2 |
|
if ((string)$current == (string)$end) { |
|
88
|
2 |
|
$foundTarget = $current; |
|
89
|
|
|
// we don't need to look at its adjacents anymore, |
|
90
|
|
|
// as we do have our route |
|
91
|
2 |
|
$this->getLogger()->debug("current node is target node, exciting loop"); |
|
92
|
2 |
|
continue; |
|
93
|
|
|
} |
|
94
|
|
|
|
|
95
|
2 |
|
$adjacent = $current->getAdjacentNodes(); |
|
96
|
|
|
|
|
97
|
2 |
|
foreach ($adjacent as $node) { |
|
98
|
2 |
|
if (in_array((string)$node, $this->closed)) { |
|
99
|
2 |
|
$this->getLogger()->debug("skipping adjacent: $node as it was already processed"); |
|
100
|
2 |
|
continue; |
|
101
|
|
|
} |
|
102
|
2 |
|
$node->setParent($current); |
|
103
|
2 |
|
$this->addToOpen($node); |
|
104
|
|
|
} |
|
105
|
2 |
|
$this->addToClosed($current); |
|
106
|
|
|
|
|
107
|
|
|
} |
|
108
|
|
|
|
|
109
|
|
|
// we failed to find a route |
|
110
|
2 |
|
if (!$foundTarget && count($this->open) == 0) { |
|
111
|
|
|
$this->getLogger()->debug("no open nodes left, and no target found."); |
|
112
|
|
|
return []; |
|
113
|
|
|
} |
|
114
|
|
|
|
|
115
|
2 |
|
$this->getLogger()->debug("found route!"); |
|
116
|
|
|
/** @var Node $foundTarget */ |
|
117
|
2 |
|
return $this->createRouteList($foundTarget); |
|
118
|
|
|
|
|
119
|
|
|
} |
|
120
|
|
|
|
|
121
|
|
|
/** |
|
122
|
|
|
* @param Node $node |
|
123
|
|
|
*/ |
|
124
|
2 |
|
protected function addToOpen(Node $node) |
|
125
|
|
|
{ |
|
126
|
|
|
if ( |
|
127
|
2 |
|
isset($this->open[(string)$node]) |
|
128
|
1 |
|
&& $this->open[(string)$node]->getGCost() < $node->getGCost() |
|
129
|
|
|
) { |
|
130
|
|
|
$this->getLogger()->debug("skipping add, $node is already known and gcost <= new path"); |
|
131
|
|
|
return; |
|
132
|
|
|
} |
|
133
|
2 |
|
$this->getLogger()->debug("adding new open node: $node"); |
|
134
|
2 |
|
$this->open[(string)$node] = $node; |
|
135
|
2 |
|
} |
|
136
|
|
|
|
|
137
|
|
|
/** |
|
138
|
|
|
* @param Node $node |
|
139
|
|
|
*/ |
|
140
|
2 |
|
protected function addToClosed(Node $node) |
|
141
|
|
|
{ |
|
142
|
2 |
|
$this->getLogger()->debug("adding $node to closed"); |
|
143
|
2 |
|
$this->closed[] = (string)$node; |
|
144
|
2 |
|
} |
|
145
|
|
|
|
|
146
|
|
|
/** |
|
147
|
|
|
* @param Node $foundTarget |
|
148
|
|
|
* @return Node[] |
|
149
|
|
|
*/ |
|
150
|
2 |
|
protected function createRouteList(Node $foundTarget) |
|
151
|
|
|
{ |
|
152
|
2 |
|
$route = []; |
|
153
|
2 |
|
$route[] = $foundTarget; |
|
154
|
|
|
|
|
155
|
2 |
|
while ($foundTarget = $foundTarget->getParent()) { |
|
156
|
2 |
|
$route[] = $foundTarget; |
|
157
|
|
|
} |
|
158
|
2 |
|
$route = array_reverse($route); |
|
159
|
|
|
|
|
160
|
2 |
|
return $route; |
|
161
|
|
|
} |
|
162
|
|
|
|
|
163
|
|
|
/** |
|
164
|
|
|
* return a logger |
|
165
|
|
|
* @return \Psr\Log\LoggerInterface |
|
166
|
|
|
*/ |
|
167
|
2 |
|
protected function getLogger() |
|
168
|
|
|
{ |
|
169
|
|
|
// as no one has set a logger, we use Psr's null logger here |
|
170
|
|
|
// as a default behaviour |
|
171
|
2 |
|
if (is_null($this->logger)) { |
|
172
|
2 |
|
$this->setLogger(new \Psr\Log\NullLogger()); |
|
173
|
|
|
} |
|
174
|
2 |
|
return $this->logger; |
|
175
|
|
|
} |
|
176
|
|
|
} |
|
177
|
|
|
|
ppetermann /
pathfinder
