public function computePath(Node $source, Node $target)
{
if (!$this->grid instanceof NodeGrid) {
throw new \RuntimeException('Invalid Grid');
}
$priorityQueue = new NodePriorityQueueMin();
$distanceMap = new NodeMap();
$previousMap = new NodeMap();
$priorityQueue->insert($source, 0);
$distanceMap->insert($source, 0);
while (!$priorityQueue->isEmpty()) {
$node = $priorityQueue->extract();
if ($node->getId() === $target->getId()) {
return new NodePath($previousMap->lookupFrom($node));
}
$neighbors = $this->grid->getWalkableNeighbors($node);
foreach ($neighbors as $neighbor) {
$alternativeDistance = $distanceMap->lookup($node) + $this->distance->compute($node, $neighbor);
if (!$distanceMap->exists($neighbor) || $alternativeDistance < $distanceMap->lookup($neighbor)) {
$priorityQueue->insert($neighbor, $alternativeDistance);
$distanceMap->insert($neighbor, $alternativeDistance);
$previousMap->insert($neighbor, $node);
}
}
}
return array();
// no path found
}
private function computeDistanceGraph(array $nodesList)
{
foreach ($nodesList as $source) {
$priorityQueue = new NodePriorityQueueMin();
$priorityQueue->insert($source, 0);
while (!$priorityQueue->isEmpty()) {
$node = $priorityQueue->extract();
$neighbors = $this->grid->getWalkableNeighbors($node);
foreach ($neighbors as $neighbor) {
$alternativeDistance = $this->distanceGraph->getEdgeBetween($source, $node) + $this->distance->compute($node, $neighbor);
if (!$this->distanceGraph->existsEdgeBetween($source, $neighbor) || $alternativeDistance < $this->distanceGraph->getEdgeBetween($source, $neighbor)) {
$priorityQueue->insert($neighbor, $alternativeDistance);
$this->distanceGraph->createEdgeBetween($source, $neighbor, $alternativeDistance);
}
}
}
}
}
public function computePath(Node $source, Node $target)
{
if (!$this->grid instanceof NodeGrid) {
throw new \RuntimeException('Invalid Grid');
}
$fScorePriorityQueue = new NodePriorityQueueMin();
$gScoreMap = new NodeMap();
$openedMap = new NodeMap();
$closedMap = new NodeMap();
$previousMap = new NodeMap();
$fScorePriorityQueue->insert($source, 0);
$gScoreMap->insert($source, 0);
$openedMap->insert($source);
while (!$fScorePriorityQueue->isEmpty()) {
$node = $fScorePriorityQueue->extract();
$closedMap->insert($node);
if ($node->getId() === $target->getId()) {
return new NodePath($previousMap->lookupFrom($node));
}
$neighbors = $this->grid->getWalkableNeighbors($node);
foreach ($neighbors as $neighbor) {
if ($closedMap->exists($neighbor)) {
continue;
}
$gScore = $gScoreMap->lookup($node) + $this->distance->compute($node, $neighbor);
if (!$openedMap->exists($neighbor) || $gScore < $gScoreMap->lookup($neighbor)) {
$fScore = $gScore + $this->heuristic->compute($node, $target);
$fScorePriorityQueue->insert($neighbor, $fScore);
$gScoreMap->insert($neighbor, $gScore);
$openedMap->insert($neighbor);
$previousMap->insert($neighbor, $node);
}
}
}
// no path found
return array();
}
public function testPriorityQueue()
{
$priorityQueue = new NodePriorityQueueMin();
$simpleNode = new Node(1, 1);
$priorityQueue->insert($simpleNode, 0);
$this->assertSame($simpleNode->getId(), $priorityQueue->extract()->getId());
$this->assertTrue($priorityQueue->isEmpty());
$aNode = new Node(2, 1);
$bNode = new Node(2, 2);
$cNode = new Node(2, 3);
$priorityQueue->insert($aNode, 2);
$priorityQueue->insert($bNode, 1);
$priorityQueue->insert($cNode, 3);
$this->assertSame($bNode->getId(), $priorityQueue->extract()->getId());
$this->assertSame($aNode->getId(), $priorityQueue->extract()->getId());
$this->assertSame($cNode->getId(), $priorityQueue->extract()->getId());
$this->assertTrue($priorityQueue->isEmpty());
$dNode = new Node(3, 1);
$eNode = new Node(3, 2);
$fNode = new Node(3, 3);
$priorityQueue->insert($dNode, 0);
$priorityQueue->insert($eNode, 0);
$priorityQueue->insert($fNode, 0);
$priorityQueue->insert($dNode, 3);
$priorityQueue->insert($eNode, 2);
$priorityQueue->insert($fNode, 1);
$this->assertSame($fNode->getId(), $priorityQueue->extract()->getId());
$this->assertSame($eNode->getId(), $priorityQueue->extract()->getId());
$this->assertSame($dNode->getId(), $priorityQueue->extract()->getId());
$this->assertTrue($priorityQueue->isEmpty());
}
