替换新的地磁校准算法

Assets/BowArrow/Scripts/Bluetooth/BluetoothAim.cs

@@ -4,6 +4,7 @@ using UnityEngine;
 using System.Collections.Generic;
 using System.Linq;
 using UnityEngine.UI;
+using Newtonsoft.Json;
 
 public class BluetoothAim : MonoBehaviour
 {
@@ -184,9 +185,11 @@ class AimHandler
         return (float) shortNum; 
     }
 
-    o0MagneticCalibraterSimple MagCalibrater;
+    // o0MagneticCalibraterSimple MagCalibrater;
+    o0MagneticCalibraterEllipsoidFitting MagCalibrater;
     o0GyrCalibrater GyrCalibrater;
     long msOld = 0;
+
     public AimHandler(
         Transform controlObj, 
         Button SetIdentity, 
@@ -207,44 +210,70 @@ class AimHandler
             SetIdentity.onClick.AddListener(DoIdentity);
         }
         
-        MagCalibrater = new o0MagneticCalibraterSimple();
-        string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
-        if (magDataStr.Length > 0)
+        // MagCalibrater = new o0MagneticCalibraterSimple();
+        // string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
+        // if (magDataStr.Length > 0)
+        // {
+        //     string[] dataStrs = magDataStr.Split(',');
+        //     if (dataStrs.Length == 6)
+        //     {
+        //         MagCalibrater._Center =  new Vector3(float.Parse(dataStrs[0]), float.Parse(dataStrs[1]), float.Parse(dataStrs[2]));
+        //         MagCalibrater._Radius =  new Vector3(float.Parse(dataStrs[3]), float.Parse(dataStrs[4]), float.Parse(dataStrs[5]));
+        //     }
+        // }
+        // if (MagCalibrationButton != null)
+        // {
+        //     MagCalibrationButton.onClick.AddListener(delegate {
+
+        //         // if (MagCalibrater.Calibration)
+        //         if (MagCalibrater.Calibration)
+        //         {
+        //             Debug.Log(MagCalibrater.Calibration);
+        //             MagCalibrater.Calibration = false;
+        //             Debug.Log(MagCalibrater.Calibration);
+        //             MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
+        //             float[] dataFloats = new float[6];
+        //             dataFloats[0] = MagCalibrater.Center.x;
+        //             dataFloats[1] = MagCalibrater.Center.y;
+        //             dataFloats[2] = MagCalibrater.Center.z;
+        //             dataFloats[3] = MagCalibrater.Radius.x;
+        //             dataFloats[4] = MagCalibrater.Radius.y;
+        //             dataFloats[5] = MagCalibrater.Radius.z;
+        //             string dataStr = String.Join(",", dataFloats);
+        //             PlayerPrefs.SetString("o0MagneticCalibrater", dataStr);
+        //         }
+        //         else
+        //         {
+        //             Debug.Log(MagCalibrater.Calibration);
+        //             MagCalibrater.Calibration = true;
+
+        //             Debug.Log(MagCalibrater.Calibration);
+        //             MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
+        //         }
+        //     });
+        // }
+        try {
+            string magDataStr = PlayerPrefs.GetString("o0MagneticCalibrater");
+            MagCalibrater = JsonConvert.DeserializeObject<o0MagneticCalibraterEllipsoidFitting>(magDataStr);
+        } catch(Exception) {
+            MagCalibrater = null;
+        }
+        if (MagCalibrater == null) 
         {
-            string[] dataStrs = magDataStr.Split(',');
-            if (dataStrs.Length == 6)
-            {
-                MagCalibrater._Center =  new Vector3(float.Parse(dataStrs[0]), float.Parse(dataStrs[1]), float.Parse(dataStrs[2]));
-                MagCalibrater._Radius =  new Vector3(float.Parse(dataStrs[3]), float.Parse(dataStrs[4]), float.Parse(dataStrs[5]));
-            }
+            MagCalibrater = new o0MagneticCalibraterEllipsoidFitting();
         }
         if (MagCalibrationButton != null)
         {
             MagCalibrationButton.onClick.AddListener(delegate {
-
-                // if (MagCalibrater.Calibration)
                 if (MagCalibrater.Calibration)
                 {
-                    Debug.Log(MagCalibrater.Calibration);
                     MagCalibrater.Calibration = false;
-                    Debug.Log(MagCalibrater.Calibration);
                     MagCalibrationButton.GetComponentInChildren<Text>().text = "开始地磁计校准";
-                    float[] dataFloats = new float[6];
-                    dataFloats[0] = MagCalibrater.Center.x;
-                    dataFloats[1] = MagCalibrater.Center.y;
-                    dataFloats[2] = MagCalibrater.Center.z;
-                    dataFloats[3] = MagCalibrater.Radius.x;
-                    dataFloats[4] = MagCalibrater.Radius.y;
-                    dataFloats[5] = MagCalibrater.Radius.z;
-                    string dataStr = String.Join(",", dataFloats);
-                    PlayerPrefs.SetString("o0MagneticCalibrater", dataStr);
+                    PlayerPrefs.SetString("o0MagneticCalibrater", JsonConvert.SerializeObject(MagCalibrater));
                 }
                 else
                 {
-                    Debug.Log(MagCalibrater.Calibration);
                     MagCalibrater.Calibration = true;
-
-                    Debug.Log(MagCalibrater.Calibration);
                     MagCalibrationButton.GetComponentInChildren<Text>().text = "停止地磁计校准";
                 }
             });

Assets/BowArrow/Scripts/Bluetooth/o09Axis.cs

@@ -1,9 +1,10 @@
-using ArduinoBluetoothAPI;
-using System;
+using System;
 using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
-using UnityEngine.UI;
+using Newtonsoft.Json;
+using MathNet;
+using MathNet.Numerics.LinearAlgebra;
 
 public class o0Vector3Filter
 {
@@ -23,6 +24,310 @@ public class o0Vector3Filter
         return state;
     }
 }
