CtrEditor/Simulacion/CoordinateConverter.cs

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>
    /// Clase centralizada para manejar todas las conversiones entre coordenadas WPF y BEPU
    /// WPF: Y hacia abajo, ángulos en sentido horario, Top-Left como referencia
    /// BEPU: Y hacia arriba, ángulos en sentido antihorario, Center como referencia
    /// </summary>
    public static class CoordinateConverter
    {
        /// <summary>
        /// Convierte ángulo de WPF a BEPU (invierte signo) - USO INTERNO
        /// </summary>
        private static float WpfAngleToBepuAngle(float wpfAngle)
        {
            return -wpfAngle;
        }

        /// <summary>
        /// Convierte ángulo de BEPU a WPF (invierte signo) - USO INTERNO
        /// </summary>
        private static float BepuAngleToWpfAngle(float bepuAngle)
        {
            return -bepuAngle;
        }

        /// <summary>
        /// Convierte posición Y de WPF a BEPU (invierte signo) - USO INTERNO
        /// </summary>
        internal static float WpfYToBepuY(float wpfY)
        {
            return -wpfY;
        }

        /// <summary>
        /// Convierte posición Y de BEPU a WPF (invierte signo) - RETORNA VALOR WPF
        /// </summary>
        public static float BepuYToWpfY(float bepuY)
        {
            return -bepuY;
        }

        /// <summary>
        /// Convierte Vector2 de WPF a BEPU (solo invierte Y) - USO INTERNO
        /// </summary>
        internal static Vector2 WpfToBepuVector2(Vector2 wpfVector)
        {
            return new Vector2(wpfVector.X, -wpfVector.Y);
        }

        /// <summary>
        /// Convierte Vector2 de BEPU a WPF (solo invierte Y) - RETORNA VALOR WPF
        /// </summary>
        public static Vector2 BepuToWpfVector2(Vector2 bepuVector)
        {
            return new Vector2(bepuVector.X, -bepuVector.Y);
        }

        /// <summary>
        /// Convierte Vector3 de BEPU a Vector2 WPF (proyección XY con Y invertida) - RETORNA VALOR WPF
        /// </summary>
        public static Vector2 BepuVector3ToWpfVector2(Vector3 bepuVector)
        {
            return new Vector2(bepuVector.X, -bepuVector.Y);
        }

        /// <summary>
        /// Calcula la posición del centro en coordenadas BEPU desde Top-Left WPF - USO INTERNO
        /// Maneja correctamente la rotación del objeto
        /// </summary>
        internal static Vector3 CalculateBepuCenterFromWpfTopLeft(Vector2 wpfTopLeft, float width, float height, float wpfAngle, float zPosition)
        {
            // Calcular el offset del centro desde Top-Left (sin rotación)
            var offsetX = width / 2f;
            var offsetY = height / 2f;

            // Convertir ángulo WPF a radianes para cálculos trigonométricos
            var angleRadians = simBase.GradosARadianes(wpfAngle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);

            // Rotar el offset alrededor del Top-Left usando el ángulo WPF
            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;

            // Calcular nueva posición del centro manteniendo Top-Left fijo
            var centerX = wpfTopLeft.X + rotatedOffsetX;
            var centerY = wpfTopLeft.Y + rotatedOffsetY;

            // Convertir a 3D con Y invertida para BEPU
            var bepuY = WpfYToBepuY(centerY);
            var result = new Vector3(centerX, bepuY, zPosition);

            return result;
        }

        /// <summary>
        /// Calcula la posición Top-Left WPF desde el centro BEPU - RETORNA VALOR WPF
        /// Maneja correctamente la rotación del objeto
        /// </summary>
        public static Vector2 CalculateWpfTopLeftFromBepuCenter(Vector3 bepuCenter, float width, float height, float wpfAngle)
        {
            // Convertir centro BEPU a WPF
            var wpfCenterX = bepuCenter.X;
            var wpfCenterY = BepuYToWpfY(bepuCenter.Y);

            // Calcular el offset del centro al Top-Left (sin rotación)
            var offsetX = -width / 2f;
            var offsetY = -height / 2f;

            // Convertir ángulo WPF a radianes para cálculos trigonométricos
            var angleRadians = simBase.GradosARadianes(wpfAngle);
            var cos = (float)Math.Cos(angleRadians);
            var sin = (float)Math.Sin(angleRadians);

            // Rotar el offset usando el ángulo WPF
            var rotatedOffsetX = offsetX * cos - offsetY * sin;
            var rotatedOffsetY = offsetX * sin + offsetY * cos;

            // Calcular Top-Left desde el centro
            var topLeftX = wpfCenterX + rotatedOffsetX;
            var topLeftY = wpfCenterY + rotatedOffsetY;

            return new Vector2(topLeftX, topLeftY);
        }

        /// <summary>
        /// Crea un Quaternion para BEPU desde un ángulo WPF - USO INTERNO
        /// </summary>
        internal static Quaternion CreateBepuQuaternionFromWpfAngle(float wpfAngle)
        {
            var bepuAngle = WpfAngleToBepuAngle(wpfAngle);
            return Quaternion.CreateFromAxisAngle(Vector3.UnitZ, simBase.GradosARadianes(bepuAngle));
        }

        /// <summary>
        /// Extrae el ángulo WPF desde un Quaternion BEPU - RETORNA VALOR WPF
        /// </summary>
        public static float ExtractWpfAngleFromBepuQuaternion(Quaternion bepuQuaternion)
        {
            // Extraer ángulo Z del quaternion
            var bepuAngleRadians = (float)Math.Atan2(
                2.0 * (bepuQuaternion.W * bepuQuaternion.Z + bepuQuaternion.X * bepuQuaternion.Y),
                1.0 - 2.0 * (bepuQuaternion.Y * bepuQuaternion.Y + bepuQuaternion.Z * bepuQuaternion.Z)
            );

            var bepuAngleDegrees = simBase.RadianesAGrados(bepuAngleRadians);
            return BepuAngleToWpfAngle(bepuAngleDegrees);
        }

        /// <summary>
        /// Actualiza la posición de un body BEPU manteniendo su rotación actual - USO INTERNO
        /// </summary>
        internal static void UpdateBepuBodyPosition(Simulation simulation, BodyHandle bodyHandle, Vector3 newBepuPosition)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                bodyReference.Pose.Position = newBepuPosition;
            }
        }

        /// <summary>
        /// Actualiza la rotación de un body BEPU manteniendo su posición actual - USO INTERNO
        /// </summary>
        internal static void UpdateBepuBodyRotation(Simulation simulation, BodyHandle bodyHandle, float wpfAngle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                bodyReference.Pose.Orientation = CreateBepuQuaternionFromWpfAngle(wpfAngle);
            }
        }

        /// <summary>
        /// Actualiza posición y rotación de un body BEPU simultáneamente - USO INTERNO
        /// </summary>
        internal static void UpdateBepuBodyPose(Simulation simulation, BodyHandle bodyHandle, Vector3 newBepuPosition, float wpfAngle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                bodyReference.Pose.Position = newBepuPosition;
                bodyReference.Pose.Orientation = CreateBepuQuaternionFromWpfAngle(wpfAngle);
            }
        }

        /// <summary>
        /// Obtiene la posición del centro en coordenadas BEPU - USO INTERNO
        /// </summary>
        internal static Vector3 GetBepuBodyPosition(Simulation simulation, BodyHandle bodyHandle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                return bodyReference.Pose.Position;
            }
            return Vector3.Zero;
        }

        /// <summary>
        /// Obtiene el ángulo WPF desde un body BEPU - RETORNA VALOR WPF
        /// </summary>
        public static float GetWpfAngleFromBepuBody(Simulation simulation, BodyHandle bodyHandle)
        {
            if (simulation != null && simulation.Bodies.BodyExists(bodyHandle))
            {
                var bodyReference = simulation.Bodies.GetBodyReference(bodyHandle);
                return ExtractWpfAngleFromBepuQuaternion(bodyReference.Pose.Orientation);
            }
            return 0f;
        }

        /// <summary>
        /// ✅ NUEVO: Convierte directamente de grados WPF a radianes BEPU - USO INTERNO
        /// Maneja tanto la inversión de signo como la conversión a radianes en una sola operación
        /// </summary>
        internal static float WpfDegreesToBepuRadians(float wpfDegrees)
        {
            return simBase.GradosARadianes(WpfAngleToBepuAngle(wpfDegrees));
        }

        /// <summary>
        /// ✅ NUEVO: Convierte directamente de radianes BEPU a grados WPF - RETORNA VALOR WPF
        /// Maneja tanto la conversión a grados como la inversión de signo en una sola operación
        /// </summary>
        public static float BepuRadiansToWpfDegrees(float bepuRadians)
        {
            return BepuAngleToWpfAngle(simBase.RadianesAGrados(bepuRadians));
        }
    }

}