有关路径搜索的一个算法

由各个直线组成的路网，求一点到另一点的所有路径：

FindRateWay.h文件代码如下：

#include <list>
#include <vector>
#include <stack>
#include "GELNSG3D.h"

typedef std::vector<AcGeLineSeg3d> vecLineSeg;

//死胡同点记录
struct DeadList
{
AcGePoint3d ptOri; //参照点
AcGePoint3dArray ptDeadAry; //死胡同点（即从参照点出发的不能走的下一点）
};
typedef std::vector<DeadList> vecDeadPt;

class CFindRateWay
{
public:
CFindRateWay(std::list<AcGeLineSeg3d>& lstRate,AcGePoint3d ptStart,AcGePoint3d ptEnd);
virtual ~CFindRateWay();

//寻找所有路径（排除回路），没找到返回FALSE
BOOL FindWay(std::vector<vecLineSeg>& vecWay);

private:
//检查路径点是否可继续向下走，如果可走则返回TRUE并返回一个可走的邻接点ptNext
BOOL IsValid(AcGePoint3d pt, std::stack<AcGePoint3d>& staRatePt,std::vector<vecLineSeg>& vecWay, IN vecDeadPt& vecDead, OUT AcGePoint3d& ptNext);

//查找某点的所有邻接点
void FindPtNear(AcGePoint3d pt,AcGePoint3dArray& PtAry);

//从栈中寻找指定点，找到返回TRUE
BOOL FindPtFromStack(AcGePoint3d pt, IN std::stack<AcGePoint3d>& staPt);

//通过栈中轨迹记录到路径组中
void SaveRate(std::stack<AcGePoint3d>& staPt,std::vector<vecLineSeg>& vecWay);

//通过两点从m_lstRate中获得AcGeLineSeg3d
BOOL FindLineSegFromList(AcGePoint3d pt1, AcGePoint3d pt2, AcGeLineSeg3d& Line);

//将栈中点记录到点数组中
void SaveStaPt2PtAry(std::stack<AcGePoint3d>& staPt,AcGePoint3dArray& ptAry);

//判断从起点到pt整个路径是否已经属于成功路径结果的一部分，条件：pt不在栈中
BOOL IsPartOfSuccRate(AcGePoint3d pt, std::stack<AcGePoint3d>& staRatePt, std::vector<vecLineSeg>& vecWay);

//判断一个点pt1是否为另一个点pt2的死胡同点
BOOL IsDeadPt(AcGePoint3d pt1, AcGePoint3d pt2,IN vecDeadPt& vecDead);

std::list<AcGeLineSeg3d> m_lstRate;
AcGePoint3d m_ptStart; //出发点
AcGePoint3d m_ptEnd; //目的点

};

------------------------------------------------------------

FindRateWay.cpp文件代码如下：

// FindRateWay.cpp: implementation of the CFindRateWay class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "resource.h"
#include "FindRateWay.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

CFindRateWay::CFindRateWay(std::list<AcGeLineSeg3d>& lstRate,AcGePoint3d ptStart,AcGePoint3d ptEnd)
{
m_lstRate = lstRate;
m_ptStart = ptStart;
m_ptEnd = ptEnd;
}

CFindRateWay::~CFindRateWay()
{

}

//寻找所有路径（排除回路），没找到返回FALSE
BOOL CFindRateWay::FindWay(std::vector<vecLineSeg>& vecWay)
{
//排除出发点和目标点相同的情况
if (m_ptStart.isEqualTo(m_ptEnd))
return FALSE;

//从起点出发
AcGePoint3d ptCur = m_ptStart;
vecDeadPt vecDead;
std::stack<AcGePoint3d> staPt;
do
{
if (ptCur.isEqualTo(m_ptEnd))
{
//找到一条通路，记下所有路径
SaveRate(staPt, vecWay);
//清除死胡同点组
vecDead.clear();
//回退当前点
ptCur = staPt.top();
staPt.pop();
}
AcGePoint3d ptNext;
if (IsValid(ptCur,staPt,vecWay,vecDead,ptNext))
{
staPt.push(ptCur);
ptCur = ptNext;
}
else
{
//当前点不能继续往下走，回退
if (staPt.empty())
break;
//记录死胡同的点
if (!ptCur.isEqualTo(m_ptEnd))
{
if (!IsPartOfSuccRate(ptCur,staPt,vecWay))
{
DeadList clsDead;
clsDead.ptOri = staPt.top();
clsDead.ptDeadAry.append(ptCur);
vecDead.push_back(clsDead);
}
}
ptCur = staPt.top();
staPt.pop();
}
} while(!staPt.empty());
return (vecWay.size() == 0)?FALSE:TRUE;
}

