SimulacionPataJansen/JansenLegViewer/Rod.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Numerics;
using System.Drawing;

namespace JansenLegSimulator
{
    public class Rod
    {
        #region Constructores
        // Constructor de barra libre
        public Rod() { }

        // Constructor de barra con un extremo fijado a un punto
        public Rod(Vector2 begining)
        {
            _startPosition = begining;
        }
        #endregion
        #region Conexiones
        /// <summary>
        /// Barra a la que se une por el extremo inicial
        /// </summary>
        private Rod JointBegining { get; set; } = null;
        /// <summary>
        /// Barras a las cuales se une por el extremo final
        /// </summary>
        private List<Rod> JointsEnding { get; set; } = new();
        /// <summary>
        /// Conecta el inicio de una barra a otra barra
        /// </summary>
        /// <param name="linkage">La barra a la cual se unirá esta</param>
        public void ConnectBegin(ref Rod linkage)
        {
            if (linkage == null) throw new NullReferenceException("The rod cannot be null");
            if (JointBegining != null) throw new Exception("The beginning is already assigned");
            JointBegining = linkage;
        }
        /// <summary>
        /// Conecta el final de una barra a otra barra
        /// </summary>
        /// <param name="linkage">La barra a la cual se unirá esta</param>
        public void ConnectEnd(ref Rod linkage)
        {
            if (linkage == null) throw new NullReferenceException("The rod cannot be null");
            if (!JointsEnding.Contains(linkage))
                JointsEnding.Add(linkage);
        }
        /// <summary>
        /// Desconecta (si está conectada) la barra especificada
        /// </summary>
        /// <param name="linkage">Barra a ser desconectada</param>
        public void Disconnect(ref Rod linkage)
        {
            if (JointBegining == linkage) JointBegining = null;
            if (JointsEnding.Contains(linkage)) JointsEnding.Remove(linkage);
        }
        #endregion

        #region Propiedades de la barra
        public string RodName { get; set; } = "Unnamed";
        /// <summary>
        /// Longitud de la barra
        /// </summary>
        public float Length { get; set; } = 20;

        private float? _alpha = null;
        /// <summary>
        /// Ángulo de rotación de la barra. Si no se establece será 0 radianes.
        /// </summary>
        public float Alpha
        {
            get => (_alpha != null) ? (float)_alpha : 0;
            set => _alpha = value;
        }

        private Vector2? _startPosition = null;
        /// <summary>
        /// Posición de incio (eje de esta barra).
        /// Si se ha unido el inicio a otra barra, se calculará a partir de la posición final de la barra sobre la cual está 
        /// </summary>
        public virtual Vector2 StartPosition
        {
            get => (_startPosition != null) ? (Vector2)_startPosition :
                   (JointBegining != null) ? JointBegining.EndPosition : new Vector2(0, 0);
            set => _startPosition = value;
        }
        /// <summary>
        /// Posición final.
        /// Se calcula a partir de la posición inicial, el ángulo y la longitud.
        /// </summary>
        public virtual Vector2 EndPosition
        {
            get => new Vector2(
                StartPosition.X + Length * MathF.Cos(Alpha),
                StartPosition.Y + Length * MathF.Sin(Alpha)
                );
        }
        /// <summary>
        /// Posición central.
        /// Se calcula a partir de la posición inicial, el ángulo y la longitud.
        /// </summary>
        public virtual Vector2 Center
        {
            get => new Vector2(
                StartPosition.X + Length * MathF.Cos(Alpha) / 2,
                StartPosition.Y + Length * MathF.Sin(Alpha) / 2
                );
        }
        #endregion

        /// <summary>
        /// Calcula el punto de intersección entre dos barras de longitud y eje de rotación determinados.
        /// </summary>
        /// <param name="A">Primera barra</param>
        /// <param name="B">Segunda barra</param>
        /// <param name="solution">En caso de que existan dos intersecciones, número de la intersección a elegir</param>
        /// <returns>El vector que representa el punto en el que se unen las dos barras.</returns>
        /// <exception cref="RodsDoNotIntersectException">Si las barras no intersecan en ningún punto, lanzará una excepción.</exception>
        /// <exception cref="IntersectionOutOfRangeException">Si las barras intersecan pero no se encuentra la intersección especificada, lanzará una excepción.</exception>
        public static Vector2 GetIntersection(Rod A, Rod B, int solution = -1)
        {
            Vector2[] intersects = CosasQueNoSeExplicanEnMatesDePrimero.CalculateCircleIntersections(
                    A.StartPosition,
                    B.StartPosition,
                    A.Length,
                    B.Length);

            return (intersects.Length == 0) ? throw new RodsDoNotIntersectException("No existe conexión posible entre estas dos barras. Pruebe a darles la vuelta.") :
                (intersects.Length == 1 || solution < 0) ? intersects[0] :
                (intersects.Length > solution) ? intersects[solution] :
                throw new IntersectionOutOfRangeException("La intersección especificada no existe. Pruebe con las soluciones 0 y 1");
        }

        /// <summary>
        /// Calcula la diferencia entre dos vectores
        /// </summary>
        /// <param name="A">Vector final</param>
        /// <param name="B">Vector inicial</param>
        /// <returns>Vector final - Vector inicial</returns>
        public static Vector2 Delta(Vector2 A, Vector2 B) => A - B;


        /// <summary>
        /// Calcula para cada elemento el ángulo de la intersección de dos vectores a su posición respecto el 0 en el primer cuadrante.
        /// </summary>
        /// <param name="A">Vector 1</param>
        /// <param name="B">Vector 2</param>
        /// <param name="solution">Devuelve los angulos de cada eje a la intersección</param>
        /// <returns></returns>
        public static (float, float) GetIntersectionAlphas(Rod A, Rod B, int solution = -1)
        {
            (float, float) result = new();

            // Calcular las intersecciones y los vectores de
            // incremento entre el eje de la barra y su unión.
            Vector2
                I = GetIntersection(A, B, solution),
                DeltaA = Delta(I, A.StartPosition),
                DeltaB = Delta(I, B.StartPosition);

            // Obtener los ángulos mediante la inversa del coseno
            result.Item1 = MathF.Acos(DeltaA.X / DeltaA.Length());
            result.Item2 = MathF.Acos(DeltaB.X / DeltaB.Length());
            // Voltear verticalmente según la dirección vertical
            // del vector incremento
            if (DeltaA.Y < 0) result.Item1 *= -1.0f;
            if (DeltaB.Y < 0) result.Item2 *= -1.0f;

            return result;
        }


        [Serializable]
        public class RodsDoNotIntersectException : Exception
        {
            public RodsDoNotIntersectException() { }
            public RodsDoNotIntersectException(string message) : base(message) { }
            public RodsDoNotIntersectException(string message, Exception inner) : base(message, inner) { }
            protected RodsDoNotIntersectException(
              System.Runtime.Serialization.SerializationInfo info,
              System.Runtime.Serialization.StreamingContext context) : base(info, context) { }
        }

        [Serializable]
        public class IntersectionOutOfRangeException : Exception
        {
            public IntersectionOutOfRangeException() { }
            public IntersectionOutOfRangeException(string message) : base(message) { }
            public IntersectionOutOfRangeException(string message, Exception inner) : base(message, inner) { }
            protected IntersectionOutOfRangeException(
              System.Runtime.Serialization.SerializationInfo info,
              System.Runtime.Serialization.StreamingContext context) : base(info, context) { }
        }
    }
}