float SquareDistance(MyPoint point1, MyPoint point2)
 {
     return((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
 }
Exemple #2
0
        public void SearchRoute(Vehicle v)
        {
            v.RouteIndex = 0;
            v.cost       = 0;
            v.TPtr       = 0;// tFram = 0;
            v.StopTime   = ConstDefine.STOP_TIME;

            if (!checkXY(v))
            {
                v.CurState = State.cannotToDestination;
                MessageBox.Show("起点或终点超出地图界限");
                return;
            }
            ////AstarSearch astarSearch = new AstarSearch(Elc);
            List <MyPoint> scannerNode = new List <MyPoint>();

            if (!Elc.IsSpecialArea(v.BeginX, v.BeginY))
            {
                scannerNode = Elc.GetScanner();
            }
            List <MyPoint> routeList = astarSearch.Search(Elc, scannerNode, v.LockNode, v.Id, Elc.WidthNum, Elc.HeightNum, v.BeginX, v.BeginY, v.EndX, v.EndY, v.Dir);

            //this.Speed = 0;
            Elc.mapnode[v.BeginX, v.BeginY].NodeCanUsed = v.Id;
            // Elc.mapnode[startX, startY].NodeCanUsed = false;//搜索完,小车自己所在的地方被小车占用
            if (routeList.Count < 1)
            {
                // MessageBox.Show("没有搜索到路线:"+v_num);
                v.CurState = State.cannotToDestination;
                //v.LockNode.cl;
            }
            else
            {
                v.Route = routeList;
                if (Elc.IsQueueEntra(v.EndX, v.EndY))
                {
                    MyPoint        nextEnd  = ElecMap.Instance.CalculateScannerPoint(new MyPoint(v.EndX, v.EndY));
                    List <MyPoint> addRoute = astarSearch.Search(Elc, new List <MyPoint>(), v.LockNode, v.Id, Elc.WidthNum, Elc.HeightNum, v.EndX, v.EndY, nextEnd.X, nextEnd.Y, v.Dir);
                    if (addRoute != null && addRoute.Count > 1)
                    {
                        for (int i = 1; i < addRoute.Count; i++)
                        {
                            v.Route.Add(addRoute[i]);
                        }
                        //v.EndX = nextEnd.X;
                        //v.EndY = nextEnd.Y;
                        v.EndLoc = "ScanArea";
                    }
                }
                // this.vehical_state = State.carried;

                //Route = new ConcurrentDictionary<int, MyLocation>();
                //List<int> specialPoint = new List<int>();
                //checkSpecialPoint(routeList, specialPoint);

                //for (int i = 0; i < routeList.Count; i++)
                //{
                //    this.Route.TryAdd(i, routeList[i]);
                //}

                //for (int i = 0; i < specialPoint.Count - 1; i++)
                //{
                //    FormRoute(routeList, specialPoint[i], specialPoint[i + 1]);
                //}
            }
        }