public WheelSet(World world, Car car, Vector2 position, float orientation) { Matrix rotation = Matrix.CreateRotationZ(orientation); LeftFront = new RevoluteWheel(world, car, position + Vector2.Transform(Car.WHEEL_POS_LF, rotation), orientation); RightFront = new RevoluteWheel(world, car, position + Vector2.Transform(Car.WHEEL_POS_RF, rotation), orientation); LeftRear = new FixedWheel(world, car, position + Vector2.Transform(Car.WHEEL_POS_LR, rotation), Car.WHEEL_POS_LR, orientation); RightRear = new FixedWheel(world, car, position + Vector2.Transform(Car.WHEEL_POS_RR, rotation), Car.WHEEL_POS_RR, orientation); Controller = new TensorDampingController(); Controller.SetAxisAligned(0, 100); Controller.AddBody(LeftFront.Body); Controller.AddBody(RightFront.Body); Controller.AddBody(LeftRear.Body); Controller.AddBody(RightRear.Body); world.AddController(Controller); }
public RevoluteWheel(World world, Car car, Vector2 position, float orientation) : base(world, position, orientation) { Joint = JointFactory.CreateRevoluteJoint(world, car.Body, Body, Vector2.Zero); Joint.MotorEnabled = true; Joint.MaxMotorTorque = 100; Joint.LimitEnabled = true; Joint.LowerLimit = -Car.MAX_WHEEL_DEFLECTION; Joint.UpperLimit = Car.MAX_WHEEL_DEFLECTION; }
protected override void Initialize() { graphics.PreferMultiSampling = true; graphics.PreferredBackBufferWidth = WINDOWED_WIDTH; graphics.PreferredBackBufferHeight = WINDOWED_HEIGHT; graphics.IsFullScreen = false; graphics.ApplyChanges(); spriteBatch = new SpriteBatch(GraphicsDevice); console = new GameConsole(this, spriteBatch, new GameConsoleOptions() { OpenOnWrite = false, Prompt = ">", Height = 500, ToggleKey = Keys.Escape }); console.AddCommand("load", args => { missionFile = "../../../../../missions/" + args[0] + ".json"; ClearWorld(); InitSim(false); return "Mission loaded."; }, "Load a mission file"); console.AddCommand("experiment", args => { if (args[0] == "random") new SmootherExperiment(this, expFile, 100); else if (args[0] == "parking") new ParkingExperiment(this, world, expFile); else return "Experiment not found."; return "Experiment started."; }, "Run an experiment"); console.AddCommand("antialias", args => { graphics.PreferMultiSampling = !graphics.PreferMultiSampling; graphics.ApplyChanges(); return "Antialias " + (graphics.PreferMultiSampling ? "on" : "off"); }, "Toggles antialias"); console.AddCommand("debug", args => { showDebugInfo = !showDebugInfo; return "Debug " + (showDebugInfo ? "on" : "off"); }, "Toggles the debug info"); console.AddCommand("dashboard", args => { showDashboard = !showDashboard; return "Dashboard " + (showDashboard ? "on" : "off"); }, "Toggles the dashboard"); console.AddCommand("param", args => { if (args.Length < 1) return ""; switch (args[0].ToLower()) { case "smoother": return Smoother.HandleParamCommand(args); default: return ""; } }, "Prints or updates parameters"); console.AddCommand("smooth", args => { if (pathSmoothDone) { bg = new Thread(() => { DateTime now = DateTime.Now; smoothedPath = Smoother.Smooth(astar.Path, grid); int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0; console.WriteLine("Smoothing Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)"); controller = new StanleyFSMController(smoothedPath, goalPose); }); bg.IsBackground = true; bg.Priority = ThreadPriority.Lowest; bg.Start(); } return ""; }, "Smooths the path using the current parameters"); console.AddCommand("smoothing", args => { smoothing = !smoothing; return "Smoothing " + (smoothing ? "on" : "off"); }, "Toggles smoothing" ); dashboard = new Dashboard(this); world = new World(Vector2.Zero); car = new Car(world, new Pose()); camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f); camera.Position = new Vector3(75f, 75f, 180f); base.Initialize(); }
public FixedWheel(World world, Car car, Vector2 position, Vector2 anchorPos, float orientation) : base(world, position, orientation) { Joint = JointFactory.CreateWeldJoint(world, car.Body, Body, anchorPos, Vector2.Zero); }
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(); }