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