public IPlan ToDetailedPlan(IWorldDefinition world)
{
var detailedTrajectory = new List <IActionTrajectory>();
for (int i = 0; i < Trajectory.Count; i++)
{
var trajectory = Trajectory[i];
var state = trajectory.State;
var action = trajectory.Action;
if (action != null)
{
var nextTrajectorySegmentTime = i < Trajectory.Count - 1
? Trajectory[i + 1].Time
: TimeToGoal;
var timeStep = nextTrajectorySegmentTime - trajectory.Time;
var predictions = world.MotionModel.CalculateNextState(state, action, timeStep);
var previousState = trajectory.State;
foreach (var(time, predictedState) in predictions)
{
var currentTime = trajectory.Time + time;
detailedTrajectory.Add(new ActionTrajectory(currentTime, predictedState, action, trajectory.TargetWayPoint));
previousState = predictedState;
}
}
}
return(new Plan(TimeToGoal, detailedTrajectory));
}
public RRTPlanner(
double goalBias,
int maximumNumberOfIterations,
IWorldDefinition world,
Random random,
TimeSpan timeStep,
IGoal goal)
{
if (goalBias > 0.5)
{
throw new ArgumentOutOfRangeException($"Goal bias must be at most 0.5 (given {goalBias}).");
}
this.goalBias = goalBias;
this.maximumNumberOfIterations = maximumNumberOfIterations;
this.vehicleModel = world.VehicleModel;
this.motionModel = world.MotionModel;
this.track = world.Track;
this.collisionDetector = world.CollisionDetector;
this.random = random;
this.timeStep = timeStep;
this.goal = goal;
this.actions = world.Actions;
distances = new DistanceMeasurement(track.Width, track.Height);
ExploredStates = exploredStates;
}
public IWorld Build(IWorldDefinition definition)
{
var heightGenerator = new HeightMapGenerator();
var heightMesh = heightGenerator.Generate(definition.Size.Width, definition.Size.Height);
var temperatureGenerator = new TemperatureMapGenerator();
var temperatureMesh = temperatureGenerator.Generate(definition.Size.Width, definition.Size.Height);
var moistureGenerator = new MoistureMapGenerator();
var moistureMesh = moistureGenerator.Generate(definition.Size.Width, definition.Size.Height);
var map = new WorldMap(definition.Size.Width, definition.Size.Height);
for (var x = 0; x < definition.Size.Width; x++)
{
for (var y = 0; y < definition.Size.Height; y++)
{
map.SetValue(x, y,
heightMesh.GetValue(x, y).Value,
moistureMesh.GetValue(x, y).Value,
temperatureMesh.GetValue(x, y).Value);
}
}
return(new World(map));
}
public Simulation(IAgent agent, IWorldDefinition world)
{
this.agent = agent;
this.world = world;
log = new Log();
Events = log.Events;
}
public HybridAStarPlanner(
TimeSpan timeStep,
IWorldDefinition world,
IReadOnlyList <IGoal> wayPoints,
bool greedy = false)
{
this.timeStep = timeStep;
this.greedy = greedy;
this.wayPoints = wayPoints;
vehicleModel = world.VehicleModel;
motionModel = world.MotionModel;
track = world.Track;
actions = world.Actions;
collisionDetector = world.CollisionDetector;
discretizer = new StateDiscretizer(
positionXCellSize: vehicleModel.Width / 2,
positionYCellSize: vehicleModel.Width / 2,
headingAngleCellSize: 2 * Math.PI / 36);
ExploredStates = exploredStates;
}
//public void SetTile(int x, int y, TileDefinition brush)
//{
// _world[x, y] = brush;
//}
//public TileDefinition GetTile(int x, int y)
//{
// return _world[x, y];
//}
public void SetItem(int x, int y, IWorldDefinition definition)
{
_world[x, y] = definition;
}
public IPlan ToDetailedPlan(IWorldDefinition world)
{
throw new NotImplementedException();
}
