float SquareDistance(MyPoint point1, MyPoint point2)
{
return((point1.X - point2.X) * (point1.X - point2.X) + (point1.Y - point2.Y) * (point1.Y - point2.Y));
}
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]);
//}
}
}