Exemplo n.º 1
0
        public static void Main()
        {
            List<Segment> seg = new List<Segment>();
            Random rand = new Random();
            Segment vector = new Segment(new Point(rand.Next(), rand.Next()), new Point(rand.Next(), rand.Next()));
            Stopwatch stopwatch = new Stopwatch();

            for (int i = 1; i <= 10000; i = i * 10)
            {
                // create the test set
                seg.Clear();
                for (int j = 0; j < i; j++)
                    seg.Add(new Segment(new Point(rand.Next(), rand.Next()), new Point(rand.Next(), rand.Next())));

                // benchmark
                stopwatch.Start();
                for (int j = 0; j < 100; j++)
                {
                    foreach (Segment s in seg)
                    {
                        vector.Intersects(s);
                    }
                }
                stopwatch.Stop();

                // display
                using (TextWriter fs = new StreamWriter("bench.txt", true))
                {
                    Console.Out.WriteLine("{0} {1}", i, stopwatch.ElapsedMilliseconds / 100);
                }
            }
        }
Exemplo n.º 2
0
        public double getDistance()
        {
            // Retrieve the environment
            Environment env = Environment.Instance;
            double maxDistance = -1;

            // Create the range segment
            this.range = new Segment();
            range.Start.X = env.AgentPosition.X;
            range.Start.Y = env.AgentPosition.Y;
            range.End.X = env.AgentPosition.X;
            range.End.Y = env.AgentPosition.Y + length;
            range = range.Rotate(env.AgentAngle + angleOffset);

            // Go through the list of segments, not neat O(n) but execution
            // fast enough for this simulation purpose
            foreach (Segment s in env.Segments)
            {
                Point p;
                if ((p = range.Intersects(s)) != null)
                {
                    double v = p.Distance(range.Start);
                    if (v < maxDistance || maxDistance == -1)
                    {
                        maxDistance = v;
                    }
                }
            }

            return maxDistance;
        }
Exemplo n.º 3
0
        /// <summary>
        /// Return the point of intersection of the two segments AB and CD
        /// if the two segments does not intersect returns null
        /// </summary>
        /// <param name="seg">segment</param>
        /// <returns>A point of intersection or null</returns>
        public Point Intersects(Segment seg)
        {
            double deltaACy = this.Start.Y - seg.Start.Y;
            double deltaDCx = seg.End.X - seg.Start.X;
            double deltaACx = this.Start.X - seg.Start.X;
            double deltaDCy = seg.End.Y - seg.Start.Y;
            double deltaBAx = this.End.X - this.Start.X;
            double deltaBAy = this.End.Y - this.Start.Y;

            double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx;
            double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy;

            if (denominator == 0)
            {
                if (numerator == 0)
                {
                    // collinear. Potentially infinite intersection points.
                    // Check and return one of them.
                    if (this.Start.X >= seg.Start.X && this.Start.X <= seg.End.X)
                    {
                        return this.Start;
                    }
                    else if (seg.Start.X >= this.Start.X && seg.Start.X <= this.End.X)
                    {
                        return seg.Start;
                    }
                    else
                    {
                        return null;
                    }
                }
                else
                { // parallel
                    return null;
                }
            }

            double r = numerator / denominator;
            if (r < 0 || r > 1)
            {
                return null;
            }

            double s = (deltaACy * deltaBAx - deltaACx * deltaBAy) / denominator;
            if (s < 0 || s > 1)
            {
                return null;
            }

            return new Point(this.Start.X + r * deltaBAx, this.Start.Y + r * deltaBAy);
        }
Exemplo n.º 4
0
        public void straight(int speed)
        {
            Environment env = Environment.Instance;

            Segment nextPos = new Segment();
            nextPos.Start.X = env.AgentPosition.X;
            nextPos.Start.Y = env.AgentPosition.Y;
            nextPos.End.X = env.AgentPosition.X;
            //nextPos.End.Y = env.AgentPosition.Y + (longDistance > 0 ? Math.Min(longDistance, 1) : 1);
            nextPos.End.Y = env.AgentPosition.Y + 1;
            nextPos = nextPos.Rotate(env.AgentAngle);

            env.AgentPosition = nextPos.End;
        }
Exemplo n.º 5
0
        private UIElement drawSegment(Segment s, SolidColorBrush color)
        {
            Line line = new Line();

            line.X1 = s.Start.X;
            line.Y1 = s.Start.Y;
            line.X2 = s.End.X;
            line.Y2 = s.End.Y;
            line.Stroke = color;

            //canvas1.Children.Add(line);
            grid1.Children.Add(line);

            return line;
        }
Exemplo n.º 6
0
        /// <summary>
        /// 
        /// </summary>
        /// <param name="p">Point to translate the origin to</param>
        /// <returns>translated segment</returns>
        public Segment Translate(Point p)
        {
            Segment r = new Segment();

            r.Start = p;
            r.End.X = this.End.X - this.Start.X + p.X;
            r.End.Y = this.End.Y - this.Start.Y + p.Y;

            return r;
        }
