Ejemplo n.º 1
0
        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);
        }
Ejemplo n.º 2
0
 public void Update(CarControls controls)
 {
     Update(controls.Gas, controls.Brake, controls.Steer);
 }
Ejemplo n.º 3
0
        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();
        }