From 09980689fb18ccd2f4764ea463cb66731299414b Mon Sep 17 00:00:00 2001 From: Miguel Date: Thu, 30 May 2024 18:48:37 +0200 Subject: [PATCH] Adaptacion con simCurve --- ObjetosSim/Dinamicos/ucBotella.xaml.cs | 11 + ObjetosSim/Dinamicos/ucBotellaCuello.xaml | 19 ++ ObjetosSim/Dinamicos/ucBotellaCuello.xaml.cs | 175 ++++++++++++ ObjetosSim/Emuladores/ucFiller.xaml.cs | 36 ++- ObjetosSim/Estaticos/ucGuia.xaml.cs | 2 +- .../Estaticos/ucTransporteCurva.xaml.cs | 2 + .../SensoresComandos/ucPhotocell.xaml.cs | 28 +- ObjetosSim/Traces/ucTrace3.xaml | 15 +- ObjetosSim/Traces/ucTrace3.xaml.cs | 139 +++++++--- ObjetosSim/osBase.cs | 81 +++++- Simulacion/Aether.cs | 262 +++++++++++++++--- Simulacion/InterseccionCirculoRectangulo.cs | 21 +- Simulacion/OverlapedArea.cs | 243 ++++++++++++++++ 13 files changed, 925 insertions(+), 109 deletions(-) create mode 100644 ObjetosSim/Dinamicos/ucBotellaCuello.xaml create mode 100644 ObjetosSim/Dinamicos/ucBotellaCuello.xaml.cs create mode 100644 Simulacion/OverlapedArea.cs diff --git a/ObjetosSim/Dinamicos/ucBotella.xaml.cs b/ObjetosSim/Dinamicos/ucBotella.xaml.cs index 73388e7..569ff99 100644 --- a/ObjetosSim/Dinamicos/ucBotella.xaml.cs +++ b/ObjetosSim/Dinamicos/ucBotella.xaml.cs @@ -48,6 +48,16 @@ namespace CtrEditor.ObjetosSim [ObservableProperty] private float inercia_desde_simulacion; + [ObservableProperty] + bool test; + + partial void OnTestChanged(bool value) + { + SimGeometria.Body.LinearVelocity = new Vector2(1,1); + } + + [ObservableProperty] + private float porcentaje_Traccion; [ObservableProperty] private float mass; @@ -119,6 +129,7 @@ namespace CtrEditor.ObjetosSim RemoverDesdeSimulacion = true; Velocidad_desde_simulacion = SimGeometria.Body.LinearVelocity.ToString(); Inercia_desde_simulacion = SimGeometria.Body.Inertia; + Porcentaje_Traccion = SimGeometria.OverlapPercentage; } public override void ucLoaded() diff --git a/ObjetosSim/Dinamicos/ucBotellaCuello.xaml b/ObjetosSim/Dinamicos/ucBotellaCuello.xaml new file mode 100644 index 0000000..3e94af4 --- /dev/null +++ b/ObjetosSim/Dinamicos/ucBotellaCuello.xaml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + diff --git a/ObjetosSim/Dinamicos/ucBotellaCuello.xaml.cs b/ObjetosSim/Dinamicos/ucBotellaCuello.xaml.cs new file mode 100644 index 0000000..df47b4d --- /dev/null +++ b/ObjetosSim/Dinamicos/ucBotellaCuello.xaml.cs @@ -0,0 +1,175 @@ +using System.Windows; +using System.Windows.Controls; +//using using Microsoft.Xna.Framework; +using CtrEditor.Convertidores; +using CtrEditor.Siemens; +using CtrEditor.Simulacion; +using CommunityToolkit.Mvvm.ComponentModel; +using nkast.Aether.Physics2D.Common; +using System.Windows.Media; + +namespace CtrEditor.ObjetosSim.Dinamicos +{ + /// + /// Interaction logic for ucBotellaCuelloCuello.xaml + /// + public partial class osBotellaCuello : osBase, IosBase + { + private simBotella SimGeometria; + + // Otros datos y métodos relevantes para la simulación + + public static string NombreClase() + { + return "Botella con Cuello"; + } + private string nombre = NombreClase(); + public override string Nombre + { + get => nombre; + set => SetProperty(ref nombre, value); + } + + [ObservableProperty] + private Brush colorButton_oculto; + + [ObservableProperty] + private float diametro; + + partial void OnDiametroChanged(float value) + { + SimGeometria?.SetDiameter(Diametro); + } + + [ObservableProperty] + private string velocidad_desde_simulacion; + [ObservableProperty] + private float inercia_desde_simulacion; + + + [ObservableProperty] + private float mass; + partial void OnMassChanged(float value) + { + SimGeometria?.SetMass(value); + } + + public Vector2 GetCentro() + { + return new Vector2(Left + Diametro / 2, Top + Diametro / 2); + } + + public void SetCentro(float x, float y) + { Left = x; Top = y; } + + public void SetCentro(Vector2 centro) + { + Left = centro.X - Diametro / 2; + Top = centro.Y - Diametro / 2; + } + + private void ActualizarGeometrias() + { + if (SimGeometria != null) + { + + SimGeometria.SetPosition(GetCentro()); + } + } + + public osBotellaCuello() + { + Diametro = 0.10f; + Mass = 1; + ColorButton_oculto = Brushes.Gray; + } + + public void UpdateAfterMove() + { + ActualizarGeometrias(); + SimGeometria?.SetDiameter(Diametro); + } + + public override void UpdateGeometryStart() + { + // Se llama antes de la simulacion + ActualizarGeometrias(); + SimGeometria?.SetDiameter(Diametro); + } + public override void UpdateGeometryStep() + { + // Se llama antes de la simulacion + ActualizarGeometrias(); + } + public override void UpdateControl(int elapsedMilliseconds) + { + SetCentro(SimGeometria.Center); + if (SimGeometria.isRestricted) + ColorButton_oculto = Brushes.Yellow; + else + { + if (SimGeometria.isOnTransports > 0) + ColorButton_oculto = Brushes.Red; + else + ColorButton_oculto = Brushes.Gray; + } + if (SimGeometria.Descartar) // Ha sido marcada para remover + RemoverDesdeSimulacion = true; + Velocidad_desde_simulacion = SimGeometria.Body.LinearVelocity.ToString(); + Inercia_desde_simulacion = SimGeometria.Body.Inertia; + } + + public override void ucLoaded() + { + // El UserControl ya se ha cargado y podemos obtener las coordenadas para + // crear el objeto de simulacion + ActualizarLeftTop(); + SimGeometria = simulationManager.AddCircle(Diametro, GetCentro(), Mass); + } + public override void ucUnLoaded() + { + // El UserControl se esta eliminando + // eliminar el objeto de simulacion + simulationManager.Remove(SimGeometria); + } + + } + + public partial class ucBotellaCuello : UserControl, IDataContainer + { + public osBase? Datos { get; set; } + + public ucBotellaCuello() + { + InitializeComponent(); + this.Loaded += OnLoaded; + this.Unloaded += OnUnloaded; + } + private void OnLoaded(object sender, RoutedEventArgs e) + { + Datos?.ucLoaded(); + } + private void OnUnloaded(object sender, RoutedEventArgs e) + { + Datos?.ucUnLoaded(); + } + + public void Resize(float width, float height) { } + public void Move(float LeftPixels, float TopPixels) + { + if (Datos != null) + { + Datos.Left = PixelToMeter.Instance.calc.PixelsToMeters(LeftPixels); + Datos.Top = PixelToMeter.Instance.calc.PixelsToMeters(TopPixels); + if (Datos is osBotellaCuello BotellaCuello) + BotellaCuello.UpdateAfterMove(); + } + } + public void Rotate(float Angle) { } + public void Highlight(bool State) { } + public int ZIndex() + { + return 10; + } + } +} diff --git a/ObjetosSim/Emuladores/ucFiller.xaml.cs b/ObjetosSim/Emuladores/ucFiller.xaml.cs index 46ab28a..5912787 100644 --- a/ObjetosSim/Emuladores/ucFiller.xaml.cs +++ b/ObjetosSim/Emuladores/ucFiller.xaml.cs @@ -3,6 +3,7 @@ using CtrEditor.Siemens; using System.Windows; using System.Windows.Controls; using CommunityToolkit.Mvvm.ComponentModel; +using System.Diagnostics; namespace CtrEditor.ObjetosSim { @@ -11,6 +12,7 @@ namespace CtrEditor.ObjetosSim /// public partial class osFiller : osBase, IosBase { + TimerTON_TOFF _TON_TOFF = new TimerTON_TOFF(); // Otros datos y métodos relevantes para la simulación private float TiempoRestante; @@ -31,8 +33,23 @@ namespace CtrEditor.ObjetosSim private float offsetLeftSalida; [ObservableProperty] private float offsetTopSalida; + + [ObservableProperty] + private string tag_consenso; [ObservableProperty] private bool consenso; + + partial void OnConsensoChanged(bool value) + { + _TON_TOFF.Senal = value; + } + [ObservableProperty] + private bool consenso_NC; + [ObservableProperty] + private bool consenso_Filtrado; + [ObservableProperty] + float filtro_consenso_s; + [ObservableProperty] private float botellas_hora; @@ -54,8 +71,6 @@ namespace CtrEditor.ObjetosSim [ObservableProperty] private float diametro_botella; [ObservableProperty] - private string tag_consenso; - [ObservableProperty] private float ancho; [ObservableProperty] private float alto; @@ -70,16 +85,29 @@ namespace CtrEditor.ObjetosSim Velocidad_actual_percentual = 0; Diametro_botella = 0.1f; Botellas_hora = 10000; + Filtro_consenso_s = 1; } public override void UpdatePLC(PLCModel plc, int elapsedMilliseconds) { - Consenso = LeerBitTag(Tag_consenso); + if (Consenso_NC) + Consenso = !LeerBitTag(Tag_consenso); + else + Consenso = LeerBitTag(Tag_consenso); } public override void UpdateControl(int elapsedMilliseconds) { - if (Consenso && Velocidad_actual_percentual > 0) + bool habilitado; + + _TON_TOFF.Tiempo_ON_s = _TON_TOFF.Tiempo_OFF_s = Filtro_consenso_s; + + if (Consenso_Filtrado) + habilitado = _TON_TOFF.SenalFiltrada(); + else + habilitado = Consenso; + + if (habilitado && Velocidad_actual_percentual > 0) { TiempoRestante -= elapsedMilliseconds / 1000.0f; if (TiempoRestante <= 0) diff --git a/ObjetosSim/Estaticos/ucGuia.xaml.cs b/ObjetosSim/Estaticos/ucGuia.xaml.cs index 021c9a4..46dc94f 100644 --- a/ObjetosSim/Estaticos/ucGuia.xaml.cs +++ b/ObjetosSim/Estaticos/ucGuia.xaml.cs @@ -117,7 +117,7 @@ namespace CtrEditor.ObjetosSim public void Highlight(bool State) { } public int ZIndex() { - return 1; + return 3; } } diff --git a/ObjetosSim/Estaticos/ucTransporteCurva.xaml.cs b/ObjetosSim/Estaticos/ucTransporteCurva.xaml.cs index 008d34a..c9fb51c 100644 --- a/ObjetosSim/Estaticos/ucTransporteCurva.xaml.cs +++ b/ObjetosSim/Estaticos/ucTransporteCurva.xaml.cs @@ -91,6 +91,8 @@ namespace CtrEditor.ObjetosSim { // Se llama antes de la simulacion ActualizarGeometrias(); + + } public override void UpdatePLC(PLCModel plc, int elapsedMilliseconds) { diff --git a/ObjetosSim/SensoresComandos/ucPhotocell.xaml.cs b/ObjetosSim/SensoresComandos/ucPhotocell.xaml.cs index a06515c..12e70a7 100644 --- a/ObjetosSim/SensoresComandos/ucPhotocell.xaml.cs +++ b/ObjetosSim/SensoresComandos/ucPhotocell.xaml.cs @@ -32,9 +32,9 @@ namespace CtrEditor.ObjetosSim } [ObservableProperty] - public Brush color; + Brush color; [ObservableProperty] - public bool luzCortada; + bool luzCortada; partial void OnLuzCortadaChanged(bool value) { @@ -65,6 +65,20 @@ namespace CtrEditor.ObjetosSim } + [ObservableProperty] + float distancia_cuello; + + [ObservableProperty] + bool detectarCuello; + + partial void OnDetectarCuelloChanged(bool value) + { + if (Simulation_Photocell == null) return; + + Simulation_Photocell.DetectNeck = value; + } + + [ObservableProperty] float filter_Frecuency; @@ -149,7 +163,11 @@ namespace CtrEditor.ObjetosSim } public override void UpdateControl(int elapsedMilliseconds) { - LuzCortada = Simulation_Photocell.LuzCortada > 0; + Distancia_cuello = Simulation_Photocell.Distancia; + if (DetectarCuello) + LuzCortada = Simulation_Photocell.LuzCortadaNeck; + else + LuzCortada = Simulation_Photocell.LuzCortada > 0; } public override void UpdatePLCPrimerCiclo() { @@ -164,10 +182,10 @@ namespace CtrEditor.ObjetosSim { // El UserControl ya se ha cargado y podemos obtener las coordenadas para // crear el objeto de simulacion - ActualizarLeftTop(); + base.ucLoaded(); if (_visualRepresentation is ucPhotocell uc) - Simulation_Photocell = AddBarrera(simulationManager, uc.Photocell, Alto, Ancho, Angulo); + Simulation_Photocell = AddBarrera(simulationManager, uc.Photocell, Alto, Ancho, Angulo, DetectarCuello); } public override void ucUnLoaded() { diff --git a/ObjetosSim/Traces/ucTrace3.xaml b/ObjetosSim/Traces/ucTrace3.xaml index 1d450a7..3d886e6 100644 --- a/ObjetosSim/Traces/ucTrace3.xaml +++ b/ObjetosSim/Traces/ucTrace3.xaml @@ -18,7 +18,8 @@ - + @@ -28,14 +29,14 @@ - + - + - - - - + + + + diff --git a/ObjetosSim/Traces/ucTrace3.xaml.cs b/ObjetosSim/Traces/ucTrace3.xaml.cs index 9366711..2de1bb7 100644 --- a/ObjetosSim/Traces/ucTrace3.xaml.cs +++ b/ObjetosSim/Traces/ucTrace3.xaml.cs @@ -43,11 +43,17 @@ namespace CtrEditor.ObjetosSim.Traces [ObservableProperty] float ancho; + [ObservableProperty] + bool serie1_Tipo_Bool; [ObservableProperty] string tag_Serie1; [ObservableProperty] + bool serie2_Tipo_Bool; + [ObservableProperty] string tag_Serie2; [ObservableProperty] + bool serie3_Tipo_Bool; + [ObservableProperty] string tag_Serie3; [ObservableProperty] @@ -96,6 +102,45 @@ namespace CtrEditor.ObjetosSim.Traces Max_Serie_1 = 2; Max_Serie_2 = 2; Max_Serie_3 = 2; + Serie1_Tipo_Bool = true; + Serie2_Tipo_Bool = true; + Serie3_Tipo_Bool = true; + + } + + public override void UpdatePLCPrimerCiclo() + { + // Escribimos el valor actual al iniciar la conexion + } + public override void UpdatePLC(PLCModel plc, int elapsedMilliseconds) + { + float v1, v2, v3; + + base.UpdatePLC(plc, elapsedMilliseconds); + + if (Serie1_Tipo_Bool) + v1 = LeerBitTag(Tag_Serie1) ? 0 : 1; + else + v1 = LeerWordTag(Tag_Serie1); + + if (Serie2_Tipo_Bool) + v2 = LeerBitTag(Tag_Serie2) ? 0 : 1; + else + v2 = LeerWordTag(Tag_Serie2); + + if (Serie3_Tipo_Bool) + v3 = LeerBitTag(Tag_Serie3) ? 0 : 1; + else + v3 = LeerWordTag(Tag_Serie3); + + AddData(v1, v2, v3); + } + public override void ucLoaded() + { + // El UserControl ya se ha cargado y podemos obtener las coordenadas para + // crear el objeto de simulacion + base.ucLoaded(); + } public void AddData(float series1Value, float series2Value, float series3Value) @@ -121,13 +166,14 @@ namespace CtrEditor.ObjetosSim.Traces { uc.ChartCanvas.Children.Clear(); - DrawSeries(uc.ChartCanvas,_series1, Brushes.Red,Min_Serie_1, Max_Serie_1, Y_offset_1); - DrawSeries(uc.ChartCanvas,_series2, Brushes.Green, Min_Serie_2, Max_Serie_2, Y_offset_2); - DrawSeries(uc.ChartCanvas,_series3, Brushes.Blue, Min_Serie_3, Max_Serie_3, Y_offset_3); + DrawSeries(uc.ChartCanvas, _series1, Brushes.Red, Min_Serie_1, Max_Serie_1, Y_offset_1); + DrawSeries(uc.ChartCanvas, _series2, Brushes.Green, Min_Serie_2, Max_Serie_2, Y_offset_2); + DrawSeries(uc.ChartCanvas, _series3, Brushes.Blue, Min_Serie_3, Max_Serie_3, Y_offset_3); } } + private void DrawSeries(Canvas ChartCanvas, List series, Brush color, float minY, float maxY, float yoffset) { if (series.Count < 2) return; @@ -139,53 +185,78 @@ namespace CtrEditor.ObjetosSim.Traces } if (minY == maxY) return; - float canvasHeight = (float) ChartCanvas.ActualHeight; - float canvasWidth = (float) ChartCanvas.ActualWidth; + float canvasHeight = (float)ChartCanvas.ActualHeight; + float canvasWidth = (float)ChartCanvas.ActualWidth; float scaleX = canvasWidth / Max_Cantidad_Elementos; float scaleY = canvasHeight / (maxY - minY + yoffset); - for (int i = 1; i < series.Count; i++) - { - float x1 = (i - 1) * scaleX; - float y1 = canvasHeight - (series[i - 1] - minY + yoffset) * scaleY; - float x2 = i * scaleX; - float y2 = canvasHeight - (series[i] - minY + yoffset) * scaleY; + int startIndex = 0; + float lastX = 0; + float lastY = canvasHeight - (series[startIndex] - minY + yoffset) * scaleY; - var line = new Line + while (startIndex < series.Count - 1) + { + // Buscar el próximo cambio en la serie + int endIndex = startIndex + 1; + while (endIndex < series.Count && series[endIndex] == series[startIndex]) + { + endIndex++; + } + + // Dibujar la línea horizontal desde startIndex hasta endIndex - 1 + float x1 = lastX; + float y1 = lastY; + float x2 = (endIndex - 1) * scaleX; + float y2 = y1; + + var horizontalLine = new Line { Stroke = color, - StrokeThickness = 2, + StrokeThickness = 0.4, X1 = x1, Y1 = y1, X2 = x2, Y2 = y2 }; - ChartCanvas.Children.Add(line); + ChartCanvas.Children.Add(horizontalLine); + + // Dibujar la línea vertical en endIndex - 1 si no es el último punto + if (endIndex < series.Count) + { + float xVertical = x2; + float yVerticalStart = y2; + float yVerticalEnd = canvasHeight - (series[endIndex] - minY + yoffset) * scaleY; + + var verticalLine = new Line + { + Stroke = color, + StrokeThickness = 0.4, + X1 = xVertical, + Y1 = yVerticalStart, + X2 = xVertical, + Y2 = yVerticalEnd + }; + + ChartCanvas.Children.Add(verticalLine); + + // Actualizar la última posición dibujada + lastX = xVertical; + lastY = yVerticalEnd; + } + else + { + // Actualizar la última posición dibujada para la última línea horizontal + lastX = x2; + lastY = y2; + } + + // Actualizar startIndex + startIndex = endIndex; } } - public override void UpdatePLCPrimerCiclo() - { - // Escribimos el valor actual al iniciar la conexion - } - public override void UpdatePLC(PLCModel plc, int elapsedMilliseconds) - { - base.UpdatePLC(plc, elapsedMilliseconds); - float v1 = LeerBitTag(Tag_Serie1) ? 0 : 1; - float v2 = LeerBitTag(Tag_Serie2) ? 0 : 1; - float v3 = LeerBitTag(Tag_Serie3) ? 0 : 1; - - AddData(v1, v2, v3); - } - public override void ucLoaded() - { - // El UserControl ya se ha cargado y podemos obtener las coordenadas para - // crear el objeto de simulacion - base.ucLoaded(); - - } } public partial class ucTrace3 : UserControl, IDataContainer diff --git a/ObjetosSim/osBase.cs b/ObjetosSim/osBase.cs index f0a6e0f..4144ecf 100644 --- a/ObjetosSim/osBase.cs +++ b/ObjetosSim/osBase.cs @@ -22,6 +22,7 @@ using System.Windows.Media.Imaging; using System.Windows.Input; using CommunityToolkit.Mvvm.ComponentModel; using System.Windows.Media.Animation; +using System.Diagnostics; namespace CtrEditor.ObjetosSim { @@ -272,7 +273,7 @@ namespace CtrEditor.ObjetosSim if (!_mainViewModel.IsSimulationRunning) _storyboard.SetSpeedRatio(0); else - _storyboard.SetSpeedRatio(velocidadActual); + _storyboard.SetSpeedRatio(Math.Abs(velocidadActual)); } /// @@ -321,7 +322,7 @@ namespace CtrEditor.ObjetosSim } } - public float LeerWordTagScaled(PLCModel _plc, string Tag, float IN_scale_Min, float IN_scale_Max, float OUT_scale_Min, float OUT_scale_Max) + public float LeerWordTagScaled(string Tag, float IN_scale_Min = 0, float IN_scale_Max = 27648, float OUT_scale_Min = 0, float OUT_scale_Max = 100) { if (_plc == null) return 0; if (!string.IsNullOrEmpty(Tag)) @@ -332,7 +333,23 @@ namespace CtrEditor.ObjetosSim { SDataValue plcData = _plc.LeerTag(Tag); float Value = plcData.UInt16; // WORD - return (Value - OUT_scale_Min) / (OUT_scale_Max - OUT_scale_Min) * (IN_scale_Max - IN_scale_Min) + IN_scale_Min; + return (Value - IN_scale_Min) / (IN_scale_Max - IN_scale_Min) * (OUT_scale_Max - OUT_scale_Min) + OUT_scale_Min; + } + } + return 0; + } + public float LeerWordTag(string Tag) + { + if (_plc == null) return 0; + if (!string.IsNullOrEmpty(Tag)) + { + if (float.TryParse(Tag, out float v)) + return v; + if (_plc != null) + { + SDataValue plcData = _plc.LeerTag(Tag); + float Value = plcData.UInt16; // WORD + return Value; } } return 0; @@ -460,9 +477,9 @@ namespace CtrEditor.ObjetosSim return simulationManager.AddRectangle(Ancho, Alto, GetRectangleCenter(wpfRect), Angulo); } - public simBarrera AddBarrera(SimulationManagerFP simulationManager, System.Windows.Shapes.Rectangle wpfRect, float Alto, float Ancho, float Angulo) + public simBarrera AddBarrera(SimulationManagerFP simulationManager, System.Windows.Shapes.Rectangle wpfRect, float Alto, float Ancho, float Angulo, bool detectarCuello) { - return simulationManager.AddBarrera(Ancho, Alto, GetRectangleCenter(wpfRect), Angulo); + return simulationManager.AddBarrera(Ancho, Alto, GetRectangleCenter(wpfRect), Angulo, detectarCuello); } public void UpdateOrCreateLine(simGuia simGuia, System.Windows.Shapes.Rectangle wpfRect) @@ -505,6 +522,60 @@ namespace CtrEditor.ObjetosSim } + public class TimerTON_TOFF + { + Stopwatch _stopwatch_ON = new Stopwatch(); + Stopwatch _stopwatch_OFF = new Stopwatch(); + private bool _senalFiltrada; + + public float Tiempo_ON_s { get; set; } + public float Tiempo_OFF_s { get; set; } + + private bool senal; + public bool Senal + { + get => senal; + set + { + senal = value; + if (value) + { + ReiniciarTimer(_stopwatch_ON); + _stopwatch_OFF.Reset(); + } + else + { + _stopwatch_ON.Reset(); + ReiniciarTimer(_stopwatch_OFF); + } + } + } + + public TimerTON_TOFF() + { + Senal = false; + Tiempo_ON_s = 1; + Tiempo_OFF_s = 1; + } + + public bool SenalFiltrada() + { + if (_stopwatch_ON.ElapsedMilliseconds > (Tiempo_ON_s * 1000)) + _senalFiltrada = true; + if (_stopwatch_OFF.ElapsedMilliseconds > (Tiempo_OFF_s * 1000)) + _senalFiltrada = false; + + return _senalFiltrada; + } + + void ReiniciarTimer(Stopwatch timer) + { + timer.Reset(); + timer.Start(); + } + + } + [AttributeUsage(AttributeTargets.Property)] public class HiddenAttribute : Attribute { diff --git a/Simulacion/Aether.cs b/Simulacion/Aether.cs index e278b78..a97c1aa 100644 --- a/Simulacion/Aether.cs +++ b/Simulacion/Aether.cs @@ -17,6 +17,7 @@ using nkast.Aether.Physics2D.Dynamics.Contacts; using nkast.Aether.Physics2D.Collision; using Transform = nkast.Aether.Physics2D.Common.Transform; using nkast.Aether.Physics2D.Dynamics.Joints; +using static System.Runtime.InteropServices.JavaScript.JSType; namespace CtrEditor.Simulacion { @@ -27,7 +28,7 @@ namespace CtrEditor.Simulacion public void RemoverBody() { - if (Body != null && _world.BodyList.Count>0) + if (Body != null && _world.BodyList.Count>0 && _world.BodyList.Contains(Body)) { _world.Remove(Body); } @@ -38,7 +39,11 @@ namespace CtrEditor.Simulacion } public void SetPosition(Vector2 centro) { - Body.SetTransform(centro, Body.Rotation); + try + { + Body.SetTransform(centro, Body.Rotation); + } + catch { } } } @@ -78,13 +83,13 @@ namespace CtrEditor.Simulacion // Crear la geometría del sensor de curva List segments = CreateCurveVertices(_innerRadius, _outerRadius, _startAngle, _endAngle); - Body = new Body(); + Body = _world.CreateBody(); foreach (var segment in segments) { var shape = new PolygonShape(segment, 1f); var fixture = Body.CreateFixture(shape); fixture.IsSensor = true; - } + } Body.Position = position; Body.BodyType = BodyType.Static; Body.Tag = this; @@ -175,10 +180,13 @@ namespace CtrEditor.Simulacion public class simTransporte : simBase { public float Speed { get; set; } // Velocidad para efectos de cinta transportadora + public float Friction { get; set; } // Friccion para efectos de cinta transportadora public float DistanceGuide2Guide { get; set; } public bool isBrake { get; set; } public bool TransportWithGuides = false; private List _deferredActions; + public float Width { get; set; } + public float Height { get; set; } public simTransporte(World world, List deferredActions, float width, float height, Vector2 position, float angle = 0) { @@ -209,10 +217,15 @@ namespace CtrEditor.Simulacion var newShape = new PolygonShape(PolygonTools.CreateRectangle(width / 2, height / 2), 1f); Body.CreateFixture(newShape); + Width = width; + Height = height; } public void Create(float width, float height, Vector2 position, float angle = 0) { RemoverBody(); + Width = width; + Height = height; + Friction = 0.1f; Body = _world.CreateRectangle( width, height, 1f, position); Body.FixtureList[0].IsSensor = true; Body.BodyType = BodyType.Static; @@ -223,13 +236,22 @@ namespace CtrEditor.Simulacion public class simBarrera : simBase { + public float Distancia; public int LuzCortada; + public bool LuzCortadaNeck; + public bool DetectNeck; + public List ListSimBotellaContact; + float _height; + private List _deferredActions; - public simBarrera(World world, List deferredActions, float width, float height, Vector2 position, float angle = 0) + public simBarrera(World world, List deferredActions, float width, float height, Vector2 position, float angle = 0, bool detectectNeck = false) { _world = world; + _height = height; + DetectNeck = detectectNeck; _deferredActions = deferredActions; + ListSimBotellaContact = new List(); Create(width, height, position, angle); } @@ -252,9 +274,11 @@ namespace CtrEditor.Simulacion Body.CreateFixture(newShape); } - public void Create(float width, float height, Vector2 position, float angle = 0) + public void Create(float width, float height, Vector2 position, float angle = 0, bool detectectNeck = false) { RemoverBody(); + _height = height; + DetectNeck = detectectNeck; Body = _world.CreateRectangle( width, height, 1f, position); Body.FixtureList[0].IsSensor = true; Body.BodyType = BodyType.Static; @@ -262,6 +286,56 @@ namespace CtrEditor.Simulacion Body.Tag = this; // Importante para la identificación durante la colisión LuzCortada = 0; } + + public void CheckIfNecksIsTouching() + { + if (LuzCortada == 0) return; + + foreach (var botella in ListSimBotellaContact) + { + // Obtener el centro de la barrera + Vector2 sensorCenter = Body.Position; + + // Calcular el vector de la línea horizontal centrada de la barrera + float halfHeight = _height / 2; + float cos = (float)Math.Cos(Body.Rotation); + float sin = (float)Math.Sin(Body.Rotation); + + // Calcular los puntos inicial y final de la línea horizontal centrada y rotada + Vector2 lineStart = sensorCenter + new Vector2(-halfHeight * cos, halfHeight * sin); + Vector2 lineEnd = sensorCenter + new Vector2(halfHeight * cos, -halfHeight * sin); + + // Proyectar el centro de la botella sobre la línea horizontal + Vector2 fixtureCenter = botella.Body.Position; + Vector2 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd); + + // Calcular la distancia entre el punto más cercano y el centro del cuello de la botella + float distance; + Vector2.Distance(ref closestPoint,ref fixtureCenter, out distance); + Distancia = distance; + if (distance <= botella._neckRadius) + { + LuzCortadaNeck = true; + return; + } + } + LuzCortadaNeck = false; + } + + private Vector2 ProjectPointOntoLine(Vector2 point, Vector2 lineStart, Vector2 lineEnd) + { + Vector2 lineDirection = lineEnd - lineStart; + lineDirection.Normalize(); + + Vector2 pointToLineStart = point - lineStart; + float projectionLength; + Vector2.Dot(ref pointToLineStart, ref lineDirection, out projectionLength); + + return lineStart + projectionLength * lineDirection; + } + + + } public class simGuia : simBase @@ -290,29 +364,39 @@ namespace CtrEditor.Simulacion public class simBotella : simBase { - private float _radius; + public float Radius; private float _mass; public bool Descartar = false; public int isOnTransports; - public List ListOnTransports; + public List ListOnTransports; public bool isRestricted; public bool isNoMoreRestricted; public float OriginalMass; public simTransporte ConveyorRestrictedTo; public Fixture axisRestrictedBy; + + public float OverlapPercentage; + + public float _neckRadius; PrismaticJoint _activeJoint; private List _deferredActions; - public simBotella(World world, List deferredActions, float diameter, Vector2 position, float mass) + public simBotella(World world, List deferredActions, float diameter, Vector2 position, float mass,float neckRadius = 0) { _world = world; - ListOnTransports = new List(); + ListOnTransports = new List(); _deferredActions = deferredActions; - _radius = diameter / 2; + Radius = diameter / 2; _mass = mass; _activeJoint = null; + + if (neckRadius<=0) + neckRadius = diameter / 4; + + _neckRadius = neckRadius; + Create(position); } @@ -349,7 +433,8 @@ namespace CtrEditor.Simulacion RemoverBody(); isOnTransports = 0; _activeJoint = null; - Body = _world.CreateCircle( _radius, 1f, position); + + Body = _world.CreateCircle( Radius, 1f, position); Body.BodyType = BodyType.Dynamic; // Restablecer manejador de eventos de colisión @@ -359,12 +444,12 @@ namespace CtrEditor.Simulacion Body.Tag = this; // Importante para la identificación durante la colisión // Configurar la fricción - Body.SetFriction(0.5f); + Body.SetFriction(0.2f); // Configurar amortiguamiento Body.LinearDamping = 1f; // Ajustar para controlar la reducción de la velocidad lineal Body.AngularDamping = 1f; // Ajustar para controlar la reducción de la velocidad angular - Body.SetRestitution(0.1f); // Baja restitución para menos rebote + Body.SetRestitution(0f); // Baja restitución para menos rebote Body.SleepingAllowed = false; Body.IsBullet = true; @@ -372,7 +457,7 @@ namespace CtrEditor.Simulacion public void SetDiameter(float diameter) { - _radius = diameter / 2; + Radius = diameter / 2; Create(Body.Position); // Recrear el círculo con el nuevo tamaño } @@ -386,11 +471,13 @@ namespace CtrEditor.Simulacion if (fixtureB.Body.Tag is simBarrera Sensor) { Sensor.LuzCortada += 1; + Sensor.ListSimBotellaContact.Add(this); return true; } else if (fixtureB.Body.Tag is simCurve curve) { - curve.ApplyCurveEffect(fixtureA); + isOnTransports += 1; + ListOnTransports.Add(curve); return true; // No aplicar respuestas físicas } else if (fixtureB.Body.Tag is simDescarte) @@ -419,22 +506,30 @@ namespace CtrEditor.Simulacion return true; // No aplicar respuestas físicas } - public static bool IntersectAABBs(ref AABB aabbA, ref AABB aabbB, out AABB overlap) + private float CalculateOverlapPercentage(simTransporte conveyor) { - overlap = new AABB(); + CircleShape circleShape = Body.FixtureList[0].Shape as CircleShape; + PolygonShape polygonShape = conveyor.Body.FixtureList[0].Shape as PolygonShape; - float minX = Math.Max(aabbA.LowerBound.X, aabbB.LowerBound.X); - float minY = Math.Max(aabbA.LowerBound.Y, aabbB.LowerBound.Y); - float maxX = Math.Min(aabbA.UpperBound.X, aabbB.UpperBound.X); - float maxY = Math.Min(aabbA.UpperBound.Y, aabbB.UpperBound.Y); + // Obtener centro y radio del círculo + Vector2 centroCirculo = Body.Position; + float radio = circleShape.Radius; - if (minX < maxX && minY < maxY) + // Obtener los vértices del polígono (rectángulo) + Vector2[] vertices = new Vector2[polygonShape.Vertices.Count]; + float cos = (float)Math.Cos(conveyor.Body.Rotation); + float sin = (float)Math.Sin(conveyor.Body.Rotation); + + for (int i = 0; i < polygonShape.Vertices.Count; i++) { - overlap.LowerBound = new Vector2(minX, minY); - overlap.UpperBound = new Vector2(maxX, maxY); - return true; + Vector2 vertex = polygonShape.Vertices[i]; + float rotatedX = vertex.X * cos - vertex.Y * sin + conveyor.Body.Position.X; + float rotatedY = vertex.X * sin + vertex.Y * cos + conveyor.Body.Position.Y; + vertices[i] = new Vector2(rotatedX, rotatedY); } - return false; + + // Calcular el porcentaje de la superficie compartida + return InterseccionCirculoRectangulo.CalcularSuperficieCompartida(vertices, centroCirculo, radio); } private void HandleOnSeparation(Fixture sender, Fixture fixtureB, Contact contact) @@ -445,29 +540,107 @@ namespace CtrEditor.Simulacion ListOnTransports.Remove(transport); isOnTransports -= 1; } + if (isOnTransports > 0 && fixtureB.Body.Tag is simCurve) + { + if (fixtureB.Body.Tag is simCurve transport) + ListOnTransports.Remove(transport); + isOnTransports -= 1; + } if (isRestricted && fixtureB == axisRestrictedBy) { isRestricted = false; isNoMoreRestricted = true; } if (fixtureB.Body.Tag is simBarrera Sensor) - Sensor.LuzCortada -= 1; - } - - public void ApplyConveyorSpeed() - { - Body.LinearVelocity = new Vector2(0.0f, 0.0f); - foreach (var conveyor in ListOnTransports) { - ApplyConveyorEffect(conveyor); + Sensor.LuzCortada -= 1; + Sensor.ListSimBotellaContact.Remove(this); + } + } + /// + /// Aplica la fuerza de traccion a la botellas segun los + /// transportes sobre los que se encuentra + /// + /// + public void ApplyConveyorSpeed(float deltaTime_s) + { + foreach (var transporte in ListOnTransports) + { + if (transporte is simTransporte conveyorRect) + if (ApplyConveyorEffect(deltaTime_s, conveyorRect)) + break; + if (transporte is simCurve conveyorCurve) + conveyorCurve.ApplyCurveEffect(Body.FixtureList[0]); } } - private void ApplyConveyorEffect(simTransporte conveyor) + private bool ApplyConveyorEffect(float deltaTime_s, simTransporte conveyor) { + // Calcular el porcentaje de superficie sobrepuesta + float overlapPercentage = CalculateOverlapedArea(this, conveyor); // CalculateOverlapPercentage(conveyor); + OverlapPercentage = overlapPercentage; + + // Calcular la velocidad deseada del transporte float speedMetersPerSecond = conveyor.Speed / 60.0f; Vector2 desiredVelocity = new Vector2((float)Math.Cos(conveyor.Body.Rotation), (float)Math.Sin(conveyor.Body.Rotation)) * speedMetersPerSecond; - Body.LinearVelocity += desiredVelocity; + + // Obtener la velocidad actual de la botella + Vector2 currentVelocity = Body.LinearVelocity; + + // Calcular la diferencia de velocidad deseada + Vector2 velocityDifference = desiredVelocity - currentVelocity; + + // Calcular la fuerza de fricción necesaria - 0 : ya esta en velocidad - 1 : esta detenido + float proporcionalVelocityNeeded = 1-CalculateProportion(currentVelocity, desiredVelocity); + float frictionCoefficient; + + switch (proporcionalVelocityNeeded) { + case > 0.3f: + frictionCoefficient = conveyor.Friction * overlapPercentage; + break; + + default: + frictionCoefficient = proporcionalVelocityNeeded * overlapPercentage; + break; + } + + + if (isRestricted && conveyor == ConveyorRestrictedTo && overlapPercentage > 0.5f) + { + Body.LinearVelocity = desiredVelocity; + return true; + } + // Aplicar la fuerza de fricción en función del porcentaje de superficie sobrepuesta + Body.LinearVelocity += frictionCoefficient * desiredVelocity; + //Body.ApplyForce(frictionForce * overlapPercentage); + return false; + } + + public float CalculateOverlapedArea(simBotella botella, simTransporte conveyor) + { + //float areaBotella = (float) ((botella.Radius * botella.Radius * Math.PI) / (Math.PI / 4)); // Math.PI/4 porque es un cuadrado en vez de un circulo + //float area = OverlapedArea.CalculateOverlapedArea(botella.Body, conveyor.Body); + //if (areaBotella == 0) return 0; + //return area/areaBotella; + return OverlapedAreaFast.CalculateOverlapedArea(botella, conveyor); + } + + public static float CalculateProportion(Vector2 currentVelocity, Vector2 desiredVelocity) + { + // Calcular la proyección escalar de v2 sobre v1 + float dotProduct; + Vector2.Dot(ref desiredVelocity, ref currentVelocity, out dotProduct); + float magnitudeV1Squared = desiredVelocity.LengthSquared(); + + // Si la magnitud de v1 es 0, la proporción no está definida + if (magnitudeV1Squared == 0) + { + return 0; + } + + // Calcular la proporción + float proportion = dotProduct / magnitudeV1Squared; + return proportion; } public void CenterFixtureOnConveyor() @@ -559,19 +732,22 @@ namespace CtrEditor.Simulacion { if (cuerpo is simBotella botella) { - botella.ApplyConveyorSpeed(); + botella.ApplyConveyorSpeed(elapsedMilliseconds/1000); if (botella.isRestricted) { - botella.CenterFixtureOnConveyor(); + botella.CenterFixtureOnConveyor(); botella.Body.Inertia = 0; - botella.Body.Mass = 20; - } else if (botella.isNoMoreRestricted) + botella.Body.Mass = 100; + } + else if (botella.isNoMoreRestricted) { botella.isNoMoreRestricted = false; botella.Body.Mass = botella.OriginalMass; } } + else if (cuerpo is simBarrera barrera) + barrera.CheckIfNecksIsTouching(); } // Procesa las acciones diferidas @@ -612,9 +788,9 @@ namespace CtrEditor.Simulacion return rectangle; } - public simBarrera AddBarrera(float width, float height, Vector2 position, float angle) + public simBarrera AddBarrera(float width, float height, Vector2 position, float angle, bool detectarCuello) { - simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle); + simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle, detectarCuello); Cuerpos.Add(rectangle); return rectangle; } diff --git a/Simulacion/InterseccionCirculoRectangulo.cs b/Simulacion/InterseccionCirculoRectangulo.cs index f8c3667..6ebe678 100644 --- a/Simulacion/InterseccionCirculoRectangulo.cs +++ b/Simulacion/InterseccionCirculoRectangulo.cs @@ -12,9 +12,9 @@ namespace CtrEditor.Simulacion internal class InterseccionCirculoRectangulo { // Definición de la función CalcularSuperficieCompartida - public static float CalcularSuperficieCompartida(Vector2[] vertices, Vector2 center, float r) + public static float CalcularSuperficieCompartida(Vector2[] vertices, Vector2 center, float radio) { - float totalCircleArea = (float)Math.PI * r * r; + float totalCircleArea = (float) (Math.PI * radio * radio); // Distancia a líneas ajustado float[] distances = new float[4]; @@ -32,30 +32,30 @@ namespace CtrEditor.Simulacion float d = Math.Abs(minDistance); float sharedArea = 0; - if (Array.TrueForAll(distances, dist => Math.Abs(dist) > r)) + if (Array.TrueForAll(distances, dist => Math.Abs(dist) > radio)) { sharedArea = totalCircleArea; } - else if (d < r) + else if (d < radio) { - float cosTheta = Math.Min(1, d / r); - float sinTheta = (float)Math.Sqrt(Math.Max(0, r * r - d * d)); + float cosTheta = Math.Min(1, d / radio); + float sinTheta = (float)Math.Sqrt(Math.Max(0, radio * radio - d * d)); if (minDistance < 0) // El centro está dentro del rectángulo { - float areaOutside = r * r * (float)Math.Acos(cosTheta) - d * sinTheta; + float areaOutside = radio * radio * (float)Math.Acos(cosTheta) - d * sinTheta; sharedArea = totalCircleArea - areaOutside; } else // El centro está fuera del rectángulo { - sharedArea = r * r * (float)Math.Acos(cosTheta) - d * sinTheta; + sharedArea = radio * radio * (float)Math.Acos(cosTheta) - d * sinTheta; } } else { sharedArea = 0; } - - return sharedArea / totalCircleArea; + var area = (sharedArea / totalCircleArea) * 1.1f; + return area > 1 ? 1 : area; } public static float DistanceFromLine(Vector2 point, Vector2 start, Vector2 end) @@ -68,6 +68,7 @@ namespace CtrEditor.Simulacion } } + } diff --git a/Simulacion/OverlapedArea.cs b/Simulacion/OverlapedArea.cs new file mode 100644 index 0000000..9d860ba --- /dev/null +++ b/Simulacion/OverlapedArea.cs @@ -0,0 +1,243 @@ +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 aVertices = GetRotatedVertices(bodyA); + List bVertices = GetRotatedVertices(bodyB); + + List 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 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 SutherlandHodgmanClip(List subjectPolygon, List clipPolygon) + { + List outputList = new List(subjectPolygon); + + for (int i = 0; i < clipPolygon.Count; i++) + { + Vector2 clipEdgeStart = clipPolygon[i]; + Vector2 clipEdgeEnd = clipPolygon[(i + 1) % clipPolygon.Count]; + List inputList = outputList; + outputList = new List(); + + 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 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 + { + 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 GetRotatedVertices(Body body) + { + List vertices = new List(); + + // 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(-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; + } + } + +}