Добавление немонотонной эвристики к реализации A* php

Я использую алгоритм поиска A* в PHP aaz, чтобы помочь мне найти кратчайший маршрут через трехмерный граф узлов.

Он делает это хорошо, но возвращает первый найденный маршрут, который может быть не оптимальным. Поскольку набор узлов является трехмерным, эвристика немонотонна. Как мне приспособить эту имплиментацию для поиска оптимального маршрута, а не только кратчайшего?

class astar extends database
{
// Binary min-heap with element values stored separately

var $map = array();
var $r;  //  Range to jump
var $d;  //  distance between $start and $target
var $x;  //  x co-ords of $start
var $y;  //  y co-ords of $start
var $z;  //  z co-ords of $start


function heap_float(&$heap, &$values, $i, $index) {
    for (; $i; $i = $j) {
        $j = ($i + $i%2)/2 - 1;
        if ($values[$heap[$j]] < $values[$index])
            break;
        $heap[$i] = $heap[$j];
    }
    $heap[$i] = $index;
}

function heap_push(&$heap, &$values, $index) {
    $this->heap_float($heap, $values, count($heap), $index);
}

function heap_raise(&$heap, &$values, $index) {
    $this->heap_float($heap, $values, array_search($index, $heap), $index);
}

function heap_pop(&$heap, &$values) {
    $front = $heap[0];
    $index = array_pop($heap);
    $n = count($heap);
    if ($n) {
        for ($i = 0;; $i = $j) {
            $j = $i*2 + 1;
            if ($j >= $n)
                break;
            if ($j+1 < $n && $values[$heap[$j+1]] < $values[$heap[$j]])
                ++$j;
            if ($values[$index] < $values[$heap[$j]])
                break;
            $heap[$i] = $heap[$j];
        }
        $heap[$i] = $index;
    }
    return $front;
}

function a_star($start, $target) {
    $open_heap = array($start); // binary min-heap of indexes with values in $f
    $open      = array($start => TRUE); // set of indexes
    $closed    = array();               // set of indexes

    $g[$start] = 0;
    $d[$start] = 0;
    $h[$start] = $this->heuristic($start, $target);
    $f[$start] = $g[$start] + $h[$start];
    while ($open) {
        $i = $this->heap_pop($open_heap, $f);
        unset($open[$i]);
        $closed[$i] = TRUE;

        if ($i == $target) {
            $path = array();
            for (; $i != $start; $i = $from[$i])
                $path[] = $i;
            return array_reverse($path);
        }

        foreach ($this->neighbors($i) as $j => $rng)
            if (!array_key_exists($j, $closed))
                if (!array_key_exists($j, $open) || $d[$i] + $rng < $d[$j])     {
                    $d[$j] = $d[$i]+$rng;
                    $g[$j] = $g[$i] + 1;
                    $h[$j] = $this->heuristic($j, $target);
                    $f[$j] = $g[$j] + $h[$j];
                    $from[$j] = $i;

                    if (!array_key_exists($j, $open)) {
                        $open[$j] = TRUE;
                        $this->heap_push($open_heap, $f, $j);
                    } else
                        $this->heap_raise($open_heap, $f, $j);
                }
    }

    return FALSE;
}

function jumpRange($i, $j){

    $sx = $this->map[$i]->x;
    $sy = $this->map[$i]->y;
    $sz = $this->map[$i]->z;
    $dx = $this->map[$j]->x;
    $dy = $this->map[$j]->y;
    $dz = $this->map[$j]->z;

    return sqrt((($sx-$dx)*($sx-$dx)) + (($sy-$dy)*($sy-$dy)) + (($sz-$dz)*($sz-$dz)))/9460730472580800;
}

function heuristic($i, $j) {

    $rng = $this->jumpRange($i, $j);
    return ceil($rng/$this->r);
}

function neighbors($sysID)
{
    $neighbors = array();
    foreach($this->map as $solarSystemID=>$system)
    {
        $rng = $this->jumpRange($sysID,$solarSystemID);
        $j = ceil($rng/$this->r);
        $this->map[$solarSystemID]->h = $j;
        if($j == 1 && $this->map[$solarSystemID]->s)
        {
            $neighbors[$solarSystemID] = $rng;
        }
    }
    arsort($neighbors);
    return $neighbors;
}

function fillMap()
{
    $res = $this->query("SELECT * FROM mapSolarSystems WHERE SQRT(
  (
   ($this->x-x)*($this->x-x)
  ) + (
   ($this->y-y)*($this->y-y)
  ) + (
   ($this->z-z)*($this->z-z)
  )
 )/9460730472580800 <= '$this->d'","SELECT");
    while($line=mysql_fetch_object($res))
    {
        $this->map[$line->solarSystemID] = $line;
        $this->map[$line->solarSystemID]->h = 0;
        $this->map[$line->solarSystemID]->s = false;
    }
    $res = $this->query("SELECT solarSystemID FROM staStations UNION SELECT solarSystemID FROM staConqureable","SELECT");
    while($line=mysql_fetch_object($res))
    {
        if(isset($this->map[$line->solarSystemID]))
            $this->map[$line->solarSystemID]->s = true;
    }
}
}