D* lite optimized: what is wrong in my implementation of Search() function?

I'm trying to implement the D*-Lite pathfinding algorithm, as described in the 2002 article by Koenig and Likhachev for grid-based navgraph. But I stuck with problem. My ComputeShortestPath (in my implementation it is Search() function) can't find the path to source and visited nodes looks very similar non-dependent from source and target positions (it always seeks diagonally to left and top from target node).

Looks like that I have some bug here but I can't find it by myself. If somebody can tell me what is wrong in this code, I will be very happy.

Thank you very much.

class DStarLiteKey
{
public:
DStarLiteKey(double left = 0, double right = 0) : left(left), right(right) {}

bool operator==(const DStarLiteKey &other) const
{
return isEqual(left, other.left) && isEqual(right, other.right);
}

bool operator > (const DStarLiteKey &other) const {
if (left - EPSILON > other.left) return true;
else if (left < other.left - EPSILON) return false;
return right > other.right;
}

bool operator <= (const DStarLiteKey &other) const {
if (left < other.left) return true;
else if (left > other.left) return false;
return right < other.right + EPSILON;
}

bool operator < (const DStarLiteKey &other) const {
if (left + EPSILON < other.left) return true;
else if (left - EPSILON > other.left) return false;
return right < other.right;
}

public:
double left, right;

protected:
static constexpr double EPSILON = std::numeric_limits<double>::epsilon();
};

class HeuristicEightCondist
{
public:
HeuristicEightCondist(){}

static double calculate(const Graph &graph, IdxType node1, IdxType node2)
{
Graph::NodeType nd1 = graph.GetNode(node1);
Graph::NodeType nd2 = graph.GetNode(node2);
double temp;
double min = fabs(nd1.position.x - nd2.position.x);
double max = fabs(nd1.position.y - nd2.position.y);
if (min > max)
{
temp = min;
min = max;
max = temp;
}
return ((M_SQRT2-1.0)*min + max);
}
};

template <class Heuristic>
class DStarLiteSearch
{
public:
static constexpr double M_INF = std::numeric_limits<double>::min();
static constexpr double INF = std::numeric_limits<double>::max();

public:
DStarLiteSearch(Graph &graph) :
m_graph(graph),
m_pq(),
m_nodePqIndeces(graph.numNodes()),
m_pqNodes(graph.numNodes()),
m_source(-1),
m_target(-1),
m_pathExists(false)
{}

void Init(IdxType source, IdxType target)
{
m_pq.init(m_graph.numNodes(), DStarLiteKey(M_INF, M_INF),
DStarLiteKey(INF, INF));
m_km = 0;
m_gCosts = std::vector<double>(m_graph.numNodes(), std::numeric_limits<double>::max());
m_rhsCosts = std::vector<double>(m_graph.numNodes(), std::numeric_limits<double>::max());
m_nodePqIndeces = std::vector<IdxType>(m_graph.numNodes(), 0);
m_pqNodes = std::vector<IdxType>(m_graph.numNodes(), 0);
m_source = source;
m_target = target;
m_pathExists = false;

// Algorithm begins
m_rhsCosts[m_target] = 0;

IdxType nodeIdx = m_pq.insert(DStarLiteKey(Heuristic::calculate(m_graph, m_source, m_target), 0));
m_nodePqIndeces[nodeIdx] = m_target;
m_pqNodes[m_target] = nodeIdx;
}

bool Search();

std::list<IdxType> GetPath();

#ifdef DEBUG
//returns a vector containing pointers to all the edges the search has examined
std::vector<const Edge*> GetSearchTree()const{return m_spanningTree;}
#endif

bool isPathExist()
{
return m_pathExists;
}

protected:
DStarLiteKey calculateKey(IdxType node)
{
double temp = std::min(m_gCosts[node], m_rhsCosts[node]);
return DStarLiteKey(temp + Heuristic::calculate(m_graph, m_source, node) + m_km, temp);
}

void updateVertex(IdxType node)
{
bool isInPQ = m_pq.isObjectExist(m_pqNodes[node]);

if(!isEqual(m_gCosts[node], m_rhsCosts[node]) && isInPQ)
{
m_pq.update(m_pqNodes[node], calculateKey(node));
}
else if(!isEqual(m_gCosts[node], m_rhsCosts[node]) && !isInPQ)
{
IdxType nodeIdx = m_pq.insert(calculateKey(node));
m_nodePqIndeces[nodeIdx] = node;
m_pqNodes[node] = nodeIdx;
}
else if(isEqual(m_gCosts[node], m_rhsCosts[node]) && isInPQ)
{
m_pq.remove(m_pqNodes[node]);
}
}

protected:
Graph &m_graph;
MinHeap<DStarLiteKey, IdxType> m_pq;
std::vector<IdxType> m_nodePqIndeces;
std::vector<IdxType> m_pqNodes;
std::vector<double> m_gCosts;
std::vector<double> m_rhsCosts;
double m_km;
NodeIndex m_source;
NodeIndex m_target;
bool m_pathExists;

#ifdef DEBUG
//As the search progresses, this will hold all the edges the algorithm has
//examined. THIS IS NOT NECESSARY FOR THE SEARCH, IT IS HERE PURELY
//TO PROVIDE THE USER WITH SOME VISUAL FEEDBACK
std::vector<const Edge*>  m_spanningTree;
#endif
};

