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