CtrEditor/Documentation/Bulltet Sharp.md

34 KiB

using System; using System.Collections.Generic; using System.Diagnostics; using System.Linq; using System.Windows; using System.Windows.Controls; using System.Windows.Media; using System.Windows.Shapes; using BulletSharp; using BulletSharp.Math;

namespace CtrEditor.Simulacion { public class simBase { public RigidBody Body { get; protected set; } public DiscreteDynamicsWorld _world;

    public void RemoverBody()
    {
        if (Body != null && _world != null)
        {
            _world.RemoveRigidBody(Body);
            Body?.Dispose();
            Body = null;
        }
    }

    public static float Min(float Value, float Min = 0.01f)
    {
        return Value < Min ? Min : Value;
    }

    public static float GradosARadianes(float grados)
    {
        return (float)(grados * (Math.PI / 180));
    }

    public static float RadianesAGrados(float radianes)
    {
        return (float)(radianes * (180 / Math.PI));
    }

    public void SetPosition(float x, float y)
    {
        if (Body != null)
        {
            var transform = Body.WorldTransform;
            transform.Origin = new Vector3(x, y, 0);
            Body.WorldTransform = transform;
            Body.Activate();
        }
    }

    public void SetPosition(Vector3 centro)
    {
        try
        {
            if (Body != null)
            {
                var transform = Body.WorldTransform;
                transform.Origin = centro;
                Body.WorldTransform = transform;
                Body.Activate();
            }
        }
        catch
        {
        }
    }

    public Vector3 GetPosition()
    {
        return Body?.WorldTransform.Origin ?? Vector3.Zero;
    }
}

public class simCurve : simBase
{
    private float _innerRadius;
    private float _outerRadius;
    private float _startAngle;
    private float _endAngle;
    public float Speed { get; set; }

    private List<Action> _deferredActions;

