private TreeNode?nearest(List <TreeNode> nodes, VehicleState state)
{
// todo: use kd-tree
TreeNode?best = null;
var shortestDistance = double.MaxValue;
var extendableNodes = nodes.Where(node => node.CanBeExpanded);
foreach (var node in extendableNodes)
{
var currentNodeDistance = distances.DistanceBetween(node.State, state);
if (currentNodeDistance < shortestDistance)
{
best = node;
shortestDistance = currentNodeDistance;
}
}
return(best);
}
private (VehicleState?, IAction?, bool) steer(TreeNode from, VehicleState to, DistanceMeasurement distances)
{
VehicleState?state = null;
IAction? bestAction = null;
bool reachedWayPoint = 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 (wayPoints[wayPointsReached].ReachedGoal(intermediateState.Position))
{
passedGoal = true;
}
}
if (collided)
{
from.DisableAction(action);
remainingAvailableActions--;
continue;
}
var currentDistance = distances.DistanceBetween(to, resultState);
if ((!reachedWayPoint || passedGoal) && currentDistance < shortestDistance)
{
state = resultState;
bestAction = action;
reachedWayPoint = passedGoal;
}
}
if (bestAction != null)
{
from.DisableAction(bestAction);
remainingAvailableActions--;
}
if (bestAction == null || remainingAvailableActions == 0)
{
from.DisableFutureExpansions();
}
return(state, bestAction, reachedWayPoint);
}
