Ejemplo n.º 1
0
        // Check if polygon A is going to collide with polygon B for the given velocity
        public static PolygonCollisionResult PolygonCollision(Polygon polygonA, Polygon polygonB, Vector velocity)
        {
            PolygonCollisionResult result = new PolygonCollisionResult();

            result.Intersect     = true;
            result.WillIntersect = true;

            int    edgeCountA          = polygonA.Edges.Count;
            int    edgeCountB          = polygonB.Edges.Count;
            double minIntervalDistance = Double.PositiveInfinity;
            Vector translationAxis     = new Vector();
            Vector edge;

            // Loop through all the edges of both polygons
            for (int edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
            {
                if (edgeIndex < edgeCountA)
                {
                    edge = polygonA.Edges[edgeIndex];
                }
                else
                {
                    edge = polygonB.Edges[edgeIndex - edgeCountA];
                }

                // ===== 1. Find if the polygons are currently intersecting =====

                // Find the axis perpendicular to the current edge
                Vector axis = new Vector(-edge.Y, edge.X);
                axis.Normalize();

                // Find the projection of the polygon on the current axis
                double minA = 0; double minB = 0; double maxA = 0; double maxB = 0;
                ProjectPolygon(axis, polygonA, ref minA, ref maxA);
                ProjectPolygon(axis, polygonB, ref minB, ref maxB);

                // Check if the polygon projections are currentlty intersecting
                if (IntervalDistance(minA, maxA, minB, maxB) > 0)
                {
                    result.Intersect = false;
                }

                // ===== 2. Now find if the polygons *will* intersect =====

                // Project the velocity on the current axis
                double velocityProjection = axis.DotProduct(velocity);

                // Get the projection of polygon A during the movement
                if (velocityProjection < 0)
                {
                    minA += velocityProjection;
                }
                else
                {
                    maxA += velocityProjection;
                }

                // Do the same test as above for the new projection
                double intervalDistance   = IntervalDistance(minA, maxA, minB, maxB);
                if (intervalDistance > 0)
                {
                    result.WillIntersect = false;
                }

                // If the polygons are not intersecting and won't intersect, exit the loop
                if (!result.Intersect && !result.WillIntersect)
                {
                    break;
                }

                // Check if the current interval distance is the minimum one. If so store
                // the interval distance and the current distance.
                // This will be used to calculate the minimum translation vector
                intervalDistance = Math.Abs(intervalDistance);
                if (intervalDistance < minIntervalDistance)
                {
                    minIntervalDistance = intervalDistance;
                    translationAxis     = axis;

                    Vector d = polygonA.Center - polygonB.Center;
                    if (d.DotProduct(translationAxis) < 0)
                    {
                        translationAxis = -translationAxis;
                    }
                }
            }

            // The minimum translation vector can be used to push the polygons appart.
            // First moves the polygons by their velocity
            // then move polygonA by MinimumTranslationVector.
            if (result.WillIntersect)
            {
                result.MinimumTranslationVector = translationAxis * minIntervalDistance;
            }

            return(result);
        }
        // Check if polygon A is going to collide with polygon B for the given velocity
        public static PolygonCollisionResult PolygonCollision(Polygon polygonA, Polygon polygonB, Vector velocity)
        {
            PolygonCollisionResult result = new PolygonCollisionResult();
            result.Intersect = true;
            result.WillIntersect = true;

            int edgeCountA = polygonA.Edges.Count;
            int edgeCountB = polygonB.Edges.Count;
            double minIntervalDistance = Double.PositiveInfinity;
            Vector translationAxis = new Vector();
            Vector edge;

            // Loop through all the edges of both polygons
            for (int edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++)
            {
                if (edgeIndex < edgeCountA)
                {
                    edge = polygonA.Edges[edgeIndex];
                }
                else
                {
                    edge = polygonB.Edges[edgeIndex - edgeCountA];
                }

                // ===== 1. Find if the polygons are currently intersecting =====

                // Find the axis perpendicular to the current edge
                Vector axis = new Vector(-edge.Y, edge.X);
                axis.Normalize();

                // Find the projection of the polygon on the current axis
                double minA = 0; double minB = 0; double maxA = 0; double maxB = 0;
                ProjectPolygon(axis, polygonA, ref minA, ref maxA);
                ProjectPolygon(axis, polygonB, ref minB, ref maxB);

                // Check if the polygon projections are currentlty intersecting
                if (IntervalDistance(minA, maxA, minB, maxB) > 0) result.Intersect = false;

                // ===== 2. Now find if the polygons *will* intersect =====

                // Project the velocity on the current axis
                double velocityProjection = axis.DotProduct(velocity);

                // Get the projection of polygon A during the movement
                if (velocityProjection < 0)
                {
                    minA += velocityProjection;
                }
                else
                {
                    maxA += velocityProjection;
                }

                // Do the same test as above for the new projection
                double intervalDistance = IntervalDistance(minA, maxA, minB, maxB);
                if (intervalDistance > 0) result.WillIntersect = false;

                // If the polygons are not intersecting and won't intersect, exit the loop
                if (!result.Intersect && !result.WillIntersect) break;

                // Check if the current interval distance is the minimum one. If so store
                // the interval distance and the current distance.
                // This will be used to calculate the minimum translation vector
                intervalDistance = Math.Abs(intervalDistance);
                if (intervalDistance < minIntervalDistance)
                {
                    minIntervalDistance = intervalDistance;
                    translationAxis = axis;

                    Vector d = polygonA.Center - polygonB.Center;
                    if (d.DotProduct(translationAxis) < 0) translationAxis = -translationAxis;
                }
            }

            // The minimum translation vector can be used to push the polygons appart.
            // First moves the polygons by their velocity
            // then move polygonA by MinimumTranslationVector.
            if (result.WillIntersect) result.MinimumTranslationVector = translationAxis * minIntervalDistance;

            return result;
        }
 // Calculate the projection of a polygon on an axis and returns it as a [min, max] interval
 public static void ProjectPolygon(Vector axis, Polygon polygon, ref double min, ref double max)
 {
     // To project a point on an axis use the dot product
     double d = axis.DotProduct(polygon.Points[0]);
     min = d;
     max = d;
     for (int i = 0; i < polygon.Points.Count; i++)
     {
         d = polygon.Points[i].DotProduct(axis);
         if (d < min)
         {
             min = d;
         }
         else
         {
             if (d > max)
             {
                 max = d;
             }
         }
     }
 }
        /// <summary>
        /// Find rectangles in image and add possible rectangle candidates as temporary but known objects or updates
        /// existing objects from previous frames.
        /// </summary>
        /// <param name="occlusionTracking"></param>
        /// <param name="grayImage"></param>
        /// <param name="outputImage"></param>
        /// <param name="updateTime"></param>
        /// <param name="objects"></param>
        private RectangularObject[] FindRectangles(bool occlusionTracking, Image<Gray, byte> grayImage, ref Image<Rgb, byte> outputImage, DateTime updateTime, RectangularObject[] objects)
        {
            var newObjects = new List<RectangularObject>();

            var imageWidth = grayImage.Width;
            var imageHeight = grayImage.Height;
            var pixels = imageWidth * imageHeight;

            var diagonal = Math.Sqrt(Math.Pow(imageWidth, 2) + Math.Pow(imageHeight, 2));

            var maxRestoreDistance = (MaxRestoreDistance / 100.0) * diagonal;

            using (var storage = new MemStorage())
            {
                for (var contours = grayImage.FindContours(CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_SIMPLE, IsRetrieveExternal ? RETR_TYPE.CV_RETR_EXTERNAL : RETR_TYPE.CV_RETR_LIST, storage); contours != null; contours = contours.HNext)
                {
                    var lowApproxContour = contours.ApproxPoly(contours.Perimeter * 0.015, storage);

                    if (lowApproxContour.Area > ((MinContourArea / 100.0) * pixels) && lowApproxContour.Area < ((MaxContourArea / 100.0) * pixels)) //only consider contours with area greater than
                    {
                        if (IsRenderContent && IsDrawAllContours)
                            outputImage.Draw(lowApproxContour, Rgbs.BlueTorquoise, 1);
                        //outputImage.Draw(currentContour.GetConvexHull(ORIENTATION.CV_CLOCKWISE), Rgbs.BlueTorquoise, 2);

                        // Continue with next contour if current contour is not a rectangle.
                        List<DPoint> points;
                        if (!IsPlausibleRectangle(lowApproxContour, MinAngle, MaxAngle, MinDetectRightAngles, out points)) continue;

                        var highApproxContour = contours.ApproxPoly(contours.Perimeter * 0.05, storage);
                        if (IsRenderContent && IsDrawAllContours)
                            outputImage.Draw(highApproxContour, Rgbs.Yellow, 1);

                        var rectangle = highApproxContour.BoundingRectangle;
                        var minAreaRect = highApproxContour.GetMinAreaRect(storage);
                        var polygon = new Polygon(points.ToArray(), imageWidth, imageHeight);
                        var contourPoints = highApproxContour.ToArray();

                        if (!UpdateObject(occlusionTracking, maxRestoreDistance, rectangle, minAreaRect, polygon, contourPoints, updateTime, objects))
                        {
                            newObjects.Add(CreateObject(NextId(), rectangle, minAreaRect, polygon, contourPoints, updateTime));
                        }
                    }
                }
            }

            return newObjects.ToArray();
        }
        /// <summary>
        /// Updates an object if it finds an object from last frame at a max restore distance.
        /// </summary>
        /// <param name="occluded"></param>
        /// <param name="maxRestoreDistance"></param>
        /// <param name="boundingRectangle"></param>
        /// <param name="minAreaRect"></param>
        /// <param name="polygon"></param>
        /// <param name="points"></param>
        /// <param name="updateTime"></param>
        /// <param name="objects"></param>
        /// <returns></returns>
        private static bool UpdateObject(bool occluded, double maxRestoreDistance, Rectangle boundingRectangle, MCvBox2D minAreaRect, Polygon polygon, DPoint[] points, DateTime updateTime, IEnumerable<RectangularObject> objects)
        {
            var cCenter = minAreaRect.center;

            RectangularObject candidate = null;
            var leastDistance = double.MaxValue;
            foreach (var obj in objects)
            {
                var oCenter = obj.Shape.center;
                var distance = Math.Sqrt(Math.Pow(oCenter.X - cCenter.X, 2) + Math.Pow(oCenter.Y - cCenter.Y, 2));

                if (!(leastDistance > distance)) continue;

                candidate = obj;
                leastDistance = distance;
            }

            if (leastDistance > maxRestoreDistance || candidate == null)
                return false;

            candidate.State = occluded ? TrackingState.Occluded : TrackingState.Tracked;
            candidate.LastUpdate = updateTime;
            candidate.Center = new WPoint(cCenter.X, cCenter.Y);
            candidate.Bounds = boundingRectangle;
            candidate.Shape = minAreaRect;
            candidate.Polygon = polygon;
            candidate.Points = points;

            return true;
        }
 /// <summary>
 /// Adds a new temporary object that will be used to identify itself in the preceeding frames.
 /// </summary>
 /// <param name="id"></param>
 /// <param name="boundingRectangle"></param>
 /// <param name="minAreaRect"></param>
 /// <param name="polygon"></param>
 /// <param name="points"></param>
 /// <param name="updateTime"></param>
 private static RectangularObject CreateObject(long id, Rectangle boundingRectangle, MCvBox2D minAreaRect, Polygon polygon, DPoint[] points, DateTime updateTime)
 {
     return new RectangularObject
     {
         Id = id,
         State = TrackingState.Tracked,
         LastUpdate = updateTime,
         Center = new WPoint(minAreaRect.center.X, minAreaRect.center.Y),
         Bounds = boundingRectangle,
         Shape = minAreaRect,
         Polygon = polygon,
         Points = points,
     };
 }