#endif // GRAPHALGORITHMS_H

template<class Heuristic>
bool DStarLiteSearch<Heuristic>::Search()
{
#ifdef DEBUG
m_spanningTree.clear();
#endif

IdxType node;
DStarLiteKey kOld, kNew;
double gOld, minSuccessorCost, temp;

while(m_pq.min() < calculateKey(m_source) || m_rhsCosts[m_source] > m_gCosts[m_source])
{
node = m_nodePqIndeces[m_pq.minIdx()];
kOld = m_pq.min();
kNew = calculateKey(node);

if(kOld < kNew)
{
m_pq.update(m_pqNodes[node], kNew);
}
else if(m_gCosts[node] > m_rhsCosts[node])
{
m_gCosts[node] = m_rhsCosts[node];
m_pq.remove(m_pqNodes[node]);

Graph::ConstPredecessorEdgeIterator edges(m_graph, node);
for (const Graph::EdgeType * curEdge = edges.begin();
!edges.end();
curEdge = edges.next())
{
m_spanningTree.push_back(curEdge);

if(curEdge->from != m_target)
{
m_rhsCosts[curEdge->from] = std::min(m_rhsCosts[curEdge->from], curEdge->cost + m_gCosts[node]);
updateVertex(curEdge->from);
}
}
}
else
{
gOld = m_gCosts[node];
m_gCosts[node] = INF;

Graph::ConstPredecessorEdgeIterator edges(m_graph, node);
for (const Graph::EdgeType * curEdge = edges.begin();
!edges.end();
curEdge = edges.next())
{
m_spanningTree.push_back(curEdge);

if(m_rhsCosts[curEdge->from] == curEdge->cost + gOld)
{
if(curEdge->from != m_target)
{
minSuccessorCost = INF;

Graph::ConstEdgeIterator edges(m_graph, curEdge->from);
for (const Graph::EdgeType * edge = edges.begin();
!edges.end();
curEdge = edges.next())
{
//m_spanningTree.push_back(edge);

temp = edge->cost + m_gCosts[edge->to];

if(minSuccessorCost > temp)
{
minSuccessorCost = temp;
}
}
m_rhsCosts[curEdge->from] = minSuccessorCost;
}
}
updateVertex(curEdge->from);
}

//updateVertex(node);
}
}

if(!isEqual(m_rhsCosts[m_source], INF))
{
m_pathExists = true;
return true;
}

return false;
}