    public simCurve(DiscreteDynamicsWorld world, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
    {
        _world = world;
        _deferredActions = deferredActions;
        _innerRadius = innerRadius;
        _outerRadius = outerRadius;
        _startAngle = GradosARadianes(startAngle);
        _endAngle = GradosARadianes(endAngle);
        Create(position);
    }

    public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
    {
        if (_world == null)
            return;
        _innerRadius = innerRadius;
        _outerRadius = outerRadius;
        _startAngle = GradosARadianes(startAngle);
        _endAngle = GradosARadianes(endAngle);
        Create(position);
    }

    public void Create(Vector3 position)
    {
        RemoverBody();

        // Crear una forma compuesta para la curva
        var compoundShape = new CompoundShape();
        
        // Crear segmentos de la curva como cajas pequeñas
        int segments = 32;
        float angleStep = (_endAngle - _startAngle) / segments;
        float thickness = 0.1f; // Grosor de la curva
        
        for (int i = 0; i < segments; i++)
        {
            float angle = _startAngle + i * angleStep;
            float midRadius = (_innerRadius + _outerRadius) / 2f;
            float segmentWidth = (_outerRadius - _innerRadius);
            float segmentLength = midRadius * angleStep;
            
            var segmentShape = new BoxShape(segmentLength / 2f, segmentWidth / 2f, thickness / 2f);
            
            var segmentTransform = Matrix.Identity;
            segmentTransform.Origin = new Vector3(
                midRadius * (float)Math.Cos(angle),
                midRadius * (float)Math.Sin(angle),
                0
            );
            segmentTransform = Matrix.RotationZ(angle) * segmentTransform;
            
            compoundShape.AddChildShape(segmentTransform, segmentShape);
        }

        var motionState = new DefaultMotionState(Matrix.Translation(position));
        var rbInfo = new RigidBodyConstructionInfo(0, motionState, compoundShape, Vector3.Zero);
        Body = new RigidBody(rbInfo);
        Body.UserObject = this;
        Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
        
        _world.AddRigidBody(Body);
    }

    public void SetSpeed(float speed)
    {
        Speed = speed;
    }

    public void ApplyCurveEffect(RigidBody bottle)
    {
        Vector3 centerToBottle = bottle.WorldTransform.Origin - Body.WorldTransform.Origin;
        float distanceToCenter = centerToBottle.Length;
        
        if (distanceToCenter >= _innerRadius && distanceToCenter <= _outerRadius)
        {
            float overlapPercentage = CalculateAngleOverlap(bottle);
            
            if (overlapPercentage > 0)
            {
                float speedMetersPerSecond = Speed / 60.0f;
                Vector3 tangent = new Vector3(-centerToBottle.Y, centerToBottle.X, 0);
                tangent.Normalize();
                
                if (speedMetersPerSecond < 0)
                    tangent = -tangent;
                
                Vector3 currentVelocity = bottle.LinearVelocity;
                float currentSpeed = currentVelocity.Length;
                float conveyorSpeed = Math.Abs(speedMetersPerSecond);
                float targetSpeed = Math.Max(currentSpeed, conveyorSpeed);
                
                Vector3 currentDir = currentVelocity;
                if (currentDir.LengthSquared > 0)
                    currentDir.Normalize();
                else
                    currentDir = tangent;
                
                Vector3 newDirection = currentDir * (1 - overlapPercentage) + tangent * overlapPercentage;
                newDirection.Normalize();
                
                bottle.LinearVelocity = newDirection * targetSpeed;
            }
        }
    }

    private float CalculateAngleOverlap(RigidBody bottle)
    {
        Vector3 centerToBottle = bottle.WorldTransform.Origin - Body.WorldTransform.Origin;
        float bottleAngle = (float)Math.Atan2(centerToBottle.Y, centerToBottle.X);
        
        float normalizedBottleAngle = bottleAngle < 0 ? bottleAngle + 2 * (float)Math.PI : bottleAngle;
        float normalizedStartAngle = _startAngle < 0 ? _startAngle + 2 * (float)Math.PI : _startAngle;
        float normalizedEndAngle = _endAngle < 0 ? _endAngle + 2 * (float)Math.PI : _endAngle;
        
        if (normalizedStartAngle > normalizedEndAngle)
        {
            if (!(normalizedBottleAngle >= normalizedStartAngle || normalizedBottleAngle <= normalizedEndAngle))
                return 0;
        }
        else
        {
            if (normalizedBottleAngle < normalizedStartAngle || normalizedBottleAngle > normalizedEndAngle)
                return 0;
        }
        
        float distanceToCenter = centerToBottle.Length;
        if (distanceToCenter < _innerRadius || distanceToCenter > _outerRadius)
            return 0;
        
        float angleFromStart, angleToEnd, totalAngle;
        
        if (normalizedStartAngle > normalizedEndAngle)
        {
            totalAngle = (2 * (float)Math.PI - normalizedStartAngle) + normalizedEndAngle;
            if (normalizedBottleAngle >= normalizedStartAngle)
            {
                angleFromStart = normalizedBottleAngle - normalizedStartAngle;
                angleToEnd = (2 * (float)Math.PI - normalizedBottleAngle) + normalizedEndAngle;
            }
            else
            {
                angleFromStart = (2 * (float)Math.PI - normalizedStartAngle) + normalizedBottleAngle;
                angleToEnd = normalizedEndAngle - normalizedBottleAngle;
            }
        }
        else
        {
            angleFromStart = normalizedBottleAngle - normalizedStartAngle;
            angleToEnd = normalizedEndAngle - normalizedBottleAngle;
            totalAngle = normalizedEndAngle - normalizedStartAngle;
        }
        
        float minAngleDistance = Math.Min(angleFromStart, angleToEnd);
        float overlapPercentage = Math.Min(minAngleDistance / (totalAngle * 0.1f), 1.0f);
        return overlapPercentage;
    }
}

public class simDescarte : simBase
{
    private float _radius;
    private List<Action> _deferredActions;

    public simDescarte(DiscreteDynamicsWorld world, List<Action> deferredActions, float diameter, Vector3 position)
    {
        _world = world;
        _deferredActions = deferredActions;
        _radius = diameter / 2;
        Create(position);
    }

    public void SetDiameter(float diameter)
    {
        if (diameter <= 0)
            diameter = 0.01f;
        _radius = diameter / 2;
        Create(Body.WorldTransform.Origin);
    }

