protected override void Update(GameTime gameTime) { currKeyboard = Keyboard.GetState(); if (currKeyboard.IsKeyDown(Keys.Escape) && currKeyboard.IsKeyDown(Keys.LeftShift)) this.Exit(); if (!console.Opened) { camera.HandleInput(); // View options if (keyPressed(Keys.F11)) toggleFullscreen(); if (keyPressed(Keys.F12)) backgroundLight = !backgroundLight; if (keyPressed(Keys.C)) drawCar = !drawCar; if (keyPressed(Keys.R)) rotateCar = !rotateCar; if (keyPressed(Keys.G)) { drawGrid = !drawGrid; drawVoro = false; drawHeur = false; } if (keyPressed(Keys.V)) { drawVoro = !drawVoro; drawGrid = false; drawHeur = false; } if (keyPressed(Keys.H)) { drawHeur = !drawHeur; drawVoro = false; drawGrid = false; } if (keyPressed(Keys.B)) if (currKeyboard.IsKeyDown(Keys.LeftShift) || currKeyboard.IsKeyDown(Keys.RightShift)) watchSearch = !watchSearch; else drawSearch = !drawSearch; if (keyPressed(Keys.P)) drawPath = !drawPath; if (keyPressed(Keys.L)) { drawSmoothedPath = !drawSmoothedPath; drawController = false; } if (keyPressed(Keys.O)) { drawController = !drawController; drawSmoothedPath = false; } if (keyPressed(Keys.F)) drawFrontPath = !drawFrontPath; if (keyPressed(Keys.Home)) drawStart = !drawStart; if (keyPressed(Keys.End)) drawGoal = !drawGoal; if (keyPressed(Keys.Scroll)) drawCurrent = !drawCurrent; if (keyPressed(Keys.N)) drawDebugData = !drawDebugData; if (keyPressed(Keys.OemComma)) HybridAStar.Delay = HybridAStar.Delay == 0 ? 1 : 0; if (keyPressed(Keys.OemPeriod)) Smoother.Delay = Smoother.Delay == 0 ? 10 : 0; // Zoom if (currKeyboard.IsKeyDown(Keys.OemPlus)) scale *= 1.02f; if (currKeyboard.IsKeyDown(Keys.OemMinus)) scale *= 0.98f; // Translate if (currKeyboard.IsKeyDown(Keys.Up)) yOffset += 5f / scale; if (currKeyboard.IsKeyDown(Keys.Down)) yOffset -= 5f / scale; if (currKeyboard.IsKeyDown(Keys.Right)) xOffset += 5f / scale; if (currKeyboard.IsKeyDown(Keys.Left)) xOffset -= 5f / scale; if (currKeyboard.IsKeyDown(Keys.W)) gas = 1; else if (currKeyboard.IsKeyDown(Keys.S)) gas = -1; else gas = 0; if (currKeyboard.IsKeyDown(Keys.D)) steer = -1; else if (currKeyboard.IsKeyDown(Keys.A)) steer = 1; else steer = 0; if (currKeyboard.IsKeyDown(Keys.Space)) brake = 1; else brake = 0; if (gas != 0 || brake != 0 || steer != 0) autoDrive = false; if (currKeyboard.IsKeyDown(Keys.Enter) && pathSmoothDone) run = true; // Camera views if (keyPressed(Keys.D1)) cameraView = 1; if (keyPressed(Keys.D2)) cameraView = 2; if (keyPressed(Keys.D3)) cameraView = 3; if (keyPressed(Keys.D4)) cameraView = 4; if (keyPressed(Keys.D5)) cameraView = 5; if (keyPressed(Keys.D0)) { ClearWorld(); InitSim(false); } if (keyPressed(Keys.Back)) paused = !paused; } prevKeyboard = currKeyboard; if (!paused) { if (autoDrive) { if (run) { currentControls = controller.Update(car.Pose, car.WheelAngle, car.SpeedMetersPerSecond, gameTime); car.Update(currentControls); } } else { currentControls = new CarControls(gas, brake, steer); car.Update(new CarControls(gas, brake, steer)); } if (run && controller.State == StanleyFSMController.ControllerState.MissionComplete && MissionCompleted != null) MissionCompleted(this, controller.Info); world.Step((float)gameTime.ElapsedGameTime.TotalSeconds); dashboard.Update(car.WheelAngle, currentControls.Gas, currentControls.Brake, car.SpeedMPH); if (isCollided && Collided != null) Collided(this, null); } base.Update(gameTime); }
public void Update(CarControls controls) { Update(controls.Gas, controls.Brake, controls.Steer); }
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(); }