CtrEditor/Simulacion/OverlapedArea.cs

using nkast.Aether.Physics2D.Collision.Shapes;
using nkast.Aether.Physics2D.Common;
using nkast.Aether.Physics2D.Dynamics;
using System.Diagnostics;
using CtrEditor.Simulacion;

namespace CtrEditor.Simulacion
{

    class OverlapedArea
    {
        public static float CalculateOverlapedArea(Body bodyA, Body bodyB)
        {
            List<Vector2> aVertices = GetRotatedVertices(bodyA);
            List<Vector2> bVertices = GetRotatedVertices(bodyB);

            List<Vector2> intersectionPolygon = SutherlandHodgmanClip(aVertices, bVertices);

            //Debug.WriteLine("");
            //Debug.WriteLine("");
            //Debug.WriteLine("subjectPolygon :");
            //foreach (var vertex in aVertices)
            //{
            //    Debug.WriteLine(vertex);
            //}
            //Debug.WriteLine("clipPolygon :");
            //foreach (var vertex in bVertices)
            //{
            //    Debug.WriteLine(vertex);
            //}

            //Debug.WriteLine("intersectionPolygon:");
            //foreach (var vertex in intersectionPolygon)
            //{
            //    Debug.WriteLine(vertex);
            //}
            return ComputePolygonArea(intersectionPolygon);
        }

        public static float ComputePolygonArea(List<Vector2> polygon)
        {
            float area = 0;
            int count = polygon.Count;
            for (int i = 0; i < count; i++)
            {
                Vector2 current = polygon[i];
                Vector2 next = polygon[(i + 1) % count];
                area += current.X * next.Y - next.X * current.Y;
            }
            return Math.Abs(area) / 2.0f;
        }

        public static List<Vector2> SutherlandHodgmanClip(List<Vector2> subjectPolygon, List<Vector2> clipPolygon)
        {
            List<Vector2> outputList = new List<Vector2>(subjectPolygon);

            for (int i = 0; i < clipPolygon.Count; i++)
            {
                Vector2 clipEdgeStart = clipPolygon[i];
                Vector2 clipEdgeEnd = clipPolygon[(i + 1) % clipPolygon.Count];
                List<Vector2> inputList = outputList;
                outputList = new List<Vector2>();

                for (int j = 0; j < inputList.Count; j++)
                {
                    Vector2 currentVertex = inputList[j];
                    Vector2 previousVertex = inputList[(j + inputList.Count - 1) % inputList.Count];

                    if (IsInside(clipEdgeStart, clipEdgeEnd, currentVertex))
                    {
                        if (!IsInside(clipEdgeStart, clipEdgeEnd, previousVertex))
                        {
                            outputList.Add(ComputeIntersection(clipEdgeStart, clipEdgeEnd, previousVertex, currentVertex));
                        }
                        outputList.Add(currentVertex);
                    }
                    else if (IsInside(clipEdgeStart, clipEdgeEnd, previousVertex))
                    {
                        outputList.Add(ComputeIntersection(clipEdgeStart, clipEdgeEnd, previousVertex, currentVertex));
                    }
                }
            }

            return outputList;
        }

        private static bool IsInside(Vector2 edgeStart, Vector2 edgeEnd, Vector2 point)
        {
            return (edgeEnd.X - edgeStart.X) * (point.Y - edgeStart.Y) > (edgeEnd.Y - edgeStart.Y) * (point.X - edgeStart.X);
        }

        private static Vector2 ComputeIntersection(Vector2 edgeStart, Vector2 edgeEnd, Vector2 point1, Vector2 point2)
        {
            Vector2 edge = edgeEnd - edgeStart;
            Vector2 segment = point2 - point1;
            float edgeCrossSegment = Cross(edge, segment);

            if (Math.Abs(edgeCrossSegment) < float.Epsilon)
            {
                return point1; // Return any point on the segment since they are nearly parallel.
            }

            float t = Cross(point1 - edgeStart, edge) / edgeCrossSegment;
            return point1 + t * segment;
        }

        public static float Cross(Vector2 v1, Vector2 v2)
        {
            return v1.X * v2.Y - v1.Y * v2.X;
        }

        public static List<Vector2> GetRotatedRectangleVertices(Vector2 center, float width, float height, float rotation)
        {
            float halfWidth = width / 2.0f;
            float halfHeight = height / 2.0f;
            float cos = (float)Math.Cos(rotation);
            float sin = (float)Math.Sin(rotation);

            return new List<Vector2>
            {
                new Vector2(center.X + cos * (-halfWidth) - sin * (-halfHeight), center.Y + sin * (-halfWidth) + cos * (-halfHeight)),
                new Vector2(center.X + cos * (halfWidth) - sin * (-halfHeight), center.Y + sin * (halfWidth) + cos * (-halfHeight)),
                new Vector2(center.X + cos * (halfWidth) - sin * (halfHeight), center.Y + sin * (halfWidth) + cos * (halfHeight)),
                new Vector2(center.X + cos * (-halfWidth) - sin * (halfHeight), center.Y + sin * (-halfWidth) + cos * (halfHeight))
            };
        }

