CtrEditor/Simulacion/simTransporte.cs

238 lines
9.4 KiB
C#

using BepuPhysics.Collidables;
using BepuPhysics;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using System.Text;
using System.Threading.Tasks;
namespace CtrEditor.Simulacion
{
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; }
// ✅ NUEVAS PROPIEDADES - cachear cálculos costosos
public Vector3 DirectionVector { get; private set; }
public List<simBotella> BottlesOnTransport { get; private set; } = new List<simBotella>();
public simTransporte(Simulation simulation, List<Action> deferredActions, float width, float height, Vector2 topLeft, float angle = 0, SimulationManagerBEPU simulationManager = null)
{
_simulation = simulation;
_deferredActions = deferredActions;
_simulationManager = simulationManager; // ✅ NUEVO: Almacenar referencia
Width = width;
Height = height;
// Usar el nuevo método Create que maneja Top-Left correctamente
Create(width, height, topLeft, angle);
// ✅ INICIALIZAR PROPIEDADES CRÍTICAS
UpdateCachedProperties();
}
public float Angle
{
get
{
return GetRotationZ();
}
set
{
SetRotation(value);
}
}
/// <summary>
/// ✅ SOBRESCRITO: SetRotation que actualiza automáticamente las propiedades cacheadas
/// </summary>
public new void SetRotation(float wpfAngle)
{
base.SetRotation(wpfAngle);
// ✅ CRÍTICO: Actualizar propiedades cacheadas y velocidad después del cambio de rotación
UpdateCachedProperties();
}
public new void SetPosition(float x, float y, float z = 0)
{
base.SetPosition(x, y, z);
}
/// <summary>
/// ✅ NUEVO: Actualiza posición desde Top-Left WPF con dimensiones y ángulo actuales
/// </summary>
internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
{
var currentWpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
var zPosition = GetPosition().Z; // Mantener Z actual
var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, currentWpfAngle, zPosition);
SetPosition(bepuCenter);
}
/// <summary>
/// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
/// </summary>
internal Vector2 GetWpfTopLeft()
{
var bepuCenter = GetPosition();
var wpfAngle = GetRotationZ(); // Ya usa CoordinateConverter
return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(bepuCenter, Width, Height, wpfAngle);
}
/// <summary>
/// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
/// </summary>
internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle)
{
var zPosition = GetPosition().Z; // Mantener Z actual
var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, Width, Height, wpfAngle, zPosition);
CoordinateConverter.UpdateBepuBodyPose(_simulation, BodyHandle, bepuCenter, wpfAngle);
// ✅ CRÍTICO: Actualizar propiedades cacheadas y velocidad después del cambio de orientación
UpdateCachedProperties();
}
// ✅ NUEVO MÉTODO - actualizar propiedades cacheadas y velocidad cinemática
internal void UpdateCachedProperties()
{
// ✅ CORREGIDO: La dirección siempre es UnitX rotado por el ángulo del transporte
// NO depende de las dimensiones (Width >= Height) sino solo de la rotación
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
{
var bodyReference = _simulation.Bodies.GetBodyReference(BodyHandle);
var bepuQuaternion = bodyReference.Pose.Orientation;
// ✅ SIEMPRE usar UnitX y aplicar la rotación
DirectionVector = Vector3.Transform(Vector3.UnitX, bepuQuaternion);
// 🔍 DEBUG: Agregar información detallada
var wpfAngle = GetRotationZ();
//System.Diagnostics.Debug.WriteLine($"[UpdateCached] WPF Angle: {wpfAngle}°, DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}");
}
else
{
// ✅ CORREGIDO: Aplicar conversión de coordenadas WPF→BEPU en el vector
var wpfAngle = GetRotationZ(); // Ángulo WPF en grados
var wpfAngleRadians = simBase.GradosARadianes(wpfAngle);
// Calcular el vector en coordenadas WPF
var wpfX = (float)Math.Cos(wpfAngleRadians);
var wpfY = (float)Math.Sin(wpfAngleRadians);
// ✅ APLICAR CONVERSIÓN Y: En WPF Y+ es abajo, en BEPU Y+ es arriba
DirectionVector = new Vector3(wpfX, -wpfY, 0); // Invertir Y para conversión WPF→BEPU
// 🔍 DEBUG: Agregar información detallada
System.Diagnostics.Debug.WriteLine($"[UpdateCached-Fallback] WPF Angle: {wpfAngle}°, WPF Vector: ({wpfX:F3}, {wpfY:F3}), BEPU DirectionVector: {DirectionVector}, Length: {DirectionVector.Length()}");
}
// ✅ NUEVO: Actualizar la velocidad del cuerpo cinemático siempre que cambien las propiedades
ActualizarVelocidadCinematica();
}
// ✅ MODIFICADO: Actualizar la velocidad y luego la del cuerpo cinemático
public void SetSpeed(float speed)
{
Speed = speed;
ActualizarVelocidadCinematica();
}
/// <summary>
/// ✅ NUEVO: Aplica la velocidad al cuerpo cinemático de BEPU.
/// </summary>
public void ActualizarVelocidadCinematica()
{
if (_simulation != null && this.BodyHandle.Value >= 0 && _simulation.Bodies.BodyExists(this.BodyHandle))
{
var body = _simulation.Bodies.GetBodyReference(BodyHandle);
// ✅ CORREGIDO: Convertir velocidad de m/min a m/s para la simulación
body.Velocity.Linear = this.DirectionVector * (this.Speed / SpeedConversionFactor);
}
}
/// <summary>
/// Detiene completamente el transporte
/// </summary>
public void StopTransport()
{
SetSpeed(0f);
}
/// <summary>
/// Invierte la dirección del transporte manteniendo la misma velocidad
/// </summary>
public void ReverseTransport()
{
SetSpeed(-Speed);
}
public void SetDimensions(float width, float height)
{
Width = width;
Height = height;
// ✅ CORREGIDO: Usar ChangeBodyShape para limpiar la forma anterior
if (_simulation != null && _simulation.Bodies.BodyExists(BodyHandle))
{
var box = new Box(width, height, zAltura_Transporte);
var shapeIndex = _simulation.Shapes.Add(box);
ChangeBodyShape(shapeIndex);
}
// ✅ CRÍTICO: Actualizar propiedades cacheadas después del cambio de dimensiones
UpdateCachedProperties();
}
public void Create(float width, float height, Vector2 wpfTopLeft, float wpfAngle = 0)
{
// ✅ USAR COORDINATECONVERTER para conversión centralizada
var zPosition = zAltura_Transporte / 2f + zPos_Transporte;
var bepuCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, width, height, wpfAngle, zPosition);
Create(width, height, bepuCenter, wpfAngle);
}
/// <summary>
/// ✅ SIMPLIFICADO: RemoverBody ya no necesita limpiar restricciones de botellas.
/// </summary>
public new void RemoverBody()
{
base.RemoverBody();
}
public void Create(float width, float height, Vector3 bepuPosition, float wpfAngle = 0)
{
RemoverBody();
var box = new Box(width, height, zAltura_Transporte);
var shapeIndex = _simulation.Shapes.Add(box);
// ✅ USAR COORDINATECONVERTER para conversión centralizada
var bodyDescription = BodyDescription.CreateKinematic(
new RigidPose(bepuPosition, CoordinateConverter.CreateBepuQuaternionFromWpfAngle(wpfAngle)),
new CollidableDescription(shapeIndex, 0.1f),
new BodyActivityDescription(-1f)
);
BodyHandle = _simulation.Bodies.Add(bodyDescription);
_bodyCreated = true; // Marcar que hemos creado un cuerpo
// ✅ CRÍTICO: Actualizar propiedades cacheadas después de crear el body
UpdateCachedProperties();
}
}
}