//将栈中点记录到点数组中
void CFindRateWay::SaveStaPt2PtAry(std::stack<AcGePoint3d>& staPt,AcGePoint3dArray& ptAry)
{
std::stack<AcGePoint3d> staPt2; //临时记录栈中元素
while (!staPt.empty())
{
AcGePoint3d ptTop = staPt.top();
ptAry.append(ptTop);
staPt2.push(ptTop);
staPt.pop();
}
ptAry.reverse();
//恢复栈
while (!staPt2.empty())
{
staPt.push(staPt2.top());
staPt2.pop();
}
}

//通过栈中轨迹记录到路径组中
void CFindRateWay::SaveRate(std::stack<AcGePoint3d>& staPt,
std::vector<vecLineSeg>& vecWay)
{
AcGePoint3dArray ptAry;
SaveStaPt2PtAry(staPt, ptAry);

vecLineSeg vecRate;
for (int i=0; i<ptAry.logicalLength()-1; i++)
{
AcGeLineSeg3d line;
if (FindLineSegFromList(ptAry[i], ptAry[i+1], line))
vecRate.push_back(line);
}
AcGePoint3d ptStart = staPt.top();
AcGePoint3d ptEnd = m_ptEnd;
AcGeLineSeg3d EndLine(ptStart, ptEnd);
vecRate.push_back(EndLine);
vecWay.push_back(vecRate); //找到一条路径，加入
}

//通过两点从m_lstRate中获得AcGeLineSeg3d
BOOL CFindRateWay::FindLineSegFromList(AcGePoint3d pt1, AcGePoint3d pt2, AcGeLineSeg3d& Line)
{
std::list<AcGeLineSeg3d>::iterator iter;
for (iter=m_lstRate.begin(); iter!=m_lstRate.end(); iter++)
{
AcGeLineSeg3d line = *iter;
if (line.isOn(pt1) && line.isOn(pt2))
{
Line = line;
return TRUE;
}
}
return FALSE;
}

//检查路径点是否可继续向下走，如果可走则返回TRUE并返回一个可走的邻接点ptNext
BOOL CFindRateWay::IsValid(AcGePoint3d pt, std::stack<AcGePoint3d>& staRatePt,
std::vector<vecLineSeg>& vecWay,IN vecDeadPt& vecDead,
OUT AcGePoint3d& ptNext)
{
//找到邻接点
AcGePoint3dArray ptNearAry;
FindPtNear(pt, ptNearAry);
if (ptNearAry.logicalLength() == 0)
return FALSE;
//将当前判断点加入到栈中
staRatePt.push(pt);
for (int i=0; i<ptNearAry.logicalLength(); i++)
{
//在栈中存在这个邻接点则代表不能继续向下走
if (FindPtFromStack(ptNearAry[i], staRatePt))
continue;
//判断从起点到ptNearAry[i]整个路径是否已经属于成功路径结果的一部分
if (IsPartOfSuccRate(ptNearAry[i],staRatePt,vecWay))
continue;
//属于死胡同点找下一个
if (IsDeadPt(ptNearAry[i], pt, vecDead))
continue;
ptNext = ptNearAry[i];
staRatePt.pop();
return TRUE;
}
staRatePt.pop();
return FALSE;
}