    public void Create(Vector3 position)
    {
        RemoverBody();
        
        var shape = new CylinderShape(_radius, _radius, 0.1f); // Cilindro plano para simular círculo 2D
        var motionState = new DefaultMotionState(Matrix.Translation(position));
        var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
        Body = new RigidBody(rbInfo);
        Body.UserObject = this;
        Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
        
        _world.AddRigidBody(Body);
    }
}

public class simTransporte : simBase
{
    public float Speed { get; set; }
    public float Friction { get; set; }
    public float DistanceGuide2Guide { get; set; }
    public bool isBrake { get; set; }
    public bool TransportWithGuides = false;
    
    private List<Action> _deferredActions;
    public float Width { get; set; }
    public float Height { get; set; }

    public simTransporte(DiscreteDynamicsWorld world, List<Action> deferredActions, float width, float height, Vector3 position, float angle = 0)
    {
        _world = world;
        _deferredActions = deferredActions;
        Create(width, height, position, angle);
    }

    public float Angle
    {
        get
        {
            var rotation = Body.WorldTransform.GetBasis();
            return RadianesAGrados((float)Math.Atan2(rotation.M21, rotation.M11));
        }
        set
        {
            var transform = Body.WorldTransform;
            transform.Basis = Matrix3x3.CreateRotationZ(GradosARadianes(value));
            Body.WorldTransform = transform;
        }
    }

    public new void SetPosition(float x, float y)
    {
        var transform = Body.WorldTransform;
        transform.Origin = new Vector3(x, y, 0);
        Body.WorldTransform = transform;
    }

    public void SetSpeed(float speed)
    {
        Speed = speed;
    }

    public void SetDimensions(float width, float height)
    {
        RemoverBody();
        Create(width, height, Body.WorldTransform.Origin, Angle);
    }

    public void Create(float width, float height, Vector3 position, float angle = 0)
    {
        RemoverBody();
        Width = width;
        Height = height;
        Friction = 0.1f;
        
        var shape = new BoxShape(width / 2, height / 2, 0.05f); // Caja plana para simular rectángulo 2D
        var transform = Matrix.Translation(position) * Matrix.RotationZ(GradosARadianes(angle));
        var motionState = new DefaultMotionState(transform);
        var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
        Body = new RigidBody(rbInfo);
        Body.UserObject = this;
        Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
        
        _world.AddRigidBody(Body);
    }
}

public class simBarrera : simBase
{
    public float Distancia;
    public int LuzCortada;
    public bool LuzCortadaNeck;
    public bool DetectNeck;
    public List<simBotella> ListSimBotellaContact;
    
    float _height;
    private List<Action> _deferredActions;

    public simBarrera(DiscreteDynamicsWorld world, List<Action> deferredActions, float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
    {
        _world = world;
        _height = Min(height);
        DetectNeck = detectectNeck;
        _deferredActions = deferredActions;
        ListSimBotellaContact = new List<simBotella>();
        Create(Min(width), _height, position, angle);
    }

    public float Angle
    {
        get
        {
            var rotation = Body.WorldTransform.GetBasis();
            return RadianesAGrados((float)Math.Atan2(rotation.M21, rotation.M11));
        }
        set
        {
            var transform = Body.WorldTransform;
            transform.Basis = Matrix3x3.CreateRotationZ(GradosARadianes(value));
            Body.WorldTransform = transform;
        }
    }

    public new void SetPosition(float x, float y)
    {
        var transform = Body.WorldTransform;
        transform.Origin = new Vector3(x, y, 0);
        Body.WorldTransform = transform;
    }

    public void SetDimensions(float width, float height)
    {
        RemoverBody();
        Create(Min(width), Min(height), Body.WorldTransform.Origin, Angle);
    }

    public void Create(float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
    {
        RemoverBody();
        _height = Min(height);
        DetectNeck = detectectNeck;
        
        var shape = new BoxShape(Min(width) / 2, _height / 2, 0.05f);
        var transform = Matrix.Translation(position) * Matrix.RotationZ(GradosARadianes(angle));
        var motionState = new DefaultMotionState(transform);
        var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
        Body = new RigidBody(rbInfo);
        Body.UserObject = this;
        Body.CollisionFlags |= CollisionFlags.NoContactResponse; // Sensor
        
        _world.AddRigidBody(Body);
        LuzCortada = 0;
    }

    public void CheckIfNecksIsTouching()
    {
        if (LuzCortada == 0)
            return;
            
        foreach (var botella in ListSimBotellaContact)
        {
            Vector3 sensorCenter = Body.WorldTransform.Origin;
            float halfHeight = _height / 2;
            var rotation = Body.WorldTransform.GetBasis();
            float cos = rotation.M11;
            float sin = rotation.M21;
            
            Vector3 lineStart = sensorCenter + new Vector3(-halfHeight * cos, halfHeight * sin, 0);
            Vector3 lineEnd = sensorCenter + new Vector3(halfHeight * cos, -halfHeight * sin, 0);
            
            Vector3 fixtureCenter = botella.Body.WorldTransform.Origin;
            Vector3 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);
            
            float distance = (closestPoint - fixtureCenter).Length;
            Distancia = distance;
            
            if (distance <= botella._neckRadius)
            {
                LuzCortadaNeck = true;
                return;
            }
        }
        
        LuzCortadaNeck = false;
    }

    private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
    {
        Vector3 lineDirection = lineEnd - lineStart;
        lineDirection.Normalize();
        Vector3 pointToLineStart = point - lineStart;
        float projectionLength = Vector3.Dot(pointToLineStart, lineDirection);
        return lineStart + projectionLength * lineDirection;
    }
}

public class simGuia : simBase
{
    private List<Action> _deferredActions;

