CtrEditor/Simulacion/BEPU.cs

using System.Windows.Controls;
using System.Windows.Media;
using System.Windows.Shapes;
using System.Windows;
using System.Diagnostics;
using System;
using System.Collections.Generic;
using System.Numerics;
using System.Linq;
using BepuPhysics;
using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuPhysics.Constraints;
using BepuPhysics.Trees;
using BepuUtilities;
using BepuUtilities.Memory;
using CtrEditor.FuncionesBase;
using DocumentFormat.OpenXml.Vml;
using DocumentFormat.OpenXml.Spreadsheet;

namespace CtrEditor.Simulacion
{

    // Callback handlers para BEPU
    public struct NarrowPhaseCallbacks : INarrowPhaseCallbacks
    {
        private SimulationManagerBEPU _simulationManager;

        public NarrowPhaseCallbacks(SimulationManagerBEPU simulationManager)
        {
            _simulationManager = simulationManager;
        }

        public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
        {
            // ✅ NUEVO FILTRADO: Evitar colisiones innecesarias entre triángulos de curvas
            if (_simulationManager != null)
            {
                // Obtener información de los objetos involucrados
                var curveA = _simulationManager.GetCurveFromTriangleBodyHandle(a.BodyHandle);
                var curveB = _simulationManager.GetCurveFromTriangleBodyHandle(b.BodyHandle);
                var transportA = GetTransportFromCollidable(a);
                var transportB = GetTransportFromCollidable(b);
                // ✅ ELIMINADO: Las barreras ya no tienen body físico
                // var barrierA = GetBarreraFromCollidable(a);
                // var barrierB = GetBarreraFromCollidable(b);
                var discardA = GetDescarteFromCollidable(a);
                var discardB = GetDescarteFromCollidable(b);
                var botellaA = GetBotellaFromCollidable(a);
                var botellaB = GetBotellaFromCollidable(b);
                
                // ✅ FILTRO 1: No permitir colisiones entre triángulos de curvas
                if (curveA != null && curveB != null)
                {
                    return false; // Los triángulos de curva no deben colisionar entre sí
                }
                
                // ✅ FILTRO 2: No permitir colisiones entre transportes
                if (transportA != null && transportB != null)
                {
                    return false; // Los transportes no deben colisionar entre sí
                }
                
                // ✅ FILTRO 3: No permitir colisiones entre elementos estáticos (transportes, curvas, descartes)
                // ✅ ELIMINADO: barrierA y barrierB ya que las barreras no tienen body físico
                var staticA = (transportA != null || curveA != null || discardA != null);
                var staticB = (transportB != null || curveB != null || discardB != null);
                
                if (staticA && staticB)
                {
                    return false; // Los elementos estáticos no deben colisionar entre sí
                }
                
                // ✅ ELIMINADO: No bloquear colisiones físicas aquí
                // Las colisiones físicas deben permitirse siempre para mantener geometría sólida
                // Solo bloqueamos la creación de motores duplicados en ConfigureContactManifold
            }
            
            return true; // Permitir todas las demás colisiones (botella-estático, botella-botella)
        }

        public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
        {
            return true;
        }

