MathNet.Numerics - peace098beat/windows_applicaciton GitHub Wiki

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra; // https://numerics.mathdotnet.com/Matrix.html

/*
 * 
 * 多軸マニュピュレータの数値計算
 * 
 * [仕様]
 *  1. 回転行列は、右ねじ方向が正
 *  2. 回転行列と並進行列は4x4
 * 
 */
namespace _WpfApplicationOpenTKTemplate1
{
    public class IK
    {
        int ITERATION_NUMBER = 500;

        // ゼロベクトル
        public Vector<double> V0 = Vector<double>.Build.DenseOfArray(new double[4] { 0, 0, 0, 1 });

        public IK() { }

        // Util Vec4to3
        public Vector<double> Vec4To3(Vector<double> V4)
        {
            var r = Vector<double>.Build.DenseOfArray(new double[3] { V4[0], V4[1], V4[2] });
            return r;
        }

        // S
        public Matrix<double> S(double sx, double sy, double xz)
        {
            Matrix<double> Li = Matrix<double>.Build.Dense(4, 4, 0);
            return Li;
        }

        // T
        public Matrix<double> T(double tx, double ty, double tz)
        {
            Matrix<double> r = Matrix<double>.Build.DenseOfArray(new double[4, 4]
                    {
                        {  1,  0,  0,  0},
                        {  0,  1,  0,  0},
                        {  0,  0,  1,  0},
                        { tx, ty, tz,  1}});
            return r;
        }
        // T オーバーライド
        public Matrix<double> T(Vector<double> Vt)
        {
            double tx = Vt[0];
            double ty = Vt[1];
            double tz = Vt[2];
            return T(tx, ty, tz);
        }
        // Rx
        // v' = v*Rx
        public Matrix<double> Rx(double rad)
        {
            Matrix<double> r = Matrix<double>.Build.DenseOfArray(new double[4, 4]
                    {
                        { 1,               0,              0,      0},
                        { 0,   Math.Cos(rad),   Math.Sin(rad),     0},
                        { 0, - Math.Sin(rad),   Math.Cos(rad),     0},
                        { 0,               0,              0,      1}});
            return r;
        }
        // Ry
        public Matrix<double> Ry(double rad)
        {
            Matrix<double> r = Matrix<double>.Build.DenseOfArray(new double[4, 4]
                    {
                        { Math.Cos(rad), 0, -Math.Sin(rad),     0},
                        { 0,             1,              0,     0},
                        { Math.Sin(rad), 0,  Math.Cos(rad),     0},
                        { 0,             0,              0,     1}});
            return r;
        }
        // Rz
        public Matrix<double> Rz(double rad)
        {
            Matrix<double> r = Matrix<double>.Build.DenseOfArray(new double [4, 4]
                    {
                    {  Math.Cos(rad),  Math.Sin(rad), 0,     0},
                    { -Math.Sin(rad),  Math.Cos(rad), 0,     0},
                    { 0,               0,             1,     0},
                    { 0,               0,             0,     1} });
            return r;
        }
        // R
        public Matrix<double> Rxyz(double radx, double rady, double radz)
        {
            Matrix<double> r = Matrix<double>.Build.Dense(4, 4, 0);
            return r;
        }

    }

}

/*
 * IKのユニットテスト
 * 
 * http://qiita.com/mima_ita/items/55394bcc851eb8b6dc24
 */

using _WpfApplicationOpenTKTemplate1;
using MathNet.Numerics.LinearAlgebra;
using Microsoft.VisualStudio.TestTools.UnitTesting;
using System;

namespace UnitTestProject1
{
    [TestClass]
    public class UnitTest1
    {
        [TestMethod]

        public void IK生成テスト()
        {
            var ik = new IK();
        }

        [TestMethod]
        public void MathNetテスト()
        {
            // Vec(4) = Vec(4) * Matrix(4x4) 
            var M4 = Matrix<double>.Build.DenseIdentity(4); // 4x4
            var V3 = Vector<double>.Build.Dense(3);
            var V4 = Vector<double>.Build.Dense(4);
            // テスト
            var _V = V4 * M4; // (1,4)x(4,4) = (1,4)
            Assert.AreEqual(_V.Count, 4);
        }

