private IEnumerable <Vector2> snapPoints(IEnumerable <Vector2> points, PlaneCoordinates planeCoordinates, IEnumerable <Vector3> previousPoints, float snapDistance) { var snapPoints2D = previousPoints.Where(p => Mathf.Abs(planeCoordinates.Plane.GetDistanceToPoint(p)) < snapDistance).Select(p => planeCoordinates.ToPlane(p)).ToList(); var otherPlanes = new List <Ray2D>(); foreach (var plane in this.PointCloud.Planes.Take(8)) { if (plane.Equals(planeCoordinates.Plane)) { continue; } var intersect = Math3d.PlanePlaneIntersection(plane, planeCoordinates.Plane); if (intersect.HasValue) { var p1 = planeCoordinates.ToPlane(intersect.Value.origin); var p2 = planeCoordinates.ToPlane(intersect.Value.origin + intersect.Value.direction); otherPlanes.Add(new Ray2D(p1, p2 - p1)); } } foreach (var ray1 in otherPlanes) { foreach (var ray2 in otherPlanes) { if (ray1.Equals(ray2)) { continue; } snapPoints2D.Add(Math2d.LineLineIntersection(ray1, ray2)); } } foreach (var point in points) { var snapToPoint = snapPoints2D .Where(p => (point - p).magnitude < snapDistance) .Select(p => new Tuple <Vector2, float>(p, (point - p).magnitude)) .OrderBy(tuple => tuple.Value2) .FirstOrDefault(); if (snapToPoint != null) { yield return(snapToPoint.Value1); } else { var snapToPlane = otherPlanes .Select(ray => Math2d.ProjectTo2DRay(point, ray)) .Select(p => new Tuple <Vector2, float>(p, (point - p).magnitude)) .Where(tuple => tuple.Value2 < snapDistance) .OrderBy(tuple => tuple.Value2) .FirstOrDefault(); if (snapToPlane != null) { yield return(snapToPlane.Value1); } else { yield return(point); } } } }