public void System_SmallTest()
{
Edge edge = new Edge(0.91, -5.50, 3.51, -2.55);
Point h1 = edge.IntersectWithPerpendicular(new Point(3.43, -2.64));
// System_Test(5, 50);
List<Edge> edges = zeebotSystem.CoordinateSystem.PartialMap.Edges;
List<double> angles = new List<double>();
foreach (Edge e in edges)
angles.Add(e.Angle);
angles.Sort();
}
bool MergeEdge(Edge edge)
{
// Tries to merge edge into another, if they are parralel and close.
foreach (Edge e in edges)
if (Util.IsEqual(e.Angle, edge.Angle, EpsilonAngle) && edge.P1.DistanceTo(e) < Epsilon)
{
Point h1 = edge.IntersectWithPerpendicular(e.P1);
Point h2 = edge.IntersectWithPerpendicular(e.P2);
// If edge and (h1, h2) do not intersect and are too far away, ignore e.
if (!edge.Contains(h1) && !edge.Contains(h2))
{
if (Epsilon <= Math.Min(
Math.Min(e.P1.DistanceTo(edge.P1), e.P1.DistanceTo(edge.P2)),
Math.Min(e.P2.DistanceTo(edge.P1), e.P2.DistanceTo(edge.P2)))
)
continue;
}
Point boxL1, boxL2, boxR1, boxR2;
boxL1 = Point.Min(edge.P1, edge.P2, h1, h2);
boxR1 = Point.Max(edge.P1, edge.P2, h1, h2);
Point g1, g2;
g1 = e.IntersectWithPerpendicular(edge.P1);
g2 = e.IntersectWithPerpendicular(edge.P2);
boxL2 = Point.Min(e.P1, e.P2, g1, g2);
boxR2 = Point.Max(e.P1, e.P2, g1, g2);
bool wrong = false;
if (boxL1.DistanceTo(boxL2) > 0.5) wrong = true;
if (boxR1.DistanceTo(boxR2) > 0.5) wrong = true;
if (wrong)
{
throw new Exception("muie");
}
// Merging:
double W = edge.Weight + e.Weight;
double w1 = edge.Weight / W;
double w2 = e.Weight / W;
Point old1 = e.P1;
Point old2 = e.P2;
e.P1 = (boxL1 * w1) + (boxL2 * w2);
e.P2 = (boxR1 * w1) + (boxR2 * w2);
// if (Math.Max(old1.DistanceTo(e.P1), old2.DistanceTo(e.P2)) > 0.5)
// throw new Exception(string.Format("Muie {0}!", e));
e.Weight = W;
int max = 14;
if (e.P1.X > max || e.P2.X > max || e.P2.Y > max || e.P1.Y > max)
throw new Exception(string.Format("Muie {0}!", e));
return true;
}
return false;
}
