public VehicleState FindProjection(VehicleState state, int waypoint)
{
var nearest = Segments.First().State;
var smallestDistance = Distance.Between(state.Position, nearest.Position);
foreach (var segment in Segments)
{
if (segment.TargetWayPoint < waypoint)
{
continue;
}
if (segment.TargetWayPoint > waypoint + 1)
{
break;
}
var distance = Distance.Between(state.Position, segment.State.Position);
if (distance < smallestDistance)
{
nearest = segment.State;
smallestDistance = distance;
}
}
return(nearest);
}
public void Distance_between_DublinSpire_and_GuinnessStorehouse_is_between_194and195()
{
var distance = Distance.Between(DublinSpire, GuinnessStorehouse);
//1.94km
Assert.True(distance > Distance.From(1.94, UnitType.Km) && distance < Distance.From(1.95, UnitType.Km));
}
public void Distance_between_Moscow_and_NewYork_is_between_7505and7506()
{
var distance = Distance.Between(Moscow, NewYork);
//1.94km
Assert.True(distance > Distance.From(7500, UnitType.Km) && distance < Distance.From(7510, UnitType.Km));
}
private static ISummary log(IPlan plan)
{
var log = new Log();
var previousState = plan.Trajectory.First().State;
var distance = 0.0;
foreach (var trajectory in plan.Trajectory)
{
var state = trajectory.State;
var action = trajectory.Action;
log.SimulationTimeChanged(trajectory.Time);
if (action != null)
{
log.ActionSelected(action);
}
log.StateUpdated(state);
distance += Distance.Between(previousState.Position, state.Position);
previousState = state;
}
return(new SimulationSummary(plan.TimeToGoal, Result.Suceeded, log.History, distance));
}
public void Between_getsThreePoints_returns2dDistance()
{
//arrange
Record record = new Record();
record.Number = 1;
int[] a = { 1, 1, 4 };
record.Pixels = a;
Record record1 = new Record();
record.Number = 1;
int[] b = { 1, 3, 4 };
record1.Pixels = b;
Result expectedResult = new Result();
expectedResult.Distance = 2;
expectedResult.Number = 1;
Distance distance = new Distance();
//act
var result = distance.Between(record, record1);
//assert
Debug.WriteLine(result);
System.Console.WriteLine(result);
Assert.AreEqual(expectedResult.Distance, result.Distance);
}
public void Calculations_Work_GivenDifferentMetricUnits()
{
double meters = Distance.Between(p1, p2).In(MetricUnit.meters);
double kilometers = Distance.Between(p1, p2).In(MetricUnit.kilometers);
Assert.AreEqual(meters / 1000, kilometers);
}
public void Generate(PlayerInfo owner, GameState state, ActionCandidates candidates)
{
if (!owner.IsBallOwner)
{
return;
}
foreach (var pass in Velocities)
{
var path = BallPath.Create(owner.Position, pass, PickUpTimer, MaximumPathLength);
// We don't want to risk passes ending up in our own goal.
if (path.End != BallPath.Ending.GoalOwn)
{
var catchUp = path.GetCatchUps(state.Current.Players).FirstOrDefault();
// safe to pass.
if (catchUp == null || catchUp.Player.IsOwn && catchUp.Player != owner)
{
var length = catchUp == null ? path.Count : catchUp.Turn;
var target = catchUp == null?path.Last() : catchUp.Position;
if (Distance.Between(target, owner) > MinimumPassDistance)
{
candidates.Add(
Evaluator.GetPositionImprovement(owner, target, length),
Actions.Shoot(owner, owner.Position + pass, PassPower));
}
}
}
}
}
/// <summary>Creates a player path.</summary>
public static PlayerPath Create(Position player, Velocity velocity, IPoint target, int fallenTimer, int maxLength, Distance tolerance)
{
var path = new PlayerPath();
for (var fallen = fallenTimer; fallen < 0; fallen++)
{
path.Add(player);
}
var pos = player;
var vel = velocity;
for (var turn = -fallenTimer; turn < maxLength; turn++)
{
if (Distance.Between(pos, target) < tolerance)
{
path.ReachedTarget = true;
break;
}
path.Add(pos);
pos += vel;
vel = NewVelocity(vel, pos, target);
}
return(path);
}
public static Angle GetShootAngle(IPoint ball, IPoint opponent, Power power)
{
var speed = power.Speed;
var distance = (float)Distance.Between(ball, opponent);
return(ShootAngles[SpeedToKey(speed), DistanceToKey(distance)]);
}
private double fitnessOf(ISummary summary)
{
var fitness = 0.0;
// time-out x crashing x nothing
switch (summary.Result)
{
case Result.TimeOut:
return(0.0);
case Result.Failed:
// still better than timing out
break;
case Result.Suceeded:
fitness += 100; // on top of the "distance travelled" score
break;
}
// points for every way point passed
fitness += summary.DistanceTravelled * 100 / Math.Max(1.0, summary.SimulationTime.TotalSeconds);
var actionsInTotal = 0;
var actionsWithHightThrottleAndSmallSteeringAngle = 0;
var totalDistanceTravelled = 0.0;
VehicleState?previous = null;
foreach (var log in summary.Log)
{
// ideas:
// - proportion of time when it was going at max speed
// - minimize number of direction changes
if (log is IActionSelectedEvent selected)
{
actionsInTotal++;
if (selected.Action.Throttle > 0.75 && Math.Abs(selected.Action.Steering) < 0.2)
{
actionsWithHightThrottleAndSmallSteeringAngle++;
}
}
else if (log is VehicleStateUpdatedEvent updated)
{
if (previous.HasValue)
{
totalDistanceTravelled += Distance.Between(updated.State.Position, previous.Value.Position);
}
previous = updated.State;
}
}
fitness += 10 * ((double)actionsWithHightThrottleAndSmallSteeringAngle / actionsInTotal);
fitness += 10 * summary.SimulationTime.TotalSeconds / totalDistanceTravelled;
return(Math.Max(0.0, fitness));
}
public void GetDistance_From3x150yToOwn_233f02()
{
var point = new Position(3, 150);
var act = Goal.Own.GetDistance(point);
var exp = Distance.Between(point, new Position(0, 383));
Assert.AreEqual(exp, act);
}
public void GetDistance_From1500x750yToOwn_1501f()
{
var point = new Position(1500, 750);
var act = Goal.Own.GetDistance(point);
var exp = Distance.Between(point, new Position(0, 695));
Assert.AreEqual(exp, act);
}
private VehicleState interpolate(VehicleState state, VehicleState target, VehicleState afterTarget, double lookahead)
{
var intermediatePosition = calculateIntersection(state.Position, lookahead, target.Position, afterTarget.Position);
var t = Distance.Between(intermediatePosition, target.Position) / Distance.Between(afterTarget.Position, target.Position);
return(new VehicleState(
position: intermediatePosition,
headingAngle: t * target.HeadingAngle + (1 - t) * afterTarget.HeadingAngle,
angularVelocity: t * target.AngularVelocity + (1 - t) * afterTarget.AngularVelocity,
speed: t * target.Speed + (1 - t) * afterTarget.Speed));
}
/// <summary>Gets the distance to the goal.</summary>
/// <remarks>
/// If above or under the goal, it takes the closed corner, otherwise
/// it takes the X difference.
/// </remarks>
public Distance GetDistance(IPoint other)
{
var goal = Bottom;
// above the goal.
if (other.Y <= MinimumY)
{
goal = Top;
}
// between Top and bottom
else if (other.Y < MaximumY)
{
return(new Distance(X - other.X));
}
return(Distance.Between(goal, other));
}
public double DistanceToClosestObstacle(Vector origin, double angle)
{
var ray = origin;
var direction = Vector.From(track.TileSize, angle);
while (!track.IsOccupied(ray + direction))
{
ray += direction;
if (Distance.Between(origin, ray) > maximumDistance)
{
return(maximumDistance);
}
}
return(Distance.Between(origin, ray));
}
private int findClosestPoint(VehicleState state)
{
var closestState = 0;
var shortestDistance = double.MaxValue;
for (int i = 0; i < path.Count; i++)
{
var segment = path[i];
var distance = Distance.Between(state.Position, segment.State.Position);
if (distance < shortestDistance)
{
closestState = i;
shortestDistance = distance;
}
}
return(closestState);
}
private ShortestPathNode simplifyPath(ShortestPathNode head)
{
var start = head;
var node = head.Next;
while (node != null)
{
if (node.IsGoal || node.Next != null && (!areInLineOfSight(start.Position, node.Next.Position) || start.TargetWayPoint != node.TargetWayPoint))
{
start.Next = node;
start.CostToNext = TimeSpan.FromSeconds(Distance.Between(start.Position, node.Position) / maxSpeed);
start = node;
}
node = node.Next;
}
return head;
}
private double calculateSteering(VehicleState currentState, int target)
{
var targetPosition = path[target].State.Position;
var followingPointPosition = target < path.Count - 1
? path[target + 1].State.Position
: targetPosition + (targetPosition - path[target - 1].State.Position); // extend the last segment in the same direction
var targetDirection = followingPointPosition - targetPosition;
var directionToTarget = targetPosition - currentState.Position;
var leftOrRight = Math.Sign(targetDirection.Cross(directionToTarget));
var distance = Distance.Between(targetPosition, currentState.Position);
crossTrackError.OnNext(leftOrRight * distance);
var steering = -steeringController.Calculate(target: 0, leftOrRight * distance);
Console.WriteLine($"cte={leftOrRight * distance}, steering={steering}");
return(Math.Clamp(steering, -1, 1));
}
private double travelledDistance(VehicleState vehicleState, IReadOnlyList <IGoal> wayPoints, int lastTargetWayPoint)
{
if (lastTargetWayPoint == wayPoints.Count)
{
return(1.0);
}
var distanceBetweenWayPoints = Distance.Between(
wayPoints[lastTargetWayPoint].Position,
lastTargetWayPoint > 0
? wayPoints[lastTargetWayPoint - 1].Position
: world.Track.Circuit.Start);
var distanceToTheWayPoint = Distance.Between(wayPoints[lastTargetWayPoint].Position, vehicleState.Position);
var distanceProportion = distanceToTheWayPoint / distanceBetweenWayPoints;
var distanceTravelledBetweenWayPoints = Math.Clamp(1 - distanceProportion, -1.0, 1.0);
return((double)lastTargetWayPoint / wayPoints.Count + distanceTravelledBetweenWayPoints / wayPoints.Count);
}
public void Generate(PlayerInfo owner, GameState state, ActionCandidates candidates)
{
if (!owner.IsBallOwner || owner.DistanceToOtherGoal > MaximumShootDistance)
{
return;
}
var veloTop = Goal.Other.Top - owner.Position;
var veloBot = Goal.Other.Bottom - owner.Position;
var goalAngle = Angle.Between(veloTop, veloBot);
var power95Per = Power.Maximum * (0.5f * (float)goalAngle / (float)TwoStandardDeviation);
var target = owner.Position + veloBot.Rotate((double)goalAngle * 0.5);
var turns = (float)Distance.Between(owner, target) / (float)power95Per;
if (turns <= PickUpTimer)
{
candidates.Add(float.MaxValue, Actions.Shoot(owner, target, power95Per));
return;
}
else
{
foreach (var power in ShootPowers)
{
foreach (var goal in GoalTargets)
{
var velocity = Shoot.ToTarget(owner, goal, power);
var path = BallPath.Create(owner.Position, velocity, 7, 200);
var catchUp = path.GetCatchUps(state.Current.OtherPlayers).FirstOrDefault();
if (catchUp == null)
{
candidates.Add(1000, Actions.Shoot(owner, goal, power));
return;
}
}
}
}
}
/// <summary>Gets the catch ups for the path.</summary>
public static IEnumerable <CatchUp> GetCatchUps(List <Position> path, int pickUpTimer, IEnumerable <PlayerInfo> players)
{
var queue = new Queue <PlayerInfo>(players);
for (var turn = pickUpTimer; turn < path.Count; turn++)
{
if (queue.Count == 0)
{
break;
}
for (var p = 0; p < queue.Count; p++)
{
var player = queue.Dequeue();
// the player can not run yet.
if (-player.FallenTimer > turn)
{
queue.Enqueue(player); continue;
}
var distanceToBall = Distance.Between(path[turn], player.Position);
var speed = PlayerPath.GetInitialSpeed(player, path[turn]);
var playerReach = PlayerPath.GetDistance(speed, turn + player.FallenTimer, 40);
if (distanceToBall <= playerReach)
{
yield return(new CatchUp()
{
Turn = turn,
Player = player,
Position = path[turn],
});
}
else
{
queue.Enqueue(player);
}
}
}
}
public VehicleState FindTarget(VehicleState state, int waypoint, double lookahead)
{
var lastSegment = Segments.Count - 1;
var target = lastSegment;
var orderedCandidates = Segments.Where(segment => segment.TargetWayPoint == waypoint).Reverse();
for (var i = lastSegment; i >= 0; i--)
{
if (Distance.Between(state.Position, Segments[i].State.Position) < lookahead)
{
target = i;
break;
}
}
if (target == lastSegment)
{
// if the target is the last point, don't look any further
return(Segments[lastSegment].State);
}
// calculate a point along the segment between the next points
return(interpolate(state, Segments[target].State, Segments[target + 1].State, lookahead));
}
public void Between_ReturnsPointToPointDistance_GivenTwoGeoPoints()
{
Assert.IsInstanceOfType(Distance.Between(p1, p2), typeof(PointToPointDistance));
}
public double DistanceBetween(Vector a, Vector b) => (Distance.Between(a, b) / maximumDistance);
public void Calculations_ReturnsSaneValues()
{
Assert.AreEqual(492451, Math.Round(Distance.Between(p1, p2).In(MetricUnit.meters)));
}
public void Distance_MustBeReflexive()
{
Distance distance = Distance.Between(p1, p2);
Assert.AreEqual(distance, Distance.Between(p2, p1));
}
private ShortestPathNode? findShortestPath(Vector start, IReadOnlyList<IGoal> wayPoints, double stepSize)
{
var open = new BinaryHeapOpenSet<GridKey, GridSearchNode>();
var closed = new ClosedSet<GridKey>();
open.Add(
new GridSearchNode(
key: new GridKey(start, wayPoints.Count),
start,
previous: null,
distanceFromStart: 0.0,
estimatedCost: 0.0,
wayPoints,
targetWayPoint: 0));
while (!open.IsEmpty)
{
var nodeToExpand = open.DequeueMostPromissing();
if (nodeToExpand.RemainingWayPoints.Count == 0)
{
var head = new ShortestPathNode(
wayPoints.Last().Position, // use the goal position directly
targetWayPoint: wayPoints.Count - 1); // the goal way point should be kept here
var backtrackingNode = nodeToExpand.Previous;
while (backtrackingNode != null)
{
var node = new ShortestPathNode(backtrackingNode.Position, backtrackingNode.TargetWayPoint);
node.CostToNext = TimeSpan.FromSeconds(Distance.Between(node.Position, head.Position) / maxSpeed);
node.Next = head;
head = node;
backtrackingNode = backtrackingNode.Previous;
}
return head;
}
closed.Add(nodeToExpand.Key);
for (var dx = -1; dx <= 1; dx++)
{
for (var dy = -1; dy <= 1; dy++)
{
if (dx == 0 && dy == 0) continue;
var nextPoint = new Vector(
nodeToExpand.Position.X + dx * stepSize,
nodeToExpand.Position.Y + dy * stepSize);
var reachedWayPoint = nodeToExpand.RemainingWayPoints[0].ReachedGoal(nextPoint);
var remainingWayPoints = reachedWayPoint
? nodeToExpand.RemainingWayPoints.Skip(1).ToList().AsReadOnly()
: nodeToExpand.RemainingWayPoints;
var targetWayPoint = wayPoints.Count - nodeToExpand.RemainingWayPoints.Count; // I want to keep the ID of the waypoint in the node which reaches the waypoint and only increase it for its childer
var key = new GridKey(nextPoint, remainingWayPoints.Count);
if (closed.Contains(key))
{
continue;
}
if (collisionDetector.IsCollision(nextPoint))
{
closed.Add(key);
continue;
}
var distance = nodeToExpand.DistanceFromStart + (nextPoint - nodeToExpand.Position).CalculateLength();
var node = new GridSearchNode(key, nextPoint, nodeToExpand, distance, distance + 0, remainingWayPoints, targetWayPoint);
if (open.Contains(node.Key))
{
if (node.DistanceFromStart < open.Get(node.Key).DistanceFromStart)
{
open.ReplaceExistingWithTheSameKey(node);
}
}
else
{
open.Add(node);
}
}
}
}
return null;
}
public void Calculations_ReturnFixedDistances()
{
Assert.IsInstanceOfType(Distance.Between(p1, p2).AddDistance(Distance.Meters(100)), typeof(FixedDistance));
Assert.IsInstanceOfType(Distance.Between(p1, p2).SubtractDistance(Distance.Meters(100)), typeof(FixedDistance));
}
protected override void TraverseNext(ref Path path, ref Path unvisitedCities, ref int calculationAmount)
{
var shortestDistance = double.MaxValue;
City nextCity = null;
var index = 0;
// Path is empty, so start at first city
if (path.Count == 0)
{
// Next city is the first city in the queue
nextCity = unvisitedCities.First();
// Add next city
path.Add(nextCity);
unvisitedCities.Remove(nextCity);
}
// Make first edge
else if (path.Count == 1)
{
foreach (var unvisitedCity in unvisitedCities)
{
var distance = Distance.Between(unvisitedCity, path.Last());
// Increase calculation counter
calculationAmount++;
if (distance < shortestDistance)
{
shortestDistance = distance;
nextCity = unvisitedCity;
}
}
// Add next city
path.Add(nextCity);
unvisitedCities.Remove(nextCity);
}
// Search for closest city to an edge in the path
else
{
foreach (var unvisitedCity in unvisitedCities)
{
for (int i = 0; i < path.Count - 1; i++)
{
var firstEdgeCity = path[i];
var secondEdgeCity = path[i + 1];
// Distance from unvisited city to the line segment between first edge city and second edge city
var distance = Distance.Between(unvisitedCity, firstEdgeCity, secondEdgeCity);
// Increment calculation counter
calculationAmount++;
if (distance < shortestDistance)
{
// Update shortest distance and the next city
shortestDistance = distance;
nextCity = unvisitedCity;
index = path.IndexOf(secondEdgeCity);
}
}
}
System.Diagnostics.Debug.Write($"{nextCity.Name} -> ");
// Add next city
path.Insert(index, nextCity);
unvisitedCities.Remove(nextCity);
}
}
public void Project_ReturnsExpectedDistance()
{
GeoPoint pointTwo = geoPoint.Project(Distance.Meters(587), Bearing.DecimalDegrees(237));
Assert.AreEqual(587, Math.Round(Distance.Between(geoPoint, pointTwo).In(MetricUnit.meters)));
}
