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