Exemplo n.º 1
0
        public void FindIntersectionTest(double tX, double tY, int expected, bool correct)
        {
            var intersections = new[]
            {
                new Intersection(new Vector(100, 100), 10),
                new Intersection(new Vector(100, 200), 10),
                new Intersection(new Vector(200, 200), 10),
                new Intersection(new Vector(200, 100), 10)
            };

            var city = new CityBuilder()
                       .Intersection(intersections[0])
                       .Intersection(intersections[1])
                       .Intersection(intersections[2])
                       .Intersection(intersections[3])
                       .Road(new Road(new Vector(100, 105), new Vector(100, 195), 10, 100))
                       .Road(new Road(new Vector(105, 100), new Vector(195, 100), 10, 100))
                       .Road(new Road(new Vector(105, 200), new Vector(195, 200), 10, 100))
                       .Road(new Road(new Vector(200, 105), new Vector(200, 195), 10, 100))
                       .Build();

            var intersection = GPSSystem.FindIntersection(new Vector(tX, tY));

            if (correct)
            {
                Assert.AreEqual(intersections[expected], intersection);
            }
            else
            {
                Assert.AreNotEqual(intersections[expected], intersection);
            }
        }
Exemplo n.º 2
0
        public Destination Calculate(long callerId, Destination carDestination, Destination endDestination)
        {
            var network   = NodeNetwork.NodeNetwork.GetInstance();
            var startNode = new Node(GPSSystem.FindIntersection(carDestination.Location).Location);
            var endNodes  = AlgorithmTools.IntersectionTupleToNodeTuple(
                AlgorithmTools.GetIntersectionOrderForRoadSide(endDestination.Road, endDestination.Location));

            if (startNode.Equals(endNodes.Item1))
            {
                #if DEBUG
                AlgorithmDebuggerWindow.Instance.AddNetworkResult(_debuggerIndex++.ToString(), network,
                                                                  startNode, endNodes.Item2, endNodes);
                #endif
                return(endDestination);
            }

            AssignNodeValues(ref network, ref startNode);

            var nextNode = FindNextNodeOnBestRoute(ref network, endNodes.Item1);
            #if DEBUG
            AlgorithmDebuggerWindow.Instance.AddNetworkResult("DY" + _debuggerIndex++, network,
                                                              startNode, nextNode, endNodes);
            #endif
            var roadX = (startNode.PositionX - nextNode.PositionX) / 2.0 + nextNode.PositionX;
            var roadY = (startNode.PositionY - nextNode.PositionY) / 2.0 + nextNode.PositionY;
            return(new Destination
            {
                Location = new Vector(nextNode.PositionX, nextNode.PositionY),
                Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
            });
        }
Exemplo n.º 3
0
        public Destination Calculate(long callerId, Destination carDestination, Destination endDestination)
        {
            var network   = NodeNetwork.NodeNetwork.GetInstance();
            var startNode = new Node(GPSSystem.FindIntersection(carDestination.Location).Location);
            var endNodes  = AlgorithmTools.IntersectionTupleToNodeTuple(
                AlgorithmTools.GetIntersectionOrderForRoadSide(endDestination.Road, endDestination.Location));

            Node   nextNode;
            double roadX, roadY;

            if (startNode.Equals(endNodes.Item1))
            {
                #if DEBUG
                AlgorithmDebuggerWindow.Instance.AddNetworkResult(_debuggerIndex++.ToString(), network,
                                                                  startNode, endNodes.Item2, endNodes);
                #endif
                return(endDestination);
            }

            if (_calculationCache.ContainsKey(callerId))
            {
                var tuple = _calculationCache[callerId];
                if (tuple.Item1.Equals(endDestination))
                {
                    var path     = tuple.Item2;
                    var thisNode = path.Single(n => n.Equals(startNode));
                    nextNode = thisNode.ConnectedTo;

                    #if DEBUG
                    AlgorithmDebuggerWindow.Instance.AddNetworkResult(_debuggerIndex++.ToString(), network,
                                                                      startNode, nextNode, endNodes);
                    #endif
                    roadX = (startNode.PositionX - nextNode.PositionX) / 2.0 + nextNode.PositionX;
                    roadY = (startNode.PositionY - nextNode.PositionY) / 2.0 + nextNode.PositionY;
                    return(new Destination
                    {
                        Location = new Vector(nextNode.PositionX, nextNode.PositionY),
                        Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
                    });
                }

                _calculationCache.Remove(callerId);
            }

            var endNode = endNodes.Item1;
            nextNode = CalculatePathNextNode(callerId, ref network, ref startNode, ref endNode, endDestination);

            #if DEBUG
            AlgorithmDebuggerWindow.Instance.AddNetworkResult("AS" + _debuggerIndex++, network,
                                                              startNode, nextNode, endNodes);
            #endif
            roadX = (startNode.PositionX - nextNode.PositionX) / 2.0 + nextNode.PositionX;
            roadY = (startNode.PositionY - nextNode.PositionY) / 2.0 + nextNode.PositionY;
            return(new Destination
            {
                Location = new Vector(nextNode.PositionX, nextNode.PositionY),
                Road = GPSSystem.NearestRoad(new Vector(roadX, roadY))
            });
        }
