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