public void OnMouseUp(ICanvas canvas, UnitPoint point, ISnapPoint snappoint) { try { SysParameter sys = AGVClientDAccess.GetParameterByCode("DefaultLenth"); if (sys != null) { double DefaultLenth = 0; try { DefaultLenth = Convert.ToDouble(sys.ParameterValue); } catch (Exception ex) { return; } this.Lenth = DefaultLenth; } } catch (Exception ex) { throw ex; } }
//计算转向和前进和后退 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 List <LandmarkInfo> GetRoute(LandmarkInfo StartLand, LandmarkInfo EndLand) { try { List <List <LandmarkInfo> > hs_route = new List <List <LandmarkInfo> >(); AcountRoute(StartLand, EndLand); hs_route.Add(CreateDeepCopy <List <LandmarkInfo> >(RouteList)); List <LandmarkInfo> OrialRoute = CreateDeepCopy <List <LandmarkInfo> >(RouteList); List <LandmarkInfo> OrialClose = CreateDeepCopy <List <LandmarkInfo> >(CloseList); Stack NewPivotAnotherLand = CreateDeepCopy <Stack>(PivotAnotherLand); while (NewPivotAnotherLand.Count > 0) { PivotAnotherLand.Clear(); RouteList = CreateDeepCopy <List <LandmarkInfo> >(OrialRoute); CloseList = CreateDeepCopy <List <LandmarkInfo> >(OrialClose); Hashtable hs = NewPivotAnotherLand.Pop() as Hashtable; ArrayList keyList = new ArrayList(hs.Keys); LandmarkInfo PivotLand = hs[keyList[0].ToString()] as LandmarkInfo; int index = RouteList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1; int indexClose = CloseList.FindIndex(p => p.LandmarkCode == keyList[0].ToString()) + 1; RouteList.RemoveRange(index, RouteList.Count - index); CloseList.RemoveRange(indexClose, CloseList.Count - indexClose); AcountRoute(PivotLand, EndLand); hs_route.Add(CreateDeepCopy <List <LandmarkInfo> >(RouteList)); } List <LandmarkInfo> Routes = null; SysParameter sys = AGVClientDAccess.GetParameterByCode("RouteCountMode"); if (sys != null) { if (sys.ParameterValue == "最少地标数") { Routes = (from a in hs_route where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0 orderby a.Count ascending select a).FirstOrDefault(); } else if (sys.ParameterValue == "最少拐弯") { Routes = (from a in hs_route where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0 orderby a.Count ascending select a).FirstOrDefault(); Routes = (from a in hs_route where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0 orderby AcountInflect(a) ascending select a).FirstOrDefault(); } } else { Routes = (from a in hs_route where a.Where(p => p.LandmarkCode == EndLand.LandmarkCode).Count() > 0 orderby a.Count ascending select a).FirstOrDefault(); } if (Routes == null) { Routes = new List <LandmarkInfo>(); } //this.RouteList = Routes; AcountDirect(ref Routes); return(Routes); } catch (Exception ex) { throw ex; } }