diff --git a/Simulacion/BEPU.cs b/Simulacion/BEPU.cs index b1a1025..175430d 100644 --- a/Simulacion/BEPU.cs +++ b/Simulacion/BEPU.cs @@ -14,6 +14,7 @@ using BepuPhysics.Constraints; using BepuUtilities; using BepuUtilities.Memory; using CtrEditor.FuncionesBase; +using DocumentFormat.OpenXml.Vml; namespace CtrEditor.Simulacion { @@ -250,7 +251,7 @@ namespace CtrEditor.Simulacion protected bool _bodyCreated = false; // Bandera para saber si hemos creado un cuerpo // ✅ 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 + public const float SPEED_CONVERSION_FACTOR = 1f; // 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 @@ -491,7 +492,7 @@ namespace CtrEditor.Simulacion // 🔍 DEBUG: Agregar información detallada var wpfAngle = GetRotationZ(); - System.Diagnostics.Debug.WriteLine($"[UpdateCached] WPF Angle: {wpfAngle}°, DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}"); + //System.Diagnostics.Debug.WriteLine($"[UpdateCached] WPF Angle: {wpfAngle}°, DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}"); } else { @@ -844,6 +845,7 @@ namespace CtrEditor.Simulacion // ✅ NUEVO SISTEMA SIMPLIFICADO: Motor dinámico que se crea solo cuando es necesario public ConstraintHandle CurrentMotor { get; private set; } = default; + public ConstraintHandle CurrentDistanceLimit { get; private set; } = default; // ✅ NUEVO: Para curvas public simBase CurrentMotorTarget { get; private set; } = null; // Transporte o curva actual public bool _hasMotor = false; // ✅ BANDERA PÚBLICA para acceso desde callbacks public bool HasMotor => _hasMotor; @@ -1049,43 +1051,71 @@ namespace CtrEditor.Simulacion return; } - // ✅ CALCULAR VELOCIDAD EFECTIVA - var effectiveSpeed = CalculateEffectiveSpeed(direction, speed); + // ✅ NORMALIZAR LA DIRECCIÓN TANGENCIAL + var tangentDir = Vector3.Normalize(direction); // ✅ CREAR MOTOR CONECTADO AL TARGET var motor = new LinearAxisMotor() { - LocalOffsetA = Vector3.Zero, // Botella - LocalOffsetB = Vector3.Zero, // Target - LocalAxis = Vector3.UnitX, // Dirección fija del motor - TargetVelocity = effectiveSpeed, + LocalOffsetA = Vector3.Zero, // Target + LocalOffsetB = Vector3.Zero, // Botella + LocalAxis = tangentDir, // ✅ CORREGIDO: Usar la dirección tangencial calculada + TargetVelocity = speed, // ✅ CORREGIDO: Usar la velocidad directamente Settings = new MotorSettings(Math.Max(_mass * 20f, 8f), 4f) }; // ✅ CONECTAR BOTELLA CON EL TARGET (transporte o curva) - CurrentMotor = _simulation.Solver.Add(BodyHandle, BodyHandle, motor); - - // ✅ VERIFICAR QUE EL MOTOR SE CREÓ CORRECTAMENTE - if (CurrentMotor.Value == 0) - { - System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ❌ Error: Motor no se creó correctamente"); - return; - } - + CurrentMotor = _simulation.Solver.Add(target.BodyHandle, BodyHandle, motor); + CurrentMotorTarget = target; _hasMotor = true; // ✅ ESTABLECER BANDERA - - // ✅ ACTUALIZAR PROPIEDADES INTERNAS - CurrentDirection = direction; + + if (target is simCurve curva) { + // Calcular el vector desde el centro de la curva hasta la botella (en el plano XY) + var curveCenter = curva.CurveCenter; + var bottlePosition = GetPosition(); + + var radiusVector = new Vector3(bottlePosition.X - curveCenter.X, bottlePosition.Y - curveCenter.Y, 0f); + var radius = radiusVector.Length(); + + if (radius > 1e-3f) + { + + // Calcular offsets locales + var localOffsetA = curveCenter; // Desde el centro de la curva hasta el punto de anclaje + var localOffsetB = Vector3.Zero; // ✅ SIMPLIFICADO: Conectar al centro de la botella + + var distanceLimit = new DistanceLimit() + { + LocalOffsetA = Vector3.Zero, + LocalOffsetB = Vector3.Zero, + MinimumDistance = radius, // Distancia mínima = radio actual + MaximumDistance = radius, // Distancia máxima = radio actual (mantener distancia fija) + SpringSettings = new SpringSettings(30f, 0f) + }; + + CurrentDistanceLimit = _simulation.Solver.Add(target.BodyHandle, BodyHandle, distanceLimit); + + //System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget-Curve] 📏 DistanceLimit creado:"); + //System.Diagnostics.Debug.WriteLine($" Radio actual: {radius:F3}"); + //System.Diagnostics.Debug.WriteLine($" Punto de anclaje: {anchorPoint}"); + //System.Diagnostics.Debug.WriteLine($" LocalOffsetA (curva): {localOffsetA}"); + //System.Diagnostics.Debug.WriteLine($" LocalOffsetB (botella): {localOffsetB} (centro)"); + //System.Diagnostics.Debug.WriteLine($" Distancia objetivo: {radius:F3}"); + //System.Diagnostics.Debug.WriteLine($" DistanceLimit Handle: {CurrentDistanceLimit}"); + } + } + // ✅ ACTUALIZAR PROPIEDADES INTERNAS + CurrentDirection = direction; CurrentSpeed = speed; IsOnElement = Math.Abs(speed) > 0.001f; - System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ✅ Motor creado:"); - System.Diagnostics.Debug.WriteLine($" Botella: {BodyHandle}"); - System.Diagnostics.Debug.WriteLine($" Target: {target.BodyHandle} ({target.GetType().Name})"); - System.Diagnostics.Debug.WriteLine($" Motor Handle: {CurrentMotor} (Value: {CurrentMotor.Value})"); - System.Diagnostics.Debug.WriteLine($" Dirección: {direction}"); - System.Diagnostics.Debug.WriteLine($" Velocidad efectiva: {effectiveSpeed:F3}"); + //System.Diagnostics.Debug.WriteLine($"[CreateMotorForTarget] ✅ Motor creado:"); + //System.Diagnostics.Debug.WriteLine($" Botella: {BodyHandle}"); + //System.Diagnostics.Debug.WriteLine($" Target: {target.BodyHandle} ({target.GetType().Name})"); + //System.Diagnostics.Debug.WriteLine($" Motor Handle: {CurrentMotor} (Value: {CurrentMotor.Value})"); + //System.Diagnostics.Debug.WriteLine($" Dirección: {direction}"); + //System.Diagnostics.Debug.WriteLine($" Velocidad efectiva: {effectiveSpeed:F3}"); } catch (Exception ex) { @@ -1105,14 +1135,15 @@ namespace CtrEditor.Simulacion return; } + // ✅ NORMALIZAR LA DIRECCIÓN TANGENCIAL + var tangentDir = Vector3.Normalize(direction); + // ✅ OBTENER LA DESCRIPCIÓN ACTUAL DEL MOTOR _simulation.Solver.GetDescription(CurrentMotor, out LinearAxisMotor motor); - // ✅ CALCULAR VELOCIDAD EFECTIVA - var effectiveSpeed = CalculateEffectiveSpeed(direction, speed); - - // ✅ ACTUALIZAR SOLO LA VELOCIDAD - motor.TargetVelocity = effectiveSpeed; + // ✅ ACTUALIZAR DIRECCIÓN Y VELOCIDAD + motor.LocalAxis = tangentDir; + motor.TargetVelocity = speed; // ✅ ACTUALIZAR EL MOTOR EN EL SOLVER _simulation.Solver.ApplyDescription(CurrentMotor, motor); @@ -1122,7 +1153,7 @@ namespace CtrEditor.Simulacion CurrentSpeed = speed; IsOnElement = Math.Abs(speed) > 0.001f; - System.Diagnostics.Debug.WriteLine($"[UpdateMotorSpeed] 🔄 Velocidad actualizada: {effectiveSpeed:F3}"); + //System.Diagnostics.Debug.WriteLine($"[UpdateMotorSpeed] 🔄 Velocidad actualizada: {effectiveSpeed:F3}"); } catch (Exception ex) { @@ -1158,6 +1189,20 @@ namespace CtrEditor.Simulacion System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] ⚠️ Motor ya eliminado o inválido: {CurrentMotor}"); } } + + // ✅ NUEVO: Eliminar DistanceLimit si existe + if (CurrentDistanceLimit.Value != 0) + { + try + { + _simulation.Solver.Remove(CurrentDistanceLimit); + System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] 🗑️ DistanceLimit eliminado: {CurrentDistanceLimit}"); + } + catch (Exception removeEx) + { + System.Diagnostics.Debug.WriteLine($"[RemoveCurrentMotor] ⚠️ Error eliminando DistanceLimit {CurrentDistanceLimit}: {removeEx.Message}"); + } + } } catch (Exception ex) { @@ -1167,6 +1212,7 @@ namespace CtrEditor.Simulacion { // ✅ LIMPIAR REFERENCIAS SIEMPRE CurrentMotor = default; + CurrentDistanceLimit = default; // ✅ NUEVO: Limpiar DistanceLimit CurrentMotorTarget = null; _hasMotor = false; // ✅ LIMPIAR BANDERA CurrentDirection = Vector3.UnitX; @@ -1183,31 +1229,7 @@ namespace CtrEditor.Simulacion UpdateMotorSpeed(CurrentDirection, 0f); } - /// - /// ✅ NUEVO MÉTODO: Calcular velocidad efectiva para dirección deseada - /// - private float CalculateEffectiveSpeed(Vector3 desiredDirection, float desiredSpeed) - { - // ✅ NORMALIZAR LA DIRECCIÓN DESEADA - var normalizedDirection = Vector3.Normalize(desiredDirection); - - // ✅ CALCULAR LA PROYECCIÓN EN EL EJE X (dirección del motor fijo) - // Como el motor está fijo en UnitX, solo podemos controlar el movimiento en X - var projectionX = normalizedDirection.X; - - // ✅ CALCULAR VELOCIDAD EFECTIVA - // Si la dirección es principalmente en X, usar la velocidad completa - // Si la dirección es perpendicular a X, la velocidad será 0 - var effectiveSpeed = projectionX * desiredSpeed; - - System.Diagnostics.Debug.WriteLine($"[CalculateEffectiveSpeed] 📐 Cálculo:"); - System.Diagnostics.Debug.WriteLine($" Dirección deseada: {desiredDirection} (Normalizada: {normalizedDirection})"); - System.Diagnostics.Debug.WriteLine($" Proyección en X: {projectionX:F3}"); - System.Diagnostics.Debug.WriteLine($" Velocidad deseada: {desiredSpeed:F3}"); - System.Diagnostics.Debug.WriteLine($" Velocidad efectiva: {effectiveSpeed:F3}"); - - return effectiveSpeed; - } + public void SetDiameter(float diameter) { @@ -1810,17 +1832,17 @@ namespace CtrEditor.Simulacion if (botella != null && (transportA != null || transportB != null)) { var transport = transportA ?? transportB; - + // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE transport.UpdateCachedProperties(); var direction = transport.DirectionVector; var speed = transport.SpeedMetersPerSecond; botella.CreateOrUpdateMotor(transport, direction, speed); - - // Fricción alta para transportes - pairMaterial.FrictionCoefficient = 0.9f; - pairMaterial.MaximumRecoveryVelocity = 1f; - pairMaterial.SpringSettings = new SpringSettings(80, 6); + + //Fricción alta para transportes + //pairMaterial.FrictionCoefficient = 0.9f; + //pairMaterial.MaximumRecoveryVelocity = 1f; + //pairMaterial.SpringSettings = new SpringSettings(80, 6); } // ✅ CONTACTO BOTELLA-CURVA: Crear o actualizar motor inmediatamente else if (botella != null && (curveA != null || curveB != null))