A* pathfinding ไม่ใช้ระยะทางที่สั้นที่สุด

ผมกำลังศึกษา A* pathfinding อยู่ ลองพยายามปรับหลายรอบแล้วแต่ไม่รู้ว่าติดตรงไหน
ปัญหาคือเวลาเดินทางจากจุดเริ่มไปยังจุดปลายมันไม่ยอมใช้ระยะทางที่สั้นที่สุดในบางครั้ง กรณีสร้างกำแพง
มีคำแนะนำอะไรบ้างไหมครับ


อย่างในรูปมันเคลื่อนที่ลงมาจุดนึง แทนที่จะตรงไปเพราะใช้ cost น้อยกว่า


รูปนี้มีช่วงที่มันโดดขึ้นไป ซึ่งห่างจากจุดปลาย


[Spoil] คลิกเพื่อดูข้อความที่ซ่อนไว้
cNode FindInClose(int x, int y) //หาค่าตำแหน่งโนดใน vector close  แล้วส่งค่ากลับเป็นโนดนั้น (ใช้ตอนไล่ระยะทางจากจุดหมายมาจุดเริ่มต้น)
{
    for(vector<cNode>::iterator iter = close.begin(); iter != close.end(); iter++)
    {
        if(iter->Equal(x,y)) return *iter;
    }
}

vector<cNode> FindPath(int ax, int ay, int bx, int by) //เก็บระยะทางเป็นโนดต่อๆกัน โดยรับพารามิเตอร์เป็นตำแหน่งเริ่มต้นและตำแหน่งสุดท้ายคือ a และ b ตามลำดับ
{
    vector<cNode> path;
    cNode a(ax, ay, 0.0, calH_fixed(ax,ay,bx,by),ax,ay); // กำหนดโนดสำหรับเป็นจุดเริ่มต้น
    cNode b(bx, by, 0.0, 0.0, 0, 0);    //กำหนดโนดปลายทาง

    open.clear();   //เคลียร์เว็คเตอร์ (สำหรับการเล่นครั้งต่อไป)
    close.clear();

    cNode cur = a;  //กำหนดให้โนดอ้างอิงเป็นจุดเริ่มต้น
    open.push_back(cur);    //เพิ่มโนดเข้าไปในเว็คเตอร์

    while(open.size())
    {

        float f = 888.0;
        float g = 888.0;
        cNode node = open[0];
        vector<cNode>::iterator iter_to_del; //กำหนดตัวชี้สำหรับลบโนดใน vector open
      
        
        for(vector<cNode>::iterator iter = open.begin(); iter != open.end(); iter++)
        {
            if(iter->f < f ){ f = iter->f;

            iter_to_del = iter;}
        
            //เลือกโนดใน vector ที่มีระยะทางน้อยที่สุดก่อนเสมอ


            
        }

        if(iter_to_del->Equal(b))  // เช็คว่าถึงจุดปายทางหรือยัง
            {

                cout << "\n\n" <<iter_to_del->f ;
                cNode now = *iter_to_del;  //ให้ now เป็นโนดอ้างอิง
                //now = FindInClose(now.px, now.py);
                while(!now.Equal(a)) // ย้อนโนดจากจุดสุดท้ายไปยังจุดเริ่มต้น
                {
                    path.push_back(now);
                    now = FindInClose(now.px, now.py);
                }
                reverse(path.begin(), path.end()); //กลับค่าใน vector ให้เป็นจากจุดเริ่มต้นไปจุดสุดท้าย

                return path; //คืนค่าระยะทาง
            }

        cur = *iter_to_del; //กำหนดให้โนดปอ้างอิงชี้ไปที่เดียวกับโนดที่จะลบ
        open.erase(iter_to_del);    //ลบโนดออกจาก vector open
        close.push_back(cur); //ในโนดที่ลบไปใส่ใน vector close


        for(int i = cur.y-1; i < cur.y+2; i++) //เช็คช่องว่าง +- 1 ในแกน y โดยหากไม่ซ้ำจุดเดิม ไม่มีสิ่งกีดขวาง และไม่อยู่ใน vector open
        {                                         // จะทำการนำโนดไปต่อท้ายใน vector open
            cNode now(cur.x, i, cur.g+1, calH_fixed(cur.x,i,b.x,b.y), cur.x, cur.y);
            if(map[now.y][now.x] != ' ' || IsInClose(now)) continue;//ข้ามฟังก์ชั่นต่อๆ ไปภายในลูปถ้าเงื่อนไขเป็นจริง //sqrt(pow(fabs(cur.x-b.x),2)+pow(fabs(i-b.y),2))


                if(!IsInOpen(now)){
                open.push_back(now);
                }

        }



        for(int i = cur.x-1; i < cur.x+2; i++)  // เหมือนกับด้านบนแต่เปลี่ยนเป็นแกน x
        {

            cNode now(i, cur.y, cur.g+1,calH_fixed(i,cur.y,b.x,b.y), cur.x, cur.y);
            if(map[now.y][now.x] != ' ' || IsInClose(now)) continue;        //sqrt(pow(fabs(i-b.x),2)+pow(fabs(cur.y-b.y),2))

            
                if(!IsInOpen(now)){
                open.push_back(now);
                }



        }
        for(int i = cur.y-1; i < cur.y+2; i++) //เช็คช่องว่าง +- 1 ในแกน y และ x-1
        {
            cNode now(cur.x-1, i, cur.g+sqrt(2.0),calH_fixed(cur.x-1,i,b.x,b.y), cur.x, cur.y);
            if(map[now.y][now.x] != ' ' || IsInClose(now)) continue;//ข้ามฟังก์ชั่นต่อๆ ไปภายในลูปถ้าเงื่อนไขเป็นจริง //sqrt(pow(fabs(cur.x-1-b.x),2)+pow(fabs(i-b.y),2))

            
                if(!IsInOpen(now)){
                open.push_back(now);
                }




        }
        for(int i = cur.y-1; i < cur.y+2; i++) //เช็คช่องว่าง +- 1 ในแกน y และ  x+1
        {

            cNode now(cur.x+1, i, cur.g+sqrt(2.0), calH_fixed(cur.x+1,i,b.x,b.y), cur.x, cur.y);
            if(map[now.y][now.x] != ' ' || IsInClose(now)) continue;//ข้ามฟังก์ชั่นต่อๆ ไปภายในลูปถ้าเงื่อนไขเป็นจริง //sqrt(pow(fabs(cur.x+1-b.x),2)+pow(fabs(i-b.y),2))

        
                if(!IsInOpen(now)){
                open.push_back(now);
                }


        }



    } // ============================ WHILE END

    //return path;
}