示例#1
0
        public static void test_Matrix3d()
        {
            {
                Matrix3d Identity = new Matrix3d(true);
                for (int r = 0; r < 3; ++r)
                {
                    for (int c = 0; c < 3; ++c)
                    {
                        Util.gDevAssert((r == c) ? Identity[r, c] == 1 : Identity[r, c] == 0);
                    }
                }
            }

            {
                double[] buffer = new double[9] {
                    1, 2, 3, 4, 5, 6, 7, 8, 9
                };
                Matrix3d M = new Matrix3d(buffer);
                for (int r = 0; r < 3; ++r)
                {
                    for (int c = 0; c < 3; ++c)
                    {
                        Util.gDevAssert(M[r, c] == buffer[r * 3 + c]);
                    }
                }
                for (int k = 0; k < 9; ++k)
                {
                    Util.gDevAssert(M[k] == buffer[k]);
                }

                Matrix3d M2 = new Matrix3d(new Vector3d(1, 2, 3), new Vector3d(4, 5, 6), new Vector3d(7, 8, 9), true);
                Util.gDevAssert(M.EpsilonEqual(M2, 0));
                Matrix3d M3 = new Matrix3d(new Vector3d(1, 4, 7), new Vector3d(2, 5, 8), new Vector3d(3, 6, 9), false);
                Util.gDevAssert(M.EpsilonEqual(M3, 0));

                Util.gDevAssert(M.Transpose().Transpose().EpsilonEqual(M, 0));
                Util.gDevAssert(M.EpsilonEqual(new Matrix3d(M.ToBuffer()), 0));

                Matrix3d Det = new Matrix3d(6, 1, 1, 4, -2, 5, 2, 8, 7);
                Util.gDevAssert(Det.Determinant == -306);

                Matrix3d Mult       = M * M;
                Matrix3d MultResult = new Matrix3d(30, 36, 42, 66, 81, 96, 102, 126, 150);
                Util.gDevAssert(Mult.EpsilonEqual(MultResult, 0));

                Matrix3d Mult2 = new Matrix3d(M * M.Column(0), M * M.Column(1), M * M.Column(2), false);
                Util.gDevAssert(Mult2.EpsilonEqual(MultResult, 0));
            }


            {
                Random     r       = new Random(31337);
                Vector3d[] axes    = TestUtil.RandomPoints3(100, r, Vector3d.Zero);
                double[]   angles  = TestUtil.RandomScalars(100, r, new Interval1d(-180, 180));
                Vector3d[] testPts = TestUtil.RandomPoints3(100, r, Vector3d.Zero, 10);

                for (int k = 0; k < angles.Length; ++k)
                {
                    Vector3d    axis     = axes[k].Normalized;
                    double      angle    = angles[k];
                    Vector3d    pt       = testPts[k];
                    Matrix3d    MRotAxis = Matrix3d.AxisAngleD(axis, angle);
                    Quaterniond QRotAxis = Quaterniond.AxisAngleD(axis, angle);
                    Util.gDevAssert(
                        (MRotAxis * pt).EpsilonEqual(QRotAxis * pt, 100 * MathUtil.Epsilon));

                    Matrix3d QRotAxisM = QRotAxis.ToRotationMatrix();
                    Util.gDevAssert(MRotAxis.EpsilonEqual(QRotAxisM, 10 * MathUtil.Epsilon));

                    Quaterniond QuatCons = new Quaterniond(MRotAxis);
                    Util.gDevAssert(
                        (QuatCons * pt).EpsilonEqual(QRotAxis * pt, 100 * MathUtil.Epsilon));


                    // test 3x3 SVD

                    Matrix3d tmpM = MRotAxis;
                    tmpM[0, 0] += 0.0001; tmpM[1, 1] -= 0.0001; tmpM[0, 2] += 0.1;

                    SingularValueDecomposition fullsvd = new SingularValueDecomposition(3, 3, 999);
                    uint     result = fullsvd.Solve(tmpM.ToBuffer(), -1);
                    double[] U      = new double[9], V = new double[9], S = new double[3];
                    fullsvd.GetU(U); fullsvd.GetV(V);
                    fullsvd.GetSingularValues(S);
                    Matrix3d MU = new Matrix3d(U), MV = new Matrix3d(V);
                    Matrix3d Sdiag = new Matrix3d(S[0], S[1], S[2]);

                    // U is eigenvectors of ATA, V is eigenvectors of AAT
                    Vector3d rRight = Vector3d.Zero, rLeft = Vector3d.Zero;
                    Matrix3d ATA = tmpM.Transpose() * tmpM, AAT = tmpM * tmpM.Transpose();
                    for (int j = 0; j < 3; ++j)
                    {
                        double   eval       = Sdiag[j, j] * Sdiag[j, j];
                        Vector3d right_evec = MV.Column(j);
                        rRight[j] = (ATA * right_evec - eval * right_evec).Length;
                        Vector3d left_evec = MU.Column(j);
                        rLeft[j] = (AAT * left_evec - eval * left_evec).Length;
                    }
                    Util.gDevAssert(rRight.Length < MathUtil.ZeroTolerancef && rLeft.Length < MathUtil.ZeroTolerancef);

                    // [RMS] if U or V contains a reflection, we need to get rid of it
                    if (MU.Determinant < 0)
                    {
                        MU    *= -1;
                        Sdiag *= -1;
                    }
                    if (MV.Determinant < 0)
                    {
                        MV    *= -1;
                        Sdiag *= -1;
                    }

                    Matrix3d MRecons = MU * Sdiag * MV.Transpose();
                    Util.gDevAssert((tmpM * pt).EpsilonEqual(MRecons * pt, 1000 * MathUtil.Epsilon));

                    Quaterniond qU = new Quaterniond(MU);
                    Util.gDevAssert((MU * pt).EpsilonEqual(qU * pt, 100 * MathUtil.Epsilon));
                    Quaterniond qV = new Quaterniond(MV.Transpose());
                    Util.gDevAssert((MV.Transpose() * pt).EpsilonEqual(qV * pt, 100 * MathUtil.Epsilon));
                    Vector3d ptQ = qU * (Sdiag * (qV * pt));
                    Util.gDevAssert((tmpM * pt).EpsilonEqual(ptQ, 1000 * MathUtil.Epsilon));

                    Matrix3d MQRecons = qU.ToRotationMatrix() * Sdiag * qV.ToRotationMatrix();
                    Util.gDevAssert((tmpM * pt).EpsilonEqual(MQRecons * pt, 1000 * MathUtil.Epsilon));


                    // U is eigenvectors of ATA, V is eigenvectors of AAT
                    Vector3d qRight = Vector3d.Zero, qLeft = Vector3d.Zero;
                    for (int j = 0; j < 3; ++j)
                    {
                        double   eval       = Sdiag[j, j] * Sdiag[j, j];
                        Vector3d right_evec = qV.Conjugate().ToRotationMatrix().Column(j);
                        qRight[j] = (ATA * right_evec - eval * right_evec).Length;
                        Vector3d left_evec = qU.ToRotationMatrix().Column(j);
                        qLeft[j] = (AAT * left_evec - eval * left_evec).Length;
                    }
                    Util.gDevAssert(qRight.Length < MathUtil.ZeroTolerancef && qLeft.Length < MathUtil.ZeroTolerancef);


                    double fast_eps = 0.001;

                    FastQuaternionSVD svd      = new FastQuaternionSVD(tmpM, MathUtil.Epsilon, 4);
                    Matrix3d          QQRecons = svd.ReconstructMatrix();
                    Util.gDevAssert((tmpM * pt).EpsilonEqual(QQRecons * pt, fast_eps));

                    Matrix3d svdS = new Matrix3d(svd.S[0], svd.S[1], svd.S[2]);

                    Matrix3d QQRecons2 =
                        svd.U.ToRotationMatrix() * svdS * svd.V.Conjugate().ToRotationMatrix();
                    Util.gDevAssert((tmpM * pt).EpsilonEqual(QQRecons2 * pt, fast_eps));

                    Vector3d ptQSVD = svd.U * (svdS * (svd.V.Conjugate() * pt));
                    Util.gDevAssert((tmpM * pt).EpsilonEqual(ptQSVD, fast_eps));

                    // U is eigenvectors of ATA, V is eigenvectors of AAT
                    Vector3d qqRight = Vector3d.Zero, qqLeft = Vector3d.Zero;
                    for (int j = 0; j < 3; ++j)
                    {
                        double   eval       = svdS[j, j] * svdS[j, j];
                        Vector3d right_evec = svd.V.ToRotationMatrix().Column(j);
                        qqRight[j] = (ATA * right_evec - eval * right_evec).Length;
                        Vector3d left_evec = svd.U.ToRotationMatrix().Column(j);
                        qqLeft[j] = (AAT * left_evec - eval * left_evec).Length;
                    }
                    Util.gDevAssert(qqRight.Length < fast_eps && qqLeft.Length < fast_eps);
                }
            }
        }