Location: PHPKode > projects > PhpBlock > A9.8/script/algorithm/a-star.class.php
<?php
/**
 //From http://en.wikipedia.org/wiki/A*_search_algorithm
 function A*(start,goal)
 var closed := the empty set
 var q := make_queue(path(start))
 while q is not empty
 var p := remove_first(q)
 var x := the last node of p
 if x in closed
 continue
 if x = goal
 return p
 add x to closed
 foreach y in successors(p)
 enqueue(q, y)
 return failure
 */
class AStar {
	private $listOfCoordForSuccessors = false;
	private $maxRange;
	private $maxIteration;
	public $globalAvoidUnitCostMux;
	public $globalReturnLastChoiceOnFailure;

	public function __construct($parMaxStep, $parMaxRange) {
		$this->maxRange = $parMaxRange;
		$this->maxIteration = $parMaxStep;
		$this->listOfCoordForSuccessors = $this->getListOfCoordForSuccessors();
		$this->globalAvoidUnitCostMux = 1.0;
		$this->globalReturnLastChoiceOnFailure = false;
	}

	public function getArea($x, $y, $z) {
		if(!isset($this->area[$x][$y][$z])) {
			if(Zone::staticCanExistXYZ($x, $y, $z)) {
				$locZoneId = Zone::getZoneId($x, $y, $z);
				$this->area[$x][$y][$z] = $this->getAStarElement($this->realObject, $locZoneId, $this->zoneStart, $this->zoneGoal);
			} else {
				return false;
			}
		}
		return $this->area[$x][$y][$z];
	}

	protected function getAStarElement($parObject, $parZoneId, $parZoneStart, $parZoneGoal) {
		return new AStarPathElement($this, $parObject, $parZoneId, $parZoneStart, $parZoneGoal);
	}

	public function getPathZoneId($parRealObject, $parZoneStart, $parZoneGoal, &$parMessage, &$parResult) {
		$locBegin = microtime(true);
		if(!$parZoneStart) {
			$parMessage = "start zone is not set";
			return false;
		}
		if(!$parZoneGoal) {
			$parMessage = "goal zone is not set";
			return false;
		}
		$locRange = Zone::staticGetRange3d($parZoneStart->zoneId, $parZoneGoal->zoneId);
		if($locRange > $this->maxRange) {
			$parMessage = "choose a nearer destination, because max range is $this->maxRange and you choose a destination at $locRange";
			return false;
		}
		if($parZoneStart->zoneId == $parZoneGoal->zoneId) {
			$parMessage = "start zoneId is equals of goal: ".Zone::staticGetTextZoneCoord($parZoneGoal->zoneId);
			return false;
		}
		global $gloObjectManager;
		$this->area = array();
		$this->elementStack = array();
		$this->realObject = $parRealObject;
		$this->zoneGoal = $parZoneGoal;
		$this->zoneStart = $parZoneStart;
		Log::debug("start: ".$parZoneStart->zoneId);
		Log::debug("goal: ".$parZoneGoal->zoneId);

		$q = $this->getArea($parZoneStart->x, $parZoneStart->y, $parZoneStart->z);
		$q->closed=false;//In case where ...
		array_push($this->elementStack, $q);

		$locStepCount=0;

		//echo "count(this->elementStack) ".count($this->elementStack)."<br>";
		//echo "q->zone->zoneId: ".$q->zone->zoneId."<br>\n";
		while(!empty($this->elementStack) && $locStepCount < $this->maxIteration) {
			//echo "a ".((microtime(true)-$locBegin)*1000)."ms<br>";
			$locStepCount++;
			$p = array_shift($this->elementStack);
			//echo "p->zone->zoneId: ".$p->zone->zoneId.", this->elementStack size: ".count($this->elementStack)."<br>\n";

			if($p->closed) {//&& $p->zone->zoneId != $parZoneStart->zoneId
				//echo "p is closed<br>";
				continue;
			}

			if($p->zone->zoneId == $parZoneGoal->zoneId) {
				$this->initResult($parResult, $parMessage, $p);
				return true;
			}
			//echo "b ".((microtime(true)-$locBegin)*1000)."ms<br>";
			$this->addSuccessors($p);
			//echo "count(this->elementStack) ".count($this->elementStack)."<br>";
		}
		if($this->globalReturnLastChoiceOnFailure) {
			$parMessage = "No path have been found with dungeon A* (max iteration: $this->maxIteration), a nearer destination have been taken";
		} else {
			$parMessage = "No path have been found with dungeon A* (max iteration: $this->maxIteration), choose a nearer destination";
		}
		//echo "area size: ".count($this->area)." stack size: ".count($this->elementStack)."<br>";
		if($this->globalReturnLastChoiceOnFailure) {
			$this->initResult($parResult, $parMessage, $p);
			return true;
		}
		return false;
	}
	
