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