public ChannelInfo(PathNodeInfo start, PathNodeInfo dest, double direction, double distance)
{
this.placeStart = start;
this.placeDestination = dest;
this.direction = direction;
this.distance = distance;
this.placeStart.Channels.Add(dest, this);
}
/// <summary>
/// return 값이 true 일 경우 사이클링 발생상태.
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public bool CycleCheck(PathNodeInfo node)
{
if (Parent != null)
{
return(Channel.StartingPoint == node || Parent.CycleCheck(node));
}
return(Channel.StartingPoint == node);
}
public PathNodeInfo(PathNodeInfo basePlace, double direction, double distance, string name)
: this()
{
this.type = PathNodeType.Relative;
this.basePlace = basePlace;
this.direction = direction;
this.distance = distance;
this.position_x = distance * Math.Cos(direction);
this.position_y = distance * Math.Sin(direction);
this.name = name;
}
/// <summary>
/// Dijkstra Algorithm 을 이용한 최단거리 탐색
/// </summary>
/// <param name="start"></param>
/// <param name="dest"></param>
/// <returns></returns>
public List <ChannelInfo> GetPath(PathNodeInfo start, PathNodeInfo dest)
{
DijkstraNode seedNode = new DijkstraNode(new ChannelInfo(new PathNodeInfo(), start, 0, 0), null);
DijkstraNode shortestPath = SearchPath(seedNode, dest);
if (shortestPath == null)
{
return(null);
}
return(shortestPath.ChannelList);
}
public PathNodeInfo()
{
this.name = "";
this.type = PathNodeType.Absolute;
this.basePlace = this;
this.position_x = 0;
this.position_y = 0;
this.direction = 0;
this.distance = 0;
this.landmarkID = 0;
this.channels = new Dictionary <PathNodeInfo, ChannelInfo>();
}
public RobotStatus()
{
IsInitialized = false;
XPosition = 0;
YPosition = 0;
Direction = 0;
LeftEncoderCount = 0;
RightEncoderCount = 0;
DatumPoint = new PathNodeInfo(0, 0, 0, "Start Point");
AreaID = 0;
TargetPoint = null;
Destination = null;
}
public PathNodeInfoData(PathNodeInfo node)
: this()
{
this.name = node.Name;
this.type = node.Type;
this.basePlace = node.BasePlace != null ? node.BasePlace.Name : "";
this.position_x = node.XPosition;
this.position_y = node.YPosition;
this.direction = node.Direction;
this.distance = node.Distance;
foreach (ChannelInfo channel in node.Channels.Values)
{
this.channels.Add(new ChannelInfoData(channel));
}
}
public PathNodeInfo GetNode(string name)
{
Dictionary <string, PathNodeInfo> dicNodes = new Dictionary <string, PathNodeInfo>();
foreach (PathNodeInfo node in listNode)
{
dicNodes.Add(node.Name, node);
}
PathNodeInfo pni = null;
if (!dicNodes.TryGetValue(name, out pni))
{
return(null);
}
return(pni);
}
private DijkstraNode SearchPath(DijkstraNode node, PathNodeInfo dest)
{
DijkstraNode resultNode = null;
if (node.Node == dest)
{
// node가 목표 노드일 경우 현 노드의 탐색 종료
return(node);
}
foreach (ChannelInfo channel in node.Node.Channels.Values)
{
DijkstraNode childnode = new DijkstraNode(channel, node);
if (!childnode.CycleCheck())
{
// 싸이클링 되지 않는 경우에만 노드 검색을 계속 한다.
DijkstraNode searchedNode = SearchPath(childnode, dest);
if (searchedNode != null)
{
if (resultNode == null)
{
// 기 검색된 노드가 없을 경우 새노드를 결과 노드로...
resultNode = searchedNode;
}
else
{
// 이전 결과 노드와 새 노드의 거리 비교
if (resultNode.PathLength > searchedNode.PathLength)
{
resultNode = searchedNode;
}
else if (resultNode.PathLength == searchedNode.PathLength)
{
// 이전 결과노드와 새노드 거리가 같을 경우는 경로 수 비교
if (resultNode.PathCount > searchedNode.PathCount)
{
resultNode = searchedNode;
}
}
}
}
}
}
return(resultNode);
}
public List <ChannelInfo> GetPath(string start, string dest)
{
Dictionary <string, PathNodeInfo> dicNodes = new Dictionary <string, PathNodeInfo>();
foreach (PathNodeInfo node in listNode)
{
dicNodes.Add(node.Name, node);
}
PathNodeInfo startnode = GetNode(start);
PathNodeInfo destnode = GetNode(dest);
if (startnode == null || destnode == null)
{
return(null);
}
return(GetPath(startnode, destnode));
}
public bool LoadMap(string path)
{
// XML Serialize
PathNodeMapData mapdata;
XmlSerializer xs = new XmlSerializer(typeof(PathNodeMapData));
StreamReader sr = new StreamReader(path);
mapdata = xs.Deserialize(sr) as PathNodeMapData;
sr.Close();
if (mapdata == null)
{
return(false);
}
// 정보 초기화
List <PathNodeInfo> listNode = new List <PathNodeInfo>();
List <ChannelInfo> listChannel = new List <ChannelInfo>();
// basePlace 의 cycling 이 발생할 수도 있기 때문에 Node 의 생성과 BasePlace 연동을 구분하여 수행한다.
Dictionary <string, PathNodeInfo> dicNodes = new Dictionary <string, PathNodeInfo>();
// Node 정보 적용
foreach (PathNodeInfoData nodedata in mapdata.nodes)
{
PathNodeInfo node = new PathNodeInfo(nodedata.type, nodedata.position_x, nodedata.position_y, nodedata.direction, nodedata.distance, nodedata.landmarkID, nodedata.name);
dicNodes.Add(node.Name, node);
listNode.Add(node);
}
// BasePlace 연동
foreach (PathNodeInfoData nodedata in mapdata.nodes)
{
//NodeInfo node = new NodeInfo(nodedata.type, nodedata.position_x, nodedata.position_y, nodedata.direction, nodedata.distance, nodedata.name);
PathNodeInfo node;
if (!dicNodes.TryGetValue(nodedata.name, out node))
{
return(false);
}
PathNodeInfo baseNode;
if (!dicNodes.TryGetValue(nodedata.basePlace, out baseNode))
{
return(false);
}
node.BasePlace = baseNode;
}
// Channel 설정
foreach (PathNodeInfoData nodedata in mapdata.nodes)
{
PathNodeInfo start;
if (!dicNodes.TryGetValue(nodedata.name, out start))
{
return(false);
}
foreach (ChannelInfoData channeldata in nodedata.channels)
{
PathNodeInfo dest;
if (!dicNodes.TryGetValue(channeldata.PlaceDestination, out dest))
{
return(false);
}
listChannel.Add(new ChannelInfo(start, dest, channeldata.Direction, channeldata.Distance));
}
}
// 맵 정보 교체
this.listNode = listNode;
this.listChannel = listChannel;
return(true);
}
