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