Simulator/DroneClient/utils/Vector.cs

using System;
using System.Collections.Generic;
using System.Data;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

namespace DroneClient.utils
{
    internal class Vector
    {
        public const float TO_GRAD = (float)(180 / Math.PI);
        public const float TO_RAD = (float)(Math.PI / 180);
        private float x, y, z;

        public float X
        {
            get => x;
            set => x = value;
        }

        public float Y
        {
            get => y;
            set => y = value;
        }

        public float Z
        {
            get => z;
            set => z = value;
        }

        public Vector() { }

        public Vector(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public bool IsZero()
        {
            return x == 0 && y == 0 && z == 0;
        }

        public float getNorm()
        {
            if (IsZero()) return 0;
            return (float)Math.Sqrt(x * x + y * y + z * z);
        }

        public void Normalize()
        {
            if (IsZero()) return;
            float norm = getNorm();
            x /= norm;
            y /= norm;
            z /= norm;
        }

        static public Vector operator *(float a, Vector v)
        {
            return new Vector(a * v.x, a * v.y, a * v.z);
        }

        static public Vector operator *(Vector v, float a)
        {
            return new Vector(a * v.x, a * v.y, a * v.z);
        }

        static public Vector operator -(Vector a, Vector b)
        {
            return new Vector(a.x - b.x, a.y - b.y, a.z - b.z);
        }

        static public float Mul_Scalar(Vector a, Vector b)
        {
            return a.x * b.x + a.y * b.y + a.z * b.z;
        }

        static public Vector Mul_Vector(Vector a, Vector b)
        {
            return new Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
        }

        static public float AngleBetweenVectors(Vector a, Vector b)
        {
            float mul_scalar = Mul_Scalar(a, b);
            float normA = a.getNorm();
            float normB = b.getNorm();

            if ((normA == 0) || (normB == 0))
            {
                return 0;
            }
            else
            {
                return (float)Math.Acos(Vector.constraint(mul_scalar / normA / normB));
            }
        }

        static public float AngleBetweenVectors2(Vector a, Vector b)
        {
            float normAB = Mul_Vector(a, b).getNorm();
            float normA = a.getNorm();
            float normB = b.getNorm();

            if ((normA == 0) || (normB == 0))
            {
                return 0;
            }
            else
            {
                return (float)Math.Asin(Vector.constraint(normAB / normA / normB));
            }
        }

        static public Vector getRotationVector(Vector a, Vector b)
        {
            Vector n = Vector.Mul_Vector(a, b);
            if (n.IsZero())
            {
                return Vector.Mul_Vector(a, new Vector(1, 0, 0));
            }
            n.Normalize();
            return Vector.AngleBetweenVectors(a, b) * n;
        }

        ///<summary>
        ///Ограничивает value в пределах между low и high. Если low >= high будет возвращено value.
        ///</summary>
        static public double constraint(double value, double low = -1, double high = 1)
        {
            if (low >= high) return value;
            if (value < low) return low;
            if (value > high) return high;
            return value;
        }

        override public string ToString()
        {
            return $"({x}, {y}, {z})";
        }
    }
}