public StateType Classify(VehicleState state)
{
if (collisionDetector.IsCollision(state))
{
return(StateType.Collision);
}
if (goal.ReachedGoal(state.Position))
{
return(StateType.Goal);
}
return(StateType.Free);
}
private (VehicleState?, IAction?) steer(TreeNode from, VehicleState to, DistanceMeasurement distances, out bool reachedGoal)
{
VehicleState?state = null;
IAction? bestAction = null;
reachedGoal = false;
var shortestDistance = double.MaxValue;
// todo what if there are no available actions?
var availableActions = from.SelectAvailableActionsFrom(actions.AllPossibleActions).ToArray();
var remainingAvailableActions = availableActions.Length;
foreach (var action in availableActions)
{
var predictedStates = motionModel.CalculateNextState(from.State, action, timeStep).ToList();
var elapsedTime = predictedStates.Last().relativeTime;
var resultState = predictedStates.Last().state;
var passedGoal = false;
var collided = false;
foreach (var(simulationTime, intermediateState) in predictedStates)
{
if (collisionDetector.IsCollision(intermediateState))
{
elapsedTime = simulationTime;
resultState = intermediateState;
passedGoal = false;
collided = true;
break;
}
if (goal.ReachedGoal(intermediateState.Position))
{
passedGoal = true;
}
}
if (collided)
{
from.DisableAction(action);
remainingAvailableActions--;
continue;
}
var currentDistance = distances.DistanceBetween(to, resultState);
if ((!reachedGoal || passedGoal) && currentDistance < shortestDistance)
{
state = resultState;
bestAction = action;
reachedGoal = passedGoal;
}
}
if (bestAction != null)
{
from.DisableAction(bestAction);
remainingAvailableActions--;
}
if (bestAction == null || remainingAvailableActions == 0)
{
from.DisableFutureExpansions();
}
return(state, bestAction);
}