    public simGuia(DiscreteDynamicsWorld world, List<Action> deferredActions, Vector3 start, Vector3 end)
    {
        _world = world;
        _deferredActions = deferredActions;
        Create(start, end);
    }

    public void Create(Vector3 start, Vector3 end)
    {
        RemoverBody();
        
        Vector3 center = (start + end) / 2;
        Vector3 direction = end - start;
        float length = direction.Length;
        
        var shape = new BoxShape(length / 2, 0.01f, 0.05f); // Línea como caja muy delgada
        
        float angle = (float)Math.Atan2(direction.Y, direction.X);
        var transform = Matrix.Translation(center) * Matrix.RotationZ(angle);
        var motionState = new DefaultMotionState(transform);
        var rbInfo = new RigidBodyConstructionInfo(0, motionState, shape, Vector3.Zero);
        Body = new RigidBody(rbInfo);
        Body.UserObject = this;
        
        _world.AddRigidBody(Body);
    }

    public void UpdateVertices(Vector3 newStart, Vector3 newEnd)
    {
        Create(newStart, newEnd);
    }
}

public class simBotella : simBase
{
    public float Radius;
    private float _mass;
    public bool Descartar = false;
    public int isOnTransports;
    public List<simBase> ListOnTransports;
    public bool isRestricted;
    public bool isNoMoreRestricted;
    public float OriginalMass;
    public simTransporte ConveyorRestrictedTo;
    public RigidBody axisRestrictedBy;
    public float OverlapPercentage;
    public float _neckRadius;
    
    private List<Action> _deferredActions;
    private DiscreteDynamicsWorld _worldRef;

    public simBotella(DiscreteDynamicsWorld world, List<Action> deferredActions, float diameter, Vector3 position, float mass, float neckRadius = 0)
    {
        _world = world;
        _worldRef = world;
        ListOnTransports = new List<simBase>();
        _deferredActions = deferredActions;
        
        diameter = Min(diameter, 0.01f);
        Radius = diameter / 2;
        _mass = mass;
        
        if (neckRadius <= 0)
            neckRadius = diameter / 4;
        _neckRadius = neckRadius;
        Create(position);
    }

    public float CenterX => Body.WorldTransform.Origin.X;
    public float CenterY => Body.WorldTransform.Origin.Y;
    public Vector3 Center => Body.WorldTransform.Origin;

    public float Mass
    {
        get
        {
            if (_mass <= 0)
                _mass = 1;
            return _mass;
        }
        set
        {
            _mass = value;
        }
    }

