smart-bow-infrared/Assets/BowArrow/Scripts/Bluetooth/o0Project.cs

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

namespace o0Project
{
    public class Variance
    {
        List<float> _Value = new List<float>();
        int RecordCount;
        public Variance(int RecordCount = 100)
        {
            this.RecordCount = RecordCount;
        }
        public void Update(float value)
        {
            if (_Value.Count >= RecordCount)
                _Value.RemoveAt(0);
            _Value.Add(value);
        }
        public float Value
        {
            get
            {
                float average = 0;
                foreach (var i in _Value)
                    average += i;
                average /= _Value.Count;
                float variance = 0;
                foreach (var i in _Value)
                    variance += Mathf.Pow(average - i, 2);
                variance /= _Value.Count;
                return variance;
            }
        }
        public float StandardDeviation
        {
            get
            {
                return Mathf.Sqrt(Value);
            }
        }

    }
    public class Vector3f
    {
        public float x;
        public float y;
        public float z;
        public Vector3f() { }
        public Vector3f(Vector3f v)
        {
            x = v[0];
            y = v[1];
            z = v[2];
        }
        public Vector3f(float[] v)
        {
            x = v[0];
            y = v[1];
            z = v[2];
        }
        public Vector3f(float x = 0, float y = 0, float z = 0)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }
        public Vector3f assign(Vector3f v)
        {
            x = v.x;
            y = v.y;
            z = v.z;
            return this;
        }
        public Vector3f assign(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            return this;
        }
        public float length()//length of vector
        {
            return Mathf.Sqrt(x * x + y * y + z * z);
        }
        public Vector3f setNormalize()
        {
            return assign(this / length());
        }
        public Vector3f setVertical(Vector3f b)
        {
            return assign(this[1] * b[2] - this[2] * b[1],
                this[2] * b[0] - this[0] * b[2],
                this[0] * b[1] - this[1] * b[0]);
        }
        public float angle(Vector3f b)
        {
            return Mathf.Asin(Mathf.Sqrt(Mathf.Pow(this[0] - b[0], 2) + Mathf.Pow(this[1] - b[1], 2) + Mathf.Pow(this[2] - b[2], 2)) / 2) * 2;
        }
        public Vector3f setRotate(Vector3f shaft, float angle)
        {
            float cosAngle = Mathf.Cos(angle);
            float sinAngle = Mathf.Sin(angle);
            float rotateDate = (shaft[0] * this[0] + shaft[1] * this[1] + shaft[2] * this[2]) * (1 - cosAngle);
            Vector3f outerProduct = (new Vector3f(this)).setVertical(shaft);
            return assign(this[0] * cosAngle + outerProduct[0] * sinAngle + shaft[0] * rotateDate,
                this[1] * cosAngle + outerProduct[1] * sinAngle + shaft[1] * rotateDate,
                this[2] * cosAngle + outerProduct[2] * sinAngle + shaft[2] * rotateDate);
        }

