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();
}