/// <summary>
/// 加载反光柱的图层
/// </summary>
/// <returns></returns>
public static GraphAevironment LoadTheLightMsg()
{
GraphAevironment graph = null;
try
{
string[] read = File.ReadAllLines(Application.StartupPath + @"\TheLightInformation.ini");
graph = new GraphAevironment(read.Length);
double [,] TEM = new double[read.Length, read.Length];
for (int i = 0; i < read.Length; i++)
{
TheLightNode node = JsonConvert.DeserializeObject <TheLightNode>(read [i]);
graph.SetNode(node.NumberNo - 1, node); //添加节点
}
for (int J = 0; J < graph.NODE.Length; J++)
{
for (int K = J; K < graph.NODE.Length; K++)
{
if (J != K)
{
double len = Math.Sqrt(Math.Pow(graph.NODE[J].Polar, 2) + Math.Pow(graph.NODE [K].Polar, 2) - 2 * graph.NODE[J].Polar * graph.NODE[K].Polar * Math.Cos(Math.PI * (graph.NODE[J].Angle - graph.NODE[K].Angle) / 180));
graph.SetMatrix(J, K, len); //添加边
}
}
}
TEM = graph.MATRIX;
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString());
}
return(graph);
}
void ComputeMag()
{
if (LidarMode.GraphCreateState)
{
for (int i = 0; i < Polar.Length; i++)
{
TheLightNode node = new TheLightNode();
if (Polar [i] != 0 && Polar [i] != 0)
{
node.X = Polar [i] * Math.Cos(Math.PI * Angle [i] / 180);
node.Y = Polar [i] * Math.Sin(Math.PI * Angle [i] / 180);
node.NumberNo = (byte)(nodelist.Count + 1);
node.Angle = Angle [i];
node.Polar = Polar [i];
nodelist.Add(node);
}
}
}
else
{
if (LidarMode.IsNavicationMode)
{
for (int l = 0; l < agv.IsBatter.Count; l++)
{
if (agv.IsBatter[l][1] != 0 && agv.IsBatter [l][2] != 0)
{
TheLightNode node = new TheLightNode();
node.NumberNo = (byte )agv.IsBatter [l][0];
node.X = agv.IsBatter [l][1];
node.Y = agv.IsBatter [l][2];
node.Angle = agv.IsBatter [l][3];
node.Polar = agv.IsBatter [l][4];
nodelist.Add(node);
}
}
}
if (LidarMode.TargetDetectionState)
{
for (int j = 0; j < agv.NotBatter.Count; j++)
{
TheLightNode node = new TheLightNode();
node.NumberNo = (byte )agv.NotBatter [j][0];
node.X = agv.NotBatter [j][1];
node.Y = agv.NotBatter [j][2];
node.Angle = agv.NotBatter [j][3];
node.Polar = agv.NotBatter [j][4];
nodelist.Add(node);
}
}
}
}
/// <summary>
///
/// </summary>
/// <param name="edge"></param>
/// <returns></returns>
public TheLightNode[] GetCurrentWorldCoordinate(double [,] edge)
{
TheLightNode [] wordxy = new TheLightNode[edge.GetLength(0)];
int k = 0;
for (int i = 0; i < edge.GetLength(0); i++)
{
List <TheLightNode> nodelist = new List <TheLightNode> ();
for (int j = 0; j < edge.GetLength(1); j++)
{
if (i != j && edge [i, j] != 0)
{
TheLightNode[] node = new TheLightNode [2];
node [0] = new TheLightNode();
node [1] = new TheLightNode();
node = GetEdgeNode(edge [i, j]);
if (node != null)
{
nodelist.Add(node[0]);
nodelist.Add(node [1]);
}
}
}
var res = from n in nodelist
group n by n into g
orderby g.Count()
descending
select g;
if (nodelist.Count >= 4)
{
var gr = res.First();
foreach (TheLightNode itenn in gr)
{
wordxy[k] = new TheLightNode();
wordxy [k] = itenn;
k++;
break;
}
}
else
{
wordxy[k] = new TheLightNode();
wordxy [k] = null;
k++;
}
}
return(wordxy);
}
/// <summary>
/// 删除边
/// </summary>
/// <param name="node1"></param>
/// <param name="node2"></param>
public void DeleEdge(TheLightNode node1, TheLightNode node2)
{
if (!IsNode(node1) || !IsNode(node2))
{
MessageBox.Show("没有加入此节点反光柱！");
return;
}
else
{
Matrix [GetNodeIndex(node1), GetNodeIndex(node2)] = 0;
Matrix [GetNodeIndex(node2), GetNodeIndex(node1)] = 0;
--numEdge;
}
}
/// <summary>
/// 添加边
/// </summary>
/// <param name="node1"></param>
/// <param name="node2"></param>
/// <param name="value"></param>
public void SetEdge(TheLightNode node1, TheLightNode node2, double value)
{
if (!IsNode(node1) || !IsNode(node2))
{
MessageBox.Show("没有加入此节点反光柱！");
return;
}
else
{
Matrix [GetNodeIndex(node1), GetNodeIndex(node2)] = value;
Matrix [GetNodeIndex(node2), GetNodeIndex(node1)] = value;
++numEdge;
}
}
/// <summary>
/// 判断是否是其节点
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public bool IsNode(TheLightNode node)
{
foreach (TheLightNode item in Node)
{
if (-Para.XYERROR <= (item.X - node.X) && (item.X - node.X) <= Para.XYERROR)
{
if (-Para.XYERROR <= (item.Y - node.Y) && (item.Y - node.Y) <= Para.XYERROR)
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// 获取节点的编号
/// </summary>
/// <param name="node"></param>
/// <returns></returns>
public int GetNodeIndex(TheLightNode node)
{
int indexx = -1;
for (int i = 0; i < Node.Length; i++)
{
if (-Para.XYERROR <= (Node[i].X - node.X) && (Node[i].X - node.X) <= Para.XYERROR)
{
if (-Para.XYERROR <= (Node[i].Y - node.Y) && (Node[i].Y - node.Y) <= Para.XYERROR)
{
indexx = i;
}
}
}
return(indexx);
}
/// <summary>
/// 根据两个反光柱之间的距离提取两个反光柱的坐标信息
/// </summary>
/// <param name="value"></param>
/// <returns></returns>
public TheLightNode [] GetEdgeNode(double value)
{
TheLightNode[] node = new TheLightNode[2];
node [0] = new TheLightNode();
node [1] = new TheLightNode();
for (int i = 0; i < Matrix.GetLength(0); i++)
{
for (int j = 0; j < Matrix.GetLength(1); j++)
{
if (-Para.LENERROR <= (Matrix [i, j] - value) && (Matrix [i, j] - value) <= Para.LENERROR)
{
node [0] = GetNode(i);
node [1] = GetNode(j);
}
}
}
return(node);
}
/// <summary>
/// 判断两个反关柱之间是否添加边
/// </summary>
/// <param name="node1"></param>
/// <param name="node2"></param>
/// <returns></returns>
public bool IsExistEdge(TheLightNode node1, TheLightNode node2)
{
if (!IsNode(node1) || !IsNode(node2))
{
MessageBox.Show("没有加入此节点反光柱！");
return(false);
}
else
{
if (Matrix [GetNodeIndex(node1), GetNodeIndex(node2)] == 0)
{
return(false);
}
else
{
return(true);
}
}
}
/// <
/// summary>
///
/// </summary>
void GetAGVLocation()
{
if (agv.Polar != null && agv.Angle != null)
{
int [] poalte = agv.Polar.ToList().Distinct().ToArray();
float [] antem = agv.Angle.ToList().Distinct().ToArray();
double [,] edge = new double[poalte.Length, poalte.Length];
TheLightNode [] worldcoord = new TheLightNode[poalte.Length];
for (int i = 0; i < poalte.Length; i++)
{
worldcoord [i] = new TheLightNode();
}
edge = GetEdge(poalte, antem);
worldcoord = G.GetCurrentWorldCoordinate(edge);
GetBatter(worldcoord, poalte, antem);
AgvAbsoluteLocation agvlocation = new AgvAbsoluteLocation(ref agv);
if (LidarMode.TargetDetectionState) //靶标探测模式
{
SingleAgv addbiaoba = new SingleAgv(agv);
}
}
GC.Collect();
}
/// <summary>
/// 获取靶标
/// </summary>
/// <param name="agv"></param>
public void GetBaBiao(AGV agv)
{
try {
if (agv != null)
{
this.agv = agv;
if (agv.Polar != null)
{
Polar = new double[agv.Polar.Length];
Angle = new double[agv.Angle.Length];
Array.Copy(agv.Polar, 0, Polar, 0, agv.Polar.Length);
Array.Copy(agv.Angle, 0, Angle, 0, agv.Angle.Length);
if (LidarMode.GraphCreateState)
{
for (int i = 0; i < Polar.Length; i++)
{
TheLightNode node = new TheLightNode();
if (Polar [i] != 0 && Polar [i] != 0)
{
node.X = Polar [i] * Math.Cos(Math.PI * Angle [i] / 180);
node.Y = Polar [i] * Math.Sin(Math.PI * Angle [i] / 180);
node.NumberNo = (byte)(nodelist.Count + 1);
node.Angle = Angle [i];
node.Polar = Polar [i];
nodelist.Add(node);
}
}
}
else
{
if (LidarMode.IsNavicationMode)
{
List <double []> isbarrerlist = new List <double[]> ();
if (isbarrerlist.Count == 0)
{
isbarrerlist.AddRange(agv.DisIsBatter);
}
agv.DisIsBatter.Clear();
for (int i = 0; i < isbarrerlist.Count; i++)
{
TheLightNode node = new TheLightNode();
if (isbarrerlist != null)
{
if (isbarrerlist [i][0] != 0 && isbarrerlist [i][1] != 0)
{
node.X = isbarrerlist [i][1];
node.Y = isbarrerlist [i][2];
node.NumberNo = (byte )isbarrerlist [i][0];
node.Angle = isbarrerlist [i][3];
node.Polar = isbarrerlist [i][4];
isnodelist.Add(node);
}
}
}
}
if (LidarMode.TargetDetectionState)
{
List <double []> isbarrerlist = new List <double[]> ();
if (isbarrerlist.Count == 0)
{
isbarrerlist.AddRange(agv.DisIsBatter);
}
for (int i = 0; i < isbarrerlist.Count; i++)
{
TheLightNode node = new TheLightNode();
if (isbarrerlist != null)
{
if (isbarrerlist [i][0] != 0 && isbarrerlist [i][1] != 0)
{
node.X = isbarrerlist [i][1];
node.Y = isbarrerlist [i][2];
node.NumberNo = (byte )isbarrerlist [i][0];
node.Angle = isbarrerlist [i][3];
node.Polar = isbarrerlist [i][4];
isnodelist.Add(node);
}
}
}
List <double []> notbarrerlist = new List <double[]> ();
if (notnodelist.Count == 0)
{
notbarrerlist.AddRange(agv.NotBatter);
}
for (int i = 0; i < notbarrerlist.Count; i++)
{
TheLightNode node = new TheLightNode();
if (notbarrerlist != null && notbarrerlist [i][0] != 0 && notbarrerlist [i][1] != 0)
{
node.X = notbarrerlist [i][1];
node.Y = notbarrerlist [i][2];
node.NumberNo = (byte )notbarrerlist [i][0];
node.Angle = notbarrerlist [i][3];
node.Polar = notbarrerlist [i][4];
notnodelist.Add(node);
}
}
agv.DisIsBatter.Clear();
agv.NotBatter.Clear();
}
}
}
}
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString());
}
}
/// <summary>
/// 设置定点信息
/// </summary>
/// <param name="index"></param>
/// <param name="node"></param>
public void SetNode(int index, TheLightNode node)
{
Node [index] = node;
}
