Diagonal distance in 2&3 dimensions

0.引言

• D:栅格边长
• D2:正方形对角线
• D3:正方体对角线

20191030更新，理解有误，在本系统中：

• D:1
• D2:$\sqrt{2}$
• D3:$\sqrt{3}$

1.Diagonal distance in 2 dimensions

dx = abs(node.x - goal.x)
dy = abs(node.y - goal.y)
return D * (dx + dy) + (D2 - 2 * D) * min(dx, dy)

2.Diagonal distance in 3 dimensions

dx = absdiff(node.x, goal.x)
dy = absdiff(node.y, goal.y)
dz = absdiff(node.z, goal.z)
dmin = min(dx, dy, dz)
dmax = max(dx, dy, dz)
dmid = dx + dy + dz - dmin - dmax
return (D3 - D2) * dmin + (D2 - D1) * dmid + D1 * dmax

3.c++

double getHeu(GridNodePtr node1, GridNodePtr node2)
{
    double D1 = 1;
    double D2 = sqrt(2);
    double D3 = sqrt(3);
    double dx = fabs(node1->coord(0)-node2->coord(0));
    double dy = fabs(node1->coord(1)-node2->coord(1));
    double dz = fabs(node1->coord(2)-node2->coord(2));

   //Euclidean
    #define Euclidean 0
    #if Euclidean
    {
    double _Euclidean=(dx)*(dx) + (dy)*(dy)+ (dz)*(dz);
    _Euclidean=sqrt(_Euclidean);
    return _Euclidean;
    }
    #endif


    //Manhattan
    #define Manhattan 0
    #if Manhattan
    {
    double _Manhattan = dx + dy + dz;
    return _Manhattan;
    }
    #endif


    //Diagonal
    #define Diagonal 1
    #if Diagonal
    {
    double Diagonal_tmpMax = max(dx,dy);
    Diagonal_tmpMax = max(Diagonal_tmpMax,dz);

    double Diagonal_tmpMin = min(dx,dy);
    Diagonal_tmpMin = min(Diagonal_tmpMin,dz);

    double Diagonal_tmpMid = dx + dy + dz - Diagonal_tmpMax - Diagonal_tmpMin;
    double _Diagonal = (D3 - D2) * Diagonal_tmpMin + (D2 - D1) * Diagonal_tmpMid + D1 * Diagonal_tmpMax;
    return _Diagonal;
    }
    #endif
}