自己做的在C# 中计算机器人坐标系平移旋转的库

using NumSharp;
using System;

namespace MyMath
{
    public class MyCover
    {
        /// <summary>
        /// 传入参数说明
        /// </summary>
        /// <param name="px">图纸上该点的x坐标</param>
        /// <param name="py">图纸上该点的y坐标</param>
        /// <param name="pz">图纸上该点的z坐标</param>
        /// <param name="x">在X轴上位移了x长度</param>
        /// <param name="y">在Y轴上位移了y长度</param>
        /// <param name="z">在Z轴上位移了z长度</param>
        /// <param name="a">工件绕Z轴旋转的角度</param>
        /// <param name="b">工件绕Y轴旋转的角度</param>
        /// <param name="r">工件绕X轴旋转的角度</param>
        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;
        /// <summary>
        /// 位移+旋转矩阵
        /// </summary>
        public float[,] Rot
        {
            get { return rot; }
            set { rot = value; }
        }

        private float[,] line;

        /// <summary>
        /// 图纸上的坐标，四行 1列
        /// </summary>
        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);
        }

        /// <summary>
        /// 位移旋转矩阵*点的坐标
        /// 得到位移旋转后点的坐标
        /// </summary>
        /// <returns></returns>
        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();
 
 */