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()); }