А*. Не пойму о чем ошибка

#include <iostream>
#include <queue>
#include <iostream>
#include<math.h>
#include <time.h>
 
 
 
const int N = 20;
const int M = 20;
int startx = 1, starty = 1;
int endx = 6, endy = 6;
int iObstacleMap[N][N] = { 0 };
float fLengthToPoint[N][N] = { N * N };
bool bWasHere[N][N] = { 0 };
 
//float fDifficulty[N][N];
using namespace std;
struct myPoint {
    int x, y;
    myPoint() { x = 0; y = 0; };
    myPoint(int x, int y) { this->x = x; this->y = y; };
 
    
    bool operator>(const myPoint& cmp) const
    {
        // проверка по дистанции до цели
        int a1 = (x - endx) * (x - endx);
        int b1 = (y - endy) * (y - endy);
        float d1 = (a1 + b1) + fLengthToPoint[x][y];
 
 
        int a2 = (cmp.x - endx) * (cmp.x - endx);
        int b2 = (cmp.y - endy) * (cmp.y - endy);
        float d2 = (a2 + b2) +fLengthToPoint[cmp.x][cmp.y];
 
        if (d1 < d2) return true;
        return false;
    }
    
    bool operator<(const myPoint & cmp) const
    {
        cout << "<" << endl;
        cout <<x<<":"<<y <<endl;
        cout << cmp.x << ":" << cmp.y << endl;
        // проверка по дистанции до цели
        int a1 = (x - endx) * (x - endx);
        int b1 = (y - endy) * (y - endy);
        
        float d1 = (a1 + b1) + fLengthToPoint[x][y];
 
        int a2 = (cmp.x - endx) * (cmp.x - endx);
        int b2 = (cmp.y - endy) * (cmp.y - endy);
    
        float d2 = (a2 + b2) + fLengthToPoint[cmp.x][cmp.y];
 
        cout << "ending cmp"<<endl;
        if (d1 < d2) return false;
        return true;
    }
 
 
};
 
 
 
std::priority_queue <myPoint> qPointsToScan;
//can we use neigbors without errors?
bool PointInMap(int x, int y, int dx, int dy)
{
 
    
    if (x + dx >= N)
    {
        cout << "1" << endl;
        return false;
    }
    if (x + dx < 0)
    {
        cout << "2" << endl;
        return false;
    }
    if (y + dy >= N)
    {
        cout << "3" << endl;
        return false;
    }
    if (y + dy < 0)
    {
        cout << "4" << endl;
        return false;
    }
    if (iObstacleMap[(x + dx)][(y + dy)] == -1)
    {
        cout << "5" << endl;
        return false;
    }
    if (dx == dy && dx == 0)
    {
        cout << "6" << endl;
        return false;
    }
    //ShowMessage(x+dx);
    return true;
}
 
void EraseLengthArray()
{
    for (int i = 0; i < N; i++)
        for (int j = 0; j < N; j++)
            fLengthToPoint[j][i] = 9999;
}
 
void AStar(int fromX, int fromY, int toX, int toY)
{
 
    qPointsToScan.push(myPoint(fromX, fromY));
    iObstacleMap[fromX][fromY] = 0;
    fLengthToPoint[fromX][fromY] = 0;
    while (!qPointsToScan.empty())
    { 
        
        myPoint p = qPointsToScan.top();
        if (p.x == toX && p.y == toY) return;
        qPointsToScan.pop();
        float fCurLength = fLengthToPoint[p.x][p.y];
        bWasHere[p.x][p.y] = true;
        for (int i = -1; i <= 1; i++)
            for (int j = -1; j <= 1; j++)
            {
            cout <<"for point :" << p.x << ";" << p.y << ":+" << i << ":" << j << endl;
 
                if (PointInMap(p.x, p.y, i, j))
                {
                    float fCost = 1.0f;
                    // diagonal movement cost 1.5
 
                    if (abs(i) == abs(j)) fCost = 1.5f;
 
                    if (bWasHere[p.x + i][p.y + j])
                        //if new movement not better than older
                        if (fCurLength + fCost >= fLengthToPoint[p.x + i][p.y + j])
                        {
 
                            continue;
                        }
                    fLengthToPoint[p.x + i][p.y + j] = fCurLength + fCost;
                    bWasHere[p.x + i][p.y + j] = true;
                    cout << "adding point" << endl;
                    qPointsToScan.push(myPoint(p.x + i, p.y + j));
                    cout << "after adding" << endl;
                }
 
            }
    }
 
 
}
 
int main()
{
    
    EraseLengthArray();
    srand(time(NULL));
 
 
    AStar(startx, starty, endx, endy);
 
    for (int y = 0; y < N; y++)
    {
        for (int x = 0; x < N; x++)
            std::cout << fLengthToPoint[x][y] << " ";
        std::cout << std::endl;
    
    }
    system("pause");
 
    return 0;
}