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();
}
protected override void Draw(GameTime gameTime)
{
GraphicsDevice.Clear(backgroundLight ? LIGHT : DARK);
Matrix p = camera.Projection;
Matrix v = camera.View;
if (cameraView == 2)
{
float orientation = controller != null && controller.InReverse ? car.Pose.Orientation + MathHelper.Pi : car.Pose.Orientation;
Vector2 position = Car.GetCenterPosition(car.Pose);
Vector2 headingVector = new Vector2((float)Math.Cos(orientation), (float)Math.Sin(orientation));
float offset = 14f;
float height = 10f;
v = Matrix.CreateLookAt(new Vector3(position + -offset * headingVector, height), new Vector3(position + offset * headingVector, 0f), Vector3.UnitZ);
}
else if (cameraView == 3)
{
float or = car.Pose.Orientation + MathHelper.ToRadians(120f);
Vector2 headingVector = new Vector2((float)Math.Cos(or), (float)Math.Sin(or));
v = Matrix.CreateLookAt(new Vector3(car.Pose.Position + -10f * headingVector, 5f), new Vector3(Car.GetCenterPosition(car.Pose), 0f), Vector3.UnitZ);
}
else if (cameraView == 4)
{
float or = goalPose.Orientation;
Vector2 headingVector = new Vector2((float)Math.Cos(or), (float)Math.Sin(or));
v = Matrix.CreateLookAt(new Vector3(goalPose.Position + 10f * headingVector, 20f), new Vector3(Car.GetCenterPosition(car.Pose), 0f), Vector3.UnitZ);
}
else if (cameraView == 5)
{
v = Matrix.CreateLookAt(new Vector3(Car.GetCenterPosition(car.Pose), 20f), new Vector3(Car.GetCenterPosition(car.Pose), 0f), Vector3.UnitY);
if (rotateCar)
{
v *= Matrix.CreateRotationZ(-car.Body.Rotation + MathHelper.PiOver2);
}
}
if (gridDone && (drawGrid || drawVoro || drawHeur))
{
basicEffect.World = Matrix.Identity;
basicEffect.View = v;
basicEffect.Projection = p;
spriteBatch.Begin(0, null, SamplerState.PointClamp, DepthStencilState.DepthRead, RasterizerState.CullNone, basicEffect);
if (drawHeur)
{
HybridAStar.Draw(spriteBatch);
}
else
{
grid.Draw(spriteBatch, drawVoro);
}
spriteBatch.End();
}
debug.BeginCustomDraw(ref p, ref v);
debug.DrawSolidCircle(Car.GetFrontAxlePosition(car.Pose), 0.2f, Color.Green);
if ((pathSearching && watchSearch || pathSearchingDone && drawSearch) && HybridAStar.Expanded != null && HybridAStar.Expanded.Count > 0)
{
LinkedList <HybridAStar.Node> expanded = new LinkedList <HybridAStar.Node>();
lock (HybridAStar.Expanded)
{
expanded.AddAll(HybridAStar.Expanded);
}
float pathLength = expanded.Last.f;
foreach (HybridAStar.Node n in expanded)
{
if (pathDone && n.rsIndex >= 0)
{
for (int i = n.rsIndex; i < poses.Count - 1; i++)
{
debug.DrawSegment(poses[i].Position, poses[i + 1].Position, Color.Purple, 0.02f);
}
}
else if (n.from != null)
{
Color color;
if (n.cell.rev == 0)
{
color = Color.Lerp(Color.Orange, Color.Green, n.g / pathLength);
}
else
{
color = Color.Lerp(Color.Blue, Color.Cyan, n.g / pathLength);
}
debug.DrawSegment(n.from.pose.Position, n.pose.Position, color, 0.02f);
}
}
}
if (pathDone)
{
if (drawPath)
{
for (int i = 0; i < poses.Count - 1; i++)
{
Color c = poses[i].Gear == Gear.Forward ? Color.Blue : Color.Red;
debug.DrawSegment(poses[i].Position, poses[i + 1].Position, c, 0.04f);
debug.DrawPoint(poses[i].Position, 0.1f, c * 0.5f);
}
}
}
if (pathSearchingDone && !pathSmoothDone && (drawSmoothedPath || drawController))
{
Smoother.Draw(debug);
}
if (pathSmoothDone)
{
if (drawFrontPath && controller.FrontPath != null)
{
int num = controller.FrontPath.Count;
for (int i = 0; i < num - 1; i++)
{
if (controller.FrontPath[i].Gear == Gear.Forward)
{
debug.DrawSegment(controller.FrontPath[i].Position, controller.FrontPath[i + 1].Position, Color.DarkOrange);
}
else
{
debug.DrawSegment(controller.ReverseFrontPath[i].Position, controller.ReverseFrontPath[i + 1].Position, Color.Cyan);
}
}
}
if (drawSmoothedPath)
{
for (int i = 0; i < smoothedPath.Count - 1; i++)
{
debug.DrawSegment(smoothedPath[i].Position, smoothedPath[i + 1].Position, smoothedPath[i].Gear == Gear.Forward ? Color.DarkGreen : Color.Red, 0.04f);
//if (Smoother.UnsafeIndices != null && Smoother.UnsafeIndices.Contains(i))
//debug.DrawCircle(smoothedPath[i].Position, 0.2f, Color.Orange);
}
}
if (drawController)
{
controller.Draw(debug);
}
}
if (drawStart)
{
startPose.DrawPose(debug, new Color(0f, 1f, 0f, 0.9f), 1.1f);
}
if (drawGoal)
{
goalPose.DrawPose(debug, new Color(1f, 0f, 0f, 0.9f), 1.1f);
}
if (drawCurrent)
{
if (pathSmoothDone && controller.State != StanleyFSMController.ControllerState.MissionComplete)
{
debug.DrawCircle(controller.ClosestPoint, 0.1f, controller.InReverse ? Color.Aqua : Color.Orange);
if (controller.InReverse)
{
debug.DrawCircle(controller.FakeFrontAxle, 0.2f, Color.Aqua);
}
}
car.Pose.DrawPose(debug, 0.2f, Color.Red);
}
debug.EndCustomDraw();
if (drawDebugData)
{
debug.RenderDebugData(ref p, ref v);
}
if (drawCar)
{
car.Draw(v, p);
}
if (showDebugInfo)
{
string info = String.Format("Speed: {0:0.0}", Math.Round(car.SpeedMPH, 1));
if (pathSmoothDone)
{
info += String.Format("\nGas: {0:0.00}", Math.Round(currentControls.Gas * 100f, 2));
info += String.Format("\nBrake: {0:0.00}", Math.Round(currentControls.Brake * 100f, 2));
info += String.Format("\nCTE: {0:0.0000}", Math.Round(controller.CrossTrackError, 4));
info += "\n" + controller.State.ToString();
info += "\n" + controller.DebugInfo;
}
spriteBatch.Begin();
spriteBatch.DrawString(font, info, new Vector2(8, 4), !backgroundLight ? LIGHT : DARK);
spriteBatch.End();
}
if (showDashboard)
{
dashboard.Draw(gameTime);
}
base.Draw(gameTime);
}
//Generate a path and send it to the car
//We have to do it over some time to avoid a sudden stop in the simulation
IEnumerator GeneratePath(Car goalCar)
{
//Get the start positions
//The self-driving car
Car startCar = new Car(SimController.current.GetSelfDrivingCarTrans(), SimController.current.GetActiveCarData());
//The trailer (if any)
Car startTrailer = null;
Transform trailerTrans = SimController.current.TryGetTrailerTrans();
if (trailerTrans != null)
{
startTrailer = new Car(trailerTrans, SimController.current.TryGetTrailerData());
}
//First we have to check if the self-driving car is inside of the grid
if (!map.IsPosWithinGrid(startCar.rearWheelPos))
{
Debug.Log("The car is outside of the grid");
yield break;
}
//Which cell do we want to reach? We have already checked that this cell is valid
IntVector2 targetCell = map.ConvertWorldToCell(goalCar.rearWheelPos);
//To measure time, is measured in tick counts
int startTime = 0;
//To display how long time each part took
string timeText = "";
//
// Calculate Heuristics
//
//Calculate euclidean distance heuristic
startTime = Environment.TickCount;
HeuristicsController.EuclideanDistance(map, targetCell);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Euclidean Distance");
yield return(new WaitForSeconds(0.05f));
//Calculate dynamic programing = flow field
startTime = Environment.TickCount;
HeuristicsController.DynamicProgramming(map, targetCell);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Dynamic Programming");
yield return(new WaitForSeconds(0.05f));
//Calculate the final heuristics
HeuristicsController.GenerateFinalHeuristics(map);
//
// Generate the shortest path with Hybrid A*
//
//List with all expanded nodes for debugging, so we can display the search tree
List <Node> expandedNodes = new List <Node>();
startTime = Environment.TickCount;
//The output is finalPath and expandedNodes
List <Node> finalPath = HybridAStar.GeneratePath(startCar, goalCar, map, expandedNodes, startTrailer);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Hybrid A Star");
if (finalPath == null || finalPath.Count == 0)
{
UIController.current.SetFoundPathText("Failed to find a path!");
}
else
{
UIController.current.SetFoundPathText("Found a path!");
}
//
// Smooth the path and send it to the car
//
//If we have found a path
List <Node> smoothPath = null;
if (finalPath != null && finalPath.Count > 0)
{
//Modify the path to make it easier for the vehicle to follow it
//Step 1. Hybrid A* is using the rear wheel axle to generate the path, but it's easier for the car to follow it
//if we also know where the path should have been if we had used the front axle
Vector3 vehicleStartDir = SimController.current.GetSelfDrivingCarTrans().forward;
Vector3 vehicleEndDir = SimController.current.GetCarShowingEndPosTrans().forward;
ModifyPath.CalculateFrontAxlePositions(finalPath, startCar.carData, vehicleStartDir, vehicleEndDir, isMirrored: false);
//When reversing we should track a path which is a path that goes along the front axle
//but the front axle is mirrored along the rear axle
ModifyPath.CalculateFrontAxlePositions(finalPath, startCar.carData, vehicleStartDir, vehicleEndDir, isMirrored: true);
//Smooth the path by making it smoother and adding waypoints to make it easier for the car to follow the path
startTime = Environment.TickCount;
smoothPath = ModifyPath.SmoothPath(finalPath, map, isCircular: false, isDebugOn: true);
timeText += DisplayController.GetDisplayTimeText(startTime, Environment.TickCount, "Smooth path");
//The car will immediatelly start following the path
SimController.current.SendPathToActiveCar(smoothPath, isCircular: false);
}
//
// Display the results
//
//Display how long time the different parts took
Debug.Log(timeText);
//Reset display
displayController.ResetGUI();
//Always display the search tree even if we havent found a path to the goal
displayController.DisplaySearchTree(expandedNodes);
//Generate the flow field heuristic texture
displayController.GenerateTexture(map, DisplayController.TextureTypes.Flowfield_Target);
//Display the different paths
displayController.DisplayFinalPath(finalPath, smoothPath);
yield return(null);
}
