• file:A_star.cpp

  • brief:基于父节点搜索邻域格点

  • author:AIplusX

  • param[in|out] 输入:父节点内存的地址 | 输出:无

  • return:无

  • exception:无

  • note:首先根据数组索引父节点周围的格点,然后根据算法逻辑进行格点属性(G,F值)的更新

    • remarks:注意堆中内存的利用,如果申请的内存代表的坐标已经有了,或者是障碍物,那么就需要delete掉申请的内存
void A_star::search_env_block(Point* p)
{
    int temp_cost(INT_MAX);

    for (int i = 0; i < env_nums; i++) {

        Point* search_p = new Point((*p).x + offset_x[i], (*p).y + offset_y[i]);

        if (judge_env_legal(*search_p)) {

            temp_cost = (*p).Gn + calcu_cost(*p, *search_p);

            if (is_alread_set(obstacleset, search_p) || is_alread_set(fatherset, search_p)) {
                delete search_p;
                continue;
            }
            else {
                user_trajectory.map[search_p->x][search_p->y] = SEARCH;
                if (!is_alread_set(openset, search_p)) {
                    search_p->Gn = temp_cost;
                    search_p->Fn = search_p->Gn + calcu_Hn(*search_p);
                    search_p->pre_point = p;
                    add_openset(search_p);
                }else if(search_p->Gn > temp_cost){
                    search_p->Gn = temp_cost;
                    search_p->Fn = search_p->Gn + calcu_Hn(*search_p);
                    search_p->pre_point = p;
                }

            }
        }
        else {delete search_p;}
    }

}
  • file:A_star.cpp

  • brief:在向量中寻找特定的坐标值

  • author:AIplusX

  • param[in|out] 输入:待查找向量集合,目标坐标 | 输出:布尔值变量

  • return:布尔值变量

  • exception:无

  • note:无

  • remarks:注意算法需要比较的是向量里指针所指的内存里的坐标与目标坐标的数值

bool A_star::is_alread_set(std::vector<Point*>& vec, Point* p)
{
    for (int i = 0; i < vec.size(); i++) {
        if (*vec[i] == *p) {
            return true;
        }
    }
    return false;
}
  • file:A_star.cpp

  • brief:找到openset里面指针所指的坐标内存中F值最小的坐标

  • author:AIplusX

  • param[in|out] 输入:外部坐标值的引用 | 输出:F值最小的坐标的地址

  • return:F值最小的坐标的内存的地址

  • exception:无

  • note:无

  • remarks:注意算法需要比较的是向量里指针所指的内存里的坐标的F值,找到最小F值的坐标

Point* A_star::find_min_Fn(int& idx)
{
    int min_Fn(INT_MAX);
    for (int i = 0; i < openset.size(); i++) {
        if ((*openset[i]).Fn < min_Fn) {
            idx = i;
            min_Fn = (*openset[i]).Fn;
        }
    }
    return openset[idx];
}
  • file:A_star.cpp

  • brief:探索坐标的合法性

  • author:AIplusX

  • param[in|out] 输入:坐标值的常量引用 | 输出:true或者false

  • return:布尔值

  • exception:无

  • note:根据父节点去探索父节点周围的节点的时候,待探索节点可能会超出地图范围,因此需要该函数判断坐标点的合法性

  • remarks:无

bool A_star::judge_env_legal(const Point& p)
{
    if (p.x < 0 || p.x > user_trajectory.map_x || p.y < 0 || p.y > user_trajectory.map_y)
    {
        return false;
    }
    else {
        return true;
    }
}
  • file:A_star.cpp

  • brief:探索值,F值的计算方法

  • author:AIplusX

  • param[in|out]:坐标点的内存地址

  • return:数值大小

  • exception:无

  • note:计算方法相同,都是计算代价。H值计算的是坐标点到终点的预计花费,其实也就是用算G值的方法来同样计算坐标点到终点的花费,相邻坐标点的花费计算方法也是相同的

  • remarks:无

int A_star::calcu_cost(const Point& p1, const Point& p2)
{
    int dx = abs(p1.x - p2.x);
    int    dy = abs(p1.y - p2.y);
    int cost = 0;
    calcu_type_selection(cost, dx, dy);
    return cost;
}

int A_star::calcu_Hn(const Point& p1)
{
    int dx = abs(p1.x - end.x);
    int    dy = abs(p1.y - end.y);
    int Hn = 0;
    calcu_type_selection(Hn, dx, dy);
    return Hn;
}
  • file:A_star.cpp

  • brief:代价计算方法

  • author:AIplusX

  • param[in]:代价的引用和x,y坐标变化量的常量引用

  • return:无

  • exception:无

  • note:代价计算方法,根据头文件中宏定义来确定调用的启发函数

  • remarks:不同的启发函数在调用时需要在程序上做一些修改,需要修改的部分在A_star.h的头注释文件中已经说明了

void A_star::calcu_type_selection(int& data, const int& dx1, const int& dy1) {
    #ifdef Manhattan_Distance
        data = plane_cost * (dx1 + dy1);
    #elif Euclidean_Distance
        data = int(plane_cost * sqrt(dx1 * dx1 + dy1 * dy1));
    #endif
}
  • file:A_star.cpp

  • brief:获取参数结构体

  • author:AIplusX

  • param[in]:无

  • return:参数结构体

  • exception:无

  • note:无

  • remarks:无

UserPara A_star::get_astar_userpara()
{
    return user_trajectory;
}