yichael_smartbow/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
{
    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();
            newQua.SetFromToRotation(original * Direction1New, gVector);
            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;
        }
    }
}