Exemplo n.º 7
0
        public void run()
        {
            double prevReading = 0;
            double newReading = 0;

            // Wall following
            while (!gohome)
            {
                // Go to the wall
                while (!longRangeSensor.againstWall())
                {
                    movement.straight(1);
                    distance += 1;
                    System.Threading.Thread.Sleep(5);
                }

                nodes.Insert(distance, angle, 5);
                distance = 0;

                // While the agent is touching a wall
                while (longRangeSensor.againstWall())
                {
                    movement.spinLeft(1, 1);
                    angle = (angle - 1) % 360;
                    System.Threading.Thread.Sleep(5);
                }

                // start following the wall adjusting the angle
                prevReading = ShortRangeSensor.getDistance();
                while (!longRangeSensor.againstWall())
                {
                    movement.straight(1);
                    distance += 1;
                    newReading = shortRangeSensor.getDistance();

                    double alpha;

                    if (prevReading == -1)
                        alpha = 0;
                    else if (newReading == -1)
                    {
                        alpha = 90;
                    }
                    else
                    {
                        alpha = Math.Atan((newReading - prevReading) / 1) * 180 / Math.PI;
                        if (Math.Abs(alpha) < 5)
                            alpha = 0;
                    }

                    if (alpha != 0)
                    {
                        //no more wall
                        nodes.Insert(distance, angle, 5);
                        distance = 0;
                    }

                    movement.spinRight(1, alpha);
                    angle = (angle + alpha) % 360;

                    prevReading = newReading;

                    System.Threading.Thread.Sleep(5);
                }
            }

            nodes.Insert(distance, angle, 5);
            distance = 0;

            while (nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
            {
                // Go home
                // point in direction to the start
                Point A = new Point(0, 0);
                Point C = nodes.CurrentPosition(distance, angle);
                Point Btemp = C + new Point(0, 100);
                Point B = new Segment(C, Btemp).Rotate(angle).End;

                double a_sq = Math.Pow(100, 2);
                double b_sq = A.SquareDistance(C);
                double c_sq = A.SquareDistance(B);

                double gamma;

                gamma = Math.Acos((c_sq - a_sq - b_sq) / (-2 * Math.Sqrt(a_sq) * Math.Sqrt(b_sq))) * 180 / Math.PI;

                if (b_sq > c_sq)
                {
                    gamma = 90 - gamma;
                }

                if (angle < 0)
                    angle = (360 + angle);

                if (angle < 180)
                {
                    movement.spinRight(1, gamma);
                    angle = (angle + gamma) % 360;
                }
                else
                {
                    movement.spinLeft(1, gamma);
                    angle = (angle - gamma) % 360;
                }

                // no wall let's move straight
                while (longRangeSensor.getDistance() == -1)
                {
                    movement.straight(1);
                    distance++;
                    System.Threading.Thread.Sleep(5);
                }

                // A wall is in range
                Point pos = nodes.CurrentPosition(distance, angle);
                Segment sensor = new Segment(pos, new Point(longRangeSensor.getDistance(), 0)).Rotate(angle);
                BDNode near = nodes.Nearest(sensor.End, 1)[0];
                if (near.Next != null && new Segment(near.Coord, near.Next.Coord).Intersects(sensor) != null)
                    ;//MessageBox.Show("intersect");
                else if (near.Previous != null && new Segment(near.Coord, near.Previous.Coord).Intersects(sensor) != null)
                    ;//MessageBox.Show("intersect");
                else
                    ;// MessageBox.Show("unknown wall");

                // move straight until hit the wall
                while (!longRangeSensor.againstWall() && nodes.CurrentPosition(distance, angle).Distance(startPoint) > 5)
                {
                    movement.straight(1);
                    distance++;
                    System.Threading.Thread.Sleep(5);
                }

                nodes.Insert(distance, angle, 5);
                distance = 0;

                // While the agent is touching a wall
                while (longRangeSensor.inRange())
                {
                    movement.spinLeft(1, 1);
                    angle = (angle - 1) % 360;
                    System.Threading.Thread.Sleep(5);
                }
                while (shortRangeSensor.inRange())
                {
                    movement.straight(1);
                    distance++;
                    System.Threading.Thread.Sleep(5);
                }
                nodes.Insert(distance, angle, 5);
                distance = 0;
            }
        }
Exemplo n.º 8
0
        public Point CurrentPosition(double distance, double angle)
        {
            Point p;
            if (previous == null)
            {
                p = new Point(distance, 0);
            }
            else
            {
                // Touched a wall
                Segment s = new Segment();
                s.Start = previous.Coord;
                s.End = s.Start;
                s.End.X += distance;
                p = new Point(s.Rotate(angle).End);
            }

            return p;
        }