diff --git a/CtrEditor.csproj b/CtrEditor.csproj index 07894e4..5593f64 100644 --- a/CtrEditor.csproj +++ b/CtrEditor.csproj @@ -35,6 +35,7 @@ + @@ -84,6 +85,7 @@ + diff --git a/Documentation/BEPU Forces.cs b/Documentation/BEPU Forces.cs new file mode 100644 index 0000000..2d6de85 --- /dev/null +++ b/Documentation/BEPU Forces.cs @@ -0,0 +1,2616 @@ +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; + } + + } +} \ No newline at end of file diff --git a/ObjetosSim/Estaticos/ucDescarte.xaml.cs b/ObjetosSim/Estaticos/ucDescarte.xaml.cs index b4c7509..aea9f86 100644 --- a/ObjetosSim/Estaticos/ucDescarte.xaml.cs +++ b/ObjetosSim/Estaticos/ucDescarte.xaml.cs @@ -41,7 +41,7 @@ namespace CtrEditor.ObjetosSim partial void OnDiametroChanged(float value) { - SimGeometria?.SetDiameter(Diametro); + ActualizarGeometrias(); } public Vector2 GetCentro() @@ -73,8 +73,15 @@ namespace CtrEditor.ObjetosSim { if (SimGeometria != null) { - SimGeometria.SetDiameter(Diametro); - SimGeometria.SetPosition(GetCentro()); + // ✅ SISTEMA INTELIGENTE: Solo recrear si el diámetro cambió + if (HasDiscardDimensionsChanged(SimGeometria, Diametro)) + { + System.Diagnostics.Debug.WriteLine($"[osDescarte] Recreando descarte por cambio de diámetro"); + SimGeometria.SetDiameter(Diametro); + } + + // ✅ USAR MÉTODO COORDINATECONVERTER + SimGeometria.UpdateFromWpfCenter(GetCentro()); } } diff --git a/ObjetosSim/Estaticos/ucGuia.xaml.cs b/ObjetosSim/Estaticos/ucGuia.xaml.cs index fdc9bed..05be73e 100644 --- a/ObjetosSim/Estaticos/ucGuia.xaml.cs +++ b/ObjetosSim/Estaticos/ucGuia.xaml.cs @@ -37,16 +37,30 @@ namespace CtrEditor.ObjetosSim [property: Name("Grosor de la Guía")] public float altoGuia; + + private void ActualizarGeometrias() { - if (_visualRepresentation is ucGuia uc) + if (_visualRepresentation is ucGuia uc && SimGeometria != null) { - // Actualizar las propiedades del objeto de simulación - if (SimGeometria != null) + var topLeft = new Vector2(Left, Top); + + // ✅ SISTEMA INTELIGENTE: Solo recrear si las dimensiones han cambiado + if (HasGuideDimensionsChanged(SimGeometria, Ancho, AltoGuia)) { - SimGeometria.UpdateProperties(Ancho, AltoGuia, Angulo); + System.Diagnostics.Debug.WriteLine($"[osGuia] Recreando guía por cambio de dimensiones: {Ancho}x{AltoGuia}"); + + // ✅ RECREAR COMPLETAMENTE: Las dimensiones cambiaron + SimGeometria.Create(Ancho, AltoGuia, topLeft, Angulo); + SimGeometria.SetDimensions(Ancho, AltoGuia); + } + else + { + System.Diagnostics.Debug.WriteLine($"[osGuia] Solo actualizando posición/rotación: Left={Left}, Top={Top}, Angulo={Angulo}"); + + // ✅ SOLO ACTUALIZAR POSICIÓN/ROTACIÓN: Usar dimensiones reales para conversión correcta + SimGeometria.UpdateFromWpfParameters(topLeft, Angulo, Ancho, AltoGuia); } - UpdateOrCreateLine(SimGeometria, uc.Guia); } } diff --git a/ObjetosSim/Estaticos/ucTransporteCurvaGuias.xaml.cs b/ObjetosSim/Estaticos/ucTransporteCurvaGuias.xaml.cs index 818c37c..c48b84e 100644 --- a/ObjetosSim/Estaticos/ucTransporteCurvaGuias.xaml.cs +++ b/ObjetosSim/Estaticos/ucTransporteCurvaGuias.xaml.cs @@ -258,15 +258,15 @@ namespace CtrEditor.ObjetosSim } /// - /// ✅ NUEVO: Actualizar posición en BEPU cuando cambia la posición del objeto + /// ✅ CORREGIDO: Actualizar posición en BEPU usando métodos de conversión apropiados /// private void ActualizarPosicionBEPU() { if (Simulation_TransporteCurvaGuias != null) { - // Recrear la curva con nueva posición usando el mismo patrón que ucTransporteCurva + // ✅ USAR MÉTODOS DE COORDINATECONVERTER var topLeft = new Vector2(Left, Top); - Simulation_TransporteCurvaGuias.Create(RadioInterno, RadioExterno, Angulo, Angulo + Arco_en_grados, topLeft, 0); + Simulation_TransporteCurvaGuias.UpdateFromWpfParameters(topLeft, 0f, RadioInterno, RadioExterno, Angulo, Angulo + Arco_en_grados); // Recrear las guías en la nueva posición ActualizarGuiasCurvas(); @@ -286,7 +286,12 @@ namespace CtrEditor.ObjetosSim { if (_visualRepresentation is ucTransporteCurvaGuias uc) { - Simulation_TransporteCurvaGuias?.UpdateCurve(RadioInterno, RadioExterno, Angulo, Angulo + Arco_en_grados); + // ✅ USAR MÉTODO DE COORDINATECONVERTER PARA ACTUALIZACIÓN COMPLETA + if (Simulation_TransporteCurvaGuias != null) + { + var topLeft = new Vector2(Left, Top); + Simulation_TransporteCurvaGuias.UpdateFromWpfParameters(topLeft, 0f, RadioInterno, RadioExterno, Angulo, Angulo + Arco_en_grados); + } ActualizarGuiasCurvas(); SetSpeed(); } @@ -327,7 +332,7 @@ namespace CtrEditor.ObjetosSim if (radioGuiaInferior < 0.01f) radioGuiaInferior = 0.01f; - // Convertir ángulos a radianes + // ✅ CORREGIDO: Convertir ángulos a radianes usando método estándar float anguloInicioRad = simBase.GradosARadianes(Angulo); float anguloFinalRad = simBase.GradosARadianes(AnguloFinal); float rangoAngular = anguloFinalRad - anguloInicioRad; @@ -390,7 +395,7 @@ namespace CtrEditor.ObjetosSim } /// - /// Método helper para crear una guía desde dos puntos + /// ✅ CORREGIDO: Método helper para crear una guía desde dos puntos usando conversiones apropiadas /// Convierte dos puntos Vector2 a los parámetros requeridos por AddLine /// private simGuia CrearGuiaDesdeDosPuntos(Vector2 punto1, Vector2 punto2) @@ -404,10 +409,10 @@ namespace CtrEditor.ObjetosSim if (longitud < 0.001f) // Evitar líneas de longitud cero return null; - // Calcular el ángulo de la línea + // ✅ CORREGIDO: Calcular el ángulo de la línea correctamente para WPF float angulo = (float)Math.Atan2(direccion.Y, direccion.X) * 180f / (float)Math.PI; - // Usar punto1 como topLeft y la longitud como width + // ✅ USAR punto1 como topLeft - simulationManager.AddLine ya maneja las conversiones WPF->BEPU return simulationManager.AddLine(longitud, GrosorGuias, punto1, angulo); } catch (Exception ex) @@ -532,7 +537,7 @@ namespace CtrEditor.ObjetosSim if (_visualRepresentation is ucTransporteCurvaGuias uc) { - // ✅ CORRIGIDO: Usar simulationManager?.AddCurve con todos los parámetros requeridos + // ✅ CORREGIDO: Usar simulationManager?.AddCurve con conversión WPF correcta var topLeft = new Vector2(Left, Top); Simulation_TransporteCurvaGuias = simulationManager?.AddCurve(RadioInterno, RadioExterno, Angulo, Angulo + Arco_en_grados, topLeft, 0); CrearGuiasCurvas(); // Crear las guías curvas diff --git a/ObjetosSim/osBase.cs b/ObjetosSim/osBase.cs index 3b603cf..2f2e580 100644 --- a/ObjetosSim/osBase.cs +++ b/ObjetosSim/osBase.cs @@ -1747,7 +1747,7 @@ namespace CtrEditor.ObjetosSim /// /// Verifica si las dimensiones de un transporte han cambiado /// - private bool HasTransportDimensionsChanged(simTransporte transport, float newWidth, float newHeight) + protected bool HasTransportDimensionsChanged(simTransporte transport, float newWidth, float newHeight) { if (transport == null) return true; @@ -1780,7 +1780,7 @@ namespace CtrEditor.ObjetosSim /// /// Verifica si las dimensiones de una barrera han cambiado /// - private bool HasBarrierDimensionsChanged(simBarrera barrier, float newWidth, float newHeight) + protected bool HasBarrierDimensionsChanged(simBarrera barrier, float newWidth, float newHeight) { if (barrier == null) return true; @@ -1813,7 +1813,7 @@ namespace CtrEditor.ObjetosSim /// /// Verifica si las dimensiones de una guía han cambiado /// - private bool HasGuideDimensionsChanged(simGuia guide, float newWidth, float newHeight) + protected bool HasGuideDimensionsChanged(simGuia guide, float newWidth, float newHeight) { if (guide == null) return true; @@ -1843,6 +1843,41 @@ namespace CtrEditor.ObjetosSim } } + /// + /// ✅ NUEVO - Sistema inteligente de verificación de dimensiones de descartes + /// Verifica si el diámetro de un descarte ha cambiado + /// + protected bool HasDiscardDimensionsChanged(simDescarte discard, float newDiameter) + { + if (discard == null) return true; + + var newDimensions = new SimObjectDimensions + { + Width = newDiameter, + Height = newDiameter, + Radius = newDiameter / 2f, + ObjectType = 4 // Tipo 4 = Descarte + }; + + if (_lastKnownDimensions.TryGetValue(discard, out var lastDimensions)) + { + bool changed = !newDimensions.Equals(lastDimensions); + if (changed) + { + _lastKnownDimensions[discard] = newDimensions; + System.Diagnostics.Debug.WriteLine($"[Dimensions] Discard dimensions CHANGED: Ø{newDiameter}"); + } + return changed; + } + else + { + // Primera vez - consideramos como cambio + _lastKnownDimensions[discard] = newDimensions; + System.Diagnostics.Debug.WriteLine($"[Dimensions] Discard first time: Ø{newDiameter}"); + return true; + } + } + /// /// Limpia las dimensiones almacenadas para un objeto específico /// diff --git a/Simulacion/BEPU.cs b/Simulacion/BEPU.cs index 70bd328..d2218d0 100644 --- a/Simulacion/BEPU.cs +++ b/Simulacion/BEPU.cs @@ -231,8 +231,8 @@ namespace CtrEditor.Simulacion public Simulation _simulation; protected bool _bodyCreated = false; // Bandera para saber si hemos creado un cuerpo - // ✅ NUEVA CONSTANTE - unificar conversión de velocidad - public const float SPEED_CONVERSION_FACTOR = 1f; //18.5f; // Factor de conversión de velocidad interna a m/s + // ✅ CORREGIDO: Restaurar factor de conversión correcto + public const float SPEED_CONVERSION_FACTOR = 1/2f; // Factor de conversión de velocidad interna a m/s - Para LinearAxisMotor es 0.5f // Constantes para las posiciones Z de los objetos 3D public const float zPos_Transporte = 0f; // Z de la parte baja @@ -390,6 +390,20 @@ namespace CtrEditor.Simulacion } } + /// + /// ✅ SOBRESCRITO: SetRotation que actualiza automáticamente las propiedades cacheadas + /// + public new void SetRotation(float wpfAngle) + { + base.SetRotation(wpfAngle); + + // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de rotación + UpdateCachedProperties(); + + // ✅ CRÍTICO: Disparar evento para actualizar motores activos con nueva dirección + OnSpeedChanged?.Invoke(this); + } + public new void SetPosition(float x, float y, float z = 0) { base.SetPosition(x, y, z); @@ -398,7 +412,7 @@ namespace CtrEditor.Simulacion /// /// ✅ NUEVO: Actualiza posición desde Top-Left WPF con dimensiones y ángulo actuales /// - public void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft) + internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft) { var currentWpfAngle = GetRotationZ(); // Ya usa CoordinateConverter var zPosition = GetPosition().Z; // Mantener Z actual @@ -409,7 +423,7 @@ namespace CtrEditor.Simulacion /// /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual /// - public Vector2 GetWpfTopLeft() + internal Vector2 GetWpfTopLeft() { var bepuCenter = GetPosition(); var wpfAngle = GetRotationZ(); // Ya usa CoordinateConverter @@ -419,22 +433,53 @@ namespace CtrEditor.Simulacion /// /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF /// - public void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle) + internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle) { var zPosition = GetPosition().Z; // Mantener Z actual var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, wpfAngle, zPosition); CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle); - // Actualizar propiedades cacheadas después del cambio + // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de orientación UpdateCachedProperties(); + + // ✅ CRÍTICO: Disparar evento para actualizar motores activos con nueva dirección + OnSpeedChanged?.Invoke(this); } // ✅ NUEVO MÉTODO - actualizar propiedades cacheadas - public void UpdateCachedProperties() + internal void UpdateCachedProperties() { - // Calcular dirección basada en rotación actual - var angle = GetRotationZ(); - DirectionVector = new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0); + // ✅ CORREGIDO: La dirección siempre es UnitX rotado por el ángulo del transporte + // NO depende de las dimensiones (Width >= Height) sino solo de la rotación + if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle)) + { + var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle); + var bepuQuaternion = bodyReference.Pose.Orientation; + + // ✅ SIEMPRE usar UnitX y aplicar la rotación + DirectionVector = Vector3.Transform(Vector3.UnitX, bepuQuaternion); + + // 🔍 DEBUG: Agregar información detallada + var wpfAngle = GetRotationZ(); + System.Diagnostics.Debug.WriteLine($"[UpdateCached] WPF Angle: {wpfAngle}°, DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}"); + } + else + { + // ✅ CORREGIDO: Aplicar conversión de coordenadas WPF→BEPU en el vector + var wpfAngle = GetRotationZ(); // Ángulo WPF en grados + var wpfAngleRadians = GradosARadianes(wpfAngle); + + // Calcular el vector en coordenadas WPF + var wpfX = (float)Math.Cos(wpfAngleRadians); + var wpfY = (float)Math.Sin(wpfAngleRadians); + + // ✅ APLICAR CONVERSIÓN Y: En WPF Y+ es abajo, en BEPU Y+ es arriba + DirectionVector = new Vector3(wpfX, -wpfY, 0); // Invertir Y para conversión WPF→BEPU + + // 🔍 DEBUG: Agregar información detallada + System.Diagnostics.Debug.WriteLine($"[UpdateCached-Fallback] WPF Angle: {wpfAngle}°, WPF Vector: ({wpfX:F3}, {wpfY:F3}), BEPU DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}"); + } + SpeedMetersPerSecond = Speed / SPEED_CONVERSION_FACTOR; } @@ -485,6 +530,12 @@ namespace CtrEditor.Simulacion var shapeIndex = _simulation.Shapes.Add(box); ChangeBodyShape(shapeIndex); } + + // ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de dimensiones + UpdateCachedProperties(); + + // ✅ CRÍTICO: Disparar evento para actualizar motores activos con nueva dirección + OnSpeedChanged?.Invoke(this); } public void Create(float width, float height, Vector2 wpfTopLeft, float wpfAngle = 0) @@ -549,6 +600,9 @@ namespace CtrEditor.Simulacion BodyHandle = _simulation.Bodies.Add(bodyDescription); _bodyCreated = true; // Marcar que hemos creado un cuerpo + + // ✅ CRÍTICO: Actualizar propiedades cacheadas después de crear el body + UpdateCachedProperties(); } } @@ -598,7 +652,7 @@ namespace CtrEditor.Simulacion /// /// ✅ NUEVO: Actualiza posición desde Top-Left WPF con dimensiones y ángulo actuales /// - public void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft) + internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft) { var currentWpfAngle = GetRotationZ(); // Ya usa CoordinateConverter var zPosition = GetPosition().Z; // Mantener Z actual @@ -609,7 +663,7 @@ namespace CtrEditor.Simulacion /// /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual /// - public Vector2 GetWpfTopLeft() + internal Vector2 GetWpfTopLeft() { var bepuCenter = GetPosition(); var wpfAngle = GetRotationZ(); // Ya usa CoordinateConverter @@ -619,7 +673,7 @@ namespace CtrEditor.Simulacion /// /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF /// - public void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle) + internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle) { var zPosition = GetPosition().Z; // Mantener Z actual var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, wpfAngle, zPosition); @@ -735,20 +789,39 @@ namespace CtrEditor.Simulacion } /// - /// ✅ NUEVO - Actualiza solo la posición sin recrear el objeto usando CoordinateConverter + /// ✅ CORREGIDO - Actualiza solo la posición sin recrear el objeto usando CoordinateConverter + /// Requiere las dimensiones reales para conversión correcta Top-Left → Center /// - public void SetPosition(Vector2 wpfTopLeft, float wpfAngle = 0) + public void SetPosition(Vector2 wpfTopLeft, float wpfAngle, float actualWidth, float actualHeight) { - // ✅ USAR COORDINATECONVERTER para conversión centralizada + // ✅ USAR COORDINATECONVERTER para conversión centralizada con dimensiones reales var zPosition = zAltura_Guia / 2 + zPos_Guia; - // Usar las dimensiones actuales de la guía - var width = GuideThickness; // Para guías, el ancho puede ser derivado del espesor - var height = GuideThickness; - var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition); + var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, actualWidth, actualHeight, wpfAngle, zPosition); // Actualizar posición y rotación simultáneamente CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle); } + + /// + /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF (sobrecarga para compatibilidad) + /// + internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle, float width, float height) + { + // Usar el método SetPosition con dimensiones correctas + SetPosition(wpfTopLeft, wpfAngle, width, height); + + // Actualizar propiedades internas + GuideThickness = height; + } + + /// + /// ✅ LEGACY - Mantener compatibilidad con versión anterior (usar dimensiones almacenadas) + /// + public void SetPosition(Vector2 wpfTopLeft, float wpfAngle = 0) + { + // Fallback: usar GuideThickness como aproximación si no se proporcionan dimensiones + SetPosition(wpfTopLeft, wpfAngle, GuideThickness * 10f, GuideThickness); + } } public class simBotella : simBase @@ -785,9 +858,8 @@ namespace CtrEditor.Simulacion _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); + // ✅ USAR COORDINATECONVERTER para conversión centralizada + var position3D = new Vector3(position.X, CoordinateConverter.WpfYToBepuY(position.Y), Radius + zPos_Transporte + zAltura_Transporte); Create(position3D); } @@ -808,14 +880,14 @@ namespace CtrEditor.Simulacion { get { - // Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo) - return -GetPosition().Y; + // ✅ USAR COORDINATECONVERTER para conversión centralizada + return CoordinateConverter.BepuYToWpfY(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); + // ✅ USAR COORDINATECONVERTER para conversión centralizada + SetPosition(pos.X, CoordinateConverter.WpfYToBepuY(value), pos.Z); } } @@ -824,15 +896,15 @@ namespace CtrEditor.Simulacion 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); + // ✅ USAR COORDINATECONVERTER para conversión centralizada + return CoordinateConverter.BepuVector3ToWpfVector2(pos3D); } 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); + // ✅ USAR COORDINATECONVERTER para conversión centralizada + SetPosition(value.X, CoordinateConverter.WpfYToBepuY(value.Y), currentPos.Z); } } @@ -1022,8 +1094,8 @@ namespace CtrEditor.Simulacion public float InnerRadius => _innerRadius; public float OuterRadius => _outerRadius; - public float StartAngle => _startAngle; - public float EndAngle => _endAngle; + public float StartAngle => RadianesAGrados(_startAngle); // Convertir de radianes BEPU internos a grados WPF + public float EndAngle => RadianesAGrados(_endAngle); // Convertir de radianes BEPU internos a grados WPF /// /// ✅ NUEVO: Expone los triángulos originales para visualización debug @@ -1037,9 +1109,9 @@ namespace CtrEditor.Simulacion _simulationManager = simulationManager; // ✅ NUEVA REFERENCIA _innerRadius = innerRadius; _outerRadius = outerRadius; - // ✅ SENTIDO HORARIO: Convertir ángulos para que vayan en sentido horario - _startAngle = GradosARadianes(startAngle); - _endAngle = GradosARadianes(endAngle); + // ✅ CORREGIDO: Usar conversión WPF a BEPU y luego a radianes para consistencia + _startAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(startAngle)); + _endAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(endAngle)); _triangleBodyHandles = new List(); _originalTriangles = new List>(); // ✅ NUEVO: Inicializar lista de triángulos @@ -1059,14 +1131,57 @@ namespace CtrEditor.Simulacion { _innerRadius = innerRadius; _outerRadius = outerRadius; - _startAngle = GradosARadianes(startAngle); - _endAngle = GradosARadianes(endAngle); + // ✅ CORREGIDO: Usar conversión WPF a BEPU consistente + _startAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(startAngle)); + _endAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(endAngle)); // Recrear la curva con nuevos parámetros manteniendo posición actual var currentPosition = GetPosition(); Create(currentPosition); } + /// + /// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF + /// + internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle, float innerRadius, float outerRadius, float startAngle, float endAngle) + { + // Actualizar parámetros de la curva + _innerRadius = innerRadius; + _outerRadius = outerRadius; + // ✅ CORREGIDO: Usar conversión WPF a BEPU consistente + _startAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(startAngle)); + _endAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(endAngle)); + + // ✅ USAR COORDINATECONVERTER para conversión centralizada + var curveSize = outerRadius * 2f; + var zPosition = zAltura_Curve / 2f + zPos_Curve; + var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition); + + Create(bepuCenter); + } + + /// + /// ✅ NUEVO: Actualiza posición desde Top-Left WPF manteniendo parámetros actuales + /// + internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft) + { + var curveSize = _outerRadius * 2f; + var zPosition = GetPosition().Z; // Mantener Z actual + var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition); + + Create(bepuCenter); + } + + /// + /// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual + /// + internal Vector2 GetWpfTopLeft() + { + var bepuCenter = GetPosition(); + var curveSize = _outerRadius * 2f; + return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(bepuCenter, curveSize, curveSize, 0f); + } + public new void RemoverBody() { // ✅ CRÍTICO: Limpiar todos los motors conectados a esta curva ANTES de eliminar los bodies @@ -1135,8 +1250,9 @@ namespace CtrEditor.Simulacion // Actualizar parámetros internos _innerRadius = innerRadius; _outerRadius = outerRadius; - _startAngle = GradosARadianes(startAngle); - _endAngle = GradosARadianes(endAngle); + // ✅ CORREGIDO: Usar conversión WPF a BEPU consistente + _startAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(startAngle)); + _endAngle = GradosARadianes(CoordinateConverter.WpfAngleToBepuAngle(endAngle)); // ✅ USAR COORDINATECONVERTER para conversión centralizada // Para curvas, el "tamaño" es el diámetro del radio exterior @@ -1343,9 +1459,8 @@ namespace CtrEditor.Simulacion _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); + // ✅ USAR COORDINATECONVERTER para conversión centralizada + var position3D = new Vector3(position.X, CoordinateConverter.WpfYToBepuY(position.Y), zPos_Descarte + _radius); Create(position3D); } @@ -1373,12 +1488,22 @@ namespace CtrEditor.Simulacion 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); + // ✅ USAR COORDINATECONVERTER para conversión centralizada + var position3D = new Vector3(position.X, CoordinateConverter.WpfYToBepuY(position.Y), zPos_Descarte + _radius); Create(position3D); } + /// + /// ✅ NUEVO: Actualiza posición usando coordenadas WPF apropiadas + /// + internal void UpdateFromWpfCenter(Vector2 wpfCenter) + { + var position3D = new Vector3(wpfCenter.X, CoordinateConverter.WpfYToBepuY(wpfCenter.Y), zPos_Descarte + _radius); + + // Actualizar solo posición manteniendo orientación + CoordinateConverter.UpdateBepuBodyPosition(_simulation, BodyHandle, position3D); + } + private void Create(Vector3 position) { RemoverBody(); @@ -1674,16 +1799,25 @@ namespace CtrEditor.Simulacion if (transport.DirectionVector.Length() < 0.001f) return; - // ✅ SIMPLIFICAR - Usar LinearAxisMotor básico + // ✅ CORREGIDO - LocalAxis debe estar en coordenadas locales del transporte + // Para transportes, el eje local de movimiento es siempre UnitX (eje largo) + var localAxis = Vector3.UnitX; // Siempre UnitX en coordenadas locales del transporte + var motor = new LinearAxisMotor() { LocalOffsetA = Vector3.Zero, LocalOffsetB = Vector3.Zero, - LocalAxis = transport.DirectionVector, + LocalAxis = localAxis, // ✅ Usar eje local, no mundial TargetVelocity = transport.SpeedMetersPerSecond, Settings = new MotorSettings(Math.Max(bottle.Mass * 30f, 10f), 5f) }; + // 🔍 DEBUG: Comparar eje local vs mundial + System.Diagnostics.Debug.WriteLine($"[Motor] Transport angle: {transport.GetRotationZ()}°"); + System.Diagnostics.Debug.WriteLine($"[Motor] World DirectionVector: {transport.DirectionVector}"); + System.Diagnostics.Debug.WriteLine($"[Motor] Local Axis: {localAxis}"); + System.Diagnostics.Debug.WriteLine($"[Motor] Target Velocity: {transport.SpeedMetersPerSecond}"); + var motorHandle = _simulationManager.simulation.Solver.Add( transport.BodyHandle, bottle.BodyHandle,