jube-fork/Accord.Math/AForge.Math/Geometry/PointsCloud.cs

// AForge Math Library
// AForge.NET framework
// http://www.aforgenet.com/framework/
//
// Copyright © AForge.NET, 2007-2011
// [email protected]
//

namespace Accord.Math.Geometry
{
    using System;
    using System.Collections.Generic;
    using AForge;

    /// <summary>
    /// Set of tools for processing collection of points in 2D space.
    /// </summary>
    /// 
    /// <remarks><para>The static class contains set of routines, which provide different
    /// operations with collection of points in 2D space. For example, finding the
    /// furthest point from a specified point or line.</para>
    /// 
    /// <para>Sample usage:</para>
    /// <code>
    /// // create points' list
    /// List&lt;IntPoint&gt; points = new List&lt;IntPoint&gt;( );
    /// points.Add( new IntPoint( 10, 10 ) );
    /// points.Add( new IntPoint( 20, 15 ) );
    /// points.Add( new IntPoint( 15, 30 ) );
    /// points.Add( new IntPoint( 40, 12 ) );
    /// points.Add( new IntPoint( 30, 20 ) );
    /// // get furthest point from the specified point
    /// IntPoint p1 = PointsCloud.GetFurthestPoint( points, new IntPoint( 15, 15 ) );
    /// Console.WriteLine( p1.X + ", " + p1.Y );
    /// // get furthest point from line
    /// IntPoint p2 = PointsCloud.GetFurthestPointFromLine( points,
    ///     new IntPoint( 50, 0 ), new IntPoint( 0, 50 ) );
    /// Console.WriteLine( p2.X + ", " + p2.Y );
    /// </code>
    /// </remarks>
    /// 
    public static class PointsCloud
    {
        /// <summary>
        /// Shift cloud by adding specified value to all points in the collection.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to shift their coordinates.</param>
        /// <param name="shift">Point to shift by.</param>
        /// 
        public static void Shift(this IList<IntPoint> cloud, IntPoint shift)
        {
            for (int i = 0, n = cloud.Count; i < n; i++)
            {
                cloud[i] = cloud[i] + shift;
            }
        }

        /// <summary>
        /// Get bounding rectangle of the specified list of points.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to get bounding rectangle for.</param>
        /// <param name="minXY">Point comprised of smallest X and Y coordinates.</param>
        /// <param name="maxXY">Point comprised of biggest X and Y coordinates.</param>
        /// 
        public static void GetBoundingRectangle(this IEnumerable<IntPoint> cloud, out IntPoint minXY, out IntPoint maxXY)
        {
            int minX = int.MaxValue;
            int maxX = int.MinValue;
            int minY = int.MaxValue;
            int maxY = int.MinValue;

            foreach (IntPoint pt in cloud)
            {
                int x = pt.X;
                int y = pt.Y;

                // check X coordinate
                if (x < minX)
                    minX = x;
                if (x > maxX)
                    maxX = x;

                // check Y coordinate
                if (y < minY)
                    minY = y;
                if (y > maxY)
                    maxY = y;
            }

            if (minX > maxX) // if no point appeared to set either minX or maxX
                throw new ArgumentException("List of points can not be empty.");

            minXY = new IntPoint(minX, minY);
            maxXY = new IntPoint(maxX, maxY);
        }

        /// <summary>
        /// Get center of gravity for the specified list of points.
        /// </summary>
        /// 
        /// <param name="cloud">List of points to calculate center of gravity for.</param>
        /// 
        /// <returns>Returns center of gravity (mean X-Y values) for the specified list of points.</returns>
        /// 
        public static Point GetCenterOfGravity(this IEnumerable<IntPoint> cloud)
        {
            int numberOfPoints = 0;
            float xSum = 0, ySum = 0;

            foreach (IntPoint pt in cloud)
            {
                xSum += pt.X;
                ySum += pt.Y;
                numberOfPoints++;
            }

            xSum /= numberOfPoints;
            ySum /= numberOfPoints;

            return new Point(xSum, ySum);
        }

