Simulator/DroneClient/utils/Quaternion.cs

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

namespace DroneClient.utils
{
    internal class Quaternion
    {
        private float w;
        private float x;
        private float y;
        private float z;

        public float W
        {
            get => w;
            private set => w = value;
        }
        public float X
        {
            get => x;
            private set => x = value;
        }
        public float Y
        {
            get => y;
            private set => y = value;
        }
        public float Z
        {
            get => z;
            private set => z = value;
        }

        public Quaternion()
        {
            w = 1;
            x = y = z = 0;
        }

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

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

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

        public void Normalize()
        {
            float norm = getNorm();
            if (norm != 0)
            {
                w /= norm;
                x /= norm;
                y /= norm;
                z /= norm;
            }
        }

        static public Quaternion FromRotationVector(Vector Va)
        {
            float a = Va.getNorm();
            if (a == 0)
            {
                return new Quaternion();
            }

            Quaternion w = new Quaternion();
            float cos = (float)Math.Cos(a / 2);
            float sin = (float)Math.Sin(a / 2);

            w.w = cos;
            w.x = Va.X / a * sin;
            w.y = Va.Y / a * sin;
            w.z = Va.Z / a * sin;
            return w;
        }

        static public Quaternion FromEulerAngles(Vector euler)
        {
            float halfphi = euler.X / 2;
            float halftheta = euler.Y / 2;
            float halfpsi= euler.Z / 2;
            float cosx = (float)Math.Cos(halfphi);
            float sinx = (float)Math.Sin(halfphi);
            float cosy = (float)Math.Cos(halftheta);
            float siny = (float)Math.Sin(halftheta);
            float cosz = (float)Math.Cos(halfpsi);
            float sinz = (float)Math.Sin(halfpsi);

            return new Quaternion(
                cosx * cosy * cosz + sinx * siny * sinz,
                sinx * cosy * cosz - cosx * siny * sinz,
                cosx * siny * cosz + sinx * cosy * sinz,
                cosx * cosy * sinz - sinx * siny * cosz);
        }

        static public Quaternion Copy(Quaternion q)
        {
            return new Quaternion(q.w, q.x, q.y, q.z);
        }

        public Quaternion getInverse()
        {
            float n = getNorm();
            return new Quaternion(w / n, -x / n, -y / n, -z / n);
        }

        static public Quaternion Mul(Quaternion q1, Quaternion q2)
        {
            Quaternion res = new Quaternion();
            res.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
            res.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
            res.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
            res.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
            return res;
        }

        public void UpdateByRates(Vector w, float dt)
        {
            Quaternion omega = new Quaternion(0, w.X, w.Y, w.Z);
            Quaternion res = Mul(this, omega);
            this.w += res.w * 0.5f * dt;
            this.x += res.x * 0.5f * dt;
            this.y += res.y * 0.5f * dt;
            this.z += res.z * 0.5f * dt;
        }

        static public Vector RotateVector(Vector a, Quaternion q)
        {
            Quaternion fromVector = new Quaternion(0, a.X, a.Y, a.Z);
            Quaternion inverse = q.getInverse();
            Quaternion r = Mul(q, fromVector);
            r = Mul(r, inverse);
            return new Vector(r.x, r.y, r.z);
        }
        static public Quaternion QuaternionBetweenVectors(Vector a, Vector b)
        {
            Quaternion res = new Quaternion();
            Vector mul_v = Vector.Mul_Vector(a, b);
            //Если векторы коллинеарны, то нет поворота
            if (mul_v.IsZero()) return res;

            float mul_s = Vector.Mul_Scalar(a, b);
            //Хотя бы один из векторов нулевой => кватернион неопределен, но мы возвращаем (1, 0, 0, 0)
            if (mul_s == 0) return res;

            float norm_scalar = a.getNorm() * b.getNorm() + mul_s;
            float normQ = (float)Math.Sqrt(norm_scalar * norm_scalar + mul_v.getNorm() * mul_v.getNorm());

            res.w = norm_scalar / normQ;
            res.x = mul_v.X / normQ;
            res.y = mul_v.Y / normQ;
            res.z = mul_v.Z / normQ;
            return res;
        }

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