//初始化地图，在地图被更改之后调用
public void InitEngineForMap(IPositionSet_Connected mapPositionSet_Connected)
{
this.mapPositionSet_Connected = mapPositionSet_Connected;
M2MSCreater_ForGeneralM2MStruture m2m_Creater_ForGeneralM2MStruture = new M2MSCreater_ForGeneralM2MStruture();
m2m_Creater_ForGeneralM2MStruture.PartType = typeof(Part_Multi);
m2m_Creater_ForGeneralM2MStruture.SetPointInPartFactor(50);
m2m_Creater_ForGeneralM2MStruture.SetUnitNumInGridLength(3);
m2mStructure = m2m_Creater_ForGeneralM2MStruture.CreateAutomatically(mapPositionSet_Connected);
m2mStructure.Preprocessing(mapPositionSet_Connected);
BuildPartSetConnectionForM2MStructure buildPartSetConnectionForM2MStructure = new BuildPartSetConnectionForM2MStructure();
buildPartSetConnectionForM2MStructure.TraversalEveryLevelAndBuild(m2mStructure);
#region code for algorithm demo
if (GetM2MStructureInPreprocess != null)
{
GetM2MStructureInPreprocess(m2mStructure);
}
#endregion
int num = (int)(Math.Sqrt((double)mapPositionSet_Connected.GetNum()));
path = new List <IPosition_Connected>(num * 2);
if (num > 0)
{
open = new PriorityQueue <IPosition_Connected>(num * 4, com);
}
else
{
open = new PriorityQueue <IPosition_Connected>(com);
}
}
//初始化地图，在地图被更改之后调用
public void InitEngineForMap(IPositionSet_Connected map)
{
positionSet = map;
IPosition_Connected p;
//初始化
positionSet.InitToTraverseSet();
while (positionSet.NextPosition())
{
p = positionSet.GetPosition_Connected();
p.SetAttachment(new Tag());
}
int num = (int)(Math.Sqrt((double)positionSet.GetNum()));
path = new List <IPosition_Connected>(num * 2);
if (num > 0)
{
open = new PriorityQueue <IPosition_Connected>(num * 4, com);
}
else
{
open = new PriorityQueue <IPosition_Connected>(com);
}
}
//没有路径则返回null
public List <IPosition_Connected> SearchPath(IPosition_Connected start, IPosition_Connected end)
{
List <IPosition_Connected> path = new List <IPosition_Connected>();
if (start == end)
{
path.Add(start);
return(path);
}
//初始化
if (!Init(start, end))
{
return(null);
}
IPosition_Connected p = null, p_adj;
IPositionSet_Connected_Adjacency adj_set;
Tag p_tag, p_adj_tag;
float newF, newG, dx, dy;
IPriorityQueue <IPosition_Connected> open;
int num = positionSet.GetNum();
if (num > 0)
{
open = new PriorityQueue <IPosition_Connected>(num, com);
}
else
{
open = new PriorityQueue <IPosition_Connected>(com);
}
//int count = 0;
//if (debug)
// Console.WriteLine("SearchPath:");
//将起点加入open表
open.add(start);
//当open表非空时
while (open.getSize() > 0)
{
//获得下一个最近的点
p = open.removeFirst();
//到达终点则结束
if (p == end)
{
break;
}
p_tag = (Tag)p.GetAttachment();
p_tag.closed = true;
adj_set = p.GetAdjacencyPositionSet();
adj_set.InitToTraverseSet();
//if (debug)
//{
// count++;
// Console.Write(p.ToString() + "\t");
//}
while (adj_set.NextPosition())
{
p_adj = adj_set.GetPosition_Connected();
p_adj_tag = (Tag)p_adj.GetAttachment();
if (p_adj_tag.timeStamp != time_stamp)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
newF = newG + evaluate(p_adj.GetX(), p_adj.GetY(), end.GetX(), end.GetY());
p_adj_tag.parent = p;
p_adj_tag.g = newG;
p_adj_tag.f = newF;
p_adj_tag.closed = false;
p_adj_tag.timeStamp = time_stamp;
open.add(p_adj);
}
else
{
if (!p_adj_tag.closed)
{
newG = p_tag.g + adj_set.GetDistanceToAdjacency();
dx = end.GetX() - p_adj.GetX();
dy = end.GetY() - p_adj.GetY();
newF = newG + evaluate(p_adj.GetX(), p_adj.GetY(), end.GetX(), end.GetY());
if (newF < p_adj_tag.f)
{
p_adj_tag.parent = p;
p_adj_tag.g = newG;
p_adj_tag.f = newF;
open.update(p_adj);
}
}
}
//if (debug)
// Console.Write(p_adj.ToString() + ":" + p_adj.GetAttachment().ToString() + "\t");
}
//if (debug)
// Console.WriteLine();
}
//if (debug)
//{
// Console.Write("position count:");
// Console.WriteLine(count);
//}
//从终点根据标签中记录的父节点找到起点，生成路径
if (p == end)
{
path.Add(end);
p_tag = (Tag)end.GetAttachment();
while (p_tag.parent != null)
{
path.Add(p_tag.parent);
p_tag = (Tag)p_tag.parent.GetAttachment();
}
path.Reverse();
return(path);
}
else
{
return(null);
}
}