fix and complete path finding algorithm

Algorithm/PathFinding/BreadthFirstSearch.php

@@ -1 +0,0 @@
-

Algorithm/PathFinding/Dijkstra.php

@@ -1 +0,0 @@
-

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 isset($this->nodes[$y]) && isset($this->nodes[$y][$x]) && $this->nodes[$y][$x]->isWalkable();
-            return false;
-        }
-
-        return true;
     }
 
     /**
@@ -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;

Algorithm/PathFinding/IDAStar.php

@@ -1 +0,0 @@
-

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) {
+        for ($i = 0; $i < $neighborsLength; ++$i) {
-            $neighbor  = $neighbors[$i]; // todo: needs to be Node!!!
+            $neighbor = $neighbors[$i];
-            $jumpPoint = self::jump($neighbor, $node, $movement, $grid);
+
+            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 null|JumpPointNode $node     Node to find jump point from
-     * @param JumpPointNode $endNode  End node to find path to
+     * @param null|JumpPointNode $pNode    Parent node
-     * @param int           $movement Movement type
+     * @param null|JumpPointNode $endNode  End node to find path to
-     * @param Grid          $grid     Grid of the nodes
+     * @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 null|JumpPointNode $node    Node to find jump point from
-     * @param JumpPointNode $endNode End node to find path to
+     * @param null|JumpPointNode $pNode   Parent node
-     * @param Grid          $grid    Grid of the nodes
+     * @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();
+        $dx = $x - $pNode->getX();
-        $dy = $y - $endNode->getY();
+        $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
+            if (self::jumpStraight($grid->getNode($x + 1, $y), $node, $endNode, $grid) !== null
-                || self::jumpStraight($grid->getNode($x - 1, $y), $node, $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 null|JumpPointNode $node    Node to find jump point from
-     * @param JumpPointNode $endNode End node to find path to
+     * @param null|JumpPointNode $pNode   Parent node
-     * @param Grid          $grid    Grid of the nodes
+     * @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();
+        $dx = $x - $pNode->getX();
-        $dy = $y - $endNode->getY();
+        $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
+            if (self::jumpDiagonal($grid->getNode($x + $dx, $y), $node, $endNode, $grid) !== null
-                || self::jumpDiagonal($grid->getNode($x, $y + $dy), $node, $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 null|JumpPointNode $node    Node to find jump point from
-     * @param JumpPointNode $endNode End node to find path to
+     * @param null|JumpPointNode $pNode   Parent node
-     * @param Grid          $grid    Grid of the nodes
+     * @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();
+        $dx = $x - $pNode->getX();
-        $dy = $y - $endNode->getY();
+        $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
+            if (self::jumpDiagonalOneObstacle($grid->getNode($x + $dx, $y), $node, $endNode, $grid) !== null
-                || self::jumpDiagonalOneObstacle($grid->getNode($x, $y + $dy), $node, $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 null|JumpPointNode $node    Node to find jump point from
-     * @param JumpPointNode $endNode End node to find path to
+     * @param null|JumpPointNode $pNode   Parent node
-     * @param Grid          $grid    Grid of the nodes
+     * @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();
+        $dx = $x - $pNode->getX();
-        $dy = $y - $endNode->getY();
+        $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
+            if (self::jumpDiagonalNoObstacle($grid->getNode($x + $dx, $y), $node, $endNode, $grid) !== null
-                || self::jumpDiagonalNoObstacle($grid->getNode($x, $y + $dy), $node, $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;

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);
+        if (empty($this->expandedNodes)) {
-        $length  = \count($reverse);
+            //$reverse = \array_reverse($this->nodes);
+            $reverse = $this->nodes;
+            $length  = \count($reverse);
 
-        if ($length < 2) {
+            if ($length < 2) {
-            return $reverse;
+                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 = [];
+        return $this->expandedNodes;
-        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;
     }
 
     /**
@@ -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);

tests/Algorithm/PathFinding/AStarTest.php

@@ -0,0 +1,225 @@
+<?php
+/**
+ * Orange Management
+ *
+ * PHP Version 7.4
+ *
+ * @package   tests
+ * @copyright Dennis Eichhorn
+ * @license   OMS License 1.0
+ * @version   1.0.0
+ * @link      https://orange-management.org
+ */
+declare(strict_types=1);
+
+namespace phpOMS\tests\Algorithm\PathFinding;
+
+use phpOMS\Algorithm\PathFinding\Grid;
+use phpOMS\Algorithm\PathFinding\MovementType;
+use phpOMS\Algorithm\PathFinding\HeuristicType;
+use phpOMS\Algorithm\PathFinding\AStarNode;
+use phpOMS\Algorithm\PathFinding\AStar;
+
+require_once __DIR__ . '/../../Autoloader.php';
+
+/**
+ * @testdox phpOMS\tests\Algorithm\PathFinding: jump point search test
+ *
+ * @internal
+ */
+class AStarTest extends \PHPUnit\Framework\TestCase
+{
+    private array $gridArray = [
+        [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, 1, 0, 9, 0, 0, 0, 0, 0, 9, 0, 9, 9, 9,],
+        [0, 0, 0, 0, 9, 0, 0, 9, 9, 9, 9, 0, 0, 0, 0,],
+        [0, 0, 9, 9, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,],
+        [0, 0, 0, 9, 0, 0, 0, 9, 0, 0, 9, 9, 9, 9, 0,],
+        [0, 0, 0, 9, 0, 0, 0, 9, 0, 0, 0, 0, 0, 0, 0,],
+        [0, 0, 0, 9, 0, 0, 0, 9, 0, 0, 9, 9, 0, 0, 0,],
+        [0, 0, 0, 0, 0, 9, 9, 9, 0, 0, 9, 2, 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,],
+    ];
+
+    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, 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);
+    }
+}

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