A*算法启发式函数的几种距离

欧氏距离

double AstarPathFinder::getDistance_E(Vector3d n1 , Vector3d n2){
    //Euclidean
    double dis_E =  sqrt((n2(0)-n1(0))*(n2(0)-n1(0))+(n2(1)-n1(1))*(n2(1)-n1(1))+(n2(2)-n1(2))*(n2(2)-n1(2)));

    return dis_E;
}

曼哈顿距离

double AstarPathFinder::getDistance_M(Vector3d n1 , Vector3d n2){
    //Manhattan
    double dis_M =  fabs(n2(0)-n1(0))+ fabs(n2(1)-n1(1))+ fabs(n2(2)-n1(2));

    return dis_M;
}

对角距离

实质是 sqrt(3) * d_min + sqrt(2) * (d_mid - d_min) + (d_max - d_mid)

double AstarPathFinder::getDistance_D(Vector3d n1 , Vector3d n2){
    //diagonal:  3D
    double dis_D ;

    double dx = fabs(n1(0) - n2(0));
    double dy = fabs(n1(1) - n2(1));
    double dz = fabs(n1(2) - n2(2));
    double dmin = min( min(dx, dy), dz);
    double dmax = max(max(dx, dy), dz);
    double dmid = dx + dy + dz - dmin - dmax;
    dis_D = (sqrt(3) - sqrt(2))*dmin + (sqrt(2) - 1)*dmid + dmax;

    return dis_D;
}

0：Dijkstra算法

double AstarPathFinder::getDistance_0(Vector3d n1 , Vector3d n2){
    //Dijkstra
    double dis =  0;

    return dis;
}