	private function initResult(&$parResult, &$parMessage, $p) {
		//echo "SUCCESS!!<br>";
		$parResult = array();
		$locElement = $p;
		while($locElement->previousElement) {//Ignore first element which is start
			array_push($parResult, $locElement->zone->zoneId);
			//echo "locElement->zone->zoneId: ".$locElement->zone->zoneId."<br>";
			$locElement = $locElement->previousElement;
			//$parMessage .=  "\nzoneId: ".$locElement->zone->zoneId." est: $locElement->estimateCost cum: $locElement->cumulativeCost total cost: $locElement->totalCost ec $locElement->extraCost +mc $locElement->moveCost ";
		}
		$parResult = array_reverse($parResult);
		array_push($parResult, $this->zoneGoal->zoneId);
		//echo "\nstepCount = $locStepCount\n";
		//echo "area size: ".count($this->area)." stack size: ".count($this->elementStack)."<br>";
		//$locEnd = microtime(true);
		//$parMessage = "in $locStepCount steps (".($locEnd - $locBegin)."s)";
	}

	protected function getListOfCoordForSuccessors() {
		$listOfCoordForSuccessors = array();
		array_push($listOfCoordForSuccessors, array( 0, 0, -1));
		array_push($listOfCoordForSuccessors, array( 0, 0, 1));
		for($z = -1; $z <= 1; $z++) {
			array_push($listOfCoordForSuccessors, array( 0, 1, $z));
			array_push($listOfCoordForSuccessors, array( 0,-1, $z));
			array_push($listOfCoordForSuccessors, array( 1, 0, $z));
			array_push($listOfCoordForSuccessors, array(-1, 0, $z));
			array_push($listOfCoordForSuccessors, array(-1, 1, $z));
			array_push($listOfCoordForSuccessors, array(-1,-1, $z));
			array_push($listOfCoordForSuccessors, array( 1, 1, $z));
			array_push($listOfCoordForSuccessors, array( 1,-1, $z));
		}
		return $listOfCoordForSuccessors;
	}