//判断一个点pt1是否为另一个点pt2的死胡同点
BOOL CFindRateWay::IsDeadPt(AcGePoint3d pt1, AcGePoint3d pt2,IN vecDeadPt& vecDead)
{
vecDeadPt::iterator iter;
for (iter=vecDead.begin(); iter!=vecDead.end(); iter++)
{
DeadList clsDead = *iter;
if (clsDead.ptOri.isEqualTo(pt2))
{
for (int i = 0; i<clsDead.ptDeadAry.logicalLength(); i++)
{
if (clsDead.ptDeadAry[i].isEqualTo(pt1))
{
return TRUE;
}
}
}
}
return FALSE;
}

//判断从起点到pt整个路径是否已经属于成功路径结果的一部分，条件：pt不在栈中
BOOL CFindRateWay::IsPartOfSuccRate(AcGePoint3d pt, std::stack<AcGePoint3d>& staRatePt,
std::vector<vecLineSeg>& vecWay)
{
AcGePoint3dArray ptAry;
SaveStaPt2PtAry(staRatePt,ptAry);
ptAry.append(pt);

int iCount = ptAry.logicalLength();
std::vector<vecLineSeg>::iterator iter;
for (iter=vecWay.begin();iter!=vecWay.end();iter++)
{
vecLineSeg vec = *iter;
int i=0;
BOOL bNoPart = FALSE;
for (; i<vec.size(); i++)
{
AcGeLineSeg3d line = vec.at(i);
if (i>=ptAry.logicalLength()-1)
return TRUE;
if (line.isOn(ptAry[i]) && line.isOn(ptAry[i+1]))
continue;
bNoPart = TRUE;
break; //不是成功路径的一部分，循环下一条
}
if (!bNoPart)
return TRUE;
}
return FALSE;
}

//查找某点的所有邻接点
void CFindRateWay::FindPtNear(AcGePoint3d pt,AcGePoint3dArray& PtAry)
{
std::list<AcGeLineSeg3d>::iterator iter;
for (iter=m_lstRate.begin(); iter!=m_lstRate.end(); iter++)
{
AcGeLineSeg3d line = *iter;
if (line.isOn(pt) == Adesk::kTrue)
{
AcGePoint3d ptStart = line.startPoint();
AcGePoint3d ptEnd = line.endPoint();
int iFind = -1;
if (!PtAry.find(ptStart,iFind) && !pt.isEqualTo(ptStart))
PtAry.append(ptStart);
if (!PtAry.find(ptEnd,iFind) && !pt.isEqualTo(ptEnd))
PtAry.append(ptEnd);
}
}
}

//从栈中寻找指定点，找到返回TRUE
BOOL CFindRateWay::FindPtFromStack(AcGePoint3d pt, IN std::stack<AcGePoint3d>& staPt)
{
std::stack<AcGePoint3d> staPt2; //用来临时存放栈元素
BOOL bFind = FALSE;
while (!staPt.empty())
{
AcGePoint3d ptItem = staPt.top();
if (ptItem.isEqualTo(pt))
{
bFind = TRUE;
break;
}
staPt2.push(ptItem);
staPt.pop();
}
//将staPt2中元素依次压回
while (!staPt2.empty())
{
staPt.push(staPt2.top());
staPt2.pop();
}

return bFind;
}

---------------------------------

调用方式：