    private void Create(Vector3 position)
    {
        RemoverBody();
        isOnTransports = 0;
        
        var shape = new CylinderShape(Radius, Radius, 0.1f); // Cilindro para simular círculo 2D
        var motionState = new DefaultMotionState(Matrix.Translation(position));
        
        Vector3 inertia;
        shape.CalculateLocalInertia(_mass, out inertia);
        var rbInfo = new RigidBodyConstructionInfo(_mass, motionState, shape, inertia);
        
        Body = new RigidBody(rbInfo);
        Body.UserObject = this;
        Body.SetDamping(3f, 1f); // Linear y angular damping
        Body.SetSleepingThresholds(0, 0); // No dormir
        Body.CcdMotionThreshold = 1e-7f;
        Body.CcdSweptSphereRadius = Radius * 0.2f;
        
        _world.AddRigidBody(Body);
    }

    public void SetDiameter(float diameter)
    {
        diameter = Min(diameter, 0.01f);
        Radius = diameter / 2;
        Create(Body.WorldTransform.Origin);
    }

    public void SetMass(float mass)
    {
        Mass = mass;
    }

    public void ApplyConveyorSpeed(float deltaTime_s)
    {
        foreach (var transporte in ListOnTransports)
        {
            if (transporte is simTransporte conveyorRect)
                ApplyConveyorEffect(deltaTime_s, conveyorRect);
            if (transporte is simCurve conveyorCurve)
                conveyorCurve.ApplyCurveEffect(Body);
        }
    }

    private bool ApplyConveyorEffect(float deltaTime_s, simTransporte conveyor)
    {
        float overlapPercentage = CalculateOverlapedArea(this, conveyor);
        OverlapPercentage = overlapPercentage;
        
        float speedMetersPerSecond = conveyor.Speed / 60.0f;
        var rotation = conveyor.Body.WorldTransform.GetBasis();
        Vector3 desiredVelocity = new Vector3(rotation.M11, rotation.M21, 0) * speedMetersPerSecond;
        
        Vector3 currentVelocity = Body.LinearVelocity;
        Vector3 velocityDifference = desiredVelocity - currentVelocity;
        
        float proporcionalVelocityNeeded = 1 - CalculateProportion(currentVelocity, desiredVelocity);
        float frictionCoefficient;
        
        switch (proporcionalVelocityNeeded)
        {
            case > 0.3f:
                frictionCoefficient = conveyor.Friction * overlapPercentage;
                break;
            default:
                frictionCoefficient = proporcionalVelocityNeeded * overlapPercentage;
                break;
        }

        if (isRestricted && conveyor == ConveyorRestrictedTo && overlapPercentage > 0.5f)
        {
            Body.LinearVelocity = desiredVelocity;
            return true;
        }

        Body.LinearVelocity += frictionCoefficient * desiredVelocity;
        return false;
    }

    public float CalculateOverlapedArea(simBotella botella, simTransporte conveyor)
    {
        // Implementación simplificada del cálculo de área superpuesta
        Vector3 bottlePos = botella.Body.WorldTransform.Origin;
        Vector3 conveyorPos = conveyor.Body.WorldTransform.Origin;
        
        // Transformar posición de botella al espacio local del transportador
        Matrix invTransform = conveyor.Body.WorldTransform.Inverse();
        Vector3 localBottlePos = Vector3.TransformCoordinate(bottlePos, invTransform);
        
        // Verificar si la botella está dentro del rectángulo del transportador
        float halfWidth = conveyor.Width / 2;
        float halfHeight = conveyor.Height / 2;
        
        if (Math.Abs(localBottlePos.X) <= halfWidth + botella.Radius &&
            Math.Abs(localBottlePos.Y) <= halfHeight + botella.Radius)
        {
            // Calcular área superpuesta aproximada
            float overlapX = Math.Max(0, Math.Min(halfWidth, localBottlePos.X + botella.Radius) - Math.Max(-halfWidth, localBottlePos.X - botella.Radius));
            float overlapY = Math.Max(0, Math.Min(halfHeight, localBottlePos.Y + botella.Radius) - Math.Max(-halfHeight, localBottlePos.Y - botella.Radius));
            
            float overlapArea = overlapX * overlapY;
            float bottleArea = (float)(Math.PI * botella.Radius * botella.Radius);
            
            return Math.Min(1.0f, overlapArea / bottleArea);
        }
        
        return 0;
    }