        public float this[int index]
        {
            get
            {
                switch (index)
                {
                    default:
                    case 0:
                        return x;
                    case 1:
                        return y;
                    case 2:
                        return z;
                }
            }
            set
            {
                switch (index)
                {
                    default:
                    case 0:
                        x = value;
                        break;
                    case 1:
                        y = value;
                        break;
                    case 2:
                        z = value;
                        break;
                }
            }
        }
        public static Vector3f operator +(Vector3f a, Vector3f b)
        {
            return new Vector3f(a.x + b.x, a.y + b.y, a.z + b.z);
        }
        public static Vector3f operator -(Vector3f a, Vector3f b)
        {
            return new Vector3f(a.x - b.x, a.y - b.y, a.z - b.z);
        }
        public static Vector3f operator *(Vector3f a, float b)
        {
            return new Vector3f(a.x * b, a.y * b, a.z * b);
        }
        public static Vector3f operator /(Vector3f a, float b)
        {
            return new Vector3f(a.x / b, a.y / b, a.z / b);
        }
    }

    static public class o0
    {
        public static Quaternion FormQuaternion(Vector3 Direction1InIdentity, Vector3 Direction2InIdentity, Vector3 Direction1InQuaternion, Vector3 Direction2InQuaternion,float rate)
        {
            var quaIdentity = Quaternion.LookRotation(Direction1InIdentity, Direction2InIdentity);
            var quaReal = Quaternion.LookRotation(Direction1InQuaternion, Direction2InQuaternion);
            var quaIdentity2 = Quaternion.LookRotation(Direction2InIdentity, Direction1InIdentity);
            var quaReal2 = Quaternion.LookRotation(Direction2InQuaternion, Direction1InQuaternion);
            return Quaternion.Slerp(quaIdentity * Quaternion.Inverse(quaReal), quaIdentity2 * Quaternion.Inverse(quaReal2), rate);
        }
        public static Quaternion FormQuaternion(Quaternion original, Vector3 DirectionIdentity, Vector3 Direction1New, float rate)
        {
            var global = original * Direction1New;
            Vector3 DirectionIdentityNormalized = DirectionIdentity.normalized;
            float angle = Vector3.Angle(global, DirectionIdentityNormalized);
            //Vector3 gVector = Vector3.RotateTowards(acceleratorGlobal, downVector, angle / 180 * Mathf.PI * Mathf.Pow(overallGVectorCredibility, 1f) * rotationUnCredibility, 1);//偏移量,从陀螺仪数据往加速计数据的偏移量
            Vector3 gVector = Vector3.RotateTowards(global, DirectionIdentityNormalized, angle / 180 * Mathf.PI * rate, 1);//偏移量,从陀螺仪数据往加速计数据的偏移量

            var newQua = new Quaternion();
            if (float.IsNaN(gVector.x))
                return original;
            newQua.SetFromToRotation(original * Direction1New, gVector);
            /*
            Debug.Log("startIn");
            Debug.Log(original * Direction1New);
            Debug.Log(original);
            Debug.Log(Direction1New);
            Debug.Log(gVector);
            Debug.Log(newQua);
            Debug.Log("end");/**/
            if (float.IsNaN(newQua.w))
                return original;
            /*
            try
            {
                newQua.SetFromToRotation(original * Direction1New, gVector);
                if(float.IsNaN(newQua.w))
                    return original;
            }
            catch (Exception)
            {
                return original;
            }/**/
            return newQua * original;//一定要反过来乘
        }
    }
    static public class Extension
    {
        public static Quaternion Inverse(this Quaternion qua)
        {
            return new Quaternion(-qua.x,-qua.y,-qua.z,qua.w);
        }
        public static Quaternion ToQuaternion(this MatrixF2D m)
        {
            if(m.Width != 4 || m.Height!=4)
                throw new Exception("o0 FMatrix m is not 4*4 transfer matrix");
            Quaternion q = new Quaternion();
            q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2;
            q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2;
            q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2;
            q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2;
            q.x *= Mathf.Sign(q.x * (m[2, 1] - m[1, 2]));
            q.y *= Mathf.Sign(q.y * (m[0, 2] - m[2, 0]));
            q.z *= Mathf.Sign(q.z * (m[1, 0] - m[0, 1]));
            return q;
        }
        public static MatrixF2D ToMatrix(Vector3 right, Vector3 up, Vector3 forward, Vector3 position)
        {
            var m = MatrixF2D.Identity(4);
            m.SetColumn(0, new float[] { right.x,right.y,right.z,0});
            m.SetColumn(1, new float[] { up.x, up.y, up.z, 0 });
            m.SetColumn(2, new float[] { forward.x, forward.y, forward.z, 0 });
            m.SetColumn(3, new float[] { position.x, position.y, position.z, 0 });
            m[3, 3] = 1;
            return m;
        }
        public static MatrixF2D ToMatrix(Quaternion q)
        {
            return ToMatrix(q * Vector3.right, q * Vector3.up, q * Vector3.forward, Vector3.zero);
        }
    }

    public class MatrixF2D
    {
        private int _Width;
        public int Width { get { return _Width; } }
        public int Height { get { return _Element.Length / Width; } }
        private float[] _Element;
        public float[] Element { get { return _Element; } }
        public float this[int y, int x] { 
            get {
                x %= Width;
                if (x < 0) x += Width;
                y %= Height;
                if (y < 0) y += Height;
                return _Element[x + y * Width];
            }
            set{
                x %= Width;
                if (x < 0) x += Width;
                y %= Height;
                if (y < 0) y += Height;
                _Element[x + y * Width] = value;
            }
        }
        public float[] GetRow(int y)
        {
            y %= Height;
            if (y < 0) y += Height;
            float[] l = new float[Width];
            for (var i = 0; i < Width; ++i)
                l[i] = this[y, i];
            return l;
        }
        public void SetRow(int y, IEnumerable<float> value)
        {
            if (value.Count() != Width)
                throw new Exception("o0 FMatrix width different");
            for (var i = 0; i < Width; ++i)
                this[y, i] = value.ElementAt(i);
        }
        public float[] GetColumn(int x)
        {
            x %= Width;
            if (x < 0) x += Width;
            float[] l = new float[Height];
            for (var i = 0; i < Height; ++i)
                l[i] = this[i, x];
            return l;
        }
        public void SetColumn(int x, IEnumerable<float> value)
        {
            if (value.Count() != Height)
                throw new Exception("o0 FMatrix width different");
            for (var i = 0; i < Height; ++i)
                this[i, x] = value.ElementAt(i);
        }

        public MatrixF2D(int width = 2, int height = 2)
        {
            _Width = width;
            _Element = new float[width * height];
        }
        public MatrixF2D(int width, float[] element)
        {
            _Width = width;
            _Element = new float[element.Length];
            for (var i = 0; i < element.Length; ++i)
                _Element[i] = element[i];
        }
        public MatrixF2D(MatrixF2D b)
        {
            _Width = b.Width;
            _Element = new float[b.Element.Length];
            for (var i = 0; i < _Element.Length; ++i)
                _Element[i] = b.Element[i];
        }
        public static MatrixF2D operator +(MatrixF2D a, MatrixF2D b)
        {
            if (a.Width != b.Width || a.Height != b.Height)
                throw new Exception("o0 FMatrix size different");

            var c = new MatrixF2D(a.Width, a.Height);
            for (var i = 0; i < a.Element.Length; ++i)
                c.Element[i] = a.Element[i] + b.Element[i];
            return c;
        }
        public static MatrixF2D operator -(MatrixF2D a, MatrixF2D b)
        {
            if (a.Width != b.Width || a.Height != b.Height)
                throw new Exception("o0 FMatrix size different");

            var c = new MatrixF2D(a.Width, a.Height);
            for (var i = 0; i < a.Element.Length; ++i)
                c.Element[i] = a.Element[i] - b.Element[i];
            return c;
        }
        public static MatrixF2D operator *(MatrixF2D a, MatrixF2D b)
        {
            if (a.Width != b.Height)
                throw new Exception("o0 FMatrix 当矩阵A的列数（column）等于矩阵B的行数（row）时，A与B可以相乘");

            var c = new MatrixF2D(b.Width,a.Height);
            for (var x = 0;x<c.Width;++x)
                for (var y = 0; y < c.Height; ++y)
                    for (var z = 0; z < a.Width; ++z)
                    {
                        //c[x, y] += a[y, z] * b[z, x];
                        c[y, x] += a[y,z] * b[z,x];
                    }
            return c;
        }
        public static MatrixF2D operator *(MatrixF2D a, float b)
        {
            var c = new MatrixF2D(a);
            for (var x = 0; x < c.Width; ++x)
                for (var y = 0; y < c.Height; ++y)
                    c[y,x] *= b;
            return c;
        }
        public static MatrixF2D operator *(float b, MatrixF2D a)
        {
            return a*b;
        }
        public static MatrixF2D operator /(MatrixF2D a, float b)
        {
            return a*(1/b);
        }
        public MatrixF2D T
        {
            get
            {
                var matrix = new MatrixF2D(Height, Width);
                for (var x = 0; x < Width; ++x)
                    for (var y = 0; y < Height; ++y)
                        matrix[x,y] = this[y,x];
                return matrix;
            }
        }
        public float trace//tr
        {
            get
            {
                if (Width != Height)
                    throw new Exception("o0 FMatrix 非实对称矩阵");
                float x=0;
                for (var i = 0; i < Width; ++i)
                    x += this[i, i];
                return x;
            }
        }
        public float det//行列式
        {
            get
            {
                if (Width != Height)
                    throw new Exception("o0 FMatrix 非实对称矩阵");
                float d = 0;
                for (var x = 0; x < Width; ++x)
                {
                    float m = 1;
                    for (var y = 0; y < Width; ++y)
                        m *= this[y,x + y];
                    d += m;
                }
                for (var x = 0; x < Width; ++x)
                {
                    float m = 1;
                    for (var y = 0; y < Width; ++y)
                        m *= this[y,x - y];
                    d -= m;
                }
                return d;
            }
        }
        public MatrixF2D Inverse
        {
            get
            {
                if (Width != Height)//还需要额外判断条件
                    throw new Exception("o0 FMatrix 非实对称矩阵");

                var n = Width;
                int i;
                int j;
                int k;
                var Metrix = new MatrixF2D(this);
                for (k = 0; k < n; k++) 
                { 
                    for (i = 0; i < n; i++)
                    {
                        if (i != k) Metrix.Element[i * n + k] = -Metrix.Element[i * n + k] / Metrix.Element[k * n + k];
                    }
                    Metrix.Element[k * n + k] = 1 / Metrix.Element[k * n + k];
                    for (i = 0; i < n; i++) {
                        if (i != k) { 
                            for (j = 0; j < n; j++) {
                                if (j != k) Metrix.Element[i * n + j] += Metrix.Element[k * n + j] * Metrix.Element[i * n + k];
                            } 
                        } 
                    }
                    for (j = 0; j < n; j++) { if (j != k) Metrix.Element[k * n + j] *= Metrix.Element[k * n + k]; }
                }
                return Metrix;
            }
        }
        public override string ToString()
        {
            var s = "";
            for (var y = 0; y < Height; ++y)
            {
                if (y == 0)
                    s += "[";
                else
                    s += ",\n";
                for (var x = 0; x < Width; ++x)
                {
                    if (x == 0)
                        s += "[";
                    else
                        s += ",\t";
                    s += this[y,x].ToString("0.00000");
                }
                s += "]";
            }
            s += "]";
            return s;
        }
        public static MatrixF2D Identity(int size)
        {
            var m = new MatrixF2D(4,4);
            for (var i = 0; i < m.Width; ++i)
                m[i, i] = 1;
            return m;
        }
    }
    /*
    public class MatrixD2D
    {
        private int _Width;
        public int Width { get { return _Width; } }
        public int Height { get { return _Element.Length / Width; } }
        private double[] _Element;
        public double[] Element { get { return _Element; } }
        public double this[int y, int x]
        {
            get
            {
                x %= Width;
                if (x < 0) x += Width;
                y %= Height;
                if (y < 0) y += Height;
                return _Element[x + y * Width];
            }
            set
            {
                x %= Width;
                if (x < 0) x += Width;
                y %= Height;
                if (y < 0) y += Height;
                _Element[x + y * Width] = value;
            }
        }
        public double[] GetRow(int y)
        {
            y %= Height;
            if (y < 0) y += Height;
            double[] l = new double[Width];
            for (var i = 0; i < Width; ++i)
                l[i] = this[y, i];
            return l;
        }
        public void SetRow(int y, IEnumerable<float> value)
        {
            if (value.Count() != Width)
                throw new Exception("o0 FMatrix width different");
            for (var i = 0; i < Width; ++i)
                this[y, i] = value.ElementAt(i);
        }
        public double[] GetColumn(int x)
        {
            x %= Width;
            if (x < 0) x += Width;
            double[] l = new double[Height];
            for (var i = 0; i < Height; ++i)
                l[i] = this[i, x];
            return l;
        }
        public void SetColumn(int x, IEnumerable<float> value)
        {
            if (value.Count() != Height)
                throw new Exception("o0 FMatrix width different");
            for (var i = 0; i < Height; ++i)
                this[i, x] = value.ElementAt(i);
        }

        public MatrixD2D(int width = 2, int height = 2)
        {
            _Width = width;
            _Element = new double[width * height];
        }
        public MatrixD2D(int width, double[] element)
        {
            _Width = width;
            _Element = new double[element.Length];
            for (var i = 0; i < element.Length; ++i)
                _Element[i] = element[i];
        }
        public MatrixD2D(MatrixD2D b)
        {
            _Width = b.Width;
            _Element = new double[b.Element.Length];
            for (var i = 0; i < _Element.Length; ++i)
                _Element[i] = b.Element[i];
        }
        public static MatrixD2D operator +(MatrixD2D a, MatrixD2D b)
        {
            if (a.Width != b.Width || a.Height != b.Height)
                throw new Exception("o0 FMatrix size different");

            var c = new MatrixD2D(a.Width, a.Height);
            for (var i = 0; i < a.Element.Length; ++i)
                c.Element[i] = a.Element[i] + b.Element[i];
            return c;
        }
        public static MatrixD2D operator -(MatrixD2D a, MatrixD2D b)
        {
            if (a.Width != b.Width || a.Height != b.Height)
                throw new Exception("o0 FMatrix size different");

            var c = new MatrixD2D(a.Width, a.Height);
            for (var i = 0; i < a.Element.Length; ++i)
                c.Element[i] = a.Element[i] - b.Element[i];
            return c;
        }
        public static MatrixD2D operator *(MatrixD2D a, MatrixD2D b)
        {
            if (a.Width != b.Height)
                throw new Exception("o0 FMatrix 当矩阵A的列数（column）等于矩阵B的行数（row）时，A与B可以相乘");

            var c = new MatrixD2D(b.Width, a.Height);
            for (var x = 0; x < c.Width; ++x)
                for (var y = 0; y < c.Height; ++y)
                    for (var z = 0; z < a.Width; ++z)
                    {
                        //c[x, y] += a[y, z] * b[z, x];
                        c[y, x] += a[y, z] * b[z, x];
                    }
            return c;
        }
        public static MatrixD2D operator *(MatrixD2D a, float b)
        {
            var c = new MatrixD2D(a);
            for (var x = 0; x < c.Width; ++x)
                for (var y = 0; y < c.Height; ++y)
                    c[y, x] *= b;
            return c;
        }
        public static MatrixD2D operator *(float b, MatrixD2D a)
        {
            return a * b;
        }
        public static MatrixD2D operator /(MatrixD2D a, float b)
        {
            return a * (1 / b);
        }
        public MatrixD2D T
        {
            get
            {
                var matrix = new MatrixD2D(Height, Width);
                for (var x = 0; x < Width; ++x)
                    for (var y = 0; y < Height; ++y)
                        matrix[x, y] = this[y, x];
                return matrix;
            }
        }
        public double trace//tr
        {
            get
            {
                if (Width != Height)
                    throw new Exception("o0 FMatrix 非实对称矩阵");
                double x = 0;
                for (var i = 0; i < Width; ++i)
                    x += this[i, i];
                return x;
            }
        }
        public double det//行列式
        {
            get
            {
                if (Width != Height)
                    throw new Exception("o0 FMatrix 非实对称矩阵");
                double d = 0;
                for (var x = 0; x < Width; ++x)
                {
                    double m = 1;
                    for (var y = 0; y < Width; ++y)
                        m *= this[y, x + y];
                    d += m;
                }
                for (var x = 0; x < Width; ++x)
                {
                    double m = 1;
                    for (var y = 0; y < Width; ++y)
                        m *= this[y, x - y];
                    d -= m;
                }
                return d;
            }
        }
        public MatrixD2D Inverse
        {
            get
            {
                if (Width != Height)//还需要额外判断条件
                    throw new Exception("o0 FMatrix 非实对称矩阵");

                var n = Width;
                int i;
                int j;
                int k;
                var Metrix = new MatrixD2D(this);
                for (k = 0; k < n; k++)
                {
                    for (i = 0; i < n; i++)
                    {
                        if (i != k) Metrix.Element[i * n + k] = -Metrix.Element[i * n + k] / Metrix.Element[k * n + k];
                    }
                    Metrix.Element[k * n + k] = 1 / Metrix.Element[k * n + k];
                    for (i = 0; i < n; i++)
                    {
                        if (i != k)
                        {
                            for (j = 0; j < n; j++)
                            {
                                if (j != k) Metrix.Element[i * n + j] += Metrix.Element[k * n + j] * Metrix.Element[i * n + k];
                            }
                        }
                    }
                    for (j = 0; j < n; j++) { if (j != k) Metrix.Element[k * n + j] *= Metrix.Element[k * n + k]; }
                }
                return Metrix;
            }
        }
        public override string ToString()
        {
            var s = "";
            for (var y = 0; y < Height; ++y)
            {
                if (y == 0)
                    s += "[";
                else
                    s += ",\n";
                for (var x = 0; x < Width; ++x)
                {
                    if (x == 0)
                        s += "[";
                    else
                        s += ",\t";
                    s += this[y, x].ToString("0.00000");
                }
                s += "]";
            }
            s += "]";
            return s;
        }
        public static MatrixD2D Identity(int size)
        {
            var m = new MatrixD2D(4, 4);
            for (var i = 0; i < m.Width; ++i)
                m[i, i] = 1;
            return m;
        }
    }/**/
}