        public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
        {
            // ✅ CONFIGURACIÓN BÁSICA de materiales físicos
            pairMaterial = new PairMaterialProperties
            {
                FrictionCoefficient = 0.3f,
                MaximumRecoveryVelocity = 1f,
                SpringSettings = new SpringSettings(80, 6)
            };

            if (_simulationManager != null)
            {
                var botellaA = GetBotellaFromCollidable(pair.A);
                var botellaB = GetBotellaFromCollidable(pair.B);
                // ✅ ELIMINADO: Las barreras ya no tienen body físico
                // var barreraA = GetBarreraFromCollidable(pair.A);
                // var barreraB = GetBarreraFromCollidable(pair.B);
                var descarteA = GetDescarteFromCollidable(pair.A);
                var descarteB = GetDescarteFromCollidable(pair.B);
                var botella = botellaA ?? botellaB;

                // ✅ ELIMINADO: Las barreras ya no participan en colisiones físicas
                // if (barreraA != null || barreraB != null)
                // {
                //     return false; // NO generar contacto físico
                // }

                // ✅ DESCARTES como sensores puros
                if (descarteA != null || descarteB != null)
                {
                    var descarte = descarteA ?? descarteB;
                    if (botella != null)
                    {
                        _simulationManager.RegisterDescarteContact(descarte, botella);
                    }
                    return false; // NO generar contacto físico
                }

                // ✅ NUEVO SISTEMA SIMPLIFICADO: Solo registrar contactos para actualización en Step
                var transportA = GetTransportFromCollidable(pair.A);
                var transportB = GetTransportFromCollidable(pair.B);
                var curveA = GetCurveFromCollidable(pair.A);
                var curveB = GetCurveFromCollidable(pair.B);

                // ✅ CONTACTO BOTELLA-TRANSPORTE: Crear o actualizar motor inmediatamente
                if (botella != null && (transportA != null || transportB != null))
                {
                    var transport = transportA ?? transportB;

                    // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE
                    transport.UpdateCachedProperties();
                    var direction = transport.DirectionVector;
                    var speed = transport.Speed;
                    botella.CreateOrUpdateMotor(transport, direction, speed);

                    //Fricción alta para transportes
                    pairMaterial.FrictionCoefficient = 0.3f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
                // ✅ CONTACTO BOTELLA-CURVA: Crear o actualizar motor inmediatamente
                else if (botella != null && (curveA != null || curveB != null))
                {
                    var curve = curveA ?? curveB;
                    
                    // ✅ CREAR O ACTUALIZAR MOTOR DINÁMICO INMEDIATAMENTE
                    var direction = _simulationManager.CalculateCurveDirectionFromBottlePosition(curve, botella);
                    var speed = curve.SpeedMetersPerSecond; // ✅ CORREGIDO: Usar SpeedMetersPerSecond como simTransporte
                    botella.CreateOrUpdateMotor(curve, direction, speed);
                    
                    // Fricción alta para curvas
                    pairMaterial.FrictionCoefficient = 0.3f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
                // ✅ CONTACTO BOTELLA-GUÍA: Configuración específica de fricción
                else if (botella != null && (GetGuiaFromCollidable(pair.A) != null || GetGuiaFromCollidable(pair.B) != null))
                {
                    // Configuración específica para guías usando propiedades configurables
                    pairMaterial.FrictionCoefficient = 0.3f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
                // Ajustes básicos para otras botellas
                else if (botella != null)
                {
                    pairMaterial.FrictionCoefficient = 0.3f;
                    pairMaterial.MaximumRecoveryVelocity = 1f;
                    pairMaterial.SpringSettings = new SpringSettings(80, 6);
                }
            }

            return true;
        }



        private simTransporte GetTransportFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simTransporte;
                }
            }
            return null;
        }

