C# 中的一些矩阵运算方法

using MathNet.Numerics.LinearAlgebra;
using NumSharp;
using System;
using System.Numerics;

namespace ConsoleApp1
{
    internal class Program
    {

        public static Matrix4x4 QuaternionToMatrix(Quaternion quaternion)
        {
            Vector3 forward = Vector3.Transform(Vector3.UnitZ, quaternion);
            Vector3 up = Vector3.Transform(Vector3.UnitY, quaternion);
            Vector3 right = Vector3.Transform(Vector3.UnitX, quaternion);

            Matrix4x4 matrix = Matrix4x4.Identity;
            matrix.M11 = right.X;
            matrix.M12 = right.Y;
            matrix.M13 = right.Z;

            matrix.M21 = up.X;
            matrix.M22 = up.Y;
            matrix.M23 = up.Z;

            matrix.M31 = -forward.X;
            matrix.M32 = -forward.Y;
            matrix.M33 = -forward.Z;

            return matrix;
        }
    }
}

结果：

Quaternion: x=0, y=0, z=0.7071068, w=0.7071068
Transformed Vector: <3.422854E-08, 0.9999999, 0>
四元数{X:0 Y:0 Z:0.7071068 W:0.7071068}
Rotation Matrix:
3.422854E-08 0.9999999 0 0
-0.9999999 3.422854E-08 0 0
0 0 -1 0
0 0 0 1
invMatrix:DenseMatrix 2x2-Double
 -2     1
1.5  -0.5

numSharpArray:[[-2, 1],
[1.5, -0.5]]

namespace ConsoleApp1
{
        static void Main(string[] args)
        {
            /*
             在四元数中，quaternion.X、quaternion.Y、quaternion.Z和quaternion.W分别代表四元数的四个分量。

                quaternion.X代表四元数的第一个分量，也称为虚部的X分量。
                quaternion.Y代表四元数的第二个分量，也称为虚部的Y分量。
                quaternion.Z代表四元数的第三个分量，也称为虚部的Z分量。
                quaternion.W代表四元数的第四个分量，也称为实部。
            四元数的表示形式为w + xi + yj + zk，其中w是实部，(x, y, z)是虚部的分量。
            通过这四个分量的组合，可以表示旋转、变换等复杂的数学操作。
             */
            Matrix4x4 matrix = new Matrix4x4(
            0.0f, 1.0f, 0.0f, 0.0f,
            -1.0f, 0.0f, 0.0f, 0.0f,
            0.0f, 0.0f, 1.0f, 0.0f,
            2.0f, 3.0f, 4.0f, 1.0f);

            Quaternion quaternion = Quaternion.CreateFromRotationMatrix(matrix);

            // 输出四元数的各个分量
            Console.WriteLine("Quaternion: x={0}, y={1}, z={2}, w={3}", quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);

            // 定义一个原始向量
            Vector3 originalVector = new Vector3(1.0f, 0.0f, 0.0f);

            // 定义一个旋转四元数
            Quaternion rotationQuaternion = Quaternion.CreateFromAxisAngle(new Vector3(0.0f, 0.0f, 1.0f), Convert.ToSingle(Math.PI / 2));

            // 应用旋转操作进行坐标系转换
            Vector3 transformedVector = Vector3.Transform(originalVector, rotationQuaternion);

            // 输出转换后的向量
            Console.WriteLine("Transformed Vector: {0}", transformedVector);



            // 定义一个旋转四元数
            Quaternion rotationQuaternion1 = Quaternion.CreateFromAxisAngle(new Vector3(0.0f, 0.0f, 1.0f), Convert.ToSingle(Math.PI / 2));

            // 将四元数转换为旋转矩阵
            Matrix4x4 rotationMatrix = QuaternionToMatrix(rotationQuaternion);
            Console.WriteLine("四元数"+ rotationQuaternion1);
            // 输出旋转矩阵的元素
            Console.WriteLine("Rotation Matrix:");
            Console.WriteLine(rotationMatrix.M11 + " " + rotationMatrix.M12 + " " + rotationMatrix.M13 + " " + rotationMatrix.M14);
            Console.WriteLine(rotationMatrix.M21 + " " + rotationMatrix.M22 + " " + rotationMatrix.M23 + " " + rotationMatrix.M24);
            Console.WriteLine(rotationMatrix.M31 + " " + rotationMatrix.M32 + " " + rotationMatrix.M33 + " " + rotationMatrix.M34);
            Console.WriteLine(rotationMatrix.M41 + " " + rotationMatrix.M42 + " " + rotationMatrix.M43 + " " + rotationMatrix.M44);

            // 创建一个 2x2 的矩阵
            Matrix<double> matrix2 = Matrix<double>.Build.Dense(2, 2);
            matrix2[0, 0] = 1;
            matrix2[0, 1] = 2;
            matrix2[1, 0] = 3;
            matrix2[1, 1] = 4;

            // 计算矩阵的逆
            Matrix<double> invMatrix = matrix2.Inverse();

            Console.WriteLine("invMatrix:" + invMatrix);
            double[,] array2D = invMatrix.ToArray();

            NDArray numSharpArray = new NDArray(array2D);
            Console.WriteLine("numSharpArray:" + numSharpArray);
            Console.ReadKey();


        }
    public class Matrix3x3
    {
        public float M11 { get; set; }
        public float M12 { get; set; }
        public float M13 { get; set; }
        public float M21 { get; set; }
        public float M22 { get; set; }
        public float M23 { get; set; }
        public float M31 { get; set; }
        public float M32 { get; set; }
        public float M33 { get; set; }

