public void InitSim(bool autoStart, bool smoothingOn, Mission mission) { gridDone = false; pathDone = false; pathSmoothDone = false; pathSearching = false; pathSearchingDone = false; autoDrive = true; run = false; isCollided = false; currentControls = new CarControls(0f, 0f, 0f); if (camera == null) { camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f); camera.Position = new Vector3(75f, 75f, 180f); } startPose = mission.Start; goalPose = mission.Goal; HybridAStar.Epsilon = mission.AStarEpsilon; HybridAStar.GridResolution = mission.AStarGridResolution; HybridAStar.SafetyFactor = 1.5f; HybridAStar.Reset(); car = new Car(world, startPose); grid = new ObstacleGrid(world, mission.Environment); car.Body.OnCollision += new OnCollisionEventHandler(OnCollision); gridDone = false; pathDone = false; pathSearchingDone = false; bg = new Thread(() => { DateTime now = DateTime.Now; grid.BuildGVD(); console.WriteLine("GVD Generation Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms"); gridDone = true; now = DateTime.Now; pathSearching = true; astar = HybridAStar.FindPath(grid, startPose, goalPose); TimeSpan astarTime = DateTime.Now - now; poses = astar.Path; pathSearching = false; pathSearchingDone = true; if (astar.Path.Count > 0) { pathDone = true; } now = DateTime.Now; if (smoothingOn) { smoothedPath = Smoother.Smooth(astar.Path, grid); } else { smoothedPath = astar.Path; } TimeSpan smoothingTime = DateTime.Now - now; int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0; console.WriteLine("A*: Total Planning Time: " + Math.Round((astarTime + smoothingTime).TotalMilliseconds) + " ms"); console.WriteLine(" Heuristic Time: " + Math.Round(astar.HeuristicInitTime.TotalMilliseconds) + " ms"); console.WriteLine(" Searching Time: " + Math.Round((astarTime - astar.HeuristicInitTime).TotalMilliseconds) + " ms"); console.WriteLine(" Smoothing Time: " + Math.Round(smoothingTime.TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)"); console.WriteLine(" " + astar.Discovered.Count + " nodes discovered"); console.WriteLine(" " + astar.Expanded.Count + " nodes expanded"); controller = new StanleyFSMController(smoothedPath, goalPose); pathSmoothDone = true; if (autoStart) { run = true; } }); bg.IsBackground = true; bg.Priority = ThreadPriority.Lowest; bg.Start(); }