+public class o0MagneticCalibraterEllipsoidFitting//默认在无磁干扰环境下，有磁干扰则无法保证效果
+{
+    [JsonIgnore]
+    public Vector3 _Center = Vector3.zero;
+    [JsonIgnore]
+    Matrix<double> _CorrectMatrix = null;
+
+    public o0Project.Vector3f Center
+    {
+        get
+        {
+            return new o0Project.Vector3f(_Center.x, _Center.y, _Center.z);
+        }
+        set
+        {
+            _Center = new Vector3(value.x, value.y, value.z);
+        }
+    }
+    public double[] CorrectMatrix
+    {
+        get
+        {
+            if (_CorrectMatrix == null)
+                return default;
+            var m = new double[9];
+            for (var i = 0; i < 3; ++i)
+                for (var j = 0; j < 3; ++j)
+                    m[j + i * 3] = _CorrectMatrix[i,j];
+            return m;
+        }
+        set
+        {
+            _CorrectMatrix = CreateMatrix.Dense<double>(3,3);
+            for (var i = 0; i < 3; ++i)
+                for (var j = 0; j < 3; ++j)
+                    _CorrectMatrix[i, j] = value[j + i * 3];
+        }
+    }
+
+    public o0MagneticCalibraterEllipsoidFitting()
+    {
+        //Calibration = true;
+    }
+    public o0MagneticCalibraterEllipsoidFitting(o0Project.Vector3f Center, double[] CorrectMatrix)
+    {
+        this.Center = Center;
+        this.CorrectMatrix = CorrectMatrix;
+    }
+    [JsonIgnore]
+    List<Vector3> records = null;
+    [JsonIgnore]
+    public Vector3 _Radius = default;
+    [JsonIgnore]
+    public bool Calibration
+    {
+        get
+        {
+            return records != null;
+        }
+        set
+        {
+            if (value == true)
+            {
+                records = new List<Vector3>();
+            }
+            else
+            {
+
+
+
+                int mag_data_counter = records.Count;    //mag数据数量
+
+                double mag_x, mag_y, mag_z;
+
+                var mat_D = CreateMatrix.Dense<double>(mag_data_counter, 9);
+
+                //读取mag
+                for (int i = 0; i < mag_data_counter; i++)
+                {
+
+                    //mag_x_y_z赋值
+                    mag_x = records[i].x;
+                    mag_y = records[i].y;
+                    mag_z = records[i].z;
+
+                    mat_D[i, 0] = mag_x * mag_x;
+                    mat_D[i, 1] = mag_y * mag_y;
+                    mat_D[i, 2] = mag_z * mag_z;
+                    mat_D[i, 3] = 2 * mag_x * mag_y;
+                    mat_D[i, 4] = 2 * mag_x * mag_z;
+                    mat_D[i, 5] = 2 * mag_y * mag_z;
+                    mat_D[i, 6] = 2 * mag_x;
+                    mat_D[i, 7] = 2 * mag_y;
+                    mat_D[i, 8] = 2 * mag_z;
+
+                }
+
+                var mat_DT = mat_D.Transpose();
+
+                var mat_Ones = CreateMatrix.Dense<double>(mag_data_counter, 1, 1.0);
+
+                var mat_Result = (mat_DT * mat_D).Inverse() * (mat_DT * mat_Ones);
+
+                var mat_A_4x4 = CreateMatrix.Dense<double>(4, 4);
+
+                mat_A_4x4[0, 0] = mat_Result[0, 0];
+                mat_A_4x4[0, 1] = mat_Result[3, 0];
+                mat_A_4x4[0, 2] = mat_Result[4, 0];
+                mat_A_4x4[0, 3] = mat_Result[6, 0];
+
+                mat_A_4x4[1, 0] = mat_Result[3, 0];
+                mat_A_4x4[1, 1] = mat_Result[1, 0];
+                mat_A_4x4[1, 2] = mat_Result[5, 0];
+                mat_A_4x4[1, 3] = mat_Result[7, 0];
+
+                mat_A_4x4[2, 0] = mat_Result[4, 0];
+                mat_A_4x4[2, 1] = mat_Result[5, 0];
+                mat_A_4x4[2, 2] = mat_Result[2, 0];
+                mat_A_4x4[2, 3] = mat_Result[8, 0];
+
+                mat_A_4x4[3, 0] = mat_Result[6, 0];
+                mat_A_4x4[3, 1] = mat_Result[7, 0];
+                mat_A_4x4[3, 2] = mat_Result[8, 0];
+                mat_A_4x4[3, 3] = -1.0;
+
+
+                var mat_Center = -((mat_A_4x4.SubMatrix(0, 3, 0, 3)).Inverse() * mat_Result.SubMatrix(6, 3, 0, 1));
+                //椭球圆心					//分块，从0,0开始的3*3的矩阵
+
+                var mat_T_4x4 = CreateMatrix.DenseIdentity<double>(4, 4);
+                mat_T_4x4.SetSubMatrix(3, 1, 0, 3, mat_Center.Transpose());
+
+                var mat_R = mat_T_4x4 * mat_A_4x4 * mat_T_4x4.Transpose();
+
+                var evd = mat_R.SubMatrix(0, 3, 0, 3) / -mat_R[3, 3];
+                var eig = evd.Evd();
+
+                var mat_Eigval = CreateVector.Dense<double>(3);
+                var mat_Evecs = eig.EigenVectors;
+
+
+                mat_Eigval[0] = eig.EigenValues[0].Real;   //特征值的实部
+                mat_Eigval[1] = eig.EigenValues[1].Real;
+                mat_Eigval[2] = eig.EigenValues[2].Real;
+
+
+                var mat_Radii = mat_Eigval.Map(delegate (double x)
+                {
+                    return 1.0 / Math.Sqrt(Math.Abs(x));
+                });    //椭球半径，特征值倒数后开方
+
+                var mat_Scale = CreateMatrix.DenseIdentity<double>(3, 3);
+                mat_Scale[0, 0] = mat_Radii[0];
+                mat_Scale[1, 1] = mat_Radii[1];
+                mat_Scale[2, 2] = mat_Radii[2];
+
+                //double min_Radii = mat_Radii.Minimum();           //返回最小的元素
+
+                mat_Scale = mat_Scale.Inverse();// * min_Radii;
+                var mat_Correct = mat_Evecs * mat_Scale * mat_Evecs.Transpose();
+
+                //_Center = new Vector3((float)mat_Center[0], (float)mat_Center[1], (float)mat_Center[2]);
+
+                Debug.Log("The Ellipsoid center is:" + mat_Center.ToString());
+                Debug.Log("The Ellipsoid radii is:" + mat_Radii.ToString());
+                Debug.Log("The scale matrix is:" + mat_Scale.ToString());
+                Debug.Log("The correct matrix is:" + mat_Correct.ToString());
+
+                _Center = new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                _Radius = new Vector3((float)mat_Radii[0], (float)mat_Radii[1], (float)mat_Radii[2]);
+                this._CorrectMatrix = mat_Correct;
+
+                /*
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                        var MathNetV = CreateVector.Dense<double>(3);
+                        MathNetV[0] = v.x;
+                        MathNetV[1] = v.y;
+                        MathNetV[2] = v.z;
+                        //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
+                        MathNetV = (MathNetV) * mat_Correct;
+                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i;
+                        var MathNetV = CreateVector.Dense<double>(3);
+                        MathNetV[0] = v.x;
+                        MathNetV[1] = v.y;
+                        MathNetV[2] = v.z;
+                        //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
+                        MathNetV = (MathNetV) * mat_Correct;
+                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]) - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                        var MathNetV = CreateVector.Dense<double>(3);
+                        MathNetV[0] = v.x;
+                        MathNetV[1] = v.y;
+                        MathNetV[2] = v.z;
+                        MathNetV = (MathNetV * mat_Scale) * mat_Correct;
+                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                        var MathNetV = CreateVector.Dense<double>(3);
+                        MathNetV[0] = v.x;
+                        MathNetV[1] = v.y;
+                        MathNetV[2] = v.z;
+                        MathNetV = (MathNetV * mat_Correct) * mat_Scale;
+                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i - new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                        var MathNetV = CreateVector.Dense<double>(3);
+                        MathNetV[0] = v.x;
+                        MathNetV[1] = v.y;
+                        MathNetV[2] = v.z;
+                        MathNetV = MathNetV * mat_Scale;
+                        v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i - new Vector3((float)mat_Center[0, 0]/ (float)mat_Radii[0], (float)mat_Center[1, 0]/(float)mat_Radii[1], (float)mat_Center[2, 0]/(float)mat_Radii[2]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }
+                {
+                    var textV = new o0Project.Variance(records.Count);
+                    foreach (var i in records)
+                    {
+                        var v = i -new Vector3((float)mat_Center[0, 0], (float)mat_Center[1, 0], (float)mat_Center[2, 0]);
+                        textV.Update(v.magnitude);
+                    }
+                    Debug.Log(textV.Value);
+                }/**/
+
+                records = null;
+            }
+        }
+    }
+    public Vector3 Update(Vector3 v)
+    {
+        if (v.magnitude > 30)
+            Debug.Log(v);
+        if (Calibration)
+        {
+            records.Add(v);
+            return v;
+        }
+        if(_CorrectMatrix != null)
+        {
+            v -= _Center;
+
+            var MathNetV = CreateVector.Dense<double>(3);
+            MathNetV[0] = v.x;
+            MathNetV[1] = v.y;
+            MathNetV[2] = v.z;
+            //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
+            MathNetV = (MathNetV) * _CorrectMatrix;
+
+            v = new Vector3((float)MathNetV[0], (float)MathNetV[1], (float)MathNetV[2]);
+            //Debug.Log(v.magnitude);
+            return v;
+        }
+        return v;
+    }
+    public float CalibratCompletionPercentage()
+    {
+        return 0;
+    }
+}
 public class o0MagneticCalibraterSimple//默认在无磁干扰环境下，有磁干扰则无法保证效果
 {
 
@@ -85,7 +390,6 @@ public class o0MagneticCalibraterSimple//默认在无磁干扰环境下，有磁
             }
         }
     }
-    public System.Random r = new System.Random();
     public Vector3 Update(Vector3 v)
     {
         if (v.magnitude > 30)

Assets/BowArrow/Scripts/Bluetooth/o0Project.cs

@@ -455,4 +455,245 @@ namespace o0Project
             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;
+        }
+    }/**/
 }