    public static float CalculateProportion(Vector3 currentVelocity, Vector3 desiredVelocity)
    {
        float dotProduct = Vector3.Dot(desiredVelocity, currentVelocity);
        float magnitudeV1Squared = desiredVelocity.LengthSquared;
        
        if (magnitudeV1Squared == 0)
            return 0;
        
        float proportion = dotProduct / magnitudeV1Squared;
        return proportion;
    }

    public void CenterFixtureOnConveyor()
    {
        if (!isRestricted || ConveyorRestrictedTo == null)
            return;
            
        Vector3 conveyorCenter = ConveyorRestrictedTo.Body.WorldTransform.Origin;
        float halfDistance = ConveyorRestrictedTo.DistanceGuide2Guide / 2;
        var rotation = ConveyorRestrictedTo.Body.WorldTransform.GetBasis();
        
        Vector3 offset = new Vector3(halfDistance * rotation.M11, halfDistance * rotation.M21, 0);
        Vector3 lineStart = conveyorCenter - offset;
        Vector3 lineEnd = conveyorCenter + offset;
        
        Vector3 fixtureCenter = Body.WorldTransform.Origin;
        Vector3 closestPoint = ProjectPointOntoLine(fixtureCenter, lineStart, lineEnd);
        
        var transform = Body.WorldTransform;
        transform.Origin = closestPoint;
        Body.WorldTransform = transform;
    }

    private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
    {
        Vector3 lineDirection = lineEnd - lineStart;
        lineDirection.Normalize();
        Vector3 pointToLineStart = point - lineStart;
        float projectionLength = Vector3.Dot(pointToLineStart, lineDirection);
        return lineStart + projectionLength * lineDirection;
    }

    public bool IsOnAnyTransport()
    {
        return isOnTransports > 0;
    }
}

public class SimulationManagerFP
{
    private DiscreteDynamicsWorld world;
    private Canvas simulationCanvas;
    public List<simBase> Cuerpos;
    public List<Action> _deferredActions;
    private Stopwatch stopwatch;
    private double stopwatch_last;
    
    // Componentes de Bullet Physics
    private DefaultCollisionConfiguration collisionConfiguration;
    private CollisionDispatcher dispatcher;
    private DbvtBroadphase broadphase;
    private SequentialImpulseConstraintSolver solver;

    public Canvas DebugCanvas { get => simulationCanvas; set => simulationCanvas = value; }

    public SimulationManagerFP()
    {
        // Inicializar Bullet Physics
        collisionConfiguration = new DefaultCollisionConfiguration();
        dispatcher = new CollisionDispatcher(collisionConfiguration);
        broadphase = new DbvtBroadphase();
        solver = new SequentialImpulseConstraintSolver();
        
        world = new DiscreteDynamicsWorld(dispatcher, broadphase, solver, collisionConfiguration);
        world.Gravity = new Vector3(0, 0, 0); // Sin gravedad para simular 2D
        
        Cuerpos = new List<simBase>();
        _deferredActions = new List<Action>();
        stopwatch = new Stopwatch();
        stopwatch.Start();
    }

    public void Clear()
    {
        if (world.NumCollisionObjects > 0)
        {
            for (int i = world.NumCollisionObjects - 1; i >= 0; i--)
            {
                var obj = world.CollisionObjectArray[i];
                var body = RigidBody.Upcast(obj);
                if (body != null && body.MotionState != null)
                {
                    body.MotionState.Dispose();
                }
                world.RemoveCollisionObject(obj);
                obj.Dispose();
            }
        }
        
        if (Cuerpos.Count > 0)
            Cuerpos.Clear();
    }

    public void Start()
    {
        stopwatch.Start();
        stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
    }

    public void Step()
    {
        float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last);
        stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
        
        world.StepSimulation(elapsedMilliseconds / 1000.0f, 10);
        
        // Detectar colisiones manualmente para sensores
        CheckCollisions();
        
        foreach (var cuerpo in Cuerpos)
        {
            if (cuerpo is simBotella botella)
            {
                botella.ApplyConveyorSpeed(elapsedMilliseconds / 1000);
                if (botella.isRestricted)
                {
                    botella.CenterFixtureOnConveyor();
                    botella.Body.SetMassProps(100, Vector3.Zero);
                }
                else if (botella.isNoMoreRestricted)
                {
                    botella.isNoMoreRestricted = false;
                    botella.Body.SetMassProps(botella.OriginalMass, Vector3.Zero);
                }
            }
            else if (cuerpo is simBarrera barrera)
                barrera.CheckIfNecksIsTouching();
        }
        
