2616 lines
105 KiB
C#
2616 lines
105 KiB
C#
using System.Windows.Controls;
|
|
using System.Windows.Media;
|
|
using System.Windows.Shapes;
|
|
using System.Windows;
|
|
using System.Diagnostics;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Numerics;
|
|
using System.Linq;
|
|
using BepuPhysics;
|
|
using BepuPhysics.Collidables;
|
|
using BepuPhysics.CollisionDetection;
|
|
using BepuPhysics.Constraints;
|
|
using BepuUtilities;
|
|
using BepuUtilities.Memory;
|
|
using CtrEditor.FuncionesBase;
|
|
|
|
namespace CtrEditor.Simulacion
|
|
{
|
|
public class simBase
|
|
{
|
|
public BodyHandle BodyHandle { get; protected set; }
|
|
public Simulation _simulation;
|
|
protected bool _bodyCreated = false; // Bandera para saber si hemos creado un cuerpo
|
|
|
|
// Constantes para las posiciones Z de los objetos 3D
|
|
public const float zPos_Transporte = 0f; // Z de la parte baja
|
|
public const float zAltura_Transporte = 0.1f; // Altura del transporte sobre zPos
|
|
|
|
public const float zPos_Guia = 0.05f; // Z de la parte baja
|
|
public const float zAltura_Guia = 0.20f; // Altura de la guía sobre zPos
|
|
public const float zAltura_Barrera = zAltura_Guia; // Altura de la barrera sobre zPos
|
|
public const float zPos_Barrera = zPos_Guia; // Z de la parte baja
|
|
public const float zPos_Descarte = 0.1f; // Z de la parte baja
|
|
|
|
// Constantes para configuración
|
|
public const float zPos_Curve = zPos_Transporte; // Z de la parte baja de la curva
|
|
public const float zAltura_Curve = zAltura_Transporte; // Altura de la curva (triángulos planos)
|
|
|
|
|
|
public void RemoverBody()
|
|
{
|
|
// Solo intentar remover si realmente hemos creado un cuerpo antes
|
|
if (_bodyCreated && _simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
_simulation.Bodies.Remove(BodyHandle);
|
|
_bodyCreated = false; // Marcar como no creado después de remover
|
|
}
|
|
}
|
|
|
|
public static float Min(float Value, float Min = 0.01f)
|
|
{
|
|
return Math.Max(Value, Min);
|
|
}
|
|
|
|
public static float GradosARadianes(float grados)
|
|
{
|
|
return grados * (float)Math.PI / 180f;
|
|
}
|
|
|
|
public static float RadianesAGrados(float radianes)
|
|
{
|
|
return radianes * 180f / (float)Math.PI;
|
|
}
|
|
|
|
public void SetPosition(float x, float y, float z = 0)
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
bodyReference.Pose.Position = new Vector3(x, y, z);
|
|
}
|
|
}
|
|
|
|
public void SetPosition(Vector2 position)
|
|
{
|
|
// Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
|
|
// Mantener la coordenada Z actual para preservar la altura del objeto
|
|
var currentPosition = GetPosition();
|
|
SetPosition(position.X, -position.Y, currentPosition.Z);
|
|
}
|
|
|
|
public void SetPosition(Vector3 position)
|
|
{
|
|
SetPosition(position.X, position.Y, position.Z);
|
|
}
|
|
|
|
public Vector3 GetPosition()
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
return bodyReference.Pose.Position;
|
|
}
|
|
return Vector3.Zero;
|
|
}
|
|
|
|
public void SetRotation(float angleZ)
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
// ✅ Convertir de grados a radianes para Quaternion.CreateFromAxisAngle
|
|
bodyReference.Pose.Orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angleZ));
|
|
}
|
|
}
|
|
|
|
public float GetRotationZ()
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
// Extraer el ángulo Z del quaternion
|
|
var q = bodyReference.Pose.Orientation;
|
|
return (float)Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
|
|
}
|
|
return 0f;
|
|
}
|
|
}
|
|
|
|
public class simTransporte : simBase
|
|
{
|
|
public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)
|
|
public float Friction { get; set; } // Friccion para efectos de cinta transportadora
|
|
public float DistanceGuide2Guide { get; set; }
|
|
public bool isBrake { get; set; }
|
|
|
|
public bool TransportWithGuides = false;
|
|
private List<Action> _deferredActions;
|
|
public float Width { get; set; }
|
|
public float Height { get; set; }
|
|
|
|
public simTransporte(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
Width = width;
|
|
Height = height;
|
|
|
|
// Usar el nuevo método Create que maneja Top-Left correctamente
|
|
Create(width, height, topLeft, angle);
|
|
}
|
|
|
|
public float Angle
|
|
{
|
|
get
|
|
{
|
|
return GetRotationZ();
|
|
}
|
|
set
|
|
{
|
|
SetRotation(value);
|
|
}
|
|
}
|
|
|
|
public new void SetPosition(float x, float y, float z = 0)
|
|
{
|
|
base.SetPosition(x, y, z);
|
|
}
|
|
|
|
public void SetSpeed(float speed)
|
|
{
|
|
Speed = speed;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configura la velocidad del transporte en metros por segundo
|
|
/// Valores positivos mueven en la dirección del transporte, negativos en dirección opuesta
|
|
/// </summary>
|
|
/// <param name="speedMeterPerSecond">Velocidad en m/s (típicamente entre -5.0 y 5.0)</param>
|
|
public void SetTransportSpeed(float speedMeterPerSecond)
|
|
{
|
|
Speed = speedMeterPerSecond;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detiene completamente el transporte
|
|
/// </summary>
|
|
public void StopTransport()
|
|
{
|
|
Speed = 0f;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Invierte la dirección del transporte manteniendo la misma velocidad
|
|
/// </summary>
|
|
public void ReverseTransport()
|
|
{
|
|
Speed = -Speed;
|
|
}
|
|
|
|
public void SetDimensions(float width, float height)
|
|
{
|
|
Width = width;
|
|
Height = height;
|
|
|
|
// Actualizar la forma del cuerpo existente
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var box = new Box(width, height, zAltura_Transporte);
|
|
var shapeIndex = _simulation.Shapes.Add(box);
|
|
_simulation.Bodies.SetShape(BodyHandle, shapeIndex);
|
|
}
|
|
}
|
|
|
|
public void Create(float width, float height, Vector2 topLeft, float angle = 0)
|
|
{
|
|
// Calcular el offset del centro desde Top-Left (sin rotación)
|
|
var offsetX = width / 2f;
|
|
var offsetY = height / 2f;
|
|
|
|
// Rotar el offset alrededor del Top-Left usando el ángulo
|
|
// ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin
|
|
var angleRadians = GradosARadianes(angle);
|
|
var cos = (float)Math.Cos(angleRadians);
|
|
var sin = (float)Math.Sin(angleRadians);
|
|
|
|
var rotatedOffsetX = offsetX * cos - offsetY * sin;
|
|
var rotatedOffsetY = offsetX * sin + offsetY * cos;
|
|
|
|
// Calcular nueva posición del centro manteniendo Top-Left fijo
|
|
var centerX = topLeft.X + rotatedOffsetX;
|
|
var centerY = topLeft.Y + rotatedOffsetY;
|
|
|
|
// Convertir a 3D e invertir Y - Transportes en Z = Depth/2 para estar sobre el suelo
|
|
var center3D = new Vector3(centerX, -centerY, zAltura_Transporte / 2f + zPos_Transporte);
|
|
|
|
Create(width, height, center3D, -angle); // ✅ Invertir ángulo cuando se invierte Y
|
|
}
|
|
|
|
public void Create(float width, float height, Vector3 position, float angle = 0)
|
|
{
|
|
RemoverBody();
|
|
|
|
var box = new Box(width, height, zAltura_Transporte);
|
|
var shapeIndex = _simulation.Shapes.Add(box);
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(position, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angle))),
|
|
new CollidableDescription(shapeIndex, 0.1f),
|
|
new BodyActivityDescription(0.01f)
|
|
);
|
|
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true; // Marcar que hemos creado un cuerpo
|
|
}
|
|
}
|
|
|
|
public class simBarrera : simBase
|
|
{
|
|
public float Distancia;
|
|
public bool LuzCortada;
|
|
public bool LuzCortadaNeck;
|
|
public bool DetectNeck;
|
|
public List<simBotella> ListSimBotellaContact;
|
|
float _height;
|
|
private List<Action> _deferredActions;
|
|
public float Width { get; set; }
|
|
public float Height { get; set; }
|
|
|
|
public simBarrera(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0, bool detectectNeck = false)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
Width = width;
|
|
Height = height;
|
|
_height = height;
|
|
DetectNeck = detectectNeck;
|
|
ListSimBotellaContact = new List<simBotella>();
|
|
|
|
// Usar el nuevo método Create que maneja Top-Left correctamente
|
|
Create(width, height, topLeft, angle, detectectNeck);
|
|
}
|
|
|
|
public float Angle
|
|
{
|
|
get
|
|
{
|
|
return GetRotationZ();
|
|
}
|
|
set
|
|
{
|
|
SetRotation(value);
|
|
}
|
|
}
|
|
|
|
public new void SetPosition(float x, float y, float z = 0)
|
|
{
|
|
base.SetPosition(x, y, z);
|
|
}
|
|
|
|
public void SetDimensions(float width, float height)
|
|
{
|
|
_height = height;
|
|
// Actualizar la forma del cuerpo existente
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var box = new Box(width, height, zAltura_Barrera); // Altura de 20cm para detectar botellas
|
|
var shapeIndex = _simulation.Shapes.Add(box);
|
|
_simulation.Bodies.SetShape(BodyHandle, shapeIndex);
|
|
}
|
|
}
|
|
|
|
public void Create(float width, float height, Vector2 topLeft, float angle = 0, bool detectectNeck = false)
|
|
{
|
|
// Calcular el offset del centro desde Top-Left (sin rotación)
|
|
var offsetX = width / 2f;
|
|
var offsetY = height / 2f;
|
|
|
|
// Rotar el offset alrededor del Top-Left usando el ángulo
|
|
// ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin
|
|
var angleRadians = GradosARadianes(angle);
|
|
var cos = (float)Math.Cos(angleRadians);
|
|
var sin = (float)Math.Sin(angleRadians);
|
|
|
|
var rotatedOffsetX = offsetX * cos - offsetY * sin;
|
|
var rotatedOffsetY = offsetX * sin + offsetY * cos;
|
|
|
|
// Calcular nueva posición del centro manteniendo Top-Left fijo
|
|
var centerX = topLeft.X + rotatedOffsetX;
|
|
var centerY = topLeft.Y + rotatedOffsetY;
|
|
|
|
// Convertir a 3D e invertir Y - Sensores centrados en z=0.1 (rango 0-0.2) para detectar botellas
|
|
var center3D = new Vector3(centerX, -centerY, zPos_Barrera + zAltura_Barrera / 2f);
|
|
Create(width, height, center3D, -angle, detectectNeck); // ✅ Invertir ángulo cuando se invierte Y
|
|
}
|
|
|
|
public void Create(float width, float height, Vector3 position, float angle = 0, bool detectectNeck = false)
|
|
{
|
|
RemoverBody();
|
|
|
|
// Crear box con altura de 0.2m (desde z=0 hasta z=0.2) para funcionar como detector
|
|
var box = new Box(width, height, zAltura_Barrera); // Altura de 20cm para detectar botellas completas
|
|
var shapeIndex = _simulation.Shapes.Add(box);
|
|
|
|
// Crear como SENSOR (Kinematic con speculative margin 0 para detección pura)
|
|
// Configurar actividad para NUNCA dormir - los sensores deben estar siempre activos
|
|
var activityDescription = new BodyActivityDescription(-1f); // -1 = nunca dormir
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(position, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(angle))),
|
|
new CollidableDescription(shapeIndex, 0f), // Speculative margin 0 para sensores
|
|
activityDescription
|
|
);
|
|
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true; // Marcar que hemos creado un cuerpo
|
|
}
|
|
}
|
|
|
|
public class simGuia : simBase
|
|
{
|
|
private List<Action> _deferredActions;
|
|
|
|
// Propiedades para acceder a las dimensiones del objeto WPF
|
|
public float GuideThickness { get; set; } // Espesor de la guía
|
|
|
|
|
|
|
|
public simGuia(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
GuideThickness = height;
|
|
Create(width, height, topLeft, angle);
|
|
}
|
|
|
|
public void Create(float width, float height, Vector2 topLeft, float angle)
|
|
{
|
|
RemoverBody();
|
|
|
|
// Calcular el offset del centro desde Top-Left (sin rotación)
|
|
var offsetX = width / 2f;
|
|
var offsetY = height / 2f;
|
|
|
|
// Rotar el offset alrededor del Top-Left usando el ángulo
|
|
// ✅ Convertir de grados a radianes antes de usar Math.Cos/Sin
|
|
var angleRadians = GradosARadianes(angle);
|
|
var cos = (float)Math.Cos(angleRadians);
|
|
var sin = (float)Math.Sin(angleRadians);
|
|
|
|
var rotatedOffsetX = offsetX * cos - offsetY * sin;
|
|
var rotatedOffsetY = offsetX * sin + offsetY * cos;
|
|
|
|
// Calcular nueva posición del centro manteniendo Top-Left fijo
|
|
var centerX = topLeft.X + rotatedOffsetX;
|
|
var centerY = topLeft.Y + rotatedOffsetY;
|
|
|
|
// Convertir a 3D e invertir Y - Guías más altas que transportes
|
|
var center3D = new Vector3(centerX, -centerY, zAltura_Guia / 2 + zPos_Guia);
|
|
|
|
// Crear el Box con las dimensiones dadas - más alto para contener botellas
|
|
var box = new Box(width, GuideThickness, zAltura_Guia);
|
|
var shapeIndex = _simulation.Shapes.Add(box);
|
|
|
|
// ✅ Invertir ángulo cuando se invierte Y para coherencia de rotación
|
|
var orientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, GradosARadianes(-angle));
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(center3D, orientation),
|
|
new CollidableDescription(shapeIndex, 0.1f),
|
|
new BodyActivityDescription(0.01f)
|
|
);
|
|
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true;
|
|
}
|
|
|
|
// Método para actualizar las propiedades desde el objeto WPF
|
|
public void UpdateProperties(float ancho, float altoGuia, float angulo)
|
|
{
|
|
// Nota: ancho y angulo se ignoran ya que se calculan automáticamente desde start->end
|
|
GuideThickness = altoGuia;
|
|
}
|
|
}
|
|
|
|
public class simBotella : simBase
|
|
{
|
|
public float Radius;
|
|
public float Height; // Altura para la visualización del cilindro en Helix
|
|
private float _mass;
|
|
public bool Descartar = false;
|
|
public int isOnTransports;
|
|
public List<simBase> ListOnTransports;
|
|
public bool isRestricted;
|
|
public bool isNoMoreRestricted;
|
|
public float OriginalMass;
|
|
public simTransporte ConveyorRestrictedTo;
|
|
public float OverlapPercentage;
|
|
public float _neckRadius;
|
|
public bool isOnBrakeTransport; // Nueva propiedad para marcar si está en un transporte con freno
|
|
public simTransporte CurrentBrakeTransport { get; set; } // ✅ NUEVA: Referencia al transporte de freno actual
|
|
private List<Action> _deferredActions;
|
|
|
|
public simBotella(Simulation simulation, List<Action> deferredActions, float diameter, Vector2 position, float mass, float neckRadius = 0)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
Radius = diameter / 2f;
|
|
Height = diameter; // Altura igual al diámetro para mantener proporciones similares
|
|
_mass = mass;
|
|
OriginalMass = mass;
|
|
_neckRadius = neckRadius;
|
|
ListOnTransports = new List<simBase>();
|
|
|
|
// Convertir Vector2 a Vector3 con Z=0.2 (altura estándar para botellas)
|
|
// Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
|
|
var position3D = new Vector3(position.X, -position.Y, Radius + zPos_Transporte + zAltura_Transporte);
|
|
Create(position3D);
|
|
}
|
|
|
|
public float CenterX
|
|
{
|
|
get
|
|
{
|
|
return GetPosition().X;
|
|
}
|
|
set
|
|
{
|
|
var pos = GetPosition();
|
|
SetPosition(value, pos.Y, pos.Z);
|
|
}
|
|
}
|
|
|
|
public float CenterY
|
|
{
|
|
get
|
|
{
|
|
// Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo)
|
|
return -GetPosition().Y;
|
|
}
|
|
set
|
|
{
|
|
var pos = GetPosition();
|
|
// Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
|
|
SetPosition(pos.X, -value, pos.Z);
|
|
}
|
|
}
|
|
|
|
public Vector2 Center
|
|
{
|
|
get
|
|
{
|
|
var pos3D = GetPosition();
|
|
// Invertir Y de vuelta para convertir de 3D (Y hacia arriba) a WPF (Y hacia abajo)
|
|
return new Vector2(pos3D.X, -pos3D.Y);
|
|
}
|
|
set
|
|
{
|
|
// Mantener la Z actual, solo cambiar X, Y
|
|
var currentPos = GetPosition();
|
|
// Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
|
|
SetPosition(value.X, -value.Y, currentPos.Z);
|
|
}
|
|
}
|
|
|
|
public float Mass
|
|
{
|
|
get
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
return 1f / bodyReference.LocalInertia.InverseMass;
|
|
}
|
|
return _mass;
|
|
}
|
|
set
|
|
{
|
|
_mass = value;
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
// Usar esfera simple - sin complejidad de inercia personalizada
|
|
var sphere = new Sphere(Radius);
|
|
var inertia = sphere.ComputeInertia(value);
|
|
_simulation.Bodies.SetLocalInertia(BodyHandle, inertia);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void Create(Vector3 position)
|
|
{
|
|
RemoverBody();
|
|
|
|
// Usar ESFERA en BEPU para simplicidad matemática y eficiencia
|
|
var sphere = new Sphere(Radius);
|
|
var shapeIndex = _simulation.Shapes.Add(sphere);
|
|
|
|
// Inercia estándar de esfera - sin complejidad adicional
|
|
var inertia = sphere.ComputeInertia(_mass);
|
|
|
|
// NUNCA DORMIR - Valor negativo significa que las botellas JAMÁS entran en sleep mode
|
|
// Esto es crítico para detección continua de barreras, transportes y descartes
|
|
var activityDescription = new BodyActivityDescription(-1f); // -1 = NUNCA dormir
|
|
|
|
var bodyDescription = BodyDescription.CreateDynamic(
|
|
new RigidPose(position),
|
|
new BodyVelocity(),
|
|
inertia, // Inercia estándar de esfera
|
|
new CollidableDescription(shapeIndex, 0.01f),
|
|
activityDescription
|
|
);
|
|
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true; // Marcar que hemos creado un cuerpo
|
|
}
|
|
|
|
public void SetDiameter(float diameter)
|
|
{
|
|
Radius = diameter / 2f;
|
|
Height = diameter; // Mantener altura igual al diámetro para proporciones consistentes
|
|
|
|
// Actualizar la forma del cuerpo existente
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var sphere = new Sphere(Radius);
|
|
var shapeIndex = _simulation.Shapes.Add(sphere);
|
|
_simulation.Bodies.SetShape(BodyHandle, shapeIndex);
|
|
}
|
|
}
|
|
|
|
public void SetHeight(float height)
|
|
{
|
|
Height = height;
|
|
|
|
// Actualizar la forma del cuerpo existente
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var sphere = new Sphere(Radius);
|
|
var shapeIndex = _simulation.Shapes.Add(sphere);
|
|
_simulation.Bodies.SetShape(BodyHandle, shapeIndex);
|
|
}
|
|
}
|
|
|
|
public void SetMass(float mass)
|
|
{
|
|
Mass = mass;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Limita la rotación de la botella solo al plano XY (siempre "de pie")
|
|
/// Esto simplifica enormemente la simulación y es más realista para botellas
|
|
/// </summary>
|
|
public void LimitRotationToXYPlane()
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
|
|
// Extraer solo la rotación en Z (plano XY) y eliminar las rotaciones en X e Y
|
|
var currentOrientation = bodyReference.Pose.Orientation;
|
|
|
|
// Convertir a ángulo en Z solamente
|
|
var rotationZ = GetRotationZ();
|
|
|
|
// Crear nueva orientación solo con rotación en Z (botella siempre "de pie")
|
|
var correctedOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, rotationZ);
|
|
|
|
// Aplicar la orientación corregida
|
|
bodyReference.Pose.Orientation = correctedOrientation;
|
|
|
|
// También limitar la velocidad angular a solo rotación en Z
|
|
var angularVelocity = bodyReference.Velocity.Angular;
|
|
bodyReference.Velocity.Angular = new Vector3(0, 0, angularVelocity.Z);
|
|
}
|
|
}
|
|
|
|
public void ApplyLinearVelocity(Vector3 velocity)
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
bodyReference.Velocity.Linear = velocity;
|
|
}
|
|
}
|
|
|
|
public Vector3 GetLinearVelocity()
|
|
{
|
|
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
|
|
return bodyReference.Velocity.Linear;
|
|
}
|
|
return Vector3.Zero;
|
|
}
|
|
|
|
public void CenterFixtureOnConveyor()
|
|
{
|
|
// Implementar lógica de centrado si es necesario
|
|
}
|
|
|
|
public bool IsOnAnyTransport()
|
|
{
|
|
return isOnTransports > 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Método de seguridad para restaurar la masa original si la botella tiene masa aumentada
|
|
/// </summary>
|
|
public void RestoreOriginalMassIfNeeded()
|
|
{
|
|
if (OriginalMass > 0) // Solo si hay masa original guardada
|
|
{
|
|
try
|
|
{
|
|
SetMass(OriginalMass);
|
|
OriginalMass = 0; // Limpiar para evitar confusión
|
|
isOnBrakeTransport = false;
|
|
CurrentBrakeTransport = null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error restoring original mass - continue
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public class simCurve : simBase
|
|
{
|
|
private float _innerRadius;
|
|
private float _outerRadius;
|
|
private float _startAngle;
|
|
private float _endAngle;
|
|
public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)
|
|
|
|
private List<Action> _deferredActions;
|
|
internal List<BodyHandle> _triangleBodyHandles; // Lista de todos los triángulos que componen la curva
|
|
private List<List<Vector3>> _originalTriangles; // ✅ NUEVO: Triángulos originales para visualización debug
|
|
|
|
public float InnerRadius => _innerRadius;
|
|
public float OuterRadius => _outerRadius;
|
|
public float StartAngle => _startAngle;
|
|
public float EndAngle => _endAngle;
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Expone los triángulos originales para visualización debug
|
|
/// </summary>
|
|
public List<List<Vector3>> GetOriginalTriangles() => _originalTriangles ?? new List<List<Vector3>>();
|
|
|
|
public simCurve(Simulation simulation, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
_innerRadius = innerRadius;
|
|
_outerRadius = outerRadius;
|
|
// ✅ SENTIDO HORARIO: Convertir ángulos para que vayan en sentido horario
|
|
_startAngle = GradosARadianes(startAngle);
|
|
_endAngle = GradosARadianes(endAngle);
|
|
_triangleBodyHandles = new List<BodyHandle>();
|
|
_originalTriangles = new List<List<Vector3>>(); // ✅ NUEVO: Inicializar lista de triángulos
|
|
|
|
// Crear la curva con los ángulos que definen el sector
|
|
Create(innerRadius, outerRadius, startAngle, endAngle, topLeft, 0);
|
|
}
|
|
|
|
public void SetSpeed(float speed)
|
|
{
|
|
Speed = -speed;
|
|
}
|
|
|
|
public void UpdateCurve(float innerRadius, float outerRadius, float startAngle, float endAngle)
|
|
{
|
|
_innerRadius = innerRadius;
|
|
_outerRadius = outerRadius;
|
|
_startAngle = GradosARadianes(startAngle);
|
|
_endAngle = GradosARadianes(endAngle);
|
|
|
|
// Recrear la curva con nuevos parámetros manteniendo posición actual
|
|
var currentPosition = GetPosition();
|
|
Create(currentPosition);
|
|
}
|
|
|
|
public new void RemoverBody()
|
|
{
|
|
// Remover todos los triángulos de la curva
|
|
if (_triangleBodyHandles != null && _simulation != null)
|
|
{
|
|
foreach (var triangleHandle in _triangleBodyHandles)
|
|
{
|
|
if (_simulation.Bodies.BodyExists(triangleHandle))
|
|
{
|
|
_simulation.Bodies.Remove(triangleHandle);
|
|
}
|
|
}
|
|
_triangleBodyHandles.Clear();
|
|
}
|
|
|
|
// ✅ NUEVO: Limpiar también los triángulos originales
|
|
if (_originalTriangles != null)
|
|
{
|
|
_originalTriangles.Clear();
|
|
}
|
|
|
|
// Remover el cuerpo principal (si existe)
|
|
base.RemoverBody();
|
|
}
|
|
|
|
public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
|
|
{
|
|
// ✅ CORRIGIDO: Como Aether - startAngle y endAngle definen directamente el sector
|
|
// No hay rotación separada del objeto
|
|
|
|
// Actualizar parámetros internos
|
|
_innerRadius = innerRadius;
|
|
_outerRadius = outerRadius;
|
|
_startAngle = GradosARadianes(startAngle);
|
|
_endAngle = GradosARadianes(endAngle);
|
|
|
|
// Calcular el offset del centro desde Top-Left
|
|
// Para curvas, el "tamaño" es el diámetro del radio exterior
|
|
var curveSize = outerRadius * 2f;
|
|
var offsetX = curveSize / 2f;
|
|
var offsetY = curveSize / 2f;
|
|
|
|
// Calcular nueva posición del centro (sin rotación - el sector ya está definido por los ángulos)
|
|
var centerX = topLeft.X + offsetX;
|
|
var centerY = topLeft.Y + offsetY;
|
|
|
|
// Convertir a 3D e invertir Y - Curvas en el mismo nivel que transportes
|
|
var center3D = new Vector3(centerX, -centerY, zAltura_Curve / 2f + zPos_Curve);
|
|
|
|
Create(center3D); // Sin rotación adicional
|
|
}
|
|
|
|
private void Create(Vector3 position)
|
|
{
|
|
RemoverBody();
|
|
|
|
// Crear triángulos para la curva - los ángulos ya definen la forma correcta
|
|
var triangles = CreateTriangulatedCurve(_innerRadius, _outerRadius, -_startAngle, -_endAngle);
|
|
|
|
// ✅ NUEVO: Almacenar triángulos originales para visualización debug
|
|
_originalTriangles = new List<List<Vector3>>(triangles);
|
|
|
|
foreach (var triangle in triangles)
|
|
{
|
|
CreateTriangleBody(triangle, position);
|
|
}
|
|
|
|
// Crear un cuerpo principal invisible para referencia de posición
|
|
CreateMainBody(position);
|
|
}
|
|
|
|
private void CreateMainBody(Vector3 position)
|
|
{
|
|
// Crear un cuerpo principal muy pequeño e invisible solo para referencia
|
|
var smallBox = new Box(0.01f, 0.01f, 0.01f);
|
|
var shapeIndex = _simulation.Shapes.Add(smallBox);
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(position), // Sin rotación - la forma ya está definida por los ángulos
|
|
new CollidableDescription(shapeIndex, 0f), // Sin speculative margin
|
|
new BodyActivityDescription(0.01f)
|
|
);
|
|
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true;
|
|
}
|
|
|
|
private void CreateTriangleBody(List<Vector3> triangle, Vector3 basePosition)
|
|
{
|
|
if (triangle.Count != 3)
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Calcular centro y dimensiones del triángulo
|
|
var center = (triangle[0] + triangle[1] + triangle[2]) / 3f;
|
|
var worldCenter = center + basePosition;
|
|
|
|
// Calcular dimensiones aproximadas del triángulo para crear una caja plana
|
|
float minX = Math.Min(Math.Min(triangle[0].X, triangle[1].X), triangle[2].X);
|
|
float maxX = Math.Max(Math.Max(triangle[0].X, triangle[1].X), triangle[2].X);
|
|
float minY = Math.Min(Math.Min(triangle[0].Y, triangle[1].Y), triangle[2].Y);
|
|
float maxY = Math.Max(Math.Max(triangle[0].Y, triangle[1].Y), triangle[2].Y);
|
|
|
|
float width = Math.Max(maxX - minX, 0.01f);
|
|
float height = Math.Max(maxY - minY, 0.01f);
|
|
float thickness = zAltura_Curve; // Altura muy pequeña para triángulos planos
|
|
|
|
// Crear una caja plana que aproxime el triángulo
|
|
var box = new Box(width, height, thickness);
|
|
var shapeIndex = _simulation.Shapes.Add(box);
|
|
|
|
// Crear como cuerpo estático sólido (NO sensor)
|
|
var activityDescription = new BodyActivityDescription(0.01f);
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(worldCenter),
|
|
new CollidableDescription(shapeIndex, 0.1f), // Speculative margin normal
|
|
activityDescription
|
|
);
|
|
|
|
var triangleHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_triangleBodyHandles.Add(triangleHandle);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[simCurve] Error creating triangle: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private const float SegmentationFactor = 32f / 3f;
|
|
private const int MinSegments = 8;
|
|
private const int MaxSegments = 64;
|
|
|
|
private List<List<Vector3>> CreateTriangulatedCurve(float innerRadius, float outerRadius, float startAngle, float endAngle)
|
|
{
|
|
var triangles = new List<List<Vector3>>();
|
|
|
|
// Calcular número de segmentos basado en el tamaño del arco
|
|
float arcLength = Math.Abs(endAngle - startAngle) * ((innerRadius + outerRadius) / 2f);
|
|
int segments = (int)(arcLength * SegmentationFactor);
|
|
segments = Math.Max(MinSegments, Math.Min(segments, MaxSegments));
|
|
|
|
// ✅ SENTIDO HORARIO: Intercambiar ángulos para que vaya en sentido horario
|
|
float fromAngle = endAngle; // Empezar desde el ángulo final
|
|
float toAngle = startAngle; // Terminar en el ángulo inicial
|
|
float angleStep = (toAngle - fromAngle) / segments;
|
|
|
|
// Generar vértices para los arcos interior y exterior
|
|
var innerPoints = new Vector3[segments + 1];
|
|
var outerPoints = new Vector3[segments + 1];
|
|
|
|
for (int i = 0; i <= segments; i++)
|
|
{
|
|
float angle = fromAngle + i * angleStep;
|
|
float cosAngle = (float)Math.Cos(angle);
|
|
float sinAngle = (float)Math.Sin(angle);
|
|
|
|
// Triángulos planos en el plano XY (Z = 0 relativo)
|
|
innerPoints[i] = new Vector3(innerRadius * cosAngle, innerRadius * sinAngle, 0);
|
|
outerPoints[i] = new Vector3(outerRadius * cosAngle, outerRadius * sinAngle, 0);
|
|
}
|
|
|
|
// Crear triángulos
|
|
for (int i = 0; i < segments; i++)
|
|
{
|
|
// Primer triángulo del sector
|
|
var upperTriangle = new List<Vector3>
|
|
{
|
|
innerPoints[i],
|
|
outerPoints[i],
|
|
outerPoints[i + 1]
|
|
};
|
|
triangles.Add(upperTriangle);
|
|
|
|
// Segundo triángulo del sector
|
|
var lowerTriangle = new List<Vector3>
|
|
{
|
|
innerPoints[i],
|
|
outerPoints[i + 1],
|
|
innerPoints[i + 1]
|
|
};
|
|
triangles.Add(lowerTriangle);
|
|
}
|
|
|
|
return triangles;
|
|
}
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// Calcula la dirección tangencial para aplicar fuerzas de curva
|
|
/// </summary>
|
|
public Vector3 GetTangentialDirection(Vector3 bottlePosition)
|
|
{
|
|
try
|
|
{
|
|
var curvePosition = GetPosition();
|
|
var centerToBottle = new Vector2(bottlePosition.X - curvePosition.X, bottlePosition.Y - curvePosition.Y);
|
|
|
|
if (centerToBottle.Length() < 0.001f)
|
|
return Vector3.Zero;
|
|
|
|
// Vector tangente (perpendicular al radio) en el plano XY
|
|
var tangent = new Vector3(-centerToBottle.Y, centerToBottle.X, 0);
|
|
|
|
// Normalizar
|
|
if (tangent.Length() > 0.001f)
|
|
{
|
|
tangent = Vector3.Normalize(tangent);
|
|
}
|
|
|
|
// Ajustar dirección según la velocidad (signo)
|
|
if (Speed < 0)
|
|
tangent = -tangent;
|
|
|
|
return tangent;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[simCurve] Error calculating tangential direction: {ex.Message}");
|
|
return Vector3.Zero;
|
|
}
|
|
}
|
|
}
|
|
|
|
public class simDescarte : simBase
|
|
{
|
|
private float _radius;
|
|
private List<Action> _deferredActions;
|
|
public List<simBotella> ListSimBotellaContact;
|
|
|
|
public simDescarte(Simulation simulation, List<Action> deferredActions, float diameter, Vector2 position)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
_radius = diameter / 2f;
|
|
ListSimBotellaContact = new List<simBotella>();
|
|
|
|
// Convertir Vector2 a Vector3 con Z=0.3 (altura para detectar botellas)
|
|
// Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
|
|
var position3D = new Vector3(position.X, -position.Y, zPos_Descarte + _radius);
|
|
Create(position3D);
|
|
}
|
|
|
|
public float Radius
|
|
{
|
|
get { return _radius; }
|
|
set
|
|
{
|
|
_radius = Math.Max(value, 0.01f); // Mínimo 1cm
|
|
// Recrear el cuerpo con el nuevo radio
|
|
var currentPos = GetPosition();
|
|
Create(currentPos);
|
|
}
|
|
}
|
|
|
|
public void SetDiameter(float diameter)
|
|
{
|
|
Radius = diameter / 2f;
|
|
}
|
|
|
|
public float GetDiameter()
|
|
{
|
|
return _radius * 2f;
|
|
}
|
|
|
|
public void Create(Vector2 position)
|
|
{
|
|
// Convertir Vector2 a Vector3 con Z=0.3 (altura para detectar botellas)
|
|
// Invertir Y para convertir de WPF (Y hacia abajo) a 3D (Y hacia arriba)
|
|
var position3D = new Vector3(position.X, -position.Y, zPos_Descarte + _radius);
|
|
Create(position3D);
|
|
}
|
|
|
|
private void Create(Vector3 position)
|
|
{
|
|
RemoverBody();
|
|
|
|
// Crear esfera sensor para detección
|
|
var sphere = new BepuPhysics.Collidables.Sphere(_radius);
|
|
var shapeIndex = _simulation.Shapes.Add(sphere);
|
|
|
|
// Crear como SENSOR (Kinematic con speculative margin 0 para detección pura)
|
|
// Configurar actividad para NUNCA dormir - los sensores deben estar siempre activos
|
|
var activityDescription = new BodyActivityDescription(-1f); // -1 = nunca dormir
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(position),
|
|
new CollidableDescription(shapeIndex, 0f), // Speculative margin 0 para sensores
|
|
activityDescription
|
|
);
|
|
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true; // Marcar que hemos creado un cuerpo
|
|
}
|
|
}
|
|
|
|
// Callback handlers para BEPU
|
|
public struct NarrowPhaseCallbacks : INarrowPhaseCallbacks
|
|
{
|
|
private SimulationManagerBEPU _simulationManager;
|
|
|
|
public NarrowPhaseCallbacks(SimulationManagerBEPU simulationManager)
|
|
{
|
|
_simulationManager = simulationManager;
|
|
}
|
|
|
|
public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
|
|
{
|
|
// Configuración de materiales físicos por defecto
|
|
pairMaterial = new PairMaterialProperties
|
|
{
|
|
FrictionCoefficient = 0.6f, // Fricción moderada
|
|
MaximumRecoveryVelocity = 2f, // Velocidad máxima de recuperación
|
|
SpringSettings = new SpringSettings(60, 4) // Rigidez y amortiguamiento para colisiones sólidas
|
|
};
|
|
|
|
// Detectar contactos que involucren botellas para aplicar propiedades especiales
|
|
if (_simulationManager != null)
|
|
{
|
|
var botellaA = GetBotellaFromCollidable(pair.A);
|
|
var botellaB = GetBotellaFromCollidable(pair.B);
|
|
var transportA = GetTransportFromCollidable(pair.A);
|
|
var transportB = GetTransportFromCollidable(pair.B);
|
|
var barreraA = GetBarreraFromCollidable(pair.A);
|
|
var barreraB = GetBarreraFromCollidable(pair.B);
|
|
var curveA = GetCurveFromCollidable(pair.A);
|
|
var curveB = GetCurveFromCollidable(pair.B);
|
|
var botella = botellaA ?? botellaB;
|
|
|
|
// Detectar contactos con descarte
|
|
var descarteA = GetDescarteFromCollidable(pair.A);
|
|
var descarteB = GetDescarteFromCollidable(pair.B);
|
|
|
|
// IMPORTANTE: Si hay una barrera involucrada, NO generar contacto físico
|
|
if (barreraA != null || barreraB != null)
|
|
{
|
|
// Ya no necesitamos registrar contactos - usamos detección geométrica pura
|
|
// Las barreras son sensores completamente virtuales
|
|
|
|
// NO generar contacto físico para barreras - son sensores puros
|
|
return false;
|
|
}
|
|
|
|
// Si hay un descarte involucrado, NO generar contacto físico pero registrar para eliminación
|
|
if (descarteA != null || descarteB != null)
|
|
{
|
|
var descarte = descarteA ?? descarteB;
|
|
|
|
// Registrar la botella para eliminación si hay contacto
|
|
if (botella != null)
|
|
{
|
|
_simulationManager.RegisterDescarteContact(descarte, botella);
|
|
}
|
|
|
|
// NO generar contacto físico para descartes - son sensores puros
|
|
return false;
|
|
}
|
|
|
|
// Si hay una botella en contacto (pero no con barrera), aplicar mayor fricción para estabilidad
|
|
if (botella != null)
|
|
{
|
|
// Aumentar mucho la fricción para botellas (más estables)
|
|
pairMaterial.FrictionCoefficient = 0.9f;
|
|
|
|
// Reducir la velocidad de recuperación para evitar rebotes excesivos
|
|
pairMaterial.MaximumRecoveryVelocity = 1f;
|
|
|
|
// Hacer las colisiones más suaves y estables
|
|
pairMaterial.SpringSettings = new SpringSettings(80, 6);
|
|
}
|
|
|
|
// Si hay un transporte y una botella en contacto
|
|
if ((transportA != null || transportB != null) && botella != null)
|
|
{
|
|
var transporte = transportA ?? transportB;
|
|
|
|
// ✅ NUEVA LÓGICA: Detectar contactos con transporte de freno PRIMERO
|
|
if (transporte.isBrake)
|
|
{
|
|
// FRICCIÓN EXTREMA para transporte con freno - simula frenos mecánicos
|
|
pairMaterial.FrictionCoefficient = 5.0f; // Fricción extremadamente alta
|
|
|
|
// Reducir velocidad de recuperación para evitar rebotes
|
|
pairMaterial.MaximumRecoveryVelocity = 0.1f;
|
|
|
|
// Hacer el contacto más rígido para mejor control
|
|
pairMaterial.SpringSettings = new SpringSettings(200, 20);
|
|
|
|
// ✅ APLICAR FUERZAS DE FRENADO DIRECTAMENTE EN EL MANIFOLD
|
|
ApplyBrakeForces(ref manifold, botella, transporte);
|
|
|
|
// Registrar contacto con transporte de freno
|
|
_simulationManager.RegisterBrakeTransportContact(botella, transporte);
|
|
}
|
|
else
|
|
{
|
|
// ✅ NUEVA LÓGICA: Aplicar fuerzas de transporte DIRECTAMENTE en el manifold
|
|
if (Math.Abs(transporte.Speed) > 0.001f) // Solo si hay velocidad significativa
|
|
{
|
|
pairMaterial.FrictionCoefficient = 1.2f; // Fricción muy alta
|
|
|
|
// ✅ APLICAR FUERZAS DE TRANSPORTE DIRECTAMENTE EN EL MANIFOLD
|
|
ApplyTransportForces(ref manifold, botella, transporte);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Si hay una curva y una botella en contacto
|
|
if ((curveA != null || curveB != null) && botella != null)
|
|
{
|
|
var curve = curveA ?? curveB;
|
|
|
|
// Solo aplicar si hay velocidad significativa en la curva
|
|
if (Math.Abs(curve.Speed) > 0.001f)
|
|
{
|
|
pairMaterial.FrictionCoefficient = 1.0f; // Fricción alta para curvas
|
|
|
|
// Aplicar fuerzas de curva directamente en el manifold
|
|
ApplyCurveForces(ref manifold, botella, curve);
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private simTransporte GetTransportFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simTransporte;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simBotella GetBotellaFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Dynamic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simBotella;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simBarrera GetBarreraFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simBarrera;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simDescarte GetDescarteFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simDescarte;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simCurve GetCurveFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
|
|
// ✅ NUEVO: Buscar específicamente en los triángulos de las curvas
|
|
return _simulationManager.GetCurveFromTriangleBodyHandle(bodyHandle);
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado directamente en el manifold
|
|
/// </summary>
|
|
private void ApplyBrakeForces<TManifold>(ref TManifold manifold, simBotella botella, simTransporte transporte)
|
|
where TManifold : unmanaged, IContactManifold<TManifold>
|
|
{
|
|
try
|
|
{
|
|
if (_simulationManager?.simulation == null) return;
|
|
|
|
var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
|
|
var transporteBody = _simulationManager.simulation.Bodies.GetBodyReference(transporte.BodyHandle);
|
|
|
|
// Calcular velocidad relativa
|
|
var relativeVelocity = botellaBody.Velocity.Linear - transporteBody.Velocity.Linear;
|
|
|
|
// Si la botella se mueve más rápido que el transporte, aplicar fuerza de frenado
|
|
var transportDirection = GetTransportDirection(transporte);
|
|
var velocityInTransportDirection = Vector3.Dot(relativeVelocity, transportDirection);
|
|
|
|
// if (Math.Abs(velocityInTransportDirection) > Math.Abs(transporte.Speed / 18.5f))
|
|
// {
|
|
// Calcular fuerza de frenado proporcional a la diferencia de velocidad
|
|
var targetSpeed = transporte.Speed / 18.5f;
|
|
var currentSpeed = Vector3.Dot(botellaBody.Velocity.Linear, transportDirection);
|
|
var speedDifference = currentSpeed - targetSpeed;
|
|
|
|
// Aplicar impulso de frenado - esto se hace DURANTE la resolución de colisiones
|
|
var brakeImpulse = -transportDirection * speedDifference * botella.Mass * 0.5f;
|
|
botellaBody.ApplyLinearImpulse(brakeImpulse);
|
|
// }
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error applying brake forces during collision resolution
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO MÉTODO: Obtener dirección del transporte
|
|
/// </summary>
|
|
private Vector3 GetTransportDirection(simTransporte transporte)
|
|
{
|
|
var angle = transporte.GetRotationZ();
|
|
return new Vector3((float)Math.Cos(angle), (float)Math.Sin(angle), 0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO MÉTODO: Aplicar fuerzas de transporte normal directamente en el manifold
|
|
/// </summary>
|
|
private void ApplyTransportForces<TManifold>(ref TManifold manifold, simBotella botella, simTransporte transporte)
|
|
where TManifold : unmanaged, IContactManifold<TManifold>
|
|
{
|
|
try
|
|
{
|
|
if (_simulationManager?.simulation == null) return;
|
|
|
|
var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
|
|
var transporteBody = _simulationManager.simulation.Bodies.GetBodyReference(transporte.BodyHandle);
|
|
|
|
// Calcular la dirección del transporte
|
|
var transportDirection = GetTransportDirection(transporte);
|
|
|
|
// Calcular la fuerza proporcional a la velocidad deseada
|
|
var targetVelocity = transportDirection * transporte.Speed / 18.5f;
|
|
var currentVelocity = botellaBody.Velocity.Linear;
|
|
|
|
// Solo aplicar fuerza en el plano horizontal (X-Y)
|
|
var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0);
|
|
|
|
// Para transportes con freno (si aplica), limitar la velocidad máxima de la botella
|
|
bool isOnBrakeTransport = botella.isOnBrakeTransport;
|
|
if (isOnBrakeTransport)
|
|
{
|
|
var transportSpeed = Math.Abs(transporte.Speed / 18.5f);
|
|
var currentSpeed = horizontalVelocity.Length();
|
|
|
|
// Si la botella va más rápido que el transporte, limitar su velocidad
|
|
if (currentSpeed > transportSpeed * 1.1f) // 10% de tolerancia
|
|
{
|
|
var limitedVelocity = Vector3.Normalize(horizontalVelocity) * transportSpeed;
|
|
botellaBody.Velocity.Linear = new Vector3(limitedVelocity.X, limitedVelocity.Y, currentVelocity.Z);
|
|
horizontalVelocity = limitedVelocity;
|
|
}
|
|
}
|
|
|
|
var velocityDiff = targetVelocity - horizontalVelocity;
|
|
|
|
// Aplicar una fuerza proporcional a la diferencia de velocidad
|
|
var forceMultiplier = isOnBrakeTransport ? 3.0f : 1.0f; // 3x más fuerte para botellas con guías
|
|
var baseForceMagnitude = velocityDiff.Length() * botella.Mass * 10f; // Factor base
|
|
var forceMagnitude = baseForceMagnitude * forceMultiplier; // Aplicar multiplicador
|
|
|
|
if (forceMagnitude > 0.01f)
|
|
{
|
|
var maxForce = botella.Mass * 50f * forceMultiplier; // Límite también escalado
|
|
var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce);
|
|
|
|
// Aplicar impulso directamente durante la resolución de colisiones
|
|
botellaBody.ApplyLinearImpulse(force * (1f / 60f)); // Simular deltaTime típico
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error applying transport forces during collision resolution
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ SIMPLIFICADO: Aplicar fuerzas de curva directamente en el manifold
|
|
/// Si hay colisión física, la botella está sobre la curva - no necesita cálculos adicionales
|
|
/// </summary>
|
|
private void ApplyCurveForces<TManifold>(ref TManifold manifold, simBotella botella, simCurve curve)
|
|
where TManifold : unmanaged, IContactManifold<TManifold>
|
|
{
|
|
try
|
|
{
|
|
if (_simulationManager?.simulation == null) return;
|
|
|
|
var botellaBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
|
|
var botellaPosition = botellaBody.Pose.Position;
|
|
|
|
// ✅ SIMPLIFICADO: Si hay colisión física, aplicar fuerzas directamente
|
|
// Obtener la dirección tangencial
|
|
var tangentialDirection = curve.GetTangentialDirection(botellaPosition);
|
|
|
|
if (tangentialDirection.Length() > 0.001f)
|
|
{
|
|
// Calcular velocidad objetivo
|
|
var speedMetersPerSecond = Math.Abs(curve.Speed) / 18.5f; // Convertir a m/s
|
|
var targetVelocity = tangentialDirection * speedMetersPerSecond;
|
|
|
|
// Obtener velocidad actual
|
|
var currentVelocity = botellaBody.Velocity.Linear;
|
|
var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0);
|
|
|
|
// Usar directamente la dirección tangencial (sin interpolación compleja)
|
|
var newDirection = tangentialDirection;
|
|
|
|
// Usar la velocidad mayor entre la actual y la de la curva para continuidad
|
|
var currentSpeed = horizontalVelocity.Length();
|
|
var targetSpeed = Math.Max(currentSpeed, speedMetersPerSecond);
|
|
|
|
// Calcular nueva velocidad
|
|
var newVelocity = newDirection * targetSpeed;
|
|
var velocityDiff = newVelocity - horizontalVelocity;
|
|
|
|
// Aplicar impulso directamente (sin factor de overlap - la colisión es suficiente)
|
|
var forceMagnitude = velocityDiff.Length() * botella.Mass * 8f;
|
|
if (forceMagnitude > 0.01f)
|
|
{
|
|
var maxForce = botella.Mass * 40f;
|
|
var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce);
|
|
botellaBody.ApplyLinearImpulse(force * (1f / 60f)); // Simular deltaTime típico
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[simCurve] Error applying curve forces: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public void Initialize(Simulation simulation)
|
|
{
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
}
|
|
}
|
|
|
|
public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks
|
|
{
|
|
public Vector3 Gravity;
|
|
public float LinearDamping;
|
|
public float AngularDamping;
|
|
private SimulationManagerBEPU _simulationManager; // ✅ NUEVA REFERENCIA
|
|
|
|
public PoseIntegratorCallbacks(Vector3 gravity, float linearDamping = 0.999f, float angularDamping = 0.995f, SimulationManagerBEPU simulationManager = null)
|
|
{
|
|
Gravity = gravity;
|
|
LinearDamping = linearDamping;
|
|
AngularDamping = angularDamping;
|
|
_simulationManager = simulationManager; // ✅ NUEVA INICIALIZACIÓN
|
|
}
|
|
|
|
public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
|
|
|
|
public readonly bool AllowSubstepsForUnconstrainedBodies => false;
|
|
|
|
public readonly bool IntegrateVelocityForKinematics => false;
|
|
|
|
public void Initialize(Simulation simulation)
|
|
{
|
|
}
|
|
|
|
public void PrepareForIntegration(float dt)
|
|
{
|
|
}
|
|
|
|
public void IntegrateVelocity(Vector<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;
|
|
|
|
// ✅ NUEVA FUNCIONALIDAD: Aplicar fuerzas de frenado durante la integración
|
|
if (_simulationManager != null)
|
|
{
|
|
ApplyBrakeForcesInIntegration(bodyIndices, ref velocity, dt, workerIndex);
|
|
}
|
|
|
|
// Aplicar amortiguamiento lineal y angular para simular resistencia del aire
|
|
// Esto es crucial para que los cilindros se detengan de forma realista
|
|
var linearDampingWide = Vector<float>.One * LinearDamping;
|
|
var angularDampingWide = Vector<float>.One * AngularDamping;
|
|
|
|
velocity.Linear *= linearDampingWide;
|
|
velocity.Angular *= angularDampingWide; // ¡Esto es clave para detener rotaciones infinitas!
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado durante la integración de velocidades
|
|
/// ✅ SIMPLIFICADO: Solo aplicar amortiguamiento extra para botellas en transportes con freno
|
|
/// </summary>
|
|
private void ApplyBrakeForcesInIntegration(Vector<int> bodyIndices, ref BodyVelocityWide velocity, Vector<float> dt, int workerIndex)
|
|
{
|
|
try
|
|
{
|
|
// ✅ SIMPLIFICADO: Aplicar amortiguamiento extra para efectos de frenado
|
|
// Esto es más compatible con la arquitectura vectorizada de BEPU
|
|
|
|
// Amortiguamiento adicional para simular efecto de frenado
|
|
var brakeDampingWide = Vector<float>.One * 0.95f; // Más agresivo que el damping normal
|
|
|
|
// Aplicar solo si tenemos referencia al simulation manager
|
|
if (_simulationManager != null)
|
|
{
|
|
// El amortiguamiento extra se aplica globalmente,
|
|
// las fuerzas específicas se manejan en ApplyBrakeForces del NarrowPhaseCallbacks
|
|
velocity.Linear *= brakeDampingWide;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error during brake force integration
|
|
}
|
|
}
|
|
}
|
|
|
|
public class SimulationManagerBEPU
|
|
{
|
|
public Simulation simulation;
|
|
public List<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;
|
|
|
|
// Sistema de contactos de transporte para aplicar fuerzas
|
|
private Dictionary<simBotella, simTransporte> _transportContacts;
|
|
// Sistema de contactos de barrera para detección de paso
|
|
private Dictionary<simBarrera, List<simBotella>> _barreraContacts;
|
|
// Sistema de contactos de descarte para marcar botellas para eliminación
|
|
private Dictionary<simDescarte, List<simBotella>> _descarteContacts;
|
|
// Lista de botellas marcadas para eliminación
|
|
private HashSet<simBotella> _botellasParaEliminar;
|
|
// Sistema de contactos de transporte con freno para marcar botellas
|
|
private Dictionary<simBotella, simTransporte> _brakeTransportContacts;
|
|
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>
|
|
/// ✅ NUEVO: Obtiene una simCurve si el BodyHandle pertenece a uno de sus triángulos
|
|
/// </summary>
|
|
public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle)
|
|
{
|
|
// Buscar en todas las curvas si alguna contiene este BodyHandle en sus triángulos
|
|
var curves = Cuerpos.OfType<simCurve>();
|
|
|
|
foreach (var curve in curves)
|
|
{
|
|
if (curve._triangleBodyHandles != null && curve._triangleBodyHandles.Contains(bodyHandle))
|
|
{
|
|
return curve;
|
|
}
|
|
}
|
|
|
|
// Si no se encuentra en triángulos, buscar en cuerpos principales como fallback
|
|
var simBase = GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simCurve;
|
|
}
|
|
|
|
public SimulationManagerBEPU()
|
|
{
|
|
Cuerpos = new List<simBase>();
|
|
_deferredActions = new List<Action>();
|
|
_transportContacts = new Dictionary<simBotella, simTransporte>();
|
|
_barreraContacts = new Dictionary<simBarrera, List<simBotella>>();
|
|
_descarteContacts = new Dictionary<simDescarte, List<simBotella>>();
|
|
_botellasParaEliminar = new HashSet<simBotella>();
|
|
_brakeTransportContacts = new Dictionary<simBotella, simTransporte>();
|
|
bufferPool = new BufferPool();
|
|
stopwatch = new Stopwatch();
|
|
|
|
var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this);
|
|
|
|
// Configurar amortiguamiento para comportamiento realista:
|
|
// - LinearDamping: 0.999f (muy leve, objetos siguen moviéndose pero eventualmente se detienen)
|
|
// - AngularDamping: 0.995f (más agresivo para detener rotaciones infinitas de cilindros)
|
|
// ✅ MODIFICADO: Pasar referencia de this al PoseIntegratorCallbacks
|
|
var poseIntegratorCallbacks = new PoseIntegratorCallbacks(
|
|
gravity: new Vector3(0, 0, -9.81f), // Gravedad en Z
|
|
linearDamping: 0.999f, // Amortiguamiento lineal suave
|
|
angularDamping: 0.995f, // Amortiguamiento angular más fuerte para detener rotaciones
|
|
simulationManager: this // ✅ NUEVA REFERENCIA
|
|
);
|
|
|
|
var solveDescription = new SolveDescription(8, 1);
|
|
|
|
simulation = Simulation.Create(bufferPool, narrowPhaseCallbacks, poseIntegratorCallbacks, solveDescription);
|
|
}
|
|
|
|
public void Clear()
|
|
{
|
|
try
|
|
{
|
|
// ✅ NUEVO: Restaurar masas originales de todas las botellas antes de limpiar
|
|
foreach (var cuerpo in Cuerpos.OfType<simBotella>())
|
|
{
|
|
try
|
|
{
|
|
cuerpo.RestoreOriginalMassIfNeeded();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error restoring mass during clear - continue
|
|
}
|
|
}
|
|
|
|
// Limpiar contactos primero para evitar referencias colgantes
|
|
lock (_contactsLock)
|
|
{
|
|
_transportContacts.Clear();
|
|
_barreraContacts.Clear();
|
|
_descarteContacts.Clear();
|
|
_botellasParaEliminar.Clear();
|
|
_brakeTransportContacts.Clear();
|
|
}
|
|
|
|
// Remover cuerpos de forma segura
|
|
var cuerposToRemove = new List<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();
|
|
|
|
// Limpiar la simulación completamente si existe
|
|
if (simulation != null)
|
|
{
|
|
try
|
|
{
|
|
// Forzar un timestep pequeño para limpiar contactos pendientes
|
|
simulation.Timestep(1f / 1000f); // 1ms
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Warning during cleanup timestep - continue
|
|
}
|
|
}
|
|
|
|
// Limpiar visualización 3D
|
|
Visualization3DManager?.Clear();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error during clear - operation failed
|
|
}
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
stopwatch.Start();
|
|
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
|
}
|
|
|
|
public void Step()
|
|
{
|
|
try
|
|
{
|
|
var currentTime = stopwatch.Elapsed.TotalMilliseconds;
|
|
var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0);
|
|
stopwatch_last = currentTime;
|
|
|
|
// Validar deltaTime para evitar valores problemáticos
|
|
if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
|
|
{
|
|
deltaTime = 1f / 60f; // Fallback a 60 FPS
|
|
}
|
|
|
|
// Limitar deltaTime a 100ms máximo para evitar saltos tras pausas del debugger
|
|
const float maxDeltaTime = 0.1f; // 100ms
|
|
if (deltaTime > maxDeltaTime)
|
|
{
|
|
deltaTime = 1f / 60f; // Resetear a 60 FPS estándar si excede el límite
|
|
}
|
|
|
|
// Procesar acciones diferidas
|
|
foreach (var action in _deferredActions)
|
|
{
|
|
action();
|
|
}
|
|
_deferredActions.Clear();
|
|
|
|
// Validar que la simulación esté en buen estado antes del timestep
|
|
if (simulation?.Bodies == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Validar que no hay cuerpos con handles inválidos
|
|
var invalidBodies = Cuerpos.Where(c => c != null && !simulation.Bodies.BodyExists(c.BodyHandle)).ToList();
|
|
if (invalidBodies.Count > 0)
|
|
{
|
|
foreach (var invalidBody in invalidBodies)
|
|
{
|
|
Cuerpos.Remove(invalidBody);
|
|
}
|
|
}
|
|
|
|
// Paso de simulación (genera las colisiones/contactos)
|
|
var timestepValue = Math.Max(deltaTime, 1f / 120f); // Mínimo 120 FPS para mejor estabilidad
|
|
|
|
try
|
|
{
|
|
simulation.Timestep(timestepValue);
|
|
}
|
|
catch (AccessViolationException ex)
|
|
{
|
|
// Limpiar contactos que podrían estar corruptos
|
|
lock (_contactsLock)
|
|
{
|
|
// ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa
|
|
_barreraContacts.Clear();
|
|
}
|
|
throw; // Re-lanzar para debug
|
|
}
|
|
|
|
// ✅ ELIMINADO: ApplyTransportForces - Ahora se aplica directamente en ConfigureContactManifold
|
|
// ApplyTransportForces(deltaTime); // ELIMINADO - Ahora en ConfigureContactManifold
|
|
|
|
// Procesar contactos de transporte con freno para marcar/desmarcar botellas
|
|
ProcessBrakeTransportContacts();
|
|
|
|
// Limitar rotaciones de botellas solo al plano XY (siempre "de pie")
|
|
// Y GARANTIZAR que nunca entren en sleep mode
|
|
foreach (var cuerpo in Cuerpos.OfType<simBotella>().ToList()) // ToList para evitar modificación durante iteración
|
|
{
|
|
try
|
|
{
|
|
if (simulation.Bodies.BodyExists(cuerpo.BodyHandle))
|
|
{
|
|
cuerpo.LimitRotationToXYPlane();
|
|
|
|
// FORZAR que la botella esté siempre despierta - doble seguridad
|
|
simulation.Awakener.AwakenBody(cuerpo.BodyHandle);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error limiting rotation for bottle - continue
|
|
}
|
|
}
|
|
|
|
// Procesar contactos de barrera para detección de paso
|
|
ProcessBarreraContacts();
|
|
|
|
// Procesar contactos de descarte para eliminación de botellas
|
|
ProcessDescarteContacts();
|
|
|
|
// Procesar sistema de limpieza para eliminar botellas debajo de transportes
|
|
ProcessCleanupSystem();
|
|
|
|
// Limpiar contactos DESPUÉS de usarlos
|
|
lock (_contactsLock)
|
|
{
|
|
// ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa
|
|
_barreraContacts.Clear();
|
|
_descarteContacts.Clear();
|
|
_brakeTransportContacts.Clear();
|
|
}
|
|
|
|
// Sincronizar con la visualización 3D solo si está habilitado
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// En caso de error crítico, limpiar contactos para evitar estado corrupto
|
|
lock (_contactsLock)
|
|
{
|
|
// ✅ ELIMINADO: _transportContacts.Clear(); - Ya no se usa
|
|
_barreraContacts.Clear();
|
|
_brakeTransportContacts.Clear();
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Remove(simBase Objeto)
|
|
{
|
|
// ✅ NUEVO: Si es una botella con masa aumentada, restaurar masa original antes de eliminar
|
|
if (Objeto is simBotella botella && botella.OriginalMass > 0)
|
|
{
|
|
try
|
|
{
|
|
botella.SetMass(botella.OriginalMass);
|
|
botella.OriginalMass = 0;
|
|
botella.isOnBrakeTransport = false;
|
|
botella.CurrentBrakeTransport = null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error restoring mass during removal - continue with removal
|
|
}
|
|
}
|
|
|
|
Objeto.RemoverBody();
|
|
Cuerpos.Remove(Objeto);
|
|
|
|
// Actualizar visualización 3D tras eliminar objeto
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
}
|
|
|
|
public simBotella AddCircle(float diameter, Vector2 position, float mass)
|
|
{
|
|
var botella = new simBotella(simulation, _deferredActions, diameter, position, mass);
|
|
Cuerpos.Add(botella);
|
|
|
|
// Sincronizar con la visualización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return botella;
|
|
}
|
|
|
|
public simTransporte AddRectangle(float width, float height, Vector2 position, float angle)
|
|
{
|
|
var transporte = new simTransporte(simulation, _deferredActions, width, height, position, angle);
|
|
Cuerpos.Add(transporte);
|
|
// Sincronizar con la visualización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return transporte;
|
|
}
|
|
|
|
public simBarrera AddBarrera(float width, float height, Vector2 position, float angle, bool detectarCuello)
|
|
{
|
|
var barrera = new simBarrera(simulation, _deferredActions, width, height, position, angle, detectarCuello);
|
|
Cuerpos.Add(barrera);
|
|
// Sincronizar con la visualización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return barrera;
|
|
}
|
|
|
|
|
|
|
|
public simGuia AddLine(float width, float height, Vector2 topLeft, float angle)
|
|
{
|
|
var guia = new simGuia(simulation, _deferredActions, width, height, topLeft, angle);
|
|
Cuerpos.Add(guia);
|
|
// Sincronizar con la visualización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return guia;
|
|
}
|
|
|
|
public simDescarte AddDescarte(float diameter, Vector2 position)
|
|
{
|
|
var descarte = new simDescarte(simulation, _deferredActions, diameter, position);
|
|
Cuerpos.Add(descarte);
|
|
// Sincronizar con la visualización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return descarte;
|
|
}
|
|
|
|
public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
|
|
{
|
|
var curve = new simCurve(simulation, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, topLeft, unused);
|
|
Cuerpos.Add(curve);
|
|
// Sincronizar con la visualización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return curve;
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
Clear();
|
|
simulation?.Dispose();
|
|
bufferPool?.Clear();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Registra un contacto entre una botella y un transporte para aplicar fuerzas
|
|
/// </summary>
|
|
/// <param name="botella">Botella en contacto</param>
|
|
/// <param name="transporte">Transporte en contacto</param>
|
|
public void RegisterTransportContact(simBotella botella, simTransporte transporte)
|
|
{
|
|
if (botella != null && transporte != null)
|
|
{
|
|
lock (_contactsLock)
|
|
{
|
|
_transportContacts[botella] = transporte;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Registra un contacto entre una botella y un transporte con freno para marcar la botella
|
|
/// ✅ NUEVO: También aumenta la masa de la botella 10x para simular conexión mecánica
|
|
/// </summary>
|
|
/// <param name="botella">Botella en contacto</param>
|
|
/// <param name="transporte">Transporte con freno en contacto</param>
|
|
public void RegisterBrakeTransportContact(simBotella botella, simTransporte transporte)
|
|
{
|
|
if (botella != null && transporte != null && transporte.isBrake)
|
|
{
|
|
lock (_contactsLock)
|
|
{
|
|
_brakeTransportContacts[botella] = transporte;
|
|
// ✅ NUEVO: Mantener referencia directa en la botella
|
|
botella.CurrentBrakeTransport = transporte;
|
|
botella.ConveyorRestrictedTo = transporte;
|
|
|
|
// ✅ NUEVO: Aumentar masa 10x para simular conexión mecánica con el transporte
|
|
if (!botella.isOnBrakeTransport) // Solo si no estaba ya en un transporte con freno
|
|
{
|
|
// Guardar masa original si no está guardada ya
|
|
if (botella.OriginalMass <= 0)
|
|
{
|
|
botella.OriginalMass = botella.Mass;
|
|
}
|
|
|
|
// Aumentar masa 10 veces para simular "enganche" mecánico
|
|
botella.SetMass(botella.OriginalMass * 100f);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <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>
|
|
public 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>
|
|
/// Aplica fuerzas de transporte a las botellas en contacto
|
|
/// </summary>
|
|
/// <param name="deltaTime">Tiempo transcurrido desde el último paso</param>
|
|
private void ApplyTransportForces(float deltaTime)
|
|
{
|
|
try
|
|
{
|
|
// Validar deltaTime
|
|
if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
|
|
return;
|
|
|
|
lock (_contactsLock)
|
|
{
|
|
// Crear copia de contactos para evitar modificación durante iteración
|
|
var contactsCopy = new Dictionary<simBotella, simTransporte>(_transportContacts);
|
|
|
|
foreach (var contact in contactsCopy)
|
|
{
|
|
try
|
|
{
|
|
var botella = contact.Key;
|
|
var transporte = contact.Value;
|
|
|
|
// Validar objetos
|
|
if (botella == null || transporte == null || simulation?.Bodies == null)
|
|
continue;
|
|
|
|
// Verificar que ambos objetos aún existen
|
|
if (!simulation.Bodies.BodyExists(botella.BodyHandle) || !simulation.Bodies.BodyExists(transporte.BodyHandle))
|
|
continue;
|
|
|
|
// Solo aplicar si hay velocidad significativa
|
|
if (Math.Abs(transporte.Speed) <= 0.001f)
|
|
continue;
|
|
|
|
// Determinar si es una botella en transporte con freno (guías laterales)
|
|
bool isOnBrakeTransport = botella.isOnBrakeTransport;
|
|
float forceMultiplier = isOnBrakeTransport ? 3.0f : 1.0f; // 3x más fuerte para botellas con guías
|
|
|
|
// Aplicar fuerzas de movimiento (longitudinales) con control de velocidad
|
|
ApplyLongitudinalForces(botella, transporte, deltaTime, forceMultiplier, isOnBrakeTransport);
|
|
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error processing contact - continue
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error in ApplyTransport - continue
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Aplica fuerzas longitudinales para el movimiento del transporte con control de velocidad
|
|
/// </summary>
|
|
private void ApplyLongitudinalForces(simBotella botella, simTransporte transporte, float deltaTime, float forceMultiplier, bool isOnBrakeTransport)
|
|
{
|
|
try
|
|
{
|
|
// Calcular la dirección del transporte de forma segura
|
|
var angle = transporte.GetRotationZ();
|
|
|
|
// Validar ángulo
|
|
if (float.IsNaN(angle) || float.IsInfinity(angle))
|
|
return;
|
|
|
|
var cos = (float)Math.Cos(angle);
|
|
var sin = (float)Math.Sin(angle);
|
|
|
|
// Validar valores trigonométricos
|
|
if (float.IsNaN(cos) || float.IsNaN(sin))
|
|
return;
|
|
|
|
// Vector de dirección del transporte (en el plano X-Y)
|
|
var transportDirection = new Vector3(cos, sin, 0);
|
|
|
|
// Calcular la fuerza proporcional a la velocidad deseada
|
|
var targetVelocity = transportDirection * transporte.Speed / 18.5f;
|
|
var currentVelocity = botella.GetLinearVelocity();
|
|
|
|
// Validar velocidades
|
|
if (float.IsNaN(currentVelocity.X) || float.IsNaN(currentVelocity.Y) || float.IsNaN(currentVelocity.Z))
|
|
return;
|
|
|
|
// Solo aplicar fuerza en el plano horizontal (X-Y)
|
|
var horizontalVelocity = new Vector3(currentVelocity.X, currentVelocity.Y, 0);
|
|
|
|
// Para transportes con freno, limitar la velocidad máxima de la botella
|
|
if (isOnBrakeTransport)
|
|
{
|
|
var transportSpeed = Math.Abs(transporte.Speed / 18.5f);
|
|
var currentSpeed = horizontalVelocity.Length();
|
|
|
|
// Si la botella va más rápido que el transporte, limitar su velocidad
|
|
if (currentSpeed > transportSpeed * 1f) // 10% de tolerancia
|
|
{
|
|
var limitedVelocity = Vector3.Normalize(horizontalVelocity) * transportSpeed;
|
|
botella.ApplyLinearVelocity(new Vector3(limitedVelocity.X, limitedVelocity.Y, currentVelocity.Z));
|
|
horizontalVelocity = limitedVelocity;
|
|
}
|
|
}
|
|
|
|
var velocityDiff = targetVelocity - horizontalVelocity;
|
|
|
|
// Validar diferencia de velocidad
|
|
if (float.IsNaN(velocityDiff.X) || float.IsNaN(velocityDiff.Y) || float.IsNaN(velocityDiff.Z))
|
|
return;
|
|
|
|
// Aplicar una fuerza proporcional a la diferencia de velocidad
|
|
var baseForceMagnitude = velocityDiff.Length() * botella.Mass * 10f; // Factor base
|
|
var forceMagnitude = baseForceMagnitude * forceMultiplier; // Aplicar multiplicador
|
|
|
|
// Validar magnitud de fuerza
|
|
if (float.IsNaN(forceMagnitude) || float.IsInfinity(forceMagnitude))
|
|
return;
|
|
|
|
if (forceMagnitude > 0.01f)
|
|
{
|
|
var maxForce = botella.Mass * 50f * forceMultiplier; // Límite también escalado
|
|
var force = Vector3.Normalize(velocityDiff) * Math.Min(forceMagnitude, maxForce);
|
|
|
|
// Validar fuerza final
|
|
if (float.IsNaN(force.X) || float.IsNaN(force.Y) || float.IsNaN(force.Z))
|
|
return;
|
|
|
|
var bodyReference = simulation.Bodies.GetBodyReference(botella.BodyHandle);
|
|
bodyReference.ApplyLinearImpulse(force * deltaTime);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error in ApplyLongitudinalForces - continue
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Procesa contactos de transporte con freno para marcar/desmarcar botellas
|
|
/// ✅ NUEVO: También gestiona el cambio de masa - restaura masa original al salir del transporte
|
|
/// </summary>
|
|
private void ProcessBrakeTransportContacts()
|
|
{
|
|
try
|
|
{
|
|
// Primero, recopilar botellas que estaban en transporte con freno para detectar cambios
|
|
var todasLasBotellas = Cuerpos.OfType<simBotella>().ToList();
|
|
var botellasQueEstabanEnBrakeTransport = new HashSet<simBotella>();
|
|
|
|
foreach (var botella in todasLasBotellas)
|
|
{
|
|
if (botella != null && botella.isOnBrakeTransport)
|
|
{
|
|
botellasQueEstabanEnBrakeTransport.Add(botella);
|
|
botella.isOnBrakeTransport = false; // Asumir que no están en contacto
|
|
}
|
|
}
|
|
|
|
// Luego, marcar las botellas que SÍ están en contacto con transportes de freno
|
|
lock (_contactsLock)
|
|
{
|
|
var contactsCopy = new Dictionary<simBotella, simTransporte>(_brakeTransportContacts);
|
|
|
|
foreach (var contact in contactsCopy)
|
|
{
|
|
try
|
|
{
|
|
var botella = contact.Key;
|
|
var transporte = contact.Value;
|
|
|
|
// Validar objetos
|
|
if (botella == null || transporte == null || simulation?.Bodies == null)
|
|
continue;
|
|
|
|
// Verificar que ambos objetos aún existen
|
|
if (!simulation.Bodies.BodyExists(botella.BodyHandle) || !simulation.Bodies.BodyExists(transporte.BodyHandle))
|
|
continue;
|
|
|
|
// Verificar que el transporte sigue teniendo freno activado
|
|
if (transporte.isBrake)
|
|
{
|
|
botella.isOnBrakeTransport = true;
|
|
// Quitar de la lista de botellas que estaban en brake transport
|
|
botellasQueEstabanEnBrakeTransport.Remove(botella);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error processing brake transport contact - continue
|
|
}
|
|
}
|
|
}
|
|
|
|
// ✅ NUEVO: Restaurar masa original de botellas que salieron del transporte con freno
|
|
foreach (var botella in botellasQueEstabanEnBrakeTransport)
|
|
{
|
|
try
|
|
{
|
|
if (botella.OriginalMass > 0) // Solo si tenemos masa original guardada
|
|
{
|
|
botella.SetMass(botella.OriginalMass);
|
|
botella.OriginalMass = 0; // Limpiar para evitar confusión
|
|
botella.CurrentBrakeTransport = null; // Limpiar referencia
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error restoring original mass - continue
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error in ProcessBrakeTransportContacts - continue
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Procesa todas las barreras usando detección geométrica pura
|
|
/// Ya no depende de contactos físicos de BEPU - usa cálculo geométrico para ambos flags
|
|
/// </summary>
|
|
private void ProcessBarreraContacts()
|
|
{
|
|
try
|
|
{
|
|
// Obtener todas las barreras y botellas
|
|
var barreras = Cuerpos.OfType<simBarrera>().ToList();
|
|
var botellas = Cuerpos.OfType<simBotella>().ToList();
|
|
|
|
|
|
foreach (var barrera in barreras)
|
|
{
|
|
try
|
|
{
|
|
// Verificar que la barrera aún existe en la simulación
|
|
if (barrera == null || !simulation?.Bodies?.BodyExists(barrera.BodyHandle) == true)
|
|
continue;
|
|
|
|
// Resetear flags
|
|
barrera.LuzCortada = false;
|
|
barrera.LuzCortadaNeck = false;
|
|
barrera.Distancia = float.MaxValue;
|
|
|
|
// Limpiar la lista de forma segura
|
|
if (barrera.ListSimBotellaContact != null)
|
|
{
|
|
barrera.ListSimBotellaContact.Clear();
|
|
}
|
|
|
|
// Calcular detección usando geometría pura para TODAS las botellas
|
|
CalculateBarreraDetectionGeometric(barrera, botellas);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error processing barrera - continue
|
|
}
|
|
}
|
|
|
|
// Limpiar contactos ya que no los usamos más
|
|
lock (_contactsLock)
|
|
{
|
|
_barreraContacts.Clear();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error in ProcessBarrera - continue
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calcula detección geométrica pura para una barrera específica contra todas las botellas
|
|
/// Usa el mismo sistema geométrico para ambos flags: LuzCortada (radio completo) y LuzCortadaNeck (radio/2)
|
|
/// </summary>
|
|
/// <param name="barrera">Barrera que está detectando</param>
|
|
/// <param name="todasLasBotellas">Lista de TODAS las botellas en la simulación</param>
|
|
private void CalculateBarreraDetectionGeometric(simBarrera barrera, List<simBotella> todasLasBotellas)
|
|
{
|
|
try
|
|
{
|
|
// Validaciones de seguridad
|
|
if (barrera == null || todasLasBotellas == null || simulation?.Bodies == null)
|
|
return;
|
|
|
|
if (!simulation.Bodies.BodyExists(barrera.BodyHandle))
|
|
return;
|
|
|
|
var barrierBody = simulation.Bodies[barrera.BodyHandle];
|
|
var barrierPosition = barrierBody.Pose.Position;
|
|
var barrierOrientation = barrierBody.Pose.Orientation;
|
|
|
|
// Validar valores de posición y orientación
|
|
if (float.IsNaN(barrierPosition.X) || float.IsNaN(barrierPosition.Y) || float.IsNaN(barrierPosition.Z))
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Obtener las dimensiones de la barrera de forma segura
|
|
var halfWidth = Math.Max(barrera.Width / 2f, 0.01f); // Mínimo 1cm
|
|
|
|
float minDistance = float.MaxValue;
|
|
var botellasDetectadas = new List<simBotella>();
|
|
|
|
// Procesar TODAS las botellas (no solo las en contacto físico)
|
|
foreach (var botella in todasLasBotellas)
|
|
{
|
|
try
|
|
{
|
|
if (botella == null || !simulation.Bodies.BodyExists(botella.BodyHandle))
|
|
continue;
|
|
|
|
var botellaBody = simulation.Bodies[botella.BodyHandle];
|
|
var botellaPosition = botellaBody.Pose.Position;
|
|
|
|
// Validar posición de la botella
|
|
if (float.IsNaN(botellaPosition.X) || float.IsNaN(botellaPosition.Y) || float.IsNaN(botellaPosition.Z))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// CLAVE: Crear la línea del haz en el plano XY a la altura del centro de la botella
|
|
var bottleZ = botellaPosition.Z; // Altura actual del centro de la botella
|
|
|
|
// Puntos de la línea del haz en el plano XY a la altura de la botella
|
|
var localStart = new Vector3(-halfWidth, 0, 0);
|
|
var localEnd = new Vector3(halfWidth, 0, 0);
|
|
|
|
// Transformar a coordenadas mundiales pero en el plano Z de la botella
|
|
var worldStartXY = barrierPosition + Vector3.Transform(localStart, barrierOrientation);
|
|
var worldEndXY = barrierPosition + Vector3.Transform(localEnd, barrierOrientation);
|
|
|
|
// Ajustar Z para que esté en el plano de la botella
|
|
worldStartXY = new Vector3(worldStartXY.X, worldStartXY.Y, bottleZ);
|
|
worldEndXY = new Vector3(worldEndXY.X, worldEndXY.Y, bottleZ);
|
|
|
|
// Calcular distancia desde el centro de la botella a la línea del haz
|
|
var closestPoint = ProjectPointOntoLine(botellaPosition, worldStartXY, worldEndXY);
|
|
var distance = Vector3.Distance(closestPoint, botellaPosition);
|
|
|
|
// Validar distancia calculada
|
|
if (float.IsNaN(distance) || float.IsInfinity(distance))
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Actualizar distancia mínima
|
|
if (distance < minDistance)
|
|
{
|
|
minDistance = distance;
|
|
}
|
|
|
|
// NUEVO: Verificar LuzCortada usando radio completo
|
|
if (distance <= botella.Radius)
|
|
{
|
|
barrera.LuzCortada = true;
|
|
botellasDetectadas.Add(botella);
|
|
}
|
|
|
|
// Verificar detección de cuello usando radio/2 (como antes)
|
|
if (barrera.DetectNeck && botella.Radius > 0)
|
|
{
|
|
var neckRadius = botella.Radius / 2f;
|
|
if (distance <= neckRadius)
|
|
{
|
|
barrera.LuzCortadaNeck = true;
|
|
// No hacer break aquí - queremos procesar todas las botellas para LuzCortada
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error processing bottle - continue
|
|
}
|
|
}
|
|
|
|
// Asignar resultados de forma segura
|
|
barrera.Distancia = minDistance == float.MaxValue ? 0f : minDistance;
|
|
|
|
// Actualizar lista de botellas detectadas
|
|
if (barrera.ListSimBotellaContact != null)
|
|
{
|
|
barrera.ListSimBotellaContact.AddRange(botellasDetectadas);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// En caso de error, asignar valores seguros
|
|
if (barrera != null)
|
|
{
|
|
barrera.Distancia = float.MaxValue;
|
|
barrera.LuzCortada = false;
|
|
barrera.LuzCortadaNeck = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <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))
|
|
{
|
|
Remove(botella);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error removing bottle - continue
|
|
}
|
|
}
|
|
|
|
// Botellas eliminadas: {botellasAEliminar.Count}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error in RemoveMarkedBottles - continue
|
|
}
|
|
}
|
|
|
|
/// <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;
|
|
}
|
|
|
|
}
|
|
} |