        public Matrix3x3(float[,] values)
        {
            M11 = values[0, 0];
            M12 = values[0, 1];
            M13 = values[0, 2];
            M21 = values[1, 0];
            M22 = values[1, 1];
            M23 = values[1, 2];
            M31 = values[2, 0];
            M32 = values[2, 1];
            M33 = values[2, 2];
        }
    }
}

using NumSharp;
using System;

namespace MyMath
{
    public class MyCover
    {
        /// 

        /// 传入参数说明
        /// 
        /// 图纸上该点的x坐标
        /// 图纸上该点的y坐标
        /// 图纸上该点的z坐标
        /// 在X轴上位移了x长度
        /// 在Y轴上位移了y长度
        /// 在Z轴上位移了z长度
        /// 工件绕Z轴旋转的角度
        /// 工件绕Y轴旋转的角度
        /// 工件绕X轴旋转的角度
        public MyCover(float px, float py, float pz, float x, float y, float z, float a, float b, float r)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.a = a;
            this.b = b;
            this.r = r;
            this.rot = new float[4, 4]
            {
                {
                    this.Cos(this.a)*this.Cos(this.b),
                    this.Cos(this.a)*this.Sin(this.b)*this.Sin(this.r)-this.Sin(this.a)*this.Cos(this.r),
                    this.Cos(this.a)*this.Sin(this.b)*this.Cos(this.r)+this.Sin(this.a)*this.Sin(this.r),
                    this.x
                },
                {
                    this.Sin(this.a)*this.Cos(this.b),
                    this.Sin(this.a)*this.Sin(this.b)*this.Sin(this.r)+this.Cos(this.a)*this.Cos(this.r),
                    this.Sin(this.a)*this.Sin(this.b)*this.Cos(this.r)-this.Cos(this.a)*this.Sin(this.r),
                    this.y
                },
                {
                    -this.Sin(this.b),
                    this.Cos(this.b)*this.Sin(this.r),
                    this.Cos(this.b)*this.Cos(this.r),
                    this.z
                },
                {
                    0,
                    0,
                    0,
                    1
                },
            };
            this.line = new float[4, 1] { { px }, { py }, { pz }, { 1 } };
        }
        #region 字段和属性


        private float[,] rot;
        /// 

        /// 位移+旋转矩阵
        /// 
        public float[,] Rot
        {
            get { return rot; }
            set { rot = value; }
        }

        private float[,] line;

        /// 

        /// 图纸上的坐标，四行 1列
        /// 
        public float[,] Line
        {
            get { return line; }
            set { line = value; }
        }

        private float x;

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

        private float y;

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

        private float z;

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

        private float px;

        public float Px
        {
            get { return px; }
            set { px = value; }
        }

        private float py;

        public float Py
        {
            get { return py; }
            set { py = value; }
        }

        private float pz;

        public float Pz
        {
            get { return pz; }
            set { pz = value; }
        }

        private float a;

        public float A
        {
            get { return a; }
            set { a = value; }
        }

        private float b;

        public float B
        {
            get { return b; }
            set { b = value; }
        }

        private float r;

        public float R
        {
            get { return r; }
            set { r = value; }
        }

        #endregion

        public float Cos(float angle)
        {
            return (float)Math.Round(Math.Cos(angle * 1.00 * (Math.PI) / 180), 5);
        }

        public float Sin(float angle)
        {
            return (float)Math.Round(Math.Sin(angle * 1.00 * (Math.PI) / 180), 5);
        }