// This is command 'DEMOCMD'
//命令用来构建道路各段，
void ARXDemoDemoCmd()
{
// TODO: Implement the command
std::list<AcGeLineSeg3d> lstRate;
AcGeLineSeg3d line1(AcGePoint3d(300,0,0),AcGePoint3d(300,100,0));
lstRate.push_back(line1);

AcGeLineSeg3d line2(AcGePoint3d(100,100,0),AcGePoint3d(100,500,0));
lstRate.push_back(line2);

AcGeLineSeg3d line3(AcGePoint3d(100,100,0),AcGePoint3d(300,100,0));
lstRate.push_back(line3);

AcGeLineSeg3d line4(AcGePoint3d(0,500,0),AcGePoint3d(100,500,0));
lstRate.push_back(line4);

AcGeLineSeg3d line5(AcGePoint3d(100,500,0),AcGePoint3d(200,500,0));
lstRate.push_back(line5);

AcGeLineSeg3d line6(AcGePoint3d(200,400,0),AcGePoint3d(200,500,0));
lstRate.push_back(line6);

AcGeLineSeg3d line7(AcGePoint3d(200,500,0),AcGePoint3d(400,500,0));
lstRate.push_back(line7);

AcGeLineSeg3d line8(AcGePoint3d(200,400,0),AcGePoint3d(400,400,0));
lstRate.push_back(line8);

AcGeLineSeg3d line9(AcGePoint3d(200,300,0),AcGePoint3d(200,400,0));
lstRate.push_back(line9);

AcGeLineSeg3d line10(AcGePoint3d(200,200,0),AcGePoint3d(200,300,0));
lstRate.push_back(line10);

AcGeLineSeg3d line11(AcGePoint3d(200,300,0),AcGePoint3d(400,300,0));
lstRate.push_back(line11);

AcGeLineSeg3d line12(AcGePoint3d(200,200,0),AcGePoint3d(400,200,0));
lstRate.push_back(line12);

AcGeLineSeg3d line13(AcGePoint3d(400,200,0),AcGePoint3d(400,300,0));
lstRate.push_back(line13);

AcGeLineSeg3d line14(AcGePoint3d(400,300,0),AcGePoint3d(400,400,0));
lstRate.push_back(line14);

AcGeLineSeg3d line15(AcGePoint3d(400,400,0),AcGePoint3d(400,500,0));
lstRate.push_back(line15);

AcGeLineSeg3d line16(AcGePoint3d(400,500,0),AcGePoint3d(600,500,0));
lstRate.push_back(line16);

AcGeLineSeg3d line17(AcGePoint3d(400,400,0),AcGePoint3d(600,400,0));
lstRate.push_back(line17);

AcGeLineSeg3d line18(AcGePoint3d(400,300,0),AcGePoint3d(600,300,0));
lstRate.push_back(line18);

AcGeLineSeg3d line19(AcGePoint3d(400,200,0),AcGePoint3d(600,200,0));
lstRate.push_back(line19);

AcGeLineSeg3d line20(AcGePoint3d(600,400,0),AcGePoint3d(600,500,0));
lstRate.push_back(line20);

AcGeLineSeg3d line21(AcGePoint3d(600,300,0),AcGePoint3d(600,400,0));
lstRate.push_back(line21);

AcGeLineSeg3d line22(AcGePoint3d(600,200,0),AcGePoint3d(600,300,0));
lstRate.push_back(line22);

AcGeLineSeg3d line23(AcGePoint3d(600,100,0),AcGePoint3d(600,200,0));
lstRate.push_back(line23);

AcGeLineSeg3d line24(AcGePoint3d(600,100,0),AcGePoint3d(700,100,0));
lstRate.push_back(line24);

AcGeLineSeg3d line25(AcGePoint3d(600,500,0),AcGePoint3d(700,500,0));
lstRate.push_back(line25);

AcGeLineSeg3d line26(AcGePoint3d(300,100,0),AcGePoint3d(500,100,0));
lstRate.push_back(line26);

AddAllRacePt();

//按lst创建实体
g_ObjIdAry.setLogicalLength(0);
CreateLineEnt(g_ObjIdAry, lstRate);

//调用搜索类查找所有路径
CDlgSetRacePos dlg;
if (dlg.DoModal() == IDOK)
{
AcGePoint3d ptStart = g_pt[dlg.m_iStartPtIndex];
AcGePoint3d ptEnd = g_pt[dlg.m_iEndPtIndex];
CFindRateWay clsFindRate(lstRate,ptStart, ptEnd);
g_vecRateWay.clear();
clsFindRate.FindWay(g_vecRateWay);
acutPrintf(_T("\n总共%d条路"), g_vecRateWay.size());

ShowRace();
}
}