迷宫寻路的等高线算法

 对于迷宫寻路的一般算法是用递归实现，或者用栈来实现，二者其实基本思路都一样：由起点开始，逐个点遍历上下左右，重复遍历率高，效率一般。还有就是A×算法，不过掌握起来难度较大。现介绍一种效率高，同时又较简单的寻路算法：等高线算法。

所谓等高线算法，主要原理是：由终点开始延伸，每个点的四个方向相临点属于同一个高度，将每个点到终点的最小距离记录到辅助数组的相应位置，如果有权值的话每走一步加的是权值，否则直接加加，这样所有位置到终点的距离都在辅助数组里面，最后只要到辅助数组里面去收集最短路径就可以了。

此算法同样适用于有障碍物的地图寻路，这里只给出了四联通的，此算法同样适合八个方向的情况。算法代码如下：

//  MapContour.cpp : 定义控制台应用程序的入口点。
// 算法原理: 从终点往外开始延伸, 直到起始点,
//         再从起始点选择最小的号码, 逐步按照路径行走, 回到终点,即算完成
#include  < iostream >
#include  < queue >

using   namespace  std;

#define  D_MapWidth    15
#define  D_MapHeight 13
#define  D_MapSize    (D_MapWidth * D_MapHeight)

// 数组MapData记录地图中的原始数据;
// 1表示有路可走;
// 0表示没有路相通
int  MapData[D_MapHeight  *  D_MapWidth]  =  
... {
    1,1,1,1,0,0,0,0,0,1,1,1,1,1,1,
        1,1,1,1,0,0,0,0,1,1,1,1,1,1,1,
        1,1,1,1,0,0,1,1,1,1,1,1,1,1,1,
        1,1,1,0,0,1,1,1,1,1,0,1,1,1,1,
        1,1,0,0,0,1,1,1,0,0,0,1,1,1,1,
        1,1,1,1,1,1,1,0,0,0,1,1,1,1,0,
        0,1,1,1,1,1,1,0,0,1,1,1,1,0,0,
        0,1,1,1,1,1,0,0,1,1,1,1,0,0,0,
        0,0,1,1,1,0,0,1,1,1,0,0,0,0,0,
        0,0,0,1,0,0,0,1,1,1,0,1,1,1,1,
        0,0,0,0,0,1,1,1,1,1,0,1,1,1,0,
        0,0,0,0,1,1,1,1,1,1,1,1,1,0,0,
        0,0,0,0,1,1,1,1,1,1,1,0,0,0,0
} ;
// 计算用的地图数组
// BackupMap数组记录地图上的各点到达起始点的最小步数;
int  BackupMap[D_MapWidth  *  D_MapHeight];


// 记录路径的数组
// Path数组依次记录:结果路径的顶点;
int  Path[D_MapHeight  *  D_MapWidth];
// 为Path数组的容量;
int  CountOfPath  =   0 ;

bool  SetpMaker( int  Position,  int  StepNo,  int  CheckPosition);
void  QuickMapping( int  Pos_Target,  int  Pos_Start);
void  PathRecorder( int  StartPos,  int  TargetPos);
int  GetStep( int  Position,  int   & StepNo);
void  ShowPath( void );

// 由于STL的queue的函数都是正交的，
// 将Pop函数与front函数组合返回队列是非为空；
template < typename T >
bool  IsNullAfterPop(queue < T >   * que, T &  current)
... {
    if (que->empty())
    ...{
        return false;
    }
    current = que->front();
    que->pop();
    return true;
}

int  main()
... {
    //起点，终点；
    int Pos_Start = (10 * D_MapWidth) + 12;
    int Pos_Target = (1 * D_MapWidth) + 1;

    bool Found = false;
    int Step = 1;
    int Mapsize = D_MapSize;

    //初始化辅助数组；
    memset(BackupMap, 0, sizeof(int)* Mapsize);
    
    //将每个位置到终点的距离写到辅助数组BackupMap里面
    QuickMapping(Pos_Target, Pos_Start);
    //输出辅助数组内容：
    for (int i = 0; i < D_MapHeight; i++)
    ...{
        for (int j = 0; j < D_MapWidth; j++)
        ...{
            printf(" %02d", BackupMap[i*D_MapWidth + j]);
        }
        cout << endl;
    }
    //筛选最小路径
    PathRecorder(Pos_Start, Pos_Target);
    //显式路径；
    ShowPath();
    return 0;
}

