From a85d205cf36acf8c31d0b0b6473b00449925c842 Mon Sep 17 00:00:00 2001 From: Dennis Eichhorn Date: Sun, 25 Aug 2019 21:43:43 +0200 Subject: [PATCH] start implementing pathfinding --- Algorithm/PathFinding/AStar.php | 91 +++ Algorithm/PathFinding/AStarNode.php | 94 +++ Algorithm/PathFinding/BestFirstSearch.php | 1 - Algorithm/PathFinding/Grid.php | 84 +-- Algorithm/PathFinding/Heuristic.php | 67 +++ Algorithm/PathFinding/JumpPointNode.php | 94 +++ Algorithm/PathFinding/JumpPointSearch.php | 567 ++++++++++++++++++ Algorithm/PathFinding/Node.php | 26 +- Algorithm/PathFinding/NullJumpPointNode.php | 28 + Algorithm/PathFinding/NullNode.php | 28 + Algorithm/PathFinding/Path.php | 79 ++- Algorithm/PathFinding/PathFinderInterface.php | 31 +- 12 files changed, 1143 insertions(+), 47 deletions(-) create mode 100644 Algorithm/PathFinding/AStarNode.php delete mode 100644 Algorithm/PathFinding/BestFirstSearch.php create mode 100644 Algorithm/PathFinding/JumpPointNode.php create mode 100644 Algorithm/PathFinding/NullJumpPointNode.php create mode 100644 Algorithm/PathFinding/NullNode.php diff --git a/Algorithm/PathFinding/AStar.php b/Algorithm/PathFinding/AStar.php index 8b1378917..4d777dd78 100644 --- a/Algorithm/PathFinding/AStar.php +++ b/Algorithm/PathFinding/AStar.php @@ -1 +1,92 @@ +getNode($startX, $startY); + $endNode = $grid->getNode($endX, $endY); + + if ($startNode === null || $endNode === null) { + return new Path($grid); + } + + $startNode->setG(0.0); + $startNode->setF(0.0); + $startNode->setOpened(true); + + $openList = new Heap(function($node1, $node2) { return $node1->getF() - $nodeB->getF(); }); + $openList->push($startNode); + $node = null; + + while (!$openList->isEmpty()) { + $node = $openList->pop(); + $node->setClosed(true); + + if ($node->isEqual($endNode)) { + break; + } + + $neighbors = $grid->getNeighbors($node, $movement); + $neighborsLength = \count($neighbors); + for ($i = 0; $i < $neighborsLength; ++$i) { + $neighbor = $neighbors[$i]; + + if ($neighbor->isClosed()) { + continue; + } + + $ng = $node->getG() + (($neighbor->getX() - $node->getX() === 0 || $neighbor->getY() - $node->getY() === 0) ? 1 : \sqrt(2)); + + if (!$neighbor->isOpened() || $ng < $neighbor->getG()) { + $neighbor->setG($ng); + $neighbor->setH($neighbor->getG() ?? $neighbor->getWeight() * Heuristic::heuristic($neighbor, $endNode, $heuristic)); + $neighbor->setF($neighbor->getG() + $neighbor->getH()); + $neighbor->setParent($node); + + if (!$neighbor->isOpened()) { + $openList->push($neighbor); + $jumpPoint->setOpened(true); + } else { + $openList->update($neighbor); + } + } + } + } + + $path = new Path($grid); + + while ($node !== null) { + $path->addNode($node); + $node = $node->getParent(); + } + + return $path; + } +} diff --git a/Algorithm/PathFinding/AStarNode.php b/Algorithm/PathFinding/AStarNode.php new file mode 100644 index 000000000..acabd2032 --- /dev/null +++ b/Algorithm/PathFinding/AStarNode.php @@ -0,0 +1,94 @@ +isClosed; + } + + public function isOpened() : bool + { + return $this->isOpened; + } + + public function isTested() : bool + { + return $this->isTested; + } + + public function setClosed(bool $isClosed) : void + { + $this->isClosed = $isClosed; + } + + public function setOpened(bool $isOpened) : void + { + $this->isOpened = $isOpened; + } + + public function setTested(bool $isTested) : void + { + $this->isTested = $isTested; + } + + public function setG(float $g) : void + { + $this->g = $g; + } + + public function setH(?float $h) : void + { + $this->h = $h; + } + + public function setF(float $f) : void + { + $this->f = $f; + } + + public function getG() : float + { + return $this->g; + } + + public function getH() : ?float + { + return $this->h; + } + + public function getF() : float + { + return $this->f; + } +} \ No newline at end of file diff --git a/Algorithm/PathFinding/BestFirstSearch.php b/Algorithm/PathFinding/BestFirstSearch.php deleted file mode 100644 index 8b1378917..000000000 --- a/Algorithm/PathFinding/BestFirstSearch.php +++ /dev/null @@ -1 +0,0 @@ - diff --git a/Algorithm/PathFinding/Grid.php b/Algorithm/PathFinding/Grid.php index 717cb188f..6d0c125c8 100644 --- a/Algorithm/PathFinding/Grid.php +++ b/Algorithm/PathFinding/Grid.php @@ -25,32 +25,45 @@ namespace phpOMS\Algorithm\PathFinding; class Grid { private array $nodes = [[]]; - private ?Node $nullNode = null; - - public function __construct(Node $nullNode) + + public static function createGridFromArray(array $gridArray, string $node) : self { - $this->nullNode = $nullNode; - } - - public function getNullNode() : Node - { - return $this->nullNode; - } - - public function getNode(int $x, int $y) : Node - { - if (!isset($this->nodes[$x]) || $this->nodes[$x][$y]) { - return $this->nullNode; + $grid = new self(); + foreach ($gridArray as $y => $yRow) { + foreach ($yRow as $x => $xElement) { + if ($xElement === 0 || $xElement === 1 || $xElement === 2) { + $empty = new $node($x, $y, 1.0, true); + $grid->setNode($x, $y, $empty); + } elseif ($xElement === 9) { + $wall = new $node($x, $y, 1.0, false); + $grid->setNode($x, $y, $wall); + } + } } - - return $this->nodes[$x][$y]; + + return $grid; } - + + public function setNode(int $x, int $y, Node $node) : void + { + $this->nodes[$y][$x] = $node; + } + + public function getNode(int $x, int $y) : ?Node + { + if (!isset($this->nodes[$y]) || $this->nodes[$y][$x]) { + // todo: add null node to grid because we need to modify some properties later on and remember them! + return null; + } + + return $this->nodes[$y][$x]; + } + public function getNeighbors(Node $node, int $movement) : array { $x = $node->getX(); $y = $node->getY(); - + $neighbors = []; $s0 = false; $s1 = false; @@ -60,31 +73,32 @@ class Grid $d1 = false; $d2 = false; $d3 = false; - + + // todo: check $x and $y because original implementation is flipped!!! if ($this->getNode($x, $y - 1)->isWalkable()) { - $neighbors[$x][$y - 1]; + $neighbors[] = $this->getNode($x, $y - 1); $s0 = true; } - + if ($this->getNode($x + 1, $y)->isWalkable()) { - $neighbors[$x + 1][$y]; + $neighbors[] = $this->getNode($x + 1, $y); $s1 = true; } - + if ($this->getNode($x, $y + 1)->isWalkable()) { - $neighbors[$x][$y + 1]; + $neighbors[] = $this->getNode($x, $y + 1); $s2 = true; } - + if ($this->getNode($x - 1, $y)->isWalkable()) { - $neighbors[$x - 1][$y]; + $neighbors[] = $this->getNode($x - 1, $y); $s3 = true; } - + if ($movement === MovementType::STRAIGHT) { return $neighbors; } - + if ($movement === MovementType::DIAGONAL_NO_OBSTACLE) { $d0 = $s3 && $s0; $d1 = $s0 && $s1; @@ -101,23 +115,23 @@ class Grid $d2 = true; $d3 = true; } - + if ($d0 && $this->getNode($x - 1, $y - 1)->isWalkable()) { - $neighbors[] = $this->getNode($x - 1, $y - 1]); + $neighbors[] = $this->getNode($x - 1, $y - 1); } - + if ($d1 && $this->getNode($x + 1, $y - 1)->isWalkable()) { $neighbors[] = $this->getNode($x + 1, $y - 1); } - + if ($d2 && $this->getNode($x + 1, $y + 1)->isWalkable()) { $neighbors[] = $this->getNode($x + 1, $y + 1); } - + if ($d3 && $this->getNode($x - 1, $y + 1)->isWalkable()) { $neighbors[] = $this->getNode($x - 1, $y + 1); } - + return $neighbors; } } diff --git a/Algorithm/PathFinding/Heuristic.php b/Algorithm/PathFinding/Heuristic.php index 8b1378917..0e8f259c5 100644 --- a/Algorithm/PathFinding/Heuristic.php +++ b/Algorithm/PathFinding/Heuristic.php @@ -1 +1,68 @@ +getX() - $node2->getX()) + \abs($node1->getY() - $node2->getY()); + } + + public static function euclidean(Node $node1, Node $node2) : float + { + $dx = \abs($node1->getX() - $node2->getX()); + $dy = \abs($node1->getY() - $node2->getY()); + + return \sqrt($dx * $dx + $dy * $dy); + } + + public static function octile(Node $node1, Node $node2) : float + { + $dx = \abs($node1->getX() - $node2->getX()); + $dy = \abs($node1->getY() - $node2->getY()); + + return $dx < $dy ? (\sqrt(2) - 1) * $dx + $dy : (\sqrt(2) - 1) * $dy + $dx; + } + + public static function chebyshev(Node $node1, Node $node2) : float + { + return \max( + \abs($node1->getX() - $node2->getX()), + \abs($node1->getY() - $node2->getY()) + ); + } +} diff --git a/Algorithm/PathFinding/JumpPointNode.php b/Algorithm/PathFinding/JumpPointNode.php new file mode 100644 index 000000000..46c2d504c --- /dev/null +++ b/Algorithm/PathFinding/JumpPointNode.php @@ -0,0 +1,94 @@ +isClosed; + } + + public function isOpened() : bool + { + return $this->isOpened; + } + + public function isTested() : bool + { + return $this->isTested; + } + + public function setClosed(bool $isClosed) : void + { + $this->isClosed = $isClosed; + } + + public function setOpened(bool $isOpened) : void + { + $this->isOpened = $isOpened; + } + + public function setTested(bool $isTested) : void + { + $this->isTested = $isTested; + } + + public function setG(float $g) : void + { + $this->g = $g; + } + + public function setH(?float $h) : void + { + $this->h = $h; + } + + public function setF(float $f) : void + { + $this->f = $f; + } + + public function getG() : float + { + return $this->g; + } + + public function getH() : ?float + { + return $this->h; + } + + public function getF() : float + { + return $this->f; + } +} \ No newline at end of file diff --git a/Algorithm/PathFinding/JumpPointSearch.php b/Algorithm/PathFinding/JumpPointSearch.php index 8b1378917..67a99cbae 100644 --- a/Algorithm/PathFinding/JumpPointSearch.php +++ b/Algorithm/PathFinding/JumpPointSearch.php @@ -1 +1,568 @@ +getNode($startX, $startY); + $endNode = $grid->getNode($endX, $endY); + + if ($startNode === null || $endNode === null) { + return new Path($grid); + } + + $startNode->setG(0.0); + $startNode->setF(0.0); + $startNode->setOpened(true); + + $openList = new Heap(function($node1, $node2) { return $node1->getF() - $nodeB->getF(); }); + $openList->push($startNode); + $node = null; + + while (!$openList->isEmpty()) { + $node = $openList->pop(); + $node->setClosed(true); // todo: do i really want to modify the node? probably not? I should clone the grid and all it's nodes. + + if ($node->isEqual($endNode)) { + break; + } + + $openList = self::identifySuccessors($node, $grid, $heuristic, $movement, $endNode, $openList); + } + + $path = new Path($grid); + + while ($node !== null) { + $path->addNode($node); + $node = $node->getParent(); + } + + return $path; + } + + public static function identifySuccessors(Node $node, Grid $grid, int $heuristic, int $movement, Node $endNode, Heap $openList) : Heap + { + $neighbors = self::findNeighbors($node, $movement, $grid); + $neighborsLength = \count($neighbors); + + for ($i = 0, $l = $neighborsLength; $i < $l; ++$i) { + $neighbor = $neighbors[$i]; // todo: needs to be Node!!! + $jumpPoint = self::jump($neighbor, $node, $movement, $grid); + + if ($jumpPoint === null || $jumpPoint->isClosed()) { + continue; + } + + $d = Heuristic::octile($node, $jumpPoint); + $ng = $node->getG() + $d; + + if (!$jumpPoint->isOpened() || $ng < $jumpPoint->getG()) { + $jumpPoint->setG($ng); + $jumpPoint->setH($jumpPoint->getH() ?? Heuristic::heuristic($jumpPoint, $endNode, $heuristic)); + $jumpPoint->setF($jumpPoint->getG() + $jumpPoint->getH()); + $jumpPoint->setParent($node); + + if (!$jumpPoint->isOpened()) { + $openList->push($jumpPoint); + $jumpPoint->setOpened(true); + } else { + $openList->update($jumpPoint); + } + } + } + + return $openList; + } + + private static function findNeighbors(Node $node, int $movement, Grid $grid) : array + { + if ($movement === MovementType::STRAIGHT) { + return self::findNeighborsStraight($node, $grid); + } elseif ($movement === MovementType::DIAGONAL) { + return self::findNeighborsDiagonal($node, $grid); + } elseif ($movement === MovementType::DIAGONAL_ONE_OBSTACLE) { + return self::findNeighborsDiagonalOneObstacle($node, $grid); + } + + return self::findNeighborsDiagonalNoObstacle($node, $grid); + } + + private static function findNeighborsStraight(Node $node, Grid $grid) : array + { + if ($node->getParent() === null) { + return $grid->getNeighbors($node, MovementType::STRAIGHT); + } + + $x = $node->getX(); + $y = $node->getY(); + + $px = $node->getParent()->getX(); + $py = $node->getParent()->getY(); + + $dx = ($x - $px) / \max(\abs($x - $px), 1); + $dy = ($y - $py) / \may(\abs($y - $py), 1); + + $neighbors = []; + if ($dx !== 0) { + if ($grid->getNode($x, $y - 1)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y - 1); + } + + if ($grid->getNode($x, $y + 1)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + 1); + } + + if ($grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y); + } + } elseif ($dy !== 0) { + if ($grid->getNode($x - 1, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x - 1, $y); + } + + if ($grid->getNode($x + 1, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + 1, $y); + } + + if ($grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + $dy); + } + } + + return $neighbors; + } + + private static function findNeighborsDiagonal(Node $node, Grid $grid) : array + { + if ($node->getParent() === null) { + return $grid->getNeighbors($node, MovementType::DIAGONAL); + } + + $x = $node->getX(); + $y = $node->getY(); + + $px = $node->getParent()->getX(); + $py = $node->getParent()->getY(); + + $dx = ($x - $px) / \max(\abs($x - $px), 1); + $dy = ($y - $py) / \may(\abs($y - $py), 1); + + $neighbors = []; + if ($dx !== 0 && $dy !== 0) { + if ($grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + $dy); + } + + if ($grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y); + } + + if ($grid->getNode($x + $dx, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y + $dy); + } + + if (!$grid->getNode($x - $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x - $dx, $y + $dy); + } + + if (!$grid->getNode($x, $y - $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y - $dy); + } + } elseif ($dx === 0) { + if ($grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + $dy); + } + + if (!$grid->getNode($x + 1, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + 1, $y + $dy); + } + + if (!$grid->getNode($x - 1, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x - 1, $y + $dy); + } + } else { + if ($grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y); + } + + if (!$grid->getNode($x, $y + 1)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y + 1); + } + + if (!$grid->getNode($x, $y - 1)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y - 1); + } + } + + return $neighbors; + } + + private static function findNeighborsDiagonalOneObstacle(Node $node, Grid $grid) : array + { + if ($node->getParent() === null) { + return $grid->getNeighbors($node, MovementType::DIAGONAL_ONE_OBSTACLE); + } + + $x = $node->getX(); + $y = $node->getY(); + + $px = $node->getParent()->getX(); + $py = $node->getParent()->getY(); + + $dx = ($x - $px) / \max(\abs($x - $px), 1); + $dy = ($y - $py) / \may(\abs($y - $py), 1); + + $neighbors = []; + if ($dx !== 0 && $dy !== 0) { + if ($grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + $dy); + } + + if ($grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y); + } + + if ($grid->getNode($x, $y + $dy) || $grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y + $dy); + } + + if (!$grid->getNode($x - $dx, $y) && $grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x - $dx, $y + $dy); + } + + if (!$grid->getNode($x, $y - $dy) && $grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y - $dy); + } + } elseif ($dx === 0) { + if ($grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + $dy); + if (!$grid->getNode($x + 1, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + 1, $y + $dy); + } + if (!$grid->getNode($x - 1, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x - 1, $y + $dy); + } + } + } else { + if ($grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y); + if (!$grid->getNode($x, $y + 1)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y + 1); + } + if (!$grid->getNode($x, $y - 1)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y - 1); + } + } + } + + return $neighbors; + } + + private static function findNeighborsDiagonalNoObstacle(Node $node, Grid $grid) : array + { + if ($node->getParent() === null) { + return $grid->getNeighbors($node, MovementType::DIAGONAL_NO_OBSTACLE); + } + + $x = $node->getX(); + $y = $node->getY(); + + $px = $node->getParent()->getX(); + $py = $node->getParent()->getY(); + + $dx = ($x - $px) / \max(\abs($x - $px), 1); + $dy = ($y - $py) / \may(\abs($y - $py), 1); + + $neighbors = []; + if ($dx !== 0 && $dy !== 0) { + if ($grid->getNode($x, $y + $dy)->isWalkable()) { + $neighbors[] = $grid->getNode($x, $y + $dy); + } + + if ($grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y); + } + + if ($grid->getNode($x, $y + $dy) || $grid->getNode($x + $dx, $y)->isWalkable()) { + $neighbors[] = $grid->getNode($x + $dx, $y + $dy); + } + } elseif ($dx !== 0 && $dy === 0) { + $isNextWalkable = $grid->getNode($x + $dx, $y)->isWalkable(); + $isTopWalkable = $grid->getNode($x, $y + 1)->isWalkable(); + $isBottomWalkable = $grid->getNode($x, $y - 1)->isWalkable(); + + if ($isNextWalkable) { + $neighbors[] = $grid->getNode($x + $dx, $y); + if ($isTopWalkable) { + $neighbors[] = $grid->getNode($x + $dx, $y + 1); + } + + if ($isBottomWalkable) { + $neighbors[] = $grid->getNode($x + $dx, $y - 1); + } + } + + if ($isTopWalkable) { + $neighbors[] = $grid->getNode($x, $y + 1); + } + + if ($isBottomWalkable) { + $neighbors[] = $grid->getNode($x, $y - 1); + } + } elseif ($dx === 0 && $dy !== 0) { + $isNextWalkable = $grid->getNode($x, $y + $dy)->isWalkable(); + $isRightWalkable = $grid->getNode($x + 1, $y)->isWalkable(); + $isLeftWalkable = $grid->getNode($x - 1, $y)->isWalkable(); + + if ($isNextWalkable) { + $neighbors[] = $grid->getNode($x, $y + $dy); + if ($isRightWalkable) { + $neighbors[] = $grid->getNode($x + 1, $y + $dy); + } + + if ($isLeftWalkable) { + $neighbors[] = $grid->getNode($x - 1, $y + $dy); + } + } + + if ($isRightWalkable) { + $neighbors[] = $grid->getNode($x + 1, $y); + } + + if ($isLeftWalkable) { + $neighbors[] = $grid->getNode($x - 1, $y); + } + } + + return $neighbors; + } + + private static function jump(Node $node, Node $endNode, int $movement, Grid $grid) : ?Node + { + if ($movement === MovementType::STRAIGHT) { + return self::jumpStraight($node, $endNode, $grid); + } elseif ($movement === MovementType::DIAGONAL) { + return self::jumpDiagonal($node, $endNode, $grid); + } elseif ($movement === MovementType::DIAGONAL_ONE_OBSTACLE) { + return self::jumpDiagonalOneObstacle($node, $endNode, $grid); + } + + return self::jumpDiagonalNoObstacle($node, $endNode, $grid); + } + + private static function jumpStraight(Node $node, Node $endNode, Grid $grid) : ?Node + { + $x = $node->getX(); + $y = $node->getY(); + + $dx = $x - $endNode->getX(); + $dy = $y - $endNode->getY(); + + if (!$node->isWalkable()) { + return null; + } + + // not always necessary but might be important for the future + $node->setTested(true); + + if ($node->isEqual($endNode)) { + return $node; + } + + if ($dx !== 0) { + if (($grid->getNode($x, $y - 1)->isWalkable() && !$grid->getNode($x - $dx, $y - 1)->isWalkable()) + || ($grid->getNode($x, $y + 1)->isWalkable() && !$grid->getNode($x - $dx, $y + 1)->isWalkable()) + ) { + return $node; + } + } elseif ($dy !== 0) { + if (($grid->getNode($x - 1, $y)->isWalkable() && !$grid->getNode($x - 1, $y - $dy)->isWalkable()) + || ($grid->getNode($x + 1, $y)->isWalkable() && !$grid->getNode($x + 1, $y - $dy)->isWalkable()) + ) { + return $node; + } + + if (self::jumpStraight($grid->getNode($x + 1, $y), $node, $grid) !== null + || self::jumpStraight($grid->getNode($x - 1, $y), $node, $grid) !== null + ) { + return $node; + } + } else { + throw new \Exception('invalid movement'); + } + + return self::jumpStraight($grid->getNode($x + $dx, $y + $dy), $node, $grid); + } + + private static function jumpDiagonal(Node $node, Node $endNode, Grid $grid) : ?Node + { + $x = $node->getX(); + $y = $node->getY(); + + $dx = $x - $endNode->getX(); + $dy = $y - $endNode->getY(); + + if (!$node->isWalkable()) { + return null; + } + + // not always necessary but might be important for the future + $node->setTested(true); + + if ($node->isEqual($endNode)) { + return $node; + } + + if ($dx !== 0 && $dy !== 0) { + if (($grid->getNode($x - $dx, $y + $dy)->isWalkable() && !$grid->getNode($x - $dx, $y)->isWalkable()) + || ($grid->getNode($x + $dx, $y - $dy)->isWalkable() && !$grid->getNode($x, $y - $dy)->isWalkable()) + ) { + return $node; + } + + if (self::jumpDiagonal($grid->getNode($x + $dx, $y), $node, $grid) !== null + || self::jumpDiagonal($grid->getNode($x, $y + $dy), $node, $grid) !== null + ) { + return $node; + } + } elseif ($dx !== 0 && $dy === 0) { + if (($grid->getNode($x + $dx, $y + 1)->isWalkable() && !$grid->getNode($x, $y + 1)->isWalkable()) + || ($grid->getNode($x + $dx, $y - 1)->isWalkable() && !$grid->getNode($x, $y - 1)->isWalkable()) + ) { + return $node; + } + } else { + if (($grid->getNode($x + 1, $y + $dy)->isWalkable() && !$grid->getNode($x + 1, $y)->isWalkable()) + || ($grid->getNode($x - 1, $y + $dy)->isWalkable() && !$grid->getNode($x - 1, $y)->isWalkable()) + ) { + return $node; + } + } + + return self::jumpDiagonal($grid->getNode($x + $dx, $y + $dy), $node, $grid); + } + + private static function jumpDiagonalOneObstacle(Node $node, Node $endNode, Grid $grid) : ?Node + { + $x = $node->getX(); + $y = $node->getY(); + + $dx = $x - $endNode->getX(); + $dy = $y - $endNode->getY(); + + if (!$node->isWalkable()) { + return null; + } + + // not always necessary but might be important for the future + $node->setTested(true); + + if ($node->isEqual($endNode)) { + return $node; + } + + if ($dx !== 0 && $dy !== 0) { + if (($grid->getNode($x - $dx, $y + $dy)->isWalkable() && !$grid->getNode($x - $dx, $y)->isWalkable()) + || ($grid->getNode($x + $dx, $y - $dy)->isWalkable() && !$grid->getNode($x, $y - $dy)->isWalkable()) + ) { + return $node; + } + + if (self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y), $node, $grid) !== null + || self::jumpDiagonalOneObstacle($grid->getNode($x, $y + $dy), $node, $grid) !== null + ) { + return $node; + } + } elseif ($dx !== 0 && $dy === 0) { + if (($grid->getNode($x + $dx, $y + 1)->isWalkable() && !$grid->getNode($x, $y + 1)->isWalkable()) + || ($grid->getNode($x + $dx, $y - 1)->isWalkable() && !$grid->getNode($x, $y - 1)->isWalkable()) + ) { + return $node; + } + } else { + if (($grid->getNode($x + 1, $y + $dy)->isWalkable() && !$grid->getNode($x + 1, $y)->isWalkable()) + || ($grid->getNode($x - 1, $y + $dy)->isWalkable() && !$grid->getNode($x - 1, $y)->isWalkable()) + ) { + return $node; + } + } + + if ($grid->getNode($x + $dx, $y)->isWalkable() || $grid->getNode($x, $y + $dy)->isWalkable()) { + return self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y + $dy), $node, $grid); + } + + return null; + } + + private static function jumpDiagonalNoObstacle(Node $node, Node $endNode, Grid $grid) : ?Node + { + $x = $node->getX(); + $y = $node->getY(); + + $dx = $x - $endNode->getX(); + $dy = $y - $endNode->getY(); + + if (!$node->isWalkable()) { + return null; + } + + // not always necessary but might be important for the future + $node->setTested(true); + + if ($node->isEqual($endNode)) { + return $node; + } + + if ($dx !== 0 && $dy !== 0) { + if (self::jumpDiagonalNoObstacle($grid->getNode($x + $dx, $y), $node, $grid) !== null + || self::jumpDiagonalNoObstacle($grid->getNode($x, $y + $dy), $node, $grid) !== null + ) { + return $node; + } + } elseif ($dx !== 0 && $dy === 0) { + if (($grid->getNode($x, $y - 1)->isWalkable() && !$grid->getNode($x - $dx, $y - 1)->isWalkable()) + || ($grid->getNode($x, $y + 1)->isWalkable() && !$grid->getNode($x - $dx, $y + 1)->isWalkable()) + ) { + return $node; + } + } elseif ($dx === 0 && $dy !== 0) { + if (($grid->getNode($x - 1, $y)->isWalkable() && !$grid->getNode($x - 1, $y - $dy)->isWalkable()) + || ($grid->getNode($x + 1, $y)->isWalkable() && !$grid->getNode($x + 1, $y - $dy)->isWalkable()) + ) { + return $node; + } + } + + if ($grid->getNode($x + $dx, $y)->isWalkable() || $grid->getNode($x, $y + $dy)->isWalkable()) { + return self::jumpDiagonalNoObstacle($grid->getNode($x + $dx, $y + $dy), $node, $grid); + } + + return null; + } +} \ No newline at end of file diff --git a/Algorithm/PathFinding/Node.php b/Algorithm/PathFinding/Node.php index 147fdd1e9..916612701 100644 --- a/Algorithm/PathFinding/Node.php +++ b/Algorithm/PathFinding/Node.php @@ -28,7 +28,8 @@ class Node private int $y = 0; private float $weight = 1.0; private bool $isWalkable = true; - + private ?Node $parent = null; + public function __construct(int $x, int $y, float $weight = 1.0, bool $isWalkable = true) { $this->x = $x; @@ -36,24 +37,39 @@ class Node $this->weight = $weight; $this->isWalkable = $isWalkable; } - + public function isWalkable() : bool { return $this->isWalkable; } - + public function getWeight() : float { return $this->weight; } - + public function getX() : int { return $this->x; } - + public function getY() : int { return $this->y; } + + public function setParent(?Node $node) : void + { + $this->parent = $node; + } + + public function getParent() : ?Node + { + return $this->parent; + } + + public function isEqual(Node $node) : bool + { + return $this->x === $node->getX() && $this->y === $node->getY(); + } } diff --git a/Algorithm/PathFinding/NullJumpPointNode.php b/Algorithm/PathFinding/NullJumpPointNode.php new file mode 100644 index 000000000..e33665baf --- /dev/null +++ b/Algorithm/PathFinding/NullJumpPointNode.php @@ -0,0 +1,28 @@ +grid = $grid; + } + + public function addNode(Node $node) : void + { + $this->nodes[] = $node; + } + + public function expandPath() : array + { + $reverse = \array_reverse($this->nodes); + $length = count($reverse); + + if ($length < 2) { + return $reverse; + } + + $expanded = []; + for ($i = 0; $i < $length - 1; ++$i) { + $coord0 = $reverse[$i]; + $coord1 = $reverse[$i + 1]; + + $interpolated = $this->interpolate($coord0, $coord1); + $iLength = count($interpolated); + + $expanded = \array_merge($expanded, \array_slice($interpolated, 0, $iLength - 1)); + } + + $expanded[] = $reverse[$length - 1]; + + return $expanded; + } + + private function interpolate(Node $node1, Node $node2) : array + { + $dx = \abs($node2->getX() - $node1->getX()); + $dy = \abs($node2->getY() - $node1->getY()); + + $sx = ($node1->getX() < $node2->getX()) ? 1 : -1; + $sy = ($node1->getY() < $node2->getY()) ? 1 : -1; + + $node = $node1; + $err = $dx - $dy; + + $line = []; + while (true) { + $line[] = $node; + + if ($node->getX() === $node2->getX() && $node->getY() === $node2->getY()) { + break; + } + + $e2 = 2 * $err; + $x0 = 0; + + if ($e2 > -$dy) { + $err -= $dy; + $x0 = $node->getX() + $sx; + } + + $y0 = 0; + if ($e2 < $dx) { + $err += $dx; + $y0 = $node->getY() + $sy; + } + + $node = $this->grid->getNode($x0, $y0); + } + + return $line; + } } diff --git a/Algorithm/PathFinding/PathFinderInterface.php b/Algorithm/PathFinding/PathFinderInterface.php index bbf315b69..3c7740c0a 100644 --- a/Algorithm/PathFinding/PathFinderInterface.php +++ b/Algorithm/PathFinding/PathFinderInterface.php @@ -1,9 +1,32 @@ +