        /// <summary>
        /// Find furthest point from the specified point.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to search furthest point in.</param>
        /// <param name="referencePoint">The point to search furthest point from.</param>
        /// 
        /// <returns>Returns a point, which is the furthest away from the <paramref name="referencePoint"/>.</returns>
        /// 
        public static IntPoint GetFurthestPoint(this IEnumerable<IntPoint> cloud, IntPoint referencePoint)
        {
            IntPoint furthestPoint = referencePoint;
            float maxDistance = -1;

            int rx = referencePoint.X;
            int ry = referencePoint.Y;

            foreach (IntPoint point in cloud)
            {
                int dx = rx - point.X;
                int dy = ry - point.Y;
                // we are not calculating square root for finding "real" distance,
                // since it is really not important for finding furthest point
                float distance = dx * dx + dy * dy;

                if (distance > maxDistance)
                {
                    maxDistance = distance;
                    furthestPoint = point;
                }
            }

            return furthestPoint;
        }

        /// <summary>
        /// Find two furthest points from the specified line.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to search furthest points in.</param>
        /// <param name="linePoint1">First point forming the line.</param>
        /// <param name="linePoint2">Second point forming the line.</param>
        /// <param name="furthestPoint1">First found furthest point.</param>
        /// <param name="furthestPoint2">Second found furthest point (which is on the
        /// opposite side from the line compared to the <paramref name="furthestPoint1"/>);</param>
        /// 
        /// <remarks><para>The method finds two furthest points from the specified line,
        /// where one point is on one side from the line and the second point is on
        /// another side from the line.</para></remarks>
        /// 
        public static void GetFurthestPointsFromLine(this IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2,
            out IntPoint furthestPoint1, out IntPoint furthestPoint2)
        {
            float d1, d2;

            GetFurthestPointsFromLine(cloud, linePoint1, linePoint2,
                out furthestPoint1, out d1, out furthestPoint2, out d2);
        }

        /// <summary>
        /// Find two furthest points from the specified line.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to search furthest points in.</param>
        /// <param name="linePoint1">First point forming the line.</param>
        /// <param name="linePoint2">Second point forming the line.</param>
        /// <param name="furthestPoint1">First found furthest point.</param>
        /// <param name="distance1">Distance between the first found point and the given line.</param>
        /// <param name="furthestPoint2">Second found furthest point (which is on the
        /// opposite side from the line compared to the <paramref name="furthestPoint1"/>);</param>
        /// <param name="distance2">Distance between the second found point and the given line.</param>
        /// 
        /// <remarks><para>The method finds two furthest points from the specified line,
        /// where one point is on one side from the line and the second point is on
        /// another side from the line.</para></remarks>
        ///
        public static void GetFurthestPointsFromLine(this IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2,
            out IntPoint furthestPoint1, out float distance1, out IntPoint furthestPoint2, out float distance2)
        {
            furthestPoint1 = linePoint1;
            distance1 = 0;

            furthestPoint2 = linePoint2;
            distance2 = 0;

            if (linePoint2.X != linePoint1.X)
            {
                // line's equation y(x) = k * x + b
                float k = (float)(linePoint2.Y - linePoint1.Y) / (linePoint2.X - linePoint1.X);
                float b = linePoint1.Y - k * linePoint1.X;

                float div = (float)Math.Sqrt(k * k + 1);
                float distance = 0;

                foreach (IntPoint point in cloud)
                {
                    distance = (k * point.X + b - point.Y) / div;

                    if (distance > distance1)
                    {
                        distance1 = distance;
                        furthestPoint1 = point;
                    }
                    if (distance < distance2)
                    {
                        distance2 = distance;
                        furthestPoint2 = point;
                    }
                }
            }
            else
            {
                int lineX = linePoint1.X;
                float distance = 0;

                foreach (IntPoint point in cloud)
                {
                    distance = lineX - point.X;

                    if (distance > distance1)
                    {
                        distance1 = distance;
                        furthestPoint1 = point;
                    }
                    if (distance < distance2)
                    {
                        distance2 = distance;
                        furthestPoint2 = point;
                    }
                }
            }

            distance2 = -distance2;
        }

        /// <summary>
        /// Find the furthest point from the specified line.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to search furthest point in.</param>
        /// <param name="linePoint1">First point forming the line.</param>
        /// <param name="linePoint2">Second point forming the line.</param>
        /// 
        /// <returns>Returns a point, which is the furthest away from the
        /// specified line.</returns>
        /// 
        /// <remarks><para>The method finds the furthest point from the specified line.
        /// Unlike the <see cref="GetFurthestPointsFromLine( IEnumerable{IntPoint}, IntPoint, IntPoint, out IntPoint, out IntPoint )"/>
        /// method, this method find only one point, which is the furthest away from the line
        /// regardless of side from the line.</para></remarks>
        ///
        public static IntPoint GetFurthestPointFromLine(this IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2)
        {
            float d;

            return GetFurthestPointFromLine(cloud, linePoint1, linePoint2, out d);
        }

