├── README.md └── lib └── pathfinding ├── Pathfinder.php ├── block ├── BlockAStar.php └── evaluator │ ├── IdentifierEvaluator.php │ └── StairEvaluator.php └── path ├── BlockPath.php ├── PathResult.php └── node ├── BlockPathNode.php ├── TransitionType.php └── weighted ├── SortedWeightedNodeList.php └── WeightedNode.php /README.md: -------------------------------------------------------------------------------- 1 | 2 | # Pathfinding A* 3 | 4 | This is an A* algorithm aimed at developers for plugins on pocketmine. 5 | 6 | The algorithm is just a direct rewrite of a version made for Spigot where it can be found on the author section. 7 | 8 | ## Usage 9 | 10 | ### Finding path beetween nearby entity and player. 11 | 12 | ```php 13 | use lib\pathfinding\Pathfinder; 14 | 15 | $initialPath = $player->getPosition(); 16 | $entity = $sender->getWorld()->getNearestEntity($initialPath, 50); 17 | 18 | if($entity instanceof Living) { 19 | try { 20 | $start = Position::fromObject( 21 | (clone $initialPath)->subtract(0, -0.5, 0), 22 | $sender->getWorld() 23 | ); 24 | $end = Position::fromObject( 25 | (clone $entity->getPosition())->subtract(0, -0.5, 0), 26 | $sender->getWorld() 27 | ); 28 | $pathResult = Pathfinder::find($start, $end); 29 | $player->sendMessage($pathResult->getDiagnose()); 30 | } catch (\Exception $e) { 31 | $player->sendMessage('Error! Because, ' . $e->getMessage()); 32 | } 33 | } 34 | ``` 35 | 36 | ### Using path result 37 | ```php 38 | $pathResult = Pathfinder::find($start, $end); 39 | $world = $player->getWorld(); 40 | if(!$pathResult->haveFailed()) { 41 | $nodesPath = $pathResult->getPath()->getNodes(); 42 | $count = count($nodesPath); 43 | for($i = 0; $i < $count; $i++) { 44 | $node = $nodesPath[$i]; 45 | $block = VanillaBlocks::IRON(); 46 | // check if is initial or final path 47 | if($i == 0 || $i == $count-1) { 48 | $block = VanillaBlocks::EMERALD(); 49 | } 50 | $world->setBlockAt($node->x, $node->y, $node->z, $block); 51 | } 52 | } 53 | ``` 54 | 55 | 56 | 57 | ## Authors 58 | 59 | - [@guinhx](https://github.com/guinhx) (PMMP Version) 60 | - [@domisum](https://github.com/domisum/CompitumLib) (Spigot Version) 61 | -------------------------------------------------------------------------------- /lib/pathfinding/Pathfinder.php: -------------------------------------------------------------------------------- 1 | setCanUseDiagonalMovement($canUseDiagonalMovement); 23 | $pathfinder->setCanUseLadders($canUseLadders); 24 | $pathfinder->setHeuristicImportance($heuristicImportance); 25 | $pathfinder->findPath(); 26 | $result = new PathResult(); 27 | $result->setDiagnose($pathfinder->getDiagnose()); 28 | if(!$pathfinder->pathFound()) 29 | { 30 | $result->setFailure($pathfinder->getFailure()); 31 | } else { 32 | $result->setPath($pathfinder->getPath()); 33 | } 34 | return $result; 35 | } 36 | } -------------------------------------------------------------------------------- /lib/pathfinding/block/BlockAStar.php: -------------------------------------------------------------------------------- 1 | unvisitedNodes = new SortedWeightedNodeList(); 45 | } 46 | 47 | public function pathFound(): bool 48 | { 49 | return !is_null($this->path); 50 | } 51 | 52 | public function getPath(): BlockPath 53 | { 54 | return $this->path; 55 | } 56 | 57 | public function getFailure(): string 58 | { 59 | return $this->failure; 60 | } 61 | 62 | private function getNanoDuration(): float 63 | { 64 | return $this->pathfindingEndNano - $this->pathfindingStartNano; 65 | } 66 | 67 | private function getMsDuration(): float 68 | { 69 | return round($this->getNanoDuration(), 2); 70 | } 71 | 72 | public function getDiagnose(): string 73 | { 74 | $found = $this->pathFound() ? 'y' : 'n'; 75 | $diagnose = "found={$found}, "; 76 | $visited = count($this->visitedNodes); 77 | $diagnose .= "visitedNodes={$visited}, "; 78 | $diagnose .= "unvisitedNodes={$this->unvisitedNodes->getSize()}, "; 79 | $diagnose .= "retryCount={$this->retryCount}, "; 80 | $diagnose .= "durationMs={$this->getMsDuration()}, "; 81 | return $diagnose; 82 | } 83 | 84 | public function setHeuristicImportance(float $heuristicImportance): void 85 | { 86 | $this->heuristicImportance = $heuristicImportance; 87 | } 88 | 89 | public function setCanUseDiagonalMovement(bool $canUseDiagonalMovement): void 90 | { 91 | $this->canUseDiagonalMovement = $canUseDiagonalMovement; 92 | } 93 | 94 | public function setCanUseLadders(bool $canUseLadders): void 95 | { 96 | $this->canUseLadders = $canUseLadders; 97 | } 98 | 99 | public function findPath(): void 100 | { 101 | if($this->pathfindingStartNano == 0) $this->pathfindingStartNano = microtime(true); 102 | if($this->startPosition->getWorld()->getDisplayName() != $this->endPosition->getWorld()->getDisplayName()) 103 | throw new \InvalidArgumentException("The start and the end location are not in the same world!"); 104 | $startNode = new BlockPathNode( 105 | $this->startPosition->getFloorX(), 106 | $this->startPosition->getFloorY(), 107 | $this->startPosition->getFloorZ() 108 | ); 109 | $startNode->setParent(null, TransitionType::WALK, 0); 110 | $this->endNode = new BlockPathNode( 111 | $this->endPosition->getFloorX(), 112 | $this->endPosition->getFloorY(), 113 | $this->endPosition->getFloorZ() 114 | ); 115 | $this->unvisitedNodes->addSorted($startNode); 116 | 117 | $this->visitNodes(); 118 | 119 | if($this->endNode->getParent() != null || $startNode->equals($this->endNode)){ 120 | $this->path = new BlockPath($this->endNode); 121 | } 122 | else 123 | { 124 | // this looks through the provided options and checks if an ability of the pathfinder is deactivated, 125 | // if so it activates it and reruns the pathfinding. if there are no other options available, it returns 126 | if($this->retryCount >= $this->maxRetry) { 127 | $this->failure = "Max. retry count reached! Can't find path for this target."; 128 | return; 129 | } 130 | $this->retry(); 131 | } 132 | 133 | $this->pathfindingEndNano = microtime(true); 134 | } 135 | 136 | private function visitNodes(): void 137 | { 138 | while(true) 139 | { 140 | if($this->unvisitedNodes->getSize() == 0) 141 | { 142 | // no unvisited nodes left, nowhere else to go ... 143 | $this->failure = "No unvisited nodes left"; 144 | break; 145 | } 146 | 147 | if(count($this->visitedNodes) >= $this->maxNodeVisits) 148 | { 149 | // reached limit of nodes to search 150 | $this->failure = "Number of nodes visited exceeds maximum"; 151 | break; 152 | } else { 153 | print_r('Visited Nodes > ' . count($this->visitedNodes) . "\n"); 154 | } 155 | 156 | $nodeToVisit = $this->unvisitedNodes->getAndRemoveFirst(); 157 | if(is_null($nodeToVisit)) { 158 | $this->failure = "The node to visit is null!"; 159 | break; 160 | } 161 | $this->visitedNodes[] = $nodeToVisit; 162 | 163 | // pathing reached end node 164 | if($this->isTargetReached($nodeToVisit)) 165 | { 166 | $this->endNode = $nodeToVisit; 167 | break; 168 | } 169 | 170 | $this->visitNode($nodeToVisit); 171 | } 172 | } 173 | 174 | private function isTargetReached(?BlockPathNode $nodeToVisit): bool 175 | { 176 | return $nodeToVisit->equals($this->endNode); 177 | } 178 | 179 | private function visitNode(?BlockPathNode $node): void 180 | { 181 | $this->lookForWalkableNodes($node); 182 | 183 | if($this->useLadders) 184 | $this->lookForLadderNodes($node); 185 | } 186 | 187 | private function lookForWalkableNodes(?BlockPathNode $node): void 188 | { 189 | for($dX = -1; $dX <= 1; $dX++) 190 | for($dZ = -1; $dZ <= 1; $dZ++) 191 | for($dY = -1; $dY <= 1; $dY++) 192 | $this->validateNodeOffset($node, $dX, $dY, $dZ); 193 | } 194 | 195 | private function validateNodeOffset(?BlockPathNode $node, int $dX, int $dY, int $dZ): void 196 | { 197 | if($dX == 0 && $dY == 0 && $dZ == 0) 198 | return; 199 | 200 | // prevent diagonal movement if specified 201 | if(!$this->moveDiagonally && $dX * $dZ != 0) 202 | return; 203 | 204 | // prevent diagonal movement at the same time as moving up and down 205 | if($dX * $dZ != 0 && $dY != 0) 206 | return; 207 | 208 | $newNode = new BlockPathNode($node->x + $dX, $node->y + $dY, $node->z + $dZ); 209 | 210 | if($this->doesNodeAlreadyExist($newNode)) 211 | return; 212 | 213 | // check if player can stand at new node 214 | if(!$this->isValid($newNode)) 215 | return; 216 | 217 | // check if the diagonal movement is not prevented by blocks to the side 218 | if($dX * $dZ != 0 && !$this->isDiagonalMovementPossible($node, $dX, $dZ)) 219 | return; 220 | 221 | // check if the player hits his head when going up/down 222 | if($dY == 1 && !$this->isBlockUnobstructed($node->getPosition($this->startPosition->getWorld())->add(0, 2, 0))) 223 | return; 224 | 225 | 226 | if($dY == -1 && !$this->isBlockUnobstructed($newNode->getPosition($this->startPosition->getWorld())->add(0, 2, 0))) 227 | return; 228 | 229 | // get transition type (walk up stairs, jump up blocks) 230 | $transitionType = TransitionType::WALK; 231 | if($dY == 1) 232 | { 233 | $isStair = StairEvaluator::isStair($newNode, $this->startPosition->getWorld()); 234 | if(!$isStair) 235 | $transitionType = TransitionType::JUMP; 236 | } 237 | 238 | 239 | // calculate weight 240 | // TODO punish 90° turns 241 | $sumAbs = abs($dX) + abs($dY) + abs($dZ); 242 | $weight = 1; 243 | if($sumAbs == 2) 244 | $weight = 1.41; 245 | else if($sumAbs == 3) 246 | $weight = 1.73; 247 | 248 | // punish jumps to favor stair climbing 249 | if($transitionType == TransitionType::JUMP) 250 | $weight += 0.5; 251 | 252 | // actually add the node to the pool 253 | $newNode->setParent($node, $transitionType, $weight); 254 | $this->addNode($newNode); 255 | } 256 | 257 | private function doesNodeAlreadyExist(BlockPathNode $newNode): bool 258 | { 259 | if(in_array($newNode, $this->visitedNodes)) 260 | return true; 261 | 262 | return $this->unvisitedNodes->contains($newNode); 263 | } 264 | 265 | private function isValid(BlockPathNode $newNode): bool 266 | { 267 | return $this->canStandAt($newNode->getPosition($this->startPosition->getWorld())); 268 | } 269 | 270 | private function isDiagonalMovementPossible(?BlockPathNode $node, int $dX, int $dZ): bool 271 | { 272 | if(!$this->isUnobstructed((clone $node->getPosition($this->startPosition->getWorld()))->add($dX, 0, 0))) 273 | return false; 274 | return $this->isUnobstructed((clone $node->getPosition($this->startPosition->getWorld()))->add(0, 0, $dZ)); 275 | } 276 | 277 | private function isBlockUnobstructed(Vector3 $position): bool 278 | { 279 | $block = $this->startPosition->getWorld()->getBlock($position); 280 | return IdentifierEvaluator::canStandIn($block); 281 | } 282 | 283 | private function addNode(BlockPathNode $newNode): void 284 | { 285 | $newNode->setHeuristicWeight($this->getHeuristicWeight($newNode) * $this->heuristicImportance); 286 | $this->unvisitedNodes->addSorted($newNode); 287 | } 288 | 289 | private function lookForLadderNodes(?BlockPathNode $node): void 290 | { 291 | $feetPosition = $node->getPosition($this->startPosition->getWorld()); 292 | 293 | for($dY = -1; $dY <= 1; $dY++) 294 | { 295 | $position = (clone $feetPosition)->add(0, $dY, 0); 296 | $block = $this->startPosition->getWorld()->getBlock($position); 297 | if(!$block instanceof Ladder) 298 | continue; 299 | 300 | $newNode = new BlockPathNode($node->x, $node->y + $dY, $node->z); 301 | $newNode->setParent($node, TransitionType::CLIMB, self::CLIMBING_EXPENSE); 302 | 303 | if($this->doesNodeAlreadyExist($newNode)) 304 | continue; 305 | 306 | $this->addNode($newNode); 307 | } 308 | } 309 | 310 | private function getHeuristicWeight(BlockPathNode $node): float 311 | { 312 | return $this->getEuclideanDistance($node); 313 | } 314 | 315 | private function canStandAt(Position $feetPosition): bool 316 | { 317 | if(!$this->isUnobstructed($feetPosition)) { 318 | return false; 319 | } 320 | $block = $this->startPosition->getWorld()->getBlock((clone $feetPosition)->add(0, -1, 0)); 321 | return IdentifierEvaluator::canStandOn($block); 322 | } 323 | 324 | private function isUnobstructed(Vector3 $feetLocation): bool 325 | { 326 | if(!$this->isBlockUnobstructed($feetLocation)) 327 | return false; 328 | return $this->isBlockUnobstructed((clone$feetLocation)->add(0, 1, 0)); 329 | } 330 | 331 | private function getEuclideanDistance(BlockPathNode $node): float 332 | { 333 | $dX = $this->endNode->x - $node->x; 334 | $dY = $this->endNode->y - $node->y; 335 | $dZ = $this->endNode->z - $node->z; 336 | return sqrt($dX * $dX + $dY * $dY+ $dZ * $dZ); 337 | } 338 | 339 | private function retry(): void 340 | { 341 | if($this->canUseDiagonalMovement && !$this->moveDiagonally) 342 | { 343 | $this->moveDiagonally = true; 344 | } 345 | 346 | if($this->canUseLadders && !$this->useLadders) 347 | { 348 | $this->useLadders = true; 349 | } 350 | $this->retryCount++; 351 | $this->reset(); 352 | $this->findPath(); 353 | } 354 | 355 | private function reset(): void 356 | { 357 | $this->endNode = null; 358 | 359 | $this->unvisitedNodes->clear(); 360 | $this->visitedNodes = []; 361 | 362 | $this->path = null; 363 | $this->failure = ""; 364 | } 365 | } -------------------------------------------------------------------------------- /lib/pathfinding/block/evaluator/IdentifierEvaluator.php: -------------------------------------------------------------------------------- 1 | getId() == BlockLegacyIds::FENCE) return false; 14 | if($block->getId() == BlockLegacyIds::SIGN_POST) return false; 15 | if($block instanceof Slab) return true; 16 | if($block instanceof Carpet) return true; 17 | return $block->isSolid(); 18 | } 19 | 20 | public static function canStandIn(Block $block): bool 21 | { 22 | if($block->getId() == BlockLegacyIds::SIGN_POST) return true; 23 | return !$block->isSolid(); 24 | } 25 | } -------------------------------------------------------------------------------- /lib/pathfinding/block/evaluator/StairEvaluator.php: -------------------------------------------------------------------------------- 1 | getPosition($world)->add(0, -1, 0); 14 | $stairBlock = $world->getBlock($stairPosition); 15 | 16 | if($stairBlock instanceof Stair || $stairBlock instanceof Slab) { 17 | return true; 18 | } 19 | return false; 20 | } 21 | } -------------------------------------------------------------------------------- /lib/pathfinding/path/BlockPath.php: -------------------------------------------------------------------------------- 1 | generatePath($endNode); 14 | } 15 | 16 | private function generatePath(BlockPathNode $endNode): void 17 | { 18 | $currentNode = $endNode; 19 | while($currentNode != null) 20 | { 21 | $this->nodes[] = $currentNode; 22 | $currentNode = $currentNode->getParent(); 23 | } 24 | $this->nodes = array_reverse($this->nodes); 25 | } 26 | 27 | /** 28 | * @return BlockPathNode[] 29 | */ 30 | public function getNodes(): array 31 | { 32 | return $this->nodes; 33 | } 34 | 35 | } -------------------------------------------------------------------------------- /lib/pathfinding/path/PathResult.php: -------------------------------------------------------------------------------- 1 | diagnose; 16 | } 17 | 18 | /** 19 | * @param string $diagnose 20 | */ 21 | public function setDiagnose(string $diagnose): void 22 | { 23 | $this->diagnose = $diagnose; 24 | } 25 | 26 | /** 27 | * @return bool 28 | */ 29 | public function haveFailed(): bool 30 | { 31 | return strlen($this->failure) > 0; 32 | } 33 | 34 | /** 35 | * @return string 36 | */ 37 | public function getFailure(): string 38 | { 39 | return $this->failure; 40 | } 41 | 42 | /** 43 | * @param string $failure 44 | */ 45 | public function setFailure(string $failure): void 46 | { 47 | $this->failure = $failure; 48 | } 49 | 50 | /** 51 | * @return BlockPath|null 52 | */ 53 | public function getPath(): ?BlockPath 54 | { 55 | return $this->path; 56 | } 57 | 58 | /** 59 | * @param BlockPath|null $path 60 | */ 61 | public function setPath(?BlockPath $path): void 62 | { 63 | $this->path = $path; 64 | } 65 | } -------------------------------------------------------------------------------- /lib/pathfinding/path/node/BlockPathNode.php: -------------------------------------------------------------------------------- 1 | x == $this->x && $other->y == $this->y && $other->z == $this->z; 29 | } 30 | 31 | public function hashCode(): int 32 | { 33 | $hash = 0; 34 | $hash |= ($this->x%4096)<<20; // 12 bits long, in [0;11] 35 | $hash |= $this->y<<12; // 8 bits (2^8 = 256) long, in [12;19] 36 | $hash |= ($this->z%4096); // 12 bits long, in [20;31] 37 | return $hash; 38 | } 39 | 40 | public function toString(): string 41 | { 42 | return "transitionalNode[x={$this->x},y={$this->y},z=$this->z]"; 43 | } 44 | 45 | /** 46 | * @return ?BlockPathNode 47 | */ 48 | public function getParent(): ?BlockPathNode 49 | { 50 | return $this->parent; 51 | } 52 | 53 | /** 54 | * @return int 55 | */ 56 | public function getTransitionType(): int 57 | { 58 | return $this->transitionType; 59 | } 60 | 61 | public function getGValue(): float 62 | { 63 | if(is_null($this->parent)) return $this->weightFromParent; 64 | return $this->parent->getGValue() + $this->weightFromParent; 65 | } 66 | 67 | public function getHValue(): float 68 | { 69 | return $this->heuristicWeight; 70 | } 71 | 72 | public function getFValue(): float 73 | { 74 | return $this->getGValue() + $this->getHValue(); 75 | } 76 | 77 | public function getPosition(World $world): Position 78 | { 79 | return new Position($this->x, $this->y, $this->z, $world); 80 | } 81 | 82 | public function setParent(?BlockPathNode $parent, int $transitionType, int $additionalWeight): void 83 | { 84 | $this->parent = $parent; 85 | $this->transitionType = $transitionType; 86 | $this->weightFromParent = $additionalWeight; 87 | } 88 | 89 | public function setHeuristicWeight(int $heuristicWeight): void 90 | { 91 | $this->heuristicWeight = $heuristicWeight; 92 | } 93 | } -------------------------------------------------------------------------------- /lib/pathfinding/path/node/TransitionType.php: -------------------------------------------------------------------------------- 1 | nodes) == 0) return null; 15 | $result = clone $this->nodes[0]; 16 | unset($this->nodes[0]); 17 | $this->nodes = array_values($this->nodes); 18 | $index = array_search($result, $this->nodesContainsTester); 19 | unset($this->nodesContainsTester[$index]); 20 | $this->nodesContainsTester = array_values($this->nodesContainsTester); 21 | return $result; 22 | } 23 | 24 | public function contains(BlockPathNode $node): bool 25 | { 26 | foreach ($this->nodesContainsTester as $n) { 27 | if($node->equals($n)) return true; 28 | } 29 | return false; 30 | } 31 | 32 | public function getSize(): int 33 | { 34 | return count($this->nodes); 35 | } 36 | 37 | public function getValueToCompare(BlockPathNode $node): int 38 | { 39 | return $node->getFValue(); 40 | } 41 | 42 | /** @return BlockPathNode[] */ 43 | public function getNodes(): array 44 | { 45 | return $this->nodes; 46 | } 47 | 48 | public function addSorted(BlockPathNode $node): void 49 | { 50 | // don't subtract one from the size since the element could be added after the last current entry 51 | $this->nodesContainsTester[] = $node; 52 | $this->insertIntoList($node, 0, $this->getSize()); 53 | } 54 | 55 | private function insertIntoList(BlockPathNode $node, int $lowerBound, int $upperBound): void 56 | { 57 | if($lowerBound == $upperBound) 58 | { 59 | $this->nodes[$lowerBound] = $node; 60 | return; 61 | } 62 | 63 | $nodeValue = $this->getValueToCompare($node); 64 | 65 | $dividingIndex = intval(($lowerBound+$upperBound)/2); 66 | $dividingNode = $this->nodes[$dividingIndex]; 67 | $dividingValue = $this->getValueToCompare($dividingNode); 68 | 69 | if($nodeValue > $dividingValue) 70 | $this->insertIntoList($node, $dividingIndex+1, $upperBound); 71 | else 72 | $this->insertIntoList($node, $lowerBound, $dividingIndex); 73 | } 74 | 75 | public function clear(): void 76 | { 77 | $this->nodes = []; 78 | $this->nodesContainsTester = []; 79 | } 80 | 81 | public function sort(): void 82 | { 83 | usort($this->nodes, function ($n1, $n2) { 84 | return $this->getValueToCompare($n1) > $this->getValueToCompare($n2) ? 1 : -1; 85 | }); 86 | } 87 | } -------------------------------------------------------------------------------- /lib/pathfinding/path/node/weighted/WeightedNode.php: -------------------------------------------------------------------------------- 1 |