        /// 

        /// 位移旋转矩阵*点的坐标
        /// 得到位移旋转后点的坐标
        /// 
        /// 
        public NDArray TestNdArray()
        {
            //float[,] rotMatrix, float[,] pointMatrix
            float[,] rotMatrix = this.rot;
            float[,] pointMatrix = line;
            NDArray ndRotMatrix = np.array(rotMatrix);
            NDArray ndPointMatrix = np.array(pointMatrix);

            return np.matmul(ndRotMatrix, ndPointMatrix);
        }
    }
}

/*
    //int[,] Rot = new int[4, 4];
    //int[,] Lin = new int[4, 1];
    //NDArray ndarr = np.array(Rot);

    //Console.WriteLine(Math.Round(Math.Sin(90 * 1.00 * (Math.PI) / 180), 5));
    //Console.ReadKey();

    MyCover myCover = new MyCover(3, 7, 0, 10, 5, 0, 30, 0, 0);

    NDArray arr1 = myCover.TestNdArray();
    NDArray arrRot = np.array(myCover.Rot);
    //Console.WriteLine(arr1.ToString());
    //[[0],
    //[-0.5],
    //[0.86603],
    //[1]]

    Console.WriteLine("=================");
    Console.WriteLine(arrRot.ToString());
    Console.WriteLine("======****===========");
    Console.WriteLine(arrRot["0,0"].ToString());
    Console.WriteLine("=================");

    Console.WriteLine(arr1.ToString());
    Console.WriteLine("=================");
    Console.WriteLine(arr1["0, 0"].ToString()); //0
    Console.WriteLine(arr1["1, 0"].ToString()); //-0.5
    Console.WriteLine(arr1["2, 0"].ToString()); //0.86603
    Console.ReadKey();
 
 */

        public static void Main()
        {
            // 定义一个原始矩阵
            Matrix4x4 originalMatrix = new Matrix4x4(
                1.0f, 0f, 0f, 0f,
                5.0f, 6.0f, 0f, 0f,
                9.0f, 10.0f, 11.0f, 0f,
                13.0f, 14.0f, 15.0f, 16.0f
            );

            // 计算原始矩阵的逆矩阵
            Matrix4x4 invertedMatrix;
            bool result = Matrix4x4.Invert(originalMatrix, out invertedMatrix);

            if (result)
            {
                // 输出逆矩阵的元素
                Console.WriteLine("Inverted Matrix:");
                Console.WriteLine(invertedMatrix.M11 + " " + invertedMatrix.M12 + " " + invertedMatrix.M13 + " " + invertedMatrix.M14);
                Console.WriteLine(invertedMatrix.M21 + " " + invertedMatrix.M22 + " " + invertedMatrix.M23 + " " + invertedMatrix.M24);
                Console.WriteLine(invertedMatrix.M31 + " " + invertedMatrix.M32 + " " + invertedMatrix.M33 + " " + invertedMatrix.M34);
                Console.WriteLine(invertedMatrix.M41 + " " + invertedMatrix.M42 + " " + invertedMatrix.M43 + " " + invertedMatrix.M44);
            }
            else
            {
                Console.WriteLine("The matrix is not invertible.");
            }

            Console.ReadKey();
        }

Inverted Matrix:
1 0 0 0
-0.8333333 0.1666667 0 0
-0.06060606 -0.1515152 0.09090909 0
-0.02651515 -0.003787879 -0.08522727 0.0625

using System;
using System.Numerics;
using NumSharp;

public class Program
{
    public static void Main()
    {
        // 创建一个 Matrix4x4 矩阵
        Matrix4x4 matrix = new Matrix4x4(
            1.0f, 2.0f, 3.0f, 4.0f,
            5.0f, 6.0f, 7.0f, 8.0f,
            9.0f, 10.0f, 11.0f, 12.0f,
            13.0f, 14.0f, 15.0f, 16.0f
        );

        // 创建一个与 Matrix4x4 矩阵相同大小的 double[,] 数组
        double[,] matrixArray = new double[4, 4];

        // 将 Matrix4x4 的元素复制到数组
        for (int row = 0; row < 4; row++)
        {
            for (int col = 0; col < 4; col++)
            {
                matrixArray[row, col] = matrix[row, col];
            }
        }

        // 创建 NumSharp 矩阵
        NDArray npMatrix = new NDArray(matrixArray);

        // 输出转换后的 NumSharp 矩阵
        Console.WriteLine("NumSharp Matrix:");
        Console.WriteLine(npMatrix);
    }
}