        /// <summary>
        /// Find the furthest point from the specified line.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to search furthest points in.</param>
        /// <param name="linePoint1">First point forming the line.</param>
        /// <param name="linePoint2">Second point forming the line.</param>
        /// <param name="distance">Distance between the furthest found point and the given line.</param>
        /// 
        /// <returns>Returns a point, which is the furthest away from the
        /// specified line.</returns>
        /// 
        /// <remarks><para>The method finds the furthest point from the specified line.
        /// Unlike the <see cref="GetFurthestPointsFromLine( IEnumerable{IntPoint}, IntPoint, IntPoint, out IntPoint, out float, out IntPoint, out float )"/>
        /// method, this method find only one point, which is the furthest away from the line
        /// regardless of side from the line.</para></remarks>
        ///
        public static IntPoint GetFurthestPointFromLine(this IEnumerable<IntPoint> cloud, IntPoint linePoint1, IntPoint linePoint2, out float distance)
        {
            IntPoint furthestPoint = linePoint1;
            distance = 0;

            if (linePoint2.X != linePoint1.X)
            {
                // line's equation y(x) = k * x + b
                float k = (float)(linePoint2.Y - linePoint1.Y) / (linePoint2.X - linePoint1.X);
                float b = linePoint1.Y - k * linePoint1.X;

                float div = (float)Math.Sqrt(k * k + 1);
                float pointDistance = 0;

                foreach (IntPoint point in cloud)
                {
                    pointDistance = Math.Abs((k * point.X + b - point.Y) / div);

                    if (pointDistance > distance)
                    {
                        distance = pointDistance;
                        furthestPoint = point;
                    }
                }
            }
            else
            {
                int lineX = linePoint1.X;
                float pointDistance = 0;

                foreach (IntPoint point in cloud)
                {
                    distance = Math.Abs(lineX - point.X);

                    if (pointDistance > distance)
                    {
                        distance = pointDistance;
                        furthestPoint = point;
                    }
                }
            }

            return furthestPoint;
        }

        /// <summary>
        /// Relative distortion limit allowed for quadrilaterals, [0.0, 0.25].
        /// </summary>
        /// 
        /// <remarks><para>The value of this property is used to calculate distortion limit used by
        /// <see cref="FindQuadrilateralCorners"/>, when processing potential corners and making decision
        /// if the provided points form a quadrilateral or a triangle. The distortion limit is
        /// calculated as:
        /// <code lang="none">
        /// distrtionLimit = RelativeDistortionLimit * ( W * H ) / 2,
        /// </code>
        /// where <b>W</b> and <b>H</b> are width and height of the "points cloud" passed to the
        /// <see cref="FindQuadrilateralCorners"/>.
        /// </para>
        /// 
        /// <para>To explain the idea behind distortion limit, let’s suppose that quadrilateral finder routine found
        /// the next candidates for corners:<br />
        /// <img src="..\images\math\potential_corners.png" width="151" height="128" /><br />
        /// As we can see on the above picture, the shape there potentially can be a triangle, but not quadrilateral
        /// (suppose that points list comes from a hand drawn picture or acquired from camera, so some
        /// inaccuracy may exist). It may happen that the <b>D</b> point is just a distortion (noise, etc).
        /// So the <see cref="FindQuadrilateralCorners"/> check what is the distance between a potential corner
        /// (D in this case) and a line connecting two adjacent points (AB in this case). If the distance is smaller
        /// then the distortion limit, then the point may be rejected, so the shape turns into triangle.
        /// </para>
        /// 
        /// <para>An exception is the case when both <b>C</b> and <b>D</b> points are very close to the <b>AB</b> line,
        /// so both their distances are less than distortion limit. In this case both points will be accepted as corners -
        /// the shape is just a flat quadrilateral.</para>
        /// 
        /// <para>Default value is set to <b>0.1</b>.</para>
        /// </remarks>
        /// 
        public static float QuadrilateralRelativeDistortionLimit
        {
            get { return quadrilateralRelativeDistortionLimit; }
            set { quadrilateralRelativeDistortionLimit = Math.Max(0.0f, Math.Min(0.25f, value)); }
        }
        private static float quadrilateralRelativeDistortionLimit = 0.1f;