void  QuickMapping( int  Pos_Target,  int  Pos_Start)
... {
    queue<int>QueA, QueB, *SrcPointer, *DesPointer;
    int StepNo = 1,
        Position,
        MapSize = D_MapWidth*D_MapHeight,
        CurrentPos;

    SrcPointer = &QueA;
    DesPointer = &QueB;
    SrcPointer->push(Pos_Target);

    for (;;)
    ...{
        if ( !IsNullAfterPop(SrcPointer, CurrentPos) )
        ...{
            if (SrcPointer == &QueA)    
            ...{
                SrcPointer = &QueB;
                DesPointer = &QueA;
                StepNo ++;
            }
            else
            ...{
                SrcPointer = &QueA;
                DesPointer = &QueB;
                StepNo++;
            }
            //两者都空退出；
            if ( !IsNullAfterPop(SrcPointer, CurrentPos) )    
                return;
        }

        BackupMap[CurrentPos] = StepNo;
        //找到起点；结束；
        if (CurrentPos == Pos_Start)
            return;
        
        //To UP
        Position = CurrentPos - D_MapWidth;
        if (Position >= 0)
        ...{
            if (MapData[Position] != 0)
            ...{
                if (BackupMap[Position] == 0 ||
                    BackupMap[Position] > StepNo)
                ...{
                    DesPointer->push(Position);
                }
            }
        }
        //To DOWN
        Position = CurrentPos + D_MapWidth;
        if (Position < MapSize)
        ...{
            if (MapData[Position] != 0)
            ...{
                if (BackupMap[Position] == 0 ||
                    BackupMap[Position] > StepNo)
                ...{
                    DesPointer->push(Position);
                }
            }
        }
        //To LEFT
        Position = CurrentPos - 1;
        if ( (CurrentPos%D_MapWidth) > 0 )
        ...{
            if (MapData[Position] != 0)
            ...{
                if (BackupMap[Position] == 0 ||
                    BackupMap[Position] > StepNo)
                ...{
                    DesPointer->push(Position);
                }
            }
        }
        //To RIGHT
        Position = CurrentPos + 1;
        if ( (Position%D_MapWidth) < D_MapWidth )
        ...{
            if (MapData[Position] != 0)
            ...{
                if (BackupMap[Position] == 0 ||
                    BackupMap[Position] > StepNo)
                ...{
                    DesPointer->push(Position);
                }
            }
        }

    }//for(;;)
} // QuickMapping()

// 将理想的点放进Path数组中;
void  PathRecorder( int  StartPos,  int  TargetPos)
... {
    int Step = BackupMap[StartPos];
    int index = 0;
    int Pos = StartPos;

    Path[index++] = Pos;

    do 
    ...{
        Pos = GetStep(Pos, Step);
        if (Pos != -1)
        ...{
            Path[index++] = Pos;
            if (Pos == TargetPos)
            ...{
                CountOfPath = index;
                return ;
            }
        }
        else
            return;

    } while(true);
}

// 检查所在位置的四个方向; 找出最小的位置,
// 也就是最短路径的检测方法;
int  GetStep( int  Position,  int   & StepNo)
... {
    int nUp = Position - D_MapWidth;
    int nDown = Position + D_MapWidth;
    int nLeft = Position - 1;
    int nRight = Position + 1;
    bool Found = false;

    if (nUp >= 0)
    ...{
        if (BackupMap[nUp] != 0 && BackupMap[nUp] < StepNo)
        ...{
            StepNo = BackupMap[nUp];
            return nUp;
        }
    }
    if (nDown < D_MapSize)
    ...{
        if (BackupMap[nDown] != 0 && BackupMap[nDown] < StepNo)
        ...{
            StepNo = BackupMap[nDown];
            return nDown;
        }
    }
    if ( (Position%D_MapWidth) > 0)
    ...{
        if (BackupMap[nLeft] != 0 && BackupMap[nLeft] < StepNo)
        ...{
            StepNo = BackupMap[nLeft];
            return nLeft;
        }
    }
    if ( (nRight%D_MapWidth) != 0)
    ...{
        if (BackupMap[nRight] != 0 && BackupMap[nRight] < StepNo)
        ...{
            StepNo = BackupMap[nRight];
            return nRight;
        }
    }

    return -1;
}


// 先把BackuoMap数组清空;
// 再根据Path数组将路径的数据写入到BackupMap数组中,
// 最后显示BackupMap[]数组内容;
void  ShowPath( void )
... {
    int Pos;

    memset(BackupMap, 0, sizeof(int) * D_MapSize);

    for (int loop = 0; loop < CountOfPath; loop++)
    ...{
        Pos = Path[loop];
        BackupMap[Pos] = 1;
    }

    cout << " 显示路径 ";
    for (int loopy = 0; loopy < D_MapHeight; loopy++)
    ...{
        for (int loopx = 0; loopx < D_MapWidth; loopx++)
        ...{
            printf(" %02d", BackupMap[loopy*D_MapWidth + loopx]);
        }
        cout << endl;
    }
}

核心函数QuickMapping()的实现简介：使用两个队列来遍历，为了交换两个队列内容的需要，定义了两个队列指针。原理的话：当前遍历A点，找到其四个方向的点Aup，Adown， Aleft，  Aright，此时A点是源点，而Aup，Adown， Aleft，  Aright都是目标点。但是在下一次Src队列中的源点全部找完后，即Src为空，交换Src与Dest队列的内容，他们就分别都成为源点了，这样继续记录下去。

当来两个队列都为空，或者起点找到，就退出函数。