public void AddZeebot_SmallTest()
{
List<MovingPoint> positions = new List<MovingPoint>();
positions.Add(new MovingPoint(1.5, 3.5, Math.PI / 2));
positions.Add(new MovingPoint(4.5, 6.5, Math.PI));
positions.Add(new MovingPoint(3.5, 1.5, 0));
positions.Add(new MovingPoint(5.5, 4.5, Math.PI / 3));
positions.Add(new MovingPoint(2.5, 8.5, Math.PI));
List<Zeebot> zeebots = new List<Zeebot>();
// Add the zeebots in the mapcontext.
for (int i = 0; i < positions.Count; i++)
{
Zeebot z = new Zeebot();
z.MapContext = mapContext;
z.CoordinateSystem = coordinateSystem;
mapContext.AddZeebotAt(z, positions[i]);
zeebots.Add(z);
}
// Run the CoordinateSystem for the same zeebots.
for (int i = 0; i < positions.Count; i++)
coordinateSystem.AddZeebot(zeebots[i]);
bool overlap = Overlap(zeebots, Calculated);
bool overlapWithSimetry = Overlap(zeebots, CalculatedWithSimetry);
Console.WriteLine("Overlap: {0}", overlap);
Console.WriteLine("Overlap with oX simetry: {0}", overlapWithSimetry);
Assert.Equals(true, overlap || overlapWithSimetry);
}
public CoordinateSystem()
{
Zeebots = new Dictionary<Zeebot, MovingPoint>();
PartialMap = new PartialMap();
z1 = z2 = z3 = null;
Sign = 0;
}
public void TestTryMove()
{
MapContext mapContext = new MapContext(MapBuilder.Build("Fixtures/map.in"));
Zeebot zeebot = new Zeebot();
mapContext.AddZeebot(zeebot);
mapContext.Zeebots[zeebot].Facing = Math.PI;
mapContext.Zeebots[zeebot].Location = new Point(3.5, 6.5);
double success;
bool result = mapContext.TryMove(zeebot, 2.0, out success);
Assert.Equals(false, result);
Assert.Less(success, 0.5, Util.Epsilon);
zeebot = new Zeebot();
mapContext.AddZeebot(zeebot);
mapContext.Zeebots[zeebot].Facing = Math.PI / 2;
mapContext.Zeebots[zeebot].Location = new Point(6.5, 3.5);
result = mapContext.TryMove(zeebot, 4.0, out success);
Assert.Equals(true, result);
Assert.Less(success, 4.01, Util.Epsilon);
zeebot = new Zeebot();
mapContext.AddZeebot(zeebot);
mapContext.Zeebots[zeebot].Facing = 1.50;
mapContext.Zeebots[zeebot].Location = new Point(1.60, 9.96);
result = mapContext.TryMove(zeebot, 0.7, out success);
Assert.Equals(false, result);
Assert.Less(success, 0.04, 0.00);
}
public void TestQueryLaser()
{
MapContext mapContext = new MapContext(MapBuilder.Build("Fixtures/map.in"));
double result;
Zeebot zeebot = new Zeebot();
mapContext.AddZeebot(zeebot);
mapContext.Zeebots[zeebot].Facing = Math.PI;
mapContext.Zeebots[zeebot].Location = new Point(3.5, 6.5);
result = mapContext.QueryLaser(zeebot);
Assert.Equals(result, 0.5, Util.Epsilon);
zeebot = new Zeebot();
mapContext.AddZeebot(zeebot);
mapContext.Zeebots[zeebot].Facing = Math.PI / 2;
mapContext.Zeebots[zeebot].Location = new Point(6.5, 4.0);
result = mapContext.QueryLaser(zeebot);
Assert.Equals(result, 6.00, Util.Epsilon);
zeebot = new Zeebot();
mapContext.AddZeebot(zeebot);
mapContext.Zeebots[zeebot].Facing = Math.PI / 2;
mapContext.Zeebots[zeebot].Location = new Point(9.5, 4.0);
result = mapContext.QueryLaser(zeebot);
Assert.Equals(double.NaN, result);
}
public void AddZeebot(Zeebot zeebot)
{
if (z1 == null)
{
// First zeebot, assume (0, 0, ?).
z1 = zeebot;
Zeebots[zeebot] = new MovingPoint(0, 0, double.NaN);
}
else
if (z2 == null)
{
// Second zeebot, assume (d, 0, ?). Later compute ?.
z2 = zeebot;
double d = z1.DistanceTo(z2);
Zeebots[zeebot] = new MovingPoint(d, 0, double.NaN);
}
else
if (z3 == null)
{
// Third zeebot, compute (x, y, ?). After compute all ? facings.
z3 = zeebot;
double x, y, success;
ComputeZ3(out x, out y);
// If y = 0, they are colinear, so I'll move it a bit before.
if (Util.IsZero(y))
{
z3.TryMove(1.0, out success);
ComputeZ3(out x, out y);
// If y = 0, change its facing, then move it again.
if (Util.IsZero(y))
{
z3.ChangeFacing(Math.PI / 2);
z3.ChangeFacing(1.0);
ComputeZ3(out x, out y);
}
}
Zeebots[zeebot] = new MovingPoint(x, y, double.NaN);
// Can now compute all facings: move one zeebot in a direction,
// then intersect the 3 circles to find its exact position -> determine facing.
ComputeFacing(z3);
ComputeFacing(z2);
ComputeFacing(z1);
}
else
{
// We already have the base in place, now use that to compute this
// zeebot's exact coordinates.
Point location = ComputeLocation(zeebot);
Zeebots[zeebot] = new MovingPoint(location, double.NaN);
ComputeFacing(zeebot);
// Now we determing the general facing direction (+/-) of the Coordinate System.
if (Sign == 0)
ComputeFacingDirection(zeebot);
}
}
public ZeebotSystem(MapContext mapContext, int nrZeebots)
{
this.mapContext = mapContext;
coordinateSystem = new CoordinateSystem();
zeebots = new List<Zeebot>(nrZeebots);
for (int i = 0; i < nrZeebots; i++)
{
Zeebot zeebot = new Zeebot();
zeebots.Add(zeebot);
// Add to real map.
zeebot.MapContext = mapContext;
mapContext.AddZeebot(zeebot);
// Add to calculated map.
zeebot.CoordinateSystem = coordinateSystem;
coordinateSystem.AddZeebot(zeebot);
}
}
MovingPoint Calculated(Zeebot z)
{
return new MovingPoint(coordinateSystem.Zeebots[z]);
}
MovingPoint Real(Zeebot z)
{
return mapContext.Zeebots[z];
}
Point ComputeLocation(Zeebot zeebot)
{
return IntersectCircles(z1.Location, z1.DistanceTo(zeebot),
z2.Location, z2.DistanceTo(zeebot),
z3.Location, z3.DistanceTo(zeebot));
}
public Discovery(Zeebot zeebot)
{
this.zeebot = zeebot;
}
void ComputeFacing(Zeebot zeebot)
{
double success;
// Find 2 Zeebot references - z1 and (z2 or z3) - non-colinear with zeebot
Zeebot zref1, zref2;
if (zeebot == z1)
{
zref1 = z2;
zref2 = z3;
}
else
{
zref1 = z1;
if (zeebot.Location.OnLine(z1.Location, z2.Location)) zref2 = z3;
else zref2 = z2;
}
zeebot.TryMove(1.0, out success);
for (int tries = 0; tries < 20 && success < Util.Epsilon; tries++)
{
zeebot.ChangeFacing(Math.PI / 7);
zeebot.TryMove(1.0, out success);
}
if (success < Util.Epsilon)
{
// Zeebot is "boxed-in"
throw new Exception("Not implemented: boxed-in zeebot");
}
Point current = IntersectCircles(zeebot.Location, success,
zref1.Location, zref1.DistanceTo(zeebot),
zref2.Location, zref2.DistanceTo(zeebot));
Point diff = current - zeebot.Location;
double O = Math.Atan2(diff.Y, diff.X);
// Found the zeebot's facing, updating it and the new location.
zeebot.Location = current;
zeebot.Facing = O;
zeebot.Initialized = true;
}
MovingPoint CalculatedOXSimetry(Zeebot z)
{
MovingPoint toCopy = zeebotSystem.CoordinateSystem.Zeebots[z];
return new MovingPoint(toCopy.X, -toCopy.Y, - toCopy.Facing);
}
MovingPoint CalculatedNoSimetry(Zeebot z)
{
return new MovingPoint(zeebotSystem.CoordinateSystem.Zeebots[z]);
}
MovingPoint Real(Zeebot z)
{
return zeebotSystem.MapContext.Zeebots[z];
}
MovingPoint CalculatedWithSimetry(Zeebot z)
{
MovingPoint toCopy = coordinateSystem.Zeebots[z];
return new MovingPoint(toCopy.X, -toCopy.Y, 2 * Math.PI - toCopy.Facing);
}
public double DistanceTo(Zeebot zeebot)
{
return MapContext.QueryZeebotDistance(this, zeebot);
}
void ComputeFacingDirection(Zeebot zeebot)
{
double success = 0, moved = 0;
double oldFacing = zeebot.Facing;
Point oldLocation = new Point();
// Turn the zeebot around, because he's probably stuck.
zeebot.ChangeFacing(Math.PI);
moved = Math.PI;
for (int tries = 0; tries < 20 && success < Util.Epsilon; tries++)
{
zeebot.ChangeFacing(Math.PI / 7);
moved += Math.PI / 7;
oldLocation = ComputeLocation(zeebot);
zeebot.TryMove(1.0, out success);
}
if (success < Util.Epsilon)
{
// Zeebot is "boxed-in"
throw new Exception("Not implemented: boxed-in zeebot");
}
Point current = ComputeLocation(zeebot);
Point diff = current - oldLocation;
double O = Math.Atan2(diff.Y, diff.X);
// Rewrite the correct Location & Facing for zeebot.
zeebot.Location = current;
zeebot.Facing = O;
this.Sign = (Util.IsEqual(Util.ModPI(oldFacing + moved), O)) ? +1 : -1;
}