        /// <summary>
        /// Find corners of quadrilateral or triangular area, which contains the specified collection of points.
        /// </summary>
        /// 
        /// <param name="cloud">Collection of points to search quadrilateral for.</param>
        /// 
        /// <returns>Returns a list of 3 or 4 points, which are corners of the quadrilateral or
        /// triangular area filled by specified collection of point. The first point in the list
        /// is the point with lowest X coordinate (and with lowest Y if there are several points
        /// with the same X value). The corners are provided in counter clockwise order
        /// (<a href="http://en.wikipedia.org/wiki/Cartesian_coordinate_system">Cartesian
        /// coordinate system</a>).</returns>
        /// 
        /// <remarks><para>The method makes an assumption that the specified collection of points
        /// form some sort of quadrilateral/triangular area. With this assumption it tries to find corners
        /// of the area.</para>
        /// 
        /// <para><note>The method does not search for <b>bounding</b> quadrilateral/triangular area,
        /// where all specified points are <b>inside</b> of the found quadrilateral/triangle. Some of the
        /// specified points potentially may be outside of the found quadrilateral/triangle, since the
        /// method takes corners only from the specified collection of points, but does not calculate such
        /// to form true bounding quadrilateral/triangle.</note></para>
        /// 
        /// <para>See <see cref="QuadrilateralRelativeDistortionLimit"/> property for additional information.</para>
        /// </remarks>
        /// 
        public static List<IntPoint> FindQuadrilateralCorners(this IEnumerable<IntPoint> cloud)
        {
            // quadrilateral's corners
            List<IntPoint> corners = new List<IntPoint>();

            // get bounding rectangle of the points list
            IntPoint minXY, maxXY;
            PointsCloud.GetBoundingRectangle(cloud, out minXY, out maxXY);
            // get cloud's size
            IntPoint cloudSize = maxXY - minXY;
            // calculate center point
            IntPoint center = minXY + cloudSize / 2;
            // acceptable deviation limit
            float distortionLimit = quadrilateralRelativeDistortionLimit * (cloudSize.X + cloudSize.Y) / 2;

            // get the furthest point from (0,0)
            IntPoint point1 = PointsCloud.GetFurthestPoint(cloud, center);
            // get the furthest point from the first point
            IntPoint point2 = PointsCloud.GetFurthestPoint(cloud, point1);

            corners.Add(point1);
            corners.Add(point2);

            // get two furthest points from line
            IntPoint point3, point4;
            float distance3, distance4;

            PointsCloud.GetFurthestPointsFromLine(cloud, point1, point2,
                out point3, out distance3, out point4, out distance4);

            // ideally points 1 and 2 form a diagonal of the
            // quadrilateral area, and points 3 and 4 form another diagonal

            // but if one of the points (3 or 4) is very close to the line
            // connecting points 1 and 2, then it is one the same line ...
            // which means corner was not found.
            // in this case we deal with a trapezoid or triangle, where
            // (1-2) line is one of it sides.

            // another interesting case is when both points (3) and (4) are
            // very close the (1-2) line. in this case we may have just a flat
            // quadrilateral.

            if (
                 ((distance3 >= distortionLimit) && (distance4 >= distortionLimit)) ||

                 ((distance3 < distortionLimit) && (distance3 != 0) &&
                   (distance4 < distortionLimit) && (distance4 != 0)))
            {
                // don't add one of the corners, if the point is already in the corners list
                // (this may happen when both #3 and #4 points are very close to the line
                // connecting #1 and #2)
                if (!corners.Contains(point3))
                {
                    corners.Add(point3);
                }
                if (!corners.Contains(point4))
                {
                    corners.Add(point4);
                }
            }
            else
            {
                // it seems that we deal with kind of trapezoid,
                // where point 1 and 2 are on the same edge

                IntPoint tempPoint = (distance3 > distance4) ? point3 : point4;

                // try to find 3rd point
                PointsCloud.GetFurthestPointsFromLine(cloud, point1, tempPoint,
                    out point3, out distance3, out point4, out distance4);

                bool thirdPointIsFound = false;

                if ((distance3 >= distortionLimit) && (distance4 >= distortionLimit))
                {
                    if (point4.DistanceTo(point2) > point3.DistanceTo(point2))
                        point3 = point4;

                    thirdPointIsFound = true;
                }
                else
                {
                    PointsCloud.GetFurthestPointsFromLine(cloud, point2, tempPoint,
                        out point3, out distance3, out point4, out distance4);

                    if ((distance3 >= distortionLimit) && (distance4 >= distortionLimit))
                    {
                        if (point4.DistanceTo(point1) > point3.DistanceTo(point1))
                            point3 = point4;

                        thirdPointIsFound = true;
                    }
                }

                if (!thirdPointIsFound)
                {
                    // failed to find 3rd edge point, which is away enough from the temp point.
                    // this means that the clound looks more like triangle
                    corners.Add(tempPoint);
                }
                else
                {
                    corners.Add(point3);

                    // try to find 4th point
                    float tempDistance;

                    PointsCloud.GetFurthestPointsFromLine(cloud, point1, point3,
                        out tempPoint, out tempDistance, out point4, out distance4);

                    if ((distance4 >= distortionLimit) && (tempDistance >= distortionLimit))
                    {
                        if (tempPoint.DistanceTo(point2) > point4.DistanceTo(point2))
                            point4 = tempPoint;
                    }
                    else
                    {
                        PointsCloud.GetFurthestPointsFromLine(cloud, point2, point3,
                            out tempPoint, out tempDistance, out point4, out distance4);

                        if ((tempPoint.DistanceTo(point1) > point4.DistanceTo(point1)) &&
                             (tempPoint != point2) && (tempPoint != point3))
                        {
                            point4 = tempPoint;
                        }
                    }

                    if ((point4 != point1) && (point4 != point2) && (point4 != point3))
                        corners.Add(point4);
                }
            }

            // put the point with lowest X as the first
            for (int i = 1, n = corners.Count; i < n; i++)
            {
                if ((corners[i].X < corners[0].X) ||
                     ((corners[i].X == corners[0].X) && (corners[i].Y < corners[0].Y)))
                {
                    IntPoint temp = corners[i];
                    corners[i] = corners[0];
                    corners[0] = temp;
                }
            }


            // sort other points in counter clockwise order
            float k1 = (corners[1].X != corners[0].X) ?
                ((float)(corners[1].Y - corners[0].Y) / (corners[1].X - corners[0].X)) :
                ((corners[1].Y > corners[0].Y) ? float.PositiveInfinity : float.NegativeInfinity);

            float k2 = (corners[2].X != corners[0].X) ?
                ((float)(corners[2].Y - corners[0].Y) / (corners[2].X - corners[0].X)) :
                ((corners[2].Y > corners[0].Y) ? float.PositiveInfinity : float.NegativeInfinity);

            if (k2 < k1)
            {
                IntPoint temp = corners[1];
                corners[1] = corners[2];
                corners[2] = temp;

                float tk = k1;
                k1 = k2;
                k2 = tk;
            }

            if (corners.Count == 4)
            {
                float k3 = (corners[3].X != corners[0].X) ?
                    ((float)(corners[3].Y - corners[0].Y) / (corners[3].X - corners[0].X)) :
                    ((corners[3].Y > corners[0].Y) ? float.PositiveInfinity : float.NegativeInfinity);

                if (k3 < k1)
                {
                    IntPoint temp = corners[1];
                    corners[1] = corners[3];
                    corners[3] = temp;

                    float tk = k1;
                    k1 = k3;
                    k3 = tk;
                }
                if (k3 < k2)
                {
                    IntPoint temp = corners[2];
                    corners[2] = corners[3];
                    corners[3] = temp;

                    float tk = k2;
                    k2 = k3;
                    k3 = tk;
                }
            }

            return corners;
        }

        /// <summary>
        ///   Converts an ordered sequence of points into a sequence of line segments (a polygon).
        /// </summary>
        /// 
        /// <param name="points">The points.</param>
        /// 
        public static List<LineSegment> ToLineSegments(this List<IntPoint> points)
        {
            var segments = new List<LineSegment>();

            for (int i = 0; i < points.Count - 1; i++)
                segments.Add(new LineSegment(points[i], points[i + 1]));
            segments.Add(new LineSegment(points[points.Count - 1], points[0]));

            return segments;
        }
    }
}