public WayPointsFollowingRRTPlanner(
double goalBias,
int maximumNumberOfIterations,
IVehicleModel vehicleModel,
IMotionModel motionModel,
ITrack track,
ICollisionDetector collisionDetector,
Random random,
TimeSpan timeStep,
IActionSet actions,
IReadOnlyList <IGoal> wayPoints)
{
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 = vehicleModel;
this.motionModel = motionModel;
this.track = track;
this.collisionDetector = collisionDetector;
this.random = random;
this.timeStep = timeStep;
this.wayPoints = wayPoints;
this.actions = actions;
distances = new DistanceMeasurement(track.Width, track.Height);
wayPointsReached = 0;
ExploredStates = exploredStates;
}
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 MeasurementType(DistanceMeasurement measurement, char code, string singular, string plural, string abbreviation, string symbol = null)
{
Measurement = measurement;
Code = code;
SingularName = singular;
PluralName = plural;
Abbreviation = abbreviation;
Symbol = symbol ?? abbreviation;
}
// Start is called before the first frame update
void Start()
{
gotoPatrol = 2;
playerDistance = gameObject.AddComponent <DistanceMeasurement>();
playerDistance.other = player;
patrol1Distance = gameObject.AddComponent <DistanceMeasurement>();
patrol1Distance.other = patrol1;
patrol2Distance = gameObject.AddComponent <DistanceMeasurement>();
patrol2Distance.other = patrol2;
}
public static void Render(RenderManager.CameraInfo cameraInfo, DistanceMeasurement distance)
{
var renderManager = RenderManager.instance;
if (!distance.IsStraight || distance.HideOverlay)
{
return;
}
var minHeight = Mathf.Min(distance.StartPosition.y, distance.EndPosition.y);
var maxHeight = Mathf.Max(distance.StartPosition.y, distance.EndPosition.y);
renderManager.OverlayEffect.DrawSegment(cameraInfo,
distance.Flags == MeasurementFlags.Primary ? Settings.PrimaryColor : Settings.SecondaryColor,
new Segment3(distance.StartPosition, distance.EndPosition), Size, 3f,
minHeight - 20f,
maxHeight + 20f, true, true);
}
public static Vector3 GetLabelWorldPosition(DistanceMeasurement distance)
{
return(distance.Position);
}
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);
}
/// <summary>
/// Finds the n known measurements with the smallest euclidian distance to x, y, t in nearly O(n) time.
/// </summary>
private List<Measurement> calcNearest(Measurement measure)
{
DistanceMeasurement[] results = new DistanceMeasurement[this.nearest];
foreach (Measurement curr in this.known)
{
double distance = this.calcD(curr, measure);
// Test if this is a new smallest value.
bool shift = false;
DistanceMeasurement shifted = null;
for (int i = 0; i < results.Length; i++)
{
// A smaller value was found continue shifting the array.
if (shift)
{
DistanceMeasurement temp = results[i];
results[i] = shifted;
shifted = temp;
}
// Test if this is one of the first n elements.
else if (results[i] == null)
{
// Build a DistanceMeasurement for this value.
DistanceMeasurement dm = new DistanceMeasurement();
dm.Measurement = curr;
dm.Distance = distance;
results[i] = dm;
}
// Test if value smaller than current element.
else if (distance < results[i].Distance)
{
// Build a DistanceMeasurement for this value.
DistanceMeasurement dm = new DistanceMeasurement();
dm.Measurement = curr;
dm.Distance = distance;
// Swap in this value.
shifted = results[i];
results[i] = dm;
// Enter shift mode.
shift = true;
}
}
}
// Convert from DistanceMeasurement to Measurement.
List<Measurement> nearest = new List<Measurement>();
foreach (DistanceMeasurement curr in results)
{
nearest.Add(curr.Measurement);
}
return nearest;
}
