From b8ca68547341db28a88ccf791c43cd3cffdf9fd4 Mon Sep 17 00:00:00 2001 From: Dennis Eichhorn Date: Sat, 19 Oct 2019 13:10:45 +0200 Subject: [PATCH] fix and complete path finding algorithm --- Algorithm/PathFinding/BreadthFirstSearch.php | 1 - Algorithm/PathFinding/Dijkstra.php | 1 - Algorithm/PathFinding/Grid.php | 7 +- Algorithm/PathFinding/IDAStar.php | 1 - Algorithm/PathFinding/JumpPointSearch.php | 142 ++++++----- Algorithm/PathFinding/Path.php | 119 +++++---- tests/Algorithm/PathFinding/AStarTest.php | 225 ++++++++++++++++++ .../PathFinding/JumpPointSearchTest.php | 172 ++++++++++++- 8 files changed, 554 insertions(+), 114 deletions(-) delete mode 100644 Algorithm/PathFinding/BreadthFirstSearch.php delete mode 100644 Algorithm/PathFinding/Dijkstra.php delete mode 100644 Algorithm/PathFinding/IDAStar.php create mode 100644 tests/Algorithm/PathFinding/AStarTest.php diff --git a/Algorithm/PathFinding/BreadthFirstSearch.php b/Algorithm/PathFinding/BreadthFirstSearch.php deleted file mode 100644 index 8b1378917..000000000 --- a/Algorithm/PathFinding/BreadthFirstSearch.php +++ /dev/null @@ -1 +0,0 @@ - diff --git a/Algorithm/PathFinding/Dijkstra.php b/Algorithm/PathFinding/Dijkstra.php deleted file mode 100644 index 8b1378917..000000000 --- a/Algorithm/PathFinding/Dijkstra.php +++ /dev/null @@ -1 +0,0 @@ - diff --git a/Algorithm/PathFinding/Grid.php b/Algorithm/PathFinding/Grid.php index e262efed8..b29c41982 100644 --- a/Algorithm/PathFinding/Grid.php +++ b/Algorithm/PathFinding/Grid.php @@ -103,11 +103,7 @@ class Grid */ public function isWalkable(int $x, int $y) : bool { - if (!isset($this->nodes[$y]) || !isset($this->nodes[$y][$x]) || !$this->nodes[$y][$x]->isWalkable()) { - return false; - } - - return true; + return isset($this->nodes[$y]) && isset($this->nodes[$y][$x]) && $this->nodes[$y][$x]->isWalkable(); } /** @@ -136,7 +132,6 @@ class Grid $d2 = false; $d3 = false; - // todo: check $x and $y because original implementation is flipped!!! if ($this->isWalkable($x, $y - 1)) { $neighbors[] = $this->getNode($x, $y - 1); $s0 = true; diff --git a/Algorithm/PathFinding/IDAStar.php b/Algorithm/PathFinding/IDAStar.php deleted file mode 100644 index 8b1378917..000000000 --- a/Algorithm/PathFinding/IDAStar.php +++ /dev/null @@ -1 +0,0 @@ - diff --git a/Algorithm/PathFinding/JumpPointSearch.php b/Algorithm/PathFinding/JumpPointSearch.php index 1874ab13b..a73fb9849 100644 --- a/Algorithm/PathFinding/JumpPointSearch.php +++ b/Algorithm/PathFinding/JumpPointSearch.php @@ -92,12 +92,25 @@ class JumpPointSearch implements PathFinderInterface */ public static function identifySuccessors(JumpPointNode $node, Grid $grid, int $heuristic, int $movement, JumpPointNode $endNode, Heap $openList) : Heap { + if ($node->getY() === 5) { + $a = 1; + } + $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); + for ($i = 0; $i < $neighborsLength; ++$i) { + $neighbor = $neighbors[$i]; + + if ($neighbor === null) { + continue; + } + + if ($neighbor->getX() === 2) { + $a = 1; + } + + $jumpPoint = self::jump($neighbor, $node, $endNode, $movement, $grid); if ($jumpPoint === null || $jumpPoint->isClosed()) { continue; @@ -173,7 +186,7 @@ class JumpPointSearch implements PathFinderInterface /** @var int $dx */ $dx = ($x - $px) / \max(\abs($x - $px), 1); /** @var int $dy */ - $dy = ($y - $py) / \may(\abs($y - $py), 1); + $dy = ($y - $py) / \max(\abs($y - $py), 1); $neighbors = []; if ($dx !== 0) { @@ -230,7 +243,7 @@ class JumpPointSearch implements PathFinderInterface /** @var int $dx */ $dx = ($x - $px) / \max(\abs($x - $px), 1); /** @var int $dy */ - $dy = ($y - $py) / \may(\abs($y - $py), 1); + $dy = ($y - $py) / \max(\abs($y - $py), 1); $neighbors = []; if ($dx !== 0 && $dy !== 0) { @@ -307,7 +320,7 @@ class JumpPointSearch implements PathFinderInterface /** @var int $dx */ $dx = ($x - $px) / \max(\abs($x - $px), 1); /** @var int $dy */ - $dy = ($y - $py) / \may(\abs($y - $py), 1); + $dy = ($y - $py) / \max(\abs($y - $py), 1); $neighbors = []; if ($dx !== 0 && $dy !== 0) { @@ -323,11 +336,11 @@ class JumpPointSearch implements PathFinderInterface $neighbors[] = $grid->getNode($x + $dx, $y + $dy); } - if (!$grid->getNode($x - $dx, $y) && $grid->isWalkable($x, $y + $dy)) { + if (!$grid->isWalkable($x - $dx, $y) && $grid->isWalkable($x, $y + $dy)) { $neighbors[] = $grid->getNode($x - $dx, $y + $dy); } - if (!$grid->getNode($x, $y - $dy) && $grid->isWalkable($x + $dx, $y)) { + if (!$grid->isWalkable($x, $y - $dy) && $grid->isWalkable($x + $dx, $y)) { $neighbors[] = $grid->getNode($x + $dx, $y - $dy); } } elseif ($dx === 0) { @@ -380,7 +393,7 @@ class JumpPointSearch implements PathFinderInterface /** @var int $dx */ $dx = ($x - $px) / \max(\abs($x - $px), 1); /** @var int $dy */ - $dy = ($y - $py) / \may(\abs($y - $py), 1); + $dy = ($y - $py) / \max(\abs($y - $py), 1); $neighbors = []; if ($dx !== 0 && $dy !== 0) { @@ -449,50 +462,52 @@ class JumpPointSearch implements PathFinderInterface /** * Find next jump point * - * @param JumpPointNode $node Node to find jump point from - * @param JumpPointNode $endNode End node to find path to - * @param int $movement Movement type - * @param Grid $grid Grid of the nodes + * @param null|JumpPointNode $node Node to find jump point from + * @param null|JumpPointNode $pNode Parent node + * @param null|JumpPointNode $endNode End node to find path to + * @param int $movement Movement type + * @param Grid $grid Grid of the nodes * * @return null|JumpPointNode * * @since 1.0.0 */ - private static function jump(JumpPointNode $node, JumpPointNode $endNode, int $movement, Grid $grid) : ?JumpPointNode + private static function jump(?JumpPointNode $node, ?JumpPointNode $pNode, JumpPointNode $endNode, int $movement, Grid $grid) : ?JumpPointNode { if ($movement === MovementType::STRAIGHT) { - return self::jumpStraight($node, $endNode, $grid); + return self::jumpStraight($node, $pNode, $endNode, $grid); } elseif ($movement === MovementType::DIAGONAL) { - return self::jumpDiagonal($node, $endNode, $grid); + return self::jumpDiagonal($node, $pNode, $endNode, $grid); } elseif ($movement === MovementType::DIAGONAL_ONE_OBSTACLE) { - return self::jumpDiagonalOneObstacle($node, $endNode, $grid); + return self::jumpDiagonalOneObstacle($node, $pNode, $endNode, $grid); } - return self::jumpDiagonalNoObstacle($node, $endNode, $grid); + return self::jumpDiagonalNoObstacle($node, $pNode, $endNode, $grid); } /** * Find next jump point * - * @param JumpPointNode $node Node to find jump point from - * @param JumpPointNode $endNode End node to find path to - * @param Grid $grid Grid of the nodes + * @param null|JumpPointNode $node Node to find jump point from + * @param null|JumpPointNode $pNode Parent node + * @param null|JumpPointNode $endNode End node to find path to + * @param Grid $grid Grid of the nodes * * @return null|JumpPointNode * * @since 1.0.0 */ - private static function jumpStraight(JumpPointNode $node, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode + private static function jumpStraight(?JumpPointNode $node, ?JumpPointNode $pNode, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode { - if (!$node->isWalkable()) { + if ($node === null || !$node->isWalkable()) { return null; } $x = $node->getX(); $y = $node->getY(); - $dx = $x - $endNode->getX(); - $dy = $y - $endNode->getY(); + $dx = $x - $pNode->getX(); + $dy = $y - $pNode->getY(); // not always necessary but might be important for the future $node->setTested(true); @@ -514,8 +529,8 @@ class JumpPointSearch implements PathFinderInterface return $node; } - if (self::jumpStraight($grid->getNode($x + 1, $y), $node, $grid) !== null - || self::jumpStraight($grid->getNode($x - 1, $y), $node, $grid) !== null + if (self::jumpStraight($grid->getNode($x + 1, $y), $node, $endNode, $grid) !== null + || self::jumpStraight($grid->getNode($x - 1, $y), $node, $endNode, $grid) !== null ) { return $node; } @@ -523,31 +538,32 @@ class JumpPointSearch implements PathFinderInterface throw new \Exception('invalid movement'); } - return self::jumpStraight($grid->getNode($x + $dx, $y + $dy), $node, $grid); + return self::jumpStraight($grid->getNode($x + $dx, $y + $dy), $node, $endNode, $grid); } /** * Find next jump point * - * @param JumpPointNode $node Node to find jump point from - * @param JumpPointNode $endNode End node to find path to - * @param Grid $grid Grid of the nodes + * @param null|JumpPointNode $node Node to find jump point from + * @param null|JumpPointNode $pNode Parent node + * @param null|JumpPointNode $endNode End node to find path to + * @param Grid $grid Grid of the nodes * * @return null|JumpPointNode * * @since 1.0.0 */ - private static function jumpDiagonal(JumpPointNode $node, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode + private static function jumpDiagonal(?JumpPointNode $node, ?JumpPointNode $pNode, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode { - if (!$node->isWalkable()) { + if ($node === null || !$node->isWalkable()) { return null; } $x = $node->getX(); $y = $node->getY(); - $dx = $x - $endNode->getX(); - $dy = $y - $endNode->getY(); + $dx = $x - $pNode->getX(); + $dy = $y - $pNode->getY(); // not always necessary but might be important for the future $node->setTested(true); @@ -563,12 +579,12 @@ class JumpPointSearch implements PathFinderInterface return $node; } - if (self::jumpDiagonal($grid->getNode($x + $dx, $y), $node, $grid) !== null - || self::jumpDiagonal($grid->getNode($x, $y + $dy), $node, $grid) !== null + if (self::jumpDiagonal($grid->getNode($x + $dx, $y), $node, $endNode, $grid) !== null + || self::jumpDiagonal($grid->getNode($x, $y + $dy), $node, $endNode, $grid) !== null ) { return $node; } - } elseif ($dx !== 0 && $dy === 0) { + } elseif ($dx !== 0) { if (($grid->isWalkable($x + $dx, $y + 1) && !$grid->isWalkable($x, $y + 1)) || ($grid->isWalkable($x + $dx, $y - 1) && !$grid->isWalkable($x, $y - 1)) ) { @@ -582,31 +598,32 @@ class JumpPointSearch implements PathFinderInterface } } - return self::jumpDiagonal($grid->getNode($x + $dx, $y + $dy), $node, $grid); + return self::jumpDiagonal($grid->getNode($x + $dx, $y + $dy), $node, $endNode, $grid); } /** * Find next jump point * - * @param JumpPointNode $node Node to find jump point from - * @param JumpPointNode $endNode End node to find path to - * @param Grid $grid Grid of the nodes + * @param null|JumpPointNode $node Node to find jump point from + * @param null|JumpPointNode $pNode Parent node + * @param null|JumpPointNode $endNode End node to find path to + * @param Grid $grid Grid of the nodes * * @return null|JumpPointNode * * @since 1.0.0 */ - private static function jumpDiagonalOneObstacle(JumpPointNode $node, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode + private static function jumpDiagonalOneObstacle(?JumpPointNode $node, ?JumpPointNode $pNode, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode { - if (!$node->isWalkable()) { + if ($node === null || !$node->isWalkable()) { return null; } $x = $node->getX(); $y = $node->getY(); - $dx = $x - $endNode->getX(); - $dy = $y - $endNode->getY(); + $dx = $x - $pNode->getX(); + $dy = $y - $pNode->getY(); // not always necessary but might be important for the future $node->setTested(true); @@ -622,12 +639,12 @@ class JumpPointSearch implements PathFinderInterface return $node; } - if (self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y), $node, $grid) !== null - || self::jumpDiagonalOneObstacle($grid->getNode($x, $y + $dy), $node, $grid) !== null + if (self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y), $node, $endNode, $grid) !== null + || self::jumpDiagonalOneObstacle($grid->getNode($x, $y + $dy), $node, $endNode, $grid) !== null ) { return $node; } - } elseif ($dx !== 0 && $dy === 0) { + } elseif ($dx !== 0) { if (($grid->isWalkable($x + $dx, $y + 1) && !$grid->isWalkable($x, $y + 1)) || ($grid->isWalkable($x + $dx, $y - 1) && !$grid->isWalkable($x, $y - 1)) ) { @@ -642,7 +659,7 @@ class JumpPointSearch implements PathFinderInterface } if ($grid->isWalkable($x + $dx, $y) || $grid->isWalkable($x, $y + $dy)) { - return self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y + $dy), $node, $grid); + return self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y + $dy), $node, $endNode, $grid); } return null; @@ -651,25 +668,26 @@ class JumpPointSearch implements PathFinderInterface /** * Find next jump point * - * @param JumpPointNode $node Node to find jump point from - * @param JumpPointNode $endNode End node to find path to - * @param Grid $grid Grid of the nodes + * @param null|JumpPointNode $node Node to find jump point from + * @param null|JumpPointNode $pNode Parent node + * @param null|JumpPointNode $endNode End node to find path to + * @param Grid $grid Grid of the nodes * * @return null|JumpPointNode * * @since 1.0.0 */ - private static function jumpDiagonalNoObstacle(JumpPointNode $node, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode + private static function jumpDiagonalNoObstacle(?JumpPointNode $node, ?JumpPointNode $pNode, JumpPointNode $endNode, Grid $grid) : ?JumpPointNode { - if (!$node->isWalkable()) { + if ($node === null || !$node->isWalkable()) { return null; } $x = $node->getX(); $y = $node->getY(); - $dx = $x - $endNode->getX(); - $dy = $y - $endNode->getY(); + $dx = $x - $pNode->getX(); + $dy = $y - $pNode->getY(); // not always necessary but might be important for the future $node->setTested(true); @@ -679,18 +697,18 @@ class JumpPointSearch implements PathFinderInterface } 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 + if (self::jumpDiagonalNoObstacle($grid->getNode($x + $dx, $y), $node, $endNode, $grid) !== null + || self::jumpDiagonalNoObstacle($grid->getNode($x, $y + $dy), $node, $endNode, $grid) !== null ) { return $node; } - } elseif ($dx !== 0 && $dy === 0) { + } elseif ($dx !== 0) { if (($grid->isWalkable($x, $y - 1) && !$grid->isWalkable($x - $dx, $y - 1)) || ($grid->isWalkable($x, $y + 1) && !$grid->isWalkable($x - $dx, $y + 1)) ) { return $node; } - } elseif ($dx === 0 && $dy !== 0) { + } elseif ($dy !== 0) { if (($grid->isWalkable($x - 1, $y) && !$grid->isWalkable($x - 1, $y - $dy)) || ($grid->isWalkable($x + 1, $y) && !$grid->isWalkable($x + 1, $y - $dy)) ) { @@ -699,7 +717,7 @@ class JumpPointSearch implements PathFinderInterface } if ($grid->isWalkable($x + $dx, $y) || $grid->isWalkable($x, $y + $dy)) { - return self::jumpDiagonalNoObstacle($grid->getNode($x + $dx, $y + $dy), $node, $grid); + return self::jumpDiagonalNoObstacle($grid->getNode($x + $dx, $y + $dy), $node, $endNode, $grid); } return null; diff --git a/Algorithm/PathFinding/Path.php b/Algorithm/PathFinding/Path.php index 4978dfa65..c29f37720 100644 --- a/Algorithm/PathFinding/Path.php +++ b/Algorithm/PathFinding/Path.php @@ -32,22 +32,6 @@ class Path */ public array $nodes = []; - /** - * Weight/cost of the total path - * - * @var float - * @since 1.0.0 - */ - private float $weight = 0.0; - - /** - * Distance of the total path - * - * @var float - * @since 1.0.0 - */ - private float $distance = 0.0; - /** * Grid this path belongs to * @@ -56,6 +40,22 @@ class Path */ private Grid $grid; + /** + * Nodes in the path + * + * @var Nodes[] + * @since 1.0.0 + */ + private array $expandedNodes = []; + + /** + * Path length + * + * @var float + * @since 1.0.0 + */ + private float $length = 0.0; + /** * Cosntructor. * @@ -83,38 +83,76 @@ class Path } /** - * Fill all nodes in bettween + * Get the path length + * + * @return float + * + * @since 1.0.0 + */ + public function getLength() : float + { + $n = \count($this->nodes); + + $dist = 0.0; + + for ($i = 1; $i < $n; ++$i) { + $dx = $this->nodes[$i - 1]->getX() - $this->nodes[$i]->getX(); + $dy = $this->nodes[$i - 1]->getY() - $this->nodes[$i]->getY(); + + $dist += \sqrt($dx * $dx + $dy * $dy); + } + + return $dist; + } + + /** + * Get the incomplete node path + * + * @return Node[] + * + * @since 1.0.0 + */ + public function getPath() : array + { + return $this->nodes; + } + + /** + * Get the complete node path * * The path may only contain the jump points or pivot points. * In order to get every node it needs to be expanded. * - * @return array + * @return Node[] * * @since 1.0.0 */ public function expandPath() : array { - $reverse = \array_reverse($this->nodes); - $length = \count($reverse); + if (empty($this->expandedNodes)) { + //$reverse = \array_reverse($this->nodes); + $reverse = $this->nodes; + $length = \count($reverse); - if ($length < 2) { - return $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); + + $expanded = \array_merge($expanded, $interpolated); + } + + $expanded[] = $reverse[$length - 1]; + $this->expandedNodes = $expanded; } - $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; + return $this->expandedNodes; } /** @@ -141,6 +179,9 @@ class Path $node = $node1; $err = $dx - $dy; + $x0 = $node->getX(); + $y0 = $node->getY(); + $line = []; while (true) { $line[] = $node; @@ -150,17 +191,15 @@ class Path } $e2 = 2 * $err; - $x0 = 0; if ($e2 > -$dy) { $err -= $dy; - $x0 = $node->getX() + $sx; + $x0 = $x0 + $sx; } - $y0 = 0; if ($e2 < $dx) { $err += $dx; - $y0 = $node->getY() + $sy; + $y0 = $y0 + $sy; } $node = $this->grid->getNode($x0, $y0); diff --git a/tests/Algorithm/PathFinding/AStarTest.php b/tests/Algorithm/PathFinding/AStarTest.php new file mode 100644 index 000000000..8997e2841 --- /dev/null +++ b/tests/Algorithm/PathFinding/AStarTest.php @@ -0,0 +1,225 @@ + $row) { + echo "["; + + foreach ($row as $x => $value) { + if ($value === 9) { + echo "\e[0;31m" . $value . "\e[0m, "; + } elseif ($value === 3) { + echo "\e[1;32m" . $value . "\e[0m, "; + } else { + echo "" . $value . ", "; + } + } + + echo "],\n"; + } + } + + public function testPathFindingDiagonal() : void + { + $grid = Grid::createGridFromArray($this->gridArray, AStarNode::class); + $path = AStar::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::DIAGONAL + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(20.55634, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 0, 3, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, ], + [0, 3, 0, 9, 0, 0, 3, 9, 0, 3, 9, 9, 9, 9, 0, ], + [0, 3, 0, 9, 0, 3, 0, 9, 0, 0, 3, 3, 0, 0, 0, ], + [0, 0, 3, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 0, 0, 3, 0, 9, 9, 9, 0, 0, 9, 3, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } + + public function testPathFindingStraight() : void + { + $grid = Grid::createGridFromArray($this->gridArray, AStarNode::class); + $path = AStar::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::STRAIGHT + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(27.0, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 3, 3, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, ], + [0, 3, 0, 9, 0, 3, 0, 9, 3, 3, 9, 9, 9, 9, 0, ], + [0, 3, 0, 9, 0, 3, 0, 9, 0, 3, 3, 3, 3, 0, 0, ], + [0, 3, 0, 9, 3, 3, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 3, 3, 3, 3, 9, 9, 9, 0, 0, 9, 3, 3, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } + + public function testPathFindingDiagonalOneObstacle() : void + { + $grid = Grid::createGridFromArray($this->gridArray, AStarNode::class); + $path = AStar::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::DIAGONAL_ONE_OBSTACLE + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(20.55634, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 0, 3, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, ], + [0, 3, 0, 9, 0, 0, 3, 9, 0, 3, 9, 9, 9, 9, 0, ], + [0, 3, 0, 9, 0, 3, 0, 9, 0, 0, 3, 3, 0, 0, 0, ], + [0, 0, 3, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 0, 0, 3, 0, 9, 9, 9, 0, 0, 9, 3, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } + + public function testPathFindingDiagonalNoObstacle() : void + { + $grid = Grid::createGridFromArray($this->gridArray, AStarNode::class); + $path = AStar::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::DIAGONAL_NO_OBSTACLE + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(24.07107, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 3, 0, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 0, 3, 3, 3, 0, 0, 0, 0, 0, 0, ], + [0, 3, 0, 9, 0, 3, 0, 9, 0, 3, 9, 9, 9, 9, 0, ], + [0, 3, 0, 9, 3, 0, 0, 9, 0, 3, 3, 3, 3, 0, 0, ], + [0, 3, 0, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 0, 3, 3, 3, 9, 9, 9, 0, 0, 9, 3, 3, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } +} diff --git a/tests/Algorithm/PathFinding/JumpPointSearchTest.php b/tests/Algorithm/PathFinding/JumpPointSearchTest.php index d84d7f852..10f484f6e 100644 --- a/tests/Algorithm/PathFinding/JumpPointSearchTest.php +++ b/tests/Algorithm/PathFinding/JumpPointSearchTest.php @@ -47,13 +47,179 @@ class JumpPointSearchTest extends \PHPUnit\Framework\TestCase [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0,], ]; - public function testPathFinding() : void + private function renderMaze(array $grid) : void + { + echo "\n"; + foreach ($grid as $y => $row) { + echo "["; + + foreach ($row as $x => $value) { + if ($value === 9) { + echo "\e[0;31m" . $value . "\e[0m, "; + } elseif ($value === 3) { + echo "\e[1;32m" . $value . "\e[0m, "; + } else { + echo "" . $value . ", "; + } + } + + echo "],\n"; + } + } + + public function testPathFindingDiagonal() : void { $grid = Grid::createGridFromArray($this->gridArray, JumpPointNode::class); - /*$path = JumpPointSearch::findPath( + $path = JumpPointSearch::findPath( 2, 5, 11, 11, $grid, HeuristicType::EUCLIDEAN, MovementType::DIAGONAL - );*/ + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(20.55634, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 3, 0, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 3, 9, 0, 0, 3, 9, 3, 0, 9, 9, 9, 9, 0, ], + [0, 0, 3, 9, 0, 3, 0, 9, 0, 3, 3, 3, 0, 0, 0, ], + [0, 0, 3, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 0, 0, 3, 0, 9, 9, 9, 0, 0, 9, 3, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } + + public function testPathFindingStraight() : void + { + $grid = Grid::createGridFromArray($this->gridArray, JumpPointNode::class); + $path = JumpPointSearch::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::STRAIGHT + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(27.0, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 3, 3, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 3, 3, 3, 3, 0, 0, 0, 0, 0, 0, ], + [0, 3, 0, 9, 3, 3, 0, 9, 3, 0, 9, 9, 9, 9, 0, ], + [0, 3, 0, 9, 3, 0, 0, 9, 3, 3, 3, 3, 3, 0, 0, ], + [0, 3, 0, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 3, 3, 3, 3, 9, 9, 9, 0, 0, 9, 3, 3, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } + + public function testPathFindingDiagonalOneObstacle() : void + { + $grid = Grid::createGridFromArray($this->gridArray, JumpPointNode::class); + $path = JumpPointSearch::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::DIAGONAL_ONE_OBSTACLE + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(20.55634, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 3, 0, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 0, 0, 3, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 3, 9, 0, 0, 3, 9, 3, 0, 9, 9, 9, 9, 0, ], + [0, 0, 3, 9, 0, 3, 0, 9, 0, 3, 3, 3, 0, 0, 0, ], + [0, 0, 3, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 0, 0, 3, 0, 9, 9, 9, 0, 0, 9, 3, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); + } + + public function testPathFindingDiagonalNoObstacle() : void + { + $grid = Grid::createGridFromArray($this->gridArray, JumpPointNode::class); + $path = JumpPointSearch::findPath( + 2, 5, + 11, 11, + $grid, HeuristicType::EUCLIDEAN, MovementType::DIAGONAL_NO_OBSTACLE + ); + + $expanded = $path->expandPath(); + + foreach ($expanded as $node) { + $this->gridArray[$node->getY()][$node->getX()] = 3; + } + + // Visualization of path + //$this->renderMaze($this->gridArray); + + self::assertEqualsWithDelta(22.89949, $path->getLength(), 0.001); + + self::assertEquals([ + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 9, 0, 0, 0, 0, ], + [0, 0, 0, 0, 9, 9, 9, 9, 9, 0, 9, 0, 0, 0, 0, ], + [0, 0, 3, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9, ], + [0, 3, 0, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0, ], + [0, 3, 9, 9, 9, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, ], + [0, 3, 0, 9, 0, 0, 3, 9, 0, 3, 9, 9, 9, 9, 0, ], + [0, 0, 3, 9, 0, 3, 0, 9, 0, 0, 3, 3, 3, 0, 0, ], + [0, 0, 3, 9, 3, 0, 0, 9, 0, 0, 9, 9, 3, 0, 0, ], + [0, 0, 3, 3, 3, 9, 9, 9, 0, 0, 9, 3, 3, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 9, 9, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + [0, 0, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, ], + ], $this->gridArray); } }