241 lines
9.5 KiB
C#
241 lines
9.5 KiB
C#
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));
|
|
}
|
|
}
|
|
|
|
}
|