	public function addSuccessors($parElement) {
		//echo "\n\n";
		//echo "parElement->zone->x: ".$parElement->zone->x.", y: ".$parElement->zone->y."<br>";
		$parElement->closed = true;
		foreach($this->listOfCoordForSuccessors as $xyz) {
			//$locBegin = microtime(true);

			$x = $xyz[0];
			$y = $xyz[1];
			$z = $xyz[2];
			//echo "x: ".($parElement->zone->x+$x).", y: ".($parElement->zone->y+$y).", z: ".($parElement->zone->z+$z)."<br>";

			$locElementOfArea = $this->getArea($parElement->zone->x+$x, $parElement->zone->y+$y, $parElement->zone->z+$z);
			//echo "c1 ".((microtime(true)-$locBegin)*1000)."ms<br>";
			if($locElementOfArea) {
				//echo "$x $y $z $locElementOfArea->closed ".$locElementOfArea->zone->zoneId."<br>";

				if(!$locElementOfArea->closed) {
					if($locElementOfArea->previousElement) {
						if($locElementOfArea->previousElement->cumulativeCost > 
								$parElement->cumulativeCost) {
							$locElementOfArea->setPreviousElement($parElement);
							array_push($this->elementStack, $locElementOfArea);
						}
					} else {
						$locElementOfArea->setPreviousElement($parElement);
						array_push($this->elementStack, $locElementOfArea);
					}
				}
			}
			//echo "c2 ".((microtime(true)-$locBegin)*1000)."ms<br>";
		}
		//$locBegin = microtime(true);
		usort($this->elementStack, "compareAStarPathElementWithHeuristic");
		//echo "sort ".((microtime(true)-$locBegin)*1000)."ms<br>";

		//echo "\n\n this->elementStack\n";
		//foreach($this->elementStack as $a) {
		//    //echo "$a->zoneId estimateCost: $a->estimateCost $a->closed\n";
		//}
	}
}
function compareAStarPathElementWithHeuristic($a, $b) {
	if ($a->estimateCost == $b->estimateCost) {
		return ($a->totalCost > $b->totalCost) ? 1 : -1;
	} else {
		return ($a->estimateCost > $b->estimateCost ? 1 : -1);
	}
}
class AStarPathElement {
	public function __construct(AStar $parAStar, $parObject, $parZoneId, $parZoneStart, $parZoneGoal) {
		global $gloObjectManager;
		$this->aStar=$parAStar;
		$this->avoidUnitCostMux=$parAStar->globalAvoidUnitCostMux;
		$this->object = $parObject;
		$this->objectAllianceId = $parObject->allianceId;
		$this->zoneId = $parZoneId;
		$this->zone = $gloObjectManager->getZone($this->zoneId);
		if(!$this->zone) {
			die("zoneId $parZoneId didn't exist");
		}
		$this->closed = $this->isADefinitiveClose();
		if(!$this->closed) {
			$this->zoneGoal = $parZoneGoal;
			$this->zoneStart = $parZoneStart;
			$this->previousElement = false;

			$this->initCostForZone();
			$this->zCost = 0;
			$this->totalCost = $this->avoidCost + $this->extraCost + $this->moveCost;
			$this->cumulativeCost = 0;
			$this->estimateCost = 0;

			$this->distanceToGoal = $this->calculateDistanceToGoal();
		}
	}
	protected function calculateDistanceToGoal() {
		//$this->estimateRangeCostToStart = (Zone::staticSum2d($this->zone->zoneId, $this->zoneStart->zoneId));
		//$this->estimateRangeCostToGoal = Zone::staticSum2d($this->zone->zoneId, $this->zoneGoal->zoneId);
		//Euclidian range is better human looking
		return Zone::staticGetEuclideanDistance3d($this->zoneId, $this->zoneGoal->zoneId)*$this->moveCost;
	}

	protected function isADefinitiveClose() {
		return $this->zone->isOccupedWithOnlyNonMovable();
	}

	protected function initCostForZone() {
		$this->moveCost = $this->zone->getMoveCostFactor();
		$this->extraCost = 0;
		if($this->zone->isOccupedWithMovable()) {
			$this->avoidCost = 2.0*$this->avoidUnitCostMux;
		} else {
			$this->avoidCost = 0.0;
		}
	}

	protected function computeZCost(){
		$locDiffAbsZ = abs($this->zone->z - $this->zoneGoal->z);
		if($locDiffAbsZ != 0) {
			//If range with goal growth is bad !
			if((abs($this->previousElement->zone->z - $this->zoneGoal->z) < $locDiffAbsZ)) {
				$this->zCost = $this->previousElement->zCost + $locDiffAbsZ*0.1;
			} else {
				if((abs($this->previousElement->zone->z - $this->zoneGoal->z) > $locDiffAbsZ)) {
					//If it reduces it good !
					$this->zCost = $this->previousElement->zCost - $locDiffAbsZ*0.1;
				}
			}
			//While we are not in goal zone we are in bad direction
			//$this->zCost += 0.1;//no it is a lots wrong with stair...
		} else {
			$this->zCost = 0;
		}
	}

	public function setPreviousElement($parElement) {
		$this->previousElement = $parElement;
		$this->computeZCost();

		$this->cumulativeCost = $this->previousElement->cumulativeCost
		+ $this->extraCost + $this->moveCost;
		if($this->previousElement->zone->z != $this->zone->z
		&& $this->zone->isAffectedByUpAndDownCost()) {
			//Change z is expensive
			$this->cumulativeCost += $this->moveCost+Constante::$MOVE_Z_COST;
		}

		if($this->zoneGoal->zoneId == $this->zoneId) {
			//We count only previous element
			$this->estimateCost = $this->previousElement->cumulativeCost
			+ $this->previousElement->zCost;
		} else {
			$this->estimateCost = $this->cumulativeCost
			+ $this->distanceToGoal + $this->avoidCost
			+ $this->previousElement->zCost;
			$locDiffAbsZ = abs($this->zone->z - $this->zoneGoal->z);
			$this->estimateCost += $locDiffAbsZ*Constante::$MOVE_Z_COST;
		}
	}
}
?>
Return current item: PhpBlock