using System.Windows.Controls; using System.Windows.Media; using System.Windows.Shapes; using System.Windows; using System.Diagnostics; using System; using System.Collections.Generic; using System.Numerics; using System.Linq; using BepuPhysics; using BepuPhysics.Collidables; using BepuPhysics.CollisionDetection; using BepuPhysics.Constraints; using BepuPhysics.Trees; using BepuUtilities; using BepuUtilities.Memory; using CtrEditor.FuncionesBase; using DocumentFormat.OpenXml.Vml; using DocumentFormat.OpenXml.Spreadsheet; namespace CtrEditor.Simulacion { // Callback handlers para BEPU public struct NarrowPhaseCallbacks : INarrowPhaseCallbacks { private SimulationManagerBEPU _simulationManager; public NarrowPhaseCallbacks(SimulationManagerBEPU simulationManager) { _simulationManager = simulationManager; } public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin) { // ✅ NUEVO FILTRADO: Evitar colisiones innecesarias entre triángulos de curvas if (_simulationManager != null) { // Obtener información de los objetos involucrados var curveA = _simulationManager.GetCurveFromTriangleBodyHandle(a.BodyHandle); var curveB = _simulationManager.GetCurveFromTriangleBodyHandle(b.BodyHandle); var transportA = GetTransportFromCollidable(a); var transportB = GetTransportFromCollidable(b); // ✅ ELIMINADO: Las barreras ya no tienen body físico // var barrierA = GetBarreraFromCollidable(a); // var barrierB = GetBarreraFromCollidable(b); var discardA = GetDescarteFromCollidable(a); var discardB = GetDescarteFromCollidable(b); var botellaA = GetBotellaFromCollidable(a); var botellaB = GetBotellaFromCollidable(b); // ✅ FILTRO 1: No permitir colisiones entre triángulos de curvas if (curveA != null && curveB != null) { return false; // Los triángulos de curva no deben colisionar entre sí } // ✅ FILTRO 2: No permitir colisiones entre transportes if (transportA != null && transportB != null) { return false; // Los transportes no deben colisionar entre sí } // ✅ FILTRO 3: No permitir colisiones entre elementos estáticos (transportes, curvas, descartes) // ✅ ELIMINADO: barrierA y barrierB ya que las barreras no tienen body físico var staticA = (transportA != null || curveA != null || discardA != null); var staticB = (transportB != null || curveB != null || discardB != null); if (staticA && staticB) { return false; // Los elementos estáticos no deben colisionar entre sí } // ✅ ELIMINADO: No bloquear colisiones físicas aquí // Las colisiones físicas deben permitirse siempre para mantener geometría sólida // Solo bloqueamos la creación de motores duplicados en ConfigureContactManifold } return true; // Permitir todas las demás colisiones (botella-estático, botella-botella) } public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB) { return true; } public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold { // ✅ CONFIGURACIÓN BÁSICA de materiales físicos pairMaterial = new PairMaterialProperties { FrictionCoefficient = 0.3f, MaximumRecoveryVelocity = 1f, SpringSettings = new SpringSettings(80, 6) }; if (_simulationManager != null) { var botellaA = GetBotellaFromCollidable(pair.A); var botellaB = GetBotellaFromCollidable(pair.B); // ✅ ELIMINADO: Las barreras ya no tienen body físico // var barreraA = GetBarreraFromCollidable(pair.A); // var barreraB = GetBarreraFromCollidable(pair.B); var descarteA = GetDescarteFromCollidable(pair.A); var descarteB = GetDescarteFromCollidable(pair.B); var botella = botellaA ?? botellaB; // ✅ ELIMINADO: Las barreras ya no participan en colisiones físicas // if (barreraA != null || barreraB != null) // { // return false; // NO generar contacto físico // } // ✅ DESCARTES como sensores puros if (descarteA != null || descarteB != null) { var descarte = descarteA ?? descarteB; if (botella != null) { _simulationManager.RegisterDescarteContact(descarte, botella); } return false; // NO generar contacto físico } // ✅ NUEVO SISTEMA SIMPLIFICADO: Solo registrar contactos para actualización en Step var transportA = GetTransportFromCollidable(pair.A); var transportB = GetTransportFromCollidable(pair.B); var curveA = GetCurveFromCollidable(pair.A); var curveB = GetCurveFromCollidable(pair.B); // ✅ CONTACTO BOTELLA-TRANSPORTE: Crear o actualizar motor inmediatamente if (botella != null && (transportA != null || transportB != null)) { var transport = transportA ?? transportB; // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE transport.UpdateCachedProperties(); var direction = transport.DirectionVector; var speed = transport.Speed; botella.CreateOrUpdateMotor(transport, direction, speed); //Fricción alta para transportes pairMaterial.FrictionCoefficient = 0.3f; pairMaterial.MaximumRecoveryVelocity = 1f; pairMaterial.SpringSettings = new SpringSettings(80, 6); } // ✅ CONTACTO BOTELLA-CURVA: Crear o actualizar motor inmediatamente else if (botella != null && (curveA != null || curveB != null)) { var curve = curveA ?? curveB; // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE var direction = _simulationManager.CalculateCurveDirectionFromBottlePosition(curve, botella); var speed = curve.SpeedMetersPerSecond; // ✅ CORREGIDO: Usar SpeedMetersPerSecond como simTransporte botella.CreateOrUpdateMotor(curve, direction, speed); // Fricción alta para curvas pairMaterial.FrictionCoefficient = 0.3f; pairMaterial.MaximumRecoveryVelocity = 1f; pairMaterial.SpringSettings = new SpringSettings(80, 6); } // ✅ CONTACTO BOTELLA-GUÍA: Configuración específica de fricción else if (botella != null && (GetGuiaFromCollidable(pair.A) != null || GetGuiaFromCollidable(pair.B) != null)) { // Configuración específica para guías usando propiedades configurables pairMaterial.FrictionCoefficient = 0.3f; pairMaterial.MaximumRecoveryVelocity = 1f; pairMaterial.SpringSettings = new SpringSettings(80, 6); } // Ajustes básicos para otras botellas else if (botella != null) { pairMaterial.FrictionCoefficient = 0.3f; pairMaterial.MaximumRecoveryVelocity = 1f; pairMaterial.SpringSettings = new SpringSettings(80, 6); } } return true; } private simTransporte GetTransportFromCollidable(CollidableReference collidable) { if (_simulationManager?.simulation != null) { if (collidable.Mobility == CollidableMobility.Kinematic) { var bodyHandle = collidable.BodyHandle; var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle); return simBase as simTransporte; } } return null; } private simBotella GetBotellaFromCollidable(CollidableReference collidable) { if (_simulationManager?.simulation != null) { if (collidable.Mobility == CollidableMobility.Dynamic) { var bodyHandle = collidable.BodyHandle; var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle); return simBase as simBotella; } } return null; } // ✅ ELIMINADO: GetBarreraFromCollidable - las barreras ya no tienen body físico // private simBarrera GetBarreraFromCollidable(CollidableReference collidable) { ... } private simDescarte GetDescarteFromCollidable(CollidableReference collidable) { if (_simulationManager?.simulation != null) { if (collidable.Mobility == CollidableMobility.Kinematic) { var bodyHandle = collidable.BodyHandle; var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle); return simBase as simDescarte; } } return null; } private simCurve GetCurveFromCollidable(CollidableReference collidable) { if (_simulationManager?.simulation != null) { if (collidable.Mobility == CollidableMobility.Kinematic) { var bodyHandle = collidable.BodyHandle; var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle); return simBase as simCurve; } } return null; } private simGuia GetGuiaFromCollidable(CollidableReference collidable) { if (_simulationManager?.simulation != null) { if (collidable.Mobility == CollidableMobility.Kinematic) { var bodyHandle = collidable.BodyHandle; var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle); return simBase as simGuia; } } return null; } /// /// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado directamente en el manifold /// public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold) { return true; } public void Initialize(Simulation simulation) { } public void Dispose() { } } public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks { public Vector3 Gravity; public float LinearDamping; public float AngularDamping; private SimulationManagerBEPU _simulationManager; // ✅ NUEVA REFERENCIA public PoseIntegratorCallbacks(Vector3 gravity, float linearDamping = 0.999f, float angularDamping = 0.995f, SimulationManagerBEPU simulationManager = null) { Gravity = gravity; LinearDamping = linearDamping; AngularDamping = angularDamping; _simulationManager = simulationManager; // ✅ NUEVA INICIALIZACIÓN } public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving; public readonly bool AllowSubstepsForUnconstrainedBodies => false; public readonly bool IntegrateVelocityForKinematics => false; public void Initialize(Simulation simulation) { } public void PrepareForIntegration(float dt) { } public void IntegrateVelocity(Vector bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector integrationMask, int workerIndex, Vector dt, ref BodyVelocityWide velocity) { // Aplicar gravedad var gravityWide = Vector3Wide.Broadcast(Gravity); velocity.Linear += gravityWide * dt; // ✅ ELIMINADO COMPLETAMENTE - ya no se necesita lógica especial de frenado // El sistema LinearAxisMotor maneja automáticamente todas las fuerzas // Aplicar amortiguamiento lineal y angular para simular resistencia del aire // Esto es crucial para que los cilindros se detengan de forma realista var linearDampingWide = Vector.One * LinearDamping; var angularDampingWide = Vector.One * AngularDamping; velocity.Linear *= linearDampingWide; velocity.Angular *= angularDampingWide; // ¡Esto es clave para detener rotaciones infinitas! } } public class SimulationManagerBEPU { public Simulation simulation; public List Cuerpos; public List _deferredActions; private BufferPool bufferPool; private Stopwatch stopwatch; private double stopwatch_last; // Referencia al manager de visualización 3D public BEPUVisualization3DManager Visualization3DManager { get; set; } // Propiedad para controlar si la actualización 3D está habilitada public bool Is3DUpdateEnabled { get; set; } = true; // ✅ NUEVAS - para filtrado eficiente de callbacks private HashSet _transportHandles; private HashSet _curveHandles; private HashSet _barrierHandles; private HashSet _discardHandles; private HashSet _bottleHandles; private HashSet _guiaHandles; // ✅ NUEVO - contador de frames para optimizaciones private int _frameCount = 0; // ✅ CONSERVAR - sistemas existentes que funcionan bien private Dictionary> _barreraContacts; private Dictionary> _descarteContacts; private HashSet _botellasParaEliminar; // ✅ NUEVAS - propiedades configurables para fricción de guías public float GuiaFrictionCoefficient { get; set; } = 0.1f; public float GuiaMaxRecoveryVelocity { get; set; } = 0.5f; public float GuiaSpringFrequency { get; set; } = 40f; public float GuiaSpringDampingRatio { get; set; } = 2f; private object _contactsLock = new object(); /// /// Obtiene el objeto simBase correspondiente a un BodyHandle /// public simBase GetSimBaseFromBodyHandle(BodyHandle bodyHandle) { return Cuerpos.FirstOrDefault(c => c.BodyHandle.Equals(bodyHandle)); } /// /// ✅ SIMPLIFICADO: Obtiene una simCurve desde su BodyHandle principal /// public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle) { // ✅ SIMPLIFICADO: Solo buscar en cuerpos principales (ya no hay triángulos separados) var simBase = GetSimBaseFromBodyHandle(bodyHandle); return simBase as simCurve; } /// /// ✅ NUEVO: Obtiene todas las botellas que están conectadas a un elemento específico /// /// Elemento (simTransporte o simCurve) del cual obtener las botellas conectadas /// Lista de botellas conectadas al elemento public List GetBottlesConnectedToElement(simBase element) { var connectedBottles = new List(); try { if (element == null) return connectedBottles; // ✅ BUSCAR BOTELLAS QUE TENGAN ESTE ELEMENTO COMO TARGET foreach (var bottle in Cuerpos.OfType()) { if (bottle != null && bottle.HasMotor && bottle.CurrentMotorTarget == element) { connectedBottles.Add(bottle); } } System.Diagnostics.Debug.WriteLine($"[GetBottlesConnectedToElement] Encontradas {connectedBottles.Count} botellas conectadas a {element.GetType().Name}"); } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[GetBottlesConnectedToElement] ❌ ERROR: {ex.Message}"); } return connectedBottles; } // ✅ NUEVOS - gestión automática de clasificación private void RegisterObjectHandle(simBase obj) { switch (obj) { case simBotella bottle: _bottleHandles.Add(bottle.BodyHandle); break; case simTransporte transport: _transportHandles.Add(transport.BodyHandle); // Calcular propiedades iniciales transport.UpdateCachedProperties(); break; case simCurve curve: _curveHandles.Add(curve.BodyHandle); // ✅ SIMPLIFICADO: Ya no hay triángulos separados, solo el cuerpo principal break; case simBarrera barrier: // ✅ NUEVO: Las barreras ya no tienen body físico, no registrar handle // _barrierHandles.Add(barrier.BodyHandle); // ELIMINADO break; case simDescarte discard: _discardHandles.Add(discard.BodyHandle); break; case simGuia guia: _guiaHandles.Add(guia.BodyHandle); break; } } private void UnregisterObjectHandle(simBase obj) { switch (obj) { case simBotella bottle: _bottleHandles.Remove(bottle.BodyHandle); break; case simTransporte transport: _transportHandles.Remove(transport.BodyHandle); break; case simCurve curve: _curveHandles.Remove(curve.BodyHandle); // ✅ SIMPLIFICADO: Ya no hay triángulos separados break; case simBarrera barrier: // ✅ NUEVO: Las barreras ya no tienen body físico, no desregistrar handle // _barrierHandles.Remove(barrier.BodyHandle); // ELIMINADO break; case simDescarte discard: _discardHandles.Remove(discard.BodyHandle); break; case simGuia guia: _guiaHandles.Remove(guia.BodyHandle); break; } } // ✅ NUEVOS MÉTODOS - obtener objetos por handle private simBotella GetBottleByHandle(BodyHandle handle) { return Cuerpos.OfType().FirstOrDefault(b => b.BodyHandle.Equals(handle)); } private simTransporte GetTransportByHandle(BodyHandle handle) { return Cuerpos.OfType().FirstOrDefault(t => t.BodyHandle.Equals(handle)); } private simCurve GetCurveByHandle(BodyHandle handle) { // Primero buscar en cuerpos principales var curve = Cuerpos.OfType().FirstOrDefault(c => c.BodyHandle.Equals(handle)); if (curve != null) return curve; // Luego buscar en triángulos return GetCurveFromTriangleBodyHandle(handle); } private simGuia GetGuiaByHandle(BodyHandle handle) { return Cuerpos.OfType().FirstOrDefault(g => g.BodyHandle.Equals(handle)); } public SimulationManagerBEPU() { // ✅ EXISTENTES - conservar Cuerpos = new List(); _deferredActions = new List(); _barreraContacts = new Dictionary>(); _descarteContacts = new Dictionary>(); _botellasParaEliminar = new HashSet(); // ✅ NUEVOS - sistemas de filtrado y contactos _transportHandles = new HashSet(); _curveHandles = new HashSet(); _barrierHandles = new HashSet(); _discardHandles = new HashSet(); _bottleHandles = new HashSet(); _guiaHandles = new HashSet(); // ✅ CONSERVAR - resto del constructor igual bufferPool = new BufferPool(); stopwatch = new Stopwatch(); var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this); // Configurar amortiguamiento para comportamiento realista: // - LinearDamping: 0.999f (muy leve, objetos siguen moviéndose pero eventualmente se detienen) // - AngularDamping: 0.995f (más agresivo para detener rotaciones infinitas de cilindros) // ✅ MODIFICADO: Pasar referencia de this al PoseIntegratorCallbacks var poseIntegratorCallbacks = new PoseIntegratorCallbacks( gravity: new Vector3(0, 0, -9.81f), // Gravedad en Z linearDamping: 0.999f, // Amortiguamiento lineal suave angularDamping: 0.995f, // Amortiguamiento angular más fuerte para detener rotaciones simulationManager: this // ✅ NUEVA REFERENCIA ); var solveDescription = new SolveDescription(8, 1); simulation = Simulation.Create(bufferPool, narrowPhaseCallbacks, poseIntegratorCallbacks, solveDescription); } public void Clear() { try { // ✅ SIMPLIFICAR - eliminar lógica de masa especial // foreach (var cuerpo in Cuerpos.OfType()) // { // cuerpo.RestoreOriginalMassIfNeeded(); // NO SE USA MÁS // } // ✅ CONSERVAR - limpiar contactos lock (_contactsLock) { _barreraContacts.Clear(); _descarteContacts.Clear(); _botellasParaEliminar.Clear(); } // ✅ NUEVO - limpiar clasificaciones _transportHandles.Clear(); _curveHandles.Clear(); _barrierHandles.Clear(); _discardHandles.Clear(); _bottleHandles.Clear(); _guiaHandles.Clear(); // ✅ CONSERVAR - resto del método igual var cuerposToRemove = new List(Cuerpos); foreach (var cuerpo in cuerposToRemove) { try { if (cuerpo != null) { cuerpo.RemoverBody(); } } catch (Exception ex) { // Error removing body - continue with cleanup } } Cuerpos.Clear(); _deferredActions.Clear(); // ✅ NUEVO - Limpiar todas las dimensiones almacenadas CtrEditor.ObjetosSim.osBase.ClearAllStoredDimensions(); // Limpiar la simulación completamente si existe if (simulation != null) { try { // Forzar un timestep pequeño para limpiar contactos pendientes simulation.Timestep(1f / 1000f); // 1ms } catch (Exception ex) { // Warning during cleanup timestep - continue } } // Limpiar visualización 3D Visualization3DManager?.Clear(); } catch (Exception ex) { // Critical error during clear - operation failed } } public void Start() { try { // ✅ INICIALIZAR PROPIEDADES CACHEADAS foreach (var transport in Cuerpos.OfType()) { transport.UpdateCachedProperties(); } foreach (var curve in Cuerpos.OfType()) { // ✅ CORREGIDO: Usar método público para actualizar velocidad curve.SetSpeed(curve.Speed); // ✅ SIMPLIFICADO: Reinicializar velocidad } stopwatch.Start(); stopwatch_last = stopwatch.Elapsed.TotalMilliseconds; } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[SimulationManager] Error in Start: {ex.Message}"); stopwatch.Start(); stopwatch_last = stopwatch.Elapsed.TotalMilliseconds; } } public void Step() { try { _frameCount++; // ✅ CONSERVAR - validación de deltaTime var currentTime = stopwatch.Elapsed.TotalMilliseconds; var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0); stopwatch_last = currentTime; if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0) { deltaTime = 1f / 60f; } const float maxDeltaTime = 0.1f; if (deltaTime > maxDeltaTime) { deltaTime = 1f / 60f; } // ✅ CONSERVAR - procesar acciones diferidas foreach (var action in _deferredActions) { action(); } _deferredActions.Clear(); // ✅ CONSERVAR - validaciones de simulación if (simulation?.Bodies == null) { return; } var invalidBodies = Cuerpos.Where(c => c != null && !simulation.Bodies.BodyExists(c.BodyHandle)).ToList(); if (invalidBodies.Count > 0) { foreach (var invalidBody in invalidBodies) { Cuerpos.Remove(invalidBody); } } // ✅ CONSERVAR - timestep var timestepValue = Math.Max(deltaTime, 1f / 120f); try { simulation.Timestep(timestepValue); } catch (AccessViolationException ex) { lock (_contactsLock) { _barreraContacts.Clear(); } throw; } // ✅ CONSERVAR - limitación de rotación y mantener botellas despiertas foreach (var cuerpo in Cuerpos.OfType().ToList()) { try { if (simulation.Bodies.BodyExists(cuerpo.BodyHandle)) { cuerpo.LimitRotationToXYPlane(); simulation.Awakener.AwakenBody(cuerpo.BodyHandle); } } catch (Exception ex) { // Error limiting rotation for bottle - continue } } // ✅ NUEVO: Detener motores de botellas que no están en contacto con elementos // Esto se ejecuta cada 10 frames para eficiencia if (_frameCount % 10 == 0) { //StopMotorsForBottlesNotInContact(); } // ✅ CONSERVAR - sistemas que funcionan bien ProcessBarreraContacts(); ProcessDescarteContacts(); ProcessCleanupSystem(); // ✅ SIMPLIFICAR - limpiar solo contactos que usamos lock (_contactsLock) { _barreraContacts.Clear(); _descarteContacts.Clear(); } // ✅ CONSERVAR - sincronización 3D if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } } catch (Exception ex) { lock (_contactsLock) { _barreraContacts.Clear(); } } } /// /// ✅ NUEVO: Marca un objeto para eliminación diferida (más seguro) /// public void Remove(simBase Objeto) { if (Objeto == null) return; try { // ✅ SIMPLIFICADO - eliminar lógica de masa especial UnregisterObjectHandle(Objeto); // ✅ NUEVO Objeto.RemoverBody(); // ✅ CONSERVAR - remover el cuerpo de BEPU // ✅ NUEVO - Limpiar dimensiones almacenadas en osBase CtrEditor.ObjetosSim.osBase.ClearStoredDimensions(Objeto); // ✅ REMOVER de la lista inmediatamente para evitar referencias colgantes Cuerpos.Remove(Objeto); if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } //System.Diagnostics.Debug.WriteLine($"[Remove] ✅ Objeto marcado para eliminación diferida: {Objeto.GetType().Name}"); } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[Remove] ❌ Error marcando objeto {Objeto?.GetType().Name}: {ex.Message}"); // ✅ CRÍTICO: Siempre remover de la lista incluso si falló Cuerpos.Remove(Objeto); } } public simBotella AddCircle(float diameter, Vector2 position, float mass) { var botella = new simBotella(simulation, _deferredActions, diameter, position, mass); Cuerpos.Add(botella); RegisterObjectHandle(botella); // ✅ NUEVO if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return botella; } public simTransporte AddRectangle(float width, float height, Vector2 position, float angle) { var transporte = new simTransporte(simulation, _deferredActions, width, height, position, angle, this); Cuerpos.Add(transporte); RegisterObjectHandle(transporte); // ✅ NUEVO - incluye UpdateCachedProperties() if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return transporte; } internal simBarrera AddBarrera(float width, float height, Vector2 wpfTopLeft, float wpfAngle, bool detectarCuello) { // ✅ CONVERTIR coordenadas WPF a BEPU internas var zPosition = simBase.zPos_Barrera; var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition); var bepuRadians = CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle); var barrera = new simBarrera(simulation, width, height, bepuCenter, bepuRadians, detectarCuello, this); Cuerpos.Add(barrera); RegisterObjectHandle(barrera); if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return barrera; } public simGuia AddLine(float width, float height, Vector2 topLeft, float angle) { var guia = new simGuia(simulation, _deferredActions, width, height, topLeft, angle); Cuerpos.Add(guia); // ✅ NOTA: simGuia no requiere registro especial if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return guia; } public simDescarte AddDescarte(float diameter, Vector2 position) { var descarte = new simDescarte(simulation, _deferredActions, diameter, position); Cuerpos.Add(descarte); RegisterObjectHandle(descarte); // ✅ NUEVO if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return descarte; } public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0) { var curve = new simCurve(simulation, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, topLeft, unused, this); Cuerpos.Add(curve); RegisterObjectHandle(curve); // ✅ NUEVO if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return curve; } // ✅ NUEVA INTERFAZ PARA BARRERAS - Solo accesible desde SimulationManagerBEPU /// /// Crea una nueva barrera y devuelve un handle opaco para WPF /// /// Ancho en metros /// Alto en metros /// Posición central en coordenadas BEPU /// Ángulo en radianes BEPU /// Si debe detectar el cuello de las botellas /// Handle opaco para referenciar la barrera desde WPF public simBarrera CreateBarrera(float width, float height, Vector2 TopLeft, float wpfAngle, bool detectarCuello) { var barrera = new simBarrera(simulation, width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width, height, wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle), detectarCuello, this); Cuerpos.Add(barrera); RegisterObjectHandle(barrera); if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return barrera; } public void UpdateBarrera(simBarrera barrera, float width, float height, Vector2 TopLeft, float wpfAngle) { barrera.Update(width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width, height, wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle)); if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } } /// /// Elimina una barrera usando su handle /// /// Handle de la barrera a eliminar public void RemoveBarrera(simBarrera barrera) { if (barrera != null) { Remove(barrera); } } /// /// Configura si la barrera debe detectar el cuello de las botellas /// /// Handle de la barrera /// True para detectar cuello, false para detectar botella completa public void SetBarreraDetectNeck(simBarrera barrera, bool detectarCuello) { if (barrera != null) { barrera.DetectNeck = detectarCuello; } } /// /// Obtiene los datos de estado de la barrera /// /// Handle de la barrera /// Estructura con los datos de luz cortada public BarreraData GetBarreraData(simBarrera barrera) { if (barrera != null) { return new BarreraData(barrera.LuzCortada, barrera.LuzCortadaNeck); } return new BarreraData(false, false); } /// /// ✅ NUEVO: Detiene motores de botellas que no están en contacto con elementos /// private void StopMotorsForBottlesNotInContact() { try { var allBottles = Cuerpos.OfType().ToList(); foreach (var bottle in allBottles) { if (bottle != null && bottle.HasMotor && bottle.IsOnElement) { // ✅ VERIFICAR SI LA BOTELLA ESTÁ REALMENTE EN CONTACTO CON ALGÚN ELEMENTO bool isInContact = false; // Verificar contacto con transportes foreach (var transport in Cuerpos.OfType()) { if (IsBottleInContactWithTransport(bottle, transport)) { isInContact = true; break; } } // Verificar contacto con curvas if (!isInContact) { foreach (var curve in Cuerpos.OfType()) { if (IsBottleInContactWithCurve(bottle, curve)) { isInContact = true; break; } } } // ✅ DETENER MOTOR SI NO ESTÁ EN CONTACTO if (!isInContact) { bottle.StopMotor(); System.Diagnostics.Debug.WriteLine($"[StopMotorsForBottlesNotInContact] ⏹️ Detenido: {bottle.BodyHandle}"); } } } } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[StopMotorsForBottlesNotInContact] ❌ ERROR: {ex.Message}"); } } /// /// ✅ NUEVO: Verifica si una botella está en contacto con un transporte /// private bool IsBottleInContactWithTransport(simBotella bottle, simTransporte transport) { try { var bottlePos = bottle.GetPosition(); var transportPos = transport.GetPosition(); var distance = Vector2.Distance( new Vector2(bottlePos.X, bottlePos.Y), new Vector2(transportPos.X, transportPos.Y) ); // Verificar si está dentro del área del transporte + tolerancia var maxDistance = Math.Max(transport.Width, transport.Height) / 2f + bottle.Radius + 0.1f; return distance <= maxDistance; } catch { return false; } } /// /// ✅ NUEVO: Verifica si una botella está en contacto con una curva /// private bool IsBottleInContactWithCurve(simBotella bottle, simCurve curve) { try { var bottlePos = bottle.GetPosition(); var curveCenter = curve.CurveCenter; // ✅ NUEVO: Usar centro real var distance = Vector2.Distance( new Vector2(bottlePos.X, bottlePos.Y), new Vector2(curveCenter.X, curveCenter.Y) ); // Verificar si está dentro del área de la curva + tolerancia var maxDistance = curve.OuterRadius + bottle.Radius + 0.1f; return distance <= maxDistance; } catch { return false; } } /// /// ✅ NUEVO: Calcula la dirección tangencial específica basada en la posición de la botella /// public Vector3 CalculateCurveDirectionFromBottlePosition(simCurve curve, simBotella bottle) { try { // ✅ NUEVO: Usar el centro real almacenado de la curva var curveCenter = curve.CurveCenter; var bottlePosition = bottle.GetPosition(); // Calcular el vector desde el centro de la curva hasta la botella (en el plano XY) var radiusVector = new Vector3(bottlePosition.X - curveCenter.X, bottlePosition.Y - curveCenter.Y, 0f); var radius = radiusVector.Length(); if (radius < 0.001f) { System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] ⚠️ Botella muy cerca del centro de la curva"); return Vector3.UnitX; // Fallback } // Normalizar el vector radial var normalizedRadius = radiusVector / radius; // Calcular la dirección tangencial (perpendicular al radio) // En coordenadas 2D: si r = (x, y), entonces t = (-y, x) es tangente var tangentDirection = new Vector3(-normalizedRadius.Y, normalizedRadius.X, 0f); // Verificar que la dirección tangencial apunte en el sentido correcto según la velocidad de la curva if (curve.Speed < 0) { //tangentDirection = -tangentDirection; } //System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] 📐 Dirección calculada:"); //System.Diagnostics.Debug.WriteLine($" Centro curva: {curveCenter}"); //System.Diagnostics.Debug.WriteLine($" Posición botella: {bottlePosition}"); //System.Diagnostics.Debug.WriteLine($" Radio: {radius:F3}"); //System.Diagnostics.Debug.WriteLine($" Vector radial: {normalizedRadius}"); //System.Diagnostics.Debug.WriteLine($" Dirección tangencial: {tangentDirection} (Longitud: {tangentDirection.Length():F3})"); return -tangentDirection; } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] ❌ ERROR: {ex.Message}"); return Vector3.UnitX; // Fallback } } public void Dispose() { Clear(); simulation?.Dispose(); bufferPool?.Clear(); } /// /// Registra un contacto entre una barrera y una botella para detección de paso /// /// Barrera que detecta el paso /// Botella que está pasando internal void RegisterBarreraContact(simBarrera barrera, simBotella botella) { if (barrera != null && botella != null) { lock (_contactsLock) { if (!_barreraContacts.ContainsKey(barrera)) { _barreraContacts[barrera] = new List(); } if (!_barreraContacts[barrera].Contains(botella)) { _barreraContacts[barrera].Add(botella); } } } } /// /// Registra un contacto entre un descarte y una botella para marcar eliminación /// /// Descarte que detecta la botella /// Botella que debe ser eliminada public void RegisterDescarteContact(simDescarte descarte, simBotella botella) { if (descarte != null && botella != null) { lock (_contactsLock) { // Marcar la botella para eliminación _botellasParaEliminar.Add(botella); botella.Descartar = true; // También registrar el contacto para la lista del descarte if (!_descarteContacts.ContainsKey(descarte)) { _descarteContacts[descarte] = new List(); } if (!_descarteContacts[descarte].Contains(botella)) { _descarteContacts[descarte].Add(botella); } } } } /// /// ✅ NUEVO: Procesa todas las barreras usando RayCast de BEPU /// Elimina la dependencia de contactos físicos y la necesidad de mantener botellas despiertas /// private void ProcessBarreraContacts() { try { // Obtener todas las barreras var barreras = Cuerpos.OfType().ToList(); foreach (var barrera in barreras) { try { // Verificar que la barrera aún existe (solo verificar que no sea null) if (barrera == null) continue; // ✅ USAR RAYCAST NATIVO DE BEPU barrera.PerformRaycast(); // System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Barrera procesada: LuzCortada={barrera.LuzCortada}, LuzCortadaNeck={barrera.LuzCortadaNeck}, Distancia={barrera.Distancia:F3}"); } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error procesando barrera: {ex.Message}"); } } // Limpiar contactos ya que no los usamos más lock (_contactsLock) { _barreraContacts.Clear(); } } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error crítico: {ex.Message}"); } } // ✅ ELIMINADO: CalculateBarreraDetectionGeometric - ya no se usa // El nuevo sistema usa RayCast nativo de BEPU a través de simBarrera.PerformRaycast() /// /// Procesa contactos de descarte para eliminar botellas marcadas usando detección geométrica /// private void ProcessDescarteContacts() { try { // Obtener todos los descartes y botellas var descartes = Cuerpos.OfType().ToList(); var botellas = Cuerpos.OfType().ToList(); foreach (var descarte in descartes) { try { // Verificar que el descarte aún existe en la simulación if (descarte == null || !simulation?.Bodies?.BodyExists(descarte.BodyHandle) == true) continue; // Limpiar la lista de forma segura if (descarte.ListSimBotellaContact != null) { descarte.ListSimBotellaContact.Clear(); } // Calcular detección usando geometría pura para TODAS las botellas CalculateDescarteDetectionGeometric(descarte, botellas); } catch (Exception ex) { // Error processing descarte - continue } } // Eliminar botellas marcadas para eliminación (después de procesamiento) RemoveMarkedBottles(); // Limpiar contactos ya que no los usamos más lock (_contactsLock) { _descarteContacts.Clear(); } } catch (Exception ex) { // Critical error in ProcessDescarte - continue } } /// /// Calcula detección geométrica pura para un descarte específico contra todas las botellas /// Usa detección de esfera contra esfera para determinar si hay contacto /// private void CalculateDescarteDetectionGeometric(simDescarte descarte, List todasLasBotellas) { try { // Validaciones de seguridad if (descarte == null || todasLasBotellas == null || simulation?.Bodies == null) return; if (!simulation.Bodies.BodyExists(descarte.BodyHandle)) return; var descarteBody = simulation.Bodies[descarte.BodyHandle]; var descartePosition = descarteBody.Pose.Position; // Validar valores de posición if (float.IsNaN(descartePosition.X) || float.IsNaN(descartePosition.Y) || float.IsNaN(descartePosition.Z)) { return; } var botellasDetectadas = new List(); // Procesar TODAS las botellas para detección geométrica foreach (var botella in todasLasBotellas) { try { if (botella == null || !simulation.Bodies.BodyExists(botella.BodyHandle)) continue; var botellaBody = simulation.Bodies[botella.BodyHandle]; var botellaPosition = botellaBody.Pose.Position; // Validar posición de la botella if (float.IsNaN(botellaPosition.X) || float.IsNaN(botellaPosition.Y) || float.IsNaN(botellaPosition.Z)) { continue; } // Calcular distancia entre centros (detección esfera contra esfera) var distance = Vector3.Distance(descartePosition, botellaPosition); // Validar distancia calculada if (float.IsNaN(distance) || float.IsInfinity(distance)) { continue; } // Verificar si las esferas se superponen var totalRadius = descarte.Radius + botella.Radius; if (distance <= totalRadius) { // Marcar botella para eliminación botella.Descartar = true; _botellasParaEliminar.Add(botella); botellasDetectadas.Add(botella); } } catch (Exception ex) { // Error processing bottle - continue } } // Actualizar lista de botellas detectadas if (descarte.ListSimBotellaContact != null) { descarte.ListSimBotellaContact.AddRange(botellasDetectadas); } } catch (Exception ex) { // Critical error in CalculateDescarteGeometric - continue } } /// /// Elimina las botellas marcadas para eliminación de forma segura /// private void RemoveMarkedBottles() { try { List botellasAEliminar; lock (_contactsLock) { botellasAEliminar = new List(_botellasParaEliminar); _botellasParaEliminar.Clear(); } foreach (var botella in botellasAEliminar) { try { if (botella != null && Cuerpos.Contains(botella)) { //System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] 🗑️ Marcando botella para eliminación: {botella.BodyHandle}"); // ✅ USAR ELIMINACIÓN DIFERIDA (más seguro) Remove(botella); } } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ Error marcando botella {botella?.BodyHandle}: {ex.Message}"); // ✅ INTENTAR remover de la lista incluso si Remove falló if (botella != null) { Cuerpos.Remove(botella); } } } } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ ERROR CRÍTICO: {ex.Message}"); } } /// /// Sistema de limpieza que elimina botellas que estén debajo de la altura de los transportes /// Cualquier botella con Z menor al nivel superior de los transportes será eliminada /// private void ProcessCleanupSystem() { try { // Altura máxima de los transportes (nivel superior) var maxTransportHeight = simBase.zPos_Transporte + simBase.zAltura_Transporte; // Obtener todas las botellas var botellas = Cuerpos.OfType().ToList(); foreach (var botella in botellas) { try { // Validar que la botella aún existe en la simulación if (botella == null || !simulation?.Bodies?.BodyExists(botella.BodyHandle) == true) continue; var posicion = botella.GetPosition(); // Validar posición if (float.IsNaN(posicion.Z) || float.IsInfinity(posicion.Z)) continue; // Si la botella está debajo del nivel de los transportes, marcarla para eliminación if (posicion.Z < (maxTransportHeight - 2 * botella.Radius) || posicion.Z > (maxTransportHeight + 2 * botella.Radius)) { lock (_contactsLock) { botella.Descartar = true; _botellasParaEliminar.Add(botella); } } } catch (Exception ex) { // Error processing bottle in cleanup - continue } } } catch (Exception ex) { // Critical error in ProcessCleanupSystem - continue } } /// /// Proyecta un punto sobre una línea definida por dos puntos /// private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd) { var lineDirection = Vector3.Normalize(lineEnd - lineStart); var pointToStart = point - lineStart; var projectionLength = Vector3.Dot(pointToStart, lineDirection); // Restringir la proyección a los límites de la línea var lineLength = Vector3.Distance(lineStart, lineEnd); projectionLength = Math.Max(0, Math.Min(projectionLength, lineLength)); return lineStart + lineDirection * projectionLength; } } }