245 lines
9.6 KiB
C#
245 lines
9.6 KiB
C#
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;
|
|
}
|
|
}
|
|
|
|
}
|