private KeyValuePair <HalfEdge, float>?FindFirstParallelEdge(Seed seed, float length, float distance, float parallelThreshold) { Contract.Requires(seed != null); var start = seed.Origin.Position; var end = seed.Origin.Position + seed.Direction * length; var segment = new LineSegment2(start, end); //Calculate the expanded bounds to query. This is as wide as the parallel check distance var p = seed.Direction.Perpendicular() * distance / 2; var a = start + p; var b = start - p; var c = end + p; var d = end - p; var queryBounds = new BoundingRectangle( Vector2.Min(Vector2.Min(a, b), Vector2.Min(c, d)), Vector2.Max(Vector2.Max(a, b), Vector2.Max(c, d)) ); //now get all lines which intersect this bounds and check them for parallelism var candidates = _mesh.FindEdges(queryBounds); KeyValuePair <HalfEdge, float>?firstParallel = null; foreach (var candidate in candidates) { var dirCandidate = candidate.Segment.Line.Direction; var dir = segment.Line.Direction; //Dot product directions of lines to check parallelism (compare with threshold) var dot = Math.Abs(Vector2.Dot(dir, dirCandidate)); if (dot > parallelThreshold) { //Our query bounds were larger than the actual area we wanted to query (because we're limited to axis aligned bounds) //Check that this line enters the smaller OABB area //We'll do this check by checking if the line segment intersects any of the four OABB segments (AB, BC, CD, DA) if (new LineSegment2(a, b).Intersects(candidate.Segment).HasValue || new LineSegment2(b, c).Intersects(candidate.Segment).HasValue || new LineSegment2(c, d).Intersects(candidate.Segment).HasValue || new LineSegment2(d, a).Intersects(candidate.Segment).HasValue) { //check how far along this segment the parallelism starts var startDist = segment.ClosestPointDistanceAlongSegment(candidate.StartVertex.Position); var endDist = segment.ClosestPointDistanceAlongSegment(candidate.EndVertex.Position); var minDist = Math.Min(startDist, endDist); if (firstParallel == null || minDist < firstParallel.Value.Value) { firstParallel = new KeyValuePair <HalfEdge, float>(candidate, minDist); } } } } return(firstParallel); }