        foreach (var action in _deferredActions)
        {
            action();
        }
        
        _deferredActions.Clear();
    }

    private void CheckCollisions()
    {
        int numManifolds = world.Dispatcher.NumManifolds;
        
        for (int i = 0; i < numManifolds; i++)
        {
            PersistentManifold contactManifold = world.Dispatcher.GetManifoldByIndexInternal(i);
            var obA = contactManifold.Body0;
            var obB = contactManifold.Body1;
            
            var bodyA = RigidBody.Upcast(obA);
            var bodyB = RigidBody.Upcast(obB);
            
            if (bodyA?.UserObject is simBotella botella && bodyB?.UserObject is simBase other)
            {
                HandleCollision(botella, other, bodyB);
            }
            else if (bodyB?.UserObject is simBotella botella2 && bodyA?.UserObject is simBase other2)
            {
                HandleCollision(botella2, other2, bodyA);
            }
        }
    }

    private void HandleCollision(simBotella botella, simBase other, RigidBody otherBody)
    {
        switch (other)
        {
            case simBarrera sensor:
                if (!sensor.ListSimBotellaContact.Contains(botella))
                {
                    sensor.LuzCortada += 1;
                    sensor.ListSimBotellaContact.Add(botella);
                }
                break;
                
            case simCurve curve:
                if (!botella.ListOnTransports.Contains(curve))
                {
                    botella.isOnTransports += 1;
                    botella.ListOnTransports.Add(curve);
                }
                break;
                
            case simDescarte:
                botella.Descartar = true;
                break;
                
            case simTransporte conveyor:
                if (!botella.ListOnTransports.Contains(conveyor))
                {
                    botella.isOnTransports += 1;
                    botella.ListOnTransports.Add(conveyor);
                    
                    if (conveyor.TransportWithGuides && conveyor.isBrake)
                    {
                        botella.ConveyorRestrictedTo = conveyor;
                        botella.axisRestrictedBy = otherBody;
                        botella.OriginalMass = botella.Body.Mass;
                        botella.isRestricted = true;
                        botella.isNoMoreRestricted = false;
                    }
                }
                break;
        }
    }

    public void Remove(simBase Objeto)
    {
        if (Objeto != null)
        {
            Objeto.RemoverBody();
            Cuerpos.Remove(Objeto);
        }
    }

    public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector3 position)
    {
        simCurve curva = new simCurve(world, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, position);
        Cuerpos.Add(curva);
        return curva;
    }

    public simBotella AddCircle(float diameter, Vector3 position, float mass)
    {
        simBotella circle = new simBotella(world, _deferredActions, diameter, position, mass);
        Cuerpos.Add(circle);
        return circle;
    }

    public simTransporte AddRectangle(float width, float height, Vector3 position, float angle)
    {
        simTransporte rectangle = new simTransporte(world, _deferredActions, width, height, position, angle);
        Cuerpos.Add(rectangle);
        return rectangle;
    }

    public simBarrera AddBarrera(float width, float height, Vector3 position, float angle, bool detectarCuello)
    {
        simBarrera rectangle = new simBarrera(world, _deferredActions, width, height, position, angle, detectarCuello);
        Cuerpos.Add(rectangle);
        return rectangle;
    }

    public simGuia AddLine(Vector3 start, Vector3 end)
    {
        simGuia line = new simGuia(world, _deferredActions, start, end);
        Cuerpos.Add(line);
        return line;
    }

    public simDescarte AddDescarte(float diameter, Vector3 position)
    {
        simDescarte descarte = new simDescarte(world, _deferredActions, diameter, position);
        Cuerpos.Add(descarte);
        return descarte;
    }

    ~SimulationManagerFP()
    {
        Clear();
        world?.Dispose();
        solver?.Dispose();
        broadphase?.Dispose();
        dispatcher?.Dispose();
        collisionConfiguration?.Dispose();
    }
}

}