        private simBotella GetBotellaFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Dynamic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simBotella;
                }
            }
            return null;
        }

        // ✅ ELIMINADO: GetBarreraFromCollidable - las barreras ya no tienen body físico
        // private simBarrera GetBarreraFromCollidable(CollidableReference collidable) { ... }

        private simDescarte GetDescarteFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simDescarte;
                }
            }
            return null;
        }

        private simCurve GetCurveFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simCurve;
                }
            }
            return null;
        }

        private simGuia GetGuiaFromCollidable(CollidableReference collidable)
        {
            if (_simulationManager?.simulation != null)
            {
                if (collidable.Mobility == CollidableMobility.Kinematic)
                {
                    var bodyHandle = collidable.BodyHandle;
                    var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
                    return simBase as simGuia;
                }
            }
            return null;
        }





        /// <summary>
        /// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado directamente en el manifold
        /// </summary>


        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<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
        {
            // Aplicar gravedad
            var gravityWide = Vector3Wide.Broadcast(Gravity);
            velocity.Linear += gravityWide * dt;

            // ✅ ELIMINADO COMPLETAMENTE - ya no se necesita lógica especial de frenado
            // El sistema LinearAxisMotor maneja automáticamente todas las fuerzas

            // Aplicar amortiguamiento lineal y angular para simular resistencia del aire
            // Esto es crucial para que los cilindros se detengan de forma realista
            var linearDampingWide = Vector<float>.One * LinearDamping;
            var angularDampingWide = Vector<float>.One * AngularDamping;

            velocity.Linear *= linearDampingWide;
            velocity.Angular *= angularDampingWide; // ¡Esto es clave para detener rotaciones infinitas!
        }


    }



    public class SimulationManagerBEPU
    {
        public Simulation simulation;
        public List<simBase> Cuerpos;
        public List<Action> _deferredActions;
        private BufferPool bufferPool;
        private Stopwatch stopwatch;
        private double stopwatch_last;

        // Referencia al manager de visualización 3D
        public BEPUVisualization3DManager Visualization3DManager { get; set; }

        // Propiedad para controlar si la actualización 3D está habilitada
        public bool Is3DUpdateEnabled { get; set; } = true;

        // ✅ NUEVAS - para filtrado eficiente de callbacks
        private HashSet<BodyHandle> _transportHandles;
        private HashSet<BodyHandle> _curveHandles; 
        private HashSet<BodyHandle> _barrierHandles;
        private HashSet<BodyHandle> _discardHandles;
        private HashSet<BodyHandle> _bottleHandles;
        private HashSet<BodyHandle> _guiaHandles;

        
        // ✅ NUEVO - contador de frames para optimizaciones
        private int _frameCount = 0;
        
        // ✅ CONSERVAR - sistemas existentes que funcionan bien
        private Dictionary<simBarrera, List<simBotella>> _barreraContacts;
        private Dictionary<simDescarte, List<simBotella>> _descarteContacts;
        private HashSet<simBotella> _botellasParaEliminar;
        
        // ✅ NUEVAS - propiedades configurables para fricción de guías
        public float GuiaFrictionCoefficient { get; set; } = 0.1f;
        public float GuiaMaxRecoveryVelocity { get; set; } = 0.5f;
        public float GuiaSpringFrequency { get; set; } = 40f;
        public float GuiaSpringDampingRatio { get; set; } = 2f;
        
        private object _contactsLock = new object();

        /// <summary>
        /// Obtiene el objeto simBase correspondiente a un BodyHandle
        /// </summary>
        public simBase GetSimBaseFromBodyHandle(BodyHandle bodyHandle)
        {
            return Cuerpos.FirstOrDefault(c => c.BodyHandle.Equals(bodyHandle));
        }

        /// <summary>
        /// ✅ SIMPLIFICADO: Obtiene una simCurve desde su BodyHandle principal
        /// </summary>
        public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle)
        {
            // ✅ SIMPLIFICADO: Solo buscar en cuerpos principales (ya no hay triángulos separados)
            var simBase = GetSimBaseFromBodyHandle(bodyHandle);
            return simBase as simCurve;
        }

        /// <summary>
        /// ✅ NUEVO: Obtiene todas las botellas que están conectadas a un elemento específico
        /// </summary>
        /// <param name="element">Elemento (simTransporte o simCurve) del cual obtener las botellas conectadas</param>
        /// <returns>Lista de botellas conectadas al elemento</returns>
        public List<simBotella> GetBottlesConnectedToElement(simBase element)
        {
            var connectedBottles = new List<simBotella>();
            
            try
            {
                if (element == null) return connectedBottles;
                
                // ✅ BUSCAR BOTELLAS QUE TENGAN ESTE ELEMENTO COMO TARGET
                foreach (var bottle in Cuerpos.OfType<simBotella>())
                {
                    if (bottle != null && bottle.HasMotor && bottle.CurrentMotorTarget == element)
                    {
                        connectedBottles.Add(bottle);
                    }
                }
                
                System.Diagnostics.Debug.WriteLine($"[GetBottlesConnectedToElement] Encontradas {connectedBottles.Count} botellas conectadas a {element.GetType().Name}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[GetBottlesConnectedToElement] ❌ ERROR: {ex.Message}");
            }
            
            return connectedBottles;
        }

        // ✅ NUEVOS - gestión automática de clasificación
        private void RegisterObjectHandle(simBase obj)
        {
            switch (obj)
            {
                case simBotella bottle:
                    _bottleHandles.Add(bottle.BodyHandle);
                    break;
                case simTransporte transport:
                    _transportHandles.Add(transport.BodyHandle);
                    // Calcular propiedades iniciales
                    transport.UpdateCachedProperties();
                    break;
                case simCurve curve:
                    _curveHandles.Add(curve.BodyHandle);
                    // ✅ SIMPLIFICADO: Ya no hay triángulos separados, solo el cuerpo principal
                    break;
                case simBarrera barrier:
                    // ✅ NUEVO: Las barreras ya no tienen body físico, no registrar handle
                    // _barrierHandles.Add(barrier.BodyHandle); // ELIMINADO
                    break;
                case simDescarte discard:
                    _discardHandles.Add(discard.BodyHandle);
                    break;
                case simGuia guia:
                    _guiaHandles.Add(guia.BodyHandle);
                    break;
            }
        }

        private void UnregisterObjectHandle(simBase obj)
        {
            switch (obj)
            {
                case simBotella bottle:
                    _bottleHandles.Remove(bottle.BodyHandle);

                    break;
                case simTransporte transport:
                    _transportHandles.Remove(transport.BodyHandle);
                    break;
                case simCurve curve:
                    _curveHandles.Remove(curve.BodyHandle);
                    // ✅ SIMPLIFICADO: Ya no hay triángulos separados
                    break;
                case simBarrera barrier:
                    // ✅ NUEVO: Las barreras ya no tienen body físico, no desregistrar handle
                    // _barrierHandles.Remove(barrier.BodyHandle); // ELIMINADO
                    break;
                case simDescarte discard:
                    _discardHandles.Remove(discard.BodyHandle);
                    break;
                case simGuia guia:
                    _guiaHandles.Remove(guia.BodyHandle);
                    break;
            }
        }



        // ✅ NUEVOS MÉTODOS - obtener objetos por handle
        private simBotella GetBottleByHandle(BodyHandle handle)
        {
            return Cuerpos.OfType<simBotella>().FirstOrDefault(b => b.BodyHandle.Equals(handle));
        }

        private simTransporte GetTransportByHandle(BodyHandle handle)
        {
            return Cuerpos.OfType<simTransporte>().FirstOrDefault(t => t.BodyHandle.Equals(handle));
        }

        private simCurve GetCurveByHandle(BodyHandle handle)
        {
            // Primero buscar en cuerpos principales
            var curve = Cuerpos.OfType<simCurve>().FirstOrDefault(c => c.BodyHandle.Equals(handle));
            if (curve != null) return curve;
            
            // Luego buscar en triángulos
            return GetCurveFromTriangleBodyHandle(handle);
        }

        private simGuia GetGuiaByHandle(BodyHandle handle)
        {
            return Cuerpos.OfType<simGuia>().FirstOrDefault(g => g.BodyHandle.Equals(handle));
        }

        public SimulationManagerBEPU()
        {
            // ✅ EXISTENTES - conservar
            Cuerpos = new List<simBase>();
            _deferredActions = new List<Action>();
            _barreraContacts = new Dictionary<simBarrera, List<simBotella>>();
            _descarteContacts = new Dictionary<simDescarte, List<simBotella>>();
            _botellasParaEliminar = new HashSet<simBotella>();
            
                    // ✅ NUEVOS - sistemas de filtrado y contactos
        _transportHandles = new HashSet<BodyHandle>();
        _curveHandles = new HashSet<BodyHandle>();
        _barrierHandles = new HashSet<BodyHandle>();
        _discardHandles = new HashSet<BodyHandle>();
        _bottleHandles = new HashSet<BodyHandle>();
        _guiaHandles = new HashSet<BodyHandle>();
            
            
            // ✅ CONSERVAR - resto del constructor igual
            bufferPool = new BufferPool();
            stopwatch = new Stopwatch();

            var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this);

            // Configurar amortiguamiento para comportamiento realista:
            // - LinearDamping: 0.999f (muy leve, objetos siguen moviéndose pero eventualmente se detienen)
            // - AngularDamping: 0.995f (más agresivo para detener rotaciones infinitas de cilindros)
            // ✅ MODIFICADO: Pasar referencia de this al PoseIntegratorCallbacks
            var poseIntegratorCallbacks = new PoseIntegratorCallbacks(
                gravity: new Vector3(0, 0, -9.81f), // Gravedad en Z
                linearDamping: 0.999f,              // Amortiguamiento lineal suave
                angularDamping: 0.995f,             // Amortiguamiento angular más fuerte para detener rotaciones
                simulationManager: this             // ✅ NUEVA REFERENCIA
            );

            var solveDescription = new SolveDescription(8, 1);

            simulation = Simulation.Create(bufferPool, narrowPhaseCallbacks, poseIntegratorCallbacks, solveDescription);
            

        }

        public void Clear()
        {
            try
            {
                
                
                // ✅ SIMPLIFICAR - eliminar lógica de masa especial
                // foreach (var cuerpo in Cuerpos.OfType<simBotella>())
                // {
                //     cuerpo.RestoreOriginalMassIfNeeded(); // NO SE USA MÁS
                // }
                
                // ✅ CONSERVAR - limpiar contactos
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                    _descarteContacts.Clear();
                    _botellasParaEliminar.Clear();

                }
                
                        // ✅ NUEVO - limpiar clasificaciones
        _transportHandles.Clear();
        _curveHandles.Clear();
        _barrierHandles.Clear();
        _discardHandles.Clear();
        _bottleHandles.Clear();
        _guiaHandles.Clear();

                // ✅ CONSERVAR - resto del método igual
                var cuerposToRemove = new List<simBase>(Cuerpos);
                foreach (var cuerpo in cuerposToRemove)
                {
                    try
                    {
                        if (cuerpo != null)
                        {
                            cuerpo.RemoverBody();
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error removing body - continue with cleanup
                    }
                }

                Cuerpos.Clear();
                _deferredActions.Clear();
                
                // ✅ NUEVO - Limpiar todas las dimensiones almacenadas
                CtrEditor.ObjetosSim.osBase.ClearAllStoredDimensions();

                // Limpiar la simulación completamente si existe
                if (simulation != null)
                {
                    try
                    {
                        // Forzar un timestep pequeño para limpiar contactos pendientes
                        simulation.Timestep(1f / 1000f); // 1ms
                    }
                    catch (Exception ex)
                    {
                        // Warning during cleanup timestep - continue
                    }
                }

                // Limpiar visualización 3D
                Visualization3DManager?.Clear();
            }
            catch (Exception ex)
            {
                // Critical error during clear - operation failed
            }
        }

        public void Start()
        {
            try
            {
                // ✅ INICIALIZAR PROPIEDADES CACHEADAS
                foreach (var transport in Cuerpos.OfType<simTransporte>())
                {
                    transport.UpdateCachedProperties();
                }
                
                foreach (var curve in Cuerpos.OfType<simCurve>())
                {
                    // ✅ CORREGIDO: Usar método público para actualizar velocidad
                    curve.SetSpeed(curve.Speed); // ✅ SIMPLIFICADO: Reinicializar velocidad
                }
                
                stopwatch.Start();
                stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[SimulationManager] Error in Start: {ex.Message}");
                stopwatch.Start();
                stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
            }
        }

        public void Step()
        {
            try
            {
                _frameCount++;
                
                // ✅ CONSERVAR - validación de deltaTime
                var currentTime = stopwatch.Elapsed.TotalMilliseconds;
                var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0);
                stopwatch_last = currentTime;

                if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
                {
                    deltaTime = 1f / 60f;
                }

                const float maxDeltaTime = 0.1f;
                if (deltaTime > maxDeltaTime)
                {
                    deltaTime = 1f / 60f;
                }

                // ✅ CONSERVAR - procesar acciones diferidas
                foreach (var action in _deferredActions)
                {
                    action();
                }
                _deferredActions.Clear();
                
                // ✅ CONSERVAR - validaciones de simulación
                if (simulation?.Bodies == null)
                {
                    return;
                }

                var invalidBodies = Cuerpos.Where(c => c != null && !simulation.Bodies.BodyExists(c.BodyHandle)).ToList();
                if (invalidBodies.Count > 0)
                {
                    foreach (var invalidBody in invalidBodies)
                    {
                        Cuerpos.Remove(invalidBody);
                    }
                }

                // ✅ CONSERVAR - timestep
                var timestepValue = Math.Max(deltaTime, 1f / 120f);

                try
                {
                    simulation.Timestep(timestepValue);
                }
                catch (AccessViolationException ex)
                {
                    lock (_contactsLock)
                    {
                        _barreraContacts.Clear();
                    }
                    throw;
                }



                // ✅ CONSERVAR - limitación de rotación y mantener botellas despiertas
                foreach (var cuerpo in Cuerpos.OfType<simBotella>().ToList())
                {
                    try
                    {
                        if (simulation.Bodies.BodyExists(cuerpo.BodyHandle))
                        {
                            cuerpo.LimitRotationToXYPlane();
                            simulation.Awakener.AwakenBody(cuerpo.BodyHandle);
                        }
                    }
                    catch (Exception ex)
                    {
                        // Error limiting rotation for bottle - continue
                    }
                }

                // ✅ NUEVO: Detener motores de botellas que no están en contacto con elementos
                // Esto se ejecuta cada 10 frames para eficiencia
                if (_frameCount % 10 == 0)
                {
                    //StopMotorsForBottlesNotInContact();
                }

                // ✅ CONSERVAR - sistemas que funcionan bien
                ProcessBarreraContacts();
                ProcessDescarteContacts();
                ProcessCleanupSystem();

                // ✅ SIMPLIFICAR - limpiar solo contactos que usamos
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                    _descarteContacts.Clear();
                }

                // ✅ CONSERVAR - sincronización 3D
                if (Is3DUpdateEnabled)
                {
                    Visualization3DManager?.SynchronizeWorld();
                }
            }
            catch (Exception ex)
            {
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                }
            }
        }



        /// <summary>
        /// ✅ NUEVO: Marca un objeto para eliminación diferida (más seguro)
        /// </summary>
        public void Remove(simBase Objeto)
        {
            if (Objeto == null) return;
            
            try
            {
                // ✅ SIMPLIFICADO - eliminar lógica de masa especial
                UnregisterObjectHandle(Objeto); // ✅ NUEVO
                Objeto.RemoverBody(); // ✅ CONSERVAR - remover el cuerpo de BEPU

                // ✅ NUEVO - Limpiar dimensiones almacenadas en osBase
                CtrEditor.ObjetosSim.osBase.ClearStoredDimensions(Objeto);
                
                
                // ✅ REMOVER de la lista inmediatamente para evitar referencias colgantes
                Cuerpos.Remove(Objeto);

                if (Is3DUpdateEnabled)
                {
                    Visualization3DManager?.SynchronizeWorld();
                }
                
                //System.Diagnostics.Debug.WriteLine($"[Remove] ✅ Objeto marcado para eliminación diferida: {Objeto.GetType().Name}");
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[Remove] ❌ Error marcando objeto {Objeto?.GetType().Name}: {ex.Message}");
                
                // ✅ CRÍTICO: Siempre remover de la lista incluso si falló
                Cuerpos.Remove(Objeto);
            }
        }
       

        public simBotella AddCircle(float diameter, Vector2 position, float mass)
        {
            var botella = new simBotella(simulation, _deferredActions, diameter, position, mass);
            Cuerpos.Add(botella);
            RegisterObjectHandle(botella); // ✅ NUEVO

            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return botella;
        }

        public simTransporte AddRectangle(float width, float height, Vector2 position, float angle)
        {
            var transporte = new simTransporte(simulation, _deferredActions, width, height, position, angle, this);
            Cuerpos.Add(transporte);
            RegisterObjectHandle(transporte); // ✅ NUEVO - incluye UpdateCachedProperties()
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return transporte;
        }

        internal simBarrera AddBarrera(float width, float height, Vector2 wpfTopLeft, float wpfAngle, bool detectarCuello)
        {
            // ✅ CONVERTIR coordenadas WPF a BEPU internas
            var zPosition = simBase.zPos_Barrera;
            var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);
            var bepuRadians = CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle);
            
            var barrera = new simBarrera(simulation, width, height, bepuCenter, bepuRadians, detectarCuello, this);
            Cuerpos.Add(barrera);
            RegisterObjectHandle(barrera);
                       
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return barrera;
        }



        public simGuia AddLine(float width, float height, Vector2 topLeft, float angle)
        {
            var guia = new simGuia(simulation, _deferredActions, width, height, topLeft, angle);
            Cuerpos.Add(guia);
            // ✅ NOTA: simGuia no requiere registro especial
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return guia;
        }

        public simDescarte AddDescarte(float diameter, Vector2 position)
        {
            var descarte = new simDescarte(simulation, _deferredActions, diameter, position);
            Cuerpos.Add(descarte);
            RegisterObjectHandle(descarte); // ✅ NUEVO
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return descarte;
        }

        public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
        {
            var curve = new simCurve(simulation, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, topLeft, unused, this);
            Cuerpos.Add(curve);
            RegisterObjectHandle(curve); // ✅ NUEVO
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            return curve;
        }

        // ✅ NUEVA INTERFAZ PARA BARRERAS - Solo accesible desde SimulationManagerBEPU
        
        /// <summary>
        /// Crea una nueva barrera y devuelve un handle opaco para WPF
        /// </summary>
        /// <param name="width">Ancho en metros</param>
        /// <param name="height">Alto en metros</param>
        /// <param name="bepuCenter">Posición central en coordenadas BEPU</param>
        /// <param name="bepuRadians">Ángulo en radianes BEPU</param>
        /// <param name="detectarCuello">Si debe detectar el cuello de las botellas</param>
        /// <returns>Handle opaco para referenciar la barrera desde WPF</returns>
        public simBarrera CreateBarrera(float width, float height, Vector2 TopLeft, float wpfAngle, bool detectarCuello)
        {
            var barrera = new simBarrera(simulation, width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width,  height,  wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle), detectarCuello, this);
            Cuerpos.Add(barrera);
            RegisterObjectHandle(barrera);
            
            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
            
            return barrera;
        }

        public void UpdateBarrera(simBarrera barrera, float width, float height, Vector2 TopLeft, float wpfAngle)
        {
            barrera.Update(width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width, height, wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle));

            if (Is3DUpdateEnabled)
            {
                Visualization3DManager?.SynchronizeWorld();
            }
        }


        /// <summary>
        /// Elimina una barrera usando su handle
        /// </summary>
        /// <param name="handle">Handle de la barrera a eliminar</param>
        public void RemoveBarrera(simBarrera barrera)
        {
            if (barrera != null)
            {
                Remove(barrera);
            }
        }

        /// <summary>
        /// Configura si la barrera debe detectar el cuello de las botellas
        /// </summary>
        /// <param name="handle">Handle de la barrera</param>
        /// <param name="detectarCuello">True para detectar cuello, false para detectar botella completa</param>
        public void SetBarreraDetectNeck(simBarrera barrera, bool detectarCuello)
        {
            if (barrera != null)
            {
                barrera.DetectNeck = detectarCuello;
            }
        }

        /// <summary>
        /// Obtiene los datos de estado de la barrera
        /// </summary>
        /// <param name="handle">Handle de la barrera</param>
        /// <returns>Estructura con los datos de luz cortada</returns>
        public BarreraData GetBarreraData(simBarrera barrera)
        {
            if (barrera != null)
            {
                return new BarreraData(barrera.LuzCortada, barrera.LuzCortadaNeck);
            }
            return new BarreraData(false, false);
        }

        /// <summary>
        /// ✅ NUEVO: Detiene motores de botellas que no están en contacto con elementos
        /// </summary>
        private void StopMotorsForBottlesNotInContact()
        {
            try
            {
                var allBottles = Cuerpos.OfType<simBotella>().ToList();
                
                foreach (var bottle in allBottles)
                {
                    if (bottle != null && bottle.HasMotor && bottle.IsOnElement)
                    {
                        // ✅ VERIFICAR SI LA BOTELLA ESTÁ REALMENTE EN CONTACTO CON ALGÚN ELEMENTO
                        bool isInContact = false;
                        
                        // Verificar contacto con transportes
                        foreach (var transport in Cuerpos.OfType<simTransporte>())
                        {
                            if (IsBottleInContactWithTransport(bottle, transport))
                            {
                                isInContact = true;
                                break;
                            }
                        }
                        
                        // Verificar contacto con curvas
                        if (!isInContact)
                        {
                            foreach (var curve in Cuerpos.OfType<simCurve>())
                            {
                                if (IsBottleInContactWithCurve(bottle, curve))
                                {
                                    isInContact = true;
                                    break;
                                }
                            }
                        }
                        
                        // ✅ DETENER MOTOR SI NO ESTÁ EN CONTACTO
                        if (!isInContact)
                        {
                            bottle.StopMotor();
                            System.Diagnostics.Debug.WriteLine($"[StopMotorsForBottlesNotInContact] ⏹️ Detenido: {bottle.BodyHandle}");
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[StopMotorsForBottlesNotInContact] ❌ ERROR: {ex.Message}");
            }
        }

        /// <summary>
        /// ✅ NUEVO: Verifica si una botella está en contacto con un transporte
        /// </summary>
        private bool IsBottleInContactWithTransport(simBotella bottle, simTransporte transport)
        {
            try
            {
                var bottlePos = bottle.GetPosition();
                var transportPos = transport.GetPosition();
                var distance = Vector2.Distance(
                    new Vector2(bottlePos.X, bottlePos.Y),
                    new Vector2(transportPos.X, transportPos.Y)
                );
                
                // Verificar si está dentro del área del transporte + tolerancia
                var maxDistance = Math.Max(transport.Width, transport.Height) / 2f + bottle.Radius + 0.1f;
                return distance <= maxDistance;
            }
            catch
            {
                return false;
            }
        }

        /// <summary>
        /// ✅ NUEVO: Verifica si una botella está en contacto con una curva
        /// </summary>
        private bool IsBottleInContactWithCurve(simBotella bottle, simCurve curve)
        {
            try
            {
                var bottlePos = bottle.GetPosition();
                var curveCenter = curve.CurveCenter; // ✅ NUEVO: Usar centro real
                var distance = Vector2.Distance(
                    new Vector2(bottlePos.X, bottlePos.Y),
                    new Vector2(curveCenter.X, curveCenter.Y)
                );
                
                // Verificar si está dentro del área de la curva + tolerancia
                var maxDistance = curve.OuterRadius + bottle.Radius + 0.1f;
                return distance <= maxDistance;
            }
            catch
            {
                return false;
            }
        }


        /// <summary>
        /// ✅ NUEVO: Calcula la dirección tangencial específica basada en la posición de la botella
        /// </summary>
        public Vector3 CalculateCurveDirectionFromBottlePosition(simCurve curve, simBotella bottle)
        {
            try
            {
                // ✅ NUEVO: Usar el centro real almacenado de la curva
                var curveCenter = curve.CurveCenter;
                var bottlePosition = bottle.GetPosition();
                
                // Calcular el vector desde el centro de la curva hasta la botella (en el plano XY)
                var radiusVector = new Vector3(bottlePosition.X - curveCenter.X, bottlePosition.Y - curveCenter.Y, 0f);
                var radius = radiusVector.Length();
                
                if (radius < 0.001f)
                {
                    System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] ⚠️ Botella muy cerca del centro de la curva");
                    return Vector3.UnitX; // Fallback
                }
                
                // Normalizar el vector radial
                var normalizedRadius = radiusVector / radius;
                
                // Calcular la dirección tangencial (perpendicular al radio)
                // En coordenadas 2D: si r = (x, y), entonces t = (-y, x) es tangente
                var tangentDirection = new Vector3(-normalizedRadius.Y, normalizedRadius.X, 0f);
                
                // Verificar que la dirección tangencial apunte en el sentido correcto según la velocidad de la curva
                if (curve.Speed < 0)
                {
                    //tangentDirection = -tangentDirection;
                }
                
                //System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] 📐 Dirección calculada:");
                //System.Diagnostics.Debug.WriteLine($"   Centro curva: {curveCenter}");
                //System.Diagnostics.Debug.WriteLine($"   Posición botella: {bottlePosition}");
                //System.Diagnostics.Debug.WriteLine($"   Radio: {radius:F3}");
                //System.Diagnostics.Debug.WriteLine($"   Vector radial: {normalizedRadius}");
                //System.Diagnostics.Debug.WriteLine($"   Dirección tangencial: {tangentDirection} (Longitud: {tangentDirection.Length():F3})");
                
                return -tangentDirection;
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[CalculateCurveDirectionFromBottlePosition] ❌ ERROR: {ex.Message}");
                return Vector3.UnitX; // Fallback
            }
        }


        public void Dispose()
        {
            Clear();
            simulation?.Dispose();
            bufferPool?.Clear();
        }


        /// <summary>
        /// Registra un contacto entre una barrera y una botella para detección de paso
        /// </summary>
        /// <param name="barrera">Barrera que detecta el paso</param>
        /// <param name="botella">Botella que está pasando</param>
        internal void RegisterBarreraContact(simBarrera barrera, simBotella botella)
        {
            if (barrera != null && botella != null)
            {
                lock (_contactsLock)
                {
                    if (!_barreraContacts.ContainsKey(barrera))
                    {
                        _barreraContacts[barrera] = new List<simBotella>();
                    }

                    if (!_barreraContacts[barrera].Contains(botella))
                    {
                        _barreraContacts[barrera].Add(botella);
                    }
                }
            }
        }

        /// <summary>
        /// Registra un contacto entre un descarte y una botella para marcar eliminación
        /// </summary>
        /// <param name="descarte">Descarte que detecta la botella</param>
        /// <param name="botella">Botella que debe ser eliminada</param>
        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<simBotella>();
                    }

                    if (!_descarteContacts[descarte].Contains(botella))
                    {
                        _descarteContacts[descarte].Add(botella);
                    }
                }
            }
        }



        /// <summary>
        /// ✅ NUEVO: Procesa todas las barreras usando RayCast de BEPU
        /// Elimina la dependencia de contactos físicos y la necesidad de mantener botellas despiertas
        /// </summary>
        private void ProcessBarreraContacts()
        {
            try
            {
                // Obtener todas las barreras
                var barreras = Cuerpos.OfType<simBarrera>().ToList();

                foreach (var barrera in barreras)
                {
                    try
                    {
                        // Verificar que la barrera aún existe (solo verificar que no sea null)
                        if (barrera == null)
                            continue;

                        // ✅ USAR RAYCAST NATIVO DE BEPU
                        barrera.PerformRaycast();

                        // System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Barrera procesada: LuzCortada={barrera.LuzCortada}, LuzCortadaNeck={barrera.LuzCortadaNeck}, Distancia={barrera.Distancia:F3}");
                    }
                    catch (Exception ex)
                    {
                        System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error procesando barrera: {ex.Message}");
                    }
                }

                // Limpiar contactos ya que no los usamos más
                lock (_contactsLock)
                {
                    _barreraContacts.Clear();
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error crítico: {ex.Message}");
            }
        }

        // ✅ ELIMINADO: CalculateBarreraDetectionGeometric - ya no se usa
        // El nuevo sistema usa RayCast nativo de BEPU a través de simBarrera.PerformRaycast()

        /// <summary>
        /// Procesa contactos de descarte para eliminar botellas marcadas usando detección geométrica
        /// </summary>
        private void ProcessDescarteContacts()
        {
            try
            {
                // Obtener todos los descartes y botellas
                var descartes = Cuerpos.OfType<simDescarte>().ToList();
                var botellas = Cuerpos.OfType<simBotella>().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
            }
        }

        /// <summary>
        /// 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
        /// </summary>
        private void CalculateDescarteDetectionGeometric(simDescarte descarte, List<simBotella> 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<simBotella>();

                // 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
            }
        }

        /// <summary>
        /// Elimina las botellas marcadas para eliminación de forma segura
        /// </summary>
        private void RemoveMarkedBottles()
        {
            try
            {
                List<simBotella> botellasAEliminar;
                lock (_contactsLock)
                {
                    botellasAEliminar = new List<simBotella>(_botellasParaEliminar);
                    _botellasParaEliminar.Clear();
                }

                foreach (var botella in botellasAEliminar)
                {
                    try
                    {
                        if (botella != null && Cuerpos.Contains(botella))
                        {
                            //System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] 🗑️ Marcando botella para eliminación: {botella.BodyHandle}");
                            
                            // ✅ USAR ELIMINACIÓN DIFERIDA (más seguro)
                            Remove(botella);
                        }
                    }
                    catch (Exception ex)
                    {
                        System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ Error marcando botella {botella?.BodyHandle}: {ex.Message}");
                        // ✅ INTENTAR remover de la lista incluso si Remove falló
                        if (botella != null)
                        {
                            Cuerpos.Remove(botella);
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ ERROR CRÍTICO: {ex.Message}");
            }
        }

        /// <summary>
        /// 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
        /// </summary>
        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<simBotella>().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
            }
        }

        /// <summary>
        /// Proyecta un punto sobre una línea definida por dos puntos
        /// </summary>
        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;
        }
    }
}