349 lines
16 KiB
C#
349 lines
16 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
|
|
{
|
|
/// <summary>
|
|
/// Representa una curva o arco en la simulación física.
|
|
/// ✅ IMPORTANTE: Los ángulos _startAngle y _endAngle se almacenan INTERNAMENTE en radianes BEPU
|
|
/// (ya convertidos desde grados WPF usando CoordinateConverter.WpfDegreesToBepuRadians)
|
|
/// Las propiedades públicas StartAngle/EndAngle devuelven grados WPF usando CoordinateConverter.BepuRadiansToWpfDegrees
|
|
/// ✅ NUEVO: Cada triángulo actúa como un mini-transporte con su propia dirección tangencial fija
|
|
/// </summary>
|
|
public class simCurve : simBase
|
|
{
|
|
private float _innerRadius;
|
|
private float _outerRadius;
|
|
private float _startAngle; // ✅ SIEMPRE en radianes BEPU
|
|
private float _endAngle; // ✅ SIEMPRE en radianes BEPU
|
|
public float Speed { get; set; } // Velocidad para efectos de cinta transportadora (m/s)
|
|
|
|
private List<Action> _deferredActions;
|
|
|
|
// ✅ NUEVO: Almacenar el centro real de la curva
|
|
private Vector3 _curveCenter;
|
|
|
|
// ✅ SIMPLIFICADO: Propiedades esenciales únicamente
|
|
public List<simBotella> BottlesOnCurve { get; private set; } = new List<simBotella>();
|
|
|
|
// ✅ NUEVO: Almacenar triángulos creados para acceso directo
|
|
private Triangle[] _storedTriangles;
|
|
|
|
public float InnerRadius => _innerRadius;
|
|
public float OuterRadius => _outerRadius;
|
|
public float StartAngle => CoordinateConverter.BepuRadiansToWpfDegrees(_startAngle); // Convertir de radianes BEPU internos a grados WPF
|
|
public float EndAngle => CoordinateConverter.BepuRadiansToWpfDegrees(_endAngle); // Convertir de radianes BEPU internos a grados WPF
|
|
|
|
// ✅ NUEVO: Propiedad para acceder al centro real de la curva
|
|
public Vector3 CurveCenter => _curveCenter;
|
|
|
|
public simCurve(Simulation simulation, List<Action> deferredActions, float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 topLeft, float unused = 0, SimulationManagerBEPU simulationManager = null)
|
|
{
|
|
_simulation = simulation;
|
|
_deferredActions = deferredActions;
|
|
_simulationManager = simulationManager; // ✅ NUEVO: Almacenar referencia
|
|
_innerRadius = innerRadius;
|
|
_outerRadius = outerRadius;
|
|
// ✅ SIMPLIFICADO: Usar conversión directa WPF grados → BEPU radianes
|
|
_startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
|
|
_endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);
|
|
|
|
// ✅ NUEVO: Calcular y almacenar el centro real de la curva
|
|
var curveSize = outerRadius * 2f;
|
|
var zPosition = zPos_Curve;
|
|
_curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(topLeft, curveSize, curveSize, 0f, zPosition);
|
|
|
|
// ✅ SIMPLIFICADO: Crear la curva directamente
|
|
Create(innerRadius, outerRadius, startAngle, endAngle, topLeft, 0);
|
|
}
|
|
|
|
// ✅ MODIFICADO: Actualizar la velocidad y luego la del cuerpo cinemático
|
|
public void SetSpeed(float speed)
|
|
{
|
|
Speed = speed;
|
|
ActualizarVelocidadCinematica();
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Aplica la velocidad angular 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);
|
|
|
|
float effectiveRadius = (_innerRadius + _outerRadius) / 2.0f;
|
|
if (effectiveRadius > 0.001f)
|
|
{
|
|
// La velocidad tangencial (Speed) se convierte a velocidad angular (ω = v / r)
|
|
// ✅ CORREGIDO: Convertir velocidad de m/min a m/s para la simulación
|
|
float angularSpeed = (Speed / SpeedConversionFactor) / effectiveRadius;
|
|
|
|
// La rotación es alrededor del eje Z en el sistema de coordenadas de BEPU
|
|
body.Velocity.Angular = new Vector3(0, 0, angularSpeed);
|
|
}
|
|
else
|
|
{
|
|
body.Velocity.Angular = Vector3.Zero;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Detiene completamente la curva
|
|
/// </summary>
|
|
public void StopCurve()
|
|
{
|
|
SetSpeed(0f);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Invierte la dirección de la curva manteniendo la misma velocidad
|
|
/// </summary>
|
|
public void ReverseCurve()
|
|
{
|
|
SetSpeed(-Speed);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Actualiza tanto posición como rotación desde parámetros WPF
|
|
/// </summary>
|
|
internal void UpdateFromWpfParameters(Vector2 wpfTopLeft, float wpfAngle, float innerRadius, float outerRadius, float startAngle, float endAngle)
|
|
{
|
|
// Actualizar parámetros de la curva
|
|
_innerRadius = innerRadius;
|
|
_outerRadius = outerRadius;
|
|
// ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
|
|
_startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
|
|
_endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);
|
|
|
|
// ✅ NUEVO: Actualizar el centro real de la curva
|
|
var curveSize = outerRadius * 2f;
|
|
var zPosition = zPos_Curve;
|
|
_curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
|
|
|
|
Create(_curveCenter);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Actualiza posición desde Top-Left WPF manteniendo parámetros actuales
|
|
/// </summary>
|
|
internal void SetPositionFromWpfTopLeft(Vector2 wpfTopLeft)
|
|
{
|
|
var curveSize = _outerRadius * 2f;
|
|
var zPosition = GetPosition().Z; // Mantener Z actual
|
|
_curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
|
|
|
|
Create(_curveCenter);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Obtiene Top-Left WPF desde la posición actual
|
|
/// </summary>
|
|
internal Vector2 GetWpfTopLeft()
|
|
{
|
|
var curveSize = _outerRadius * 2f;
|
|
return CoordinateConverter.CalculateWpfTopLeftFromBepuCenter(_curveCenter, curveSize, curveSize, 0f);
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ CORREGIDO: Obtiene los triángulos reales creados para la curva
|
|
/// Devuelve los triángulos en coordenadas locales para evitar transformación duplicada
|
|
/// </summary>
|
|
public Triangle[] GetRealBEPUTriangles()
|
|
{
|
|
try
|
|
{
|
|
if (_storedTriangles == null || _storedTriangles.Length == 0)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] No hay triángulos almacenados");
|
|
return new Triangle[0];
|
|
}
|
|
|
|
// ✅ CORREGIDO: Devolver triángulos en coordenadas locales
|
|
// La visualización 3D aplicará la transformación una sola vez
|
|
System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] Devolviendo {_storedTriangles.Length} triángulos en coordenadas locales");
|
|
return _storedTriangles;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[GetRealBEPUTriangles] Error: {ex.Message}");
|
|
return new Triangle[0];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Obtiene los triángulos transformados a coordenadas mundiales (solo para debugging)
|
|
/// </summary>
|
|
public Triangle[] GetWorldBEPUTriangles()
|
|
{
|
|
try
|
|
{
|
|
if (_storedTriangles == null || _storedTriangles.Length == 0)
|
|
{
|
|
return new Triangle[0];
|
|
}
|
|
|
|
var worldTriangles = TransformTrianglesToWorldCoordinates(_storedTriangles);
|
|
System.Diagnostics.Debug.WriteLine($"[GetWorldBEPUTriangles] Devolviendo {worldTriangles.Length} triángulos en coordenadas mundiales");
|
|
return worldTriangles;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[GetWorldBEPUTriangles] Error: {ex.Message}");
|
|
return new Triangle[0];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ NUEVO: Transforma triángulos de coordenadas locales a mundiales
|
|
/// </summary>
|
|
private Triangle[] TransformTrianglesToWorldCoordinates(Triangle[] localTriangles)
|
|
{
|
|
try
|
|
{
|
|
if (_simulation == null || !_simulation.Bodies.BodyExists(BodyHandle))
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Cuerpo no existe, devolviendo triángulos locales");
|
|
return localTriangles; // Fallback: devolver triángulos sin transformar
|
|
}
|
|
|
|
var body = _simulation.Bodies[BodyHandle];
|
|
var bodyPosition = body.Pose.Position;
|
|
var bodyOrientation = body.Pose.Orientation;
|
|
|
|
var transformedTriangles = new Triangle[localTriangles.Length];
|
|
|
|
for (int i = 0; i < localTriangles.Length; i++)
|
|
{
|
|
var localTriangle = localTriangles[i];
|
|
|
|
// Transformar cada vértice del triángulo a coordenadas mundiales
|
|
var worldA = bodyPosition + Vector3.Transform(localTriangle.A, bodyOrientation);
|
|
var worldB = bodyPosition + Vector3.Transform(localTriangle.B, bodyOrientation);
|
|
var worldC = bodyPosition + Vector3.Transform(localTriangle.C, bodyOrientation);
|
|
|
|
transformedTriangles[i] = new Triangle(worldA, worldB, worldC);
|
|
}
|
|
|
|
System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Transformados {transformedTriangles.Length} triángulos. Body pos: {bodyPosition}");
|
|
return transformedTriangles;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[TransformTriangles] Error: {ex.Message}, devolviendo triángulos locales");
|
|
return localTriangles; // Fallback en caso de error
|
|
}
|
|
}
|
|
|
|
public new void RemoverBody()
|
|
{
|
|
// ✅ NUEVO: Limpiar triángulos almacenados
|
|
_storedTriangles = null;
|
|
|
|
// ✅ SIMPLIFICADO: Solo remover el cuerpo principal (Mesh único)
|
|
base.RemoverBody();
|
|
}
|
|
|
|
public void Create(float innerRadius, float outerRadius, float startAngle, float endAngle, Vector2 wpfTopLeft, float unused = 0)
|
|
{
|
|
// ✅ CORREGIDO: Como Aether - startAngle y endAngle definen directamente el sector
|
|
// No hay rotación separada del objeto
|
|
|
|
// Actualizar parámetros internos
|
|
_innerRadius = innerRadius;
|
|
_outerRadius = outerRadius;
|
|
// ✅ CORREGIDO: Usar conversión directa WPF grados → BEPU radianes
|
|
_startAngle = CoordinateConverter.WpfDegreesToBepuRadians(startAngle);
|
|
_endAngle = CoordinateConverter.WpfDegreesToBepuRadians(endAngle);
|
|
|
|
// ✅ NUEVO: Actualizar el centro real de la curva
|
|
var curveSize = outerRadius * 2f;
|
|
var zPosition = zPos_Curve;
|
|
_curveCenter = CoordinateConverter.CalculateBepuCenterFromWpfTopLeft(wpfTopLeft, curveSize, curveSize, 0f, zPosition);
|
|
|
|
Create(_curveCenter); // Llamar a la sobrecarga privada
|
|
}
|
|
|
|
private void Create(Vector3 position)
|
|
{
|
|
RemoverBody();
|
|
|
|
var triangles = CreateSimpleArcTriangles(_innerRadius, _outerRadius, _startAngle, _endAngle);
|
|
if (triangles.Length == 0)
|
|
{
|
|
System.Diagnostics.Debug.WriteLine($"[simCurve] No se crearon triángulos, no se creará el cuerpo.");
|
|
return;
|
|
}
|
|
_storedTriangles = triangles;
|
|
|
|
// ✅ CORREGIDO: Convertir el array de triángulos a un Buffer<Triangle> de BEPU.
|
|
_simulation.BufferPool.Take(triangles.Length, out BepuUtilities.Memory.Buffer<Triangle> triangleBuffer);
|
|
for (int i = 0; i < triangles.Length; i++)
|
|
{
|
|
triangleBuffer[i] = triangles[i];
|
|
}
|
|
var mesh = new Mesh(triangleBuffer, Vector3.One, _simulation.BufferPool);
|
|
var shapeIndex = _simulation.Shapes.Add(mesh);
|
|
|
|
var bodyDescription = BodyDescription.CreateKinematic(
|
|
new RigidPose(position, Quaternion.Identity), // La rotación se maneja con la velocidad angular, no con la pose inicial
|
|
new CollidableDescription(shapeIndex, 0.1f),
|
|
new BodyActivityDescription(-1f) // ✅ CORREGIDO: -1f para que el cuerpo cinemático NUNCA duerma y no se mueva por su cuenta.
|
|
);
|
|
BodyHandle = _simulation.Bodies.Add(bodyDescription);
|
|
_bodyCreated = true;
|
|
|
|
// ✅ NUEVO: Establecer la velocidad cinemática inicial al crear el cuerpo.
|
|
ActualizarVelocidadCinematica();
|
|
}
|
|
|
|
/// <summary>
|
|
/// ✅ SIMPLIFICADO: Crea triángulos usando Triangle de BEPU directamente
|
|
/// Solo superficie superior, eliminando complejidad innecesaria
|
|
/// Los triángulos se crean en coordenadas locales (Z=0) y la posición del cuerpo los ubica correctamente
|
|
/// </summary>
|
|
/// <param name="innerRadius">Radio interno del arco</param>
|
|
/// <param name="outerRadius">Radio externo del arco</param>
|
|
/// <param name="startAngle">Ángulo inicial en radianes BEPU</param>
|
|
/// <param name="endAngle">Ángulo final en radianes BEPU</param>
|
|
/// <returns>Array de triángulos nativos de BEPU en coordenadas locales</returns>
|
|
private Triangle[] CreateSimpleArcTriangles(float innerRadius, float outerRadius, float startAngle, float endAngle)
|
|
{
|
|
var triangles = new List<Triangle>();
|
|
|
|
// ✅ SIMPLIFICADO: Menos segmentos, menos complejidad
|
|
float arcLength = Math.Abs(endAngle - startAngle) * ((innerRadius + outerRadius) / 2f);
|
|
int segments = Math.Max(8, Math.Min((int)(arcLength * 8), 32)); // Menos segmentos
|
|
float angleStep = (endAngle - startAngle) / segments;
|
|
|
|
// ✅ SIMPLIFICADO: Sin inversión compleja de ángulos
|
|
for (int i = 0; i < segments; i++)
|
|
{
|
|
float angle1 = startAngle + i * angleStep;
|
|
float angle2 = startAngle + (i + 1) * angleStep;
|
|
|
|
// ✅ COORDENADAS LOCALES: Triángulos en Z=0, el cuerpo los posiciona correctamente
|
|
var inner1 = new Vector3(innerRadius * (float)Math.Cos(angle1), innerRadius * (float)Math.Sin(angle1), 0);
|
|
var outer1 = new Vector3(outerRadius * (float)Math.Cos(angle1), outerRadius * (float)Math.Sin(angle1), 0);
|
|
var inner2 = new Vector3(innerRadius * (float)Math.Cos(angle2), innerRadius * (float)Math.Sin(angle2), 0);
|
|
var outer2 = new Vector3(outerRadius * (float)Math.Cos(angle2), outerRadius * (float)Math.Sin(angle2), 0);
|
|
|
|
// ✅ USAR Triangle NATIVO DE BEPU (solo superficie superior)
|
|
triangles.Add(new Triangle(inner1, outer1, outer2));
|
|
triangles.Add(new Triangle(inner1, outer2, inner2));
|
|
}
|
|
|
|
System.Diagnostics.Debug.WriteLine($"[CreateSimpleArcTriangles] Creados {triangles.Count} triángulos en coordenadas locales");
|
|
return triangles.ToArray();
|
|
}
|
|
}
|
|
}
|