//计算转向和前进和后退
public void AcountDirect(ref List <LandmarkInfo> Routes)
{
try
{
if (Routes.Count <= 1)
{
return;
}
IList <AllSegment> AllSegs = AGVClientDAccess.LoadAllSegment();
DataTable dtCoor = AGVClientDAccess.LoadAGVCoordinate();
LandmarkInfo UpLand = null;
LandmarkInfo InflectLand = null;
LandmarkInfo NextLand = null;
int IsCountAngel = 0;
SysParameter sys = AGVClientDAccess.GetParameterByCode("CountTurnType");
if (sys != null && sys.ParameterValue == "角度")
{
IsCountAngel = 1;
}
if (Routes.Count >= 3)
{
for (int i = 2; i < Routes.Count; i++)
{
UpLand = Routes[i - 2];
InflectLand = Routes[i - 1];
NextLand = Routes[i];
if (i != Routes.Count - 1)
{
#region 计算转向
if (IsCountAngel == 0)
{
AllSegment Segment = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Segment != null && Segment.ExcuteTurnDirect != -1)
{
UpLand.sway = (SwayEnum)Segment.ExcuteTurnDirect;
}
else
{
if (AllSegs.Count(p => p.BeginLandMakCode == UpLand.LandmarkCode) > 1)
{
//double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
double SP = (UpLand.LandX - NextLand.LandX) * (InflectLand.LandY - NextLand.LandY) - (UpLand.LandY - NextLand.LandY) * (InflectLand.LandX - NextLand.LandX);
if (SP > 0.05)
{
UpLand.sway = SwayEnum.Left;
}
else if (SP < -0.05)
{
UpLand.sway = SwayEnum.Right;
}
else
{
UpLand.sway = SwayEnum.None;
}
}
}
if (Segment != null && Segment.ExcuteMoveDirect != -1)
{
UpLand.movedirect = (MoveDirectEnum)Segment.ExcuteMoveDirect;
}
}
#endregion
#region 计算角度
else
{
//if (Math.Round(UpLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero))
if (UpLand.LandX == InflectLand.LandX)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandY > UpLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 270;
}
}
}
}
//else if (Math.Round(UpLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero))
else if (UpLand.LandY == InflectLand.LandY)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandX > UpLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 180;
}
}
}
}
}
#endregion
}
else
{
#region 计算转向
if (IsCountAngel == 0)
{
AllSegment SegmentUPLand = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (SegmentUPLand != null && SegmentUPLand.ExcuteTurnDirect != -1)
{
UpLand.sway = (SwayEnum)SegmentUPLand.ExcuteTurnDirect;
}
else
{
if (AllSegs.Count(p => p.BeginLandMakCode == UpLand.LandmarkCode) > 1)
{
//double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
double SP = (UpLand.LandX - NextLand.LandX) * (InflectLand.LandY - NextLand.LandY) - (UpLand.LandY - NextLand.LandY) * (InflectLand.LandX - NextLand.LandX);
if (SP > 0.05)
{
UpLand.sway = SwayEnum.Left;
}
else if (SP < -0.05)
{
UpLand.sway = SwayEnum.Right;
}
else
{
UpLand.sway = SwayEnum.None;
}
}
}
if (SegmentUPLand != null && SegmentUPLand.ExcuteMoveDirect != -1)
{
UpLand.movedirect = (MoveDirectEnum)SegmentUPLand.ExcuteMoveDirect;
}
AllSegment Segment = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Segment != null && Segment.ExcuteTurnDirect != -1)
{
InflectLand.sway = (SwayEnum)Segment.ExcuteTurnDirect;
}
if (Segment != null && Segment.ExcuteMoveDirect != -1)
{
InflectLand.movedirect = (MoveDirectEnum)Segment.ExcuteMoveDirect;
}
}
#endregion
#region 计算角度
else
{
//if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
if (InflectLand.LandX == NextLand.LandX)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
//else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
else if (InflectLand.LandY == NextLand.LandY)
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
#endregion
}
}//end foreach
}
else
{
#region 计算转向
if (IsCountAngel == 0)
{
InflectLand = Routes[0];
NextLand = Routes[1];
AllSegment Segment = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Segment != null && Segment.ExcuteTurnDirect != -1)
{
InflectLand.sway = (SwayEnum)Segment.ExcuteTurnDirect;
}
if (Segment != null && Segment.ExcuteMoveDirect != -1)
{
InflectLand.movedirect = (MoveDirectEnum)Segment.ExcuteMoveDirect;
}
}
#endregion
#region 计算角度
else
{
InflectLand = Routes[0];
NextLand = Routes[1];
//if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
if (InflectLand.LandX == NextLand.LandX)
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
//else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
else if (InflectLand.LandY == NextLand.LandY)
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
#endregion
}
}
catch (Exception ex)
{ throw ex; }
}
//计算转向和前进和后退
public void AcountDirect(ref List <LandmarkInfo> Routes)
{
try
{
IList <AllSegment> AllSegs = AGVClientDAccess.LoadAllSegment();
DataTable dtCoor = AGVClientDAccess.LoadAGVCoordinate();
LandmarkInfo UpLand = null;
LandmarkInfo InflectLand = null;
LandmarkInfo NextLand = null;
if (Routes.Count <= 1)
{
return;
}
else if (Routes.Count >= 3)
{
for (int i = 2; i < Routes.Count; i++)
{
UpLand = Routes[i - 2];
InflectLand = Routes[i - 1];
NextLand = Routes[i];
//判断退进
// var A = Math.Atan2(Math.Round(NextLand.LandY, 3) - Math.Round(InflectLand.LandY, 3), Math.Round(NextLand.LandX, 3) - Math.Round(InflectLand.LandX, 3));
// var B = Math.Atan2(Math.Round(UpLand.LandY, 3) - Math.Round(InflectLand.LandY, 3), Math.Round(UpLand.LandX, 3) - Math.Round(InflectLand.LandX, 3));
// var C = B - A;
// double angel = (180 * (float)C) / Math.PI;
// if (Math.Abs(angel) < 90)
// { InflectLand.IsBack = true; }
double SP = (Math.Round(UpLand.LandX, 3) - Math.Round(NextLand.LandX, 3)) * (Math.Round(InflectLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) - (Math.Round(UpLand.LandY, 3) - Math.Round(NextLand.LandY, 3)) * (Math.Round(InflectLand.LandX, 3) - Math.Round(NextLand.LandX, 3));
if (SP > 0.05)
{
InflectLand.sway = SwayEnum.Left;
}
else if (SP < -0.05)
{
InflectLand.sway = SwayEnum.Right;
}
else
{
InflectLand.sway = SwayEnum.None;
}
//判断第一个地标角度
if (i == 2)//计算开始行走的角度
{
if (Math.Round(UpLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandY > UpLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 270;
}
}
}
}
else if (Math.Round(UpLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == UpLand.LandmarkCode && p.EndLandMarkCode == InflectLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
UpLand.Angle = Seg.ExcuteAngle;
}
else
{
if (InflectLand.LandX > UpLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
UpLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
UpLand.Angle = 180;
}
}
}
}
}
//判断最后一个地标的角度
if (i == Routes.Count - 1)//计算开始行走的角度
{
NextLand.Angle = InflectLand.Angle;
}
if (SP > 0.05 || SP < -0.05)
{
if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
}//end foreach
}
else
{
InflectLand = Routes[0];
NextLand = Routes[1];
if (Math.Round(InflectLand.LandX, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandX, 1, MidpointRounding.AwayFromZero))
{
//找对应的行走线段
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandY > InflectLand.LandY)//地图上北
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 0).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 90;
}
}
else//地图下南
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 2).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 270;
}
}
}
}
else if (Math.Round(InflectLand.LandY, 1, MidpointRounding.AwayFromZero) == Math.Round(NextLand.LandY, 1, MidpointRounding.AwayFromZero))
{
AllSegment Seg = AllSegs.FirstOrDefault(p => p.BeginLandMakCode == InflectLand.LandmarkCode && p.EndLandMarkCode == NextLand.LandmarkCode);
if (Seg != null && Seg.ExcuteAngle != -1)
{
InflectLand.Angle = Seg.ExcuteAngle;
}
else
{
if (NextLand.LandX > InflectLand.LandX)//地图右东
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 1).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 0;
}
}
else//地图左西
{
if (dtCoor.Rows.Count > 0)
{
DataRow dr = dtCoor.Select("Direction=" + 3).FirstOrDefault();
InflectLand.Angle = Convert.ToInt16(dr["Angle"]);
}
else
{
InflectLand.Angle = 180;
}
}
}
}
}
}
catch (Exception ex)
{ throw ex; }
}