        // 次元チェック
        [TestMethod]
        public void ディメンジョンテスト()
        {
            var ik = new IK();

            // TRUE : Rz = (4x4)
            var Rz = ik.Rz(Math.PI);
            Assert.AreEqual(Rz.RowCount, 4);
            Assert.AreEqual(Rz.ColumnCount, 4);

            // TRUE : Rz = (4x4)
            var Rx = ik.Rx(Math.PI);
            Assert.AreEqual(Rx.RowCount, 4);
            Assert.AreEqual(Rx.ColumnCount, 4);

            // TRUE : Ry = (4x4)
            var Ry = ik.Ry(Math.PI);
            Assert.AreEqual(Ry.RowCount, 4);
            Assert.AreEqual(Ry.ColumnCount, 4);

            // TRUE : R = (4x4)
            var Rxyz = ik.Rxyz(Math.PI, Math.PI, Math.PI);
            Assert.AreEqual(Rxyz.RowCount, 4);
            Assert.AreEqual(Rxyz.ColumnCount, 4);

            // TRUE : T = (4x4)
            var ez = Vector<double>.Build.DenseOfArray(new double[] { 1, 0, 0, 1 });
            var T = ik.T(ez);
            Assert.AreEqual(T.RowCount, 4);
            Assert.AreEqual(T.ColumnCount, 4);
        }
        # region TEST_RotationMatrix

        [TestMethod]
        public void 回転行列の正則テスト()
        {
            var ik = new IK();
            var I_ = Matrix<double>.Build.DenseIdentity(4);

            var Rx = ik.Rx(Math.PI);
            Assert.AreEqual(Rx * Rx.Transpose(), I_);

            var Ry = ik.Ry(Math.PI);
            Assert.AreEqual(Ry * Ry.Transpose(), I_);

            var Rz = ik.Rz(Math.PI);
            Assert.AreEqual(Rz * Rz.Transpose(), I_);

        }
        [TestMethod]
        public void TestRx()
        {
            // 単位座標exを-90度回転させて,eyとなるかテスト
            var ik = new IK();

            // TRUE : (0,1,0)*Rx(-PI/4) = (0,0,1)
            var ez_ = Vector<double>.Build.DenseOfArray(new double[] { 0, 0, 1 }); // 真値

            var _ey = Vector<double>.Build.DenseOfArray(new double[] { 0, 1, 0, 1 }); // ez z方向単位ベクトル
            Vector<double> _ey4 = _ey * ik.Rx(Math.PI / 2);// -90度回転
            var _ez = ik.Vec4To3(_ey4); //4=>3

            Assert.AreEqual((_ez - ez_).L2Norm(), 0, 1e-15); // 2つが重なっているかテスト
        }
        [TestMethod]
        public void TestRy()
        {
            // 単位座標exを-90度回転させて,eyとなるかテスト
            var ik = new IK();

            // TRUE : (0,1,0)*Rx(-PI/4) = (0,0,1)
            var ex_ = Vector<double>.Build.DenseOfArray(new double[] { 1, 0, 0 }); // 真値

            var _ez = Vector<double>.Build.DenseOfArray(new double[] { 0, 0, 1, 1 }); // ez z方向単位ベクトル
            Vector<double> _ey4 = _ez * ik.Ry(Math.PI / 2);// -90度回転
            var _ex = ik.Vec4To3(_ey4); //4=>3

            Assert.AreEqual((ex_ - _ex).L2Norm(), 0, 1e-15); // 2つが重なっているかテスト
        }