Exemplo n.º 4
0
        private static void PrintMap()
        {
            var builder = new StringBuilder();

            builder.Append('\n');

            for (var y = 0; y < 450; y += 10)
            {
                for (var x = 0; x < 800; x += 10)
                {
                    var vector = new Vector(x, y);
                    if (GPSSystem.FindIntersection(vector) != null)
                    {
                        builder.Append('%');
                    }
                    else if (GPSSystem.GetRoad(vector) != null)
                    {
                        builder.Append('#');
                    }
                    else if (IsCustomer(vector))
                    {
                        builder.Append('$');
                    }
                    else if (IsBuilding(vector))
                    {
                        builder.Append('@');
                    }
                    else
                    {
                        builder.Append(' ');
                    }
                }

                builder.Append('\n');
            }

            App.Console.Print(builder.ToString());
        }
Exemplo n.º 5
0
        public static void GenerateNetwork(List <Road> roads, List <Intersection> intersections)
        {
            Nodes = new Node[intersections.Count];
            Links = new Link[roads.Count];

            // Adding Nodes using Intersections
            for (var i = 0; i < Nodes.Length; ++i)
            {
                var intersectionLocation = intersections[i].Location;
                Nodes[i] = new Node(intersectionLocation.X, intersectionLocation.Y);
            }

            // Adding Links using Roads
            for (var i = 0; i < Links.Length; ++i)
            {
                var endpointA = new Node(GPSSystem.FindIntersection(roads[i].Start).Location);
                var endpointB = new Node(GPSSystem.FindIntersection(roads[i].End).Location);
                var nodeA     = Nodes.Single(node => node.Equals(endpointA));
                var nodeB     = Nodes.Single(node => node.Equals(endpointB));

                Links[i] = new Link(ref nodeA, ref nodeB);
            }
        }
