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); }