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 BepuUtilities; using BepuUtilities.Memory; using CtrEditor.FuncionesBase; namespace CtrEditor.Simulacion { public class simBase { public BodyHandle BodyHandle { get; protected set; } public Simulation _simulation; protected bool _bodyCreated = false; // Bandera para saber si hemos creado un cuerpo // Constantes para las posiciones Z de los objetos 3D public const float zPos_Transporte = 0f; // Z de la parte baja public const float zAltura_Transporte = 0.1f; // Altura del transporte sobre zPos public const float zPos_Guia = 0.05f; // Z de la parte baja public const float zAltura_Guia = 0.20f; // Altura de la guía sobre zPos public const float zAltura_Barrera = zAltura_Guia; // Altura de la barrera sobre zPos public const float zPos_Barrera = zPos_Guia; // Z de la parte baja public const float zPos_Descarte = 0.1f; // Z de la parte baja // Constantes para configuración public const float zPos_Curve = zPos_Transporte; // Z de la parte baja de la curva public const float zAltura_Curve = zAltura_Transporte; // Altura de la curva (triángulos planos) public void RemoverBody() { // Solo intentar remover si realmente hemos creado un cuerpo antes if (_bodyCreated && _simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { _simulation.Bodies.Remove(BodyHandle); _bodyCreated = false; // Marcar como no creado después de remover } } public static float Min(float Value, float Min = 0.01f) { return Math.Max(Value, Min); } public static float GradosARadianes(float grados) { return grados * (float)Math.PI / 180f; } public static float RadianesAGrados(float radianes) { return radianes * 180f / (float)Math.PI; } public void SetPosition(float x, float y, float z = 0) { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); bodyReference.Pose.Position = new Vector3(x, y, z); } } public void SetPosition(Vector2 position) { // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba) // Mantener la coordenada Z actual para preservar la altura del objeto var currentPosition = GetPosition(); SetPosition(position.X, -position.Y, currentPosition.Z); } public void SetPosition(Vector3 position) { SetPosition(position.X, position.Y, position.Z); } public Vector3 GetPosition() { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); return bodyReference.Pose.Position; } return Vector3.Zero; } public void SetRotation(float angleZ) { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); // ✅ Convertir de grados a radianes para Quaternion.CreateFromAxisAngle bodyReference.Pose.Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angleZ)); } } public float GetRotationZ() { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); // Extraer el ángulo Z del quaternion var q = bodyReference.Pose.Orientation; return (float)Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z)); } return 0f; } } public class simTransporte : simBase { public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s) 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(Simulation simulation, List deferredActions, float width, float height, Vector2 topLeft, float angle = 0) { _simulation = simulation; _deferredActions = deferredActions; Width = width; Height = height; // Usar el nuevo método Create que maneja Top-Left correctamente Create(width, height, topLeft, angle); } public float Angle { get { return GetRotationZ(); } set { SetRotation(value); } } public new void SetPosition(float x, float y, float z = 0) { base.SetPosition(x, y, z); } public void SetSpeed(float speed) { Speed = speed; } /// /// Configura la velocidad del transporte en metros por segundo /// Valores positivos mueven en la dirección del transporte, negativos en dirección opuesta /// /// Velocidad en m/s (típicamente entre -5.0 y 5.0) public void SetTransportSpeed(float speedMeterPerSecond) { Speed = speedMeterPerSecond; } /// /// Detiene completamente el transporte /// public void StopTransport() { Speed = 0f; } /// /// Invierte la dirección del transporte manteniendo la misma velocidad /// public void ReverseTransport() { Speed = -Speed; } public void SetDimensions(float width, float height) { Width = width; Height = height; // Actualizar la forma del cuerpo existente if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var box = new Box(width, height, zAltura_Transporte); var shapeIndex = _simulation.Shapes.Add(box); _simulation.Bodies.SetShape(BodyHandle, shapeIndex); } } public void Create(float width, float height, Vector2 topLeft, float angle = 0) { // Calcular el offset del centro desde Top-Left (sin rotación) var offsetX = width / 2f; var offsetY = height / 2f; // Rotar el offset alrededor del Top-Left usando el ángulo // ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin var angleRadians = GradosARadianes(angle); var cos = (float)Math.Cos(angleRadians); var sin = (float)Math.Sin(angleRadians); var rotatedOffsetX = offsetX * cos - offsetY * sin; var rotatedOffsetY = offsetX * sin + offsetY * cos; // Calcular nueva posición del centro manteniendo Top-Left fijo var centerX = topLeft.X + rotatedOffsetX; var centerY = topLeft.Y + rotatedOffsetY; // Convertir a 3D e invertir Y - Transportes en Z = Depth/2 para estar sobre el suelo var center3D = new Vector3(centerX, -centerY, zAltura_Transporte / 2f + zPos_Transporte); Create(width, height, center3D, -angle); // ✅ Invertir ángulo cuando se invierte Y } public void Create(float width, float height, Vector3 position, float angle = 0) { RemoverBody(); var box = new Box(width, height, zAltura_Transporte); var shapeIndex = _simulation.Shapes.Add(box); var bodyDescription = BodyDescription.CreateKinematic( new RigidPose(position, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angle))), new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(0.01f) ); BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; // Marcar que hemos creado un cuerpo } } public class simBarrera : simBase { public float Distancia; public bool LuzCortada; public bool LuzCortadaNeck; public bool DetectNeck; public List ListSimBotellaContact; float _height; private List _deferredActions; public float Width { get; set; } public float Height { get; set; } public simBarrera(Simulation simulation, List deferredActions, float width, float height, Vector2 topLeft, float angle = 0, bool detectectNeck = false) { _simulation = simulation; _deferredActions = deferredActions; Width = width; Height = height; _height = height; DetectNeck = detectectNeck; ListSimBotellaContact = new List(); // Usar el nuevo método Create que maneja Top-Left correctamente Create(width, height, topLeft, angle, detectectNeck); } public float Angle { get { return GetRotationZ(); } set { SetRotation(value); } } public new void SetPosition(float x, float y, float z = 0) { base.SetPosition(x, y, z); } public void SetDimensions(float width, float height) { _height = height; // Actualizar la forma del cuerpo existente if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var box = new Box(width, height, zAltura_Barrera); // Altura de 20cm para detectar botellas var shapeIndex = _simulation.Shapes.Add(box); _simulation.Bodies.SetShape(BodyHandle, shapeIndex); } } public void Create(float width, float height, Vector2 topLeft, float angle = 0, bool detectectNeck = false) { // Calcular el offset del centro desde Top-Left (sin rotación) var offsetX = width / 2f; var offsetY = height / 2f; // Rotar el offset alrededor del Top-Left usando el ángulo // ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin var angleRadians = GradosARadianes(angle); var cos = (float)Math.Cos(angleRadians); var sin = (float)Math.Sin(angleRadians); var rotatedOffsetX = offsetX * cos - offsetY * sin; var rotatedOffsetY = offsetX * sin + offsetY * cos; // Calcular nueva posición del centro manteniendo Top-Left fijo var centerX = topLeft.X + rotatedOffsetX; var centerY = topLeft.Y + rotatedOffsetY; // Convertir a 3D e invertir Y - Sensores centrados en z=0.1 (rango 0-0.2) para detectar botellas var center3D = new Vector3(centerX, -centerY, zPos_Barrera + zAltura_Barrera / 2f); Create(width, height, center3D, -angle, detectectNeck); // ✅ Invertir ángulo cuando se invierte Y } public void Create(float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false) { RemoverBody(); // Crear box con altura de 0.2m (desde z=0 hasta z=0.2) para funcionar como detector var box = new Box(width, height, zAltura_Barrera); // Altura de 20cm para detectar botellas completas var shapeIndex = _simulation.Shapes.Add(box); // Crear como SENSOR (Kinematic con speculative margin 0 para detección pura) // Configurar actividad para NUNCA dormir - los sensores deben estar siempre activos var activityDescription = new BodyActivityDescription(-1f); // -1 = nunca dormir var bodyDescription = BodyDescription.CreateKinematic( new RigidPose(position, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angle))), new CollidableDescription(shapeIndex, 0f), // Speculative margin 0 para sensores activityDescription ); BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; // Marcar que hemos creado un cuerpo } } public class simGuia : simBase { private List _deferredActions; // Propiedades para acceder a las dimensiones del objeto WPF public float GuideThickness { get; set; } // Espesor de la guía public simGuia(Simulation simulation, List deferredActions, float width, float height, Vector2 topLeft, float angle) { _simulation = simulation; _deferredActions = deferredActions; GuideThickness = height; Create(width, height, topLeft, angle); } public void Create(float width, float height, Vector2 topLeft, float angle) { RemoverBody(); // Calcular el offset del centro desde Top-Left (sin rotación) var offsetX = width / 2f; var offsetY = height / 2f; // Rotar el offset alrededor del Top-Left usando el ángulo // ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin var angleRadians = GradosARadianes(angle); var cos = (float)Math.Cos(angleRadians); var sin = (float)Math.Sin(angleRadians); var rotatedOffsetX = offsetX * cos - offsetY * sin; var rotatedOffsetY = offsetX * sin + offsetY * cos; // Calcular nueva posición del centro manteniendo Top-Left fijo var centerX = topLeft.X + rotatedOffsetX; var centerY = topLeft.Y + rotatedOffsetY; // Convertir a 3D e invertir Y - Guías más altas que transportes var center3D = new Vector3(centerX, -centerY, zAltura_Guia / 2 + zPos_Guia); // Crear el Box con las dimensiones dadas - más alto para contener botellas var box = new Box(width, GuideThickness, zAltura_Guia); var shapeIndex = _simulation.Shapes.Add(box); // ✅ Invertir ángulo cuando se invierte Y para coherencia de rotación var orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(-angle)); var bodyDescription = BodyDescription.CreateKinematic( new RigidPose(center3D, orientation), new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(0.01f) ); BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; } // Método para actualizar las propiedades desde el objeto WPF public void UpdateProperties(float ancho, float altoGuia, float angulo) { // Nota: ancho y angulo se ignoran ya que se calculan automáticamente desde start->end GuideThickness = altoGuia; } } public class simBotella : simBase { public float Radius; public float Height; // Altura para la visualización del cilindro en Helix private float _mass; public bool Descartar = false; public int isOnTransports; public List ListOnTransports; public bool isRestricted; public bool isNoMoreRestricted; public float OriginalMass; public simTransporte ConveyorRestrictedTo; public float OverlapPercentage; public float _neckRadius; public bool isOnBrakeTransport; // Nueva propiedad para marcar si está en un transporte con freno public simTransporte CurrentBrakeTransport { get; set; } // ✅ NUEVA: Referencia al transporte de freno actual private List _deferredActions; public simBotella(Simulation simulation, List deferredActions, float diameter, Vector2 position, float mass, float neckRadius = 0) { _simulation = simulation; _deferredActions = deferredActions; Radius = diameter / 2f; Height = diameter; // Altura igual al diámetro para mantener proporciones similares _mass = mass; OriginalMass = mass; _neckRadius = neckRadius; ListOnTransports = new List(); // Convertir Vector2 a Vector3 con Z=0.2 (altura estándar para botellas) // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba) var position3D = new Vector3(position.X, -position.Y, Radius + zPos_Transporte + zAltura_Transporte); Create(position3D); } public float CenterX { get { return GetPosition().X; } set { var pos = GetPosition(); SetPosition(value, pos.Y, pos.Z); } } public float CenterY { get { // Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo) return -GetPosition().Y; } set { var pos = GetPosition(); // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba) SetPosition(pos.X, -value, pos.Z); } } public Vector2 Center { get { var pos3D = GetPosition(); // Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo) return new Vector2(pos3D.X, -pos3D.Y); } set { // Mantener la Z actual, solo cambiar X, Y var currentPos = GetPosition(); // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba) SetPosition(value.X, -value.Y, currentPos.Z); } } public float Mass { get { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); return 1f / bodyReference.LocalInertia.InverseMass; } return _mass; } set { _mass = value; if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { // Usar esfera simple - sin complejidad de inercia personalizada var sphere = new Sphere(Radius); var inertia = sphere.ComputeInertia(value); _simulation.Bodies.SetLocalInertia(BodyHandle, inertia); } } } private void Create(Vector3 position) { RemoverBody(); // Usar ESFERA en BEPU para simplicidad matemática y eficiencia var sphere = new Sphere(Radius); var shapeIndex = _simulation.Shapes.Add(sphere); // Inercia estándar de esfera - sin complejidad adicional var inertia = sphere.ComputeInertia(_mass); // NUNCA DORMIR - Valor negativo significa que las botellas JAMÁS entran en sleep mode // Esto es crítico para detección continua de barreras, transportes y descartes var activityDescription = new BodyActivityDescription(-1f); // -1 = NUNCA dormir var bodyDescription = BodyDescription.CreateDynamic( new RigidPose(position), new BodyVelocity(), inertia, // Inercia estándar de esfera new CollidableDescription(shapeIndex, 0.01f), activityDescription ); BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; // Marcar que hemos creado un cuerpo } public void SetDiameter(float diameter) { Radius = diameter / 2f; Height = diameter; // Mantener altura igual al diámetro para proporciones consistentes // Actualizar la forma del cuerpo existente if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var sphere = new Sphere(Radius); var shapeIndex = _simulation.Shapes.Add(sphere); _simulation.Bodies.SetShape(BodyHandle, shapeIndex); } } public void SetHeight(float height) { Height = height; // Actualizar la forma del cuerpo existente if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var sphere = new Sphere(Radius); var shapeIndex = _simulation.Shapes.Add(sphere); _simulation.Bodies.SetShape(BodyHandle, shapeIndex); } } public void SetMass(float mass) { Mass = mass; } /// /// Limita la rotación de la botella solo al plano XY (siempre "de pie") /// Esto simplifica enormemente la simulación y es más realista para botellas /// public void LimitRotationToXYPlane() { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); // Extraer solo la rotación en Z (plano XY) y eliminar las rotaciones en X e Y var currentOrientation = bodyReference.Pose.Orientation; // Convertir a ángulo en Z solamente var rotationZ = GetRotationZ(); // Crear nueva orientación solo con rotación en Z (botella siempre "de pie") var correctedOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, rotationZ); // Aplicar la orientación corregida bodyReference.Pose.Orientation = correctedOrientation; // También limitar la velocidad angular a solo rotación en Z var angularVelocity = bodyReference.Velocity.Angular; bodyReference.Velocity.Angular = new Vector3(0, 0, angularVelocity.Z); } } public void ApplyLinearVelocity(Vector3 velocity) { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); bodyReference.Velocity.Linear = velocity; } } public Vector3 GetLinearVelocity() { if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) { var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); return bodyReference.Velocity.Linear; } return Vector3.Zero; } public void CenterFixtureOnConveyor() { // Implementar lógica de centrado si es necesario } public bool IsOnAnyTransport() { return isOnTransports > 0; } /// /// ✅ NUEVO: Método de seguridad para restaurar la masa original si la botella tiene masa aumentada /// public void RestoreOriginalMassIfNeeded() { if (OriginalMass > 0) // Solo si hay masa original guardada { try { SetMass(OriginalMass); OriginalMass = 0; // Limpiar para evitar confusión isOnBrakeTransport = false; CurrentBrakeTransport = null; } catch (Exception ex) { // Error restoring original mass - continue } } } } public class simCurve : simBase { private float _innerRadius; private float _outerRadius; private float _startAngle; private float _endAngle; public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s) private List _deferredActions; internal List _triangleBodyHandles; // Lista de todos los triángulos que componen la curva private List> _originalTriangles; // ✅ NUEVO: Triángulos originales para visualización debug public float InnerRadius => _innerRadius; public float OuterRadius => _outerRadius; public float StartAngle => _startAngle; public float EndAngle => _endAngle; /// /// ✅ NUEVO: Expone los triángulos originales para visualización debug /// public List> GetOriginalTriangles() => _originalTriangles ?? new List>(); public simCurve(Simulation simulation, List deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0) { _simulation = simulation; _deferredActions = deferredActions; _innerRadius = innerRadius; _outerRadius = outerRadius; // ✅ SENTIDO HORARIO: Convertir ángulos para que vayan en sentido horario _startAngle = GradosARadianes(startAngle); _endAngle = GradosARadianes(endAngle); _triangleBodyHandles = new List(); _originalTriangles = new List>(); // ✅ NUEVO: Inicializar lista de triángulos // Crear la curva con los ángulos que definen el sector Create(innerRadius, outerRadius, startAngle, endAngle, topLeft, 0); } public void SetSpeed(float speed) { Speed = -speed; } public void UpdateCurve(float innerRadius, float outerRadius, float startAngle, float endAngle) { _innerRadius = innerRadius; _outerRadius = outerRadius; _startAngle = GradosARadianes(startAngle); _endAngle = GradosARadianes(endAngle); // Recrear la curva con nuevos parámetros manteniendo posición actual var currentPosition = GetPosition(); Create(currentPosition); } public new void RemoverBody() { // Remover todos los triángulos de la curva if (_triangleBodyHandles != null && _simulation != null) { foreach (var triangleHandle in _triangleBodyHandles) { if (_simulation.Bodies.BodyExists(triangleHandle)) { _simulation.Bodies.Remove(triangleHandle); } } _triangleBodyHandles.Clear(); } // ✅ NUEVO: Limpiar también los triángulos originales if (_originalTriangles != null) { _originalTriangles.Clear(); } // Remover el cuerpo principal (si existe) base.RemoverBody(); } public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0) { // ✅ CORRIGIDO: Como Aether - startAngle y endAngle definen directamente el sector // No hay rotación separada del objeto // Actualizar parámetros internos _innerRadius = innerRadius; _outerRadius = outerRadius; _startAngle = GradosARadianes(startAngle); _endAngle = GradosARadianes(endAngle); // Calcular el offset del centro desde Top-Left // Para curvas, el "tamaño" es el diámetro del radio exterior var curveSize = outerRadius * 2f; var offsetX = curveSize / 2f; var offsetY = curveSize / 2f; // Calcular nueva posición del centro (sin rotación - el sector ya está definido por los ángulos) var centerX = topLeft.X + offsetX; var centerY = topLeft.Y + offsetY; // Convertir a 3D e invertir Y - Curvas en el mismo nivel que transportes var center3D = new Vector3(centerX, -centerY, zAltura_Curve / 2f + zPos_Curve); Create(center3D); // Sin rotación adicional } private void Create(Vector3 position) { RemoverBody(); // Crear triángulos para la curva - los ángulos ya definen la forma correcta var triangles = CreateTriangulatedCurve(_innerRadius, _outerRadius, -_startAngle, -_endAngle); // ✅ NUEVO: Almacenar triángulos originales para visualización debug _originalTriangles = new List>(triangles); foreach (var triangle in triangles) { CreateTriangleBody(triangle, position); } // Crear un cuerpo principal invisible para referencia de posición CreateMainBody(position); } private void CreateMainBody(Vector3 position) { // Crear un cuerpo principal muy pequeño e invisible solo para referencia var smallBox = new Box(0.01f, 0.01f, 0.01f); var shapeIndex = _simulation.Shapes.Add(smallBox); var bodyDescription = BodyDescription.CreateKinematic( new RigidPose(position), // Sin rotación - la forma ya está definida por los ángulos new CollidableDescription(shapeIndex, 0f), // Sin speculative margin new BodyActivityDescription(0.01f) ); BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; } private void CreateTriangleBody(List triangle, Vector3 basePosition) { if (triangle.Count != 3) return; try { // Calcular centro y dimensiones del triángulo var center = (triangle[0] + triangle[1] + triangle[2]) / 3f; var worldCenter = center + basePosition; // Calcular dimensiones aproximadas del triángulo para crear una caja plana float minX = Math.Min(Math.Min(triangle[0].X, triangle[1].X), triangle[2].X); float maxX = Math.Max(Math.Max(triangle[0].X, triangle[1].X), triangle[2].X); float minY = Math.Min(Math.Min(triangle[0].Y, triangle[1].Y), triangle[2].Y); float maxY = Math.Max(Math.Max(triangle[0].Y, triangle[1].Y), triangle[2].Y); float width = Math.Max(maxX - minX, 0.01f); float height = Math.Max(maxY - minY, 0.01f); float thickness = zAltura_Curve; // Altura muy pequeña para triángulos planos // Crear una caja plana que aproxime el triángulo var box = new Box(width, height, thickness); var shapeIndex = _simulation.Shapes.Add(box); // Crear como cuerpo estático sólido (NO sensor) var activityDescription = new BodyActivityDescription(0.01f); var bodyDescription = BodyDescription.CreateKinematic( new RigidPose(worldCenter), new CollidableDescription(shapeIndex, 0.1f), // Speculative margin normal activityDescription ); var triangleHandle = _simulation.Bodies.Add(bodyDescription); _triangleBodyHandles.Add(triangleHandle); } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[simCurve] Error creating triangle: {ex.Message}"); } } private const float SegmentationFactor = 32f / 3f; private const int MinSegments = 8; private const int MaxSegments = 64; private List> CreateTriangulatedCurve(float innerRadius, float outerRadius, float startAngle, float endAngle) { var triangles = new List>(); // Calcular número de segmentos basado en el tamaño del arco float arcLength = Math.Abs(endAngle - startAngle) * ((innerRadius + outerRadius) / 2f); int segments = (int)(arcLength * SegmentationFactor); segments = Math.Max(MinSegments, Math.Min(segments, MaxSegments)); // ✅ SENTIDO HORARIO: Intercambiar ángulos para que vaya en sentido horario float fromAngle = endAngle; // Empezar desde el ángulo final float toAngle = startAngle; // Terminar en el ángulo inicial float angleStep = (toAngle - fromAngle) / segments; // Generar vértices para los arcos interior y exterior var innerPoints = new Vector3[segments + 1]; var outerPoints = new Vector3[segments + 1]; for (int i = 0; i <= segments; i++) { float angle = fromAngle + i * angleStep; float cosAngle = (float)Math.Cos(angle); float sinAngle = (float)Math.Sin(angle); // Triángulos planos en el plano XY (Z = 0 relativo) innerPoints[i] = new Vector3(innerRadius * cosAngle, innerRadius * sinAngle, 0); outerPoints[i] = new Vector3(outerRadius * cosAngle, outerRadius * sinAngle, 0); } // Crear triángulos for (int i = 0; i < segments; i++) { // Primer triángulo del sector var upperTriangle = new List { innerPoints[i], outerPoints[i], outerPoints[i + 1] }; triangles.Add(upperTriangle); // Segundo triángulo del sector var lowerTriangle = new List { innerPoints[i], outerPoints[i + 1], innerPoints[i + 1] }; triangles.Add(lowerTriangle); } return triangles; } /// /// Calcula la dirección tangencial para aplicar fuerzas de curva /// public Vector3 GetTangentialDirection(Vector3 bottlePosition) { try { var curvePosition = GetPosition(); var centerToBottle = new Vector2(bottlePosition.X - curvePosition.X, bottlePosition.Y - curvePosition.Y); if (centerToBottle.Length() < 0.001f) return Vector3.Zero; // Vector tangente (perpendicular al radio) en el plano XY var tangent = new Vector3(-centerToBottle.Y, centerToBottle.X, 0); // Normalizar if (tangent.Length() > 0.001f) { tangent = Vector3.Normalize(tangent); } // Ajustar dirección según la velocidad (signo) if (Speed < 0) tangent = -tangent; return tangent; } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[simCurve] Error calculating tangential direction: {ex.Message}"); return Vector3.Zero; } } } public class simDescarte : simBase { private float _radius; private List _deferredActions; public List ListSimBotellaContact; public simDescarte(Simulation simulation, List deferredActions, float diameter, Vector2 position) { _simulation = simulation; _deferredActions = deferredActions; _radius = diameter / 2f; ListSimBotellaContact = new List(); // Convertir Vector2 a Vector3 con Z=0.3 (altura para detectar botellas) // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba) var position3D = new Vector3(position.X, -position.Y, zPos_Descarte + _radius); Create(position3D); } public float Radius { get { return _radius; } set { _radius = Math.Max(value, 0.01f); // Mínimo 1cm // Recrear el cuerpo con el nuevo radio var currentPos = GetPosition(); Create(currentPos); } } public void SetDiameter(float diameter) { Radius = diameter / 2f; } public float GetDiameter() { return _radius * 2f; } public void Create(Vector2 position) { // Convertir Vector2 a Vector3 con Z=0.3 (altura para detectar botellas) // Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba) var position3D = new Vector3(position.X, -position.Y, zPos_Descarte + _radius); Create(position3D); } private void Create(Vector3 position) { RemoverBody(); // Crear esfera sensor para detección var sphere = new BepuPhysics.Collidables.Sphere(_radius); var shapeIndex = _simulation.Shapes.Add(sphere); // Crear como SENSOR (Kinematic con speculative margin 0 para detección pura) // Configurar actividad para NUNCA dormir - los sensores deben estar siempre activos var activityDescription = new BodyActivityDescription(-1f); // -1 = nunca dormir var bodyDescription = BodyDescription.CreateKinematic( new RigidPose(position), new CollidableDescription(shapeIndex, 0f), // Speculative margin 0 para sensores activityDescription ); BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; // Marcar que hemos creado un cuerpo } } // 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) { return true; } 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 de materiales físicos por defecto pairMaterial = new PairMaterialProperties { FrictionCoefficient = 0.6f, // Fricción moderada MaximumRecoveryVelocity = 2f, // Velocidad máxima de recuperación SpringSettings = new SpringSettings(60, 4) // Rigidez y amortiguamiento para colisiones sólidas }; // Detectar contactos que involucren botellas para aplicar propiedades especiales if (_simulationManager != null) { var botellaA = GetBotellaFromCollidable(pair.A); var botellaB = GetBotellaFromCollidable(pair.B); var transportA = GetTransportFromCollidable(pair.A); var transportB = GetTransportFromCollidable(pair.B); var barreraA = GetBarreraFromCollidable(pair.A); var barreraB = GetBarreraFromCollidable(pair.B); var curveA = GetCurveFromCollidable(pair.A); var curveB = GetCurveFromCollidable(pair.B); var botella = botellaA ?? botellaB; // Detectar contactos con descarte var descarteA = GetDescarteFromCollidable(pair.A); var descarteB = GetDescarteFromCollidable(pair.B); // IMPORTANTE: Si hay una barrera involucrada, NO generar contacto físico if (barreraA != null || barreraB != null) { // Ya no necesitamos registrar contactos - usamos detección geométrica pura // Las barreras son sensores completamente virtuales // NO generar contacto físico para barreras - son sensores puros return false; } // Si hay un descarte involucrado, NO generar contacto físico pero registrar para eliminación if (descarteA != null || descarteB != null) { var descarte = descarteA ?? descarteB; // Registrar la botella para eliminación si hay contacto if (botella != null) { _simulationManager.RegisterDescarteContact(descarte, botella); } // NO generar contacto físico para descartes - son sensores puros return false; } // Si hay una botella en contacto (pero no con barrera), aplicar mayor fricción para estabilidad if (botella != null) { // Aumentar mucho la fricción para botellas (más estables) pairMaterial.FrictionCoefficient = 0.9f; // Reducir la velocidad de recuperación para evitar rebotes excesivos pairMaterial.MaximumRecoveryVelocity = 1f; // Hacer las colisiones más suaves y estables pairMaterial.SpringSettings = new SpringSettings(80, 6); } // Si hay un transporte y una botella en contacto if ((transportA != null || transportB != null) && botella != null) { var transporte = transportA ?? transportB; // ✅ NUEVA LÓGICA: Detectar contactos con transporte de freno PRIMERO if (transporte.isBrake) { // FRICCIÓN EXTREMA para transporte con freno - simula frenos mecánicos pairMaterial.FrictionCoefficient = 5.0f; // Fricción extremadamente alta // Reducir velocidad de recuperación para evitar rebotes pairMaterial.MaximumRecoveryVelocity = 0.1f; // Hacer el contacto más rígido para mejor control pairMaterial.SpringSettings = new SpringSettings(200, 20); // ✅ APLICAR FUERZAS DE FRENADO DIRECTAMENTE EN EL MANIFOLD ApplyBrakeForces(ref manifold, botella, transporte); // Registrar contacto con transporte de freno _simulationManager.RegisterBrakeTransportContact(botella, transporte); } else { // ✅ NUEVA LÓGICA: Aplicar fuerzas de transporte DIRECTAMENTE en el manifold if (Math.Abs(transporte.Speed) > 0.001f) // Solo si hay velocidad significativa { pairMaterial.FrictionCoefficient = 1.2f; // Fricción muy alta // ✅ APLICAR FUERZAS DE TRANSPORTE DIRECTAMENTE EN EL MANIFOLD ApplyTransportForces(ref manifold, botella, transporte); } } } // Si hay una curva y una botella en contacto if ((curveA != null || curveB != null) && botella != null) { var curve = curveA ?? curveB; // Solo aplicar si hay velocidad significativa en la curva if (Math.Abs(curve.Speed) > 0.001f) { pairMaterial.FrictionCoefficient = 1.0f; // Fricción alta para curvas // Aplicar fuerzas de curva directamente en el manifold ApplyCurveForces(ref manifold, botella, curve); } } } 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; } private simBarrera GetBarreraFromCollidable(CollidableReference collidable) { if (_simulationManager?.simulation != null) { if (collidable.Mobility == CollidableMobility.Kinematic) { var bodyHandle = collidable.BodyHandle; var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle); return simBase as simBarrera; } } return null; } 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; // ✅ NUEVO: Buscar específicamente en los triángulos de las curvas return _simulationManager.GetCurveFromTriangleBodyHandle(bodyHandle); } } return null; } /// /// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado directamente en el manifold /// private void ApplyBrakeForces(ref TManifold manifold, simBotella botella, simTransporte transporte) where TManifold : unmanaged, IContactManifold { try { if (_simulationManager?.simulation == null) return; var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle); var transporteBody = _simulationManager.simulation.Bodies.GetBodyReference(transporte.BodyHandle); // Calcular velocidad relativa var relativeVelocity = botellaBody.Velocity.Linear - transporteBody.Velocity.Linear; // Si la botella se mueve más rápido que el transporte, aplicar fuerza de frenado var transportDirection = GetTransportDirection(transporte); var velocityInTransportDirection = Vector3.Dot(relativeVelocity, transportDirection); // if (Math.Abs(velocityInTransportDirection) > Math.Abs(transporte.Speed / 18.5f)) // { // Calcular fuerza de frenado proporcional a la diferencia de velocidad var targetSpeed = transporte.Speed / 18.5f; var currentSpeed = Vector3.Dot(botellaBody.Velocity.Linear, transportDirection); var speedDifference = currentSpeed - targetSpeed; // Aplicar impulso de frenado - esto se hace DURANTE la resolución de colisiones var brakeImpulse = -transportDirection * speedDifference * botella.Mass * 0.5f; botellaBody.ApplyLinearImpulse(brakeImpulse); // } } catch (Exception ex) { // Error applying brake forces during collision resolution } } /// /// ✅ NUEVO MÉTODO: Obtener dirección del transporte /// private Vector3 GetTransportDirection(simTransporte transporte) { var angle = transporte.GetRotationZ(); return new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0); } /// /// ✅ NUEVO MÉTODO: Aplicar fuerzas de transporte normal directamente en el manifold /// private void ApplyTransportForces(ref TManifold manifold, simBotella botella, simTransporte transporte) where TManifold : unmanaged, IContactManifold { try { if (_simulationManager?.simulation == null) return; var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle); var transporteBody = _simulationManager.simulation.Bodies.GetBodyReference(transporte.BodyHandle); // Calcular la dirección del transporte var transportDirection = GetTransportDirection(transporte); // Calcular la fuerza proporcional a la velocidad deseada var targetVelocity = transportDirection * transporte.Speed / 18.5f; var currentVelocity = botellaBody.Velocity.Linear; // Solo aplicar fuerza en el plano horizontal (X-Y) var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0); // Para transportes con freno (si aplica), limitar la velocidad máxima de la botella bool isOnBrakeTransport = botella.isOnBrakeTransport; if (isOnBrakeTransport) { var transportSpeed = Math.Abs(transporte.Speed / 18.5f); var currentSpeed = horizontalVelocity.Length(); // Si la botella va más rápido que el transporte, limitar su velocidad if (currentSpeed > transportSpeed * 1.1f) // 10% de tolerancia { var limitedVelocity = Vector3.Normalize(horizontalVelocity) * transportSpeed; botellaBody.Velocity.Linear = new Vector3(limitedVelocity.X, limitedVelocity.Y, currentVelocity.Z); horizontalVelocity = limitedVelocity; } } var velocityDiff = targetVelocity - horizontalVelocity; // Aplicar una fuerza proporcional a la diferencia de velocidad var forceMultiplier = isOnBrakeTransport ? 3.0f : 1.0f; // 3x más fuerte para botellas con guías var baseForceMagnitude = velocityDiff.Length() * botella.Mass * 10f; // Factor base var forceMagnitude = baseForceMagnitude * forceMultiplier; // Aplicar multiplicador if (forceMagnitude > 0.01f) { var maxForce = botella.Mass * 50f * forceMultiplier; // Límite también escalado var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce); // Aplicar impulso directamente durante la resolución de colisiones botellaBody.ApplyLinearImpulse(force * (1f / 60f)); // Simular deltaTime típico } } catch (Exception ex) { // Error applying transport forces during collision resolution } } /// /// ✅ SIMPLIFICADO: Aplicar fuerzas de curva directamente en el manifold /// Si hay colisión física, la botella está sobre la curva - no necesita cálculos adicionales /// private void ApplyCurveForces(ref TManifold manifold, simBotella botella, simCurve curve) where TManifold : unmanaged, IContactManifold { try { if (_simulationManager?.simulation == null) return; var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle); var botellaPosition = botellaBody.Pose.Position; // ✅ SIMPLIFICADO: Si hay colisión física, aplicar fuerzas directamente // Obtener la dirección tangencial var tangentialDirection = curve.GetTangentialDirection(botellaPosition); if (tangentialDirection.Length() > 0.001f) { // Calcular velocidad objetivo var speedMetersPerSecond = Math.Abs(curve.Speed) / 18.5f; // Convertir a m/s var targetVelocity = tangentialDirection * speedMetersPerSecond; // Obtener velocidad actual var currentVelocity = botellaBody.Velocity.Linear; var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0); // Usar directamente la dirección tangencial (sin interpolación compleja) var newDirection = tangentialDirection; // Usar la velocidad mayor entre la actual y la de la curva para continuidad var currentSpeed = horizontalVelocity.Length(); var targetSpeed = Math.Max(currentSpeed, speedMetersPerSecond); // Calcular nueva velocidad var newVelocity = newDirection * targetSpeed; var velocityDiff = newVelocity - horizontalVelocity; // Aplicar impulso directamente (sin factor de overlap - la colisión es suficiente) var forceMagnitude = velocityDiff.Length() * botella.Mass * 8f; if (forceMagnitude > 0.01f) { var maxForce = botella.Mass * 40f; var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce); botellaBody.ApplyLinearImpulse(force * (1f / 60f)); // Simular deltaTime típico } } } catch (Exception ex) { System.Diagnostics.Debug.WriteLine($"[simCurve] Error applying curve forces: {ex.Message}"); } } 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; // ✅ NUEVA FUNCIONALIDAD: Aplicar fuerzas de frenado durante la integración if (_simulationManager != null) { ApplyBrakeForcesInIntegration(bodyIndices, ref velocity, dt, workerIndex); } // 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! } /// /// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado durante la integración de velocidades /// ✅ SIMPLIFICADO: Solo aplicar amortiguamiento extra para botellas en transportes con freno /// private void ApplyBrakeForcesInIntegration(Vector bodyIndices, ref BodyVelocityWide velocity, Vector dt, int workerIndex) { try { // ✅ SIMPLIFICADO: Aplicar amortiguamiento extra para efectos de frenado // Esto es más compatible con la arquitectura vectorizada de BEPU // Amortiguamiento adicional para simular efecto de frenado var brakeDampingWide = Vector.One * 0.95f; // Más agresivo que el damping normal // Aplicar solo si tenemos referencia al simulation manager if (_simulationManager != null) { // El amortiguamiento extra se aplica globalmente, // las fuerzas específicas se manejan en ApplyBrakeForces del NarrowPhaseCallbacks velocity.Linear *= brakeDampingWide; } } catch (Exception ex) { // Error during brake force integration } } } 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; // Sistema de contactos de transporte para aplicar fuerzas private Dictionary _transportContacts; // Sistema de contactos de barrera para detección de paso private Dictionary> _barreraContacts; // Sistema de contactos de descarte para marcar botellas para eliminación private Dictionary> _descarteContacts; // Lista de botellas marcadas para eliminación private HashSet _botellasParaEliminar; // Sistema de contactos de transporte con freno para marcar botellas private Dictionary _brakeTransportContacts; 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)); } /// /// ✅ NUEVO: Obtiene una simCurve si el BodyHandle pertenece a uno de sus triángulos /// public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle) { // Buscar en todas las curvas si alguna contiene este BodyHandle en sus triángulos var curves = Cuerpos.OfType(); foreach (var curve in curves) { if (curve._triangleBodyHandles != null && curve._triangleBodyHandles.Contains(bodyHandle)) { return curve; } } // Si no se encuentra en triángulos, buscar en cuerpos principales como fallback var simBase = GetSimBaseFromBodyHandle(bodyHandle); return simBase as simCurve; } public SimulationManagerBEPU() { Cuerpos = new List(); _deferredActions = new List(); _transportContacts = new Dictionary(); _barreraContacts = new Dictionary>(); _descarteContacts = new Dictionary>(); _botellasParaEliminar = new HashSet(); _brakeTransportContacts = new Dictionary(); 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 { // ✅ NUEVO: Restaurar masas originales de todas las botellas antes de limpiar foreach (var cuerpo in Cuerpos.OfType()) { try { cuerpo.RestoreOriginalMassIfNeeded(); } catch (Exception ex) { // Error restoring mass during clear - continue } } // Limpiar contactos primero para evitar referencias colgantes lock (_contactsLock) { _transportContacts.Clear(); _barreraContacts.Clear(); _descarteContacts.Clear(); _botellasParaEliminar.Clear(); _brakeTransportContacts.Clear(); } // Remover cuerpos de forma segura 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(); // 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() { stopwatch.Start(); stopwatch_last = stopwatch.Elapsed.TotalMilliseconds; } public void Step() { try { var currentTime = stopwatch.Elapsed.TotalMilliseconds; var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0); stopwatch_last = currentTime; // Validar deltaTime para evitar valores problemáticos if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0) { deltaTime = 1f / 60f; // Fallback a 60 FPS } // Limitar deltaTime a 100ms máximo para evitar saltos tras pausas del debugger const float maxDeltaTime = 0.1f; // 100ms if (deltaTime > maxDeltaTime) { deltaTime = 1f / 60f; // Resetear a 60 FPS estándar si excede el límite } // Procesar acciones diferidas foreach (var action in _deferredActions) { action(); } _deferredActions.Clear(); // Validar que la simulación esté en buen estado antes del timestep if (simulation?.Bodies == null) { return; } // Validar que no hay cuerpos con handles inválidos 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); } } // Paso de simulación (genera las colisiones/contactos) var timestepValue = Math.Max(deltaTime, 1f / 120f); // Mínimo 120 FPS para mejor estabilidad try { simulation.Timestep(timestepValue); } catch (AccessViolationException ex) { // Limpiar contactos que podrían estar corruptos lock (_contactsLock) { // ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa _barreraContacts.Clear(); } throw; // Re-lanzar para debug } // ✅ ELIMINADO: ApplyTransportForces - Ahora se aplica directamente en ConfigureContactManifold // ApplyTransportForces(deltaTime); // ELIMINADO - Ahora en ConfigureContactManifold // Procesar contactos de transporte con freno para marcar/desmarcar botellas ProcessBrakeTransportContacts(); // Limitar rotaciones de botellas solo al plano XY (siempre "de pie") // Y GARANTIZAR que nunca entren en sleep mode foreach (var cuerpo in Cuerpos.OfType().ToList()) // ToList para evitar modificación durante iteración { try { if (simulation.Bodies.BodyExists(cuerpo.BodyHandle)) { cuerpo.LimitRotationToXYPlane(); // FORZAR que la botella esté siempre despierta - doble seguridad simulation.Awakener.AwakenBody(cuerpo.BodyHandle); } } catch (Exception ex) { // Error limiting rotation for bottle - continue } } // Procesar contactos de barrera para detección de paso ProcessBarreraContacts(); // Procesar contactos de descarte para eliminación de botellas ProcessDescarteContacts(); // Procesar sistema de limpieza para eliminar botellas debajo de transportes ProcessCleanupSystem(); // Limpiar contactos DESPUÉS de usarlos lock (_contactsLock) { // ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa _barreraContacts.Clear(); _descarteContacts.Clear(); _brakeTransportContacts.Clear(); } // Sincronizar con la visualización 3D solo si está habilitado if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } } catch (Exception ex) { // En caso de error crítico, limpiar contactos para evitar estado corrupto lock (_contactsLock) { // ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa _barreraContacts.Clear(); _brakeTransportContacts.Clear(); } } } public void Remove(simBase Objeto) { // ✅ NUEVO: Si es una botella con masa aumentada, restaurar masa original antes de eliminar if (Objeto is simBotella botella && botella.OriginalMass > 0) { try { botella.SetMass(botella.OriginalMass); botella.OriginalMass = 0; botella.isOnBrakeTransport = false; botella.CurrentBrakeTransport = null; } catch (Exception ex) { // Error restoring mass during removal - continue with removal } } Objeto.RemoverBody(); Cuerpos.Remove(Objeto); // Actualizar visualización 3D tras eliminar objeto if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } } public simBotella AddCircle(float diameter, Vector2 position, float mass) { var botella = new simBotella(simulation, _deferredActions, diameter, position, mass); Cuerpos.Add(botella); // Sincronizar con la visualización 3D 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); Cuerpos.Add(transporte); // Sincronizar con la visualización 3D if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return transporte; } public simBarrera AddBarrera(float width, float height, Vector2 position, float angle, bool detectarCuello) { var barrera = new simBarrera(simulation, _deferredActions, width, height, position, angle, detectarCuello); Cuerpos.Add(barrera); // Sincronizar con la visualización 3D 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); // Sincronizar con la visualización 3D if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return guia; } public simDescarte AddDescarte(float diameter, Vector2 position) { var descarte = new simDescarte(simulation, _deferredActions, diameter, position); Cuerpos.Add(descarte); // Sincronizar con la visualización 3D 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); Cuerpos.Add(curve); // Sincronizar con la visualización 3D if (Is3DUpdateEnabled) { Visualization3DManager?.SynchronizeWorld(); } return curve; } public void Dispose() { Clear(); simulation?.Dispose(); bufferPool?.Clear(); } /// /// Registra un contacto entre una botella y un transporte para aplicar fuerzas /// /// Botella en contacto /// Transporte en contacto public void RegisterTransportContact(simBotella botella, simTransporte transporte) { if (botella != null && transporte != null) { lock (_contactsLock) { _transportContacts[botella] = transporte; } } } /// /// Registra un contacto entre una botella y un transporte con freno para marcar la botella /// ✅ NUEVO: También aumenta la masa de la botella 10x para simular conexión mecánica /// /// Botella en contacto /// Transporte con freno en contacto public void RegisterBrakeTransportContact(simBotella botella, simTransporte transporte) { if (botella != null && transporte != null && transporte.isBrake) { lock (_contactsLock) { _brakeTransportContacts[botella] = transporte; // ✅ NUEVO: Mantener referencia directa en la botella botella.CurrentBrakeTransport = transporte; botella.ConveyorRestrictedTo = transporte; // ✅ NUEVO: Aumentar masa 10x para simular conexión mecánica con el transporte if (!botella.isOnBrakeTransport) // Solo si no estaba ya en un transporte con freno { // Guardar masa original si no está guardada ya if (botella.OriginalMass <= 0) { botella.OriginalMass = botella.Mass; } // Aumentar masa 10 veces para simular "enganche" mecánico botella.SetMass(botella.OriginalMass * 100f); } } } } /// /// Registra un contacto entre una barrera y una botella para detección de paso /// /// Barrera que detecta el paso /// Botella que está pasando public 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); } } } } /// /// Aplica fuerzas de transporte a las botellas en contacto /// /// Tiempo transcurrido desde el último paso private void ApplyTransportForces(float deltaTime) { try { // Validar deltaTime if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0) return; lock (_contactsLock) { // Crear copia de contactos para evitar modificación durante iteración var contactsCopy = new Dictionary(_transportContacts); foreach (var contact in contactsCopy) { try { var botella = contact.Key; var transporte = contact.Value; // Validar objetos if (botella == null || transporte == null || simulation?.Bodies == null) continue; // Verificar que ambos objetos aún existen if (!simulation.Bodies.BodyExists(botella.BodyHandle) || !simulation.Bodies.BodyExists(transporte.BodyHandle)) continue; // Solo aplicar si hay velocidad significativa if (Math.Abs(transporte.Speed) <= 0.001f) continue; // Determinar si es una botella en transporte con freno (guías laterales) bool isOnBrakeTransport = botella.isOnBrakeTransport; float forceMultiplier = isOnBrakeTransport ? 3.0f : 1.0f; // 3x más fuerte para botellas con guías // Aplicar fuerzas de movimiento (longitudinales) con control de velocidad ApplyLongitudinalForces(botella, transporte, deltaTime, forceMultiplier, isOnBrakeTransport); } catch (Exception ex) { // Error processing contact - continue } } } } catch (Exception ex) { // Critical error in ApplyTransport - continue } } /// /// Aplica fuerzas longitudinales para el movimiento del transporte con control de velocidad /// private void ApplyLongitudinalForces(simBotella botella, simTransporte transporte, float deltaTime, float forceMultiplier, bool isOnBrakeTransport) { try { // Calcular la dirección del transporte de forma segura var angle = transporte.GetRotationZ(); // Validar ángulo if (float.IsNaN(angle) || float.IsInfinity(angle)) return; var cos = (float)Math.Cos(angle); var sin = (float)Math.Sin(angle); // Validar valores trigonométricos if (float.IsNaN(cos) || float.IsNaN(sin)) return; // Vector de dirección del transporte (en el plano X-Y) var transportDirection = new Vector3(cos, sin, 0); // Calcular la fuerza proporcional a la velocidad deseada var targetVelocity = transportDirection * transporte.Speed / 18.5f; var currentVelocity = botella.GetLinearVelocity(); // Validar velocidades if (float.IsNaN(currentVelocity.X) || float.IsNaN(currentVelocity.Y) || float.IsNaN(currentVelocity.Z)) return; // Solo aplicar fuerza en el plano horizontal (X-Y) var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0); // Para transportes con freno, limitar la velocidad máxima de la botella if (isOnBrakeTransport) { var transportSpeed = Math.Abs(transporte.Speed / 18.5f); var currentSpeed = horizontalVelocity.Length(); // Si la botella va más rápido que el transporte, limitar su velocidad if (currentSpeed > transportSpeed * 1f) // 10% de tolerancia { var limitedVelocity = Vector3.Normalize(horizontalVelocity) * transportSpeed; botella.ApplyLinearVelocity(new Vector3(limitedVelocity.X, limitedVelocity.Y, currentVelocity.Z)); horizontalVelocity = limitedVelocity; } } var velocityDiff = targetVelocity - horizontalVelocity; // Validar diferencia de velocidad if (float.IsNaN(velocityDiff.X) || float.IsNaN(velocityDiff.Y) || float.IsNaN(velocityDiff.Z)) return; // Aplicar una fuerza proporcional a la diferencia de velocidad var baseForceMagnitude = velocityDiff.Length() * botella.Mass * 10f; // Factor base var forceMagnitude = baseForceMagnitude * forceMultiplier; // Aplicar multiplicador // Validar magnitud de fuerza if (float.IsNaN(forceMagnitude) || float.IsInfinity(forceMagnitude)) return; if (forceMagnitude > 0.01f) { var maxForce = botella.Mass * 50f * forceMultiplier; // Límite también escalado var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce); // Validar fuerza final if (float.IsNaN(force.X) || float.IsNaN(force.Y) || float.IsNaN(force.Z)) return; var bodyReference = simulation.Bodies.GetBodyReference(botella.BodyHandle); bodyReference.ApplyLinearImpulse(force * deltaTime); } } catch (Exception ex) { // Error in ApplyLongitudinalForces - continue } } /// /// Procesa contactos de transporte con freno para marcar/desmarcar botellas /// ✅ NUEVO: También gestiona el cambio de masa - restaura masa original al salir del transporte /// private void ProcessBrakeTransportContacts() { try { // Primero, recopilar botellas que estaban en transporte con freno para detectar cambios var todasLasBotellas = Cuerpos.OfType().ToList(); var botellasQueEstabanEnBrakeTransport = new HashSet(); foreach (var botella in todasLasBotellas) { if (botella != null && botella.isOnBrakeTransport) { botellasQueEstabanEnBrakeTransport.Add(botella); botella.isOnBrakeTransport = false; // Asumir que no están en contacto } } // Luego, marcar las botellas que SÍ están en contacto con transportes de freno lock (_contactsLock) { var contactsCopy = new Dictionary(_brakeTransportContacts); foreach (var contact in contactsCopy) { try { var botella = contact.Key; var transporte = contact.Value; // Validar objetos if (botella == null || transporte == null || simulation?.Bodies == null) continue; // Verificar que ambos objetos aún existen if (!simulation.Bodies.BodyExists(botella.BodyHandle) || !simulation.Bodies.BodyExists(transporte.BodyHandle)) continue; // Verificar que el transporte sigue teniendo freno activado if (transporte.isBrake) { botella.isOnBrakeTransport = true; // Quitar de la lista de botellas que estaban en brake transport botellasQueEstabanEnBrakeTransport.Remove(botella); } } catch (Exception ex) { // Error processing brake transport contact - continue } } } // ✅ NUEVO: Restaurar masa original de botellas que salieron del transporte con freno foreach (var botella in botellasQueEstabanEnBrakeTransport) { try { if (botella.OriginalMass > 0) // Solo si tenemos masa original guardada { botella.SetMass(botella.OriginalMass); botella.OriginalMass = 0; // Limpiar para evitar confusión botella.CurrentBrakeTransport = null; // Limpiar referencia } } catch (Exception ex) { // Error restoring original mass - continue } } } catch (Exception ex) { // Critical error in ProcessBrakeTransportContacts - continue } } /// /// Procesa todas las barreras usando detección geométrica pura /// Ya no depende de contactos físicos de BEPU - usa cálculo geométrico para ambos flags /// private void ProcessBarreraContacts() { try { // Obtener todas las barreras y botellas var barreras = Cuerpos.OfType().ToList(); var botellas = Cuerpos.OfType().ToList(); foreach (var barrera in barreras) { try { // Verificar que la barrera aún existe en la simulación if (barrera == null || !simulation?.Bodies?.BodyExists(barrera.BodyHandle) == true) continue; // Resetear flags barrera.LuzCortada = false; barrera.LuzCortadaNeck = false; barrera.Distancia = float.MaxValue; // Limpiar la lista de forma segura if (barrera.ListSimBotellaContact != null) { barrera.ListSimBotellaContact.Clear(); } // Calcular detección usando geometría pura para TODAS las botellas CalculateBarreraDetectionGeometric(barrera, botellas); } catch (Exception ex) { // Error processing barrera - continue } } // Limpiar contactos ya que no los usamos más lock (_contactsLock) { _barreraContacts.Clear(); } } catch (Exception ex) { // Critical error in ProcessBarrera - continue } } /// /// Calcula detección geométrica pura para una barrera específica contra todas las botellas /// Usa el mismo sistema geométrico para ambos flags: LuzCortada (radio completo) y LuzCortadaNeck (radio/2) /// /// Barrera que está detectando /// Lista de TODAS las botellas en la simulación private void CalculateBarreraDetectionGeometric(simBarrera barrera, List todasLasBotellas) { try { // Validaciones de seguridad if (barrera == null || todasLasBotellas == null || simulation?.Bodies == null) return; if (!simulation.Bodies.BodyExists(barrera.BodyHandle)) return; var barrierBody = simulation.Bodies[barrera.BodyHandle]; var barrierPosition = barrierBody.Pose.Position; var barrierOrientation = barrierBody.Pose.Orientation; // Validar valores de posición y orientación if (float.IsNaN(barrierPosition.X) || float.IsNaN(barrierPosition.Y) || float.IsNaN(barrierPosition.Z)) { return; } // Obtener las dimensiones de la barrera de forma segura var halfWidth = Math.Max(barrera.Width / 2f, 0.01f); // Mínimo 1cm float minDistance = float.MaxValue; var botellasDetectadas = new List(); // Procesar TODAS las botellas (no solo las en contacto físico) 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; } // CLAVE: Crear la línea del haz en el plano XY a la altura del centro de la botella var bottleZ = botellaPosition.Z; // Altura actual del centro de la botella // Puntos de la línea del haz en el plano XY a la altura de la botella var localStart = new Vector3(-halfWidth, 0, 0); var localEnd = new Vector3(halfWidth, 0, 0); // Transformar a coordenadas mundiales pero en el plano Z de la botella var worldStartXY = barrierPosition + Vector3.Transform(localStart, barrierOrientation); var worldEndXY = barrierPosition + Vector3.Transform(localEnd, barrierOrientation); // Ajustar Z para que esté en el plano de la botella worldStartXY = new Vector3(worldStartXY.X, worldStartXY.Y, bottleZ); worldEndXY = new Vector3(worldEndXY.X, worldEndXY.Y, bottleZ); // Calcular distancia desde el centro de la botella a la línea del haz var closestPoint = ProjectPointOntoLine(botellaPosition, worldStartXY, worldEndXY); var distance = Vector3.Distance(closestPoint, botellaPosition); // Validar distancia calculada if (float.IsNaN(distance) || float.IsInfinity(distance)) { continue; } // Actualizar distancia mínima if (distance < minDistance) { minDistance = distance; } // NUEVO: Verificar LuzCortada usando radio completo if (distance <= botella.Radius) { barrera.LuzCortada = true; botellasDetectadas.Add(botella); } // Verificar detección de cuello usando radio/2 (como antes) if (barrera.DetectNeck && botella.Radius > 0) { var neckRadius = botella.Radius / 2f; if (distance <= neckRadius) { barrera.LuzCortadaNeck = true; // No hacer break aquí - queremos procesar todas las botellas para LuzCortada } } } catch (Exception ex) { // Error processing bottle - continue } } // Asignar resultados de forma segura barrera.Distancia = minDistance == float.MaxValue ? 0f : minDistance; // Actualizar lista de botellas detectadas if (barrera.ListSimBotellaContact != null) { barrera.ListSimBotellaContact.AddRange(botellasDetectadas); } } catch (Exception ex) { // En caso de error, asignar valores seguros if (barrera != null) { barrera.Distancia = float.MaxValue; barrera.LuzCortada = false; barrera.LuzCortadaNeck = false; } } } /// /// 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)) { Remove(botella); } } catch (Exception ex) { // Error removing bottle - continue } } // Botellas eliminadas: {botellasAEliminar.Count} } catch (Exception ex) { // Critical error in RemoveMarkedBottles - continue } } /// /// 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; } } }