From 7b66cd0250645636dc5da08b7a032c74bd3fc783 Mon Sep 17 00:00:00 2001 From: GameHerobrine Date: Wed, 19 Oct 2022 18:40:19 +0300 Subject: [PATCH] pathfinder was changed, 102 bug fixed --- src/dependencies.php | 4 +- src/entity/Entity.php | 7 +- src/entity/animal/Pig.php | 9 ++ src/utils/pathfinder/MinecraftNode.php | 98 +++++++++++++++ src/utils/pathfinder/Path.php | 21 ---- src/utils/pathfinder/PathNode.php | 34 ----- src/utils/pathfinder/Pathfinder.php | 96 -------------- src/utils/pathfinder/astar/AStar.php | 167 +++++++++++++++++++++++++ src/utils/pathfinder/astar/Node.php | 136 ++++++++++++++++++++ src/world/Level.php | 12 +- 10 files changed, 424 insertions(+), 160 deletions(-) create mode 100644 src/utils/pathfinder/MinecraftNode.php delete mode 100644 src/utils/pathfinder/Path.php delete mode 100644 src/utils/pathfinder/PathNode.php delete mode 100644 src/utils/pathfinder/Pathfinder.php create mode 100644 src/utils/pathfinder/astar/AStar.php create mode 100644 src/utils/pathfinder/astar/Node.php diff --git a/src/dependencies.php b/src/dependencies.php index 01edcccb..dd0aa8d2 100644 --- a/src/dependencies.php +++ b/src/dependencies.php @@ -77,8 +77,8 @@ require_once(FILE_PATH . "/src/math/Vector3.php"); require_once(FILE_PATH . "/src/world/Position.php"); require_once(FILE_PATH . "/src/pmf/PMF.php"); -require_once(FILE_PATH . "/src/utils/pathfinder/PathNode.php"); -require_once(FILE_PATH . "/src/utils/pathfinder/Path.php"); +require_once(FILE_PATH . "/src/utils/pathfinder/astar/Node.php"); +require_once(FILE_PATH . "/src/utils/pathfinder/astar/AStar.php"); require_once(FILE_PATH . "/src/entity/Rideable.php"); require_once(FILE_PATH . "/src/entity/Attachable.php"); diff --git a/src/entity/Entity.php b/src/entity/Entity.php index 162bc17a..ed99d8b8 100644 --- a/src/entity/Entity.php +++ b/src/entity/Entity.php @@ -141,6 +141,11 @@ class Entity extends Position{ } } + public function setVelocity($vX, $vY, $vZ){ + $this->motionX = $vX; + $this->motionY = $vY; + $this->motionZ = $vZ; + } /** * @param mixed $e Entity instance or EID @@ -492,7 +497,7 @@ class Entity extends Position{ } if($support === false){ - $this->speedY -= $this->class === ENTITY_FALLING ? 0.08 : 1.6; + $this->speedY -= $this->class === ENTITY_FALLING ? 0.08 : 0.04; //TODO: replace with $gravity $update = true; }else{ //$this->speedX = 0; diff --git a/src/entity/animal/Pig.php b/src/entity/animal/Pig.php index 588fce90..e26d4e86 100644 --- a/src/entity/animal/Pig.php +++ b/src/entity/animal/Pig.php @@ -3,6 +3,7 @@ class Pig extends Animal{ const TYPE = MOB_PIG; public $pathFinder, $path, $server; public function __construct(Level $level, $eid, $class, $type = 0, $data = array()){ + $this->pathFinder = new AStar($this); //make it before everything else as entity class constructor sends update parent::__construct($level, $eid, $class, $type, $data); $this->setHealth(isset($this->data["Health"]) ? $this->data["Health"]:10, "generic"); $this->server = ServerAPI::request(); @@ -19,4 +20,12 @@ class Pig extends Animal{ array(($this->fire > 0 ? COOKED_PORKCHOP:RAW_PORKCHOP), 0, mt_rand(0,2)), ); } + + public function update(){ + //$start = new MinecraftNode(round($this->x), round($this->y), round($this->z)); + //$stop = $start->add(5,0,5); + //$path = $this->pathFinder->findPath($start, $stop); //allows diagonals, path should be ~5 blocks + //var_dump($path); + parent::update(); + } } diff --git a/src/utils/pathfinder/MinecraftNode.php b/src/utils/pathfinder/MinecraftNode.php new file mode 100644 index 00000000..2af4c2ee --- /dev/null +++ b/src/utils/pathfinder/MinecraftNode.php @@ -0,0 +1,98 @@ +x}, {$this->y}, {$this->z})"; + } + public function add($x = 0, $y = 0, $z = 0){ + return new MinecraftNode($x+$this->x, $y+$this->y, $z+$this->z); + } + /** + * {@inheritDoc} + * @see Node::getOwnCost() + */ + public function getOwnCost() + { + return 1; + } + + public function getParentAndCleanSelf(){ + try{ + return $this->parent; + }finally{ + $this->parent = null; + $this->nodes = null; + } + } + + public function generateAdjNodes(AStar $pf) + { + $this->nodes = []; + for($x = -1; $x <= 1; $x++){ + for($z = -1; $z <= 1; $z++){ + $node = new MinecraftNode($x + $this->x, $this->y, $z + $this->z); + if(!$node->canMoveHere($pf)){ + console("[DEBUG] Obstacle at $node"); + continue; + } + $this->nodes[] = $node; + } + } + } + + /** + * {@inheritDoc} + * @see Node::getAdjacentNodes() + */ + public function getAdjacentNodes(AStar $pf) + { + if($this->nodes === null){ + $this->generateAdjNodes($pf); + } + return $this->nodes; + } + + /** + * {@inheritDoc} + * @see Node::equals() + */ + public function equals(Node $node) + { + return $node->x === $this->x && $this->z === $node->z; + } + + + /** + * {@inheritDoc} + * @see Node::getHCost() + */ + public function getHCost(Node $target) + { + return abs($target->x - $this->x) + abs($target->z - $this->z); + } + + /** + * {@inheritDoc} + * @see Node::canMoveHere() + */ + + public function canMoveHere(AStar $pf) + { + return !$pf->entity->level->getBlockWithoutVector($this->x, $this->y, $this->z)->isSolid; + } + + +} + diff --git a/src/utils/pathfinder/Path.php b/src/utils/pathfinder/Path.php deleted file mode 100644 index 3e891ff9..00000000 --- a/src/utils/pathfinder/Path.php +++ /dev/null @@ -1,21 +0,0 @@ -points[] = $point; - } - - public function getPointAndIncreaseIndex(){ - return $this->points[$this->currentPointIndex++]; - } - - public function increaseIndexAndGetPoint(){ - return isset($this->points[$this->currentPointIndex + 1]) ? $this->points[++$this->currentPointIndex] : null; - } - - public function finish(){ - $this->points = array_reverse($this->points); - } -} \ No newline at end of file diff --git a/src/utils/pathfinder/PathNode.php b/src/utils/pathfinder/PathNode.php deleted file mode 100644 index 77e0cf2e..00000000 --- a/src/utils/pathfinder/PathNode.php +++ /dev/null @@ -1,34 +0,0 @@ -x = $x; - $this->y = $y; - $this->z = $z; - $this->parent = $parent; - $this->g = $g; - $this->h = $h; - $this->f = $f; - } - public function __toString(){ - return "PathNode(x={$this->x}, y={$this->y}, z ={$this->z}, g={$this->g}, h={$this->h}, f={$this->f})"; - } - public function calcH($endNode){ - return pow(($endNode->x - $this->x), 2) + pow(($endNode->y - $this->y), 2) + pow(($endNode->z - $this->z), 2); - } - - public function calcF(){ - return $this->h + $this->g; - } - - public static function loadFromVector3($v3, $parent = null){ - return new Node($v3->x, $v3->y, $v3->z, $parent, 0, 0, 0); - } - - public function equals($n){ - return $n->x == $this->x && $n->y == $this->y && $n->z == $this->z; - } - -} \ No newline at end of file diff --git a/src/utils/pathfinder/Pathfinder.php b/src/utils/pathfinder/Pathfinder.php deleted file mode 100644 index 9b159644..00000000 --- a/src/utils/pathfinder/Pathfinder.php +++ /dev/null @@ -1,96 +0,0 @@ -entity = $entity; - } - - public function updateEntityPos($x, $y, $z){ - $this->entity->x = $x; - $this->entity->y = $y; - $this->entity->z = $z; - } - - public function buildTo($end){ - $goal = Node::loadFromVector3($end); //247 68 215 - $this->openList[] = new Node(ceil($this->entity->x), ceil($this->entity->y), ceil($this->entity->z), 0, 0, 0); - $this->closedList = []; - $cnt = 0; - while(!empty($this->openList)){ - $currentIndex = 0; - $currentNode = $this->openList[$currentIndex]; - foreach($this->openList as $index => $node){ - if($node->f < $currentNode->f){ - $currentIndex = $index; - $currentNode = $node; - } - } - array_splice($this->openList, $currentIndex, 1); - $this->closedList[] = $currentNode; - if($currentNode->equals($goal)){ - $path = new Path(); - $current = $currentNode; - while($current != null){ - $path->addPoint($current); - $current = $current->parent; - } - $path->finish(); - return $path; - } - $children = []; - //y was removed - for($x = -1; $x <= 1; $x++){ - for($z = -1; $z <= 1; $z++){ - $nodePosition = new Vector3($currentNode->x + $x, $currentNode->y, $currentNode->z + $z); - if(!$this->isPositionValid($nodePosition)){ - continue; - } - if($currentNode->equals($nodePosition)){ - continue; - } - $node = Node::loadFromVector3($nodePosition, $currentNode); - $children[] = $node; - } - } - - foreach($children as $child){ - foreach($this->closedList as $closed){ - if($child->equals($closed)){ - goto exitLoopLbl; - } - } - exitLoopLbl: - $child->g = $currentNode->g + 1; - $child->h = $child->calcH($end); - $child->f = $child->g + $child->h; - - $this->openList[] = $child; - - } - - $cnt++; - if($cnt === 6){ - break; - } - } - - return Pathfinder::NO_PATH_FOUND; - - } - - public function isPositionValid($nodePosition){ - return $nodePosition->x >= 0 && $nodePosition->x < 256 && $nodePosition->y > 0 && $nodePosition->y < 128 && $nodePosition->z >= 0 && $nodePosition->z < 256 && $this->isEmpty($nodePosition); - } - public function distanceBetween($p1, $p2){ - return sqrt(pow(($p1->x - $p2->x),2) + pow(($p1->y - $p2->y),2) + pow(($p1->z - $p2->z),2)); - } - - public function isEmpty($v3){ - return !$this->entity->level->getBlock($v3)->isSolid; - } - -} \ No newline at end of file diff --git a/src/utils/pathfinder/astar/AStar.php b/src/utils/pathfinder/astar/AStar.php new file mode 100644 index 00000000..6a30395d --- /dev/null +++ b/src/utils/pathfinder/astar/AStar.php @@ -0,0 +1,167 @@ +entity = $entity; + } + + /** + * find a path from Node $start to Node $end + * @param Node $start + * @param Node $end + * @param number $maxOffset + * @return array|Node[] + */ + public function findPath(Node $start, Node $end) + { + // no path if there is no need for moving + if ($start->equals($end)) { + //console("[DEBUG] $start equals $end, route is empty"); + return []; + } + if(!$end->canMoveHere($this)){ + //console("[DEBUG] $end is not accessible, path will not be found"); + return false; + } + $maxOffsetX = abs($start->x - $end->x); + $maxOffsetZ = abs($start->z - $end->z); + + $this->addToOpen($start); + + $foundTarget = false; + + while (!$foundTarget && count($this->open) > 0) { + //console("[DEBUG] sorting open nodes by cost. Node count:" . count($this->open)); + uasort( + $this->open, + function (Node $a, Node $b) use ($end) { + return $a->getFCost($end) - $b->getFCost($end); + } + ); + + /** @var Node $current */ + $current = $this->open[array_keys($this->open)[0]]; + unset($this->open[array_keys($this->open)[0]]); + + //console("[DEBUG] current node selected: " . $current); + + if ((string)$current == (string)$end) { + $foundTarget = $current; + //console("[DEBUG] current node is target node, exiting loop"); + continue; + } + + $adjacent = $current->getAdjacentNodes($this); + + foreach ($adjacent as $node) { + if (in_array((string)$node, $this->closed)) { + //console("[DEBUG] skipping adjacent: $node as it was already processed"); + continue; + } + $node->setParent($current); + $this->addToOpen($node); + } + $this->addToClosed($current); + + } + + // we failed to find a route + if (!$foundTarget && count($this->open) == 0) { + //console("[DEBUG] no open nodes left, and no target found."); + return false; + } + + //console("[DEBUG] found route!"); + /** @var Node $foundTarget */ + try{ + return $this->createRouteList($foundTarget); + }finally{ //removing all elements from them + $this->closed = []; + $this->open = []; + } + + } + + /** + * @param Node $node + */ + protected function addToOpen(Node $node) + { + if ( + isset($this->open[(string)$node]) + && $this->open[(string)$node]->getGCost() < $node->getGCost() + ) { + //console("[DEBUG] skipping add, $node is already known and gcost <= new path"); + return; + } + //console("[DEBUG] adding new open node: $node"); + $this->open[(string)$node] = $node; + } + + /** + * @param Node $node + */ + protected function addToClosed(Node $node) + { + //console("[DEBUG] adding $node to closed"); + $this->closed[] = (string)$node; + } + + /** + * @param Node $foundTarget + * @return Node[] + */ + protected function createRouteList(Node $foundTarget) + { + $route = []; + $route[] = $foundTarget; + + while ($foundTarget = $foundTarget->getParentAndCleanSelf()) { + $route[] = $foundTarget; + } + $route = array_reverse($route); + + return $route; + } +} \ No newline at end of file diff --git a/src/utils/pathfinder/astar/Node.php b/src/utils/pathfinder/astar/Node.php new file mode 100644 index 00000000..b0810fa2 --- /dev/null +++ b/src/utils/pathfinder/astar/Node.php @@ -0,0 +1,136 @@ +parent = $parent; + } + + /** + * @return Node|bool + */ + public function getParent() + { + return $this->parent; + } + + public function getParentAndCleanSelf(){ + try{ + return $this->parent; + }finally{ + $this->parent = null; + } + } + + /** + * @return int|float + */ + public function getGCost() + { + if(is_null($this->gCost)) { + $this->gCost = + $this->parent ? $this->parent->getGCost() + $this->getOwnCost() : $this->getOwnCost(); + } + + return $this->gCost; + } + + /** + * @param Node $target + * @return float|int + */ + public function getFCost(Node $target) + { + return $this->getGCost() + $this->getHCost($target); + } + + /** + * @return int|float + */ + abstract public function getOwnCost(); + + /** + * @param Node $target + * @return int|float + */ + abstract public function getHCost(Node $target); + + /** + * @param AStar $pf + * @return Node[] + */ + abstract public function getAdjacentNodes(AStar $pf); + + /** + * @param Node $compareTo + * @return bool + */ + abstract public function equals(Node $compareTo); + + /** + * Check can this point be used + * @param AStar $pf + * @return bool + */ + abstract public function canMoveHere(AStar $pf); + + /** + * should return a unique string for this + * + * @return string + */ + public function __toString(){ + return parent::__toString(); + } + + /** + * this method should allow a node + * to get the data from the target node getHostCost requires for its heuristic (if needed) + * + * @return array + */ + public function getDataForH() + { + return []; + } +} \ No newline at end of file diff --git a/src/world/Level.php b/src/world/Level.php index 6d1739ca..7f0d1cd1 100644 --- a/src/world/Level.php +++ b/src/world/Level.php @@ -35,12 +35,12 @@ class Level{ public function getCubes(AxisAlignedBB $aABB) { $aABBs = []; - $x0 = (int)$aABB->minX; - $x1 = (int)($aABB->maxX + 1.0); - $y0 = (int)$aABB->minY; - $y1 = (int)($aABB->maxY + 1.0); - $z0 = (int)$aABB->minZ; - $z1 = (int)($aABB->maxZ + 1.0); + $x0 = floor($aABB->minX); + $x1 = ceil($aABB->maxX + 1.0); + $y0 = floor($aABB->minY); + $y1 = ceil($aABB->maxY + 1.0); + $z0 = floor($aABB->minZ); + $z1 = ceil($aABB->maxZ + 1.0); $x0 = $x0 < 0 ? 0 : $x0; $y0 = $y0 < 0 ? 0 : $y0; $z0 = $z0 < 0 ? 0 : $z0;