diff --git a/Documentation/Bulltet Sharp.md b/Documentation/Bulltet Sharp.md new file mode 100644 index 0000000..7f1eb16 --- /dev/null +++ b/Documentation/Bulltet Sharp.md @@ -0,0 +1,937 @@ +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 _deferredActions; + + public simCurve(DiscreteDynamicsWorld world, List 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 _deferredActions; + + public simDescarte(DiscreteDynamicsWorld world, List 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 _deferredActions; + public float Width { get; set; } + public float Height { get; set; } + + public simTransporte(DiscreteDynamicsWorld world, List 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 ListSimBotellaContact; + + float _height; + private List _deferredActions; + + public simBarrera(DiscreteDynamicsWorld world, List 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(); + 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 _deferredActions; + + public simGuia(DiscreteDynamicsWorld world, List 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 ListOnTransports; + public bool isRestricted; + public bool isNoMoreRestricted; + public float OriginalMass; + public simTransporte ConveyorRestrictedTo; + public RigidBody axisRestrictedBy; + public float OverlapPercentage; + public float _neckRadius; + + private List _deferredActions; + private DiscreteDynamicsWorld _worldRef; + + public simBotella(DiscreteDynamicsWorld world, List deferredActions, float diameter, Vector3 position, float mass, float neckRadius = 0) + { + _world = world; + _worldRef = world; + ListOnTransports = new List(); + _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 Cuerpos; + public List _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(); + _deferredActions = new List(); + 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(); + } + } +} \ No newline at end of file diff --git a/ObjetosSim/SensoresComandos/ucEncoderMotorLineal.xaml b/ObjetosSim/SensoresComandos/ucEncoderMotorLineal.xaml index d1f033c..fa4a94d 100644 --- a/ObjetosSim/SensoresComandos/ucEncoderMotorLineal.xaml +++ b/ObjetosSim/SensoresComandos/ucEncoderMotorLineal.xaml @@ -11,6 +11,6 @@ - + diff --git a/Simulacion/Aether.cs b/Simulacion/Aether.cs index 79d9c0e..b450eca 100644 --- a/Simulacion/Aether.cs +++ b/Simulacion/Aether.cs @@ -514,9 +514,6 @@ namespace CtrEditor.Simulacion public float OverlapPercentage; public float _neckRadius; PrismaticJoint _activeJoint; - public bool isBeingPressedByOtherBottles; - public int bottleContactCount; - public float debugPenetrationLevel; // Para debug: nivel de penetración actual private List _deferredActions; public simBotella(World world, List deferredActions, float diameter, Vector2 position, float mass, float neckRadius = 0) { @@ -586,8 +583,6 @@ namespace CtrEditor.Simulacion RemoverBody(); isOnTransports = 0; _activeJoint = null; - isBeingPressedByOtherBottles = false; - bottleContactCount = 0; Body = _world.CreateCircle(Radius, 1f, position); Body.BodyType = BodyType.Dynamic; // Restablecer manejador de eventos de colisión @@ -597,8 +592,8 @@ namespace CtrEditor.Simulacion // Configurar la fricción //Body.SetFriction(0.2f); // Configurar amortiguamiento - Body.LinearDamping = 3f; // Ajustar para controlar la reducción de la velocidad lineal - Body.AngularDamping = 1f; // Ajustar para controlar la reducción de la velocidad angular + Body.LinearDamping = 5f; // Ajustar para controlar la reducción de la velocidad lineal + Body.AngularDamping = 21f; // Ajustar para controlar la reducción de la velocidad angular //Body.SetRestitution(0f); // Baja restitución para menos rebote Body.SleepingAllowed = false; Body.IsBullet = true; @@ -653,11 +648,6 @@ namespace CtrEditor.Simulacion return true; // No aplicar respuestas físicas } - else if (fixtureB.Body.Tag is simBotella) - { - bottleContactCount++; - return true; // Permitir respuestas físicas entre botellas - } return true; // No aplicar respuestas físicas } @@ -712,11 +702,6 @@ namespace CtrEditor.Simulacion Sensor.LuzCortada -= 1; Sensor.ListSimBotellaContact.Remove(this); } - - if (fixtureB.Body.Tag is simBotella) - { - bottleContactCount = Math.Max(0, bottleContactCount - 1); - } } /// @@ -768,17 +753,8 @@ namespace CtrEditor.Simulacion return true; } - // Si la botella está siendo presionada por múltiples botellas, reducir la aplicación de fuerza - float pressureReductionFactor = 1.0f; - if (isBeingPressedByOtherBottles) - { - // Reducir el factor basándose en la cantidad de contactos - pressureReductionFactor = Math.Max(0.1f, 1.0f / (1.0f + bottleContactCount * 0.5f)); - } - - // Aplicar la fuerza de fricción en función del porcentaje de superficie sobrepuesta y la presión - Vector2 finalVelocityChange = frictionCoefficient * desiredVelocity * pressureReductionFactor; - Body.LinearVelocity += finalVelocityChange; + // Aplicar la fuerza de fricción en función del porcentaje de superficie sobrepuesta + Body.LinearVelocity += frictionCoefficient * desiredVelocity; //Body.ApplyForce(frictionForce * overlapPercentage); return false; } @@ -866,9 +842,6 @@ namespace CtrEditor.Simulacion _deferredActions = new List(); stopwatch = new Stopwatch(); stopwatch.Start(); - - // Configurar el callback PreSolve para manejar contactos entre botellas - world.ContactManager.PreSolve += OnPreSolve; } public void Clear() @@ -889,20 +862,13 @@ namespace CtrEditor.Simulacion public void Step() { - // Resetear las flags de presión al inicio del frame - ResetBottlePressureFlags(); - - // Verificar y corregir el espaciado entre botellas antes del step - PreventBottleOverlapping(); - - // Actualizar el estado de presión basado en contactos reales - UpdateBottlePressureStatus(); - // Detener el cronómetro y obtener el tiempo transcurrido en milisegundos float elapsedMilliseconds = (float)(stopwatch.Elapsed.TotalMilliseconds - stopwatch_last); stopwatch_last = stopwatch.Elapsed.TotalMilliseconds; // Pasar el tiempo transcurrido al método Step + world.Step(elapsedMilliseconds / 1000.0f); + foreach (var cuerpo in Cuerpos) { if (cuerpo is simBotella botella) @@ -912,7 +878,7 @@ namespace CtrEditor.Simulacion { botella.CenterFixtureOnConveyor(); botella.Body.Inertia = 0; - botella.Body.Mass = botella.OriginalMass * 100; + botella.Body.Mass = 100; } else if (botella.isNoMoreRestricted) { @@ -1084,212 +1050,5 @@ namespace CtrEditor.Simulacion return polygon; } - - private void OnPreSolve(Contact contact, ref nkast.Aether.Physics2D.Collision.Manifold oldManifold) - { - var fixtureA = contact.FixtureA; - var fixtureB = contact.FixtureB; - - // Verificar si el contacto es entre dos botellas - if (fixtureA.Body.Tag is simBotella botellaA && fixtureB.Body.Tag is simBotella botellaB) - { - // Calcular la distancia entre centros de las botellas - Vector2 centerA = botellaA.Body.Position; - Vector2 centerB = botellaB.Body.Position; - Vector2 direction = centerB - centerA; - float distance = direction.Length(); - - // Distancia mínima permitida (suma de radios + margen de seguridad) - float minDistance = botellaA.Radius + botellaB.Radius + 0.01f; // 1cm de margen - - // Calcular la velocidad relativa - Vector2 relativeVelocity = botellaB.Body.LinearVelocity - botellaA.Body.LinearVelocity; - float relativeSpeed = relativeVelocity.Length(); - - // Solo marcar como presionadas si hay verdadera interpenetración O múltiples contactos - if (distance < minDistance * 0.95f) // Solo si están muy cerca (interpenetración significativa) - { - botellaA.isBeingPressedByOtherBottles = true; - botellaB.isBeingPressedByOtherBottles = true; - - // Ajustar propiedades del contacto para reducir fuerzas explosivas - float penetrationRatio = Math.Max(0, (minDistance - distance) / minDistance); - - // Reducir restitución progresivamente según la penetración - contact.Restitution = Math.Min(contact.Restitution, 0.1f * (1.0f - penetrationRatio)); - - // Aumentar fricción para disipar energía - contact.Friction = Math.Max(contact.Friction, 0.7f + penetrationRatio * 0.2f); - } - - // Si la velocidad relativa es muy alta, también ajustar contacto - if (relativeSpeed > 2.0f) - { - contact.Restitution = 0.0f; // Sin rebote - contact.Friction = Math.Max(contact.Friction, 0.9f); - } - } - } - - public void ResetBottlePressureFlags() - { - foreach (var cuerpo in Cuerpos) - { - if (cuerpo is simBotella botella) - { - botella.isBeingPressedByOtherBottles = false; - } - } - } - - private void UpdateBottlePressureStatus() - { - var botellas = new List(); - - // Recopilar todas las botellas - foreach (var cuerpo in Cuerpos) - { - if (cuerpo is simBotella botella) - { - botellas.Add(botella); - } - } - - // Verificar el estado de presión de cada botella basado en sus contactos reales - foreach (var botella in botellas) - { - int proximityCount = 0; - float totalPenetration = 0; - - foreach (var otherBottle in botellas) - { - if (botella == otherBottle) continue; - - Vector2 direction = otherBottle.Body.Position - botella.Body.Position; - float distance = direction.Length(); - float minDistance = botella.Radius + otherBottle.Radius + 0.01f; // 1cm margen - - if (distance < minDistance) - { - proximityCount++; - totalPenetration += (minDistance - distance); - } - } - - // Actualizar información de debug - botella.debugPenetrationLevel = totalPenetration; - - // Solo marcar como presionada si tiene múltiples contactos cercanos O penetración significativa - // Y además la botella está en un transportador (para evitar marcar botellas libres) - if ((proximityCount >= 2 || totalPenetration > 0.01f) && botella.IsOnAnyTransport()) - { - botella.isBeingPressedByOtherBottles = true; - } - } - } - - private void PreventBottleOverlapping() - { - var botellas = new List(); - - // Recopilar todas las botellas - foreach (var cuerpo in Cuerpos) - { - if (cuerpo is simBotella botella) - { - botellas.Add(botella); - } - } - - // Verificar cada par de botellas múltiples veces para resolver interpenetraciones complejas - int iterations = 3; // Múltiples iteraciones para resolver conflictos - - for (int iter = 0; iter < iterations; iter++) - { - bool anyCorrection = false; - - for (int i = 0; i < botellas.Count; i++) - { - for (int j = i + 1; j < botellas.Count; j++) - { - var botellaA = botellas[i]; - var botellaB = botellas[j]; - - Vector2 centerA = botellaA.Body.Position; - Vector2 centerB = botellaB.Body.Position; - Vector2 direction = centerB - centerA; - float distance = direction.Length(); - - // Distancia mínima permitida (suma de radios + margen de seguridad) - float minDistance = botellaA.Radius + botellaB.Radius + 0.005f; // 5mm de margen - - // Si están demasiado cerca - if (distance < minDistance && distance > 0.001f) - { - anyCorrection = true; - - // Normalizar la dirección - direction.Normalize(); - - // Calcular la corrección necesaria - float penetration = minDistance - distance; - float correction = penetration * 0.5f; - - // Factor de corrección que decrece con las iteraciones - float correctionFactor = 0.3f / (iter + 1); // Más agresivo en la primera iteración - Vector2 correctionVector = direction * correction * correctionFactor; - - // Aplicar corrección considerando masas (botellas más pesadas se mueven menos) - float massA = botellaA.Body.Mass; - float massB = botellaB.Body.Mass; - float totalMass = massA + massB; - - float ratioA = massB / totalMass; // Botella más liviana se mueve más - float ratioB = massA / totalMass; - - // Solo mover si las botellas no están restringidas por transportadores - if (!botellaA.isRestricted) - { - Vector2 newPosA = centerA - correctionVector * ratioA; - if (!float.IsNaN(newPosA.X) && !float.IsNaN(newPosA.Y)) - { - botellaA.Body.Position = newPosA; - } - } - - if (!botellaB.isRestricted) - { - Vector2 newPosB = centerB + correctionVector * ratioB; - if (!float.IsNaN(newPosB.X) && !float.IsNaN(newPosB.Y)) - { - botellaB.Body.Position = newPosB; - } - } - - // Solo marcar como presionadas si hay interpenetración significativa - if (penetration > 0.005f) // Solo si penetran más de 5mm - { - botellaA.isBeingPressedByOtherBottles = true; - botellaB.isBeingPressedByOtherBottles = true; - } - - // Reducir velocidades para evitar acumulación de energía - float velocityDamping = 0.9f; - if (penetration > 0.01f) // Si la penetración es significativa (> 1cm) - { - velocityDamping = 0.7f; // Reducción más agresiva - } - - botellaA.Body.LinearVelocity *= velocityDamping; - botellaB.Body.LinearVelocity *= velocityDamping; - } - } - } - - // Si no hubo correcciones en esta iteración, podemos parar - if (!anyCorrection) - break; - } - } } } \ No newline at end of file