示例#1
0
        public static float fastNormF(FMatrix5x5 M)
        {
            float sum = 0;

            sum += M.a11 * M.a11 + M.a12 * M.a12 + M.a13 * M.a13 + M.a14 * M.a14 + M.a15 * M.a15;
            sum += M.a21 * M.a21 + M.a22 * M.a22 + M.a23 * M.a23 + M.a24 * M.a24 + M.a25 * M.a25;
            sum += M.a31 * M.a31 + M.a32 * M.a32 + M.a33 * M.a33 + M.a34 * M.a34 + M.a35 * M.a35;
            sum += M.a41 * M.a41 + M.a42 * M.a42 + M.a43 * M.a43 + M.a44 * M.a44 + M.a45 * M.a45;
            sum += M.a51 * M.a51 + M.a52 * M.a52 + M.a53 * M.a53 + M.a54 * M.a54 + M.a55 * M.a55;

            return((float)Math.Sqrt(sum));
        }
示例#2
0
        public static float normF(FMatrix5x5 M)
        {
            float scale = CommonOps_FDF5.elementMaxAbs(M);

            if (scale == 0.0f)
            {
                return(0.0f);
            }

            float a11 = M.a11 / scale,
                  a12 = M.a12 / scale,
                  a13 = M.a13 / scale,
                  a14 = M.a14 / scale,
                  a15 = M.a15 / scale;
            float a21 = M.a21 / scale,
                  a22 = M.a22 / scale,
                  a23 = M.a23 / scale,
                  a24 = M.a24 / scale,
                  a25 = M.a25 / scale;
            float a31 = M.a31 / scale,
                  a32 = M.a32 / scale,
                  a33 = M.a33 / scale,
                  a34 = M.a34 / scale,
                  a35 = M.a35 / scale;
            float a41 = M.a41 / scale,
                  a42 = M.a42 / scale,
                  a43 = M.a43 / scale,
                  a44 = M.a44 / scale,
                  a45 = M.a45 / scale;
            float a51 = M.a51 / scale,
                  a52 = M.a52 / scale,
                  a53 = M.a53 / scale,
                  a54 = M.a54 / scale,
                  a55 = M.a55 / scale;

            float sum = 0;

            sum += a11 * a11 + a12 * a12 + a13 * a13 + a14 * a14 + a15 * a15;
            sum += a21 * a21 + a22 * a22 + a23 * a23 + a24 * a24 + a25 * a25;
            sum += a31 * a31 + a32 * a32 + a33 * a33 + a34 * a34 + a35 * a35;
            sum += a41 * a41 + a42 * a42 + a43 * a43 + a44 * a44 + a45 * a45;
            sum += a51 * a51 + a52 * a52 + a53 * a53 + a54 * a54 + a55 * a55;

            return(scale * (float)Math.Sqrt(sum));
        }
示例#3
0
        /**
         * Converts {@link FMatrixRMaj} into {@link FMatrix5x5}
         *
         * @param input Input matrix.
         * @param output Output matrix.  If null a new matrix will be declared.
         * @return Converted matrix.
         */
        public static FMatrix5x5 convert(FMatrixRMaj input, FMatrix5x5 output)
        {
            if (output == null)
            {
                output = new FMatrix5x5();
            }

            if (input.getNumRows() != output.getNumRows())
            {
                throw new ArgumentException("Number of rows do not match");
            }
            if (input.getNumCols() != output.getNumCols())
            {
                throw new ArgumentException("Number of columns do not match");
            }

            output.a11 = input.data[0];
            output.a12 = input.data[1];
            output.a13 = input.data[2];
            output.a14 = input.data[3];
            output.a15 = input.data[4];
            output.a21 = input.data[5];
            output.a22 = input.data[6];
            output.a23 = input.data[7];
            output.a24 = input.data[8];
            output.a25 = input.data[9];
            output.a31 = input.data[10];
            output.a32 = input.data[11];
            output.a33 = input.data[12];
            output.a34 = input.data[13];
            output.a35 = input.data[14];
            output.a41 = input.data[15];
            output.a42 = input.data[16];
            output.a43 = input.data[17];
            output.a44 = input.data[18];
            output.a45 = input.data[19];
            output.a51 = input.data[20];
            output.a52 = input.data[21];
            output.a53 = input.data[22];
            output.a54 = input.data[23];
            output.a55 = input.data[24];

            return(output);
        }