Exemplo n.º 6
0
        /// <summary>
        /// Runs the main car logic. Needs to be subscribed to the MainLoop.
        /// </summary>
        public void Update()
        {
            // If we're not initialized, initialize!
            if (!initialized)
            {
                Init();
            }

            // Update the passenger count.
            Car.PassengerCount = Car.Passengers.Count;

            // Update the current road with the road at our location.
            Car.CurrentRoad         = GPSSystem.GetRoad(Car.Location);
            Car.CurrentIntersection = GPSSystem.FindIntersection(Car.Location);

            if (Car.CurrentRoad == null && Car.CurrentIntersection == null)
            {
                if (!resetTarget)
                {
                    resetTarget = true;
                    var closestRoad = GPSSystem.NearestRoad(Car.Location);
                    if (closestRoad != null && Car != null)
                    {
                        Car.CurrentTarget =
                            MathUtil.Distance(closestRoad.End, Car.Location) >
                            MathUtil.Distance(closestRoad.Start, Car.Location)
                                ? closestRoad.Start
                                : closestRoad.End;
                        App.Console.Print($"[C{Car.Id}] Lost road, trying to get back on, targeting {Car.CurrentTarget}", Colors.Blue);
                    }
                    ;
                }
            }
            else if (resetTarget)
            {
                App.Console.Print($"[C{Car.Id}] Road found again", Colors.Blue);
                resetTarget = false;
            }

            // Calculate the distance to the local target (usually the next intersection).
            var distanceToTarget = MathUtil.Distance(Car.Location, Car.CurrentTarget);
            // Calculate the distance to the destination.
            var distanceToDestination = MathUtil.Distance(Car.Location, Car.Destination.Location);
            // Calculate the relative yaw (in degrees).
            var yaw = MathUtil.VectorToAngle(Car.Rotation, Car.Direction);
            // Get the current velocity of the car.
            var velocity = Car.Velocity;
            // Create a variable to store the added rotation in this update call.
            var addedRotation      = 0.0;
            var closeToDestination = CloseToDestination();

            // Check if we're close to our target but not the destination.
            if (distanceToTarget < 20 && !closeToDestination && !resetTarget)
            {
                // Check if we've not obtained a new target yet.
                if (!obtainedNewTarget)
                {
                    // Find the nearest intersection.
                    var intersection = GPSSystem.FindIntersection(GetClosestRoadPoint(Car.Location));
                    if (intersection == null)
                    {
                        return;
                    }

                    App.Console.Print($"[C{Car.Id}] Requesting new target from intersection {intersection.Location}...", Colors.Blue);

                    // Request the next target from the GPSSystem.
                    var target = GPSSystem.GetDirection(Car, intersection);

                    // Update our target.
                    newTarget         = target.Location;
                    obtainedNewTarget = true;

                    var distance = Math.Round(MathUtil.Distance(newTarget, Car.Location));
                    App.Console.Print($"[C{Car.Id}] Obtained a new target {newTarget} ({distance}m away)", Colors.Blue);
                }
            }
            else
            {
                obtainedNewTarget = false;
            }

            // Check if we are turning.
            if (turning > 0)
            {
                turning--;

                // Check if we locked on the new target yet.
                if (newTarget.X > -1)
                {
                    // Lock on the new target and reset newTarget.
                    Car.CurrentTarget = newTarget;
                    newTarget         = new Vector(-1, -1);
                    App.Console.Print($"[C{Car.Id}] Locked on to target", Colors.Blue);
                }

                // Call the handle function to turn.
                HandleTurn(ref velocity, ref yaw, ref addedRotation);
            }
            // Check if we're still turning around.
            else if (flipping)
            {
                // Stop turning around.
                flipping = false;
                App.Console.Print($"[C{Car.Id}] Turn-around done", Colors.Blue);
            }

            // Check if we're on an intersection.
            if (Car.CurrentIntersection != null)
            {
                // Reset our turn timer.
                turning = 20;
            }
            else
            {
                // Check if we're not close to the destination.
                if (!closeToDestination)
                {
                    // If we're still approaching, stop approaching.
                    if (approach)
                    {
                        approach = false;
                        App.Console.Print($"[C{Car.Id}] Approach done", Colors.Blue);
                    }

                    // Call the handle functions to stay in the lane and accelerate/deccelerate.
                    HandleStayInLane(ref velocity, ref yaw, ref addedRotation);
                    HandleAccelerate(ref velocity, ref distanceToTarget);
                }
                else
                {
                    // If we're not approaching, start approaching.
                    if (!approach)
                    {
                        approach = true;
                        App.Console.Print($"[C{Car.Id}] Initiating approach", Colors.Blue);
                    }

                    // Call the handle function to approach the target.
                    HandleApproachTarget(ref velocity, ref yaw, ref addedRotation, ref distanceToDestination);
                }
            }

            // Update the car's velocity with the result of the handle functions:
            // Temporarily store the current speed.
            var speed = velocity.Length;

            // Rotate the velocity based on the addedRotation this tick.
            velocity = MathUtil.RotateVector(velocity, -addedRotation);
            // Normalize the velocity and multiply with the speed to make sure it stays the same.
            velocity.Normalize();
            velocity = Vector.Multiply(velocity, speed);
            // Actually update the car's velocity.
            Car.Velocity = velocity;

            // Calculate the rotation by normalizing the velocity.
            var rotation = new Vector(velocity.X, velocity.Y);

            rotation.Normalize();

            // If the rotation vector is invalid or the car is not moving, set the rotation to the absolute direction.
            if (rotation.Length < 0.9 || double.IsNaN(rotation.Length) || velocity.Length < 0.1)
            {
                rotation = Car.Direction.GetVector();
            }

            // Actually update the rotation and update the absolute direction.
            Car.Rotation  = rotation;
            Car.Direction = DirectionCarMethods.Parse(rotation);

            // Update the car's location by adding the velocity to it.
            Car.Location          = Vector.Add(Car.Location, Car.Velocity);
            Car.DistanceTraveled += Car.Velocity.Length;
        }