/// <summary> 搜尋最靠近車體位置的節點 </summary>
/// <param name="a_atNodeLocal">[IN] node list </param>
/// <param name="a_tCarInfo">[IN] car information </param>
/// <returns> The closest node </returns>
public static NodeId rtAGV_FindCurrentNode(rtAGV_MAP_node[] a_atNodeLocal, rtCarData a_tCarInfo)
{
NodeId tNodeId = new NodeId();
double MinDistance = EMPTY_DATA;
for (int i = 0; i < a_atNodeLocal.Length; i++)
{
double EachDistance = rtVectorOP_2D.GetDistance(a_atNodeLocal[i].tCoordinate, a_tCarInfo.tPosition);
if (EachDistance <= MinDistance)
{
MinDistance = EachDistance;
tNodeId = a_atNodeLocal[i].tNodeId;
}
}
return tNodeId;
}
/// <summary> Find Node list of Path to Destination </summary>
/// <param name="a_lSrc">[IN] source node </param>
/// <param name="a_lDst">[IN] Destination node </param>
/// <param name="a_atNodeLocal">[IN] node information in current region </param>
/// <param name="a_alMapCurrent">[IN] look up table of node in current region </param>
/// <param name="a_lNodeNum">[IN] node number in current region </param>
/// <param name="a_lPathLength">[INOUT] wanted path size </param>
/// <param name="a_atPathInfo">[INOUT] wanted path </param>
public static void rtAGV_FindPathNode2Dest(int a_lSrc, int a_lDst, rtAGV_MAP_node[] a_atNodeLocal, int[] a_alMapCurrent, int a_lNodeNum, ref int a_lPathLength, ref rtPath_Info[] a_atPathInfo)
{
int lCnt = 0;
int[] alPathResult = new int[0];
alPathResult = FindPathofNode(a_lSrc, a_lDst, a_alMapCurrent, a_lNodeNum, ref a_lPathLength);
a_lPathLength--; // 路徑段數比節點數少1
a_atPathInfo = rtPath_Info.InitSet(a_lPathLength);
for (lCnt = 0; lCnt < a_lPathLength; lCnt++)
{
a_atPathInfo[lCnt].tSrc.eX = a_atNodeLocal[alPathResult[lCnt]].tCoordinate.eX;
a_atPathInfo[lCnt].tSrc.eY = a_atNodeLocal[alPathResult[lCnt]].tCoordinate.eY;
a_atPathInfo[lCnt].tDest.eX = a_atNodeLocal[alPathResult[lCnt+1]].tCoordinate.eX;
a_atPathInfo[lCnt].tDest.eY = a_atNodeLocal[alPathResult[lCnt+1]].tCoordinate.eY;
}
}