示例#4
0
        /**
         * Converts {@link FMatrix5x5} into {@link FMatrixRMaj}.
         *
         * @param input Input matrix.
         * @param output Output matrix.  If null a new matrix will be declared.
         * @return Converted matrix.
         */
        public static FMatrixRMaj convert(FMatrix5x5 input, FMatrixRMaj output)
        {
            if (output == null)
            {
                output = new FMatrixRMaj(5, 5);
            }

            if (input.getNumRows() != output.getNumRows())
            {
                throw new ArgumentException("Number of rows do not match");
            }
            if (input.getNumCols() != output.getNumCols())
            {
                throw new ArgumentException("Number of columns do not match");
            }

            output.data[0]  = input.a11;
            output.data[1]  = input.a12;
            output.data[2]  = input.a13;
            output.data[3]  = input.a14;
            output.data[4]  = input.a15;
            output.data[5]  = input.a21;
            output.data[6]  = input.a22;
            output.data[7]  = input.a23;
            output.data[8]  = input.a24;
            output.data[9]  = input.a25;
            output.data[10] = input.a31;
            output.data[11] = input.a32;
            output.data[12] = input.a33;
            output.data[13] = input.a34;
            output.data[14] = input.a35;
            output.data[15] = input.a41;
            output.data[16] = input.a42;
            output.data[17] = input.a43;
            output.data[18] = input.a44;
            output.data[19] = input.a45;
            output.data[20] = input.a51;
            output.data[21] = input.a52;
            output.data[22] = input.a53;
            output.data[23] = input.a54;
            output.data[24] = input.a55;

            return(output);
        }
示例#5
0
 public static bool hasUncountable(FMatrix5x5 a)
 {
     if (UtilEjml.isUncountable(a.a11 + a.a12 + a.a13 + a.a14 + a.a15))
     {
         return(true);
     }
     if (UtilEjml.isUncountable(a.a21 + a.a22 + a.a23 + a.a24 + a.a25))
     {
         return(true);
     }
     if (UtilEjml.isUncountable(a.a31 + a.a32 + a.a33 + a.a34 + a.a35))
     {
         return(true);
     }
     if (UtilEjml.isUncountable(a.a41 + a.a42 + a.a43 + a.a44 + a.a45))
     {
         return(true);
     }
     if (UtilEjml.isUncountable(a.a51 + a.a52 + a.a53 + a.a54 + a.a55))
     {
         return(true);
     }
     return(false);
 }
示例#6
0
 public static bool isIdentical(FMatrix5x5 a, FMatrix5x5 b, float tol)
 {
     if (!MatrixFeatures_FDRM.isIdentical(a.a11, b.a11, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a12, b.a12, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a13, b.a13, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a14, b.a14, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a15, b.a15, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a21, b.a21, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a22, b.a22, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a23, b.a23, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a24, b.a24, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a25, b.a25, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a31, b.a31, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a32, b.a32, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a33, b.a33, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a34, b.a34, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a35, b.a35, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a41, b.a41, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a42, b.a42, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a43, b.a43, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a44, b.a44, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a45, b.a45, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a51, b.a51, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a52, b.a52, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a53, b.a53, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a54, b.a54, tol))
     {
         return(false);
     }
     if (!MatrixFeatures_FDRM.isIdentical(a.a55, b.a55, tol))
     {
         return(false);
     }
     return(true);
 }
示例#7
0
        public static void normalizeF(FMatrix5x5 M)
        {
            float val = normF(M);

            CommonOps_FDF5.divide(M, val);
        }