        public static List<Vector2> GetRotatedVertices(Body body)
        {
            List<Vector2> vertices = new List<Vector2>();

            // Verificar el tipo de Shape del Body
            foreach (var fixture in body.FixtureList)
            {
                if (fixture.Shape is PolygonShape polygonShape)
                {
                    // Es un rectángulo o un polígono
                    foreach (var vertex in polygonShape.Vertices)
                    {
                        vertices.Add(RotatePoint(vertex, body.Position, body.Rotation));
                    }
                }
                else if (fixture.Shape is CircleShape circleShape)
                {
                    // Es un círculo
                    float radius = circleShape.Radius;
                    float halfSide = radius; // El lado del cuadrado inscrito es igual al radio

                    Vector2[] squareVertices = new Vector2[]
                    {
                new Vector2(-halfSide, -halfSide),
                new Vector2(halfSide, -halfSide),
                new Vector2(halfSide, halfSide),
                new Vector2(-halfSide, halfSide)
                    };

                    foreach (var vertex in squareVertices)
                    {
                        vertices.Add(vertex + body.Position); // Trasladar el cuadrado a la posición del cuerpo
                    }
                }
            }

            return vertices;
        }


        private static Vector2 RotatePoint(Vector2 point, Vector2 origin, float rotation)
        {
            float cos = (float)Math.Cos(rotation);
            float sin = (float)Math.Sin(rotation);
            float x = point.X * cos - point.Y * sin;
            float y = point.X * sin + point.Y * cos;
            return new Vector2(x + origin.X, y + origin.Y);
        }
    }


    public class OverlapedAreaFast
    {

        public static float CalculateOverlapedArea(simBotella botella, simTransporte conveyor)
        {
            var porcentajeSuperpuesto = (float) (1.1 * CalculateOverlapPercentage(botella.Body.Position, botella.Radius, conveyor.Body.Position, conveyor.Width, conveyor.Height, conveyor.Body.Rotation));
            return porcentajeSuperpuesto > 1 ? 1 : porcentajeSuperpuesto;  //  1.1 Porque las botellas apollan sobre un circulo inscripto 10 % menor.
        }

        public static double[] RotatePoint(double px, double py, double ox, double oy, double angle)
        {
            double cosAngle = Math.Cos(angle);
            double sinAngle = Math.Sin(angle);

            double qx = ox + cosAngle * (px - ox) - sinAngle * (py - oy);
            double qy = oy + sinAngle * (px - ox) + cosAngle * (py - oy);

            return new double[] { qx, qy };
        }

        public static double CalculateOverlapPercentage(
            Vector2 circleCenter, double circleRadius, Vector2 rectCenter, double rectWidth, double rectHeight, double rectAngle)
        {
            // Transform the center of the circle to the coordinate system of the rotated rectangle
            double[] newCircleCenter = RotatePoint(circleCenter.X, circleCenter.Y, rectCenter.X, rectCenter.Y, -rectAngle);

            // Create a square with the same rotation as the rectangle
            double squareSide = 2 * circleRadius;
            double[] squareCenter = newCircleCenter;

            // Coordinates of the square (non-rotated)
            double x3 = squareCenter[0] - circleRadius;
            double y3 = squareCenter[1] - circleRadius;
            double x4 = squareCenter[0] + circleRadius;
            double y4 = squareCenter[1] + circleRadius;

            // Coordinates of the rectangle (non-rotated)
            double x1 = rectCenter.X - rectWidth / 2;
            double y1 = rectCenter.Y - rectHeight / 2;
            double x2 = rectCenter.X + rectWidth / 2;
            double y2 = rectCenter.Y + rectHeight / 2;

            // Limits of the intersection
            double xLeft = Math.Max(x1, x3);
            double xRight = Math.Min(x2, x4);
            double yBottom = Math.Max(y1, y3);
            double yTop = Math.Min(y2, y4);

            // Width and height of the intersection
            double intersectWidth = Math.Max(0, xRight - xLeft);
            double intersectHeight = Math.Max(0, yTop - yBottom);

            // Area of the intersection
            double intersectionArea = intersectWidth * intersectHeight;

            // Area of the square
            double squareArea = squareSide * squareSide;

            // Overlap percentage relative to the total area of the square
            double squareOverlapPercentage = (squareArea > 0) ? (intersectionArea / squareArea) : 0; 

            return squareOverlapPercentage;
        }
    }

}