1421 lines
59 KiB
C#
1421 lines
59 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 BepuPhysics.Trees;
|
|
using BepuUtilities;
|
|
using BepuUtilities.Memory;
|
|
using CtrEditor.FuncionesBase;
|
|
using DocumentFormat.OpenXml.Vml;
|
|
using DocumentFormat.OpenXml.Spreadsheet;
|
|
|
|
namespace CtrEditor.Simulacion
|
|
{
|
|
|
|
// Callback handlers para BEPU
|
|
public struct NarrowPhaseCallbacks : INarrowPhaseCallbacks
|
|
{
|
|
private SimulationManagerBEPU _simulationManager;
|
|
|
|
public NarrowPhaseCallbacks(SimulationManagerBEPU simulationManager)
|
|
{
|
|
_simulationManager = simulationManager;
|
|
}
|
|
|
|
public bool AllowContactGeneration(int workerIndex, CollidableReference a, CollidableReference b, ref float speculativeMargin)
|
|
{
|
|
// ✅ NUEVO FILTRADO: Evitar colisiones innecesarias entre triángulos de curvas
|
|
if (_simulationManager != null)
|
|
{
|
|
// Obtener información de los objetos involucrados
|
|
var curveA = _simulationManager.GetCurveFromTriangleBodyHandle(a.BodyHandle);
|
|
var curveB = _simulationManager.GetCurveFromTriangleBodyHandle(b.BodyHandle);
|
|
var transportA = GetTransportFromCollidable(a);
|
|
var transportB = GetTransportFromCollidable(b);
|
|
// ✅ ELIMINADO: Las barreras ya no tienen body físico
|
|
// var barrierA = GetBarreraFromCollidable(a);
|
|
// var barrierB = GetBarreraFromCollidable(b);
|
|
var discardA = GetDescarteFromCollidable(a);
|
|
var discardB = GetDescarteFromCollidable(b);
|
|
var botellaA = GetBotellaFromCollidable(a);
|
|
var botellaB = GetBotellaFromCollidable(b);
|
|
|
|
// ✅ FILTRO 1: No permitir colisiones entre triángulos de curvas
|
|
if (curveA != null && curveB != null)
|
|
{
|
|
return false; // Los triángulos de curva no deben colisionar entre sí
|
|
}
|
|
|
|
// ✅ FILTRO 2: No permitir colisiones entre transportes
|
|
if (transportA != null && transportB != null)
|
|
{
|
|
return false; // Los transportes no deben colisionar entre sí
|
|
}
|
|
|
|
// ✅ FILTRO 3: No permitir colisiones entre elementos estáticos (transportes, curvas, descartes)
|
|
// ✅ ELIMINADO: barrierA y barrierB ya que las barreras no tienen body físico
|
|
var staticA = (transportA != null || curveA != null || discardA != null);
|
|
var staticB = (transportB != null || curveB != null || discardB != null);
|
|
|
|
if (staticA && staticB)
|
|
{
|
|
return false; // Los elementos estáticos no deben colisionar entre sí
|
|
}
|
|
|
|
// ✅ ELIMINADO: No bloquear colisiones físicas aquí
|
|
// Las colisiones físicas deben permitirse siempre para mantener geometría sólida
|
|
// Solo bloqueamos la creación de motores duplicados en ConfigureContactManifold
|
|
}
|
|
|
|
return true; // Permitir todas las demás colisiones (botella-estático, botella-botella)
|
|
}
|
|
|
|
public bool AllowContactGeneration(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public bool ConfigureContactManifold<TManifold>(int workerIndex, CollidablePair pair, ref TManifold manifold, out PairMaterialProperties pairMaterial) where TManifold : unmanaged, IContactManifold<TManifold>
|
|
{
|
|
// ✅ CONFIGURACIÓN BÁSICA de materiales físicos
|
|
pairMaterial = new PairMaterialProperties
|
|
{
|
|
FrictionCoefficient = 0.3f,
|
|
MaximumRecoveryVelocity = 1f,
|
|
SpringSettings = new SpringSettings(80, 6)
|
|
};
|
|
|
|
if (_simulationManager != null)
|
|
{
|
|
var botellaA = GetBotellaFromCollidable(pair.A);
|
|
var botellaB = GetBotellaFromCollidable(pair.B);
|
|
var descarteA = GetDescarteFromCollidable(pair.A);
|
|
var descarteB = GetDescarteFromCollidable(pair.B);
|
|
var botella = botellaA ?? botellaB;
|
|
|
|
// ✅ DESCARTES como sensores puros
|
|
if (descarteA != null || descarteB != null)
|
|
{
|
|
if (botella != null)
|
|
{
|
|
// ✅ CORREGIDO: Marcar la botella para eliminar directamente, en lugar de registrar el contacto.
|
|
// Esto es más directo y evita el error de compilación.
|
|
botella.Descartar = true;
|
|
}
|
|
return false; // NO generar contacto físico
|
|
}
|
|
|
|
// ✅ NUEVO SISTEMA SIMPLIFICADO: Solo registrar contactos para actualización en Step
|
|
var transportA = GetTransportFromCollidable(pair.A);
|
|
var transportB = GetTransportFromCollidable(pair.B);
|
|
var curveA = GetCurveFromCollidable(pair.A);
|
|
var curveB = GetCurveFromCollidable(pair.B);
|
|
|
|
// ✅ CONTACTO BOTELLA-TRANSPORTE: USAR FRICCIÓN Y VELOCIDAD CINEMÁTICA
|
|
if (botella != null && (transportA != null || transportB != null))
|
|
{
|
|
// La velocidad del cuerpo cinemático del transporte se establece directamente
|
|
// en la clase simTransporte. El motor de físicas se encarga del resto.
|
|
|
|
// ✅ NUEVO: Lógica de control de presión/flotación
|
|
botella.IsTouchingTransport = true;
|
|
var botellaCollidable = GetBotellaFromCollidable(pair.A) != null ? pair.A : pair.B;
|
|
if (botellaCollidable.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botellaCollidable.BodyHandle))
|
|
{
|
|
var body = _simulationManager.simulation.Bodies.GetBodyReference(botellaCollidable.BodyHandle);
|
|
botella.LastTransportCollisionZ = body.Pose.Position.Z;
|
|
}
|
|
// Decrementamos la presión acumulada al tocar un transporte. Se reduce más rápido de lo que aumenta.
|
|
botella.PressureBuildup = Math.Max(0, botella.PressureBuildup - 5);
|
|
|
|
// ✅ Fricción ajustada para un arrastre firme pero no excesivo.
|
|
var transport = transportA ?? transportB;
|
|
if (transport.isBrake)
|
|
{
|
|
// ✅ NUEVO: Centrar la botella en el transporte de frenado la primera vez que entra.
|
|
// if (!botella.isOnBrakeTransport)
|
|
{
|
|
if (botella.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botella.BodyHandle) &&
|
|
transport.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(transport.BodyHandle))
|
|
{
|
|
var bottleBody = _simulationManager.simulation.Bodies.GetBodyReference(botella.BodyHandle);
|
|
var transportBody = _simulationManager.simulation.Bodies.GetBodyReference(transport.BodyHandle);
|
|
|
|
var bottlePose = bottleBody.Pose;
|
|
var transportPose = transportBody.Pose;
|
|
|
|
// Proyectar la posición de la botella sobre el eje longitudinal del transporte.
|
|
Vector3 transportForward = transport.DirectionVector;
|
|
Vector3 vectorToBottle = bottlePose.Position - transportPose.Position;
|
|
float projectionLength = Vector3.Dot(vectorToBottle, transportForward);
|
|
Vector3 newPositionOnCenterline = transportPose.Position + transportForward * projectionLength;
|
|
|
|
// Crear la nueva pose manteniendo la Z original de la botella para no hundirla.
|
|
var newBottlePosition = new Vector3(newPositionOnCenterline.X, newPositionOnCenterline.Y, bottlePose.Position.Z);
|
|
bottleBody.Pose.Position = newBottlePosition;
|
|
|
|
// Opcional: Anular la velocidad lateral para un acoplamiento más suave.
|
|
var lateralVelocity = Vector3.Dot(bottleBody.Velocity.Linear, transport.DirectionVector);
|
|
bottleBody.Velocity.Linear = transport.DirectionVector * lateralVelocity;
|
|
}
|
|
}
|
|
|
|
botella.isOnBrakeTransport = true;
|
|
pairMaterial.FrictionCoefficient = 14f;
|
|
pairMaterial.MaximumRecoveryVelocity = 1.0f;
|
|
pairMaterial.SpringSettings = new SpringSettings(80, 8);
|
|
}
|
|
else
|
|
{
|
|
botella.isOnBrakeTransport = false;
|
|
pairMaterial.FrictionCoefficient = 1.2f;
|
|
pairMaterial.MaximumRecoveryVelocity = 1.0f;
|
|
pairMaterial.SpringSettings = new SpringSettings(80, 1);
|
|
}
|
|
|
|
}
|
|
// ✅ CONTACTO BOTELLA-CURVA: USAR FRICCIÓN Y VELOCIDAD CINEMÁTICA
|
|
else if (botella != null && (curveA != null || curveB != null))
|
|
{
|
|
// El motor de físicas usará la velocidad angular del cuerpo cinemático de la curva
|
|
// para calcular las fuerzas de fricción y arrastrar la botella.
|
|
|
|
// ✅ NUEVO: Lógica de control de presión/flotación
|
|
botella.IsTouchingTransport = true;
|
|
var botellaCollidable = GetBotellaFromCollidable(pair.A) != null ? pair.A : pair.B;
|
|
if (botellaCollidable.BodyHandle.Value >= 0 && _simulationManager.simulation.Bodies.BodyExists(botellaCollidable.BodyHandle))
|
|
{
|
|
var body = _simulationManager.simulation.Bodies.GetBodyReference(botellaCollidable.BodyHandle);
|
|
botella.LastTransportCollisionZ = body.Pose.Position.Z;
|
|
}
|
|
// Decrementamos la presión acumulada al tocar una curva.
|
|
botella.PressureBuildup = Math.Max(0, botella.PressureBuildup - 5);
|
|
|
|
// ✅ Fricción ajustada para un arrastre firme pero no excesivo.
|
|
pairMaterial.FrictionCoefficient = 1f;
|
|
pairMaterial.MaximumRecoveryVelocity = 1.0f;
|
|
pairMaterial.SpringSettings = new SpringSettings(80, 1);
|
|
}
|
|
// ✅ CONTACTO BOTELLA-GUÍA: Configuración específica de fricción
|
|
else if (botella != null && (GetGuiaFromCollidable(pair.A) != null || GetGuiaFromCollidable(pair.B) != null))
|
|
{
|
|
// Fricción baja para las guías, deben deslizar, no arrastrar.
|
|
pairMaterial.FrictionCoefficient = 0.1f;
|
|
pairMaterial.MaximumRecoveryVelocity = 1f;
|
|
pairMaterial.SpringSettings = new SpringSettings(20, 1);
|
|
}
|
|
// ✅ NUEVO: CONTACTO BOTELLA-BOTELLA: Configuración más suave para evitar el comportamiento "pegajoso".
|
|
else if (botellaA != null && botellaB != null)
|
|
{
|
|
if (botella.isOnBrakeTransport)
|
|
{
|
|
pairMaterial.FrictionCoefficient = 2.0f; // Un poco menos de fricción.
|
|
pairMaterial.MaximumRecoveryVelocity = 1.5f; // Menos rebote.
|
|
pairMaterial.SpringSettings = new SpringSettings(80, 1f); // Muelle MUCHO más suave y críticamente amortiguado.
|
|
}
|
|
else
|
|
{
|
|
pairMaterial.FrictionCoefficient = 0.01f; // Un poco menos de fricción.
|
|
pairMaterial.MaximumRecoveryVelocity = 1.5f; // Menos rebote.
|
|
pairMaterial.SpringSettings = new SpringSettings(20, 0.5f); // Muelle MUCHO más suave y críticamente amortiguado.
|
|
}
|
|
}
|
|
// Ajustes básicos para otras botellas
|
|
else if (botella != null)
|
|
{
|
|
// Fricción moderada para colisiones entre botellas.
|
|
pairMaterial.FrictionCoefficient = 0.3f;
|
|
pairMaterial.MaximumRecoveryVelocity = 1f;
|
|
pairMaterial.SpringSettings = new SpringSettings(80, 1);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
|
|
private simTransporte GetTransportFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simTransporte;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simBotella GetBotellaFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Dynamic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simBotella;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
// ✅ ELIMINADO: GetBarreraFromCollidable - las barreras ya no tienen body físico
|
|
// private simBarrera GetBarreraFromCollidable(CollidableReference collidable) { ... }
|
|
|
|
private simDescarte GetDescarteFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simDescarte;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simCurve GetCurveFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simCurve;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
private simGuia GetGuiaFromCollidable(CollidableReference collidable)
|
|
{
|
|
if (_simulationManager?.simulation != null)
|
|
{
|
|
if (collidable.Mobility == CollidableMobility.Kinematic)
|
|
{
|
|
var bodyHandle = collidable.BodyHandle;
|
|
var simBase = _simulationManager.GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simGuia;
|
|
}
|
|
}
|
|
return null;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO MÉTODO: Aplicar fuerzas de frenado directamente en el manifold
|
|
/// </summary>
|
|
|
|
|
|
public bool ConfigureContactManifold(int workerIndex, CollidablePair pair, int childIndexA, int childIndexB, ref ConvexContactManifold manifold)
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public void Initialize(Simulation simulation)
|
|
{
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
}
|
|
}
|
|
|
|
public struct PoseIntegratorCallbacks : IPoseIntegratorCallbacks
|
|
{
|
|
public Vector3 Gravity;
|
|
public float LinearDamping;
|
|
public float AngularDamping;
|
|
private SimulationManagerBEPU _simulationManager; // ✅ NUEVA REFERENCIA
|
|
|
|
public PoseIntegratorCallbacks(Vector3 gravity, float linearDamping = 0.999f, float angularDamping = 0.995f, SimulationManagerBEPU simulationManager = null)
|
|
{
|
|
Gravity = gravity;
|
|
LinearDamping = linearDamping;
|
|
AngularDamping = angularDamping;
|
|
_simulationManager = simulationManager; // ✅ NUEVA INICIALIZACIÓN
|
|
}
|
|
|
|
public readonly AngularIntegrationMode AngularIntegrationMode => AngularIntegrationMode.Nonconserving;
|
|
|
|
public readonly bool AllowSubstepsForUnconstrainedBodies => false;
|
|
|
|
public readonly bool IntegrateVelocityForKinematics => false;
|
|
|
|
public void Initialize(Simulation simulation)
|
|
{
|
|
}
|
|
|
|
public void PrepareForIntegration(float dt)
|
|
{
|
|
}
|
|
|
|
// ✅ REESCRITO COMPLETAMENTE: para corregir errores de compilación y aplicar la lógica de amortiguamiento de forma segura.
|
|
public void IntegrateVelocity(Vector<int> bodyIndices, Vector3Wide position, QuaternionWide orientation, BodyInertiaWide localInertia, Vector<int> integrationMask, int workerIndex, Vector<float> dt, ref BodyVelocityWide velocity)
|
|
{
|
|
// 1. APLICAR GRAVEDAD Y AMORTIGUAMIENTO ESTÁNDAR (VECTORIZADO)
|
|
var gravityWide = Vector3Wide.Broadcast(Gravity);
|
|
velocity.Linear.X += gravityWide.X * dt;
|
|
velocity.Linear.Y += gravityWide.Y * dt;
|
|
velocity.Linear.Z += gravityWide.Z * dt;
|
|
|
|
var linearDampingFactor = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - LinearDamping, 0, 1), dt[0]));
|
|
var angularDampingFactor = new Vector<float>(MathF.Pow(MathHelper.Clamp(1 - AngularDamping, 0, 1), dt[0]));
|
|
|
|
velocity.Linear *= linearDampingFactor;
|
|
velocity.Angular *= angularDampingFactor;
|
|
|
|
// 2. LÓGICA PERSONALIZADA PARA BOTELLAS (ESCALAR)
|
|
const float bottleExtraZAngularDamping = 0.8f;
|
|
var bottleZAngularDampingFactor = MathF.Pow(MathHelper.Clamp(1 - bottleExtraZAngularDamping, 0, 1), dt[0]);
|
|
|
|
// Recorremos cada "carril" del lote de simulación.
|
|
for (int i = 0; i < Vector<int>.Count; ++i)
|
|
{
|
|
if (integrationMask[i] != 0)
|
|
{
|
|
var bodyIndex = bodyIndices[i];
|
|
// Asegurarse de que el índice del cuerpo es válido para el conjunto activo.
|
|
if (bodyIndex < _simulationManager.simulation.Bodies.ActiveSet.Count)
|
|
{
|
|
var handle = _simulationManager.simulation.Bodies.ActiveSet.IndexToHandle[bodyIndex];
|
|
if (_simulationManager.CollidableData.TryGetValue(handle.Value, out simBase baseObj) && baseObj is simBotella)
|
|
{
|
|
// B. DAMPING ANGULAR EXTRA EN EJE Z
|
|
// Para modificar un solo carril de un Vector<T>, la forma más segura es extraer
|
|
// los valores a un array, modificar el índice y crear un nuevo Vector<T>.
|
|
var zVel = new float[Vector<float>.Count];
|
|
velocity.Angular.Z.CopyTo(zVel, 0);
|
|
zVel[i] *= bottleZAngularDampingFactor;
|
|
velocity.Angular.Z = new Vector<float>(zVel);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
public class SimulationManagerBEPU : IDisposable
|
|
{
|
|
public Simulation simulation;
|
|
public List<simBase> Cuerpos;
|
|
public List<Action> _deferredActions;
|
|
private BufferPool bufferPool;
|
|
private Stopwatch stopwatch;
|
|
private double stopwatch_last;
|
|
|
|
// Referencia al manager de visualización 3D
|
|
public BEPUVisualization3DManager Visualization3DManager { get; set; }
|
|
|
|
// Propiedad para controlar si la actualización 3D está habilitada
|
|
public bool Is3DUpdateEnabled { get; set; } = true;
|
|
|
|
// ✅ NUEVAS - para filtrado eficiente de callbacks
|
|
private HashSet<BodyHandle> _transportHandles;
|
|
private HashSet<BodyHandle> _curveHandles;
|
|
private HashSet<BodyHandle> _barrierHandles;
|
|
private HashSet<BodyHandle> _discardHandles;
|
|
private HashSet<BodyHandle> _bottleHandles;
|
|
private HashSet<BodyHandle> _guiaHandles;
|
|
|
|
|
|
// ✅ NUEVO - contador de frames para optimizaciones
|
|
private int _frameCount = 0;
|
|
|
|
// ✅ CONSERVAR - sistemas existentes que funcionan bien
|
|
private Dictionary<simBarrera, List<simBotella>> _barreraContacts;
|
|
private Dictionary<simDescarte, List<simBotella>> _descarteContacts;
|
|
private HashSet<simBotella> _botellasParaEliminar;
|
|
|
|
// ✅ NUEVAS - propiedades configurables para fricción de guías
|
|
public float GuiaFrictionCoefficient { get; set; } = 0.1f;
|
|
public float GuiaMaxRecoveryVelocity { get; set; } = 0.5f;
|
|
public float GuiaSpringFrequency { get; set; } = 40f;
|
|
public float GuiaSpringDampingRatio { get; set; } = 2f;
|
|
|
|
private object _contactsLock = new object();
|
|
|
|
// ✅ NUEVO: Diccionario para mapear BodyHandle a simBase para acceso O(1).
|
|
public Dictionary<int, simBase> CollidableData = new Dictionary<int, simBase>();
|
|
public float GlobalTime { get; private set; }
|
|
|
|
/// <summary>
|
|
/// Obtiene el objeto simBase correspondiente a un BodyHandle
|
|
/// </summary>
|
|
public simBase GetSimBaseFromBodyHandle(BodyHandle bodyHandle)
|
|
{
|
|
return Cuerpos.FirstOrDefault(c => c.BodyHandle.Equals(bodyHandle));
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ SIMPLIFICADO: Obtiene una simCurve desde su BodyHandle principal
|
|
/// </summary>
|
|
public simCurve GetCurveFromTriangleBodyHandle(BodyHandle bodyHandle)
|
|
{
|
|
// ✅ SIMPLIFICADO: Solo buscar en cuerpos principales (ya no hay triángulos separados)
|
|
var simBase = GetSimBaseFromBodyHandle(bodyHandle);
|
|
return simBase as simCurve;
|
|
}
|
|
|
|
// ✅ ELIMINADO: GetBottlesConnectedToElement ya no es necesario.
|
|
// La conexión ahora es implícita a través de la física de contacto.
|
|
|
|
// ✅ NUEVOS - gestión automática de clasificación
|
|
private void RegisterObjectHandle(simBase obj)
|
|
{
|
|
switch (obj)
|
|
{
|
|
case simBotella bottle:
|
|
_bottleHandles.Add(bottle.BodyHandle);
|
|
break;
|
|
case simTransporte transport:
|
|
_transportHandles.Add(transport.BodyHandle);
|
|
// Calcular propiedades iniciales
|
|
transport.UpdateCachedProperties();
|
|
break;
|
|
case simCurve curve:
|
|
_curveHandles.Add(curve.BodyHandle);
|
|
// ✅ SIMPLIFICADO: Ya no hay triángulos separados, solo el cuerpo principal
|
|
break;
|
|
case simBarrera barrier:
|
|
// ✅ NUEVO: Las barreras ya no tienen body físico, no registrar handle
|
|
// _barrierHandles.Add(barrier.BodyHandle); // ELIMINADO
|
|
break;
|
|
case simDescarte discard:
|
|
_discardHandles.Add(discard.BodyHandle);
|
|
break;
|
|
case simGuia guia:
|
|
_guiaHandles.Add(guia.BodyHandle);
|
|
break;
|
|
}
|
|
}
|
|
|
|
private void UnregisterObjectHandle(simBase obj)
|
|
{
|
|
switch (obj)
|
|
{
|
|
case simBotella bottle:
|
|
_bottleHandles.Remove(bottle.BodyHandle);
|
|
|
|
break;
|
|
case simTransporte transport:
|
|
_transportHandles.Remove(transport.BodyHandle);
|
|
break;
|
|
case simCurve curve:
|
|
_curveHandles.Remove(curve.BodyHandle);
|
|
// ✅ SIMPLIFICADO: Ya no hay triángulos separados
|
|
break;
|
|
case simBarrera barrier:
|
|
// ✅ NUEVO: Las barreras ya no tienen body físico, no desregistrar handle
|
|
// _barrierHandles.Remove(barrier.BodyHandle); // ELIMINADO
|
|
break;
|
|
case simDescarte discard:
|
|
_discardHandles.Remove(discard.BodyHandle);
|
|
break;
|
|
case simGuia guia:
|
|
_guiaHandles.Remove(guia.BodyHandle);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
// ✅ NUEVOS MÉTODOS - obtener objetos por handle
|
|
private simBotella GetBottleByHandle(BodyHandle handle)
|
|
{
|
|
return Cuerpos.OfType<simBotella>().FirstOrDefault(b => b.BodyHandle.Equals(handle));
|
|
}
|
|
|
|
private simTransporte GetTransportByHandle(BodyHandle handle)
|
|
{
|
|
return Cuerpos.OfType<simTransporte>().FirstOrDefault(t => t.BodyHandle.Equals(handle));
|
|
}
|
|
|
|
private simCurve GetCurveByHandle(BodyHandle handle)
|
|
{
|
|
// Primero buscar en cuerpos principales
|
|
var curve = Cuerpos.OfType<simCurve>().FirstOrDefault(c => c.BodyHandle.Equals(handle));
|
|
if (curve != null) return curve;
|
|
|
|
// Luego buscar en triángulos
|
|
return GetCurveFromTriangleBodyHandle(handle);
|
|
}
|
|
|
|
private simGuia GetGuiaByHandle(BodyHandle handle)
|
|
{
|
|
return Cuerpos.OfType<simGuia>().FirstOrDefault(g => g.BodyHandle.Equals(handle));
|
|
}
|
|
|
|
public SimulationManagerBEPU()
|
|
{
|
|
// ✅ EXISTENTES - conservar
|
|
Cuerpos = new List<simBase>();
|
|
_deferredActions = new List<Action>();
|
|
_barreraContacts = new Dictionary<simBarrera, List<simBotella>>();
|
|
_descarteContacts = new Dictionary<simDescarte, List<simBotella>>();
|
|
_botellasParaEliminar = new HashSet<simBotella>();
|
|
|
|
// ✅ NUEVOS - sistemas de filtrado y contactos
|
|
_transportHandles = new HashSet<BodyHandle>();
|
|
_curveHandles = new HashSet<BodyHandle>();
|
|
_barrierHandles = new HashSet<BodyHandle>();
|
|
_discardHandles = new HashSet<BodyHandle>();
|
|
_bottleHandles = new HashSet<BodyHandle>();
|
|
_guiaHandles = new HashSet<BodyHandle>();
|
|
|
|
|
|
// ✅ CONSERVAR - resto del constructor igual
|
|
bufferPool = new BufferPool();
|
|
stopwatch = new Stopwatch();
|
|
|
|
var narrowPhaseCallbacks = new NarrowPhaseCallbacks(this);
|
|
|
|
// Configurar amortiguamiento para comportamiento realista:
|
|
// - LinearDamping: 0.999f (muy leve, objetos siguen moviéndose pero eventualmente se detienen)
|
|
// - AngularDamping: 0.995f (más agresivo para detener rotaciones infinitas de cilindros)
|
|
// ✅ MODIFICADO: Pasar referencia de this al PoseIntegratorCallbacks
|
|
var poseIntegratorCallbacks = new PoseIntegratorCallbacks(
|
|
gravity: new Vector3(0, 0, -9.81f), // Gravedad en Z
|
|
linearDamping: 0.999f, // Amortiguamiento lineal suave
|
|
angularDamping: 0.995f, // Amortiguamiento angular más fuerte para detener rotaciones
|
|
simulationManager: this // ✅ NUEVA REFERENCIA
|
|
);
|
|
|
|
var solveDescription = new SolveDescription(8, 1);
|
|
|
|
simulation = Simulation.Create(bufferPool, narrowPhaseCallbacks, poseIntegratorCallbacks, solveDescription);
|
|
|
|
// ✅ NUEVO: Suscribirse a los eventos del solver para controlar la velocidad de los cuerpos cinemáticos.
|
|
simulation.Solver.SubstepStarted += OnSubstepStarted;
|
|
simulation.Solver.SubstepEnded += OnSubstepEnded;
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Se ejecuta al inicio de cada sub-paso de la simulación.
|
|
/// Restaura las velocidades de los cuerpos cinemáticos para que el solver pueda usarlas para calcular la fricción.
|
|
/// </summary>
|
|
private void OnSubstepStarted(int substepIndex)
|
|
{
|
|
foreach (var transport in Cuerpos.OfType<simTransporte>())
|
|
{
|
|
transport.ActualizarVelocidadCinematica();
|
|
}
|
|
foreach (var curve in Cuerpos.OfType<simCurve>())
|
|
{
|
|
curve.ActualizarVelocidadCinematica();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Se ejecuta al final de cada sub-paso de la simulación, pero antes de la integración de la pose.
|
|
/// Pone a cero las velocidades de los cuerpos cinemáticos para evitar que el PoseIntegrator mueva su posición.
|
|
/// </summary>
|
|
private void OnSubstepEnded(int substepIndex)
|
|
{
|
|
foreach (var transport in Cuerpos.OfType<simTransporte>())
|
|
{
|
|
if (transport.BodyHandle.Value >= 0 && simulation.Bodies.BodyExists(transport.BodyHandle))
|
|
{
|
|
var body = simulation.Bodies.GetBodyReference(transport.BodyHandle);
|
|
if (body.Kinematic)
|
|
{
|
|
body.Velocity.Linear = Vector3.Zero;
|
|
}
|
|
}
|
|
}
|
|
foreach (var curve in Cuerpos.OfType<simCurve>())
|
|
{
|
|
if (curve.BodyHandle.Value >= 0 && simulation.Bodies.BodyExists(curve.BodyHandle))
|
|
{
|
|
var body = simulation.Bodies.GetBodyReference(curve.BodyHandle);
|
|
if (body.Kinematic)
|
|
{
|
|
body.Velocity.Angular = Vector3.Zero;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Clear()
|
|
{
|
|
try
|
|
{
|
|
|
|
|
|
// ✅ SIMPLIFICAR - eliminar lógica de masa especial
|
|
// foreach (var cuerpo in Cuerpos.OfType<simBotella>())
|
|
// {
|
|
// cuerpo.RestoreOriginalMassIfNeeded(); // NO SE USA MÁS
|
|
// }
|
|
|
|
// ✅ CONSERVAR - limpiar contactos
|
|
lock (_contactsLock)
|
|
{
|
|
_barreraContacts.Clear();
|
|
_descarteContacts.Clear();
|
|
_botellasParaEliminar.Clear();
|
|
|
|
}
|
|
|
|
// ✅ NUEVO - limpiar clasificaciones
|
|
_transportHandles.Clear();
|
|
_curveHandles.Clear();
|
|
_barrierHandles.Clear();
|
|
_discardHandles.Clear();
|
|
_bottleHandles.Clear();
|
|
_guiaHandles.Clear();
|
|
|
|
// ✅ CONSERVAR - resto del método igual
|
|
var cuerposToRemove = new List<simBase>(Cuerpos);
|
|
foreach (var cuerpo in cuerposToRemove)
|
|
{
|
|
try
|
|
{
|
|
if (cuerpo != null)
|
|
{
|
|
cuerpo.RemoverBody();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error removing body - continue with cleanup
|
|
}
|
|
}
|
|
|
|
Cuerpos.Clear();
|
|
_deferredActions.Clear();
|
|
|
|
// ✅ NUEVO - Limpiar todas las dimensiones almacenadas
|
|
CtrEditor.ObjetosSim.osBase.ClearAllStoredDimensions();
|
|
|
|
// Limpiar la simulación completamente si existe
|
|
if (simulation != null)
|
|
{
|
|
try
|
|
{
|
|
// Forzar un timestep pequeño para limpiar contactos pendientes
|
|
simulation.Timestep(1f / 1000f); // 1ms
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Warning during cleanup timestep - continue
|
|
}
|
|
}
|
|
|
|
// Limpiar visualización 3D
|
|
Visualization3DManager?.Clear();
|
|
|
|
// ✅ NUEVO: Limpiar también el nuevo diccionario.
|
|
CollidableData.Clear();
|
|
GlobalTime = 0f;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error during clear - operation failed
|
|
}
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
try
|
|
{
|
|
// ✅ INICIALIZAR PROPIEDADES CACHEADAS Y VELOCIDADES CINEMÁTICAS
|
|
foreach (var transport in Cuerpos.OfType<simTransporte>())
|
|
{
|
|
transport.UpdateCachedProperties();
|
|
}
|
|
|
|
foreach (var curve in Cuerpos.OfType<simCurve>())
|
|
{
|
|
curve.SetSpeed(curve.Speed);
|
|
}
|
|
|
|
stopwatch.Start();
|
|
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[SimulationManager] Error in Start: {ex.Message}");
|
|
stopwatch.Start();
|
|
stopwatch_last = stopwatch.Elapsed.TotalMilliseconds;
|
|
}
|
|
}
|
|
|
|
public void Step()
|
|
{
|
|
try
|
|
{
|
|
_frameCount++;
|
|
|
|
// ✅ NUEVO: Resetear el estado de contacto para el sistema de presión
|
|
foreach (var botella in Cuerpos.OfType<simBotella>())
|
|
{
|
|
botella.IsTouchingTransport = false;
|
|
}
|
|
|
|
// ✅ CONSERVAR - validación de deltaTime
|
|
var currentTime = stopwatch.Elapsed.TotalMilliseconds;
|
|
var deltaTime = (float)((currentTime - stopwatch_last) / 1000.0);
|
|
stopwatch_last = currentTime;
|
|
|
|
if (float.IsNaN(deltaTime) || float.IsInfinity(deltaTime) || deltaTime <= 0)
|
|
{
|
|
deltaTime = 1f / 60f;
|
|
}
|
|
|
|
const float maxDeltaTime = 0.1f;
|
|
if (deltaTime > maxDeltaTime)
|
|
{
|
|
deltaTime = 1f / 60f;
|
|
}
|
|
|
|
// ✅ CONSERVAR - procesar acciones diferidas
|
|
foreach (var action in _deferredActions)
|
|
{
|
|
action();
|
|
}
|
|
_deferredActions.Clear();
|
|
|
|
// ✅ CONSERVAR - validaciones de simulación
|
|
if (simulation?.Bodies == null)
|
|
{
|
|
return;
|
|
}
|
|
|
|
var invalidBodies = Cuerpos.Where(c => c != null && !simulation.Bodies.BodyExists(c.BodyHandle)).ToList();
|
|
if (invalidBodies.Count > 0)
|
|
{
|
|
foreach (var invalidBody in invalidBodies)
|
|
{
|
|
Cuerpos.Remove(invalidBody);
|
|
}
|
|
}
|
|
|
|
// ✅ CONSERVAR - timestep
|
|
var timestepValue = Math.Max(deltaTime, 1f / 120f);
|
|
|
|
try
|
|
{
|
|
simulation.Timestep(timestepValue);
|
|
}
|
|
catch (AccessViolationException ex)
|
|
{
|
|
lock (_contactsLock)
|
|
{
|
|
_barreraContacts.Clear();
|
|
}
|
|
throw;
|
|
}
|
|
|
|
|
|
|
|
// ✅ CONSERVAR - limitación de rotación y mantener botellas despiertas
|
|
foreach (var cuerpo in Cuerpos.OfType<simBotella>().ToList())
|
|
{
|
|
try
|
|
{
|
|
if (simulation.Bodies.BodyExists(cuerpo.BodyHandle))
|
|
{
|
|
// cuerpo.LimitRotationToXYPlane();
|
|
simulation.Awakener.AwakenBody(cuerpo.BodyHandle);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error limiting rotation for bottle - continue
|
|
}
|
|
}
|
|
|
|
// ✅ CONSERVAR - sistemas que funcionan bien
|
|
ProcessBarreraContacts();
|
|
ProcessDescarteContacts();
|
|
ProcessPressureSystem();
|
|
ProcessCleanupSystem();
|
|
|
|
// ✅ SIMPLIFICAR - limpiar solo contactos que usamos
|
|
lock (_contactsLock)
|
|
{
|
|
_barreraContacts.Clear();
|
|
_descarteContacts.Clear();
|
|
}
|
|
|
|
// ✅ CONSERVAR - sincronización 3D
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
lock (_contactsLock)
|
|
{
|
|
_barreraContacts.Clear();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Sistema para controlar la "presión" y evitar que las botellas floten
|
|
/// Si una botella no toca un transporte por varios frames, se reestablece su altura.
|
|
/// </summary>
|
|
private void ProcessPressureSystem()
|
|
{
|
|
try
|
|
{
|
|
var botellas = Cuerpos.OfType<simBotella>().ToList();
|
|
|
|
foreach (var botella in botellas)
|
|
{
|
|
try
|
|
{
|
|
if (botella == null || !simulation.Bodies.BodyExists(botella.BodyHandle))
|
|
continue;
|
|
|
|
if (!botella.IsTouchingTransport && botella.LastTransportCollisionZ.HasValue)
|
|
{
|
|
var body = simulation.Bodies.GetBodyReference(botella.BodyHandle);
|
|
var currentZ = body.Pose.Position.Z;
|
|
|
|
// ✅ NUEVA CONDICIÓN: Solo actuar si la botella ha subido
|
|
if (currentZ > botella.LastTransportCollisionZ.Value)
|
|
{
|
|
// Incrementar el contador de presión.
|
|
botella.PressureBuildup++;
|
|
|
|
// Si se acumula suficiente presión (más de 2 frames sin contacto y subiendo),
|
|
// es un indicio de que está flotando. La reestablecemos a su última Z conocida.
|
|
if (botella.PressureBuildup > 2)
|
|
{
|
|
var pose = body.Pose;
|
|
pose.Position.Z = botella.LastTransportCollisionZ.Value;
|
|
body.Pose = pose;
|
|
|
|
// Opcional: resetear el contador para no aplicarlo constantemente en cada frame
|
|
// botella.PressureBuildup = 0;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[ProcessPressureSystem] Error processing bottle {botella?.BodyHandle}: {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[ProcessPressureSystem] Critical error: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Marca un objeto para eliminación diferida (más seguro)
|
|
/// </summary>
|
|
public void Remove(simBase Objeto)
|
|
{
|
|
if (Objeto != null)
|
|
{
|
|
// Aplazar la eliminación para evitar problemas de concurrencia
|
|
_deferredActions.Add(() =>
|
|
{
|
|
UnregisterObjectHandle(Objeto);
|
|
Objeto.RemoverBody();
|
|
Cuerpos.Remove(Objeto);
|
|
});
|
|
}
|
|
}
|
|
|
|
|
|
public simBotella AddBotella(float diameter, Vector2 position, float mass)
|
|
{
|
|
var botella = new simBotella(simulation, _deferredActions, diameter, position, mass);
|
|
Cuerpos.Add(botella);
|
|
RegisterObjectHandle(botella);
|
|
return botella;
|
|
}
|
|
|
|
public simTransporte AddRectangle(float width, float height, Vector2 position, float angle)
|
|
{
|
|
var transporte = new simTransporte(simulation, _deferredActions, width, height, position, angle, this);
|
|
Cuerpos.Add(transporte);
|
|
RegisterObjectHandle(transporte); // ✅ NUEVO - incluye UpdateCachedProperties()
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
return transporte;
|
|
}
|
|
|
|
internal simBarrera AddBarrera(float width, float height, Vector2 wpfTopLeft, float wpfAngle, bool detectarCuello)
|
|
{
|
|
// ✅ CONVERTIR coordenadas WPF a BEPU internas
|
|
var zPosition = simBase.zPos_Barrera;
|
|
var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);
|
|
var bepuRadians = CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle);
|
|
|
|
var barrera = new simBarrera(simulation, width, height, bepuCenter, bepuRadians, detectarCuello, this);
|
|
Cuerpos.Add(barrera);
|
|
RegisterObjectHandle(barrera);
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
return barrera;
|
|
}
|
|
|
|
|
|
|
|
public simGuia AddLine(float width, float height, Vector2 topLeft, float angle)
|
|
{
|
|
var guia = new simGuia(simulation, _deferredActions, width, height, topLeft, angle);
|
|
Cuerpos.Add(guia);
|
|
// ✅ NOTA: simGuia no requiere registro especial
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
return guia;
|
|
}
|
|
|
|
public simDescarte AddDescarte(float diameter, Vector2 position)
|
|
{
|
|
var descarte = new simDescarte(simulation, _deferredActions, diameter, position);
|
|
Cuerpos.Add(descarte);
|
|
RegisterObjectHandle(descarte); // ✅ NUEVO
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
return descarte;
|
|
}
|
|
|
|
public simCurve AddCurve(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0)
|
|
{
|
|
var curve = new simCurve(simulation, _deferredActions, innerRadius, outerRadius, startAngle, endAngle, topLeft, unused, this);
|
|
Cuerpos.Add(curve);
|
|
RegisterObjectHandle(curve); // ✅ NUEVO
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
return curve;
|
|
}
|
|
|
|
// ✅ NUEVA INTERFAZ PARA BARRERAS - Solo accesible desde SimulationManagerBEPU
|
|
|
|
/// <summary>
|
|
/// Crea una nueva barrera y devuelve un handle opaco para WPF
|
|
/// </summary>
|
|
/// <param name="width">Ancho en metros</param>
|
|
/// <param name="height">Alto en metros</param>
|
|
/// <param name="bepuCenter">Posición central en coordenadas BEPU</param>
|
|
/// <param name="bepuRadians">Ángulo en radianes BEPU</param>
|
|
/// <param name="detectarCuello">Si debe detectar el cuello de las botellas</param>
|
|
/// <returns>Handle opaco para referenciar la barrera desde WPF</returns>
|
|
public simBarrera CreateBarrera(float width, float height, Vector2 TopLeft, float wpfAngle, bool detectarCuello)
|
|
{
|
|
var barrera = new simBarrera(simulation, width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width, height, wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle), detectarCuello, this);
|
|
Cuerpos.Add(barrera);
|
|
RegisterObjectHandle(barrera);
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
|
|
return barrera;
|
|
}
|
|
|
|
public void UpdateBarrera(simBarrera barrera, float width, float height, Vector2 TopLeft, float wpfAngle)
|
|
{
|
|
barrera.Update(width, height, CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(TopLeft, width, height, wpfAngle, simBase.zPos_Barrera), CoordinateConverter.WpfDegreesToBepuRadians(wpfAngle));
|
|
|
|
if (Is3DUpdateEnabled)
|
|
{
|
|
Visualization3DManager?.SynchronizeWorld();
|
|
}
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Elimina una barrera usando su handle
|
|
/// </summary>
|
|
/// <param name="handle">Handle de la barrera a eliminar</param>
|
|
public void RemoveBarrera(simBarrera barrera)
|
|
{
|
|
if (barrera != null)
|
|
{
|
|
Remove(barrera);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configura si la barrera debe detectar el cuello de las botellas
|
|
/// </summary>
|
|
/// <param name="handle">Handle de la barrera</param>
|
|
/// <param name="detectarCuello">True para detectar cuello, false para detectar botella completa</param>
|
|
public void SetBarreraDetectNeck(simBarrera barrera, bool detectarCuello)
|
|
{
|
|
if (barrera != null)
|
|
{
|
|
barrera.DetectNeck = detectarCuello;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Obtiene los datos de estado de la barrera
|
|
/// </summary>
|
|
/// <param name="handle">Handle de la barrera</param>
|
|
/// <returns>Estructura con los datos de luz cortada</returns>
|
|
public BarreraData GetBarreraData(simBarrera barrera)
|
|
{
|
|
if (barrera != null)
|
|
{
|
|
return new BarreraData(barrera.LuzCortada, barrera.LuzCortadaNeck);
|
|
}
|
|
return new BarreraData(false, false);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Procesa todas las barreras usando RayCast de BEPU
|
|
/// Elimina la dependencia de contactos físicos y la necesidad de mantener botellas despiertas
|
|
/// </summary>
|
|
private void ProcessBarreraContacts()
|
|
{
|
|
try
|
|
{
|
|
// Obtener todas las barreras
|
|
var barreras = Cuerpos.OfType<simBarrera>().ToList();
|
|
|
|
foreach (var barrera in barreras)
|
|
{
|
|
try
|
|
{
|
|
// Verificar que la barrera aún existe (solo verificar que no sea null)
|
|
if (barrera == null)
|
|
continue;
|
|
|
|
// ✅ USAR RAYCAST NATIVO DE BEPU
|
|
barrera.PerformRaycast();
|
|
|
|
// System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Barrera procesada: LuzCortada={barrera.LuzCortada}, LuzCortadaNeck={barrera.LuzCortadaNeck}, Distancia={barrera.Distancia:F3}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error procesando barrera: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
// Limpiar contactos ya que no los usamos más
|
|
lock (_contactsLock)
|
|
{
|
|
_barreraContacts.Clear();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[ProcessBarreraContacts] Error crítico: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
/// <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();
|
|
}
|
|
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)
|
|
{
|
|
// ✅ NUEVO: Añadir botellas marcadas como "Descartar" desde los callbacks
|
|
foreach (var botella in Cuerpos.OfType<simBotella>())
|
|
{
|
|
if (botella.Descartar)
|
|
{
|
|
_botellasParaEliminar.Add(botella);
|
|
}
|
|
}
|
|
|
|
botellasAEliminar = new List<simBotella>(_botellasParaEliminar);
|
|
_botellasParaEliminar.Clear();
|
|
}
|
|
|
|
foreach (var botella in botellasAEliminar)
|
|
{
|
|
try
|
|
{
|
|
if (botella != null && Cuerpos.Contains(botella))
|
|
{
|
|
//System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] 🗑️ Marcando botella para eliminación: {botella.BodyHandle}");
|
|
|
|
// ✅ USAR ELIMINACIÓN DIFERIDA (más seguro)
|
|
Remove(botella);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ Error marcando botella {botella?.BodyHandle}: {ex.Message}");
|
|
// ✅ INTENTAR remover de la lista incluso si Remove falló
|
|
if (botella != null)
|
|
{
|
|
Cuerpos.Remove(botella);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[RemoveMarkedBottles] ❌ ERROR CRÍTICO: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Sistema de limpieza que elimina botellas que estén debajo de la altura de los transportes
|
|
/// Cualquier botella con Z menor al nivel superior de los transportes será eliminada
|
|
/// </summary>
|
|
private void ProcessCleanupSystem()
|
|
{
|
|
try
|
|
{
|
|
// Altura máxima de los transportes (nivel superior)
|
|
var maxTransportHeight = simBase.zPos_Transporte + simBase.zAltura_Transporte;
|
|
|
|
// Obtener todas las botellas
|
|
var botellas = Cuerpos.OfType<simBotella>().ToList();
|
|
|
|
foreach (var botella in botellas)
|
|
{
|
|
try
|
|
{
|
|
// Validar que la botella aún existe en la simulación
|
|
if (botella == null || !simulation?.Bodies?.BodyExists(botella.BodyHandle) == true)
|
|
continue;
|
|
|
|
var posicion = botella.GetPosition();
|
|
|
|
// Validar posición
|
|
if (float.IsNaN(posicion.Z) || float.IsInfinity(posicion.Z))
|
|
continue;
|
|
|
|
// Si la botella está debajo del nivel de los transportes, marcarla para eliminación
|
|
if (posicion.Z < (maxTransportHeight - 2 * botella.Radius) || posicion.Z > (maxTransportHeight + 2 * botella.Radius))
|
|
{
|
|
lock (_contactsLock)
|
|
{
|
|
botella.Descartar = true;
|
|
_botellasParaEliminar.Add(botella);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Error processing bottle in cleanup - continue
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Critical error in ProcessCleanupSystem - continue
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Proyecta un punto sobre una línea definida por dos puntos
|
|
/// </summary>
|
|
private Vector3 ProjectPointOntoLine(Vector3 point, Vector3 lineStart, Vector3 lineEnd)
|
|
{
|
|
var lineDirection = Vector3.Normalize(lineEnd - lineStart);
|
|
var pointToStart = point - lineStart;
|
|
var projectionLength = Vector3.Dot(pointToStart, lineDirection);
|
|
|
|
// Restringir la proyección a los límites de la línea
|
|
var lineLength = Vector3.Distance(lineStart, lineEnd);
|
|
projectionLength = Math.Max(0, Math.Min(projectionLength, lineLength));
|
|
|
|
return lineStart + lineDirection * projectionLength;
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
Clear();
|
|
simulation?.Dispose();
|
|
bufferPool?.Clear();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Registra un contacto entre una barrera y una botella para detección de paso
|
|
/// </summary>
|
|
/// <param name="barrera">Barrera que detecta el paso</param>
|
|
/// <param name="botella">Botella que está pasando</param>
|
|
internal void RegisterBarreraContact(simBarrera barrera, simBotella botella)
|
|
{
|
|
if (barrera != null && botella != null)
|
|
{
|
|
lock (_contactsLock)
|
|
{
|
|
if (!_barreraContacts.ContainsKey(barrera))
|
|
{
|
|
_barreraContacts[barrera] = new List<simBotella>();
|
|
}
|
|
|
|
if (!_barreraContacts[barrera].Contains(botella))
|
|
{
|
|
_barreraContacts[barrera].Add(botella);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
} |