        [TestMethod]
        public void TestRy2() { 
            var ik = new IK();

            // ベクトル TestVector = (1,1,1)
            var TestVector = Vector<double>.Build.DenseOfArray(new double[] { 1, 1, 1, 1 }); // 真値

            // 回転 Ry(Math.PI/2)
            Vector<double> _RotatedVector4 = TestVector * ik.Ry(-Math.PI / 2);// -90度回転
            Vector<double> _RotatedVector = ik.Vec4To3(_RotatedVector4);

            // 回転後 TrueVector = (-1, 1, 1)
            var TrueVector = Vector<double>.Build.DenseOfArray(new double[] { -1, 1, 1 }); // 真値

            Assert.AreEqual((TrueVector - _RotatedVector).L2Norm(), 0, 1e-15); // 2つが重なっているかテスト


        }
        [TestMethod]
        public void TestRz()
        {
            // 単位座標exを-90度回転させて,eyとなるかテスト
            var ik = new IK();

            // TRUE : (1,0,0)*Rz(-PI/4) = (0,1,0)
            var ey_ = Vector<double>.Build.DenseOfArray(new double[] { 0, 1, 0 }); // 真値

            var _ez = Vector<double>.Build.DenseOfArray(new double[] { 1, 0, 0, 1 }); // ez z方向単位ベクトル
            Vector<double> _ey4 = _ez * ik.Rz(Math.PI / 2);// -90度回転
            var _ey = ik.Vec4To3(_ey4); //4=>3

            Assert.AreEqual((_ey - ey_).L2Norm(), 0, 1e-15); // 2つが重なっているかテスト
        }
        #endregion
        [TestMethod]
        public void TestT_double()
        {
            // 単位座標exを-90度回転させて,eyとなるかテスト
            var ik = new IK();

            // TRUE : (1,0,0)*Rz(-PI/4) = (0,1,0)
            var TrueVector = Vector<double>.Build.DenseOfArray(new double[3] { 5, 1, 0 }); // 真値

            var _OriginVector = Vector<double>.Build.DenseOfArray(new double[4] { 1, 1, 0, 1 }); // ez z方向単位ベクトル

            var Transform = Vector<double>.Build.DenseOfArray(new double[3] { 4, 0, 0 });
            double tx = Transform[0];
            double ty = Transform[1];
            double tz = Transform[2];
            // テスト
            Matrix<double> MatrixT = ik.T(tx, ty, tz);// 並進行列 4x4

            var _TransformedVector4 =  _OriginVector * MatrixT;
            var _TransformedVector = ik.Vec4To3(_TransformedVector4);

            Assert.AreEqual((TrueVector - _TransformedVector).L2Norm(), 0, 1e-15); // 2つが重なっているかテスト
        }
        [TestMethod]
        public void TestT_Vector()
        {
            // 単位座標exを-90度回転させて,eyとなるかテスト
            var ik = new IK();

            // TRUE : (1,0,0)*Rz(-PI/4) = (0,1,0)
            var TrueVector = Vector<double>.Build.DenseOfArray(new double[3] { 5, 1, 0 }); // 真値

            var _OriginVector = Vector<double>.Build.DenseOfArray(new double[4] { 1, 1, 0, 1 }); // ez z方向単位ベクトル

            var Transform = Vector<double>.Build.DenseOfArray(new double[3] { 4, 0, 0 });
            // テスト
            Matrix<double> MatrixT = ik.T(Transform);// 並進行列 4x4

            var _TransformedVector4 = _OriginVector * MatrixT;
            var _TransformedVector = ik.Vec4To3(_TransformedVector4);

            Assert.AreEqual((TrueVector - _TransformedVector).L2Norm(), 0, 1e-15); // 2つが重なっているかテスト
        }
        //Utility Vec4To3のテスト
        [TestMethod]
        public void TestVec4To3()
        {
            var ik = new IK();
            var _P4 = Vector<double>.Build.DenseOfArray(new double[] { 1, 2, 3, 4 });
            var P3_ = Vector<double>.Build.DenseOfArray(new double[] { 1, 2, 3 });
            // TEST
            var _P3 = ik.Vec4To3(_P4);
            Assert.AreEqual(_P3, P3_);

        }
    }
}