phpOMS/Algorithm/PathFinding/JumpPointSearch.php
2019-10-06 17:41:41 +02:00

568 lines
19 KiB
PHP

<?php
/**
* Orange Management
*
* PHP Version 7.4
*
* @package phpOMS\Algorithm\PathFinding
* @copyright Dennis Eichhorn
* @license OMS License 1.0
* @version 1.0.0
* @link https://orange-management.org
*/
declare(strict_types=1);
namespace phpOMS\Algorithm\PathFinding;
/**
* Perform path finding.
*
* @package phpOMS\Algorithm\PathFinding
* @license OMS License 1.0
* @link https://orange-management.org
* @since 1.0.0
*/
class JumpPointSearch implements PathFinderInterface
{
public static function findPath(
int $startX, int $startY,
int $endX, int $endY,
Grid $grid,
int $heuristic, int $movement
) : Path {
$startNode = $grid->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() - $node2->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::metric($node->getCoordinates(), $jumpPoint->getCoordinates(), HeuristicType::OCTILE);
$ng = $node->getG() + $d;
if (!$jumpPoint->isOpened() || $ng < $jumpPoint->getG()) {
$jumpPoint->setG($ng);
$jumpPoint->setH($jumpPoint->getH() ?? Heuristic::metric($jumpPoint->getCoordinates(), $endNode->getCoordinates(), $metric));
$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;
}
}